search

stack-of-tasks / pinocchio

A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
http://stack-of-tasks.github.io/pinocchio/
BSD 2-Clause "Simplified" License
1.67k stars 357 forks source link

Different inertia parametrizations and Physical consistency #2296

Open simeon-ned opened 2 weeks ago

simeon-ned commented 2 weeks ago

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:

  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 commented 2 weeks ago

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

jcarpent commented 2 weeks ago

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 commented 2 weeks ago

@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 commented 2 weeks ago

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 commented 1 week ago

@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 commented 1 week ago

@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!