How do I make MoveIt KDLKinematics consume the robot's current velocity?

asked 2020-01-10 20:35:07 -0600

taro gravatar image

I have a setup using JointStateHandles to report position and velocity to MoveIt, which is using the KDLKinematics solver. I am starting a new solve while the robot is already moving, so the joint velocity is reporting non-zero. I would like the new trajectory to incorporate the velocity state into its solve, but every new trajectory has all the joints starting at velocity 0.

How can I make the solve incorporate the robot's current velocity into its plan?


edit retag flag offensive close merge delete


Afaik this is not supported, as MoveIt (or really: OMPL) does not take any dynamics into account, but is purely a position based planner.

KDL itself is only used for solving FK and IK. None of the dynamics parts of KDL are used IIRC.

Adding timing information is done in a later step, with the time parameterisation plugins. But those assume they've been handed a complete trajectory (eg: from 0-state to 0-state).

You'd have to check whether they already do, but by changing the parameterisation to accept an initial state vector you could achieve what you are asking. But again, it's not supported out-of-the-box.

PS: an alternative may be to not do this and instead rely on the trajectory replacement functionality provided by ros_controls joint_trajectory_controller.

gvdhoorn gravatar imagegvdhoorn ( 2020-01-11 02:19:58 -0600 )edit

thanks for the helpful response. the joint trajectory looks promising, but won't that result in dips in the velocity due to the new trajectory starting from 0 velocity?

taro gravatar imagetaro ( 2020-01-13 20:07:01 -0600 )edit

I believe the page I linked to about trajectory replacement discusses this.

gvdhoorn gravatar imagegvdhoorn ( 2020-01-14 09:17:20 -0600 )edit

it does. it seems that the answer is yes, it will result in dips. I'm also having some trouble getting trajectory replacement to work in the way I want.

I have MoveIt repeatedly planning to the same goal and feeding it to the controller. I've manually adjusted the time_from_start for all of the points in the replacement trajectory so that any points with positive time are forward from where the robot will be at the time it receives the new trajectory. However, the robot is jerking backwards each time it receives a new trajectory. I'm not sure if I'm doing something wrong or the jerkiness is to be expected.


taro gravatar imagetaro ( 2020-01-14 15:12:28 -0600 )edit

If you've created your trajectories properly there should be no jerk, but it also depends on how you're interfacing with your robot controller and which requirements that places on input signals (ie: does it do any filtering, or does it require jerk-limited trajectories fi).

We've successfully used trajectory replacement for system with a 20 Hz update rate. I don't remember any issues with "jerking backwards", but we did not always use MoveIt.

gvdhoorn gravatar imagegvdhoorn ( 2020-01-14 16:15:16 -0600 )edit