Set contact force in body frame instead of point frame to compute torque objective function.
Set xStamp back in PGData.
Use XStamp in collisionConstr to avoid to have to recompute distance in compute and jacobian.