Motion planning and controlling robot (YUMI) with two arms.

edit

Hi there,

So I have this situation. I can't figure out how to control both Yumi arms simultaneously. Basically does anybody know how to control Yumi with given 2 different target positions and orientations for both arms? This is how I simply control one arm.

moveit::planning_interface::MoveGroupInterface group("left_arm");
geometry_msgs::Pose target_pose;
target_pose.orientation.w = 1;
target_pose.orientation.x = 0;
target_pose.orientation.y = 0;
target_pose.orientation.z = 0;
target_pose.position.x = 0.4;
target_pose.position.y = -0.5;
target_pose.position.z = 0.5; 
group.setPoseTarget(target_pose);
moveit::planning_interface::MoveGroup::Plan plan;
group.plan(plan);
success = group.move();

I have a solution how to control both arms simultaneously, but only if I set joint values for both arms like this.

moveit::planning_interface::MoveGroupInterface group("both_arms");
std::map< std::string, double > goal;
goal["yumi_joint_1_l"] = 0.0;
goal["yumi_joint_2_l"] = -0.7292;
goal["yumi_joint_7_l"] = 0.0000;
goal["yumi_joint_3_l"] = 0.0000;
goal["yumi_joint_4_l"] = 0.0000;
goal["yumi_joint_5_l"] = 0.0000;
goal["yumi_joint_6_l"] = 0.0000;
goal["yumi_joint_1_r"] = 0.0000;
goal["yumi_joint_2_r"] = -0.7292;
goal["yumi_joint_7_r"] = 0.0000;
goal["yumi_joint_3_r"] = 0.0000;
goal["yumi_joint_4_r"] = 0.0000;
goal["yumi_joint_5_r"] = 0.0000;
goal["yumi_joint_6_r"] = 0.0000;
group.setJointValueTarget(goal);
moveit::planning_interface::MoveGroup::Plan plan;
group.plan(plan);
success = group.move();

Is there any way to control both arms simultaneously with given pose, or is there any way to convert target_pose to joint values?