Bimanual Motion

The BimanualMotion class provides an interface for general point-to-point motion planning for dual-arm robots, with arbitrary waypoints, linear approach and retraction, and task constraints. Until this point, we have only seen how to plan motions for a single robot. However, in many applications, two robots share the same workspace and need to be aware of each other’s movement, or even work together to achieve a common goal. The BimanualMotion class allows to plan such motions.

Before we construct and plan a BimanualMotion, we need to define a dual-arm robot. It can either be a native dual-arm robot from our library, like the ABB YuMi IRB 14000, or it can be constructed using any two single robot arms.

robot = DualArm(arm_left, arm_right)

Next, we need to define a start and goal waypoint for each arm. For dual-arm robots, we use a MultiRobotPoint type, which maps robots to their respective waypoints. The waypoints can be defined in joint or Cartesian space. Any combination of joint and Cartesian waypoints is possible, meaning that one arm can have a joint space waypoint while the other arm has a Cartesian waypoint. Both start points must be defined, but only one goal point is required.

start = MultiRobotPoint({
    robot.left: [0.09326, -0.14990,  0.02635,  0.25723,  0.24147, -0.40426, 0.0], # joint space
    robot.right: CartesianWaypoint(Frame.from_euler(0.5, 0.3, 0.2, 3.141, 0.0, 0.0)) # Cartesian space
})
goal = MultiRobotPoint({
    robot.left: CartesianWaypoint(Frame.from_euler(0.2, 0.3, 0.2, 3.141, 0.0, 0.0)), # Cartesian space
    robot.right: [-0.07329,  0.27970,  0.10302,  0.37514, -0.06689,  0.20232, 0.0] # joint space
})

Now we can construct a BimanualMotion object with a unique name, the robot that should execute the motion, and a start and goal waypoint. We can constrain the motion by setting joint limits for each arm individually.

dual_arm_motion = BimanualMotion('grasp-to-place', robot, start, goal)
dual_arm_motion.robot.left.max_acceleration = [5.0, 5.0, 6.0, 10.0, 10.0, 12.0]  # [rad/s^2]
dual_arm_motion.robot.right.max_acceleration = [4.0, 4.0, 6.0, 10.0, 10.0, 10.0]  # [rad/s^2]

Again, 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).

Coordinated Motion

By default, the constructed BimanualMotion will plan an uncoordinated motion. The arm motions are optimized simultaneously, but each arm will move independently to its goal waypoint, while avoiding collisions with the environment and with the other arm. However, Jacobi also supports planning coordinated bimanual motions, where both arms move together to achieve a common goal. This is especially useful when the arms need to work together to achieve a task, e.g., when moving large objects. To enable coordinated motion, we set the is_coordinated parameter:

dual_arm_motion.is_coordinated = True

The coordinated motion is calculated by using a leader-follower strategy, where first the motion of the leader arm is optimized, and then the follower arm motion is planned to keep a constant offset from the leader arm, defined implcitily by the start waypoints. The lead arm can be set by the lead_arm parameter, which defaults to the left arm:

dual_arm_motion.lead_arm = robot.right

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. For a BimanualMotion, we can set the linear approach and retraction for each arm individually.

dual_arm_motion.linear_retraction = MultiRobotLinearSection({
    robot.left: LinearSection(offset=Frame.z(-0.1)),  # [m]
    robot.right: LinearSection(offset=Frame.z(-0.1))  # [m]
})

In this example, both arms would retract from the start point by 10cm in the z direction. 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. Any combination of linear approaches and retractions is possible, e.g., one arm can have both a linear approach and retraction while the other arm does not have any or only one of them.

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. BimanualMotion supports soft collision checking to find a trajectory that leaves the state in collision as fast as possible without moving into a collision again. Note that soft collision checking is only supported either for both arms or for none, since motion planning for both arms is done simultaneously.

motion.soft_collision_start = True
motion.soft_collision_goal = False

Intermediate Waypoints

The BimanualMotion supports adding up to 3 intermediate waypoints in a motion. The robot trajectory will pass through these waypoints in the order they are added. Like start and goal waypoints, intermediate waypoints are defined as MultiRobotPoint objects, where each arm can have a joint or Cartesian waypoint.

motion.waypoints = [
    MultiRobotPoint({
        robot.left: CartesianWaypoint(Frame.from_euler(0.1, 0.1, -0.1, 3.141, 0.0, 0.0)),
        robot.right: CartesianWaypoint(Frame.from_euler(0.2, 0.2, -0.1, 3.141, 0.0, 0.0))
    }),
    MultiRobotPoint({
        robot.left: CartesianWaypoint(Frame.from_euler(0.2, 0.2, -0.1, 3.141, 0.0, 0.0)),
        robot.right: CartesianWaypoint(Frame.from_euler(0.3, 0.3, -0.1, 3.141, 0.0, 0.0))
    })
]

We covered all of the motion types that are available in Jacobi Motion library. In the next section, we will show how to use the Planner object to plan motions with the presented motion interface.