I have a 7-DOF robot arm, and I want it to follow a trajectory. This trajectory is defined as a dense sequence of SE(3) waypoints, which were collected by moving the robot arm manually (i.e. by providing a demonstration of a task).

So at each time step, I have a translation error and a rotation error (the error is the distance between the robot's current pose, and the nearest-neighbour demonstration pose). These then need to be converted into a translational velocity and rotational velocity.

But if I just divide each by the time step, (i.e. velocity = distance / time), then this would cause the robot to have a very high velocity when it is further away from the demonstration trajectory. However, I want the robot to move smoothly, with a smooth (ideally constant) translation and rotation velocity. How can I do this?


  • 1
    $\begingroup$ Wouldn't just "saturating" those velocities not be an option? So if their magnitudes are bigger than a maximum value normalize those velocities to magnitudes of that maximum value. $\endgroup$
    – fibonatic
    Feb 20, 2020 at 9:13
  • $\begingroup$ This is not an answer and may be a shot in the blue, but have you tried some kind of regression or polynomial interpolation? $\endgroup$
    – Spaceman
    Feb 22, 2020 at 22:53

1 Answer 1


The Orocos-KDL library might have what you're looking for.

You can first construct a spline-path using Path__RoundedComposite. Each way point is defined with a desired position and orientation. Once the path has been defined, add velocity information using the VelocityProfile class. For a smooth motion, you've 2 options: spline and trapezoidal. If sudden start and stop is acceptable, you could go with rectangular too.

Here's an example from their official repository.

Once you've the trajectory down, extract inverse position and velocity kinematics using KDL's inverse kinematics solvers (assuming you've defined your robot as a KDL chain).

The math underlying all this (for eg. adding timing law to a Cartesian path, rotation interpolation) is pretty interesting, but it might take some time to code all that yourself ... that'd be a great learning experience nonetheless.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.