Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Add small sphere, make teddy small too

Browse Source 
Add physics time scale in VR mode
Fix compound collision margin in physics server
Enable visuals for rigid body (non-multi-body) URDF/SDF loading in physics server.
This commit is contained in:
erwincoumans
2016-09-22 08:50:28 -07:00
parent bb214e823b
commit 46b32f17bb
6 changed files with 159 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -401,15 +401,18 @@ bool BulletURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWor
return true;
}
static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes)
static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale)
{
btCompoundShape* compound = new btCompoundShape();
compound->setMargin(gUrdfDefaultCollisionMargin);
btTransform identity;
identity.setIdentity();
for (int s = 0; s<(int)shapes.size(); s++)
{
btConvexHullShape* convexHull = new btConvexHullShape();
convexHull->setMargin(gUrdfDefaultCollisionMargin);
tinyobj::shape_t& shape = shapes[s];
int faceCount = shape.mesh.indices.size();
@@ -420,17 +423,18 @@ static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t
pt.setValue(shape.mesh.positions[shape.mesh.indices[f] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]);
convexHull->addPoint(pt,false);
convexHull->addPoint(pt*geomScale,false);
pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]);
convexHull->addPoint(pt, false);
convexHull->addPoint(pt*geomScale, false);
pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]);
convexHull->addPoint(pt, false);
convexHull->addPoint(pt*geomScale, false);
}
convexHull->recalcLocalAabb();
@@ -464,16 +468,18 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
vertices.push_back(vert);
}
btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
cylZShape->setMargin(gUrdfDefaultCollisionMargin);
cylZShape->initializePolyhedralFeatures();
btConvexHullShape* convexHull = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
convexHull->setMargin(gUrdfDefaultCollisionMargin);
convexHull->initializePolyhedralFeatures();
convexHull->optimizeConvexHull();
//btConvexShape* cylZShape = new btConeShapeZ(cyl->radius,cyl->length);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
//btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
shape = cylZShape;
shape = convexHull;
break;
}
case URDF_GEOM_BOX:
@@ -555,7 +561,8 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes, fullPath, collisionPathPrefix);
//create a convex hull for each shape, and store it in a btCompoundShape
shape = createConvexHullFromShapes(shapes);
shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale);
return shape;
}
break;
@@ -685,13 +692,11 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
shape = trimesh;
} else
{
//btConvexHullShape* cylZShape = new btConvexHullShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, sizeof(GLInstanceVertex));
btConvexHullShape* cylZShape = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
//cylZShape->initializePolyhedralFeatures();
//btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
cylZShape->setMargin(gUrdfDefaultCollisionMargin);
shape = cylZShape;
btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
convexHull->optimizeConvexHull();
//convexHull->initializePolyhedralFeatures();
convexHull->setMargin(gUrdfDefaultCollisionMargin);
shape = convexHull;
}
} else
{

34
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -28,7 +28,9 @@
#include "SharedMemoryCommands.h"
btVector3 gLastPickPos(0, 0, 0);
bool gEnableRealTimeSimVR=false;
bool gEnableRealTimeSimVR=true;
int gCreateObjectSimVR = -1;
btScalar simTimeScalingFactor = 1;
struct UrdfLinkNameMapUtil
{
@@ -558,6 +560,7 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor()
createEmptyDynamicsWorld();
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.0001;
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
}
@@ -1880,13 +1883,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
applyJointDamping(i);
}
btScalar deltaTimeScaled = m_data->m_physicsDeltaTime*simTimeScalingFactor;
if (m_data->m_numSimulationSubSteps > 0)
{
m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime, m_data->m_numSimulationSubSteps, m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps);
m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, m_data->m_numSimulationSubSteps, m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps);
}
else
{
m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime, 0);
m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, 0);
}
SharedMemoryStatus& serverCmd =serverStatusOut;
@@ -2864,14 +2869,28 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
{
static btAlignedObjectArray<char> gBufferServerToClient;
gBufferServerToClient.resize(SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
int bodyId = 0;
if (gCreateObjectSimVR >= 0)
{
gCreateObjectSimVR = -1;
btMatrix3x3 mat(gVRGripperOrn);
btScalar spawnDistance = 0.1;
btVector3 spawnDir = mat.getColumn(0);
btVector3 shiftPos = spawnDir*spawnDistance;
btVector3 spawnPos = gVRGripperPos + shiftPos;
loadUrdf("sphere_small.urdf", spawnPos, gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
InteralBodyData* parentBody = m_data->getHandle(bodyId);
if (parentBody->m_multiBody)
{
parentBody->m_multiBody->setBaseVel(spawnDir * 3);
}
}
if (!m_data->m_hasGround)
{
m_data->m_hasGround = true;
int bodyId = 0;
loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
@@ -2950,7 +2969,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());
loadUrdf("teddy_vhacd.urdf", btVector3(-0.1, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere_small.urdf", btVector3(-0.1, 0.6, 1.25), gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
@@ -2999,7 +3019,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
gSubStep = m_data->m_physicsDeltaTime;
}
int numSteps = m_data->m_dynamicsWorld->stepSimulation(dtInSec,maxSteps, gSubStep);
int numSteps = m_data->m_dynamicsWorld->stepSimulation(dtInSec*simTimeScalingFactor,maxSteps, gSubStep);
gDroppedSimulationSteps += numSteps > maxSteps ? numSteps - maxSteps : 0;
if (numSteps)

83
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -23,6 +23,9 @@ extern btVector3 gVRGripperPos;
extern btQuaternion gVRGripperOrn;
extern btScalar gVRGripperAnalog;
extern bool gEnableRealTimeSimVR;
extern int gCreateObjectSimVR;
static int gGraspingController = -1;
extern btScalar simTimeScalingFactor;
extern bool gVRGripperClosed;
@@ -54,6 +57,7 @@ enum MultiThreadedGUIHelperCommunicationEnums
eGUIHelperRegisterGraphicsInstance,
eGUIHelperCreateCollisionShapeGraphicsObject,
eGUIHelperCreateCollisionObjectGraphicsObject,
eGUIHelperCreateRigidBodyGraphicsObject,
eGUIHelperRemoveAllGraphicsInstances,
eGUIHelperCopyCameraImageData,
};
@@ -305,7 +309,20 @@ public:
return m_cs;
}
virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color){}
btRigidBody* m_body;
btVector3 m_color3;
virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color)
{
m_body = body;
m_color3 = color;
m_cs->lock();
m_cs->setSharedParam(1,eGUIHelperCreateRigidBodyGraphicsObject);
m_cs->unlock();
while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
{
b3Clock::usleep(1000);
}
}
btCollisionObject* m_obj;
btVector3 m_color2;
@@ -776,6 +793,14 @@ void PhysicsServerExample::stepSimulation(float deltaTime)
m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eGUIHelperCreateRigidBodyGraphicsObject:
{
m_multiThreadedHelper->m_childGuiHelper->createRigidBodyGraphicsObject(m_multiThreadedHelper->m_body,m_multiThreadedHelper->m_color3);
m_multiThreadedHelper->getCriticalSection()->lock();
m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eGUIHelperRegisterTexture:
{
@@ -937,10 +962,15 @@ void PhysicsServerExample::renderScene()
}
}
if (gDebugRenderToggle)
if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
{
gEnableRealTimeSimVR = true;
}
if (gDebugRenderToggle)
if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
{
B3_PROFILE("Draw Debug HUD");
//some little experiment to add text/HUD to a VR camera (HTC Vive/Oculus Rift)
@@ -970,7 +1000,7 @@ void PhysicsServerExample::renderScene()
count = 0;
sprintf(line0, "Graphics FPS (worse) = %f, frame %d", worseFps, frameCount / 2);
sprintf(line1, "Physics Steps = %d, Dropped = %d, dt %f, Substep %f)", gNumSteps, gDroppedSimulationSteps, gDtInSec, gSubStep);
sprintf(line1, "Physics Steps = %d, Drop = %d, time scale=%f, dt %f, Substep %f)", gNumSteps, gDroppedSimulationSteps, simTimeScalingFactor,gDtInSec, gSubStep);
gDroppedSimulationSteps = 0;
worseFps = 1000000;
@@ -996,15 +1026,25 @@ void PhysicsServerExample::renderScene()
m[14]=+gVRTeleportPos[2];
viewTr.setFromOpenGLMatrix(m);
btTransform viewTrInv = viewTr.inverse();
float upMag = -.6;
btVector3 side = viewTrInv.getBasis().getColumn(0);
btVector3 up = viewTrInv.getBasis().getColumn(1);
up+=0.6*side;
m_guiHelper->getAppInterface()->drawText3D(line0,pos[0]+upMag*up[0],pos[1]+upMag*up[1],pos[2]+upMag*up[2],1);
btVector3 fwd = viewTrInv.getBasis().getColumn(2);
float upMag = 0;
float sideMag = 2.2;
float fwdMag = -4;
m_guiHelper->getAppInterface()->drawText3D(line0,pos[0]+upMag*up[0]-sideMag*side[0]+fwdMag*fwd[0],pos[1]+upMag*up[1]-sideMag*side[1]+fwdMag*fwd[1],pos[2]+upMag*up[2]-sideMag*side[2]+fwdMag*fwd[2],1);
//btVector3 fwd = viewTrInv.getBasis().getColumn(2);
upMag = -0.7;
m_guiHelper->getAppInterface()->drawText3D(line1,pos[0]+upMag*up[0],pos[1]+upMag*up[1],pos[2]+upMag*up[2],1);
up = viewTrInv.getBasis().getColumn(1);
upMag = -0.3;
m_guiHelper->getAppInterface()->drawText3D(line1,pos[0]+upMag*up[0]-sideMag*side[0]+fwdMag*fwd[0],pos[1]+upMag*up[1]-sideMag*side[1]+fwdMag*fwd[1],pos[2]+upMag*up[2]-sideMag*side[2]+fwdMag*fwd[2],1);
}
//m_args[0].m_cs->unlock();
@@ -1112,7 +1152,6 @@ class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOpt
}
static int gGraspingController = -1;
void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orn[4])
{
@@ -1130,10 +1169,34 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
{
gVRTeleportPos = gLastPickPos;
}
} else
{
if (button == 1)
{
if (state == 1)
{
gDebugRenderToggle = 1;
} else
{
gDebugRenderToggle = 0;
simTimeScalingFactor *= 0.5;
if (simTimeScalingFactor==0)
{
simTimeScalingFactor = 1;
}
if (simTimeScalingFactor<0.01)
{
simTimeScalingFactor = 0;
}
}
} else
{
}
}
if (button==32 && state==0)
{
gDebugRenderToggle = !gDebugRenderToggle;
gCreateObjectSimVR = 1;
}