Motion Planning

The Planner is the core of the Jacobi library and calculates the actual trajectories. We also introduce the motion interface using the Motion class for complex use-cases, e.g. to change robot parameters such as joint limits or end-effector obstacles for each motion easily.

Waypoints

Waypoints - both in joint as well as Cartesian space - are the foundation for specifying robot motions. Waypoints are usually used to define a position, but they hold information about the velocity and acceleration as well. For now, we want to plan a motion from a Cartesian waypoint (e.g. given by a grasp planner) to a fixed joint position:

PythonC++

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

In general, we can pass either joint-space Waypoint, a CartesianWaypoint, a joint-space position Config, or a Frame position as start and goal points. The 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.

Query the Planner

Collision-free Motions

We construct the main Planner object that takes 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.

PythonC++

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

The planner can also take the robot as input directly if you want to plan motions without environment obstacles.

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 to pass a start and goal to the planner:

PythonC++

# Start and goal can either be:
#  1. a Waypoint in joint space,
#  2. a CartesianWaypoint, or
#  3. a Config (as a List[float] of the joint position)
trajectory = planner.plan(start, goal)

Motions along Waypoints

A common use-case is to plan a motion along a given list of waypoints. Here, Jacobi does not check for collision but assumes that the waypoints will result in a collision-free trajectory.

PythonC++

# In case the start and goal are known exactly
waypoints = [
    [ 0.20567,  0.34335,  0.26038,  0.84611, -0.34577, -0.60799],
    [ 0.09326, -0.14990,  0.02635,  0.25723,  0.24147, -0.40426],
    [-0.07329,  0.27970,  0.10302,  0.37514, -0.06689,  0.20232],
    [ 0.19213, -0.25143,  0.25726, -0.01240,  0.45425, -0.13753]
]
trajectory = planner.plan(waypoints)

Jacobi works best for a smaller number of waypoints. Then, it will pass exactly through the waypoints. For a larger number of waypoints (more than 20), Jacobi might interpolate the waypoints in a way that the resulting trajectory will stay close, however not reaching the waypoints exactly.

Motion Interface

More complex motions can be planned using the motion interface. First, we construct a Motion with a unique name, the robot that should execute the motion, and a start and goal waypoint:

PythonC++

grasp_to_place_motion = Motion('grasp-to-place', robot,
    start=grasp_waypoint,
    goal=place_waypoint,
)
grasp_to_place_motion.robot.max_acceleration = [5.0, 5.0, 6.0, 10.0, 10.0, 12.0]  # [rad/s^2]
grasp_to_place_motion.robot.max_jerk = [50.0, 50.0, 50.0, 100.0, 100.0, 100.0]  # [rad/s^3]

Note that a copy of the robot instance is made, and thus you can change each parameter of the motion without changing the original robot afterwards. This way, you can keep default settings for all motions (or even make a copy of the robot yourself for really complex setups). In this example, we want to reduce the maximum acceleration so that the robot won’t loose the object during the motion.

Before using the motion interface for planning, we need to add all defined motions to the planner via

PythonC++

planner.add_motion(grasp_to_place_motion)

Then, we can plan a motion given its name only and, in case the start or goal were not specified, the exact start and goal waypoints.

PythonC++

# In case the start and goal are known exactly
trajectory = planner.plan('home-to-bin')

# In case the goal is a region
trajectory = planner.plan('home-to-bin', goal=goal)

Linear Approach & Retraction

Oftentimes, the robot should approach a waypoint along a linear motion in Cartesian space. For this, a motion can take a relative transformation (given in the end-effector frame) at both the start and goal waypoint via

PythonC++

grasp_to_place_motion.linear_cartesian_start = Frame(z=-0.1) # [m]

e.g. to approach the grasp point with a 10cm linear motion in this example. Of course, Jacobi will optimize the complete motion end-to-end and does not stop at the intermediate waypoint.

Soft Collision Checking

Sometimes, the start or goal pose might be in collision as the collision geometries are usually bounding volumes. By default, Jacobi returns an error if a motion would be requested then. Jacobi supports soft collision checking to calculate a motion that leaves the state in collision as fast as possible without moving into a collision again.

PythonC++

motion.soft_collision_start = True
motion.soft_collision_goal = False

End-Effector Orientation

Jaocbi supports a soft constraint on the end-effector (TCP) orientation of the robot through the motion. Precesily, it will minimize the maximum deviation from a target orientation, and does this jointly with the time minimization. The orientation_loss_weight allows to weigh the orientation loss in comparison to the trajectory duration.

PythonC++

motion.orientation_loss_weight = 0.5
motion.orientation_target = [0, 0, 0.1]  # Vector in world frame

End-Effector Forces

Jacobi allows to limit the Cartesian acceleration on the end-effector. Please let us know if you are interested in this feature!

Trajectory

The Trajectory class includes all information about the calculated joint positions, velocities, accelerations and their timings. To loop over the positons with their respective timings, you can

PythonC++

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

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

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.