Kinematics tree root and rnea()

[Gregwar]

Hello,

Suppose, on an humanoid robot standing on one foot, I have the robot state q and v, and a target a acceleration for the joints.

If I want to know which torques woult it require to achieve such an acceleration on the joints I can use the inverse dynamics by calling rnea().

However, the robot kinematic root (and thus the floating base) is attached to the trunk of the robot. The only external force acting on the robot is the gravity, so if the support foot was the root of the kinematic tree, rnea() would give a direct result.

Is there a way to change the root on the fly ?
If not, what would you do to compute this ?

[jcarpent]

Hi @Gregwar,

It seems you want to solve an inverse dynamics problem, meaning that you want to find the control actuation (torque) together with the contact forces to apply on the system in order to susten its weight. This called task-space inverse dynamics in the robotics jargon. I would suggest you having a look at https://github.com/stack-of-tasks/tsid, which builds on top of Pinocchio a dedicated solver for this kind of problem.

[Gregwar]

Thanks, I will have a look at that.

I do understand that in the general case (involving multiple supports) this is a complicated problem.
However, is there an answer to my above question, since in single support case it would just be a change of the robot tree root, isn't it?

[jcarpent]

Yes, indeed. You're correct. It won't be so hard to implement an algorithm that change the root kinematic tree.

[Gregwar]

It seems that it would be useful from an API point of view (and apparently other people are pushing in this direction)

However I perfectly understand that everything could be done without changing the root of the tree. I'm curious what would be the steps in that particular case.

[jcarpent]

If you want to give a try, I may provide feedback on top of your PR.

[Gregwar]

I'm not sure I want to give a try to implementing this now since I'm new to the code base

What I meant is that I'm curious how it would be to compute torques without changing the floating base. But I agree that be able to change floating base would be a good feature.

[jmirabel]

You could also achieve this without changing the current code.

1. compute the centroidal inertial forces due to the acceleration you want.
2. apply newton law on the full robot (inertial forces + weight + reaction force = 0)
3. check that the reaction force is in a friction cone, otherwise, your robot falls.
4. apply RNEA with the reaction force computed above.

[Gregwar]

@jmirabel ok thanks

Sorry if I misunderstood, but imagine that:

1. All the mass is in the support foot
2. We want to bend the legs

In that case, computing the centroidal forces will give an upward force (because it is computed in the trunk of the robot, the foot appear to be lifting). To exagerate, imagine that this inertial force is the same as weight, the reaction forces will then vanishes. However, I would expect to get zero torque since we want to move the mass-less top part of the robot. What am I getting wrong?

[jmirabel]

If you apply what I told you and you get:

• a pulling reaction force, then your robot will jump.
• a force outside the friction cone, then your robot will slip.

Try yourself to bend the your leg. If you do it fast, then your leg will momentarily loose contact with the ground. So what you say is expected.

[Gregwar]

Ok, I understand that, but if the reaction force is zero, what is the difference between controlling a robot on a leg and an robotic arm?
I guess you also need to feed the desired floating base acceleration when calling rnea, isn't it?

[jmirabel]

I guess you also need to feed the desired floating base acceleration when calling rnea, isn't it?

You can compute it by assuming a rigid contact of the foot on the ground.

if the reaction force is zero

In the beginning, the reaction force will be opposite to the gravity. When bending your leg, the reaction force will first decrease. Then you have two cases:

• it reaches zero and the dynamic of your system changes. You cannot assume a rigid contact anymore. Instead, you have the dynamic of an articulated falling object.
• at some point it starts increasing again.

The difference between a legged robot and a manipulator arm is that the dynamics for the former can change while it can't for the latter.

[jmirabel]

If you aren't confident with what I told you, then I second @jcarpent suggestion to have a look at TSID.

[Gregwar]

I am confident with what you said, and I totally agree on using TSID since it looks like a great framework that solves much more complicated problems in the general case.

Still, do you agree that your method also implies feeding rnea() with desired acceleration for the floating base universal joint?

[jmirabel]

Still, do you agree that your method also implies feeding rnea() with desired acceleration for the floating base universal joint?

Yes, you have to provide it to RNEA.

[mehdi-benallegue]

@jmirabel ok thanks

Sorry if I misunderstood, but imagine that:

1. All the mass is in the support foot

2. We want to bend the legs

In that case, computing the centroidal forces will give an upward force (because it is computed in the trunk of the robot, the foot appear to be lifting). To exagerate, imagine that this inertial force is the same as weight, the reaction forces will then vanishes. However, I would expect to get zero torque since we want to move the mass-less top part of the robot. What am I getting wrong?

I would just add that this example is not good because you cannot use forward dynamics with massless objects, and thus inverse dynamics is meaningless.

Also, if you have a bilateral contact (a contact where we can pull, push and apply basically any force) then your dynamics would be the same as a fixed base manipulator robot.

[Gregwar]

I would just add that this example is not good because you cannot use forward dynamics with massless objects, and thus inverse dynamics is meaningless.

This is just a checking example, think of epsilon masses everywhere except in foot; you expect the torques to be epsilon as well to actuate the upper body of the robot (and thus the result of rnea to be epsilon torques with the input you will feed it)

Also, if you have a bilateral contact (a contact where we can pull, push and apply basically any force) then your dynamics would be the same as a fixed base manipulator robot.

I agree, only with a different root in the kinematic tree (which is the origin of this issue)

We can continue to talk but I guess I can close the issue itself

[mehdi-benallegue]

This is just a checking example, think of epsilon masses everywhere except in foot; you expect the torques to be epsilon as well to actuate the upper body of the robot (and thus the result of rnea to be epsilon torques with the input you will feed it)

The forces computed by the centroidal dynamics are related to the acceleration of the center of mass, but in your example, the CoM is basically in the foot, so bending the knee will not give it any acceleration, and thus the reaction forces are identical to those of a static robot and the torques will be epsilon as expected.

I agree, only with a different root in the kinematic tree (which is the origin of this issue)

We can control a manipulator by placing the root in any part of the robot, the only difference is that if it is not the fixed base, we are doomed to compute the intereaction forces at the fixation point. Therefore, I agree that the feature you request is interesting, but since I am not an active developer in the framework i won't be able to help more with that.

Good luck!

[jcarpent]

I am not an active developer in the framework i won't be able to help more with that.

Your contributions are of course more than welcome dear @mehdi-benallegue. I don't know which tools you are currently using at AIST?