Merge pull request #1079 from erwincoumans/master
tinyrenderer: disable backface culling. expose lightpos to glinstancingrenderer. bump up pybullet setup.py version
This commit is contained in:
@@ -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)
|
||||
{
|
||||
|
||||
@@ -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){};
|
||||
|
||||
@@ -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]);
|
||||
|
||||
@@ -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()
|
||||
{
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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"
|
||||
|
||||
@@ -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];
|
||||
|
||||
@@ -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();
|
||||
|
||||
@@ -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();
|
||||
|
||||
|
||||
@@ -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],
|
||||
|
||||
@@ -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)
|
||||
{
|
||||
|
||||
@@ -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() {
|
||||
|
||||
@@ -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 = 400
|
||||
|
||||
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:
|
||||
@@ -36,22 +44,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])
|
||||
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)
|
||||
|
||||
@@ -76,3 +101,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
149
examples/pybullet/examples/vrhand_vive_tracker.py
Normal 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)
|
||||
6
setup.py
6
setup.py
@@ -405,7 +405,7 @@ elif _platform == "darwin":
|
||||
|
||||
setup(
|
||||
name = 'pybullet',
|
||||
version='0.1.7',
|
||||
version='1.0.1',
|
||||
description='Official Python Interface for the Bullet Physics SDK Robotics Simulator',
|
||||
long_description='pybullet is an easy to use Python module for physics simulation, robotics and machine learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.',
|
||||
url='https://github.com/bulletphysics/bullet3',
|
||||
@@ -420,7 +420,7 @@ setup(
|
||||
extra_compile_args=CXX_FLAGS.split(),
|
||||
include_dirs = include_dirs + ["src","examples/ThirdPartyLibs","examples/ThirdPartyLibs/Glew", "examples/ThirdPartyLibs/enet/include","examples/ThirdPartyLibs/clsocket/src"]
|
||||
) ],
|
||||
classifiers=['Development Status :: 4 - Beta',
|
||||
classifiers=['Development Status :: 5 - Production/Stable',
|
||||
'License :: OSI Approved :: zlib/libpng License',
|
||||
'Operating System :: Microsoft :: Windows',
|
||||
'Operating System :: POSIX :: Linux',
|
||||
@@ -429,6 +429,8 @@ setup(
|
||||
"Programming Language :: Python",
|
||||
'Programming Language :: Python :: 2.7',
|
||||
'Programming Language :: Python :: 3.4',
|
||||
'Programming Language :: Python :: 3.5',
|
||||
'Programming Language :: Python :: 3.6',
|
||||
'Topic :: Games/Entertainment :: Simulation',
|
||||
'Framework :: Robot Framework'],
|
||||
package_data = {
|
||||
|
||||
Reference in New Issue
Block a user