diff --git a/build_visual_studio.bat b/build_visual_studio.bat index 399461122..0b852e498 100644 --- a/build_visual_studio.bat +++ b/build_visual_studio.bat @@ -1,6 +1,6 @@ cd build3 -premake4 --enable_openvr --targetdir="../bin" vs2010 +rem premake4 --enable_openvr --targetdir="../bin" vs2010 -rem premake4 --double --enable_pybullet --python_include_dir="C:/Python-3.6.0a3/include" --python_lib_dir="C:/Python-3.6.0a3/libs" --enable_openvr --targetdir="../bin" vs2010 +premake4 --double --enable_pybullet --python_include_dir="C:/Python-3.5.2/include" --python_lib_dir="C:/Python-3.5.2/libs" --enable_openvr --targetdir="../bin" vs2010 pause diff --git a/data/sphere_small.urdf b/data/sphere_small.urdf new file mode 100644 index 000000000..c5712bc35 --- /dev/null +++ b/data/sphere_small.urdf @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/teddy_vhacd.urdf b/data/teddy_vhacd.urdf index 7df9e7919..1868cb9ff 100644 --- a/data/teddy_vhacd.urdf +++ b/data/teddy_vhacd.urdf @@ -8,23 +8,23 @@ - - + + - + - - + + - + diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 320c0b6e6..1f16758a4 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -401,15 +401,18 @@ bool BulletURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWor return true; } -static btCollisionShape* createConvexHullFromShapes(std::vector& shapes) +static btCollisionShape* createConvexHullFromShapes(std::vector& 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::vectoraddPoint(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 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 { diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 85839e322..b12ee1af0 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -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 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) diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 2dec1401c..e0f495ccf 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -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; }