Inverse/Forward Dynamics of a closed kinematic chain robot

[deepakr2210]

Context:
Hi, based on this example 4 bar code I tried to execute constraintDynamics() on my robot. The robot has a closed kinematic chain in its leg, forming a 4 bar linkage similar to Digit at the knee and ankle. (I have attached a pic for your reference). My main goal is to execute inverse dynamics on this.

Issue:

I am able to find only pin.ContactType.CONTACT_3D or pin.ContactType.CONTACT_6D to add as an constraint in pin.RigidConstraintModel().

Based on the example code, when I tried CONTACT_3D, the simulation becomes numerically unstable and it fails.
(I have tried playing around the simulation parameters, like the regularization, barrier weights and tolerances, and I could not get a robust soultion out of it)

When I use the CONTACT_6D, it rigidly constraints the loop and there is not DOF left for the knee joint to rotate.

Basically, I need it to sort of have a 5D constraint where it is free to move only in the axis of rotation of knee joint.

My codes:
close_loop_3d.txt
close_loop_6d.txt

Can you please tell me if I am heading in the right direction? or if I have made any mistakes in the code?

[jorisv]

Hello @deepakr2210,

You can try to split one link in two and add a CONTACT_6D constraint on it between his two support joints as shown in the following picture:

Also, you can maybe create a CONTACT_6D on J1 and add a revolute joint that support J1 to allow the joint to rotate.

Pinocchio 4 will add loop support in ABA algorithm.

[deepakr2210]

Hi, Sorry for the extremely delayed response.

I tried to implement the suggestions provided. But I am still facing a similar issue.

Implementation code - close_loop.py

Pinocchio version - 3.4.0

The 4th link is already made of 2 pieces in my robot. Below is my URDF tree structure:

└── hip_joint_right
    ├── knee_joint_right
    │   └── knee_foot_joint_right
    │       └── shock_a_right
    └── shock_a_joint_right

I have added the 6D constraint at the point where the 2 pieces are bolted together. I am achieving convergence for the KKT loop. But the entire 4 bar is rigidly held together when I apply a torque to the joints.

I also tried the second suggestion. "Also, you can maybe create a CONTACT_6D on J1 and add a revolute joint that support J1 to allow the joint to rotate." This too did not work.

I have not added any logs as the model is being built without any errors. And I am unable to query the constraint Jacobian to check its rank.