# 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 '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 '20 at 22:53

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.