machines-in-motion / mim_estimation

This package contains estimation algorithm C++ implementation, their Python bindings and Python prototypes
BSD 3-Clause "New" or "Revised" License
3 stars 1 forks source link

Decide for the quaternion convention in the EKF implementation #4

Closed ahmadgazar closed 3 years ago

ahmadgazar commented 3 years ago

Note: For rendering github equations with MathJax please install either mathjax github plugin for firefox or mathjax github plugin for chrome

Goal:

This issue is part of this epic that aims to settle for a unified design of a generic EKF for BLMC robots.

One important aspect is to decide which quaternion convention to use in representing the base orientations. The two main conventions can be summarized in the following table, where $q_v$ represents the imaginary vector part of the quaternion, and $q_w$ represents the scalar part.

Screenshot from 2021-01-06 09-07-46

As a start, in this commit, I used the pyQuaternion package, which uses the Hamilton convention. If I am not mistaken it's the same as what is used by @nrotella (correct me if I am mistaken). Pinocchio however, uses the JPL convention.

In the end, this design choice plays an important role for the whole computations of the EKF (jacobians, update, ..etc). This means to stick to one design option to avoid confusion later. Tell me what do you think? @majidkhadiv @jviereck @MaximilienNaveau

MaximilienNaveau commented 3 years ago

Quarternion convention must be the one used by pinocchio. As pinocchio is now widely spread among our code base it makes no sens to use something else. Moreover they have full support over log/exp/multiplication and so forth in pinocchio so we should use these ones.

ahmadgazar commented 3 years ago

agreed, will check that. I could not find the lie algebra on quaternions however in the python API of pinocchio. Do you know if they implement the python bindings for the quaternion namespace? For now, the only thing I can find is to create quaternion objects and perform multiplications. Sorrowfully, I could not find the rest of the operations, or am I missing something?

ahmadgazar commented 3 years ago

It seems as well that in robot_centroidal_controller, @avadesh02 and @jviereck implemented those operations without relying on Pinocchio.

jviereck commented 3 years ago

It seems as well that in robot_centroidal_controller, @avadesh02 and @jviereck implemented those operations without relying on Pinocchio.

That's for other reasons and is not best practice ;)

In the kino-dyn-opt, we are using the pinocchio provided methods for computing differences between quaternions.

nrotella commented 3 years ago

The two different quaternion conventions is a common source of confusion, enough to warrant this nice paper (https://arxiv.org/pdf/1801.07478.pdf) concluding that the JPL ("flipped") convention should be abandoned. Admittedly I didn't read the whole paper, but Table I is pretty insightful - most people and major software including Eigen use the Hamiltonian convention.

I believe I was actually using the JPL convention for my estimation work because, I think SL was using JPL as well (I have to go back and check this), and I remember it being incompatible with Eigen. We thus had to implement all our own quaternion math, which I would assume pinocchio does as well somewhere?

Is there a reason pinocchio chose the JPL convention? On the one hand I agree we should remain consistent with pinocchio to avoid constant conversions, but on the other hand, if compatibility with most other libraries is important, then it makes sense to use Hamilton convention.

MaximilienNaveau commented 3 years ago

Pinocchio is based on eigen so they used eigen convention which is not the one used by SL.

I believe I was actually using the JPL convention for my estimation work because, I think SL was using JPL as well

I think you are correct Nick in you assumption.

I would pick the one we have out of the box from pinocchio (Eigen). As @jviereck said in kino-dyn-planner we use the pinnochio python API order to perform the log/exp on quaternion. The trick is convert them in matrix before which is not too slow using eigen anyway.

nrotella commented 3 years ago

Pinocchio is based on eigen so they used eigen convention which is not the one used by SL.

I believe I was actually using the JPL convention for my estimation work because, I think SL was using JPL as well

I think you are correct Nick in you assumption.

I would pick the one we have out of the box from pinocchio (Eigen). As @jviereck said in kino-dyn-planner we use the pinnochio python API order to perform the log/exp on quaternion. The trick is convert them in matrix before which is not too slow using eigen anyway.

I think there is some confusion about conventions, as it appears to me that the pinocchio source (https://github.com/stack-of-tasks/pinocchio/blob/master/src/math/quaternion.hpp) indeed uses the same convention as Eigen as @MaximilienNaveau has said. This is the common Hamiltonian convention, not JPL. @ahmadgazar what makes you think pinocchio is using the JPL convention?

ahmadgazar commented 3 years ago

Oh I see, I was probably confused because of the way the coeffs are print from the Quaternion object in pinocchio, as they are being print in the opposite order. Thanks for clearing the misconception.

nrotella commented 3 years ago

Oh I see, I was probably confused because of the way the coeffs are print from the Quaternion object in pinocchio, as they are being print in the opposite order. Thanks for clearing the misconception.

No worries, the issue of convention gets more confusing than it appears because it can be the case that two libraries use the Hamilton convention, however they use different storage orders (see Joan Sola's technical report which is an excellent source). It looks like pyQuaternion uses the order [w,x,y,z] while Eigen uses x,y,z,w, so this needs to be handled carefully if dealing with the underlying storage directly from either side since pinocchio uses the Eigen ordering. This doesn't mean it uses JPL convention though, just the unusual ordering.

This all gets very confusing and I'm struggling to remember the details myself, but I agree we should use the Hamilton/Eigen/pinocchio convention even if it means possibly modifying the EKF equations a bit (this is the next thing to check).

ahmadgazar commented 3 years ago

I was actually just reading it as we speak, and yes it's very informative :)). Indeed this ordering was the one causing the whole confusion to me, that's why I aimed for pyQuaternion to avoid this confusion in the beginning. Just to confirm, all the math in the paper is again based on Hamilton convention? Because based on this report, there should be modifications be made to the jacobians, and transient model computations .. etc.

MaximilienNaveau commented 3 years ago

If you can write the equations in terms of exp(w) and log(q) then you should have no issue switching convention no?

nrotella commented 3 years ago

I was actually just reading it as we speak, and yes it's very informative :)). Indeed this ordering was the one causing the whole confusion to me, that's why I aimed for pyQuaternion to avoid this confusion in the beginning. Just to confirm, all the math in the paper is again based on Hamilton convention? Because based on this report, there should be modifications be made to the jacobians, and transient model computations .. etc.

Unfortunately I think this is not the case :( I checked my old linearization notes which state that my derivations used the JPL convention. I believe this is because SL used JPL convention. My derivations were based on the properties described in Trawny et al, 2005 which seem to agree with the SL documentation's brief section on quaternions. This paper I mentioned before says that Trawny et al used JPL convention. I don't think I properly understood the implications at the time, so I just used whatever SL used.

This affects the linearization (Jacobians) only for those states involving quaternion math, so for the base quaternion dynamics and possibly the foot quaternion measurement (not needed for point feet). I have to dig deeper to understand what the implications are, to be honest. I think the most obvious change would be some transposes since the two conventions differ on local-to-global vs global-to-local. Let me spend some more time later, but it will just require some care to get it right.

ahmadgazar commented 3 years ago

@MaximilienNaveau It's not about the Lie algebra of the quaternions in itself. The essence of the confusion is the consequence of the convention on the math. For example, integrating the quaternion using JPL convention is \begin{align} \tag{1} q_{k+1} = \text{exp}(\Delta t \omega_k) \otimes q_k, \end{align}

which leads to integrating the base orientation in the global frame (this is also what is used in the paper of @nrotella as well, and also appears in Michael Bloesch's paper). However, if we use the Hamilton convention, integrating the base quaternion will be

\begin{align} \tag{2} q_{k+1} = q_k \otimes \text{exp}(\Delta t \omega_k), \end{align}

which leads to integrating the base orientation in the base frame (local frame) (which should be our interest as the IMU info is obtained in the base frame). This will also be evident if we include in the future the foot orientation state in case of robots with rectangular feet.

ahmadgazar commented 3 years ago

@nrotella Ok, but that makes a lot of sense now. However, I think there is still one more confusing aspect. If the quaternions in the paper are using JPL, then the rotation matrix $^\mathcal{I}C_{\mathcal{B}}$ should be the rotation matrix representing the quaternion of the base $q$ from the base frame to the global frame, which means integrating the position $p$ and velocity $v$ of the base in equations 30 and 31 in the paper should include $C$ instead of $C^T$ as those quantities are expressed in the global frame as well.

nrotella commented 3 years ago

@nrotella Ok, but that makes a lot of sense now. However, I think there is still one more confusing aspect. If the quaternions in the paper are using JPL, then the rotation matrix $^\mathcal{I}C_{\mathcal{B}}$ should be the rotation matrix representing the quaternion of the base $q$ from the base frame to the global frame, which means integrating the position $p$ and velocity $v$ of the base in equations 30 and 31 in the paper should include $C$ instead of $C^T$ as those quantities are expressed in the global frame as well.

Thanks for this and the comment above, I agree there are more implications here due to the choice of convention. Based on the table you have in the first comment, JPL convention should mean that q denotes a rotation from global (world) to local (base) frames, which means the equations should use $C^T$, am I reading this correctly?

There also may be some confusion in how the rotation matrix $C(q)$ is actually computed, since this quaternionToRotationMatrix function has different forms between conventions and no paper gives the equation. I do vaguely remember having some issues which were solved by adding in a transpose, which would be a symptom of this convention mismatch. I will take a look back at my old C++ implementation at some point today to see what I can understand. It would be great to fully resolve these quirks and implement things consistently using the Hamilton convention.

MaximilienNaveau commented 3 years ago

Can we avoid using C? but world_R_base ($ ^{world} R_{base} $)? I know this is more verbose, but much either to debug orientation stuff! idem for quaternions: world_q_base ($ ^{world} q_{base} $, orientation of the base in the world frame) or base_q_world ($ ^{base} q_{world} $).

MaximilienNaveau commented 3 years ago

Also we are having math choices here, can we add to the repo a tex file and a makefile (I can do one). That is a technical report on the algorithm in the repo?

ahmadgazar commented 3 years ago

Based on the table you have in the first comment, JPL convention should mean that q denotes a rotation from global (world) to local (base) frames, which means the equations should use $C^T$, am I reading this correctly?

Yes, you are correct (in JPL convention). What I meant is since we are now aiming for Hamilton convention, then $q \equiv C(q)$ should be the quaternion/rotation matrix C representing the rotation from the base frame to the world frame, and in that case since we representing the position and velocities of the base in the world frame (local-to-global), then transpose should be dropped (referring only to equations 30, 31 in the paper for now). Don't you think so, or I missed something?

It would be great to fully resolve these quirks and implement things consistently using the Hamilton convention.

I totally agree. I am also trying to check the recent papers to check the convention there as well.

Another thing we shall also consider in the computations, the position and orientation of the IMU w.r.t. the base frame as well, but for now we can assume it's attached to the base frame till we figure out all the confusing details related to the conventions first.

ahmadgazar commented 3 years ago

Can we avoid using C? but world_R_base (worldRbase)? I know this is more verbose, but much either to debug orientation stuff! idem for quaternions: world_q_base (worldqbase, orientation of the base in the world frame) or base_q_world (baseqworld).

I agree, we can just switch that later in the code once we agree on the formulation first.

ahmadgazar commented 3 years ago

Also we are having math choices here, can we add to the repo a tex file and a makefile (I can do one). That is a technical report on the algorithm in the repo?

I think we can open that in a separate issue, to setup all the notation, and the technical report can follow from there as well.

MaximilienNaveau commented 3 years ago

In pinocchio one can compute the jacobian of the quaternion using: pinocchio::Jlog3 In python: pinocchio.Jlog3()

ahmadgazar commented 3 years ago

To summarize the issue, I think we established the difference and implications of using JPL and Hamilton conventions for quaternions. In JPL convention, rotational quantities are expressed in the global (world) frame. This might be beneficial in control I think since Trajectory planning is done in the global frame. Paper by Bloesch et al 2012, and Rotella et al 2014 used JPL convention. In Hamilton convention, rotational quantities are expressed in the local frame, and if I am correct it became the design choice over the recent years. Papers by Bloesch et al 2013, Fallon et al 2014, and recently Pronto.

Finally, I think we shall be coherent with the standard of Hamilton. If we all agree on this, shall we close this issue and proceed with discussing the filter's notation and formulation? @nrotella @MaximilienNaveau @jviereck @majidkhadiv

MaximilienNaveau commented 3 years ago

I think we all agree so I will close this issue.

nrotella commented 3 years ago

I think we all agree so I will close this issue.

Yes, agreed, Hamilton convention should definitely be used. I'm glad we took the time to understand this subtle difference between works.