# 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. – fibonatic Feb 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? – Spaceman Feb 22 at 22:53

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.