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)
auto robot = std::make_shared<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
})
MultiRobotPoint start {
{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
};
MultiRobotPoint goal {
{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]
BimanualMotion dual_arm_motion("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
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
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]
})
dual_arm_motion.linear_retraction = MultiRobotLinearSection({
{robot->left, LinearSection(Frame::z(-0.1))}, // [m]
{robot->right, LinearSection(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
motion.linear_cartesian_start = true;
motion.linear_cartesian_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))
})
]
motion.waypoints = std::vector<MultiRobotPoint> {
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.