Planning with Waypoints¶

Waypoints¶

Waypoints - both in joint as well as Cartesian space - are the foundation for specifying robot motions. Waypoints are usually used to define a robot’s position, but they may hold information about the velocity and acceleration as well. Our planner allows to mix Cartesian and joint spaces freely. However, we recommend to let the planner calculate the inverse kinematics by itself so that Jacobi can optimize for the best possible joint position.

In the simplest case of point-to-point motions, we need to specify two waypoints: a start and a goal. These waypoints can be defined in the code:

as a Config (a list of joint positions).
as a Waypoint (a joint position with possible velocity and acceleration values in joint space),
as a Frame (a pose in Cartesian space),
as a CartesianWaypoint type (a Cartesian position with possible velocity and acceleration values in Cartesian space).

In this tutorial, we want to plan a motion from a Cartesian waypoint (e.g., given by a grasp planner) to a fixed joint position:

start = Frame(x=0.5, z=0.75)  # [m]
goal = [0.5, 0.2, 0.3, 0.4, 0.5, 0.1]  # [rad]

auto start = Frame::from_translation(0.5, 0.0, 0.75);  // [m]
auto goal = {0.5, 0.2, 0.3, 0.4, 0.5, 0.1};  // [rad]

Query the Planner¶

Collision-free Motions¶

We construct the main Planner object by passing in the environment and the cycle time of the system (called delta_time). The cycle time is going to be the sampling rate for the computed trajectories, and it makes sense to use the control rate of the robot here.

planner = Planner(environment, delta_time=0.002)  # [s]

Planner planner {environment, 0.002};  // delta_time in [s]

The planner can also take the robot as input directly if you want to plan motions without environment obstacles. If delta_time is not specified, the planner will define it using a sampling rate of the passed robot.

Now, we can actually query the planner and calculate highly time-optimized, jerk-limited, and collision-free trajectories. The simplest way to get started is to construct a planner and pass the previously defined start and goal to the planner:

trajectory = planner.plan(start, goal)

auto trajectory = planner.plan(start, goal);

Failure Cases¶

When planning with waypoints, the plan method will throw an error if the planning problem is not well defined, namely if:

the start or goal waypoint have a different number of degrees of freedom than the robot,
the goal position is outside the robot’s limits,
the start or goal state at a joint position is in collision with specified obstacles,
an inverse kinematics solution for the start or the goal pose cannot be found.

To avoid planning failures, ensure that these conditions are met before calling the plan method.

Trajectory¶

The Trajectory class is the output of our planner. It includes all information about the calculated joint positions, velocities, accelerations and their timings. To loop over the positons with their respective timings, you can use the following code:

print('Exact trajectory duration:', trajectory.duration, '[s]')

for t, p in zip(trajectory.times, trajectory.positions):
    print(t, p)

std::cout << "Exact trajectory duration: " << trajectory.duration << " [s]" << std::endl;

for (size_t i = 0; i < trajectory.size(); ++i) {
    auto& p = trajectory.positions[i];
    std::cout << trajectory.times[i] << " " << p[0] << std::endl; // ... p[1] ...
}

You can access the velocities and accelerations at the discrete time steps similarly. The sample rate of the trajectory is given by the delta_time argument of the planner. Note that the last sampling duration might be a bit shorter due to the overall trajectory duration.

We have methods to append to or slice from a trajectory, as well as to filter a path of sparse waypoints within a given distance of the original trajectory.

Most importantly, you can pass a Trajectory directly to our robot drivers to run the motion on a real robot.

In the next tutorial, we will delve into the Jacobi motion interface, which allows tackling more complex motion planning problems, such as planning with different task constraints or planning a motion that exactly follows a desired Cartesian path.