Revision history [back]

I've defined my SRDF like this:

<group name="a_bot">
    <joint name="a_bot_shoulder_pan_joint" />
    <joint name="a_bot_shoulder_lift_joint" />
    <joint name="a_bot_elbow_joint" />
    <joint name="a_bot_wrist_1_joint" />
    <joint name="a_bot_wrist_2_joint" />
    <joint name="a_bot_wrist_3_joint" />
    <chain base_link="a_bot_base_link" tip_link="a_bot_gripper_tip_link" />
</group>
<group name="b_bot">
    <joint name="b_bot_shoulder_pan_joint" />
    <joint name="b_bot_shoulder_lift_joint" />
    <joint name="b_bot_elbow_joint" />
    <joint name="b_bot_wrist_1_joint" />
    <joint name="b_bot_wrist_2_joint" />
    <joint name="b_bot_wrist_3_joint" />
    <chain base_link="b_bot_base_link" tip_link="b_bot_robotiq_85_tip_link" />
</group>
<group name="both_bots">
    <group name="a_bot" />
    <group name="b_bot" />
</group>

This preserves the end-effector information, so you can set multiple goals like this:

group.setPoseTarget(home1, ee_link_a);  
group.setPoseTarget(targetpose, ee_link_b);
success_plan = group.plan(myplan);

I've defined my SRDF like this:

<group name="a_bot">
    <joint name="a_bot_shoulder_pan_joint" />
    <joint name="a_bot_shoulder_lift_joint" />
    <joint name="a_bot_elbow_joint" />
    <joint name="a_bot_wrist_1_joint" />
    <joint name="a_bot_wrist_2_joint" />
    <joint name="a_bot_wrist_3_joint" />
    <chain base_link="a_bot_base_link" tip_link="a_bot_gripper_tip_link" />
</group>
<group name="b_bot">
    <joint name="b_bot_shoulder_pan_joint" />
    <joint name="b_bot_shoulder_lift_joint" />
    <joint name="b_bot_elbow_joint" />
    <joint name="b_bot_wrist_1_joint" />
    <joint name="b_bot_wrist_2_joint" />
    <joint name="b_bot_wrist_3_joint" />
    <chain base_link="b_bot_base_link" tip_link="b_bot_robotiq_85_tip_link" />
</group>
<group name="both_bots">
    <group name="a_bot" />
    <group name="b_bot" />
</group>

This preserves the end-effector information, so you can set multiple goals like this:

group.setPoseTarget(home1, ee_link_a);  
group.setPoseTarget(targetpose, ee_link_b);
success_plan = group.plan(myplan);

But this is for "free" planning, and you asked about cartesian planning with waypoints. I don't think that the current implementation supports this, and what you described (generating a cartesian path for each arm separately, then combining the trajectories) is the most appropriate workaround at the moment.

I don't think that there is a convenience function to check for self-collisions in a given trajectory, but you can make one yourself. Step through each waypoint of your trajectory, set the RobotState of a PlanningScene object to the state at that waypoint using setCurrentState, then check collisions with isStateValid or isStateColliding.

This does not guarantee safety since the states between waypoints are not checked, but with enough padding and a small step size it should be fine.