Differences with pink

[kevinzakka]

1. I chose to implement the frame task with the twist and the Jacobian in the spatial frame since MuJoCo only computes Jacobians in the world frame. There's a few code differences that arise due to this difference and I should investigate if the math is airtight everywhere.
2. Velocity limits are not specified in MuJoCo XMLs, as such limits need to be explicitly added to the QP definition unlike in pink where the configuration object automatically creates them upon initialization.

[stephane-caron]

I chose to implement the frame task with the twist and the Jacobian in the spatial frame since MuJoCo only computes Jacobians in the world frame. There's a few code differences that arise due to this difference and I should investigate if the math is airtight everywhere.

Things should work out as long as the Jacobian and twist are computed in the same frame, be it ${}_0 J_{0b}(q) v = {}_0 e(q)$ or ${}_b J_{0b}(q) v = {}_b e(q)$, with 0 the inertial frame and $b$ the target.

The rationale for deriving the errors from affine transforms stems from

$$\dot{T}_{0b} = [{}_0 \xi \times] T_{0b} = T_{0b} [{}_b \xi \times] \Longrightarrow T_{0b'} = \exp({}_0 \xi) T_{0b} = T_{0b} \exp({}_b \xi)$$

• The error in body frame is such that $T_{0b} \exp({}_b e) = T_{0t}$, thus ${}_b e = \log_6(T_{b0} T_{0t}) = \log_6(T_{bt})$. (More math on this e.g. in this note.)
• The error in the world frame will be such that $\exp({}_0 e) T_{0b} = T_{0t}$, thus ${}_0 e = \log_6(T_{0t} T_{b0})$.

It looks like the frame task here is currently written using body vectors.