diff --git a/data/jenga/jenga.mtl b/data/jenga/jenga.mtl
new file mode 100644
index 000000000..a89fe6ca2
--- /dev/null
+++ b/data/jenga/jenga.mtl
@@ -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
+
+
+
diff --git a/data/jenga/jenga.obj b/data/jenga/jenga.obj
new file mode 100644
index 000000000..b90863be1
--- /dev/null
+++ b/data/jenga/jenga.obj
@@ -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
+
+
\ No newline at end of file
diff --git a/data/jenga/jenga.png b/data/jenga/jenga.png
new file mode 100644
index 000000000..1564309d9
Binary files /dev/null and b/data/jenga/jenga.png differ
diff --git a/data/jenga/jenga.urdf b/data/jenga/jenga.urdf
new file mode 100644
index 000000000..6ca727c78
--- /dev/null
+++ b/data/jenga/jenga.urdf
@@ -0,0 +1,31 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/data/l_finger_collision.stl b/data/l_finger_collision.stl
index b12b2dd9a..b4e45652b 100644
Binary files a/data/l_finger_collision.stl and b/data/l_finger_collision.stl differ
diff --git a/data/pr2_gripper.urdf b/data/pr2_gripper.urdf
index 01ab8233b..6ca8dc674 100644
--- a/data/pr2_gripper.urdf
+++ b/data/pr2_gripper.urdf
@@ -32,18 +32,23 @@
-
-
+
+
-
+
-
+
+
+
+
+
+
-
+
@@ -55,8 +60,8 @@
-
-
+
+
@@ -71,7 +76,7 @@
-
+
@@ -86,18 +91,23 @@
-
-
+
+
-
+
-
+
+
+
+
+
+
-
+
@@ -109,8 +119,8 @@
-
-
+
+
@@ -125,7 +135,7 @@
-
+
diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
index dcd28d826..320c0b6e6 100644
--- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
+++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
@@ -28,6 +28,7 @@ subject to the following restrictions:
#include "../ImportMeshUtility/b3ImportMeshUtility.h"
+static btScalar gUrdfDefaultCollisionMargin = 0.001;
#include
#include
@@ -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)
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index fe45001c9..85839e322 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -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()
@@ -2847,7 +2848,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;
@@ -2919,26 +2920,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());
}
}
}
@@ -2952,6 +2952,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)
diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp
index 1ff054a0f..2dec1401c 100644
--- a/examples/SharedMemory/PhysicsServerExample.cpp
+++ b/examples/SharedMemory/PhysicsServerExample.cpp
@@ -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)
diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
index 603ad167d..a23ee3d88 100644
--- a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
+++ b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
@@ -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 ))