build dynamics model based on multi-chain

[qweesiic]

hi， now i need to build dynamics model for a humanoid robot,which has two arms of 7dofs 、 waist of 3dofs and head of 3dofs. how did i build entire dynamics model with kdl?

[qweesiic]

yeah, i just found someting new about kdl, such as 'treeidsolver_recursive_newton_euler.cpp', maybe this part can meet my needs?

[francofusco]

Hello, I am the author of that new solver. Yes, the solver can be used for the dynamics of a kinematic tree. Just note that there are few limitations:

1. It assumes a fixed base for the tree. If your humanoid robot does not have legs, this should not be a problem. Otherwise, I believe it would be necessary to add (up to) six "dummy joints" (3 prismatic + 3 revolute) to represent the motion of the base in the ambient space.
2. It operates on the whole tree; if you need to use it for a "sub-tree" you will need to add some code to extract it before creating the solver.

[guru-florida]

I am working on the same as @qweesiic. At the moment I am using ChainIdSolver_RNE to compute torques on the joints of a single leg. I put half the mass of the upper torso and arms as a wrench (newton force) into the first joint at the hip. I get sensible joint torques if I imagine the leg dangling in free space, but I need joint torques as if the foot is in contact with the floor and the mass of the robot is bearing down on these joints. That means there is the floor-force pushing up on the foot, and torso mass pushing down thus squeezing the inner joints. I can't figure out how to affix the foot joint/segment to emulate the "equal and opposite floor-force".

Do I need to reverse the chain so it is recursively solved from the foot up?...and the hip being the last joint with mass/2 wrench force attached.

Also, perhaps I should be using your TreeIdSolver to wholly calculate these forces. When I stumbled on the ChainId solver I assumed I had to break the robot into these separate parts.

Don't mean to hijack this issue, but I think we are concerned with similar things so felt it was worth keeping under one issue. FYI Orocos forum is quiet for 3 years or I'd have posted there.

[francofusco]

Unfortunately I do not know enough about the dynamics of humanoid robots, so my answer will be probably inadequate... :sweat_smile: However, here there are some ideas:

Do I need to reverse the chain so it is recursively solved from the foot up?...and the hip being the last joint with mass/2 wrench force attached.

If you know that the foot will always be fixed to the ground, that would probably be the easiest choice. The foot would be the root of your chain/tree and you can evaluate the dynamics for all the motors. However, you must verify "manually" that the reaction force exerted by the ground is physically feasible and meaningful. Depending on the configuration and the foot contact type, the required force might be infeasible, eg, a force pulling downward might be required for equilibrium. The only "easy" way I can think of to integrate information about the ground reaction forces is to add some dummy links/joints at the foot, whose axes are aligned with the principal axes of the foot frame. If you solve for the dynamics by keeping these joints all at zero position, velocity & acceleration, their joint forces should represent the reaction wrench exerted by the ground. Note that this is just a (more or less) educated guess, you would better check that what I am saying actually makes sense.

perhaps I should be using your TreeIdSolver to wholly calculate these forces. When I stumbled on the ChainId solver I assumed I had to break the robot into these separate parts.

Yes, I think that if you have multiple limbs you should evaluate the dynamics of the whole tree. For the moment, keep on working with the "single leg" simplification to see if you can achieve correct results. Later on you can try to consider the whole body using TreeIdSolver_RNE.

A final remark: when you will work with the complete bipedal robot, you could consider to use two different trees: one with the left foot as root, the other with the right one. In this way, when a step has been performed, you can simply switch between trees and, in principle, everything should be working. If there are phases in which both feet are in contact with the ground, then I believe that you should also start considering the constraints induced by the closed-loop formed by the two legs. Perhaps a solution based on Lagrange multipliers would do the the job, but as I said I have very limited knowledge in humanoid robots' dynamics and therefore this might be the wrong way of addressing the problem!

[guru-florida]

I think I have made it far enough along to answer my own questions now, so I will post here for the next wayward traveller. I had the wrong idea earlier, "affixing" an end-effector joint is somewhat non-sensical as far as Orocos IK/ID solvers. Affixing a joint is purely our problem as a user of Orocos API. The base segment and the end effector segment is by definition "affixed" as far as Orocos is concerned, Orocos only calculates the internal positions (IK) or joint torques (ID). Orocos IK takes end-chain frame as relative to the base joint (think base joint is origin). Similar with ID we must supply the forces at the base or end effector and Orocos will calculate the internal joint forces. So ground contact means nothing with IK or ID as we must supply such things as foot position or ground contact reaction force.

Reversing a chain is not necessary. We can easily work with KDL::Frames relative to any segment and then re-base (transform) back to base segment transform before calling IK or ID. Regardless, base and end-effector locations (or forces) are input to Orocos IK/ID solvers and we get back the internal joint details.

My algorithm decides where the foot should be and where the base should be and I must detect if the foot is in ground contact. The Orocos IK determines the internal joint positions and the ID solver determines internal joint forces but we must determine external forces. I calculate the robot Center of Mass (CoM) and for each leg in contact with the ground I calculate the portion of mass over each contact leg, then in effect I have the Ground Reaction forces (GRF) pushing up on each leg. This is as easy as F=ma, where 'a' is gravity, 'm' is the portion of total mass over each leg and is a function of each leg's distance from CoM. For each leg chain then, if its in contact with the ground, I feed the GRF of that foot as a wrench force into the ID solver. This and gravity determines the torgue forces on each joint. For legs in ground contact they will be in compression between the force of gravity and the GRF. For legs in a lift phase they will be in a dangling state. Detecting feet in ground contact is also used to calculate the support polgyon so doing this work is required by you. I am getting pretty high accuracy, higher than expected!

I am currently solving ID for each limb as separate chains. I see a Tree ID solver and I may switch to that. Regardless I dont think it changes much, the above details still apply. I hope this helps you. :)

[MatthijsBurgh]

Good collaboration! Closing due to inactivity.