Revision history [back]

Hi @GG,

I think you missed a key point I mentioned in the above post. Sending a set of joint angles as the FK solution instead is a well known method. The beauty of the aforementioned solution is that you can send it 3D pose locations (x,y,z, q1,q2,q3,q4) like you would send to group.set_pose_goal() and the planner will plan in Joint-space/Configuration-space instead of Cartesian-space. This makes all the difference.

Coming to your problem, a simple fix for yours would be to just find few close enough points along the curve and send it to to set_joint_value_target(pose_goal) and run this within a loop, with each iteration executing one of those points.

This might still cause jerky motion and if it does, a slightly more sophisticated solution would be to find the mathematical equation of that curve and let that equation dictate the next waypoint based on the current start state along the way.

If this still doesn't work well, you can try using smoothing filters on top of your existing path planner. For instance, STOMP or CHOMP could be used as a smoothing filter over OMPL. Check out this link.

Hope that helps. :)