Motion

The Motion class provides an interface for general point-to-point motion planning with arbitrary waypoints, linear approach and retraction, and task constraints. First, we construct a Motion object with a unique name, the robot that should execute the motion, and a start and goal waypoint:

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]

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.

Linear Approach & Retraction

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

grasp_to_place_motion.linear_retraction = LinearSection(offset=Frame(z=-0.1)) # [m]

e.g., to approach the grasp point with a 10cm linear motion in this example. There is also a speed parameter to slow down a linear section. By default, 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.

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. Precisely, 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 relative to the trajectory duration.

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

Intermediate Waypoints

The Motion interface supports adding up to 3 intermediate waypoints in a motion. The robot trajectory will pass through these waypoints in the order they are added.

motion.waypoints = [
    [ 0.09326, -0.14990,  0.02635,  0.25723,  0.24147, -0.40426],
    [-0.07329,  0.27970,  0.10302,  0.37514, -0.06689,  0.20232],
]

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!


In the next tutorial, we will take a look at the LinearMotion class, which is a specialized motion type for singularity-free linear motions in Cartesian space.