Momentum regressor #2411

lvjonok · 2024-09-09T11:54:39Z

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:

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 $$

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 · 2024-09-09T11:54:44Z

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 · 2024-11-03T22:53:59Z

Hi, @lvjonok,

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

What is the definition of $Q$ here?
What is the definition of $\theta$ here?
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}$?
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 · 2024-11-04T00:50:57Z

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

Q is generalized force from Euler-Lagrange equation
$\theta$ is a stacked vector of dynamical parameters (ordinary form with mass, mass * CoM, inertia)
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.

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 · 2024-11-04T02:52:56Z

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

Q is generalized force from Euler-Lagrange equation

θ
is a stacked vector of dynamical parameters (ordinary form with mass, mass * CoM, inertia)

Yes, it is really confusing, actually data.dpartial_lagrangian_q is

∂ L ∂ q ∂ q ∂ θ

I think I better rename this variable.

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:

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$.
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$?
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 · 2024-11-04T03:58:30Z

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

Q is generalized force from Euler-Lagrange equation

θ
is a stacked vector of dynamical parameters (ordinary form with mass, mass * CoM, inertia)

Yes, it is really confusing, actually data.dpartial_lagrangian_q is

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

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:
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
θ
.
For data.dpartial_lagrangian_q, why does it have
  ∂
  q


  ∂
  θ
inside? Do you mean the joint angles
q
is a function of
θ
?
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!

It is external force (motor torques), yes, it does not depend on the configuration or velocity of dynamical system.
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!
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 · 2024-11-07T18:24:56Z

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 · 2024-11-07T23:34:52Z

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 · 2024-11-08T00:41:47Z

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 · 2024-11-08T01:20:31Z

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 · 2024-11-10T14:06:32Z

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.

lvjonok · 2024-11-17T07:02:19Z

I want to thank @pwensing for a great suggestion about implementation of indirect regressors rather my proposed approach. It really makes sense and I was able to successfully transfer the implementation to my PR.

I have struggled for some time and you can see it by comments where I was discovering the correspondence between notation in spatial_v2 and pinocchio, but unittests are passing!

As the next step, I will provide python bindings and a small example with system identification to showcase how regressors can be used!

Cfather · 2024-11-17T15:52:29Z

include/pinocchio/algorithm/regressor.hpp

+    const Eigen::MatrixBase<ConfigVectorType> & q,
+    const Eigen::MatrixBase<TangentVectorType> & v);
+
+  template<


Hi, could you please add a comment to explain the mathematical meaning of computeIndirectRegressors?

I think there is no mathematical meaning behind. It is just that the function in Wensing's implementation had this comment:

function [Y_Hqd, Y_CTqd, Y_Hqd_rot, Y_CTqd_rot] = Regressor_HqdandCTqd( model, q , qd) % Indirect regressors term YHqd a = H qd and YCTqd = C^T qd

Cfather · 2024-11-22T04:04:22Z

include/pinocchio/algorithm/regressor.hpp

+  /// of momentum and transposed coriolis matrix times velocity.
+  ///
+  /// These regressors are such that:
+  /// \f$ p = Y_Hqd(q, v) \pi$ and \f$ C^T v = Y_CTqd(q, v) \pi \f$


Just a very small nitpicking: does this include the gravity vector?
Like, should this be \f$ C^T v - g = Y_CTqd(q, v) \pi \f$?

No, it does not include gravity. In sysid example I provide a complete comparison and matching of these terms: https://github.com/stack-of-tasks/pinocchio/pull/2411/files#diff-de29e11f15faef229b4e025cc6c9f09af5d32c5268973e47484fc0b08afa6d77R154-R165

include/pinocchio/multibody/data.hpp

github-actions bot reviewed Sep 9, 2024

View reviewed changes

lvjonok changed the base branch from master to devel September 9, 2024 11:55

lvjonok added 2 commits September 9, 2024 14:26

feat: init implementation

fecf8f8

docs: add proper description

d5dcdbd

jcarpent force-pushed the master branch from 6054499 to d5dcdbd Compare September 9, 2024 12:26

lvjonok added 2 commits September 18, 2024 21:51

Merge branch 'devel' into master

df5edf7

Merge branch 'devel' into master

41a05d9

jcarpent self-assigned this Oct 15, 2024

Merge branch 'devel' into master

fa9f74c

feat: implement indirect regressors

24a888e

Cfather reviewed Nov 17, 2024

View reviewed changes

lvjonok added 2 commits November 18, 2024 14:14

feat: remove previous implementation

ba972a6

feat: add sysid example

569347b

Cfather reviewed Nov 22, 2024

View reviewed changes

include/pinocchio/multibody/data.hpp Outdated Show resolved Hide resolved

fix: update data attributes

2306714

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Momentum regressor #2411

Momentum regressor #2411

lvjonok commented Sep 9, 2024

hrp2-14 commented Sep 9, 2024

github-actions bot left a comment

Cfather commented Nov 3, 2024 •

edited

Loading

lvjonok commented Nov 4, 2024

Cfather commented Nov 4, 2024

lvjonok commented Nov 4, 2024 •

edited

Loading

Cfather commented Nov 7, 2024

lvjonok commented Nov 7, 2024

pwensing commented Nov 8, 2024

lvjonok commented Nov 8, 2024

lvjonok commented Nov 10, 2024

lvjonok commented Nov 17, 2024

Cfather Nov 17, 2024

lvjonok Nov 17, 2024

Cfather Nov 22, 2024

lvjonok Nov 22, 2024

Momentum regressor #2411

Are you sure you want to change the base?

Momentum regressor #2411

Conversation

lvjonok commented Sep 9, 2024

Derivation Overview

Changes Introduced in the PR

hrp2-14 commented Sep 9, 2024

github-actions bot left a comment

Choose a reason for hiding this comment

Cfather commented Nov 3, 2024 • edited Loading

lvjonok commented Nov 4, 2024

Cfather commented Nov 4, 2024

lvjonok commented Nov 4, 2024 • edited Loading

Cfather commented Nov 7, 2024

lvjonok commented Nov 7, 2024

pwensing commented Nov 8, 2024

lvjonok commented Nov 8, 2024

lvjonok commented Nov 10, 2024

lvjonok commented Nov 17, 2024

Cfather Nov 17, 2024

Choose a reason for hiding this comment

lvjonok Nov 17, 2024

Choose a reason for hiding this comment

Cfather Nov 22, 2024

Choose a reason for hiding this comment

lvjonok Nov 22, 2024

Choose a reason for hiding this comment

Cfather commented Nov 3, 2024 •

edited

Loading

lvjonok commented Nov 4, 2024 •

edited

Loading