AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-04 17:40:06 +00:00
Code Issues Releases Wiki Activity

decrease some gjk/epa tolerance to improve collision detection using very small collision margins (0.001/1mm)

Browse Source 
tweak pr2 gripper.
This commit is contained in:
erwin coumans 2016-09-20 12:37:13 -07:00
parent 8d4c99801e
commit 0a628f06cc
10 changed files with 214 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
data/jenga/jenga.mtl Normal file
View File

@ -0,0 +1,16 @@
newmtl jenga
Ns 10.0000
Ni 1.5000
d 1.0000
Tr 0.0000
Tf 1.0000 1.0000 1.0000
illum 2
Ka 0.0000 0.0000 0.0000
Kd 0.5880 0.5880 0.5880
Ks 0.0000 0.0000 0.0000
Ke 0.0000 0.0000 0.0000
map_Ka jenga.tga
map_Kd jenga.png

113
data/jenga/jenga.obj Normal file
View File

@ -0,0 +1,113 @@
# jenga.obj
#
o jenga
mtllib jenga.mtl
v -0.5 -0.5 0.5
v 0.5 -0.5 0.5
v 0.5 0.5 0.5
v -0.5 0.5 0.5
v -0.5 -0.5 -0.5
v 0.5 -0.5 -0.5
v 0.5 0.5 -0.5
v -0.5 0.5 -0.5
v -0.5 -0.5 -0.5
v -0.5 0.5 -0.5
v -0.5 0.5 0.5
v -0.5 -0.5 0.5
v 0.5 -0.5 -0.5
v 0.5 0.5 -0.5
v 0.5 0.5 0.5
v 0.5 -0.5 0.5
v -0.5 -0.5 -0.5
v -0.5 -0.5 0.5
v 0.5 -0.5 0.5
v 0.5 -0.5 -0.5
v -0.5 0.5 -0.5
v -0.5 0.5 0.5
v 0.5 0.5 0.5
v 0.5 0.5 -0.5
vt 0 1
vt 0 0.75
vt 0.25 0.75
vt 0.25 1
vt 0.25 0.5
vt 0.25 0.75
vt 0.5 0.75
vt 0.5 0.5
vt 1 0.75
vt 0.75 0.75
vt 0.75 1
vt 1 1
vt 0.25 0.75
vt 0.5 0.75
vt 0.5 1
vt 0.25 1
vt 0 0.5
vt 0 0.75
vt 0.25 0.75
vt 0.25 0.5
vt 0.75 0.75
vt 0.75 1
vt 0.5 1
vt 0.5 0.75
vn 0 0 1
vn 0 0 1
vn 0 0 1
vn 0 0 1
vn 0 0 -1
vn 0 0 -1
vn 0 0 -1
vn 0 0 -1
vn -1 0 0
vn -1 0 0
vn -1 0 0
vn -1 0 0
vn 1 0 0
vn 1 0 0
vn 1 0 0
vn 1 0 0
vn 0 -1 0
vn 0 -1 0
vn 0 -1 0
vn 0 -1 0
vn 0 1 0
vn 0 1 0
vn 0 1 0
vn 0 1 0
g jenga
usemtl jenga
s 1
f 1/1/1 2/2/2 3/3/3
f 1/1/1 3/3/3 4/4/4
s 2
f 7/7/7 6/6/6 5/5/5
f 8/8/8 7/7/7 5/5/5
s 3
f 11/11/11 10/10/10 9/9/9
f 12/12/12 11/11/11 9/9/9
s 4
f 13/13/13 14/14/14 15/15/15
f 13/13/13 15/15/15 16/16/16
s 5
f 19/19/19 18/18/18 17/17/17
f 20/20/20 19/19/19 17/17/17
s 6
f 21/21/21 22/22/22 23/23/23
f 21/21/21 23/23/23 24/24/24

BIN
data/jenga/jenga.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 280 KiB

31
data/jenga/jenga.urdf Normal file
View File

@ -0,0 +1,31 @@
<?xml version="0.0" ?>
<robot name="jenga.urdf">
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<spinning_friction value="0.5"/>
<inertia_scaling value="3.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="1.0"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="jenga.obj" scale="0.15 0.05 0.03"/>
</geometry>
<material name="blue">
<color rgba="0.5 0.5 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.15 0.05 0.03"/>
</geometry>
</collision>
</link>
</robot>

BIN
data/l_finger_collision.stl
View File

Binary file not shown.

42
data/pr2_gripper.urdf
View File

@ -32,18 +32,23 @@
<link name="left_gripper">
<contact>
<lateral_friction value="5.0"/>
<spinning_friction value="0.1"/>
<lateral_friction value="1.0"/>
<spinning_friction value="1.5"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="l_finger.stl"/>
<mesh filename="l_finger_collision.stl"/>
</geometry>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="l_finger_collision.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.5"/>
<mass value="0.1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
@ -55,8 +60,8 @@
<link name="left_tip">
<contact>
<lateral_friction value="5.0"/>
<spinning_friction value="0.1"/>
<lateral_friction value="1.0"/>
<spinning_friction value="1.5"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
@ -71,7 +76,7 @@
<origin rpy="0.0 0 0" xyz="0.105 0.00495 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<mass value="0.1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
@ -86,18 +91,23 @@
<link name="right_gripper">
<contact>
<lateral_friction value="5.0"/>
<spinning_friction value="0.3"/>
<lateral_friction value="1.0"/>
<spinning_friction value="1.5"/>
</contact>
<visual>
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="l_finger.stl"/>
<mesh filename="l_finger_collision.stl"/>
</geometry>
</visual>
<collision>
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="l_finger_collision.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<mass value="0.1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
@ -109,8 +119,8 @@
<link name="right_tip">
<contact>
<lateral_friction value="5.0"/>
<spinning_friction value=".2"/>
<lateral_friction value="1.0"/>
<spinning_friction value="1.5"/>
</contact>
<visual>
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
@ -125,7 +135,7 @@
<origin rpy="-3.1415 0 0" xyz="0.105 0.00495 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<mass value="0.1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>

19
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@ -28,6 +28,7 @@ subject to the following restrictions:
#include "../ImportMeshUtility/b3ImportMeshUtility.h"
static btScalar gUrdfDefaultCollisionMargin = 0.001;
#include <iostream>
#include <fstream>
@ -347,7 +348,7 @@ void BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVect
}
localInertiaDiagonal.setValue(principalInertiaX, principalInertiaY, principalInertiaZ);
inertialFrame.setOrigin(link->m_inertia.m_linkLocalFrame.getOrigin());
inertialFrame.setBasis(linkInertiaBasis * link->m_inertia.m_linkLocalFrame.getBasis());
inertialFrame.setBasis(link->m_inertia.m_linkLocalFrame.getBasis()*linkInertiaBasis);
}
else
{
@ -464,7 +465,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
}
btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
cylZShape->setMargin(0.001);
cylZShape->setMargin(gUrdfDefaultCollisionMargin);
cylZShape->initializePolyhedralFeatures();
//btConvexShape* cylZShape = new btConeShapeZ(cyl->radius,cyl->length);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
@ -481,7 +482,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
btBoxShape* boxShape = new btBoxShape(extents*0.5f);
//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
shape = boxShape;
shape ->setMargin(0.001);
shape ->setMargin(gUrdfDefaultCollisionMargin);
break;
}
case URDF_GEOM_SPHERE:
@ -490,7 +491,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
btScalar radius = collision->m_geometry.m_sphereRadius;
btSphereShape* sphereShape = new btSphereShape(radius);
shape = sphereShape;
shape ->setMargin(0.001);
shape ->setMargin(gUrdfDefaultCollisionMargin);
break;
break;
@ -689,7 +690,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
//cylZShape->initializePolyhedralFeatures();
//btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
cylZShape->setMargin(0.001);
cylZShape->setMargin(gUrdfDefaultCollisionMargin);
shape = cylZShape;
}
} else
@ -748,7 +749,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
}
btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
cylZShape->setMargin(0.001);
cylZShape->setMargin(gUrdfDefaultCollisionMargin);
convexColShape = cylZShape;
break;
}
@ -760,7 +761,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
btBoxShape* boxShape = new btBoxShape(extents*0.5f);
//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
convexColShape = boxShape;
convexColShape->setMargin(0.001);
convexColShape->setMargin(gUrdfDefaultCollisionMargin);
break;
}
case URDF_GEOM_SPHERE:
@ -768,7 +769,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
btScalar radius = visual->m_geometry.m_sphereRadius;
btSphereShape* sphereShape = new btSphereShape(radius);
convexColShape = sphereShape;
convexColShape->setMargin(0.001);
convexColShape->setMargin(gUrdfDefaultCollisionMargin);
break;
break;
@ -1166,7 +1167,7 @@ btCollisionShape* BulletURDFImporter::getAllocatedCollisionShape(int index)
btCompoundShape* compoundShape = new btCompoundShape();
m_data->m_allocatedCollisionShapes.push_back(compoundShape);
compoundShape->setMargin(0.001);
compoundShape->setMargin(gUrdfDefaultCollisionMargin);
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
btAssert(linkPtr);
if (linkPtr)

20
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -596,8 +596,9 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer();
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.04;
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.05;
}
void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies()
@ -2839,7 +2840,7 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
m_data->m_logPlayback = pb;
}
btVector3 gVRGripperPos(0,0,0);
btVector3 gVRGripperPos(0,0,0.2);
btQuaternion gVRGripperOrn(0,0,0,1);
btScalar gVRGripperAnalog = 0;
bool gVRGripperClosed = false;
@ -2911,26 +2912,25 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
loadUrdf("kuka_iiwa/model.urdf", btVector3(3, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("cube_small.urdf", btVector3(0.2, 0.2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("cube_small.urdf", btVector3(0.3, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
// loadUrdf("cube_small.urdf", btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
#endif
//#define JENGA 1
#ifdef JENGA
int jengaHeight = 17;
#if 0
int jengaHeight = 10;
for (int j = 0; j < jengaHeight; j++)
{
for (int i = 0; i < 3; i++)
{
if (j & 1)
{
loadUrdf("jenga/jenga.urdf", btVector3(-0.5+0, 0.025*i, .0151*0.5 + .015*j), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("jenga/jenga.urdf", btVector3(-0.5, 0.05*i, .03*0.5 + .03*j), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
}
else
{
btQuaternion orn(btVector3(0, 0, 1), SIMD_HALF_PI);
loadUrdf("jenga/jenga.urdf", btVector3(-0.5 -1 / 3.*0.075 + 0.025*i, +1 / 3.*0.075,0.0151*0.5 + .015*j), orn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("jenga/jenga.urdf", btVector3(-0.5 -1 / 3.*0.15 + 0.05*i, +1 / 3.*0.15,0.03*0.5 + .03*j), orn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
}
}
}
@ -2944,6 +2944,8 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
loadSdf("kiva_shelf/model.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true);
loadUrdf("teddy_vhacd.urdf", btVector3(1, 1, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
}
if (m_data->m_gripperRigidbodyFixed && m_data->m_gripperMultiBody)

9
examples/SharedMemory/PhysicsServerExample.cpp
View File

@ -19,6 +19,10 @@
extern btVector3 gLastPickPos;
btVector3 gVRTeleportPos(0,0,0);
btQuaternion gVRTeleportOrn(0, 0, 0,1);
extern btVector3 gVRGripperPos;
extern btQuaternion gVRGripperOrn;
extern btScalar gVRGripperAnalog;
extern bool gEnableRealTimeSimVR;
extern bool gVRGripperClosed;
@ -936,6 +940,7 @@ void PhysicsServerExample::renderScene()
if (gDebugRenderToggle)
if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
{
gEnableRealTimeSimVR = true;
B3_PROFILE("Draw Debug HUD");
//some little experiment to add text/HUD to a VR camera (HTC Vive/Oculus Rift)
@ -1157,10 +1162,6 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
}
}
extern btVector3 gVRGripperPos;
extern btQuaternion gVRGripperOrn;
extern btScalar gVRGripperAnalog;
extern bool gEnableRealTimeSimVR;
void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4], float analogAxis)

4
src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
View File

@ -39,7 +39,7 @@ subject to the following restrictions:
//temp globals, to improve GJK/EPA/penetration calculations
int gNumDeepPenetrationChecks = 0;
int gNumGjkChecks = 0;
btScalar gGjkEpaPenetrationTolerance = 0.001;
btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
@ -304,7 +304,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
}
bool catchDegeneratePenetrationCase =
(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01));
(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < gGjkEpaPenetrationTolerance));
//if (checkPenetration && !isValid)
if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))