AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-18 21:10:05 +00:00
Code Issues Releases Wiki Activity

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

Browse Source
This commit is contained in:
Erwin Coumans 2017-04-26 21:33:19 -07:00
commit eaa6255e06
17 changed files with 336 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/CommonInterfaces/CommonParameterInterface.h
View File

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

2
examples/CommonInterfaces/CommonRenderInterface.h
View File

@ -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){};

5
examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
View File

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

14
examples/Importers/ImportURDFDemo/UrdfParser.h
View File

@ -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,

38
examples/OpenGLWindow/GLInstancingRenderer.cpp
View File

@ -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]);

4
examples/OpenGLWindow/GLInstancingRenderer.h
View File

@ -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()
{

4
examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.glsl
View File

@ -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;

3
examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.h
View File

@ -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"

8
examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp
View File

@ -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];

4
examples/OpenGLWindow/SimpleOpenGL2Renderer.h
View File

@ -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();

79
examples/RenderingExamples/TinyRendererSetup.cpp
View File

@ -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;
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 +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;
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();

23
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -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],

2
examples/SharedMemory/SharedMemoryPublic.h
View File

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

8
examples/TinyRenderer/TinyRenderer.cpp
View File

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

4
examples/pybullet/examples/hand.ino
View File

@ -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() {

63
examples/pybullet/examples/hand.py
View File

@ -20,12 +20,28 @@ objects = p.loadMJCF("MPL/MPL.xml")
hand=objects[0]
#clamp in range 400-600
minV = 400
maxV = 600
#minV = 400
#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)
ser = serial.Serial(port='COM13',baudrate=115200,parity=serial.PARITY_ODD,stopbits=serial.STOPBITS_TWO,bytesize=serial.SEVENBITS)
if (ser.isOpen()):
return (b)
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)
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[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)
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)
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")

149
examples/pybullet/examples/vrhand_vive_tracker.py Normal file
View File

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