Add the relative frame task

[stephane-caron]

This PR adds a task similar to the existing FrameTask, but a bit more general.

It is, on purpose, not a parent/child class of FrameTask. We can merge them if applicable in a later PR.

Thanks @ymontmarin for helping with the derivation of this task! 👍

[coveralls]

Pull Request Test Coverage Report for Build 8458281785

Details

• 90 of 96 (93.75%) changed or added relevant lines in 5 files are covered.
• No unchanged relevant lines lost coverage.
• Overall coverage decreased (-0.2%) to 99.45%

---

Changes Missing Coverage	Covered Lines	Changed/Added Lines	%
pink/tasks/relative_frame_task.py	53	59	89.83%

Totals
Change from base Build 8422815704:	-0.2%
Covered Lines:	1265
Relevant Lines:	1272

---

💛 - Coveralls

[ymontmarin]

Maybe I am wrong but line 207 of relative_frame_task.py look weird to me.

Other than that looks good and I proposed a (tiny) simplification

[ymontmarin]

Should'nt it be transform_root_to_frame.act(self.transform_target_to_root) ?

[stephane-caron]

Woah, at least the variable name (if not more 🙈) is definitely wrong. I will fix that.

[ymontmarin]

Store the inverse ${}^tT_r$ instead:
self.transform_root_to_target = transform_target_to_root.inverse().copy()

[stephane-caron]

This would make sense in internal calculations, but from an API standpoint I think it's better in this direction for consistency with the FrameTask:

• Frame task has "transform from target to world" (because common practice is to work with $T_{0b}$ rather than $T_{b0}$)
• Relative frame task has "transform from target to root"

[ymontmarin]

It becomes:

transform_frame_to_root = configuration.get_transform(
    self.frame, self.root
)  # rTf
transform_frame_to_target = self.transform_root_to_target.act(
    transform_frame_to_root
)  # tTf = tTr rTf
error_in_frame: np.ndarray = - pin.log(transform_frame_to_target).vector
# log(fTt) = -log(tTf)

[stephane-caron]

When it is one line of code, you can use the "Add suggestion" button (or <Ctrl+g>) on GitHub.

If it is more than one line of code, either the PR author does it, or you can commit directly to the PR (you are welcome to 😃). Or, in this instance since it was already merged, open a new PR 👌

[ymontmarin]

Anycase with the new formulation we can calculate:
$J_e = - Jlog({}^tT_f) J$
with $J$ left jacobian of ${}^tT_f = {}^tT_r {}^wT_r^{-1} {}^wT_f$

Recall that $[J v ]^{up} = {}^tT_f^{-1}\frac{d}{dt} ({}^tT_f)$ and we calculate:
${}^tT_f^{-1} d/dt ({}^tT_f) = {}^wT_f^{-1} {}^wT_r {}^tT_r^{-1}({}^tT_r(-{}^wT_r^{-1}{}^w\dot{T}_r{}^wT_r^{-1} {}^wT_f + {}^wT_r^{-1}{}^w\dot{T}_f))$
And it simplifies to:
$-{}^fT_r({}^wT_r^{-1}{}^w\dot{T}_r){}^fT_r^{-1} + {}^wT_f^{-1}{}^w\dot{T}_f = [{}^fX_r J_r v + J_f v]^{up}$

And finaly $J = -{}^fX_r J_r + J_f$.
And $J_e = Jlog({}^tT_f) ({}^fX_r J_r - J_f)$

[ymontmarin]

transform_frame_to_root = configuration.get_transform(
    self.frame, self.root
)  # rTf
transform_frame_to_target = self.transform_root_to_target.act(
    transform_frame_to_root
)  # tTf = tTr rTf

action_root_to_frame = transform_frame_to_root.actionInverse

jacobian_frame_in_frame = configuration.get_frame_jacobian(self.frame)  # J_f
jacobian_root_in_root = configuration.get_frame_jacobian(self.root)  # J_r

J = pin.Jlog6(transform_frame_to_target) @ (
    action_root_to_frame @ jacobian_root_in_root
    - jacobian_frame_in_frame
)  # Je = Jlog (- fXr Jr + J_f)
return J

[stephane-caron]

Thank you @ymontmarin! (And apologies for merging early after our chat, I thought this one was good to go.) I will take care of the fix in #81.

Can you open a PR for your alternative calculation proposal?