Merge branch 'master' of https://github.com/erwincoumans/bullet3

examples/CommonInterfaces/CommonParameterInterface.h

@@ -26,7 +26,7 @@ struct SliderParams
 	m_callback(0),
 	m_paramValuePointer(targetValuePointer),
 	m_userPointer(0),
-	m_clampToNotches(true),
+	m_clampToNotches(false),
     m_clampToIntegers(false),
     m_showValues(true)
 	{

examples/CommonInterfaces/CommonRenderInterface.h

@@ -28,6 +28,8 @@ struct CommonRenderInterface
 	virtual CommonCameraInterface* getActiveCamera()=0;
 	virtual void setActiveCamera(CommonCameraInterface* cam)=0;
 	
+	virtual void setLightPosition(const float lightPos[3]) = 0;
+	virtual void setLightPosition(const double lightPos[3]) = 0;
 
 	virtual void renderScene()=0;
 	virtual void renderSceneInternal(int renderMode=B3_DEFAULT_RENDERMODE){};

examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp

@@ -1741,6 +1741,11 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
 				}
 				break;
 			}
+			case URDF_GEOM_UNKNOWN:
+                        {
+				break;
+			}
+
 			} // switch geom
 
 			if (childShape)

examples/Importers/ImportURDFDemo/UrdfParser.h

@@ -50,9 +50,9 @@ enum UrdfGeomTypes
 	URDF_GEOM_BOX,
 	URDF_GEOM_CYLINDER,
 	URDF_GEOM_MESH,
-    URDF_GEOM_PLANE,
+	URDF_GEOM_PLANE,
-	URDF_GEOM_CAPSULE//non-standard URDF?
+	URDF_GEOM_CAPSULE, //non-standard URDF?
-    
+	URDF_GEOM_UNKNOWN, 
 };
 
 
@@ -83,6 +83,14 @@ struct UrdfGeometry
 
 	UrdfMaterial m_localMaterial;
 	bool m_hasLocalMaterial;
+
+	UrdfGeometry()
+	:m_type(URDF_GEOM_UNKNOWN),
+	m_hasFromTo(false),
+	m_hasLocalMaterial(false)
+	{
+	}
+
 };
 
 bool findExistingMeshFile(const std::string& urdf_path, std::string fn,

examples/OpenGLWindow/GLInstancingRenderer.cpp

@@ -79,7 +79,6 @@ float shadowMapWorldSize=10;
 static InternalDataRenderer* sData2;
 
 GLint lineWidthRange[2]={1,1};
-static b3Vector3 gLightPos=b3MakeVector3(-5,12,-4);
 
 struct b3GraphicsInstance
 {
@@ -156,6 +155,7 @@ struct InternalDataRenderer : public GLInstanceRendererInternalData
 	GLfloat m_projectionMatrix[16];
 	GLfloat m_viewMatrix[16];
 
+	b3Vector3 m_lightPos;
 
 	GLuint				m_defaultTexturehandle;
 	b3AlignedObjectArray<InternalTextureHandle>	m_textureHandles;
@@ -172,6 +172,8 @@ struct InternalDataRenderer : public GLInstanceRendererInternalData
 		m_shadowTexture(0),
 		m_renderFrameBuffer(0)
 	{
+		m_lightPos=b3MakeVector3(-50,30,100);
+
 		//clear to zero to make it obvious if the matrix is used uninitialized
 		for (int i=0;i<16;i++)
 		{
@@ -996,23 +998,26 @@ void GLInstancingRenderer::setActiveCamera(CommonCameraInterface* cam)
 	m_data->m_activeCamera = cam;
 }
 
+void GLInstancingRenderer::setLightPosition(const float lightPos[3])
+{
+	m_data->m_lightPos[0] = lightPos[0];
+	m_data->m_lightPos[1] = lightPos[1];
+	m_data->m_lightPos[2] = lightPos[2];
+}
+
+void GLInstancingRenderer::setLightPosition(const double lightPos[3])
+{
+	m_data->m_lightPos[0] = lightPos[0];
+	m_data->m_lightPos[1] = lightPos[1];
+	m_data->m_lightPos[2] = lightPos[2];
+}
+
 
 void GLInstancingRenderer::updateCamera(int upAxis)
 {
 
     b3Assert(glGetError() ==GL_NO_ERROR);
 	m_upAxis = upAxis;
-	switch (upAxis)
-	{
-    case 1:
-    		gLightPos = b3MakeVector3(-50.f,100,30);
-    	break;
-    case 2:
-			gLightPos = b3MakeVector3(-50.f,30,100);
-			break;
-    default:
-    b3Assert(0);
-	};
 
 	m_data->m_activeCamera->setCameraUpAxis(upAxis);
 	m_data->m_activeCamera->setAspectRatio((float)m_screenWidth/(float)m_screenHeight);
@@ -1554,9 +1559,10 @@ void GLInstancingRenderer::renderSceneInternal(int renderMode)
 	//float upf[3];
 	//m_data->m_activeCamera->getCameraUpVector(upf);
 	b3Vector3 up, fwd;
-	b3PlaneSpace1(gLightPos,up,fwd);
+	b3Vector3 lightDir = m_data->m_lightPos.normalized();
+	b3PlaneSpace1(lightDir,up,fwd);
 //	b3Vector3 up = b3MakeVector3(upf[0],upf[1],upf[2]);
-	b3CreateLookAt(gLightPos,center,up,&depthViewMatrix[0][0]);
+	b3CreateLookAt(m_data->m_lightPos,center,up,&depthViewMatrix[0][0]);
 	//b3CreateLookAt(lightPos,m_data->m_cameraTargetPosition,b3Vector3(0,1,0),(float*)depthModelViewMatrix2);
 
 	GLfloat depthModelMatrix[4][4];
@@ -1726,7 +1732,7 @@ b3Assert(glGetError() ==GL_NO_ERROR);
 							glUniformMatrix4fv(ProjectionMatrix, 1, false, &m_data->m_projectionMatrix[0]);
 							glUniformMatrix4fv(ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]);
 							
-							b3Vector3 gLightDir = gLightPos;
+							b3Vector3 gLightDir = m_data->m_lightPos;
 							gLightDir.normalize();
 							glUniform3f(regularLightDirIn,gLightDir[0],gLightDir[1],gLightDir[2]);
 
@@ -1763,7 +1769,7 @@ b3Assert(glGetError() ==GL_NO_ERROR);
 							float MVP[16];
 							b3Matrix4x4Mul16(m_data->m_projectionMatrix,m_data->m_viewMatrix,MVP);
 							glUniformMatrix4fv(useShadow_MVP, 1, false, &MVP[0]);
-							b3Vector3 gLightDir = gLightPos;
+							b3Vector3 gLightDir = m_data->m_lightPos;
 							gLightDir.normalize();
 							glUniform3f(useShadow_lightDirIn,gLightDir[0],gLightDir[1],gLightDir[2]);
 							glUniformMatrix4fv(useShadow_DepthBiasModelViewMatrix, 1, false, &depthBiasMVP[0][0]);

examples/OpenGLWindow/GLInstancingRenderer.h

@@ -117,7 +117,9 @@ public:
 	virtual CommonCameraInterface* getActiveCamera();
 	virtual void setActiveCamera(CommonCameraInterface* cam);
 	
-	
+	virtual void setLightPosition(const float lightPos[3]);
+	virtual void setLightPosition(const double lightPos[3]);
+
 	virtual void	resize(int width, int height);
 	virtual int	getScreenWidth()
 	{

examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.glsl

@@ -39,7 +39,9 @@ void main(void)
 	
 
 	float visibility = texture(shadowMap, vec3(ShadowCoord.xy,(ShadowCoord.z)/ShadowCoord.w));
-	
+	if (intensity<0.5)
+		visibility = 0;
+
 	intensity = 0.7*intensity  + 0.3*intensity*visibility;
 	
 	cf = intensity*(vec3(1.0,1.0,1.0)-ambient)+ambient;

examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.h

@@ -32,7 +32,8 @@ static const char* useShadowMapInstancingFragmentShader= \
 "	\n"
 "	\n"
 "	float visibility = texture(shadowMap, vec3(ShadowCoord.xy,(ShadowCoord.z)/ShadowCoord.w));\n"
-"	\n"
+"	if (intensity<0.5)\n"
+"		visibility = 0;\n"
 "	intensity = 0.7*intensity  + 0.3*intensity*visibility;\n"
 "	\n"
 "	cf = intensity*(vec3(1.0,1.0,1.0)-ambient)+ambient;\n"

examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp

@@ -78,6 +78,14 @@ void SimpleOpenGL2Renderer::setActiveCamera(CommonCameraInterface* cam)
 	b3Assert(0);//not supported yet
 }
 
+void SimpleOpenGL2Renderer::setLightPosition(const float lightPos[3])
+{
+}
+void SimpleOpenGL2Renderer::setLightPosition(const double lightPos[3])
+{
+}
+
+
 void SimpleOpenGL2Renderer::updateCamera(int upAxis)
 {
     float projection[16];

examples/OpenGLWindow/SimpleOpenGL2Renderer.h

@@ -25,6 +25,10 @@ public:
 	virtual CommonCameraInterface* getActiveCamera();
 	virtual void setActiveCamera(CommonCameraInterface* cam);
 
+	virtual void setLightPosition(const float lightPos[3]);
+	virtual void setLightPosition(const double lightPos[3]);
+
+
 	virtual void	resize(int width, int height);
 
     virtual void removeAllInstances();

examples/RenderingExamples/TinyRendererSetup.cpp

@@ -18,6 +18,8 @@
 #include "../CommonInterfaces/CommonParameterInterface.h"
 
 
+
+
 struct TinyRendererSetupInternalData
 {
 	
@@ -42,6 +44,8 @@ struct TinyRendererSetupInternalData
 	int m_textureHandle;
 	int m_animateRenderer;
 
+	btVector3 m_lightPos;
+
 	TinyRendererSetupInternalData(int width, int height)
 		:
 		m_rgbColorBuffer(width,height,TGAImage::RGB),
@@ -53,6 +57,8 @@ struct TinyRendererSetupInternalData
 	m_textureHandle(0),
 		m_animateRenderer(0)
 	{
+		m_lightPos.setValue(-3,15,15);
+		
 		m_depthBuffer.resize(m_width*m_height);
         m_shadowBuffer.resize(m_width*m_height);
 //        m_segmentationMaskBuffer.resize(m_width*m_height);
@@ -115,9 +121,7 @@ struct TinyRendererSetup : public CommonExampleInterface
 
 	virtual bool	keyboardCallback(int key, int state);
 
-	virtual void	renderScene()
+	virtual void	renderScene();
-	{
-	}
    
     void animateRenderer(int animateRendererIndex)
     {
@@ -295,6 +299,28 @@ void TinyRendererSetup::initPhysics()
     m_guiHelper->getParameterInterface()->registerComboBox( comboParams);
     }
     
+	{
+        SliderParams slider("LightPosX",&m_internalData->m_lightPos[0]);
+        slider.m_minVal=-10;
+        slider.m_maxVal=10;
+		if (m_guiHelper->getParameterInterface())
+	        m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    }
+	{
+        SliderParams slider("LightPosY",&m_internalData->m_lightPos[1]);
+        slider.m_minVal=-10;
+        slider.m_maxVal=10;
+		if (m_guiHelper->getParameterInterface())
+	        m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    }
+	{
+        SliderParams slider("LightPosZ",&m_internalData->m_lightPos[2]);
+        slider.m_minVal=-10;
+        slider.m_maxVal=10;
+		if (m_guiHelper->getParameterInterface())
+	        m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    }
+
 }
 
 
@@ -303,14 +329,33 @@ void TinyRendererSetup::exitPhysics()
 
 }
 
-
 void TinyRendererSetup::stepSimulation(float deltaTime)
+{
+    m_internalData->updateTransforms();
+}
+
+void	TinyRendererSetup::renderScene()
 {
     m_internalData->updateTransforms();
     
+	btVector4 from(m_internalData->m_lightPos[0],m_internalData->m_lightPos[1],m_internalData->m_lightPos[2],1);
+	btVector4 toX(m_internalData->m_lightPos[0]+0.1,m_internalData->m_lightPos[1],m_internalData->m_lightPos[2],1);
+	btVector4 toY(m_internalData->m_lightPos[0],m_internalData->m_lightPos[1]+0.1,m_internalData->m_lightPos[2],1);
+	btVector4 toZ(m_internalData->m_lightPos[0],m_internalData->m_lightPos[1],m_internalData->m_lightPos[2]+0.1,1);
+	btVector4 colorX(1,0,0,1);
+	btVector4 colorY(0,1,0,1);
+	btVector4 colorZ(0,0,1,1);
+	int width=2;
+	m_guiHelper->getRenderInterface()->drawLine( from,toX,colorX,width);
+	m_guiHelper->getRenderInterface()->drawLine( from,toY,colorY,width);
+	m_guiHelper->getRenderInterface()->drawLine( from,toZ,colorZ,width);
+
     if (!m_useSoftware)
     {
-        
+     
+		btVector3 lightPos(m_internalData->m_lightPos[0],m_internalData->m_lightPos[1],m_internalData->m_lightPos[2]);
+		m_guiHelper->getRenderInterface()->setLightPosition(lightPos);
+
         for (int i=0;i<m_internalData->m_transforms.size();i++)
         {
             m_guiHelper->getRenderInterface()->writeSingleInstanceTransformToCPU(m_internalData->m_transforms[i].getOrigin(),m_internalData->m_transforms[i].getRotation(),i);
@@ -358,17 +403,7 @@ void TinyRendererSetup::stepSimulation(float deltaTime)
                     m_internalData->m_renderObjects[o]->m_viewMatrix[i][j] = viewMat[i+4*j];
                     m_internalData->m_renderObjects[o]->m_projectionMatrix[i][j] = projMat[i+4*j];
                     
-                    btVector3 lightDirWorld;
+                    btVector3 lightDirWorld = btVector3(m_internalData->m_lightPos[0],m_internalData->m_lightPos[1],m_internalData->m_lightPos[2]);
-                    switch (m_app->getUpAxis())
-                    {
-                        case 1:
-                            lightDirWorld = btVector3(-50.f,100,30);
-                            break;
-                        case 2:
-                            lightDirWorld = btVector3(-50.f,30,100);
-                            break;
-                        default:{}
-                    };
                     
                     m_internalData->m_renderObjects[o]->m_lightDirWorld = lightDirWorld.normalized();
                     
@@ -399,17 +434,7 @@ void TinyRendererSetup::stepSimulation(float deltaTime)
                     m_internalData->m_renderObjects[o]->m_viewMatrix[i][j] = viewMat[i+4*j];
                     m_internalData->m_renderObjects[o]->m_projectionMatrix[i][j] = projMat[i+4*j];
                     
-					btVector3 lightDirWorld;
+					btVector3 lightDirWorld = btVector3(m_internalData->m_lightPos[0],m_internalData->m_lightPos[1],m_internalData->m_lightPos[2]);
-					switch (m_app->getUpAxis())
-					{
-					case 1:
-    						lightDirWorld = btVector3(-50.f,100,30);
-    					break;
-					case 2:
-							lightDirWorld = btVector3(-50.f,30,100);
-							break;
-					default:{}
-					};
 					
 					m_internalData->m_renderObjects[o]->m_lightDirWorld = lightDirWorld.normalized();
                     

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -2792,10 +2792,21 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 								fwrite(line,len,1,f);
 							}
 							{
-								sprintf(line,"p.connect(p.SHARED_MEMORY)\n");
+								sprintf(line,"cin = p.connect(p.SHARED_MEMORY)\n");
 								int len = strlen(line);
 								fwrite(line,len,1,f);
 							}
+							{
+								sprintf(line,"if (cin < 0):\n");
+								int len = strlen(line);
+								fwrite(line,len,1,f);
+							}
+							{
+								sprintf(line,"    cin = p.connect(p.GUI)\n");
+								int len = strlen(line);
+								fwrite(line,len,1,f);
+							}
+
 							//for each objects ...
 							for (int i=0;i<m_data->m_saveWorldBodyData.size();i++)
 							{
@@ -2830,15 +2841,21 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 													int len = strlen(line);
 													fwrite(line,len,1,f);
 												}
+												if (strstr(sd.m_fileName.c_str(),".xml") && i==0)
+												{
+													sprintf(line,"objects = p.loadMJCF(\"%s\")\n",sd.m_fileName.c_str());
+													int len = strlen(line);
+													fwrite(line,len,1,f);
+												}
 
-												if (strstr(sd.m_fileName.c_str(),".sdf") || ((strstr(sd.m_fileName.c_str(),".urdf")) && mb->getNumLinks()) )
+												if (strstr(sd.m_fileName.c_str(),".sdf") || strstr(sd.m_fileName.c_str(),".xml") || ((strstr(sd.m_fileName.c_str(),".urdf")) && mb->getNumLinks()) )
 												{
 													sprintf(line,"ob = objects[%d]\n",i);
 													int len = strlen(line);
 													fwrite(line,len,1,f);
 												}
 
-												if (strstr(sd.m_fileName.c_str(),".sdf"))
+												if (strstr(sd.m_fileName.c_str(),".sdf")||strstr(sd.m_fileName.c_str(),".xml"))
 												{
 													sprintf(line,"p.resetBasePositionAndOrientation(ob,[%f,%f,%f],[%f,%f,%f,%f])\n",
 														comTr.getOrigin()[0],comTr.getOrigin()[1],comTr.getOrigin()[2],

examples/SharedMemory/SharedMemoryPublic.h

@@ -276,7 +276,7 @@ enum b3VREventType
 #define MAX_VR_BUTTONS 64
 #define MAX_VR_CONTROLLERS 8
 
-#define MAX_RAY_INTERSECTION_BATCH_SIZE 1024
+#define MAX_RAY_INTERSECTION_BATCH_SIZE 256
 #define MAX_RAY_HITS MAX_RAY_INTERSECTION_BATCH_SIZE
 #define MAX_KEYBOARD_EVENTS 256
 

examples/TinyRenderer/TinyRenderer.cpp

@@ -426,14 +426,6 @@ static bool clipTriangleAgainstNearplane(const mat<4,3,float>& triangleIn, b3Ali
 {
 
 
-	float orientation = (triangleIn[0][1] - triangleIn[0][0]) * (triangleIn[1][2] - triangleIn[1][0]) 
-	- (triangleIn[1][1] - triangleIn[1][0]) * (triangleIn[0][2] - triangleIn[0][0]);
-	
-	if (orientation < 0.0) 
-	{
-		return true;
-	}
-
 	//discard triangle if all vertices are behind near-plane
 	if (triangleIn[3][0]<0 && triangleIn[3][1] <0 && triangleIn[3][2] <0)
 	{

examples/pybullet/examples/hand.ino

@@ -10,6 +10,10 @@ int sensorPin3 = A3;
 void setup() {
   // put your setup code here, to run once:
   Serial.begin(115200);
+  digitalWrite(A0, INPUT_PULLUP);
+  digitalWrite(A1, INPUT_PULLUP);
+  digitalWrite(A2, INPUT_PULLUP);
+  digitalWrite(A3, INPUT_PULLUP);
 }
 
 void loop() {

examples/pybullet/examples/hand.py

@@ -20,12 +20,28 @@ objects = p.loadMJCF("MPL/MPL.xml")
 
 hand=objects[0]
 #clamp in range 400-600
-minV = 400
+#minV = 400
-maxV = 600
+#maxV = 600
+minVarray = [275,280,350,290]
+maxVarray = [450,550,500,400]
+
+pinkId = 0
+middleId = 1
+indexId = 2
+thumbId = 3
 
 p.setRealTimeSimulation(1)
 
-def convertSensor(x):
+def getSerialOrNone(portname):
+    try:
+       return serial.Serial(port=portname,baudrate=115200,parity=serial.PARITY_ODD,stopbits=serial.STOPBITS_TWO,bytesize=serial.SEVENBITS)
+    except:
+       return None
+
+def convertSensor(x, fingerIndex):
+	minV = minVarray[fingerIndex]
+	maxV = maxVarray[fingerIndex]
+	
 	v = minV
 	try:
 		v = float(x)
@@ -36,22 +52,39 @@ def convertSensor(x):
 	if (v>maxV):
 		v=maxV
 	b = (v-minV)/float(maxV-minV)
-	return (1.0-b)
+	return (b)
-	
+
-ser = serial.Serial(port='COM13',baudrate=115200,parity=serial.PARITY_ODD,stopbits=serial.STOPBITS_TWO,bytesize=serial.SEVENBITS)
+ser = None
-if (ser.isOpen()):
+portindex = 0
+while (ser is None and portindex < 30):
+	portname = 'COM'+str(portindex)
+	print(portname)
+	ser = getSerialOrNone(portname)
+	if (ser is None):
+		portname = "/dev/cu.usbmodem14"+str(portindex)
+		print(portname)
+		ser = getSerialOrNone(portname)
+		if (ser is not None):
+			print("COnnected!")
+	portindex = portindex+1
+
+if (ser is None):
+	ser = serial.Serial(port = "/dev/cu.usbmodem1421",baudrate=115200,parity=serial.PARITY_ODD,stopbits=serial.STOPBITS_TWO,bytesize=serial.SEVENBITS)
+pi=3.141592
+
+if (ser is not None and ser.isOpen()):
 	while True:
 		while ser.inWaiting() > 0:
 			line = str(ser.readline())
 			words = line.split(",")
 			if (len(words)==6):
-				middle = convertSensor(words[1])
+				pink = convertSensor(words[1],pinkId)
-				pink = convertSensor(words[2])
+				middle = convertSensor(words[2],middleId)
-				index = convertSensor(words[3])
+				index = convertSensor(words[3],indexId)
-				thumb = convertSensor(words[4])
+				thumb = convertSensor(words[4],thumbId)
-
+			
-				p.setJointMotorControl2(hand,7,p.POSITION_CONTROL,thumb)
+				p.setJointMotorControl2(hand,7,p.POSITION_CONTROL,pi/4.)	
-				p.setJointMotorControl2(hand,9,p.POSITION_CONTROL,thumb)
+				p.setJointMotorControl2(hand,9,p.POSITION_CONTROL,thumb+pi/10)
 				p.setJointMotorControl2(hand,11,p.POSITION_CONTROL,thumb)
 				p.setJointMotorControl2(hand,13,p.POSITION_CONTROL,thumb)
 				
@@ -76,3 +109,5 @@ if (ser.isOpen()):
 				#print(pink)
 				#print(index)
 				#print(thumb)
+else:
+	print("Cannot find port")

examples/pybullet/examples/vrhand_vive_tracker.py

@@ -0,0 +1,149 @@
+#script to control a simulated robot hand using a VR glove
+#see https://twitter.com/erwincoumans/status/821953216271106048
+#and https://www.youtube.com/watch?v=I6s37aBXbV8
+#vr glove was custom build using Spectra Symbolflex sensors (4.5")
+#inside a Under Armour Batting Glove, using DFRobot Bluno BLE/Beetle
+#with BLE Link to receive serial (for wireless bluetooth serial)
+
+import serial
+import time
+import pybullet as p
+
+#first try to connect to shared memory (VR), if it fails use local GUI
+c = p.connect(p.SHARED_MEMORY)
+if (c<0):
+	c = p.connect(p.GUI)
+	
+p.setInternalSimFlags(0)#don't load default robot assets etc
+p.resetSimulation()
+
+#p.resetSimulation()
+p.setGravity(0,0,-10)
+objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
+
+objects = [p.loadURDF("jenga/jenga.urdf", 1.300000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
+objects = [p.loadURDF("jenga/jenga.urdf", 1.200000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
+objects = [p.loadURDF("jenga/jenga.urdf", 1.100000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
+objects = [p.loadURDF("jenga/jenga.urdf", 1.000000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
+objects = [p.loadURDF("jenga/jenga.urdf", 0.900000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
+objects = [p.loadURDF("jenga/jenga.urdf", 0.800000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
+objects = [p.loadURDF("table/table.urdf", 1.000000,-0.200000,0.000000,0.000000,0.000000,0.707107,0.707107)]
+objects = [p.loadURDF("cube_small.urdf", 0.950000,-0.100000,0.700000,0.000000,0.000000,0.707107,0.707107)]
+objects = [p.loadURDF("sphere_small.urdf", 0.850000,-0.400000,0.700000,0.000000,0.000000,0.707107,0.707107)]
+
+		
+#load the MuJoCo MJCF hand
+objects = p.loadMJCF("MPL/mpl2.xml")
+
+hand=objects[0]
+ho = p.getQuaternionFromEuler([3.14,-3.14/2,0])
+hand_cid = p.createConstraint(hand,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[-0.05,0,0.02],[0.500000,0.300006,0.700000],ho)
+print ("hand_cid")
+print (hand_cid)
+for i in range (p.getNumJoints(hand)):
+	p.setJointMotorControl2(hand,i,p.POSITION_CONTROL,0,0)
+
+
+#clamp in range 400-600
+#minV = 400
+#maxV = 600
+minV = 250
+maxV = 450
+
+
+POSITION=1
+ORIENTATION=2
+BUTTONS=6
+
+p.setRealTimeSimulation(1)
+
+def convertSensor(x):
+	v = minV
+	try:
+		v = float(x)
+	except ValueError:
+		v = minV    
+	if (v<minV):
+		v=minV
+	if (v>maxV):
+		v=maxV
+	b = (v-minV)/float(maxV-minV)
+	return (b)
+	
+controllerId = -1
+
+serialSteps = 0
+serialStepsUntilCheckVREvents = 3
+
+def getSerialOrNone(portname):
+    try:
+       return serial.Serial(port=portname,baudrate=115200,parity=serial.PARITY_ODD,stopbits=serial.STOPBITS_TWO,bytesize=serial.SEVENBITS)
+    except:
+       return None
+
+
+ser = None
+
+portindex = 10
+while (ser is None and portindex < 30):
+	portname = 'COM'+str(portindex)
+	print(portname)
+	ser = getSerialOrNone(portname)
+	if (ser is None):
+		portname = "/dev/cu.usbmodem14"+str(portindex)
+		print(portname)
+		ser = getSerialOrNone(portname)
+		if (ser is not None):
+			print("COnnected!")
+	portindex = portindex+1
+
+p.saveWorld("setupTrackerWorld.py")
+
+
+if (ser.isOpen()):
+	while True:
+
+		events = p.getVREvents(deviceTypeFilter=p.VR_DEVICE_GENERIC_TRACKER)
+		for e in (events):
+				p.changeConstraint(hand_cid,e[POSITION],e[ORIENTATION], maxForce=50)
+			
+		serialSteps = 0
+		while ser.inWaiting() > 0:
+			serialSteps=serialSteps+1
+			if (serialSteps>serialStepsUntilCheckVREvents):
+				ser.flushInput()
+				break
+			line = str(ser.readline())
+			words = line.split(",")
+			if (len(words)==6):
+				pink = convertSensor(words[1])
+				middle = convertSensor(words[2])
+				index = convertSensor(words[3])
+				thumb = convertSensor(words[4])+0.2
+
+				p.setJointMotorControl2(hand,5,p.POSITION_CONTROL,1.3)
+				p.setJointMotorControl2(hand,7,p.POSITION_CONTROL,thumb)
+				p.setJointMotorControl2(hand,9,p.POSITION_CONTROL,thumb)
+				p.setJointMotorControl2(hand,11,p.POSITION_CONTROL,thumb)
+				
+				p.setJointMotorControl2(hand,15,p.POSITION_CONTROL,index)
+				p.setJointMotorControl2(hand,17,p.POSITION_CONTROL,index)
+				p.setJointMotorControl2(hand,19,p.POSITION_CONTROL,index)
+
+				p.setJointMotorControl2(hand,22,p.POSITION_CONTROL,middle)
+				p.setJointMotorControl2(hand,24,p.POSITION_CONTROL,middle)
+				p.setJointMotorControl2(hand,26,p.POSITION_CONTROL,middle)
+				
+				p.setJointMotorControl2(hand,38,p.POSITION_CONTROL,pink)
+				p.setJointMotorControl2(hand,40,p.POSITION_CONTROL,pink)
+				p.setJointMotorControl2(hand,42,p.POSITION_CONTROL,pink)
+
+				ringpos = 0.5*(pink+middle)
+				p.setJointMotorControl2(hand,30,p.POSITION_CONTROL,ringpos)
+				p.setJointMotorControl2(hand,32,p.POSITION_CONTROL,ringpos)
+				p.setJointMotorControl2(hand,34,p.POSITION_CONTROL,ringpos)
+								
+				#print(middle)
+				#print(pink)
+				#print(index)
+				#print(thumb)