Closed mposa closed 10 years ago
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.
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
michael - do you consider this resolved, with Andres' update?
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
)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.