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

edit

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.

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?

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

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.

Thanks.

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.

Part of the jerk is due to the sudden drop in velocity between the two trajectories. Since the initial velocity does not match what the robot is doing, there will be a brief moment in time when it transitions from the old path to the new path where the new path is behind the old path, since it thinks it's starting from a stop. This is why I was hoping the planner could consume the initial velocity. Do you know of any planners that would be able to start planning with a robot that's already in movement? I'm not sure what the proper terminology is to search for such a thing. Thanks.