computeCartesianPath() unable to create trajectory

[steven i.]

Hi all

I hope someone can give me a hint or some input on what is going wrong. I am trying to create trajectories using the computeCartesianPath() functionality of the MoveGroup class. In order to test the setup I wanted to start with really simple trajectories (straight lines) but am not getting any results.

I am creating waypoints like so:

std::vector<geometry_msgs::Pose> waypoints;

Then I add two poses, the first is equal to the current pose (though I did try without that one as well), the second represents the target pose, which is a tested pose that is reachable e.g. when calling "setPoseTarget(...)" and "move()". However when I use these waypoints to call computeCartesianPath() it doesn't get me anywhere, the resulting trajectory only contains the start point and the move_group node tells me:

[ INFO] [1425253465.296343735, 14707.055000000]: Received request to compute Cartesian path
[ INFO] [1425253465.296547215, 14707.055000000]: Attempting to follow 2 waypoints for link 'robot_arm_link_6' using a step of 1.000000 m and jump threshold 0.000000 (in global reference frame)
[ INFO] [1425253465.304634966, 14707.055000000]: Computed Cartesian path with 1 points (followed 0.000000% of requested trajectory)

I have tried all kinds of settings for computeCartesianPath(), even with ridiculously low constraints as you can see in the above output, but to no avail. I have also tried with more waypoints along the trajectory, but the output remains the same - none. In older posts about the function it was mentioned that the function caused troubles for arms with less than 6DOF, mine has 5. Is this still an issue? And what can I do? I'm very thankful for all inputs.

Thanks, Steve

[Moises Estrada]

Hi Steve,

It seems that you are using computeCartesian correctly. What robot do you have? I also worked with a 5dof, had problems with the kdl kinematics, so I made my own custom plugin.

I'll admit that the returned fraction number being 0% is strange.

Have you tried the ikfast plugin, it has a 5dof solver, I never could get it to work on my robot but maybe you'll have more luck than I.

[Dave Hershberger]

Hi Steve,

I believe the problem is that computeCartesianPath() just calls IK repeatedly, and if you are using a 6DOF IK solver, it will only be happy if all poses are exactly reachable along the path.  With a 5DOF robot, the chance is vanishingly small that any interpolated 6DOF cartesian pose will be reachable.  (That said, if your start and goal poses are both achievable and your max_step size is longer than the requested path, it should succeed.)

The solution is to find or write an IK solver which returns success when the 5DOF robot gets as close as it "should" to a given 6DOF pose.  (Where you need to define "should" appropriately for your application.  For instance, maybe position is more important than orientation and IK should return success if XYZ are correct and Yaw is correct but roll and/or pitch don't need to match.)

When I work on debugging a situation like this, I often recompile the IK solver I'm using with some debug-writes in it.  Find out what it is trying to solve, see how close it gets before giving up, etc.

Dave

[steven i.]

Thanks a lot for your answers! I had an error in the calculation for my current pose which thus probably was unreachable. WIth that fixed moveit now does calculate trajectories as expected, the 5DOF arm seems not to be an issue so far. I'll see what happens when I switch to more challenging trajectories!