0
$\begingroup$

Rosanswers logo

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 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<moveit_msgs::Grasp> 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 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?


Originally posted by DebasmitaGhose on ROS Answers with karma: 58 on 2018-08-01

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

  1. No. Those functions in the tutorial supply the end effector postures in opened and closed state, which are then entered into the moveit_msgs/Grasp.msg. Yes, you should do it how it is done in the tutorial.

  2. The move group of the robot, whose joints you are actuating to move the end effector to the desired grasp pose. Just like in the tutorial.

  3. I don't understand what you mean by "reference roll, pitch and yaw values" of any robot. Just try some orientations, and move the robot to them if you are confused.

  4. There is a separate setting inside MoveIt that affects planning attempts of the solver (if I remember right), but each grasp pose supplied to the Pick and Place functions also starts a separate pipeline that attempts the solution of the whole manipulation problem (approach, attach/detach, retreat). Each pipeline is somewhat like a new planning attempt. The code defining them is here.

  5. I don't know how to answer, because it seems like you are misunderstanding something fundamental. The Kinetic tutorial also uses a PoseStamped object to define the grasp pose. tf::createQuaternionMsgFromRollPitchYaw(-M_PI / 2, -M_PI / 4, -M_PI / 2); just sets the orientation of the Pose.

With that said, it looks like your end effector groups are still incorrect (don't they include the same joints as "left_arm" and "right_arm"?), although the last answer told you about this, and I told you here before. Considering that this is the sixth place I see you asking about this, I strongly suggest that you spend more time reading the tutorials on your own. Most of your questions would be solved by studying the code, making small changes, and figuring out what was affected by each change.

As gvdhoorn commented in your last thread, there is no ready-made solution for your bimanual manipulation problem. You will have to develop it, and you won't be able to do that without being able to read and understand the underlying code. Lastly, I don't want to be mean, but before you post the next question, consider this, and try to spend an extra day debugging.


Originally posted by fvd with karma: 2180 on 2018-08-01

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by Geoff on 2018-08-01:
If you are confused about what goes in the manipulator group and what goes in the end effector group, go back to your previous question and look at the links I gave you. The screen shots make it clear.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.