Add example of collision with a point cloud

[jcarpent]

Related to #1632.

[jcarpent]

@arjung128 This PR is related to your request.

[arjung128]

Hi @jcarpent,

Thank you so much for such a detailed example! I have been struggling with this for quite some time now, so this is extremely helpful.

For reproducibility, I downloaded the panda URDF and meshes from Gepetto/example-robot-data (here), and am on Pinocchio version '2.6.4' and hppfcl version '1.8.1'; However, I am getting the following error. Are you on a different Pinocchio / hppfcl version by any chance?

---------------------------------------------------------------------------
ArgumentError                             Traceback (most recent call last)
Input In [1], in <cell line: 114>()
    114 while not is_collision:
    115     q = pin.randomConfiguration(model)
--> 117     is_collision = pin.computeCollisions(model, data, collision_model, collision_data, q)
    119 print("Found a configuration in collision:",q)
    120 viz.display(q)

ArgumentError: Python argument types in
    pinocchio.pinocchio_pywrap.computeCollisions(Model, Data, GeometryModel, GeometryData, numpy.ndarray)
did not match C++ signature:
    computeCollisions(int num_thread, pinocchio::GeometryPoolTpl<double, 0, pinocchio::JointCollectionDefaultTpl> {lvalue} pool, Eigen::Matrix<double, -1, -1, 0, -1, -1> q, Eigen::Ref<Eigen::Matrix<bool, -1, 1, 0, -1, 1>, 0, Eigen::InnerStride<1> > res)
    computeCollisions(int num_thread, pinocchio::GeometryPoolTpl<double, 0, pinocchio::JointCollectionDefaultTpl> {lvalue} pool, Eigen::Matrix<double, -1, -1, 0, -1, -1> q, Eigen::Ref<Eigen::Matrix<bool, -1, 1, 0, -1, 1>, 0, Eigen::InnerStride<1> > res, bool stop_at_first_collision)
    computeCollisions(int num_thread, pinocchio::GeometryPoolTpl<double, 0, pinocchio::JointCollectionDefaultTpl> {lvalue} pool, Eigen::Matrix<double, -1, -1, 0, -1, -1> q)
    computeCollisions(int num_thread, pinocchio::GeometryPoolTpl<double, 0, pinocchio::JointCollectionDefaultTpl> {lvalue} pool, Eigen::Matrix<double, -1, -1, 0, -1, -1> q, bool stop_at_first_collision)
    computeCollisions(int num_thread, pinocchio::ModelTpl<double, 0, pinocchio::JointCollectionDefaultTpl> model, pinocchio::DataTpl<double, 0, pinocchio::JointCollectionDefaultTpl> {lvalue} data, pinocchio::GeometryModel geometry_model, pinocchio::GeometryData {lvalue} geometry_data, Eigen::Matrix<double, -1, 1, 0, -1, 1> q)
    computeCollisions(int num_thread, pinocchio::ModelTpl<double, 0, pinocchio::JointCollectionDefaultTpl> model, pinocchio::DataTpl<double, 0, pinocchio::JointCollectionDefaultTpl> {lvalue} data, pinocchio::GeometryModel geometry_model, pinocchio::GeometryData {lvalue} geometry_data, Eigen::Matrix<double, -1, 1, 0, -1, 1> q, bool stop_at_first_collision)
    computeCollisions(int num_thread, pinocchio::GeometryModel geometry_model, pinocchio::GeometryData {lvalue} geometry_data)
    computeCollisions(int num_thread, pinocchio::GeometryModel geometry_model, pinocchio::GeometryData {lvalue} geometry_data, bool stop_at_first_collision)
    computeCollisions(pinocchio::ModelTpl<double, 0, pinocchio::JointCollectionDefaultTpl> model, pinocchio::DataTpl<double, 0, pinocchio::JointCollectionDefaultTpl> {lvalue} data, pinocchio::GeometryModel geometry_model, pinocchio::GeometryData {lvalue} geometry_data, Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > q, bool stop_at_first_collision)
    computeCollisions(pinocchio::GeometryModel geometry_model, pinocchio::GeometryData {lvalue} geometry_data, bool stop_at_first_collision)

Thanks once again for your continued help! It is much appreciated.

[arjung128]

Replacing is_collision = pin.computeCollisions(model, data, collision_model, collision_data, q) with is_collision = pin.computeCollisions(model, data, collision_model, collision_data, q, False) fixed the error for me.

[jcarpent]

I've fixed the issue you mentioned @arjung128

[arjung128]

Hi @jcarpent,

Thanks for the quick fix and for such a detailed example! I really appreciate it.

The example seems to be converting the point cloud into a height field and checking for collisions against the height field. Is there a reason why this was done, and why collision checking wasn't done directly with the point cloud? When I try to do collision checking directly with the point cloud (by replacing the height field with the point cloud), I am getting an error:

---> 14 is_collision = pin.computeCollisions(model, data, collision_model, collision_data, q, False)
         15 if is_collision:
         16     viz.display(q)

ValueError: Problem when trying to check the collision of collision pair #0 (11,8)
hpp-fcl original error:
From file: /home/conda/feedstock_root/build_artifacts/hpp-fcl_1647827680493/work/include/hpp/fcl/internal/traversal_node_setup.h
in function: bool hpp::fcl::initialize(hpp::fcl::MeshShapeCollisionTraversalNode<BV, S, 0>&, const hpp::fcl::BVHModel<BV>&, const hpp::fcl::Transform3f&, const S&, const hpp::fcl::Transform3f&, const hpp::fcl::GJKSolver*, hpp::fcl::CollisionResult&) [with BV = hpp::fcl::OBBRSS; S = hpp::fcl::ConvexBase]
at line: 360
message: model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.

Any ideas what might be going on here?

On a related note, while converting the point cloud to a height field for collision checking works for many cases, e.g. the box, it may not work for slightly more complicated shapes (a naive example is two boxes on top of one another, i.e. if you add points = points[:, (points[2, :] >= 0.9) | (points[2, :] <= 0.1)] to split the one box into two boxes after generating the points). I was wondering -- is there another data structure apart from point clouds for which Pinocchio supports collision checking of obstacles with? Unfortunately the 3D meshes I have also contain more complex shapes, and a height field may not be general enough to capture the full geometry.

Thanks again for all your support!