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.82k stars 383 forks source link

add python test for `contactInverseDynamics` #2263

Closed fabinsch closed 4 months ago

fabinsch commented 4 months ago

In a similar way to the cpp test for the contactInverseDynamics function, I propose to add a python test and check if this function is usable from Python.

the c++ function requires for the contact models, data and cones

contactInverseDynamics(..., 
const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> & contact_models,
std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_datas,
const std::vector<CoulombFrictionConeTpl<Scalar>, CoulombFrictionConelAllocator> & cones,

and this works well when constructing them as StdVec_RigidConstraintData() and then appending contact data here (see test1). However, if you try to pass a list of contact data, it does not work (see test3). This is not the case for contact models and cones, they work both as list of model/cones and as pin.StdVec (see test2).

The different is, that models/cones are const std::vectors while data is not const.

fabinsch commented 4 months ago

The problem of calling the Python bindings of contactInverseDynamics with a list of RigidConstraintData objects described above is fixed with https://github.com/stack-of-tasks/pinocchio/pull/2263/commits/5dc80d65e714890bc6c78ea52d54c844cf90532a.

ManifoldFR commented 4 months ago

The problem of calling the Python bindings of contactInverseDynamics with a list of RigidConstraintData objects described above is fixed with 5dc80d6.

I wasn't aware this list.hpp header even existed. I don't even know if it's actually useful.

This is a super-subtle bug, great investigation job @fabinsch !

fabinsch commented 4 months ago

I don't even know if it's actually useful.

I checked and we use it here in bindings/python/parsers/urdf/geometry.cpp. However, I am not sure why we also include it in expose-model.cpp and expose-rnea.cpp. This should probably be removed there.

jcarpent commented 4 months ago

I don't even know if it's actually useful.

I checked and we use it here in bindings/python/parsers/urdf/geometry.cpp. However, I am not sure why we also include it in expose-model.cpp and expose-rnea.cpp. This should probably be removed there.

We use in rnea because of fext arg, we might be a list.

fabinsch commented 4 months ago

yes, you are right that the external forces can be passed as a list from python to the rnea bindings.

I checked why our unittest for this case is still working (see CI) without the list.hpp header in expose-rnea. The reason is, that the StdAlignedVectorPythonVisitor for Force, called here to expose StdVec_Force, implements a FromPythonListConverter here. I also verified this on my machine with a quick test.