Endow NavState with group operations #1930
Conversation
ProfFan
left a comment
ProfFan
left a comment
There was a problem hiding this comment.
Approving sans some comments. I verified most equations against my notes but I haven't verified formally the adjoint's Jacobians, no bandwidth for that.
I am still very concerned about the use of epsilon around the repository. I fixed (while leaving some side effects unsolved) Rot3 in #780, and I am pretty sure there could be more. But this is not super related to this PR, so we can always check later.
| 0.5 * ((1 - phi2 / 2 - c) / phi4 - 3 * (phi - s - phi3 / 6.) / phi5) * | ||
| (WVW * W + W * WVW); | ||
|
|
||
| // TODO(Frank): this threshold is *different*. Why? |
There was a problem hiding this comment.
The following is probably not correct, refer to #780. Have no bandwidth to look into this more carefully.
There was a problem hiding this comment.
You're right! And epsilon hides it: we never use it, actually :-) So, I removed epsilon, use same threshold in both cases, and corrected as
v + 0.5 * w.cross(v)
| EXPECT(assert_equal(t2, t1.retract(d12))); | ||
| Vector d21 = t2.localCoordinates(t1); | ||
| EXPECT(assert_equal(t1, t2.retract(d21))); | ||
| // EXPECT(assert_equal(d12, -d21)); |
There was a problem hiding this comment.
Not expected for retractions
This is a smaller PR than #1613 that adds$SE_2(3)$ group operations to NavState. Be careful though: we use R,t,v order.
A lot of this code is written by @varunagrawal and possibly @ProfFan.
This PR:
ExpmapTranslationtoPose3, whose derivative with respect to angular velocity is exactly QPose3::ExpmapandNavState::ExpmapExpmapTranslationavoids code duplication and clearly exposes the structure of the exponential map Jacobian.NavState::Expmapis also faster because we don't create the 3x3 W matrices unless the derivative is asked for.