MoveIt Pick Randomly Fails/Succeeds?

asked 2018-09-18 12:48:31 -0500

JulianR gravatar image

updated 2018-09-29 02:25:21 -0500

Update 3:

I am using a different updated arm with an additional Joint, making it a 6 DOF with 8 joints, 2 being mimic joints. Now planning always works after 1 attemps.

However, this is what i found out so far:

I think it turns out that Mimic Joints are not part of the problem. The Robot Arm I used had 7 joints in it's chain, but 2 of those were Mimic Joints. I think that makes it a 5 DOF robot.

I have found a few discussions about 5 DOF robots not properly being supported by the kinematics plugins, although these discussions where quite old. I do not know if that is still up to date. However, when I comment out one mimic joints (tested with both), making it a 6 DOF robot, a solution can easily be found by MoveIt.

So what I did before was just plan over and over again (up to 500 times with the same grasp position), and eventually it found a trajectory. Sometimes after the first attempt, sometimes none after 500 attemps. I think this is because MoveIt chooses a random start position (Link), and for my robot it was just very unlikely for a valid position to be found.

As I said before, when I set the PreGrasp, Graps etc. positions manually with setPoseTarget() and attach/detach the object manually with attachObject() / detachObject() all planning succeeds. However, I noticed that from the PreGrasp Position to the Graps position, the trajectory does not move in a straight line forward.

When using Pick and Place a pre_grasp_approach.direction.vector is given. Here it can be defined that the object to be grasped should be approached from the x direction, but no movement towards y and z for example. Maybe this is what makes my planning fail, but I am not sure about that.

Theoretically it should have been possible for the old arm to move in a straight line forward.


I did more testing and I think it fails due to setting min_distance and desired_distance for the grasps wrong. I do not know why they would be wrong and what are the correct values though.

What I currently am doing, and what did not fail yet, is just calling pick over and over again till it does not fail. I do not know why it can find a solution by giving the same data, and after how many attempts that solution is found seems random to be. Sometime it works after the first try, sometimes after 15.

The Code looks like this, place is handled the same:

bool pick(moveit::planning_interface::MoveGroupInterface& move_group) {
    int directions_tested = 51;
    std::vector<moveit_msgs::Grasp> grasps;
    for (int i = 0; i < directions_tested; i++) {
        grasps[i].grasp_pose.header.frame_id  = "root_link";
        grasps[i].grasp_pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, 0);
        grasps[i].grasp_pose.pose.position.x = object_origin_x - DISTANCE_TO_PALM - object_size_x / 2;
        grasps[i].grasp_pose.pose.position.y = object_origin_y;
        grasps[i].grasp_pose.pose.position.z = object_origin_z;

        grasps ...
edit retag flag offensive close merge delete



I'm currently facing the same issue. My code is in Python though I have a grasp generator which is basically trying all the different combinations of orientations for the grasp attempt. Doesn't seem to make much difference. How many DOF is your arm and what is the kinematics solver?

Wintermute gravatar imageWintermute ( 2018-09-18 22:54:19 -0500 )edit

There are 7 joints in my arm chain, but only 5 of them are actuated and 2 are mimic joints. That makes 5 DOF? On start I get an error about dynamic solvers not being initialized because of mimic, but I do not know if that causes something. I use the standard kdl solver and plan with RTTConnect

JulianR gravatar imageJulianR ( 2018-09-19 01:09:25 -0500 )edit

I don't know about mimic joints, but I remember that the PickPlace tutorial did not work reliably for me until I increased the amount of planning attempts. I would suggest doing that and then digging into the manipulation pipeline. Would be glad to see this thread updated.

fvd gravatar imagefvd ( 2018-09-29 01:31:22 -0500 )edit

I updated what I found out so far. I changed my robot's hardware and now use a slightly different arm with one more joint and all works fine. So I think the main issue is using a 5 DOF robot instead of 6 or 7 DOF in MoveIt.

JulianR gravatar imageJulianR ( 2018-09-29 02:24:13 -0500 )edit

Hi Julian, I am also working on a robot arm with mimic joints. But I am wondering how to create mimic joints by .xcaro file. Could you share your .xacro file or .urdf file? Many thanks! My email address is

Xiaowei gravatar imageXiaowei ( 2018-12-29 00:58:14 -0500 )edit