Momentum regressor

[lvjonok]

Hello Pinocchio Team!

I have previously contributed kinetic and potential energy regressors, which are useful for system identification and adaptation. Now, it is time to extend this with momentum regressors.

Derivation Overview

Below is a quick derivation to support my code:

1. The mass inertia matrix in regressor form can be expressed as:

$$ M(q)\dot{v} = Y(q, 0, \dot{v})\pi - Y(q, 0, 0)\pi $$

2. The momentum of the system in regressor form can be written as:

$$ p = M(q) v = \frac{\partial L}{\partial v} = Y(q, 0, v)\pi - Y(q, 0, 0)\pi = Y_p(q, v) \pi $$

Next, we consider the derivative of the momentum:

$$ \dot{p} = \frac{d}{dt} \left( \frac{\partial L}{\partial v} \right) = \frac{\partial L}{\partial q} + Q $$

For system identification, we can construct the following equation:

$$ (Y_p(q_2, v_1) - Y_p(q_1, v_1)) \pi = \int_{t_1}^{t_2} \left( \frac{\partial L}{\partial q}\frac{\partial q}{\partial \theta} \pi + Q \right) dt $$

Changes Introduced in the PR

In my PR, I have augmented the data structure with two members: data.momentumRegressor and data.dpartial_lagrangian_q. These fields will be populated after the call to computeMomentumRegressor.

I am looking forward to your review and am open to any questions or comments!

Additionally, I am excited to gain experience writing C++ code. If there are any changes needed, I would prefer to make them myself rather than having you do it, as it would be a great learning opportunity for me. 😊

[hrp2-14]

Hi ! This project doesn't usually accept pull requests on the main branch.
If this wasn't intentionnal, you can change the base branch of this PR to devel
(No need to close it for that). Best, a bot.

[github-actions]

👋 Hi,
This is a reminder message to assign an extra build label to this Pull Request if needed.
By default, this PR will be build with minimal build options (URDF support and Python bindings)
The possible extra labels are:

• build_collision (build Pinocchio with coal support)
• build_casadi (build Pinocchio with CasADi support)
• build_autodiff (build Pinocchio with CppAD support)
• build_codegen (build Pinocchio with CppADCodeGen support)
• build_extra (build Pinocchio with extra algorithms)
• build_mpfr (build Pinocchio with Boost.Multiprecision support)
• build_sdf (build Pinocchio with SDF parser)
• build_accelerate (build Pinocchio with APPLE Accelerate framework support)
• build_all (build Pinocchio with ALL the options stated above)

Thanks.
The Pinocchio development team.

[Cfather]

Hi, @lvjonok,

I am very interested in your work! May I ask a few questions here?

1. What is the definition of $Q$ here?
2. What is the definition of $\theta$ here?
3. For data.dpartial_lagrangian_q, is it equal to $\frac{\partial L}{\partial q}$ in your fourth equation? In that instance, how to compute $\frac{\partial L}{\partial q} \frac{\partial q}{\partial \theta}$?
4. I guess the computation relationship would be clearer if you can provide a unit test for data.dpartial_lagrangian_q?

I appreciate your response in advance!

[lvjonok]

Hello @Cfather, unfortunately some of your equations didn't display properly, but I will try to address your questions!

1. Q is generalized force from Euler-Lagrange equation
2. $\theta$ is a stacked vector of dynamical parameters (ordinary form with mass, mass * CoM, inertia)
3. Yes, it is really confusing, actually data.dpartial_lagrangian_q is

$$\frac{\partial L}{\partial q}\frac{\partial q}{\partial \theta}$$

I think I better rename this variable.

4. I think this variable is tested in test_momentum_regressor already because it is computed inside the function. I have found it hard to come up with some example of exact data.dpartial_lagrangian_q numerical test.

Thanks for your interest! Hope I have answered the questions!

[Cfather]

Hello @Cfather, unfortunately some of your equations didn't display properly, but I will try to address your questions!

1. Q is generalized force from Euler-Lagrange equation
2. θ
   is a stacked vector of dynamical parameters (ordinary form with mass, mass * CoM, inertia)
3. Yes, it is really confusing, actually data.dpartial_lagrangian_q is

∂ L ∂ q ∂ q ∂ θ

I think I better rename this variable.

4. I think this variable is tested in test_momentum_regressor already because it is computed inside the function. I have found it hard to come up with some example of exact data.dpartial_lagrangian_q numerical test.

Thanks for your interest! Hope I have answered the questions!

Hi, @lvjonok Thanks a lot for your quick response! Could I ask a few follow-up questions:

1. Sorry I'm not that familiar with the Euler-Lagrange formulation. Does "generalized force" refer to the external torque, for example, from the robot motors? And the "generalized force" is not a function of dynamical parameters $\theta$.
2. For data.dpartial_lagrangian_q, why does it have $\frac{\partial q}{\partial \theta}$ inside? Do you mean the joint angles $q$ is a function of $\theta$?
3. Using the last equation you provided in your original post, I guess you can perform system identification without having to estimate acceleration. Have you tried this method on any hardware platforms in the real world yet? Did you encounter any issues in general?

Again, I appreciate your help in advance!

[lvjonok]

Hello @Cfather, unfortunately some of your equations didn't display properly, but I will try to address your questions!

1. Q is generalized force from Euler-Lagrange equation
2. θ
   is a stacked vector of dynamical parameters (ordinary form with mass, mass * CoM, inertia)
3. Yes, it is really confusing, actually data.dpartial_lagrangian_q is

∂ L ∂ q ∂ q ∂ θ
I think I better rename this variable.

4. I think this variable is tested in test_momentum_regressor already because it is computed inside the function. I have found it hard to come up with some example of exact data.dpartial_lagrangian_q numerical test.

Thanks for your interest! Hope I have answered the questions!

Hi, @lvjonok Thanks a lot for your quick response! Could I ask a few follow-up questions:

1. Sorry I'm not that familiar with the Euler-Lagrange formulation. Does "generalized force" refer to the external torque, for example, from the robot motors? And the "generalized force" is not a function of dynamical parameters
   θ
   .

2. For data.dpartial_lagrangian_q, why does it have

     ∂
     q
   
   
     ∂
     θ
   

   inside? Do you mean the joint angles
   q
   is a function of
   θ
   ?

3. Using the last equation you provided in your original post, I guess you can perform system identification without having to estimate acceleration. Have you tried this method on any hardware platforms in the real world yet? Did you encounter any issues in general?

Again, I appreciate your help in advance!

1. It is external force (motor torques), yes, it does not depend on the configuration or velocity of dynamical system.
2. data.dpartial_lagrangian_q is designed to keep variation of robot configuration q with respect to parameters $\frac{\partial q}{\theta}$ to produce regressor form linear with respect to dynamical parameters. I am not sure I have answered this question completely, but feel free to ask more!
3. Exactly! I am quite interested in system identification and adaptation. I am basically implementing some features from my thesis here :) Last time I did energy regressors: Kinetic and potential energy regressors #2282 and now it is time for momentum one. I didn't have a chance to perform some real data experiments, only simulation ones. However, the result is quite promising because indeed it does not require you to measure acceleration. I think you can take a look at my bachelor thesis presentation, it may not be a top notch one, but I did my best: https://lvjonok.quarto.pub/bach-thesis-physically-consistent-energy-regressor-based-adaptive-laws/#/title-slide

If you are interested in system identification and adaptation algorithms development and research, I think we can get in touch and discuss more! I am open for collaboration!

[Cfather]

Hi, @lvjonok , I have tried to generate a unit test for data.dpartial_lagrangian_q. But the result does not match so I'm not sure if I understand the concepts correctly.
In this instance, you would need a manipulator model instead of a humanoid model with floating base, so that the trajectories can be easily parameterized with time.

  pinocchio::Model model;
  buildModels::manipulator(model);

  // define a random trajectory for all joints
  const Eigen::VectorXd amplitude = Eigen::VectorXd::Random(model.nv);
  auto positions = [](Eigen::VectorXd amplitude, double time) -> Eigen::VectorXd {
    return amplitude * std::cos(time);
  };
  auto velocities = [](Eigen::VectorXd amplitude, double time) -> Eigen::VectorXd {
    return -amplitude * std::sin(time);
  };
  auto accelerations = [](Eigen::VectorXd amplitude, double time) -> Eigen::VectorXd {
    return -amplitude * std::cos(time);
  };

  const double time_step = 1e-8;
  double t = std::rand() / (double)RAND_MAX;
  double t_plus = t + time_step;
  double t_minus = t - time_step;

  const Eigen::VectorXd q_test = positions(amplitude, t);
  const Eigen::VectorXd v_test = velocities(amplitude, t);
  const Eigen::VectorXd a_test = accelerations(amplitude, t);
  const Eigen::VectorXd q_plus = positions(amplitude, t_plus);
  const Eigen::VectorXd v_plus = velocities(amplitude, t_plus);
  const Eigen::VectorXd q_minus = positions(amplitude, t_minus);
  const Eigen::VectorXd v_minus = velocities(amplitude, t_minus);

  // compute momemtum at each time instance
  computeMomentumRegressor(model, data, q_test, v_test);
  const Eigen::VectorXd momentum_test = data.momentumRegressor * params;
  const Eigen::MatrixXd dpartial_lagrangian_q_test = data.dpartial_lagrangian_q;

  computeMomentumRegressor(model, data, q_plus, v_plus);
  const Eigen::VectorXd momentum_plus = data.momentumRegressor * params;

  computeMomentumRegressor(model, data, q_minus, v_minus);
  const Eigen::VectorXd momentum_minus = data.momentumRegressor * params;

  // compute generalized torque according to the trajectories
  rnea(model, data, q_test, v_test, a_test);
  const Eigen::VectorXd tau = data.tau;

  // compute the time derivative of momentum analytically and numerically
  const Eigen::VectorXd momentum_time_derivative_analytical = 
    dpartial_lagrangian_q_test * params + tau;
  const Eigen::VectorXd momentum_time_derivative_numerical = 
    (momentum_plus - momentum_minus) / (2 * time_step);

  // theoretically, the two should be approximately equal
  std::cout << "analytical: " << momentum_time_derivative_analytical.transpose() << std::endl;
  std::cout << "numerical: " << momentum_time_derivative_numerical.transpose() << std::endl;

Please let me know if there're any issues in this unit test. I appreciate your review in advance!

[lvjonok]

Hi, @lvjonok , I have tried to generate a unit test for data.dpartial_lagrangian_q. But the result does not match so I'm not sure if I understand the concepts correctly. In this instance, you would need a manipulator model instead of a humanoid model with floating base, so that the trajectories can be easily parameterized with time.

Thanks! I will look into this in a couple of days!

[pwensing]

Nice work here, guys! I wanted to offer a quick comment regarding how you might broaden the use cases for these algorithms.

I understand that the motivation for this algorithm comes from the fact that, when working with generalized coordinates, the generalized momentum satisfies:
$\dot{p} = \tau + \frac{\partial L}{\partial q}$

However, you can show with a little work that:
$\frac{\partial T}{\partial q} = C^T \dot{q}$
where $C$ is any Coriolis factorization satisfying the usual skew property $\dot{H} = C+C^T$.

In this regard, the generalized momentum evolves as: $\dot{p} = \tau + C^T \dot{q} - g$, where $g = \nabla V$ in this case.

This expression for $\dot{p}$ using $C^T \dot{q}$ also works when using generalized speeds (like you need with floating bases) in the sense that for equations: $H \dot{v} + C v + g = \tau$ and $p = H v$ you still have:
$\dot{p} = \tau + C^T v - g$

In this respect, I think it could broaden the use cases if you instead provide the regressor $\frac{\partial}{\partial \theta} [C^T v - g]$. In case it helps, you can find some relevant code (that also works with floating bases) at the following links: recursive regressor method and recursive method for p_dot. Both are based on this paper 10.1115/1.4007557 by Hanlei Wang.

[lvjonok]

Nice work here, guys! I wanted to offer a quick comment regarding how you might broaden the use cases for these algorithms.

I understand that the motivation for this algorithm comes from the fact that, when working with generalized coordinates, the generalized momentum satisfies: p ˙ = τ + ∂ L ∂ q

However, you can show with a little work that: ∂ T ∂ q = C T q ˙ where C is any Coriolis factorization satisfying the usual skew property H ˙ = C + C T .

In this regard, the generalized momentum evolves as: p ˙ = τ + C T q ˙ − g , where g = ∇ V in this case.

This expression for p ˙ using C T q ˙ also works when using generalized speeds (like you need with floating bases) in the sense that for equations: H v ˙ + C v + g = τ and p = H v you still have: p ˙ = τ + C T v − g

In this respect, I think it could broaden the use cases if you instead provide the regressor ∂ ∂ θ [ C T v − g ] . In case it helps, you can find some relevant code (that also works with floating bases) at the following links: recursive regressor method and recursive method for p_dot. Both are based on this paper 10.1115/1.4007557 by Hanlei Wang.

Thanks for this comment! I really appreciate you explaining the concept from this perspective.

My initial motivation was that derivation of $p' = \tau + \frac{\partial L}{\partial q}$ can be done using already implemented jointTorqueRegressor which greatly simplified derivation process for me back then. However, your proposal totally makes sense since it is more general. I will definitely look into this, maybe it will not be done this PR, but it is on my TODO list from now. Either way I can later check that both approaches lead to the same result.

[lvjonok]

Hi, @lvjonok , I have tried to generate a unit test for data.dpartial_lagrangian_q. But the result does not match so I'm not sure if I understand the concepts correctly. In this instance, you would need a manipulator model instead of a humanoid model with floating base, so that the trajectories can be easily parameterized with time.

  pinocchio::Model model;
  buildModels::manipulator(model);

  // define a random trajectory for all joints
  const Eigen::VectorXd amplitude = Eigen::VectorXd::Random(model.nv);
  auto positions = [](Eigen::VectorXd amplitude, double time) -> Eigen::VectorXd {
    return amplitude * std::cos(time);
  };
  auto velocities = [](Eigen::VectorXd amplitude, double time) -> Eigen::VectorXd {
    return -amplitude * std::sin(time);
  };
  auto accelerations = [](Eigen::VectorXd amplitude, double time) -> Eigen::VectorXd {
    return -amplitude * std::cos(time);
  };

  const double time_step = 1e-8;
  double t = std::rand() / (double)RAND_MAX;
  double t_plus = t + time_step;
  double t_minus = t - time_step;

  const Eigen::VectorXd q_test = positions(amplitude, t);
  const Eigen::VectorXd v_test = velocities(amplitude, t);
  const Eigen::VectorXd a_test = accelerations(amplitude, t);
  const Eigen::VectorXd q_plus = positions(amplitude, t_plus);
  const Eigen::VectorXd v_plus = velocities(amplitude, t_plus);
  const Eigen::VectorXd q_minus = positions(amplitude, t_minus);
  const Eigen::VectorXd v_minus = velocities(amplitude, t_minus);

  // compute momemtum at each time instance
  computeMomentumRegressor(model, data, q_test, v_test);
  const Eigen::VectorXd momentum_test = data.momentumRegressor * params;
  const Eigen::MatrixXd dpartial_lagrangian_q_test = data.dpartial_lagrangian_q;

  computeMomentumRegressor(model, data, q_plus, v_plus);
  const Eigen::VectorXd momentum_plus = data.momentumRegressor * params;

  computeMomentumRegressor(model, data, q_minus, v_minus);
  const Eigen::VectorXd momentum_minus = data.momentumRegressor * params;

  // compute generalized torque according to the trajectories
  rnea(model, data, q_test, v_test, a_test);
  const Eigen::VectorXd tau = data.tau;

  // compute the time derivative of momentum analytically and numerically
  const Eigen::VectorXd momentum_time_derivative_analytical = 
    dpartial_lagrangian_q_test * params + tau;
  const Eigen::VectorXd momentum_time_derivative_numerical = 
    (momentum_plus - momentum_minus) / (2 * time_step);

  // theoretically, the two should be approximately equal
  std::cout << "analytical: " << momentum_time_derivative_analytical.transpose() << std::endl;
  std::cout << "numerical: " << momentum_time_derivative_numerical.transpose() << std::endl;

Please let me know if there're any issues in this unit test. I appreciate your review in advance!

I am still in the process of investigation, but one thing I have figured out is forwardKinematics(model, data_ref, q_test, v_test); is missing for proper dpartial_lagrangian_q calculation. It causes spatial velocity to be zero.

However even with this changes I was not able to successfully fix your proposed test. I have reached something like:

analytical: -6.93615 -32.9158  3.87874 -11.6978 -5.25708 -4.77154
numerical: -4.97148 -20.7156 0.822588 -9.45094   -3.466 -3.29854

Which is closer, but I will look more.