TRAC_IK plugin fails to find a solution for 5DOF custom chain

[Adrian Zwiener]

I tested TRACK_IK with our 5DOF arm consisting of several Robotis Dynamixel MX motors, since motion planning with moveit and KDL is not very satisfying.

At first, I take 1000 random joint states (moveit RobotState::setToRandomPositions) and calculate forward kinematics (FK) and use the pose to calculate the joint values via inverse kinematics (IK). Now I calculated the pose of the IK joint values and compare the result with the pose obtained by the random joint values. As expected, these results are equal (apart from numerical deviations; agreement up to 1e-5).

However using specific, hard coded joint values IK is not able to find a solution. Although this state is valid. (See attached code. Test searchIKandFK works but test singleState does not.)

Using KDL everything is working.

[Patrick Beeson]

TRAC-IK runs KDL as part of it's calculations, so I find it odd that KDL works and TRAC-IK does not. I will look at your test case and get back to you.

[Patrick Beeson]

Adrian,

Can you please include your arm_ik_plugin package that this test code depends on?

[Adrian Zwiener]

Oh sorry, I copied the test case from a package containing a self written kinematic plug-in. These files are not needed in the test case test_trac_ik.cpp. However the problem persists.

[Adrian Zwiener]

I think I see the problem now. It seems that the correct solution is found but since the MoveitErrorCode is not equal to Success the return value is false even though a solution is found. In the implementation of searchPositionIK(...)

    int rc = ik_solver->CartToJnt(in, frame, out, bounds);


    solution.resize(num_joints_);

    if (rc >=0) {

      for (uint z=0; z< num_joints_; z++)
        solution[z]=out(z);

      if(!solution_callback.empty()) {
        solution_callback(ik_pose,solution,error_code);
      }
      if(error_code.val == error_code.SUCCESS)
         return true;
    }

    error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
    return false;

Setting (in my tests):

 moveit_msgs::MoveItErrorCodes error_code;  error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;

solves the problem.

Is this behaviour wanted?

[Patrick Beeson]

That bug was fixed yesterday I believe. Can you git pull and make sure you are running the latest?

[Patrick Beeson]

Commit 4969f1a

[Patrick Beeson]

Should have been fixed with commit 4969f1a