A Few Small Problems In Example Scripts

[Cfather]

Hi, I would like to report a few (really) small bugs in the example scripts.

1. simulation-closed-kinematic-chains.py: line 161

   constraint_model.corrector.Kd = 2.0 * np.sqrt(constraint_model.corrector.Kp)

   doesn't work for me. I got the following error:

   RuntimeError: The number of elements does not fit with the vector type.

   I changed it to

   constraint_model.corrector.Kd[:] = 2.0 * np.sqrt(constraint_model.corrector.Kp)

   and it worked. not sure why I can't directly assign a numpy vector to it.

2. simulation-contact-dynamics.py: This is really small... Line 16-18 seems to call

   pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models")

   one more time. It might be unnecessary.

3. talos-simulation.py: Line 4:

   from example_robot_data import loadTalos

   seems to belong to an older version of example_robot_data? At least I installed example_robot_data through conda a few weeks ago and loadTalos is not there anymore. I changed it to:

   import example_robot_data

   and line 11 to:

   robot = example_robot_data.load("talos")

   and the code works. But the visualization seems to break. When I run gepetto-gui, there's nothing showing up in the window (just the GUI window, I went to 127.0.0.1 and there was nothing). There was following error from gepetto-gui:

   libpng warning: iCCP: known incorrect sRGB profile
   libpng warning: iCCP: known incorrect sRGB profile
   libpng warning: iCCP: known incorrect sRGB profile
   libpng warning: iCCP: known incorrect sRGB profile
   Error reading file /home/yuzhen/miniconda3/share/gepetto-viewer/fonts/arial.ttfQOpenGLContext::swapBuffers() called with non-exposed window, behavior is undefined

   The simulation works fine but I can not see the visualization. not sure if this is just problem on my side or if I used gepetto-gui incorrectly.

• OS: [Ubuntu 22.04]
• Pinocchio version: [3.0.0]

[Sang-SC]

I've been studying the simulation-closed-kinematic-chains.py example in the Pinocchio repository, which demonstrates the use of pin.constraintDynamics for forward dynamics simulation of a four-bar linkage closed-loop structure.

I have a question related to this: Is there a way to calculate constrained inverse dynamics in Pinocchio? I'm looking for functionality similar to the RigidBodyDynamics::InverseDynamicsConstraints function in the RBDL library.

Specifically, I'm interested in computing inverse dynamics for closed-loop systems like the four-bar linkage, taking into account the constraint forces. Is there an equivalent method or approach in Pinocchio for this type of calculation?

Any insights or guidance on this matter would be greatly appreciated. Thank you in advance for your help!

[Cfather]

One more things in talos-simulation.py.

Line 40-46 should be:

contact_model_lf1 = pinocchio.RigidConstraintModel(
        pinocchio.ContactType.CONTACT_6D,   
        robot.model,
        foot_joint_ids[j],
        robot.model.frames[frame_id].placement,
        0,
        data.oMf[frame_id],
    )

[olivier-stasse]

Dear @Sang-SC there is a dedicated inverse dynamics library based on pinocchio here: https://github.com/stack-of-tasks/TSID where you can use various solvers.

As for closed loop systems this would need a dedicated discussion.

[jcarpent]

@Cfather Could you open a PR with a fix? The examples might be indeed a little bit outdated, and the fix you have done are the good ones.

[Sang-SC]

@olivier-stasse Thank you very much!