pinocchio
pinocchio copied to clipboard
stack-of-tasks
Add option to reorder joint after parsing URDF
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.
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.
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...
@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.
Sorry, but we are terribly lacking in manpower. Your contribution @tlemarec will be really appreciated for implementing this feature!
I'll see what we can do (@davidfauthoux), our understanding of the inner workings of pinocchio is still limited
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.