Add Inverse dynamics class using TSID #68

EtienneAr · 2025-07-28T11:52:06Z

This PR is still in progress, here is my todo list before asking for any review. Skip this comment to have a clearer summary of this PR.

…pile

… compensate for base position task)

…sate for base pos)

…erence and performance (if needed)

…ficient

EtienneAr · 2025-09-29T14:30:45Z

This PR is (almost) ready for review. The only thing left is to wait for a new release of TSID (hopefully coming soon) in order to have the fix I made regarding adding and removing rigid contacts.

Here is a overview of the proposed changes :

Refactor RobotModelHandler and RoboDataHandler (mostly for feet anagment) :
- Now all the function concerning the feet are only indexed using the feet nb and not the feet name. This is for homogeneity concerns and efficiency (for the cases it is needed). The method getFootNb(std::string name) provide the index of the foot knowing it's name, if necessary.
- There are now 2 types of foot, point foot (which can be used to model a quadruped foot, for instance) and quad foot (which can be used to model a humanoid foot). The latest require passing 4 points defining the shape/size of the foot.
inverse Dynamics :
- I added two classes to perform inverse dynamics, one for kinodynamics settings and another one for a centroidal setting.
- Both class are extensively tested (each cost/task is tested separately and one test integrate everything together, both on quadruped and biped robots)
- Design of ID classes:
  - Each class contains a Settings struct containing all the necessary params to initialize it. Mostly costs and weight for each of the tasks. (Note: if a weight is set to a negative value, which is the default value, the the task is ignored)
  - Each class then have a:
    - setTarget member that takes as arguments pieces of the MPC output, and compute/set all the setpoints of the tasks
    - solve member, that takes the current state as argument and return the torque
- All the python examples have been updated to include those ID as demonstration
  - Note: while I have been able to tune the centroidal ID in a static setting for the Talos humanoid robot, I haven't been able to tune it to work with the MPC, so the example explode... However everything has been tested in this ID so I am confident it is not due to a bug of the ID.
  - I tried to clean / homogenize a bit the examples in the meantime

edantec

First draft of review, pointing some mistakes (mainly wrong signedness for int)
Also pointing bugs in test and examples

edantec · 2025-10-08T08:26:42Z

bindings/expose-inverse-dynamics.cpp

+      return a;
+    }
+
+    void setTarget_CentroidalID(


Why a _ dash here? It breaks naming conventions, isn't it?

edantec · 2025-10-08T08:59:09Z

examples/go2_kinodynamics.py

+        t = step * dt_mpc + sub_step * dt_simu
+
+        delay = sub_step / float(N_simu) * dt_mpc
+        xs_interp = interpolator.interpolateLinear(delay, dt_mpc, xss)


You have to use interpolateState to correctly interpolate the configuration, which is defined on a Lie group.

edantec · 2025-10-08T09:02:06Z

examples/talos_kinodynamics.py

+        t = step * dt_mpc + sub_step * dt_simu
+
+        delay = sub_step / float(N_simu) * dt_mpc
+        xs_interp = interpolator.interpolateLinear(delay, dt_mpc, xss)


Same comment: use interpolateState

edantec · 2025-10-08T09:08:46Z

include/simple-mpc/inverse-dynamics/centroidal.hpp

+  private:
+    std::shared_ptr<tsid::tasks::TaskComEquality> comTask_;
+    std::vector<tsid::tasks::TaskSE3Equality> trackingTasks_;
+    tsid::trajectories::TrajectorySample sampleCom_;                    // TODO: no need to store it


Can you remove the TODO?

edantec · 2025-10-08T09:09:47Z

include/simple-mpc/inverse-dynamics/centroidal.hpp

+    const Settings settings_;
+
+  private:
+    std::shared_ptr<tsid::tasks::TaskComEquality> comTask_;


Why use a shared_ptr for comTask but not for the trackingTasks? I am in favor of handling std::shared_ptr for all tsid tasks.

edantec · 2025-10-08T12:05:06Z

src/kinodynamics.cpp

    Motion v_ref = Motion::Zero();
    int i = 0;
-    for (auto const & name : model_handler_.getFeetNames())
+    for (int foot_nb = 0; foot_nb < model_handler_.getFeetNb(); foot_nb++)


change to size_t foot_nb

edantec · 2025-10-08T12:07:04Z

src/mpc.cpp

+    data_handler_->updateInternalData(model_handler.getReferenceState(), true);
    std::map<std::string, Eigen::Vector3d> starting_poses;
-    for (auto const & name : ocp_handler_->getModelHandler().getFeetNames())
+    for (int foot_nb = 0; foot_nb < model_handler.getFeetNb(); foot_nb++)


Change to size_t foot_nb

edantec · 2025-10-08T12:10:27Z

src/mpc.cpp

      if (contact_state[i])
      {
-        contact_forces.segment((long)i * 3, 3) = mc_data->constraint_datas_[force_id].contact_force.linear();
+        contact_forces.row(i) = mc_data->constraint_datas_[force_id].contact_force.linear();


contact_forces.row((long)i)

edantec · 2025-10-08T12:10:32Z

src/mpc.cpp

      else
      {
-        contact_forces.segment((long)i * 3, 3).setZero();
+        contact_forces.row(i).setZero();


contact_forces.row((long)i)

edantec · 2025-10-08T13:11:04Z

examples/talos_centroidal.py

This fails on my machine with the error Assertion `last_solution_.status == tsid::solvers::HQPStatus::HQP_STATUS_OPTIMAL' failed

earlaud added 30 commits July 23, 2025 20:33

Remove deprecated qp solvers

a74aa8e

Copy paste TSID implementation from go2_mpc-experiments

3be8eb2

Move ID setting in nested class

f1f7395

Remove un-necessary members of ID

aced2a6

Minor change to remove hardcoded base name from go2

99248f9

WiP: write ID test

7d2c35a

Test ID posture task

7da7d87

Enable base task

1822f56

WiP: properly add foot contacts in tsid

71c6cbf

Add contacts and base costs

2d8dfd3

Properly compute dry run data in ID

8ab667a

Contact now equality constraint and not cost

b96ddeb

Contact position computed with measured data, not target

50ff095

Add tsid to package xml

99808ba

Minor test changes

2072499

Do not add tasks if weight is LE than 0.

7836cca

Write ID contact test

4ce49aa

Make contact task cost not constraint

2ee11a9

Roughly tune contact weight in test

23fa376

Port contact tunning in default settings

81037cb

Skip first point in all tasks test

7a3cc32

FIX QUATERNION CONVENTION for target state

cd8cd5e

Make propper q_start for ID tests

70c7633

Adjust default ID tuning and adjust tests

57df367

Fix test all tasks

397d10d

Remove default values for ID gain and weights

f5d9bdd

Add baseTask test

e409e99

Add Pos Vel joint bounds

8a73a0d

Add actuation limit task

b221d1f

Add test on ID limits

535ce08

earlaud added 4 commits September 19, 2025 16:04

Set baseTask mask properly for centroidal

30bd6fb

Minor cleaning

912f3f0

Minor update of default ID parameters

746d709

Add ID test in cmakelists

6aab3e1

EtienneAr force-pushed the tsid branch from 96d366c to 6aab3e1 Compare September 19, 2025 15:50

earlaud added 19 commits September 22, 2025 19:13

Minor tuning in talos kino example

662fb8b

Homogenize centroidal and kino talos examples

c27cacf

Set of gains that works for STATIC centroidal

2059fab

Minor fix in talos centroidal example but overall still not working

092858d

copy paste kino test to centroidal

d4aec23

remove easy initilization of ID Settings and make centroidal test com…

5d3d789

…pile

Make centroidal contact point tests pass by adding small com cost (to…

e161e2b

… compensate for base position task)

Make centroidal baseTask test pass by adding small com task (to compe…

489f4fb

…sate for base pos)

Improve base orientation task test in centroidalID

e364e33

remove useless task in centroidal baseTask test

9b72b70

Add test on centroidalID com task

756e40e

Add centroidal foot tracking test

3a04d4d

Make setReferenceFootPlacement use foot index instead of name for coh…

66b9301

…erence and performance (if needed)

Fix benchmark compilation

d277faf

Rename parent frame ref for foot placement

5677470

Fix default reference foot frame placement

354c585

No need to set foot ref placement in examples & test, default one suf…

2e488fb

…ficient

Use getFeetNb instead of doing len() by hand

05d0689

Compatility with pinocchio 3

0982a50

EtienneAr marked this pull request as ready for review September 29, 2025 15:30

earlaud added 2 commits September 30, 2025 11:24

Update tsid to have fix and proxqp flag by default

4ba2990

Use tsid latest build to actually have proxqp

1eb20ff

EtienneAr requested a review from edantec October 7, 2025 18:22

edantec requested changes Oct 8, 2025

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

Add Inverse dynamics class using TSID #68

Add Inverse dynamics class using TSID #68

EtienneAr commented Jul 28, 2025 •

edited

Loading

Uh oh!

EtienneAr commented Sep 29, 2025 •

edited

Loading

Uh oh!

edantec left a comment

Uh oh!

edantec Oct 8, 2025

Uh oh!

edantec Oct 8, 2025

Uh oh!

edantec Oct 8, 2025

Uh oh!

edantec Oct 8, 2025

Uh oh!

edantec Oct 8, 2025

Uh oh!

edantec Oct 8, 2025

Uh oh!

edantec Oct 8, 2025

Uh oh!

edantec Oct 8, 2025

Uh oh!

edantec Oct 8, 2025

Uh oh!

edantec Oct 8, 2025

Uh oh!

Uh oh!

Add Inverse dynamics class using TSID #68

Are you sure you want to change the base?

Add Inverse dynamics class using TSID #68

Conversation

EtienneAr commented Jul 28, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

EtienneAr commented Sep 29, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

edantec left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

EtienneAr commented Jul 28, 2025 •

edited

Loading

EtienneAr commented Sep 29, 2025 •

edited

Loading