Position tracking controller

Next:
- Virtual PD controller for torso and swing-hip control
- Balance feedback