Add small sphere, make teddy small too

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.

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

data/sphere_small.urdf

@@ -0,0 +1,30 @@
+<?xml version="0.0" ?>
+<robot name="urdf_robot">
+  <link name="base_link">
+    <contact>
+      <rolling_friction value="0.001"/>
+      <spinning_friction value="0.001"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".1"/>
+       <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="textured_sphere_smooth.obj" scale="0.03 0.03 0.03"/>      
+      </geometry>
+      <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+	 <sphere radius="0.03"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
+

data/teddy_vhacd.urdf

@@ -8,23 +8,23 @@
       <contact_erp value="1.0"/>
     </contact>
     <inertial>
-      <origin rpy="0 0 0" xyz="0.7 0.5 0.3"/>
+      <origin rpy="0 0 0" xyz="0.07 0.05 0.03"/>
-       <mass value="1.0"/>
+       <mass value=".1"/>
        <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="teddy2_VHACD_CHs.obj" scale="1 1 1"/>
+        <mesh filename="teddy2_VHACD_CHs.obj" scale=".1 .1 .1"/>
       </geometry>
-       <material name="white">
+       <material name="red">
-        <color rgba="1 1 1 1"/>
+        <color rgba="1 0.4 0.4 1"/>
       </material>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-	 	<mesh filename="teddy2_VHACD_CHs.obj" scale="1 1 1"/>
+	 	<mesh filename="teddy2_VHACD_CHs.obj" scale=".1 .1 .1"/>
       </geometry>
     </collision>
   </link>

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -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));
+            btConvexHullShape* convexHull = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
-            cylZShape->setMargin(gUrdfDefaultCollisionMargin);
+            convexHull->setMargin(gUrdfDefaultCollisionMargin);
-			cylZShape->initializePolyhedralFeatures();
+			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* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
-								btConvexHullShape* cylZShape = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
+								convexHull->optimizeConvexHull();
-								//cylZShape->initializePolyhedralFeatures();
+								//convexHull->initializePolyhedralFeatures();
-								//btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
+								convexHull->setMargin(gUrdfDefaultCollisionMargin);
-								//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
+								shape = convexHull;
-								cylZShape->setMargin(gUrdfDefaultCollisionMargin);
-								shape = cylZShape;
 							}
 						} else
 						{

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<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)

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;
+		btVector3 fwd = viewTrInv.getBasis().getColumn(2);
-		m_guiHelper->getAppInterface()->drawText3D(line0,pos[0]+upMag*up[0],pos[1]+upMag*up[1],pos[2]+upMag*up[2],1);
+
+		
+		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;
+		up = viewTrInv.getBasis().getColumn(1);
-		m_guiHelper->getAppInterface()->drawText3D(line1,pos[0]+upMag*up[0],pos[1]+upMag*up[1],pos[2]+upMag*up[2],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;
 	}