Different inertia parametrizations and Physical consistency

[simeon-ned]

Pseudo Inertia and Log Cholesky Parametrization

This PR introduces the capability to use pseudo inertia and Log Cholesky parametrization within Pinocchio. These additions are based on recent advancements in ensuring the physical consistency of inertial parameters and provide a novel approach to parameterize the pseudo inertia matrix. The new functionalities include conversions to/from pseudo inertia, Log Cholesky parametrization, and related handy functions such as the Jacobian of Log Cholesky to dynamic parameters. Here you may find some short related notes on these parametrizations.

Key Contributions

1. Pseudo Inertia Parametrization:

   • Implementation of the pseudo inertia matrix as described in the paper Linear Matrix Inequalities for Physically-Consistent Inertial Parameter Identification: A Statistical Perspective on the Mass Distribution.
   • Conversion functions to/from pseudo inertia.
   • Ensuring physical consistency constraints via positive definiteness.

2. Log Cholesky Parametrization:

   • Incorporation of the Log Cholesky decomposition method as detailed in Smooth Parameterization of Rigid-Body Inertia.
   • Functions for the Log Cholesky decomposition and reparameterization of the pseudo inertia matrix.
   • Jacobian computation for Log Cholesky to dynamic parameters conversion, which may be useful for adaptive control.

This PR also partially close the problem of checking physical consistency mentioned in #2204

[jorisv]

@jcarpent Do you think is a good idea to create dedicated type to store pseudo inertia and log cholesky parametrization ?

• Provide dedicated constructor to easily create this matrices
• Provide getter to matrix parameter (mass, h, sigma, H_bar)
• Store related method (i.e. calculateLogCholeskyJacobian)
• Avoid to call FromPseudoInertia or FromLogCholeskyParameters with a raw matrix (and then, making mistake)

[jcarpent]

Thanks @simeon-ned for this PR and thanks @jorisv for making a first feedback. I will have time to dive into this PR next week.

[simeon-ned]

@jcarpent Sure, looking forward to your reply! Tag me anytime, and I'll be glad to assist. Just a note, I'm not very proficient in C++, but will do my best :)

[jcarpent]

To move beyond, I do agree with @jorisv suggestions. Having dedicated parametrizations (like Eigen::Quaternion) will help us to move forward for properly differentiating along different parametrization of Inertias. I let @jorisv provides further guidance.

@jcarpent Do you think is a good idea to create dedicated type to store pseudo inertia and log cholesky parametrization ?

* Provide dedicated constructor to easily create this matrices

* Provide getter to matrix parameter (mass, h, sigma, H_bar)

* Store related method (i.e. calculateLogCholeskyJacobian)

* Avoid to call FromPseudoInertia or FromLogCholeskyParameters with a raw matrix (and then, making mistake)

[simeon-ned]

@jcarpent, sure, I will add more information in the docstrings, include myself in the contribution list, and work on implementing @jorisv's proposition regarding separate classes for log-Cholesky and pseudo inertia.

Meanwhile, I would like to discuss the conversion to log-Cholesky parameters from dynamic and pseudo inertia. I have drafted a Python implementation for this conversion, as shown below:

def pseudo2logcholesky(pseudo_inertia: np.ndarray) -> np.ndarray:
    n = pseudo_inertia.shape[0]
    indices = np.arange(n - 1, -1, -1)  # Indices to reverse the order

    # Apply the inversion using indices for rows and columns
    reversed_inertia = pseudo_inertia[indices][:, indices]

    # Perform Cholesky decomposition on the permuted matrix A'
    L_prime = np.linalg.cholesky(reversed_inertia)

    # Apply the reverse permutation to L_prime and transpose it to form U
    U = L_prime[indices][:, indices]

    alpha = np.log(U[3, 3])
    d1 = np.log(U[0, 0] / U[3, 3])
    d2 = np.log(U[1, 1] / U[3, 3])
    d3 = np.log(U[2, 2] / U[3, 3])
    s12 = U[0, 1] / U[3, 3]
    s23 = U[1, 2] / U[3, 3]
    s13 = U[0, 2] / U[3, 3]
    t1 = U[0, 3] / U[3, 3]
    t2 = U[1, 3] / U[3, 3]
    t3 = U[2, 3] / U[3, 3]
    return np.array([alpha, d1, d2, d3, s12, s23, s13, t1, t2, t3])

However, I am a bit confused about how to achieve the equivalent reversal of order in reversed_inertia and U using C++ and Eigen, similar to the approach in Python with NumPy. The challenge arises because this particular instance of log-Cholesky parameterization requires the decomposition to be in the form ( U U^T ) instead of ( U^T U ), where ( U ) is an upper triangular matrix. This form ensures that the resulting log-Cholesky parameters are interpretable and physically coupled with dynamic parameters (such as mass lever, etc.).

I certainly can achieve this with multiplication by permutation matrices, but this may not be the most efficient approach. If you have any tips or suggestions on how to handle this in C++ using Eigen in a more efficient manner, it would be greatly appreciated.

[simeon-ned]

@jcarpent, I hope you’re doing well. I’ve reworked the code to better align with @jorisv's suggestions.

I’ve introduced two new structures in inertia.hpp: PseudoInertiaTpl and LogCholeskyParametersTpl. These structures encapsulate methods for conversions between different inertia parametrizations, along with their constructors and getters.

The InertiaTpl class now includes methods to build an Inertia from either PseudoInertia or LogCholeskyParameters and convert it back to these forms.

I’ve also added a dedicated cpp test in spatial.cpp that handles conversions between the different inertia parametrizations, ensures physical consistency, and checks that the Jacobian of the Log Cholesky parametrization is not singular.

Additionally, I have included references to relevant papers in the docstrings for the new types to provide more context. I've also added myself to the contributors list :)

The remaining task is the implementation of Python bindings. I'm not very familiar with Boost.Python, so I'll give it a try but might need some help if you can offer it.

Please let me know if anything else is needed or if there are further improvements that can be made.

Thanks so much for your time and feedback!

[simeon-ned]

Hello @jorisv,

I've made progress on addressing the issues you mentioned in the C++ part of the code and updated the related unit tests. However, I'm still facing challenges with modifying the Python bindings. Here's what I've done so far:

1. Added new classes in include/pinocchio/bindings/python/spatial:

   • LogCholeskyParametersPythonVisitor
   • PseudoInertiaPythonVisitor

2. Added related methods in InertiaPythonVisitor

The new methods are now visible in pin.Inertia, but it seems that PseudoInertia and LogCholesky are not being exposed properly.

Could you please provide some help with that? Thank you!

[lvjonok]

Hello @jorisv,

I will take over this PR moving forward.

The CI seems to be running, and I have addressed most of your previous comments. However, I have just discovered that the bindings are broken, I will update it shortly.

I also have a few open questions that I hope you can help me with:

• There is a specific function FromDynamicParameters. For InertiaTpl, it is defined as a template over Vector10, but we haven't introduced it for PseudoInertiaTpl and LogCholeskyTpl. Should this be updated to maintain consistency across these classes?
• In a previous discussion, you mentioned:

  If you want to add this visitor, you need to implement a cast method as in: https://github.com/stack-of-tasks/pinocchio/blob/master/include/pinocchio/spatial/inertia.hpp#L867-L874

  • Could you elaborate on why we might need this visitor and in what cases it would be necessary?

Looking forward to your feedback!

Best regards, Lev Kozlov

[lvjonok]

Just a quick update. I have managed to fix the python bindings and it works well in testing. While doing that I have partially answered the first question about template for VectorLike10.

From my understanding, it helps to cover these casts when dealing with python bindings and eigenpy.

Other than that, I think I will just wait for the feedback from now. I believe I have completed most of the checkpoints for this PR to be ready :)

[simeon-ned]

@lvjonok Thank you so much for your help with this pull request! It's been invaluable.

@jorisv @jcarpent When you have a moment, could you please take a look at our PR? I believe we've addressed most of the issues you mentioned.

[jorisv]

Hello @simeon-ned and @lvjonok,

There is a specific function FromDynamicParameters. For InertiaTpl, it is defined as a template over Vector10, but we haven't introduced it for PseudoInertiaTpl and LogCholeskyTpl. Should this be updated to maintain consistency across these classes?

I don't think it's mandatory to have consistency between InertiaTpl, PseudoInertiaTpl and LogCholeskyTpl. Since we don't plan to be able to replace one of these type by another, we don't really need to have a common set of methods between them. If having a FromDynamicParameters implementation is useful for you, you can implement it. Otherwise, we don't need it.

In a previous discussion, you mentioned: If you want to add this visitor, you need to implement a cast method as in: https://github.com/stack-of-tasks/pinocchio/blob/master/include/pinocchio/spatial/inertia.hpp#L867-L874 Could you elaborate on why we might need this visitor and in what cases it would be necessary?

The cast visitor allow to cast to a different scalar type. It's useful if you want to convert an InertiaTpl made of double to an InertiaTpl made of casadi/cppad/cppadcg/boost.mpf scalar. I think is still a good idea to support it.

[jorisv]

I will work on the crash issue since it also affect InertiaTpl.

If you want I can also address myself the last issues since it's really minor.

[lvjonok]

I will work on the crash issue since it also affect InertiaTpl.

If you want I can also address myself the last issues since it's really minor.

Yes, sure. Thanks for explanations, it really matters

[jorisv]

All green we can merge.

@simeon-ned, @lvjonok once again, thank you for your contribution !