Stabilize planar brick contact explicit plan with feedback control in simulation

[rcory]

Taking the plan generated from issue #18 , stabilize it in simulation using feedback control. This is assumes that simply playing back the open-loop plan in simulation is not stable (typically a good assumption).

These are the tasks

• The feedback controller can compute the closest contact face online, and determine whether the desired finger face assignment provided by the planner is feasible in real time.
• The feedback controller can switch from position control to force control (where force control uses the finger face assignment information). Need to determine proper heuristics for switching.
• Keyframe parser can read and assign finger face assignments from text file.
• May require a mechanism to determine if the contact point distance error (actual vs desired) is too large to stabilize the plan.