Use += in inverse dynamics.
Relpace the two gripper planar orientation constraint by a fixed orientation constraint.
Add an accessor to bodySupFor in inverse dynamics.