Set model root link

[aelkhour]

Currently, the model root link is the same as the one in the model tree in URDF or Lua.
In some applications, it can be useful to change the root link on-the-fly (e.g. from pelvis to left foot to right foot ...)

Does the current code structure allow developing an api for setting the root link?

[jcarpent]

To be honest, such strategy is not efficient from a computational viewpoint. Indeed, Featherstone's algorithms are efficient only on balanced tree, where the branches have more or less the same number of nodes.

Which situations where you need a different base link have you got in mind ?

Yet, if you really need such feature, one painful solution consists in writing a new urdf file of your system with a different base link.

[aelkhour]

I agree that some of Featherstone's algorithms are less efficient on non-balanced trees.

When planning a motion for a humanoid robot, it can be usfeul to set alternatively the left and right foot as root links. This allows considering the robot as a fixed-based one, removing the need for additional DoFs and constraints to keep the feet on the ground. The gain in implementation and computation time has of course to be compared with the loss of computation time due to having a non-balanced tree.

Creating several URDFs is a basic solution, but it increases the complexity of maintaining different models, and requires loading several URDFs for one walking motion.

I think that we could give more freedom to the user by allowing him to choose the root link after loading the robot model, provided that we warn him about potential loss of computational efficiency. This of course depends of the technical feasibility of such an api.

[florent-lamiraux]

Hello Antonio,

I am not an expert of pinocchio, but I would suggest to keep the core API as small and simple as possible. In my opinion, pinocchio should handle kinematic trees with a given root. Closed chains should be inserted only if there is a computational gain with respect to handling them externally.

I think the function you request should be implemented in a package above pinocchio. This function would take as input a kinematic tree with a given root and build a kinematic tree with another root.

Again, this is only my high level opinion.

[aelkhour]

@florent-lamiraux I see your point.

Maybe the root link choice could then be delegated to the parser(s), which would be moved to a dedicated package. The parser would take as input:

• the file containing a tree description,
• the root link name (has to exist in the tree description),
• the root joint type (see Choose root joint type #32)

As a consequence, we would need to parse the same file several times and store multiple robot models in memory. It looks a bit like having multiple files for the same robot, but it is a bit nicer maintenance-wise.

[olivier-stasse]

Dear Antonio,
We have used the formulation in the early ages of the SoT, specifically also because it allows an ego-reference frame which might seem preferable for sensor based feedback. It was quite difficult to handle the root-switching. In addition for a walker this not practically the case, the soles drift on the ground depending on the ground and the feet configuration. You then need a localization system maintaining the position of the robot with the ground.
The reaction of Thomas and Florent was to use a global "virtual" reference frame where you compute the end-effectors and reference frames. You could also do the same for the closed-loop. The maintenance of the code was also not worth it.

[olivier-stasse]

This is of course only the expression on my humble opinion.

[fvalenza]

The only solution is to build a new pair Model - Data. This could be done from a new dedicated URDF model as suggested by Justin, although heavy to handle in practice as remarked by Antonio. Another way is to implement an algorithm that creates a new pair Model-Data from an existing one, with a different root. This should be feasible immediately in C++ (the main trick is to find how to create a new JointModel from an existing JointModelVariant, ie recovering the type inside a variant -- doubtlessly immediate with a visitor, possibly more easily with another way). It is yet not possible to do that in Python, but it should be in the next release.

About the interest of this procedure: as observed by Justin, most algorithms are more efficient with a balanced tree. Possibly, you may consider to implement a dedicated algorithm (like computeJacobian) for a specific subtree?

[aelkhour]

@fvalenza Thanks for pointing out the technical details.

@olivier-stasse Thank you for your always-welcome feedback. I guess we should have a scientific discussion before deciding whether to move ahead in this direction.

[jcarpent]

I really think that this debate can now be closed. Thanks everyone for his participation.