Working on feedback linearization force control.  Forces in the vertical direction don't work as expected.