Bimanual Pick and Place

.positions*/ // 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 a fixed sized object at a fixed position. void pick(moveit::planning_interface::MoveGroupInterface& move_group) { // BEGIN_SUB_TUTORIAL pick1 // Create a vector of grasps to be attempted, currently only creating single grasp. // This is essentially useful when using a grasp generator to generate and test multiple grasps. std::vector grasps; grasps.resize(1); // Setting grasp pose // ++++++++++++++++++++++ // This is the pose of panda_link8. |br| // From panda_link8 to the palm of the eef the distance is 0.058, the cube starts 0.01 before 5.0 (half of the length // of the cube). |br| // Therefore, the position for panda_link8 = 5 - (length of cube/2 - distance b/w panda_link8 and palm of eef - some // extra padding) //The pose of the parent link of the end effector during a grasp --> not the position of any link in the end effector grasps[0].grasp_pose.header.frame_id = "base_footprint"; //base frame grasps[0].grasp_pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(-M_PI / 2, -M_PI / 4, -M_PI / 2); // (roll,pitch,yaw) grasps[0].grasp_pose.pose.position.x = 0.379; //0.379 grasps[0].grasp_pose.pose.position.y = 0.4545; //0.4545 grasps[0].grasp_pose.pose.position.z = 0.2; //0.544 // Setting pre-grasp approach // ++++++++++++++++++++++++++ // The approach direction to take before picking up the object /* Defined with respect to frame_id */ grasps[0].pre_grasp_approach.direction.header.frame_id = "base_footprint"; /* Direction is set as positive x axis */ grasps[0].pre_grasp_approach.direction.vector.z = 1.0; grasps[0].pre_grasp_approach.min_distance = 0.2; grasps[0].pre_grasp_approach.desired_distance = 0.4; // Setting post-grasp retreat // ++++++++++++++++++++++++++ /* Defined with respect to frame_id */ // The retreat direction to take after the grasp has been completed --> object is attached to the gripper grasps[0].post_grasp_retreat.direction.header.frame_id = "base_footprint"; /* Direction is set as positive z axis */ grasps[0].post_grasp_retreat.direction.vector.y = 1.0; grasps[0].post_grasp_retreat.min_distance = 0.1; grasps[0].post_grasp_retreat.desired_distance = 0.25; // Setting posture of eef before grasp // +++++++++++++++++++++++++++++++++++ openGripper(grasps[0].pre_grasp_posture); // END_SUB_TUTORIAL // BEGIN_SUB_TUTORIAL pick2 // Setting posture of eef during grasp // +++++++++++++++++++++++++++++++++++ closedGripper(grasps[0].grasp_posture); // END_SUB_TUTORIAL After this, I have added 3 collision objects in the scene (2 support surfaces and 1 object) as done in the tutorial. This is the main function: int main(int argc, char** argv) { ros::init(argc, argv, "pem_pick_place"); //Node Handle ros::NodeHandle nh; ros::AsyncSpinner spinner(1); spinner.start(); ros::WallDuration(1.0).sleep(); moveit::planning_interface::PlanningSceneInterface planning_scene_interface; moveit::planning_interface::MoveGroupInterface group("complete_robot"); //Move Group for the poses group.setPlanningTime(45.0); addCollisionObjects(planning_scene_interface); // Wait a bit for ROS things to initialize ros::WallDuration(1.0).sleep(); pick(group); ros::WallDuration(1.0).sleep(); place(group); ros::waitForShutdown(); return 0; } I first run the launch file for my moveit configuration, which loads the robot model, then in a separate terminal, I run the executable for the cpp file. ## Errors ## Moveit [ WARN] [1533120527.279616830]: Interactive marker 'EE:goal_rh_fftip' contains unnormalized quaternions. This warning will only be output once but may be true for others; enable DEBUG messages for ros.rviz.quaternions to see more details. [ INFO] [1533120565.018137428]: Planning attempt 1 of at most 1 [ WARN] [1533120565.020146078]: Choice of end-effector for group 'complete_robot' is ambiguous. Assuming 'ee_lh' [ INFO] [1533120565.060632190]: Added plan for pipeline 'pick'. Queue is now of size 1 [ERROR] [1533120565.066561036]: No sampler was constructed [ INFO] [1533120565.066653531]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 0 [ WARN] [1533120565.067049296]: All supplied grasps failed. Retrying last grasp in verbose mode. [ INFO] [1533120565.067228491]: Re-added last failed plan for pipeline 'pick'. Queue is now of size 1 [ INFO] [1533120565.069150446]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 2 [ INFO] [1533120565.069377462]: Pickup planning completed after 0.046757 seconds [ INFO] [1533120566.117776927]: Planning attempt 1 of at most 1 [ERROR] [1533120566.118297808]: Attached body 'object' not found Pick and Place [ WARN] [1533120565.070406529]: Fail: ABORTED: No motion plan found. No execution attempted. [ WARN] [1533120566.119674534]: Fail: ABORTED: Must specify group in motion plan request I also run `rostopic echo /pickup/status` and I get the following: --- header: seq: 16451 stamp: secs: 1533120892 nsecs: 262651730 frame_id: '' status_list: - goal_id: stamp: secs: 1533120564 nsecs: 959055662 id: "/pem_pick_place-1-1533120564.959055662" status: 4 text: "No motion plan found. No execution attempted." ## Questions ## 1. Since I need the robot to bend (move both the torso joints) in order to grasp the object, is it correct that I have specified the desired joint angles of the torso joints in my open_gripper() and closed_gripper() function? Or should I just specify the joint angle values of the stated end effectors? 2. In my main function for the pick and place code, where we specify the desired move_group for manipulation, what move group should be specified? I have currently specified the move_group "complete_robot", but I get a warning which says ambiguous end effector for that move group. 3. In the pick function, the tutorial code uses the function "`tf::createQuaternionMsgFromRollPitchYaw(-M_PI / 2, -M_PI / 4, -M_PI / 2);`". I dont understand how the reference roll, pitch and yaw values for any robot are defined. 4. Are the number of planning attempts equal to the number of grasp messages specified? 5. [This indigo pick and place tutorial](https://github.com/ros-planning/moveit_tutorials/blob/indigo-devel/doc/pr2_tutorials/pick_place/src/pick_place_tutorial.cpp) does not use `tf::createQuaternionMsgFromRollPitchYaw(-M_PI / 2, -M_PI / 4, -M_PI / 2);`, but instead uses `geometry_msgs::PoseStamped p;` How is it different from what is done in the ROS Kinetic tutorial for pick and place?