Controlling a robot arm to follow an SE(3) trajectory

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?

Thanks!

• 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. Feb 20 '20 at 9:13
• This is not an answer and may be a shot in the blue, but have you tried some kind of regression or polynomial interpolation? Feb 22 '20 at 22:53