Add force/torque sensor reading
Add joint bounds

Add naive implementation of an active set method for avoiding joint
bounds. Add reading of ankle force/torque sensors.