-
Notifications
You must be signed in to change notification settings - Fork 10
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Fix integration stage for integrators of type ExplicitRungeKuttaSOMixin
#192
Fix integration stage for integrators of type ExplicitRungeKuttaSOMixin
#192
Conversation
…ixin` Co-authored-by: Diego Ferigo <[email protected]>
Probably it is worth to highlight what this PR is fixing, as this may have drastically influenced past tests. We can do that by using the RK equations from https://en.wikipedia.org/w/index.php?title=Runge%E2%80%93Kutta_methods&oldid=1227591654#Explicit_Runge%E2%80%93Kutta_methods for reference. As mentioned in https://en.wikipedia.org/w/index.php?title=Runge%E2%80%93Kutta_methods&oldid=1227591654#Examples, the Forward Euler method can be implemented as a Explicit Runge Kutta method with By using this equations, the ForwardEuler integration equation boil down to (I use where however, this can be written in more general term as: It is important to write it in such general form as indeed the jaxsim/src/jaxsim/integrators/common.py Lines 544 to 563 in 7340d43
Before this PR, that modified SO3 where So, only for the portion of was modified to be: where Note that this change changes an identity (as $a_{11} is always zero) to use instead This PR modifies the SO3-specific modification of the that still normalize the quaternions, but remaining an identity. Note that the problem was not apparent if the system had zero (or small) angular velocity in the base frame, and indeed to properly debug it we set explicitly the initial angular velocity of the system to a really high number (~2000 [rad]/s). In the long term, we could consider to modify our system to actually normalize the input quaternions to avoid having to manually modifies the internal steps of the integrators, but that is another story and it would not block the improvement proposed in this PR. |
For reference, this effectively reverts the commit in 1d2049f that apparently was introduced to handle variable step integrator. However, all tests seems to be passing also with this modification. |
Not sure I understood your comment, but in the case of Forward Euler, there should not be any |
That is exactly what is confusing me. While debugging with @xela-95, we found the problem in the call in jaxsim/src/jaxsim/integrators/common.py Line 394 in 7340d43
k_leaf=0 ?
|
Yes, I think this was called with |
I updated the comment know that @S-Dafarra explained me that |
I can chime in with further details since I'm the author of these integrators. Here below the history of the SO(3) integrators: As @traversaro pointed out, the last commit is the one to blame for the differences seen between the This workaround was originally introduced to mitigate the effect of losing slowly the norm of the quaternion in RK stages (considering no explicit normalization). This PR basically reverts to this behavior that seems more sound even if less AD-friendly.
@S-Dafarra yes you are right but the stage code is generic and it should work for any valid explicit/embedded Butcher tableau. The origin of the problem is the integration scheme of SO(3) performed in intermediate stages that cannot be considered 1:1 with the corresponding FE-like integration (with fractional
The non-SO(3) variants operate on a generic pytree of |
Before merging I want to write something that might not clear to future readers. For a generic RK scheme described by a valid Butcher tableau (explicit, not necessarily single-output), our variants on
|
Actually what I wanted to suggest is to modify our |
Just to clarify: from what I understand, the current computation (before this PR) is wrong also for stages different from 1, as in any case it uses the system angular velocity instead of the angular velocity that would result from doing the computation of |
Few notes. The base quaternion, part of the state vector of the system dynamics, is always normalized when returned to the user ( Regardless to this, there is always the need to perform a kind or projection to unary quaternions in RK stages. It cannot be worked around by simply starting from a unary quaternion. In fact, especially with high-order integrators and sufficiently large I inspected the code used by our RBDAs. I thought that they were normalizing by default the quaternions before using them, but this is actually not the case (they all call
Yes because the derivative of the quaternion was taken from |
Yes, that make sense, without this fix you get completely wrong integration steps (for all stages) especially if you a huge velocity in
That is clear, but why this is a problem? As long as the quaternion is normalized when it is used (i.e. while computing the |
This is not necessarily a problem if there is somewhere a normalization. However, in complex codebases sometimes it can get overlooked and funny things might happen. As in #192 (comment), I forgot that by default quaternions were not normalized in our RBDAs (and this could create problems with with non-SO(3) integrators). |
Just to clarify, I was not suggesting for RBDAs not to normalize quaternions, for clarity I think the normalization should be done only once and in the system dynamics, before the quaternion is passed to RBDA. The idea (that may be wrong) is that for non-experienced users is is much easier to understand what is going on at the system_dynamics level as opposed to some internal function of the integrator (at least for @xela-95 and me that was the case). Anyhow, this discussion are unrelated and do not block the merge of the PR, @flferretti I guess that if you want to review the PR then we can merge. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Thanks everybody, LGTM
This was a big and important fix. Well done everyone. |
As per title, this PR fixes how the infra-stage integration step is performed on integrators inheriting from
ExplicitRungeKuttaSOMixin
class📚 Documentation preview 📚: https://jaxsim--192.org.readthedocs.build//192/