Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests 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
Download Patch File Download Diff File Expand all files Collapse all files

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)