From 5b789ed67b0b9cf253875432606fc9e80bde63ef Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 19 Apr 2017 12:06:26 -0700 Subject: [PATCH 1/8] reduce max ray hits to 256 --- examples/SharedMemory/SharedMemoryPublic.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index ab906172e..466f3dcb3 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/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 From d2ff554e37903df5d517a2e24cec344ec674bd56 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 20 Apr 2017 14:44:47 -0700 Subject: [PATCH 2/8] tweak hand.py script to automatically find the port on Windows (COM0 to COM30) and Mac OSX. --- examples/pybullet/examples/hand.py | 36 +++++++++++++++++++++++++----- 1 file changed, 31 insertions(+), 5 deletions(-) diff --git a/examples/pybullet/examples/hand.py b/examples/pybullet/examples/hand.py index cb4b45f87..10c9fbd89 100644 --- a/examples/pybullet/examples/hand.py +++ b/examples/pybullet/examples/hand.py @@ -20,11 +20,19 @@ objects = p.loadMJCF("MPL/MPL.xml") hand=objects[0] #clamp in range 400-600 -minV = 400 -maxV = 600 +#minV = 400 +#maxV = 600 +minV = 250 +maxV = 450 p.setRealTimeSimulation(1) +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): v = minV try: @@ -37,9 +45,25 @@ def convertSensor(x): v=maxV b = (v-minV)/float(maxV-minV) return (1.0-b) - -ser = serial.Serial(port='COM13',baudrate=115200,parity=serial.PARITY_ODD,stopbits=serial.STOPBITS_TWO,bytesize=serial.SEVENBITS) -if (ser.isOpen()): + +ser = None +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) + +if (ser is not None and ser.isOpen()): while True: while ser.inWaiting() > 0: line = str(ser.readline()) @@ -76,3 +100,5 @@ if (ser.isOpen()): #print(pink) #print(index) #print(thumb) +else: + print("Cannot find port") From e8c1602232d0105be0cf1b321727c9f4beafd758 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 21 Apr 2017 10:28:20 -0700 Subject: [PATCH 3/8] add vrhand for vive tracker tweak 'saveWorld' feature a bit (mjcf, gui fallback if shared memory server is not available) --- .../PhysicsServerCommandProcessor.cpp | 23 ++- .../pybullet/examples/vrhand_vive_tracker.py | 149 ++++++++++++++++++ 2 files changed, 169 insertions(+), 3 deletions(-) create mode 100644 examples/pybullet/examples/vrhand_vive_tracker.py diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 11c58398c..625c99d33 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/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;im_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], diff --git a/examples/pybullet/examples/vrhand_vive_tracker.py b/examples/pybullet/examples/vrhand_vive_tracker.py new file mode 100644 index 000000000..1242829dd --- /dev/null +++ b/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 (vmaxV): + 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) \ No newline at end of file From 4759e5a590acc6130f2bdbfdc9df87680e727333 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 23 Apr 2017 07:35:13 -0700 Subject: [PATCH 4/8] tinyrenderer: disable triangle backface culling (doesn't work well, cull triangles that should be visible) GLInstancingRenderer: allow to set the light position --- .../CommonParameterInterface.h | 2 +- .../CommonInterfaces/CommonRenderInterface.h | 2 + .../OpenGLWindow/GLInstancingRenderer.cpp | 38 +++++---- examples/OpenGLWindow/GLInstancingRenderer.h | 4 +- .../Shaders/useShadowMapInstancingPS.glsl | 4 +- .../Shaders/useShadowMapInstancingPS.h | 3 +- .../OpenGLWindow/SimpleOpenGL2Renderer.cpp | 8 ++ examples/OpenGLWindow/SimpleOpenGL2Renderer.h | 4 + .../RenderingExamples/TinyRendererSetup.cpp | 81 ++++++++++++------- examples/TinyRenderer/TinyRenderer.cpp | 8 -- 10 files changed, 99 insertions(+), 55 deletions(-) diff --git a/examples/CommonInterfaces/CommonParameterInterface.h b/examples/CommonInterfaces/CommonParameterInterface.h index da5b35342..214c07cd9 100644 --- a/examples/CommonInterfaces/CommonParameterInterface.h +++ b/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) { diff --git a/examples/CommonInterfaces/CommonRenderInterface.h b/examples/CommonInterfaces/CommonRenderInterface.h index 0800b9b9d..69ee16117 100644 --- a/examples/CommonInterfaces/CommonRenderInterface.h +++ b/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){}; diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp index 31e30527f..8dfa8d9d5 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.cpp +++ b/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 m_textureHandles; @@ -172,6 +172,8 @@ struct InternalDataRenderer : public GLInstanceRendererInternalData m_shadowTexture(0), m_renderFrameBuffer(0) { + m_lightPos=b3MakeVector3(-5,50,50); + //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]); diff --git a/examples/OpenGLWindow/GLInstancingRenderer.h b/examples/OpenGLWindow/GLInstancingRenderer.h index 9fae49db5..d42512280 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.h +++ b/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() { diff --git a/examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.glsl b/examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.glsl index 094ae7640..1e469134c 100644 --- a/examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.glsl +++ b/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; diff --git a/examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.h b/examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.h index f7f2a6a11..de316f9d7 100644 --- a/examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.h +++ b/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" diff --git a/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp b/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp index d93b09ed2..d39622e35 100644 --- a/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp +++ b/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]; diff --git a/examples/OpenGLWindow/SimpleOpenGL2Renderer.h b/examples/OpenGLWindow/SimpleOpenGL2Renderer.h index 57c6d1a36..57798e35b 100644 --- a/examples/OpenGLWindow/SimpleOpenGL2Renderer.h +++ b/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(); diff --git a/examples/RenderingExamples/TinyRendererSetup.cpp b/examples/RenderingExamples/TinyRendererSetup.cpp index f9fbae111..51c9119c1 100644 --- a/examples/RenderingExamples/TinyRendererSetup.cpp +++ b/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; + double m_lightPos[3]; + TinyRendererSetupInternalData(int width, int height) : m_rgbColorBuffer(width,height,TGAImage::RGB), @@ -53,6 +57,10 @@ struct TinyRendererSetupInternalData m_textureHandle(0), m_animateRenderer(0) { + m_lightPos[0] = -3; + m_lightPos[1] = 3; + m_lightPos[2] = 3; + m_depthBuffer.resize(m_width*m_height); m_shadowBuffer.resize(m_width*m_height); // m_segmentationMaskBuffer.resize(m_width*m_height); @@ -115,9 +123,7 @@ struct TinyRendererSetup : public CommonExampleInterface virtual bool keyboardCallback(int key, int state); - virtual void renderScene() - { - } + virtual void renderScene(); void animateRenderer(int animateRendererIndex) { @@ -295,6 +301,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 +331,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;im_transforms.size();i++) { m_guiHelper->getRenderInterface()->writeSingleInstanceTransformToCPU(m_internalData->m_transforms[i].getOrigin(),m_internalData->m_transforms[i].getRotation(),i); @@ -358,17 +405,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; - switch (m_app->getUpAxis()) - { - case 1: - lightDirWorld = btVector3(-50.f,100,30); - break; - case 2: - lightDirWorld = btVector3(-50.f,30,100); - break; - default:{} - }; + btVector3 lightDirWorld = btVector3(m_internalData->m_lightPos[0],m_internalData->m_lightPos[1],m_internalData->m_lightPos[2]); m_internalData->m_renderObjects[o]->m_lightDirWorld = lightDirWorld.normalized(); @@ -399,17 +436,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; - switch (m_app->getUpAxis()) - { - case 1: - lightDirWorld = btVector3(-50.f,100,30); - break; - case 2: - lightDirWorld = btVector3(-50.f,30,100); - break; - default:{} - }; + btVector3 lightDirWorld = btVector3(m_internalData->m_lightPos[0],m_internalData->m_lightPos[1],m_internalData->m_lightPos[2]); m_internalData->m_renderObjects[o]->m_lightDirWorld = lightDirWorld.normalized(); diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index ed472a1f5..8d4aafb91 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/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) { From 552c85617f80515b310ef28a08ab053fb3d281d9 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 23 Apr 2017 17:12:40 -0700 Subject: [PATCH 5/8] fix compile issue --- examples/OpenGLWindow/GLInstancingRenderer.cpp | 2 +- examples/RenderingExamples/TinyRendererSetup.cpp | 8 +++----- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp index 8dfa8d9d5..f50af23ff 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.cpp +++ b/examples/OpenGLWindow/GLInstancingRenderer.cpp @@ -172,7 +172,7 @@ struct InternalDataRenderer : public GLInstanceRendererInternalData m_shadowTexture(0), m_renderFrameBuffer(0) { - m_lightPos=b3MakeVector3(-5,50,50); + 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++) diff --git a/examples/RenderingExamples/TinyRendererSetup.cpp b/examples/RenderingExamples/TinyRendererSetup.cpp index 51c9119c1..0a3025a63 100644 --- a/examples/RenderingExamples/TinyRendererSetup.cpp +++ b/examples/RenderingExamples/TinyRendererSetup.cpp @@ -44,7 +44,7 @@ struct TinyRendererSetupInternalData int m_textureHandle; int m_animateRenderer; - double m_lightPos[3]; + btVector3 m_lightPos; TinyRendererSetupInternalData(int width, int height) : @@ -57,10 +57,8 @@ struct TinyRendererSetupInternalData m_textureHandle(0), m_animateRenderer(0) { - m_lightPos[0] = -3; - m_lightPos[1] = 3; - m_lightPos[2] = 3; - + 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); From d41a2fdfd481088392f20996ccf1594344b909e3 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 24 Apr 2017 09:56:27 -0700 Subject: [PATCH 6/8] change hand.ino/py to match hardware changes (use pullup resistor, connect to gnd, instead of 5V + pull down resistor) --- examples/pybullet/examples/hand.ino | 4 ++++ examples/pybullet/examples/hand.py | 15 ++++++++------- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/examples/pybullet/examples/hand.ino b/examples/pybullet/examples/hand.ino index faf5b70bc..1e5f5df17 100644 --- a/examples/pybullet/examples/hand.ino +++ b/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() { diff --git a/examples/pybullet/examples/hand.py b/examples/pybullet/examples/hand.py index 10c9fbd89..2c16cfe88 100644 --- a/examples/pybullet/examples/hand.py +++ b/examples/pybullet/examples/hand.py @@ -23,7 +23,7 @@ hand=objects[0] #minV = 400 #maxV = 600 minV = 250 -maxV = 450 +maxV = 400 p.setRealTimeSimulation(1) @@ -44,7 +44,7 @@ def convertSensor(x): if (v>maxV): v=maxV b = (v-minV)/float(maxV-minV) - return (1.0-b) + return (b) ser = None portindex = 0 @@ -62,6 +62,7 @@ while (ser is None and portindex < 30): 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: @@ -69,13 +70,13 @@ if (ser is not None and ser.isOpen()): line = str(ser.readline()) words = line.split(",") if (len(words)==6): - middle = convertSensor(words[1]) - pink = convertSensor(words[2]) + pink = convertSensor(words[1]) + middle = convertSensor(words[2]) index = convertSensor(words[3]) thumb = convertSensor(words[4]) - - p.setJointMotorControl2(hand,7,p.POSITION_CONTROL,thumb) - p.setJointMotorControl2(hand,9,p.POSITION_CONTROL,thumb) + + p.setJointMotorControl2(hand,7,p.POSITION_CONTROL,pi/4.) + 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) From 771b197131f11777fcae7531b12e1486b5746527 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 25 Apr 2017 09:40:18 -0700 Subject: [PATCH 7/8] add option to calibrate individual fingers for the vr glove --- examples/pybullet/examples/hand.py | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/examples/pybullet/examples/hand.py b/examples/pybullet/examples/hand.py index 2c16cfe88..277e8ec79 100644 --- a/examples/pybullet/examples/hand.py +++ b/examples/pybullet/examples/hand.py @@ -22,8 +22,13 @@ hand=objects[0] #clamp in range 400-600 #minV = 400 #maxV = 600 -minV = 250 -maxV = 400 +minVarray = [275,280,350,290] +maxVarray = [450,550,500,400] + +pinkId = 0 +middleId = 1 +indexId = 2 +thumbId = 3 p.setRealTimeSimulation(1) @@ -33,7 +38,10 @@ def getSerialOrNone(portname): except: return None -def convertSensor(x): +def convertSensor(x, fingerIndex): + minV = minVarray[fingerIndex] + maxV = maxVarray[fingerIndex] + v = minV try: v = float(x) @@ -70,10 +78,10 @@ if (ser is not None and ser.isOpen()): 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]) + pink = convertSensor(words[1],pinkId) + middle = convertSensor(words[2],middleId) + index = convertSensor(words[3],indexId) + thumb = convertSensor(words[4],thumbId) p.setJointMotorControl2(hand,7,p.POSITION_CONTROL,pi/4.) p.setJointMotorControl2(hand,9,p.POSITION_CONTROL,thumb+pi/10) From e4ace90980ae5b5a17502225b6063313bbe5a373 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 25 Apr 2017 21:37:04 -0700 Subject: [PATCH 8/8] fix some potentially uninitialized variable --- .../ImportMJCFDemo/BulletMJCFImporter.cpp | 5 +++++ examples/Importers/ImportURDFDemo/UrdfParser.h | 14 +++++++++++--- 2 files changed, 16 insertions(+), 3 deletions(-) diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index 891c3c291..b7ca1f3b7 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/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) diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 680fbc1ca..3cdde368b 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/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_CAPSULE//non-standard URDF? - + URDF_GEOM_PLANE, + 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,