Featherstone's Articulated Robot Dynamics Algorithm

Featherstone's robot pendulum

This project is nowhere near as complete as I would like it to be but nevertheless it is a full implementation of Featherstone's algorithm. The algorithm is one of the fastest at computing articulated (jointed) bodies and is not the easiest to understand either.

My implementation of his algorithm doesn't yet support powered joints and so is more like a pendulum than a robot. Although easy to change from the source, the program simulates the motion of 4 bodies attached linearly. The top body rotates around a vertical axis and is attached to the ceiling, the second two are hinged on a horizontal axis and the fourth is hinged perpendicular to these on a horizontal axis.

The motion is undamped and collisions are not supported so the bodies can pass through each other. If you forget this unreal-ism then I hope you find the simulation convincing.

When you run the program you will see the total energy displayed of the system (potential and kinetic). Seeing as this fluctuates somewhat you can tell that the implementation isn't perfect. I believe the discrepancies occur because of integration performed. If I were to implement the Runge-Kutta integrator I would probably reduce the error considerably.

Download my C++ example programs.