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?