Bimanual Pick and Place
Problem Description
I have been attempting to modify the pick and place tutorial for panda arm from this tutorial for a bimanual pick and place.
I am working on a customised humanoid where there are 2 torso joints, and 5 joints on each arm and a 4 fingered hand attached to each hand. I am using ROS Kinetic on Ubuntu 16.04.
As told in this answer, I have created 4 planning groups. Here are the relevant snippets from my SRDF files.
<group name = "left_arm">
<joint name="lh_arm_platform" />
<joint name="lh_arm_0_joint" />
<joint name="lh_arm_1_joint" />
<joint name="lh_arm_2_joint" />
<joint name="lh_arm_3_joint" />
<joint name="lh_arm_4_joint" />
</group>
<group name = "right_arm">
<joint name="rh_arm_platform" />
<joint name="rh_arm_0_joint" />
<joint name="rh_arm_1_joint" />
<joint name="rh_arm_2_joint" />
<joint name="rh_arm_3_joint" />
<joint name="rh_arm_4_joint" />
</group> <group name="rh_ee">
<chain base_link="rh_arm_0_link" tip_link="rh_arm_4_link" />
</group>
<group name="lh_ee">
<chain base_link="lh_arm_0_link" tip_link="lh_arm_4_link" />
</group>
And my end effector is described as:
<end_effector name="ee_lh" parent_link="lh_arm_0_link" group="lh_ee" parent_group="left_arm" />
<end_effector name="ee_rh" parent_link="rh_arm_0_link" group="rh_ee" parent_group="right_arm" />
I also have a move group for the upper body of the robot, which includes the torso and both hands.
<group name = "complete_robot">
<group name = "left_hand" />
<group name = "right_hand" />
<group name = "left_arm" />
<group name = "right_arm" />
<group name = "torso" />
</group>
In the pick and place code, I have one function for opening the gripper and one function for closing the gripper, which includes all joints of the robot that would have a role to play for correctly grasping the object.
void openGripper(trajectory_msgs::JointTrajectory& posture)
{
// BEGIN_SUB_TUTORIAL open_gripper
/* Add both finger joints of panda robot. */
posture.joint_names.resize(12);
posture.joint_names[0] = "torso_1_joint";
posture.joint_names[1] = "torso_2_joint";
posture.joint_names[2] = "lh_arm_0_joint";
posture.joint_names[3] = "lh_arm_1_joint";
posture.joint_names[4] = "lh_arm_2_joint";
posture.joint_names[5] = "lh_arm_3_joint";
posture.joint_names[6] = "lh_arm_4_joint";
posture.joint_names[7] = "rh_arm_0_joint";
posture.joint_names[8] = "rh_arm_1_joint";
posture.joint_names[9] = "rh_arm_2_joint";
posture.joint_names[10] = "rh_arm_3_joint";
posture.joint_names[11] = "rh_arm_4_joint";
/* Set them as open, wide enough for the object to fit. */
posture.points.resize(1); //number of waypoints
posture.points[0].positions.resize(12); //number of joints
/*point p and joint j --- joint_names<j> corresponds to points<p>.positions<j>*/
// positions are in the joint space
// open - gripper -- ready
posture.points[0].positions[0] = 0.00;
posture.points[0].positions[1] = 0.00;
posture.points[0].positions[2] = 0.1792;
posture.points[0].positions[3] = -1.0268;
posture.points[0].positions[4] = 0.1629;
posture.points[0].positions[5] = 1.6461;
posture.points[0].positions[6] = -1.0593;
posture.points[0].positions[7] = -0.1792;
posture.points[0].positions[8] = 1.2224;
posture.points[0].positions[9] = -0.1629;
posture.points[0].positions[10] = -1.6461;
posture.points[0].positions[11] = 1.0593;
// END_SUB_TUTORIAL
}
This is the open_gripper function and the closed gripper function is defined in a similar manner.
In my "pick" function, I have specified the target position of the parent link of the end effector, to be center of the object that will be manipulated. In order to simplify the problem, I use ...