Use Eigen in DynamicalSystems #70

GiulioRomualdi · 2020-07-10T14:45:18Z

This goes in the direction of #56

Unfortunately in FloatingBaseSystemDynamics the conversion of the Eigen vectors/matrix in iDynTree objects is done manually. I think that it can be solved with robotology/idyntree#716

A test for the FloatingBaseSystemDynamics class is still missing.

- Store the current state of the system inside the DynamicalSystem class - dynamics() function does not require the current state as input anymore - setState() and getState() functions have been implemented

- m_stateAtNextTimeInstant is not required anymore - oneStepIntegration() function now wants as input only the current time and the samping time of the integrator

- oneStepIntegration() function is now compliant with e612ed8 - From now one we assume that the object contained in DynamicalSystem::StateType and DynamicalSystem::StateDerivativeType have the operator + defined

…ystem

…st modification of DynamicalSystem and Integrator

… modification of DynamicalSystem and Integrator

traversaro · 2020-07-10T15:09:27Z

src/System/src/FloatingBaseSystemDynamics.cpp

+    // project the base orientation matrix in SO3
+    // here we assume that the velocity is expressed using the mixed representation
+    const Eigen::Matrix3d& baseOrientation = std::get<3>(state);
+    Eigen::JacobiSVD<Eigen::Matrix3d> svd(baseOrientation,
+                                          Eigen::ComputeFullU | Eigen::ComputeFullV);
+    std::get<3>(m_state) = svd.matrixU() * svd.matrixV().transpose();


This can be slow, just to understand did you check if that is not a bit bottleneck?

Well @traversaro you are actually right. Half of the time is spent in the setState() function and the bottleneck is the svd calculation.

Do you think that a Baumgarte stabilization may solve the problem?

I don't know what the best option is at the moment, but if this PR is just about using Eigen perhaps we can leave this normalization stuff in a following PR? Or just leave the JacobiSVD for now.

BTW I just found https://github.com/ericjang/svd3, probably not the best option for maintainability but quite a neat idea.

Do you want to perform the integration on SO(3)? Why not passing through quaternions?

For sure quaternion are much easier to normalize.

Actually using quaternion is not so trivial.

Indeed If I use Eigen::Quaterniond I can't do this:

Eigen::Quaterniond a; a.setIdentity(); a = a + Eigen::Vector4d::Zero();

I get the following

error: no match for ‘operator+’ (operand types are ‘Eigen::Quaterniond {aka Eigen::Quaternion<double>}’ and ‘const ConstantReturnType {aka const Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, 4, 1> >}’

Unfortunately, this kind of code is required in addArea(). This function is template and I would avoid adding specialization there.
For the time being, I would go for the Rotation matrix + Baumgard integration. In the future, If we want to use quaternions instead of matrices we can open a new issue and discuss it there. What do you think?

I am a little lost here. The state derivative and the state are generally supposed to be different right? In this case the state is a quaternion and the derivative a Vector4 and it makes sense for them not to be added together. Before you were dealing with the rotation matrix in a specific case. How would it work now?

Let me explain better. The dynamical system class contains the state, its derivative, and the input. In case of a rotation we have:

Eigen::Matrix3d for the state (i.e. the rotation)

Eigen::Matrix3d for the state derivative (i.e. the rotation rate of change)

Eigen::Vector3d for the input (i.e. angular velocity)

The rate of change the rotation matrix is then computed as

Ṙ = ωx R
Ṙ is then integrated to compute R. The `ForwardEuler` integrator uses the function `addArea()` to compute the integral
https://github.com/dic-iit/bipedal-locomotion-framework/blob/339cf76339622cf496d941258374614f43d526af/src/System/include/BipedalLocomotion/System/ForwardEuler.h#L48

This code can compile only if the operator+() is defined between the state and the state derivative. In this case, since the rate of change of the rotation matrix and the rotation matrix are stored in an Eigen::Matrix3d, the operator+() is well defined.

In the case of quaternions we can decide to:

Store the quaternion in an Eigen::Vector4d and apply a similar approach. In this case, I won't have any problems;

Store the quaternion in an Eigen::Quaterniond and its rate of change in Eigen::Vector4d. In this case std::get<I>(x) += std::get<I>(dx) * dT will fail because the operator+() is not defined between Eigen::quaterniond and Eigen::Vector4d.

By the way, in 9870a4f I implemented the Baumgarte stabilization for rotation matrix. Differently from the original paper, I derived the equation in case of angular velocity expressed in the inertial frame. Thanks to this commit I was able to remove the svd computation from the setState() and consequentially I improved the overall performances of the classes.

Great. This has also the nice property that the simulated system remains an ODE, while with the normalization you are basically coupling an ODE with an discrete non linear instantaneous system, i.e. the block that takes in input the integrated orientation (that is the output of the ODE) and returns the normalized rotation instead.

That's very clear, thank you! In the hindsight, it is also pretty clean to have directly the rotation matrix derivative, nice solution.

src/System/tests/IntegratorTest.cpp

S-Dafarra · 2020-07-13T13:27:00Z

src/System/src/FloatingBaseSystemDynamics.cpp

+    // project the base orientation matrix in SO3
+    // here we assume that the velocity is expressed using the mixed representation
+    const Eigen::Matrix3d& baseOrientation = std::get<3>(state);
+    Eigen::JacobiSVD<Eigen::Matrix3d> svd(baseOrientation,
+                                          Eigen::ComputeFullU | Eigen::ComputeFullV);
+    std::get<3>(m_state) = svd.matrixU() * svd.matrixV().transpose();


Also, why you are performing this kind of computations in setState? Is it to make sure that the user is actually passing a rotation matrix?

Have you considered implementing the computations for SVD manually, since the matrix is small?

S-Dafarra · 2020-07-13T13:31:29Z

src/System/src/FloatingBaseSystemDynamics.cpp

+    std::get<0>(m_state) = std::get<0>(state);
+    std::get<1>(m_state) = std::get<1>(state);
+    std::get<2>(m_state) = std::get<2>(state);
+    std::get<4>(m_state) = std::get<4>(state);


I have to say that it is a little hard to remember what is what. I suspect there could be issues where we take one element for another. After a quick googling, there are some possible implementations of "tagged tuples", where the get uses a string instead of an index. Some references:

https://stackoverflow.com/a/46862694

https://gist.github.com/ilsken/dd91285d50197d6345f9

https://github.com/duckie/named_types

https://bitbucket.org/tlutz/named-variadic-template-proposal/src/master/

https://stackoverflow.com/questions/13065166/c11-tagged-tuple

Clearly, this is not related to this PR and probably there is no clean solution, and I don't know if the initializer lists would work anyway, but it may be worth discussing.

traversaro

Overall it seems fine!

…temDynamics and FloatingBaseSystemKinematics

…equals

GiulioRomualdi · 2020-07-14T07:20:34Z

Merging

GiulioRomualdi added 8 commits July 10, 2020 16:20

Redesign the DynamicalSystem class

529327e

- Store the current state of the system inside the DynamicalSystem class - dynamics() function does not require the current state as input anymore - setState() and getState() functions have been implemented

The Integrator class does not hold the state of the system anymore

f143434

Change the logic behind the FixedStepIntegrator class

e612ed8

- m_stateAtNextTimeInstant is not required anymore - oneStepIntegration() function now wants as input only the current time and the samping time of the integrator

Update the logic behind the ForwardEuler integrator.

6e21b87

- oneStepIntegration() function is now compliant with e612ed8 - From now one we assume that the object contained in DynamicalSystem::StateType and DynamicalSystem::StateDerivativeType have the operator + defined

Use Eigen objects instead of iDynTree objects in LinearTimeInvariantS…

bf4c539

…ystem

Update the FLoatingBaseSystemKinematics to be compliant with the late…

afdf409

…st modification of DynamicalSystem and Integrator

Update the FloatingBaseSystemDynamics to be compliant with the latest…

a35c863

… modification of DynamicalSystem and Integrator

Implement a test for the floating base system kinematics

e010b4e

GiulioRomualdi requested review from traversaro and S-Dafarra July 10, 2020 14:45

GiulioRomualdi self-assigned this Jul 10, 2020

GiulioRomualdi added this to the Iteration 47 - Hot restart milestone Jul 10, 2020

traversaro reviewed Jul 10, 2020

View reviewed changes

S-Dafarra reviewed Jul 13, 2020

View reviewed changes

traversaro approved these changes Jul 13, 2020

View reviewed changes

GiulioRomualdi added 2 commits July 13, 2020 21:52

Implement Baumgarte stabilization over SO(3) group in FloatingBaseSys…

9870a4f

…temDynamics and FloatingBaseSystemKinematics

Use builtin eigen functions for checking if two matrices/vectors are …

339cf76

…equals

GiulioRomualdi force-pushed the refactoring/eigen_systems branch from ff98d9a to 339cf76 Compare July 13, 2020 20:07

S-Dafarra approved these changes Jul 14, 2020

View reviewed changes

S-Dafarra mentioned this pull request Jul 14, 2020

Add toEigen in generic vector #71

Merged

GiulioRomualdi merged commit ee31072 into master Jul 14, 2020

GiulioRomualdi deleted the refactoring/eigen_systems branch July 14, 2020 07:20

This was referenced Jul 14, 2020

Improve the readability of the code in the DynamicalSystem classes #72

Open

Discuss the possibility to support different data types in the Integrators. #56

Closed

isorrentino mentioned this pull request Jan 8, 2024

Use IMU measurements to set submodel base state in RDE #793

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Use Eigen in DynamicalSystems #70

Use Eigen in DynamicalSystems #70

GiulioRomualdi commented Jul 10, 2020

traversaro Jul 10, 2020

GiulioRomualdi Jul 10, 2020

traversaro Jul 10, 2020

S-Dafarra Jul 10, 2020

traversaro Jul 10, 2020

GiulioRomualdi Jul 13, 2020 •

edited

S-Dafarra Jul 13, 2020

GiulioRomualdi Jul 13, 2020 •

edited

traversaro Jul 14, 2020

S-Dafarra Jul 14, 2020

S-Dafarra Jul 13, 2020

S-Dafarra Jul 13, 2020

traversaro left a comment

GiulioRomualdi commented Jul 14, 2020

Use Eigen in DynamicalSystems #70

Use Eigen in DynamicalSystems #70

Conversation

GiulioRomualdi commented Jul 10, 2020

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

GiulioRomualdi Jul 13, 2020 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

GiulioRomualdi Jul 13, 2020 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

traversaro left a comment

Choose a reason for hiding this comment

GiulioRomualdi commented Jul 14, 2020

GiulioRomualdi Jul 13, 2020 •

edited

GiulioRomualdi Jul 13, 2020 •

edited