Bimanual Pick and Place

edit

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 ... (more)