MPC on franka sim

Goal: Test scalability of MPC to Franka, and check the solve time

working in branch 11-speed-up-mpc

Forward simulation loop

• Forward simulation loop (without Mujoco) without orientation
  • Update dynamics to a 7DOF system, just jointspace dynamics without impedance
  • EE translation for objective function
  • Add external force to dynamics and parameters

Mujoco tests [prio B]

• Test with mujoco simulation running

If the final speed on Mujoco is good (>200 Hz, pretty reliable) then we can plan tests on the hardware. If slower, we should implement impedance dynamics in MPC problem.