Added rhythmic DMP support
- also changed the inertia matrix for 3linkarm