TSID for "pantograph/biarticular" robot leg design?

[egordv]

Hello and thank you for a great tool! It works perfectly for control of a classic rigid robot.

Right now I'm wondering what it will take to use the TSID to control a robot with an a-la biarticular leg design (something like the one mentioned here) :

I can see three main difficulties:

1. Closed kinematic chain
2. Passive joint
3. Spring element

@andreadelprete can you please guide the right way to implement the missing features for this task (or point me that it's almost impossible in TSID framework)?

1. It looks like the pinocchio3 version is heading to support the loop constraints. Is there any plans to integrate the pinocchio3 in TSID?
2. Without pinocchio3 support, as an approximate solution of closed kinematic chain with a passive joint problem, instead of using the passive joint I can remove it and get a sort of solution by implementing a new task, something like "taskTwoFramesPositionEquality", and use it with a high gain to force the solver to keep the end of two links as close as possible, emulating the passive joint in a closed kinematic loop. Will it do the job or lead to solver failure due to over-constrained design or high-gain-induced stability problems?
3. Spring element is a mystery for me. I can model it as a prismatic/rotational joint, but what is the proper way to take into account the state-dependent spring dynamics generating the actuation in that additional joint?

[andreadelprete]

Dear @egordv , thank you for the interesting questions. I'm glad you enjoy TSID. I think it should be possible to make TSID work with this kind of robots.

1. It looks like the pinocchio3 version is heading to support the loop constraints. Is there any plans to integrate the pinocchio3 in TSID?

Yes. I think we'll integrate pinocchio3 in TSID when it is officially released.

2. Without pinocchio3 support, as an approximate solution of closed kinematic chain with a passive joint problem, instead of using the passive joint I can remove it and get a sort of solution by implementing a new task, something like "taskTwoFramesPositionEquality", and use it with a high gain to force the solver to keep the end of two links as close as possible, emulating the passive joint in a closed kinematic loop. Will it do the job or lead to solver failure due to over-constrained design or high-gain-induced stability problems?

If you need to model a rigid closed loop, your idea is correct, expect that you should model it with a contact (rather than a task). Currently, contacts only exist for modeling contacts between the robot and the environment. You would need to define a new contact class for contacts between two links of the robot. This should work well in practice, and I can guide you if you need some help.

3. Spring element is a mystery for me. I can model it as a prismatic/rotational joint, but what is the proper way to take into account the state-dependent spring dynamics generating the actuation in that additional joint?

If you have a spring-damper in the closed kinematic chains (as in your fig. B), then the chains are not really closed because the spring-damper allows them to move. In this case the force generated by the spring-damper can be simply computed at each iteration and added to the nonlinear forces (gravity, Coriolis, centrifugal) in the robot dynamics. This will make TSID treat these extra forces in the same way it treats gravity: namely something to compensate for (as long as it has enough motor torques to do so). In other words: it won't be very smart about it, so I'm not sure the resulting behavior is gonna be what you expect, depending on the situation.

Ideally the computation of these spring forces should happen inside Pinocchio, but I don't think Pinocchio currently supports parallel spring-dampers in the kinematic chain, so you should implement this in TSID, maybe using a custom RobotWrapper. If the spring stiffness is too high (>10'000), you might have numerical issues, similarly to what we observed when playing with elastic contact models in [1]. If you're interested in the high-stiffness case, you may need to look into numerical integration techniques that can handle it well, such as [2].

[1] Reactive Balance Control for Legged Robots under Visco-Elastic Contacts, https://andreadelprete.github.io/publication/2020_applsci_flayols/ [2] Exponential Integration for Efficient and Accurate Multibody Simulation with Stiff Viscoelastic Contacts, https://andreadelprete.github.io/publication/2022_mbsd_hammoud/

[egordv]

Hello @andreadelprete! Thank you for such a quick and detailed answer! Glad to hear that with some effort the solution is possible. As a starting point I will try to implement a new contact class for two links passive joint as you advised above. I will keep you informed about my progress (or problems :) here.

[egordv]

Hello @andreadelprete!

I managed to implement the idea mentioned above in task N2 in TSID devel branch, and can do a PR if needed, but it's look like it's only a half of the solution.

Some details: I created a new contact class named "ContactTwoFrames", using a new motion task named "TaskTwoFramesEquality", and checked everything in simulation. It seem to work as expected, BUT in case of an fully-actuated rigid closed loop.

For example, if I use this new contact type on a classic humanoid to create a virtual spherical linkage between the robot's wrists - it's work as expected, the robot keeps it's wrists "glued" emulating a passive joint between them. To perform this task the humanoid is using all it's arms actuators.

But the problem is that this approach is useless even in simple pantograph leg scenario mentioned above. A minimalistic pantograph structure contains one actuated joint (let's say it's a "Knee joint" on figure A above) and three unactuated (passive) joints (other joints on figure A). One of these passive joints can be my "virtual passive joint". I planned to simulate other passive joints as a actuated joints with zero torque limit, but this hack seems to ruin the solution and robot starts to do weird movements.

Is there any straightforward way to add some joints as a passive joints to the unactuated part of the dynamics alongside with the floating base in TSID formulation? Or how to set the "passiveness" of a joint (in a selector matrix etc)?

[andreadelprete]

At the moment there's no way to specify which joints are passive in TSID, expect for the floating base, but it's a feature that can be added. I would first introduce this information in the RobotWrapper. Then you need to use this in the InverseDynamicsFormulation class. Probably the easiest way to handle this is to add an equality constraint for each passive joint, forcing the associated torque to zero. Torques are not decision variables however, so you would have to express them as functions of accelerations and forces, as it's already done for the base dynamics here. Basically, instead of simply selecting the top 6 rows of M, h and J to create the base-dynamics equality constraints, we should select the rows corresponding to the passive joints, which could be located anywhere inside the kinematic chain.

Another, less clean, way to achieve the same result would be to implement the TaskActuationEquality and use it to force the passive joint torques to zero.

[andreadelprete]

This issue has been inactive for a while now, so I close it, but feel free to re-open it if you need to.

[RVSagar]

Hi @egordv, sorry to add a comment to this closed issue, but I was wondering whether you have a (Python) example of this scenario you mentioned above in a comment:

For example, if I use this new contact type on a classic humanoid to create a virtual spherical linkage between the robot's wrists - it's work as expected, the robot keeps it's wrists "glued" emulating a passive joint between them. To perform this task the humanoid is using all it's arms actuators.

I have a scenario where I want to carry a box with a humanoid and wanted to use the new PR #218 you made. I've pulled the changes into my repo and tried the TwoFramesEquality task (Python bindings) between two palm links of my humanoid (PAL REEM-C) but am met with a seg fault. I haven't debugged further, I will report back if I figure out the issue but I thought I'd reach out and ask anyway.

Thanks! Look forward to the PR getting merged, it looks very nice.