Revision history [back]

Did you find a solution? I am having the same problem at the moment!

Edit:

You can not plan for pose targets for multiple planning groups simultaneously .You first need to find a fitting joint configuration for each robot using IK and then set joint targets. I opened an Issue for a similar problem here.

bool rocket_found_ik = kinematic_state->setFromIK(rocket_joint_model_group, rocket_pose, timeout);
bool groot_found_ik = kinematic_state->setFromIK(groot_joint_model_group, groot_pose, timeout);

kinematic_state->copyJointGroupPositions(rocket_joint_model_group, rocket_joint_values);
kinematic_state->copyJointGroupPositions(groot_joint_model_group, 

// planning group with rocket and groot
rng_group_interface.setJointValueTarget(rocket_joint_names, 
rng_group_interface.setJointValueTarget(groot_joint_names, 

bool success = (rng_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

I create a demo project you can find here.

dual arm