search

stack-of-tasks / pinocchio

A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
http://stack-of-tasks.github.io/pinocchio/
BSD 2-Clause "Simplified" License
1.84k stars 385 forks source link

Add option to reorder joint after parsing URDF #379

Open thomasfla opened 7 years ago

thomasfla commented 7 years ago

To be compatible with other dynamic libraries or robot drivers, it would be good to be able to choose joint order after parsing URDF file, taking as input a vector of joint names.

Even if to be efficient Pinocchio needs to place parents before their child, there is still freedom in ordering the sub-chains of the tree. If the reordering asked by user doesn't respect this parent-child constraint, Pinocchio could report an error.

This need isn't a priority as memory map is possible, but so far it has been express by T. KO, G. BUONDONNO and myself.

nmansard commented 7 years ago

I think it is a good idea. An algorithm could be implemented to re-order Model (no need to reorder Data, that would be re-created from the new model after reordering). It would also be necessary to re-order GeomModel, although it can be implemented in a second part of the work. I do not expect any blocking difficulty in implementing it, although it is a tedious work to have all the indexes correct afterward. I could supervise the implementation. Who could lead the task?

On 02/28/2017 09:41 AM, thomasfla wrote:

To be compatible with other dynamic libraries or robot drivers, it would be good to be able to choose joint order after parsing URDF file, taking as input a vector of joint names.

Even if to be efficient Pinocchio needs to place parents before their child, there is still freedom in ordering the sub-chains of the tree. If the reordering asked by user doesn't respect this parent-child constraint, Pinocchio could report an error.

This need isn't a priority as memory map is possible, but so far it has been express by T. KO, G. BUONDONNO and myself.

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/stack-of-tasks/pinocchio/issues/379, or mute the thread https://github.com/notifications/unsubscribe-auth/AAb-CGSsyHo7E5x2sDZg8AV6RObhUQVXks5rg92ogaJpZM4MOHKY.

jmirabel commented 7 years ago

For the information, the only reason why the joints are reordered is that urdfdom stores them in a map, which is an ordered set. They are sorted by joint name. IMHO, this is a mistake in urdfdom...

For the record, the problem is in Model::initTree (file urdfdom_headers/urdf_model/include/urdf_model/model.h), which should not loop on a map but on a vector. The fix is very easy here, but it will be a pain to get the change accepted by the package maintainers...

jcarpent commented 7 years ago

@jmirabel The request of the feature is not only due to URDF choice of traversing tree, but depends on the target application. For instance, with the HRP-2, the joint order is not the same in the VRML file than in the URDF file. Then, you may want that in Model, the joint ordering be the same than in old application to avoid unnecessary mapping.

cmastalli commented 5 years ago

Joint desired order could be described through a SDF.

tlemarec commented 3 years ago

Hello, any news on this issue ?

jcarpent commented 3 years ago

Sorry, but we are terribly lacking in manpower. Your contribution @tlemarec will be really appreciated for implementing this feature!

tlemarec commented 3 years ago

I'll see what we can do (@davidfauthoux), our understanding of the inner workings of pinocchio is still limited

jcarpent commented 3 years ago

Normally, this should not be so hard, knowing we already support the build of a reduced model from a given model. For efficiency reasons, you just need to have the correct ordering, meaning that the index of a parent is always below that the ones of its children + for a given subtree, the indexes should be successive.