problem with collisionDetectmex normal calculation

[mposa]

There seems to be an issue with the normal vector calculation in collisionDetectmex (but not the non-mex version) that occurs only in edge cases. For example, see the test code below (run from /examples/KneedCompassGait/dev)

options.terrain = RigidBodyFlatTerrain();
options.floating = true;
options.ignore_self_collisions = true;
p = RigidBodyManipulator('../KneedCompassGait.urdf',options);
q = zeros(p.getNumPositions,1);
q(3) = 1;

q2 = q;
q2(4) = 1e-6;
[phi,normal,d,xA,xB,idxA,idxB] = p.contactConstraints(q);
[phi2,normal2,d2,xA2,xB2,idxA2,idxB2] = p.contactConstraints(q2);

The result is normal = [0 0; 0 0; -1 -1] when the correct answer would be [0 0; 0 0; 1 1]. Note that the body indices are unchanged by the small perturbation.

[mposa]

Quick update, but I think I was mistaken. In this example, the correct value for both normal and normal2 0 0; 0 0; -1 -1], since idxA=[3;5] (robot) and idxB = [1;1] (ground). Still, this makes the value for normal incorrect (when the foot is exactly in contact with the ground)

Note that with terrain_only=true, the normal is correctly +1 and the indices are flipped.

[mposa]

It appears to have something to do with the kinematics, here is a urdf of a point mass shifted by .5 meters (visual and collision):

<?xml version="1.0"?>
<robot xmlns="http://drake.mit.edu"
 xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
 xsi:schemaLocation="http://drake.mit.edu ../../../doc/drakeURDF.xsd" name="PointMass">
  <link name="point">
    <inertial>
      <mass value="1"/>
      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
    </inertial>
    <visual>
      <origin xyz="0 0 -.5" rpy="0 0 0"/>
      <geometry>
        <sphere radius="0.1" />
      </geometry>
    </visual>
    <collision>
      <origin xyz="0 0 -.5" rpy="0 0 0"/>
      <geometry>
        <sphere radius="0.0" />
      </geometry>
    </collision>
  </link>
</robot>

Here's some simple test code that produces bad results

options.terrain = RigidBodyFlatTerrain();
options.floating = true;
options.ignore_self_collisions = true;
p = RigidBodyManipulator('shiftedPointMass.urdf',options);
q = zeros(6,1);
q(3) = .5;

[phi,normal,d,xA,xB,idxA,idxB,mu,n,D,dn,dD] = p.contactConstraints(q,false,struct('terrain_only',false));
phi
normal

q(5) = 1e-6;

[phi,normal,d,xA,xB,idxA,idxB,mu,n,D,dn,dD] = p.contactConstraints(q,false,struct('terrain_only',false));
phi
normal

[RussTedrake]

michael - do you consider this resolved, with Andres' update?