Wrote a framework for the solution. Started work on the force control part of it.