MoveIt Pick Randomly Fails/Succeeds?

edit

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.

Update2:

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;
    grasps.resize(directions_tested);
    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 ...