Pinocchio: Collision normal not evaluated after compute collisions

[nmansard]

I am trying to re-do the tutorial 4 (contact simulation) using FCL -- it is yet implemented with capsules only and with the distances computed manually. However, I am not able to obtain properly the collision point and normals. I believe it is do to my lack of understanding of FCL, or to an unproper implementation in Pinocchio. I would appreciate some advices.

When using computeCollisions, the collision results should implement the contact points and normals, but these values are not initialized. By the way: each collision pair automatically defines a number of contact points numcontact, with a list of contact points, but this list does not have the proper size or the accessor allows the user to go beyond the list size.

When using computeDistances, the closest witness points on both objects are implemented and their values properly filled, but the normal are not given (which I believe is normal).

Am I missing something before accessing the normal of the collision? Is there a way to access the normals of the distance?

[jmirabel]

However, I am not able to obtain properly the collision point and normals. I believe it is do to my lack of understanding of FCL

Welcome to the FCL world !

Did you check your CollisionRequest ?

[jcarpent]

@nmansard Ping. Do you want to maintain this issue opened?

[nmansard]

yes sorry, i did not resolved this problem yet.

[littleggghost]

I wish to know how long will this correction take?

[nmansard]

somebody should investigate. I guess it should be me. But I cannot put that as an active task in my todolist yet ... sorry.
Do you want guidance to investigate?

[nmansard]

I re-start on this issue. I just checked that it is still pending.

pio.computeCollisions(rmodel,rdata,gmodel,gdata,q,False)
res = gdata.collisionResults[1]
assert(res.isCollision)
contact = res.getContact(0)
print(contact.normal.T,contact.pos.T)

This outputs (matrix([[nan, nan, nan]]), matrix([[nan, nan, nan]]).

As suggested by @jmirabel this should come from an improper setting of my gmodel.collisionRequest. Any idea of what I should set to have FCL computing the position and normal.

NB: I could compute the position with the computeDistance function (this works, I checked), but will not give me the normal. I need the normal as well.

[jcarpent]

The example is not clear. Could you provide a full example to reproduce it?

[jmirabel]

You should enable the contacts. Something like gmodel.collisionRequest.enable_contact=True.

[jmirabel]

It is

import hppfcl
geom_data.collisionRequest.enable_contact=True

[jcarpent]

@nmansard Does this solve your issue?

[jcarpent]

@nmansard Is it working now since @jmirabel's update?

[nmansard]

Sorry, my time to work on this is limited.
I am following both suggestions. Trying to make a self-contained example for @jcarpent , I saw that the normal seem to be computed on the UR-5 robot, but they were not with my capsule-based robot. Following @jmirabel I tried to switch to the newest hpp-fcl, which caused other problems that the new release by @nim65s should fix.
I will come back with a confirmation that the proposition by @jmirabel works, or with a self-contained example to discuss it.

[nmansard]

So I confirm the proposition of @jmirabel works. Here is an example using example-robot-data.
There is maybe two pending questions:

• first, the normals were properly computed for the meshes of the UR5, but not for the capsules (check in the code, the patch of @jmirabel is only set after the normals of the UR5 collisions were computed). Is this normal?
• second: should we set gdata.collisionRequest.enable_contact to True in the construction of the pinocchio::GeometryData object?

import pinocchio as pio
pio.switchToNumpyMatrix()
from example_robot_data import loadUR
from pinocchio.utils import rotate
import numpy as np

def Capsule(name,joint,placement,radius,length,gv=None,gvroot='world/pinocchio/visuals/'):
    '''Create a FCL capsule object and add the corresponding mesh in gepetto viewer. '''
    caps = pio.GeometryObject.CreateCapsule(radius,length)
    caps.name = name
    caps.placement = placement
    caps.parentJoint = joint
    if gv is not None and hasattr(gv,'viewer'):
        gv.viewer.gui.addCapsule(gvroot+name, radius,length, [1.,0.,0.,1.])
    return caps

robot = loadUR()
robot.initViewer(loadModel=True)

q = robot.q0.copy()
q.flat[:] = [-1.25,  1.16,  2.94, -2.41,  1.73,  0.9 ]

rmodel = robot.model
rdata  = rmodel.createData()
gmodel = robot.visual_model

gmodel.addCollisionPair(pio.CollisionPair(1,4))
idx1=gmodel.addGeometryObject(Capsule('test1',0,pio.SE3(rotate('y',-1.),np.matrix([1.,1.1,.7]).T),.1,1.,robot))
idx2=gmodel.addGeometryObject(Capsule('test2',0,pio.SE3(rotate('y',0.),np.matrix([1,1,0]).T),.1,1.,robot))
gmodel.addCollisionPair(pio.CollisionPair(idx1,idx2))

gdata  = gmodel.createData()
robot.rebuildData()

import hppfcl
gdata.collisionRequest.enable_contact=True

assert(pio.computeCollisions(rmodel,rdata,gmodel,gdata,q,False))

res = gdata.collisionResults[0]
assert(res.isCollision())
contact = res.getContact(0)
print(gmodel.collisionPairs[0],contact.normal.T,contact.pos.T)

res = gdata.collisionResults[-1]
assert(res.isCollision())
contact = res.getContact(0)
print(gmodel.collisionPairs[-1],contact.normal.T,contact.pos.T)

[jmirabel]

first, the normals were properly computed for the meshes of the UR5, but not for the capsules (check in the code, the patch of @jmirabel is only set after the normals of the UR5 collisions were computed). Is this normal?

For some pairs of primitives, computing the contact points does not add any computational overhead. For others, it does. When enable_contact is false, you get the contact point for the first set of pairs of primitives.

[jmirabel]

When enable_contact is false, you get the contact point for the first set of pairs of primitives.

this is the theoretical behavior that is intended. Obviously, it may not be respected for all pairs of primitives.

[jcarpent]

for the first set of pairs of primitives

What do you mean?

[jmirabel]

When enable_contact is false, you get the contact point for pairs of primitives for which computing them does not add any computational overhead.

[jcarpent]

OK. Thanks. I think we can close this issue as it seems to be solved now.

[mzwang828]

Hi, I'm also having the problem that no contact normal is returned. I have set enable_contact as true. However, the contact results returned contain only the contact position but not the contact normal.

collision_result.getContact(0).pos.transpose() this gives me the contact position.
collision_result.getContact(0).normal.transpose() this gives me only (nan,nan,nan).

Do you have any idea why this is happening?
Thanks!

[jmirabel]

Please, provide a bit of context. What geometries ? Are they truly in collision ? ...

[mzwang828]

Please, provide a bit of context. What geometries ? Are they truly in collision ? ...

Oh sorry here are the details. I'm trying to push a box using a one DOF(prismatic) rod with a ball at the tip. Here is the code segment:

GeometryModel geom_model, box_geom_model;
pinocchio::urdf::buildGeom(model, urdf_filename, pinocchio::COLLISION, geom_model, PINOCCHIO_MODEL_DIR);
pinocchio::urdf::buildGeom(model, box_filename, pinocchio::COLLISION, box_geom_model, PINOCCHIO_MODEL_DIR);
pinocchio::SE3::Identity(), fused_model, fused_geom_model); pinocchio::appendGeometryModel(geom_model, box_geom_model);
geom_model.addAllCollisionPairs();
pinocchio::srdf::removeCollisionPairs(model, geom_model, srdf_filename);
Data data(model);
GeometryData geom_data(geom_model);

Eigen::VectorXd q = randomConfiguration(model);

// box is placed at (1.5, 0, 0)
q << 1.25, 1.5, 0, 1, 0;
forwardKinematics(model,data,q);
updateFramePlacements(model,data);

// I'm only intersted in the contact between the ball and the box
GeomIndex tip_id = geom_model.getGeometryId("tip_0");
GeomIndex box_id = geom_model.getGeometryId("box_0");
geom_data.collisionRequest.enable_contact = true;

computeCollisions(model,data,geom_model,geom_data,q);
computeDistances(model, data, geom_model, geom_data, q);

CollisionPair cp = CollisionPair(tip_id, box_id);
PairIndex cp_index = geom_model.findCollisionPair(cp);
hpp::fcl::CollisionResult cr = geom_data.collisionResults[cp_index];
hpp::fcl::DistanceResult dr = geom_data.distanceResults[cp_index];

std::cout << "collision pair: " << cp.first << " , " << cp.second << " - collision: ";
std::cout << (cr.isCollision() ? "yes" : "no");
std::cout << " - nearest point: " << dr.nearest_points[0].transpose() << "," << dr.nearest_points[1].transpose() << "\n";
std::cout << " - distance: " << dr.min_distance << std::endl;
std::cout << " contacts number: " << cr.numContacts() << "\n";
std::cout << " - normal: " << cr.getContact(0).normal.transpose() << ", " << cr.getContact(0).pos.transpose() << "\n";

Here is the output I got:

collision pair: 2 , 4 - collision: yes - nearest point: 1.5   0   0,1.5   0   0
 - distance: -0.02
 contacts number: 1
 - normal: nan nan nan, 1.5   0   0

The collision result shows the ball and the box are actually in collision and I also get the contact position (which looks right to me since the box is placed at 1.5, 0, 0). However, I only get Nan for the contact normal (which supposes to be (1,0,0) in my opinion).

Just in case you need the urdf model, drive link

Thank you!

[jmirabel]

1. box.urdf looks wrong. The collision and visual boxes do not have the same origin. (definitely not the cause of this error)
2. Could you check that the collision pair exists with existCollisionPair ?
3. Could you provide the pose of both geometry objects ? They are in geom_data.oMg.
4. DistanceResult has a normal field. Could you provide it ?

[mzwang828]

Hi Joseph,
Thanks for noticing the error in my model.
Here is the additional information and I noticed that the contact normal is only returned when the min_distance (or penetration depth) is small enough.

Here are the results for q = 1.215, 1.5, 0, 1, 0, where 1.215 is the pusher's joint value and 1.5 is the box's position along the pushing direction:

Collision pair exist: 1
Contact normal: 1 0 0
Contact pos: 1.475     0     0
Penetration depth: 0.01
Pose of tip: 
  R =
1 0 0
0 1 0
0 0 1
  p = 1.465     0     0

Pose of box: 
  R =
1 0 0
0 1 0
0 0 1
  p = 1.5   0   0

Normal from DistanceResult: 1 0 0

Here are the results for q = 1.225, 1.5, 0, 1, 0:

Collision pair exist: 1
Contact normal: nan nan nan
Contact pos: 1.475     0     0
Penetration depth: 0.02
Pose of tip: 
  R =
1 0 0
0 1 0
0 0 1
  p = 1.475     0     0

Pose of box: 
  R =
1 0 0
0 1 0
0 0 1
  p = 1.5   0   0

Normal from DistanceResult: nan nan nan

As you can see, the normal is returned as Nan when penetration depth is 0.02 but returned correctly when the depth is 0.01. Is this designed to be so?

Thanks!

[jmirabel]

I am able to reproduce your issue. I think you hit a bug.

[jmirabel]

I confirm this is a bug in hpp-fcl. I will provide a PR in the next days.

[jmirabel]

Thanks for the thorough feedback.

[jmirabel]

The bug is fixed on the devel branch of hpp-fcl.