1. Implemented the method for evalulating the foot contact
model as detailed in Kecskemethy's IUTAM 2011 paper. Didn't
work so well.

2. Implemented the full version of my mixed position and force
feedback control system. Works great. Just about to turn it into
an inner loop ... hence the backup.

Signed-off-by: mjhmilla <millard.matthew@gmail.com>