Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge remote-tracking branch 'bp/master' into pullRequest

Browse Source
This commit is contained in:
Jie Tan
2017-04-28 14:36:56 -07:00
parent 948b2b00fe fbd1a4ec2c
commit 93fc04c6c3
98 changed files with 37426 additions and 462 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
examples/pybullet/CMakeLists.txt
View File

@@ -101,11 +101,7 @@ ELSE(WIN32)
ENDIF(BUILD_PYBULLET_ENET)
IF(BUILD_PYBULLET_CLSOCKET)
IF(APPLE)
ADD_DEFINITIONS(-D_DARWIN)
ELSE()
ADD_DEFINITIONS(-D_LINUX)
ENDIF()
ADD_DEFINITIONS(${OSDEF})
ENDIF(BUILD_PYBULLET_CLSOCKET)
ENDIF(WIN32)

74
examples/pybullet/examples/dumpLog.py Normal file
View File

@@ -0,0 +1,74 @@
import time
import math
from datetime import datetime
import struct
import sys
import os, fnmatch
import argparse
from time import sleep
def readLogFile(filename, verbose = True):
f = open(filename, 'rb')
print('Opened'),
print(filename)
keys = f.readline().decode('utf8').rstrip('\n').split(',')
fmt = f.readline().decode('utf8').rstrip('\n')
# The byte number of one record
sz = struct.calcsize(fmt)
# The type number of one record
ncols = len(fmt)
if verbose:
print('Keys:'),
print(keys)
print('Format:'),
print(fmt)
print('Size:'),
print(sz)
print('Columns:'),
print(ncols)
# Read data
wholeFile = f.read()
# split by alignment word
chunks = wholeFile.split(b'\xaa\xbb')
log = list()
if verbose:
print("num chunks:")
print(len(chunks))
chunkIndex = 0
for chunk in chunks:
print("len(chunk)=",len(chunk)," sz = ", sz)
if len(chunk) == sz:
print("chunk #",chunkIndex)
chunkIndex=chunkIndex+1
values = struct.unpack(fmt, chunk)
record = list()
for i in range(ncols):
record.append(values[i])
if verbose:
print(" ",keys[i],"=",values[i])
log.append(record)
return log
numArgs = len(sys.argv)
print ('Number of arguments:', numArgs, 'arguments.')
print ('Argument List:', str(sys.argv))
fileName = "log.bin"
if (numArgs>1):
fileName = sys.argv[1]
print("filename=")
print(fileName)
verbose = True
readLogFile(fileName,verbose)

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

30
examples/pybullet/examples/ik_end_effector_orientation.py Normal file
View File

@@ -0,0 +1,30 @@
import pybullet as p
import time
import math
from datetime import datetime
from time import sleep
p.connect(p.GUI)
p.loadURDF("plane.urdf",[0,0,-0.3])
kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1])
kukaEndEffectorIndex = 6
numJoints = p.getNumJoints(kukaId)
#Joint damping coefficents. Using large values for the joints that we don't want to move.
jd=[100.0,100.0,100.0,100.0,100.0,100.0,0.5]
#jd=[0.5,0.5,0.5,0.5,0.5,0.5,0.5]
p.setGravity(0,0,0)
while 1:
p.stepSimulation()
for i in range (1):
pos = [0,0,1.26]
orn = p.getQuaternionFromEuler([0,0,3.14])
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd)
for i in range (numJoints):
p.setJointMotorControl2(bodyIndex=kukaId,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=500,positionGain=0.03,velocityGain=1)
sleep(0.05)

10
examples/pybullet/examples/manyspheres.py
View File

@@ -1,7 +1,13 @@
import pybullet as p
import time
p.connect(p.GUI)
conid = p.connect(p.SHARED_MEMORY)
if (conid<0):
p.connect(p.GUI)
p.setInternalSimFlags(0)
p.resetSimulation()
p.loadURDF("plane.urdf",useMaximalCoordinates=True)
p.loadURDF("tray/traybox.urdf",useMaximalCoordinates=True)
@@ -12,7 +18,7 @@ p.setPhysicsEngineParameter(numSolverIterations=10)
p.setPhysicsEngineParameter(contactBreakingThreshold=0.001)
for i in range (10):
for j in range (10):
for k in range (5):
for k in range (10):
ob = p.loadURDF("sphere_1cm.urdf",[0.02*i,0.02*j,0.2+0.02*k],useMaximalCoordinates=True)
p.setGravity(0,0,-10)
p.setRealTimeSimulation(1)

2
examples/pybullet/examples/quadruped.py
View File

@@ -143,7 +143,7 @@ p.setRealTimeSimulation(useRealTime)
print("quadruped Id = ")
print(quadruped)
p.saveWorld("quadru.py")
logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR,"quadrupedLog.txt",[quadruped])
logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR,"quadrupedLog.bin",[quadruped])

5
examples/pybullet/examples/vr_kuka_setup.py
View File

@@ -1,6 +1,9 @@
import pybullet as p
#p.connect(p.UDP,"192.168.86.100")
p.connect(p.SHARED_MEMORY)
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI)
p.resetSimulation()
objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]

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)

89
examples/pybullet/examples/vrtracker.py Normal file
View File

@@ -0,0 +1,89 @@
# See pybullet quickstart guide here:
# https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#
# Create a Tiltbrush-like app, drawing lines using any controller
# Line width can be changed
import pybullet as p
CONTROLLER_ID = 0
POSITION=1
ORIENTATION=2
BUTTONS=6
#assume that the VR physics server is already started before
c = p.connect(p.SHARED_MEMORY)
print(c)
if (c<0):
p.connect(p.GUI)
p.setInternalSimFlags(0)#don't load default robot assets etc
p.resetSimulation()
p.loadURDF("plane.urdf")
p.loadURDF("cube.urdf",0,0,1)
p.setGravity(0,0,-10)
p.setRealTimeSimulation(1)
prevPosition=[None]*p.VR_MAX_CONTROLLERS
colors=[0.,0.5,0.5]*p.VR_MAX_CONTROLLERS
widths = [3]*p.VR_MAX_CONTROLLERS
a=[0,0,0]
#use a few default colors
colors[0] = [0,0,0]
colors[1] = [0.5,0,0]
colors[2] = [0,0.5,0]
colors[3] = [0,0,0.5]
colors[4] = [0.5,0.5,0.]
colors[5] = [.5,.5,.5]
p.startStateLogging(p.STATE_LOGGING_VR_CONTROLLERS, "vr_hmd.bin",deviceTypeFilter=p.VR_DEVICE_HMD)
p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT, "generic_data.bin")
p.startStateLogging(p.STATE_LOGGING_CONTACT_POINTS, "contact_points.bin")
while True:
events = p.getVREvents(p.VR_DEVICE_HMD+p.VR_DEVICE_GENERIC_TRACKER)
for e in (events):
pos = e[POSITION]
mat = p.getMatrixFromQuaternion(e[ORIENTATION])
dir0 = [mat[0],mat[3],mat[6]]
dir1 = [mat[1],mat[4],mat[7]]
dir2 = [mat[2],mat[5],mat[8]]
lineLen = 0.1
dir = [-mat[2],-mat[5],-mat[8]]
to = [pos[0]+lineLen*dir[0],pos[1]+lineLen*dir[1],pos[2]+lineLen*dir[2]]
toX = [pos[0]+lineLen*dir0[0],pos[1]+lineLen*dir0[1],pos[2]+lineLen*dir0[2]]
toY = [pos[0]+lineLen*dir1[0],pos[1]+lineLen*dir1[1],pos[2]+lineLen*dir1[2]]
toZ = [pos[0]+lineLen*dir2[0],pos[1]+lineLen*dir2[1],pos[2]+lineLen*dir2[2]]
p.addUserDebugLine(pos,toX,[1,0,0],1)
p.addUserDebugLine(pos,toY,[0,1,0],1)
p.addUserDebugLine(pos,toZ,[0,0,1],1)
p.addUserDebugLine(pos,to,[0.5,0.5,0.],1,3)
events = p.getVREvents()
for e in (events):
if (e[BUTTONS][33]&p.VR_BUTTON_WAS_TRIGGERED):
prevPosition[e[CONTROLLER_ID]] = e[POSITION]
if (e[BUTTONS][32]&p.VR_BUTTON_WAS_TRIGGERED):
widths[e[CONTROLLER_ID]]=widths[e[0]]+1
if (widths[e[CONTROLLER_ID]]>20):
widths[e[CONTROLLER_ID]] = 1
if (e[BUTTONS][1]&p.VR_BUTTON_WAS_TRIGGERED):
p.resetSimulation()
#p.setGravity(0,0,-10)
p.removeAllUserDebugItems()
p.loadURDF("plane.urdf")
if (e[BUTTONS][33]==p.VR_BUTTON_IS_DOWN):
pt = prevPosition[e[CONTROLLER_ID]]
#print(prevPosition[e[0]])
#print(e[1])
diff = [pt[0]-e[POSITION][0],pt[1]-e[POSITION][1],pt[2]-e[POSITION][2]]
lenSqr = diff[0]*diff[0]+diff[1]*diff[1]+diff[2]*diff[2]
ptDistThreshold = 0.01
if (lenSqr>(ptDistThreshold*ptDistThreshold)):
p.addUserDebugLine(e[POSITION],prevPosition[e[CONTROLLER_ID]],colors[e[CONTROLLER_ID]],widths[e[CONTROLLER_ID]])
#p.loadURDF("cube_small.urdf",e[1])
colors[e[CONTROLLER_ID]] = [1-colors[e[CONTROLLER_ID]][0],1-colors[e[CONTROLLER_ID]][1],1-colors[e[CONTROLLER_ID]][2]]
prevPosition[e[CONTROLLER_ID]] = e[POSITION]

290
examples/pybullet/pybullet.c
View File

@@ -11,16 +11,25 @@
#include "../SharedMemory/PhysicsClientTCP_C_API.h"
#endif //BT_ENABLE_CLSOCKET
#ifdef __APPLE__
#if defined(__APPLE__) && (!defined(B3_NO_PYTHON_FRAMEWORK))
#include <Python/Python.h>
#else
#include <Python.h>
#endif
#ifdef B3_DUMP_PYTHON_VERSION
#define B3_VALUE_TO_STRING(x) #x
#define B3_VALUE(x) B3_VALUE_TO_STRING(x)
#define B3_VAR_NAME_VALUE(var) #var "=" B3_VALUE(var)
#pragma message(B3_VAR_NAME_VALUE(PY_MAJOR_VERSION))
#pragma message(B3_VAR_NAME_VALUE(PY_MINOR_VERSION))
#endif
#ifdef PYBULLET_USE_NUMPY
#include <numpy/arrayobject.h>
#endif
#if PY_MAJOR_VERSION >= 3
#define PyInt_FromLong PyLong_FromLong
#define PyString_FromString PyBytes_FromString
@@ -400,8 +409,8 @@ static PyObject* pybullet_disconnectPhysicsServer(PyObject* self,
return Py_None;
}
void b3pybulletExitFunc()
///to avoid memory leaks, disconnect all physics servers explicitly
void b3pybulletExitFunc(void)
{
int i;
for (i=0;i<MAX_PHYSICS_CLIENTS;i++)
@@ -2074,7 +2083,7 @@ static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args, PyObject*
int bodyUniqueId = -1;
int jointIndex = -1;
int jointInfoSize = 12; // size of struct b3JointInfo
int jointInfoSize = 13; // size of struct b3JointInfo
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = {"bodyUniqueId", "jointIndex", "physicsClientId", NULL};
@@ -2126,6 +2135,8 @@ static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args, PyObject*
PyFloat_FromDouble(info.m_jointMaxForce));
PyTuple_SetItem(pyListJointInfo, 11,
PyFloat_FromDouble(info.m_jointMaxVelocity));
PyTuple_SetItem(pyListJointInfo, 12,
PyString_FromString(info.m_linkName));
return pyListJointInfo;
}
@@ -2683,12 +2694,13 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb
int bodyUniqueIdB = -1;
int linkIndexA = -2;
int linkIndexB = -2;
int deviceTypeFilter = -1;
static char* kwlist[] = {"loggingType", "fileName", "objectUniqueIds", "maxLogDof", "bodyUniqueIdA", "bodyUniqueIdB", "linkIndexA", "linkIndexB", "physicsClientId", NULL};
static char* kwlist[] = {"loggingType", "fileName", "objectUniqueIds", "maxLogDof", "bodyUniqueIdA", "bodyUniqueIdB", "linkIndexA", "linkIndexB", "deviceTypeFilter", "physicsClientId", NULL};
int physicsClientId = 0;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oiiiiii", kwlist,
&loggingType, &fileName, &objectUniqueIdsObj, &maxLogDof, &bodyUniqueIdA, &bodyUniqueIdB, &linkIndexA, &linkIndexB, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oiiiiiii", kwlist,
&loggingType, &fileName, &objectUniqueIdsObj, &maxLogDof, &bodyUniqueIdA, &bodyUniqueIdB, &linkIndexA, &linkIndexB, &deviceTypeFilter, &physicsClientId))
return NULL;
sm = getPhysicsClient(physicsClientId);
@@ -2741,6 +2753,11 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb
b3StateLoggingSetLinkIndexB(commandHandle, linkIndexB);
}
if (deviceTypeFilter>=0)
{
b3StateLoggingSetDeviceTypeFilter(commandHandle,deviceTypeFilter);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_STATE_LOGGING_START_COMPLETED)
@@ -2812,7 +2829,7 @@ static PyObject* pybullet_setTimeOut(PyObject* self, PyObject* args, PyObject* k
return Py_None;
}
static PyObject* pybullet_rayTest(PyObject* self, PyObject* args, PyObject* keywds)
static PyObject* pybullet_rayTestObsolete(PyObject* self, PyObject* args, PyObject* keywds)
{
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
@@ -2896,6 +2913,154 @@ static PyObject* pybullet_rayTest(PyObject* self, PyObject* args, PyObject* keyw
return Py_None;
}
static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject* keywds)
{
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
PyObject* rayFromObjList = 0;
PyObject* rayToObjList = 0;
b3PhysicsClientHandle sm = 0;
int sizeFrom = 0;
int sizeTo = 0;
static char* kwlist[] = {"rayFromPositions", "rayToPositions", "physicsClientId", NULL};
int physicsClientId = 0;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist,
&rayFromObjList, &rayToObjList, &physicsClientId))
return NULL;
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
commandHandle = b3CreateRaycastBatchCommandInit(sm);
if (rayFromObjList)
{
PyObject* seqRayFromObj = PySequence_Fast(rayFromObjList, "expected a sequence of rayFrom positions");
PyObject* seqRayToObj = PySequence_Fast(rayToObjList, "expected a sequence of 'rayTo' positions");
if (seqRayFromObj && seqRayToObj)
{
int lenFrom = PySequence_Size(rayFromObjList);
int lenTo= PySequence_Size(seqRayToObj);
if (lenFrom!=lenTo)
{
PyErr_SetString(SpamError, "Size of from_positions need to be equal to size of to_positions.");
Py_DECREF(seqRayFromObj);
Py_DECREF(seqRayToObj);
return NULL;
} else
{
int i;
if (lenFrom >= MAX_RAY_INTERSECTION_BATCH_SIZE)
{
PyErr_SetString(SpamError, "Number of rays exceed the maximum batch size.");
Py_DECREF(seqRayFromObj);
Py_DECREF(seqRayToObj);
return NULL;
}
for (i = 0; i < lenFrom; i++)
{
PyObject* rayFromObj = PySequence_GetItem(rayFromObjList,i);
PyObject* rayToObj = PySequence_GetItem(seqRayToObj,i);
double rayFromWorld[3];
double rayToWorld[3];
if ((pybullet_internalSetVectord(rayFromObj, rayFromWorld)) &&
(pybullet_internalSetVectord(rayToObj, rayToWorld)))
{
b3RaycastBatchAddRay(commandHandle, rayFromWorld, rayToWorld);
} else
{
PyErr_SetString(SpamError, "Items in the from/to positions need to be an [x,y,z] list of 3 floats/doubles");
Py_DECREF(seqRayFromObj);
Py_DECREF(seqRayToObj);
Py_DECREF(rayFromObj);
Py_DECREF(rayToObj);
return NULL;
}
Py_DECREF(rayFromObj);
Py_DECREF(rayToObj);
}
}
} else
{
}
if (seqRayFromObj)
{
Py_DECREF(seqRayFromObj);
}
if (seqRayToObj)
{
Py_DECREF(seqRayToObj);
}
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED)
{
struct b3RaycastInformation raycastInfo;
PyObject* rayHitsObj = 0;
int i;
b3GetRaycastInformation(sm, &raycastInfo);
rayHitsObj = PyTuple_New(raycastInfo.m_numRayHits);
for (i = 0; i < raycastInfo.m_numRayHits; i++)
{
PyObject* singleHitObj = PyTuple_New(5);
{
PyObject* ob = PyInt_FromLong(raycastInfo.m_rayHits[i].m_hitObjectUniqueId);
PyTuple_SetItem(singleHitObj, 0, ob);
}
{
PyObject* ob = PyInt_FromLong(raycastInfo.m_rayHits[i].m_hitObjectLinkIndex);
PyTuple_SetItem(singleHitObj, 1, ob);
}
{
PyObject* ob = PyFloat_FromDouble(raycastInfo.m_rayHits[i].m_hitFraction);
PyTuple_SetItem(singleHitObj, 2, ob);
}
{
PyObject* posObj = PyTuple_New(3);
int p;
for (p = 0; p < 3; p++)
{
PyObject* ob = PyFloat_FromDouble(raycastInfo.m_rayHits[i].m_hitPositionWorld[p]);
PyTuple_SetItem(posObj, p, ob);
}
PyTuple_SetItem(singleHitObj, 3, posObj);
}
{
PyObject* normalObj = PyTuple_New(3);
int p;
for (p = 0; p < 3; p++)
{
PyObject* ob = PyFloat_FromDouble(raycastInfo.m_rayHits[i].m_hitNormalWorld[p]);
PyTuple_SetItem(normalObj, p, ob);
}
PyTuple_SetItem(singleHitObj, 4, normalObj);
}
PyTuple_SetItem(rayHitsObj, i, singleHitObj);
}
return rayHitsObj;
}
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_getMatrixFromQuaternion(PyObject* self, PyObject* args)
{
PyObject* quatObj;
@@ -3018,10 +3183,11 @@ static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject*
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
int deviceTypeFilter = VR_DEVICE_CONTROLLER;
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = {"physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId))
static char* kwlist[] = {"deviceTypeFilter", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|ii", kwlist, &deviceTypeFilter, &physicsClientId))
{
return NULL;
}
@@ -3034,6 +3200,9 @@ static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject*
}
commandHandle = b3RequestVREventsCommandInit(sm);
b3VREventsSetDeviceTypeFilter(commandHandle, deviceTypeFilter);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_REQUEST_VR_EVENTS_DATA_COMPLETED)
@@ -3046,7 +3215,7 @@ static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject*
vrEventsObj = PyTuple_New(vrEvents.m_numControllerEvents);
for (i = 0; i < vrEvents.m_numControllerEvents; i++)
{
PyObject* vrEventObj = PyTuple_New(7);
PyObject* vrEventObj = PyTuple_New(8);
PyTuple_SetItem(vrEventObj, 0, PyInt_FromLong(vrEvents.m_controllerEvents[i].m_controllerId));
{
@@ -3078,6 +3247,7 @@ static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject*
}
PyTuple_SetItem(vrEventObj, 6, buttonsObj);
}
PyTuple_SetItem(vrEventObj, 7, PyInt_FromLong(vrEvents.m_controllerEvents[i].m_deviceType));
PyTuple_SetItem(vrEventsObj, i, vrEventObj);
}
return vrEventsObj;
@@ -3087,6 +3257,84 @@ static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject*
return Py_None;
}
static PyObject* pybullet_getDebugVisualizerCamera(PyObject* self, PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = {"physicsClientId", NULL};
b3SharedMemoryCommandHandle commandHandle;
int hasCamInfo;
b3SharedMemoryStatusHandle statusHandle;
struct b3OpenGLVisualizerCameraInfo camera;
PyObject* pyCameraList =0;
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
commandHandle = b3InitRequestOpenGLVisualizerCameraCommand(sm);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
hasCamInfo = b3GetStatusOpenGLVisualizerCamera(statusHandle, &camera);
if (hasCamInfo)
{
PyObject* item=0;
pyCameraList = PyTuple_New(8);
item = PyInt_FromLong(camera.m_width);
PyTuple_SetItem(pyCameraList,0,item);
item = PyInt_FromLong(camera.m_height);
PyTuple_SetItem(pyCameraList,1,item);
{
PyObject* viewMat16 = PyTuple_New(16);
PyObject* projMat16 = PyTuple_New(16);
int i;
for (i=0;i<16;i++)
{
item = PyFloat_FromDouble(camera.m_viewMatrix[i]);
PyTuple_SetItem(viewMat16,i,item);
item = PyFloat_FromDouble(camera.m_projectionMatrix[i]);
PyTuple_SetItem(projMat16,i,item);
}
PyTuple_SetItem(pyCameraList,2,viewMat16);
PyTuple_SetItem(pyCameraList,3,projMat16);
}
{
PyObject* item=0;
int i;
PyObject* camUp = PyTuple_New(3);
PyObject* camFwd = PyTuple_New(3);
PyObject* hor = PyTuple_New(3);
PyObject* vert= PyTuple_New(3);
for (i=0;i<3;i++)
{
item = PyFloat_FromDouble(camera.m_camUp[i]);
PyTuple_SetItem(camUp,i,item);
item = PyFloat_FromDouble(camera.m_camForward[i]);
PyTuple_SetItem(camFwd,i,item);
item = PyFloat_FromDouble(camera.m_horizontal[i]);
PyTuple_SetItem(hor,i,item);
item = PyFloat_FromDouble(camera.m_vertical[i]);
PyTuple_SetItem(vert,i,item);
}
PyTuple_SetItem(pyCameraList,4,camUp);
PyTuple_SetItem(pyCameraList,5,camFwd);
PyTuple_SetItem(pyCameraList,6,hor);
PyTuple_SetItem(pyCameraList,7,vert);
}
return pyCameraList;
}
PyErr_SetString(SpamError, "Cannot get OpenGL visualizer camera info.");
return NULL;
}
static PyObject* pybullet_configureDebugVisualizer(PyObject* self, PyObject* args, PyObject* keywds)
{
int flag = 1;
@@ -5268,6 +5516,9 @@ static PyMethodDef SpamMethods[] = {
"Override the wireframe debug drawing color for a particular object unique id / link index."
"If you ommit the color, the custom color will be removed."},
{"getDebugVisualizerCamera", (PyCFunction)pybullet_getDebugVisualizerCamera, METH_VARARGS | METH_KEYWORDS,
"Get information about the 3D visualizer camera, such as width, height, view matrix, projection matrix etc."},
{"configureDebugVisualizer", (PyCFunction)pybullet_configureDebugVisualizer, METH_VARARGS | METH_KEYWORDS,
"For the 3D OpenGL Visualizer, enable/disable GUI, shadows."},
@@ -5319,9 +5570,14 @@ static PyMethodDef SpamMethods[] = {
"fileName, optional objectUniqueId, maxLogDof, bodyUniqueIdA, bodyUniqueIdB, linkIndexA, linkIndexB. Function returns int loggingUniqueId"},
{"stopStateLogging", (PyCFunction)pybullet_stopStateLogging, METH_VARARGS | METH_KEYWORDS,
"Stop logging of robot state, given a loggingUniqueId."},
{"rayTest", (PyCFunction)pybullet_rayTest, METH_VARARGS | METH_KEYWORDS,
{"rayTest", (PyCFunction)pybullet_rayTestObsolete, METH_VARARGS | METH_KEYWORDS,
"Cast a ray and return the first object hit, if any. "
"Takes two arguments (from position [x,y,z] and to position [x,y,z] in Cartesian world coordinates"},
"Takes two arguments (from_position [x,y,z] and to_position [x,y,z] in Cartesian world coordinates"},
{"rayTestBatch", (PyCFunction)pybullet_rayTestBatch, METH_VARARGS | METH_KEYWORDS,
"Cast a batch of rays and return the result for each of the rays (first object hit, if any. or -1) "
"Takes two arguments (list of from_positions [x,y,z] and a list of to_positions [x,y,z] in Cartesian world coordinates"},
{"setTimeOut", (PyCFunction)pybullet_setTimeOut, METH_VARARGS | METH_KEYWORDS,
"Set the timeOut in seconds, used for most of the API calls."},
// todo(erwincoumans)
@@ -5439,6 +5695,10 @@ initpybullet(void)
PyModule_AddIntConstant(m, "VR_MAX_CONTROLLERS", MAX_VR_CONTROLLERS);
PyModule_AddIntConstant(m, "VR_MAX_BUTTONS", MAX_VR_BUTTONS);
PyModule_AddIntConstant(m, "VR_DEVICE_CONTROLLER", VR_DEVICE_CONTROLLER);
PyModule_AddIntConstant(m, "VR_DEVICE_HMD", VR_DEVICE_HMD);
PyModule_AddIntConstant(m, "VR_DEVICE_GENERIC_TRACKER", VR_DEVICE_GENERIC_TRACKER);
PyModule_AddIntConstant(m, "KEY_IS_DOWN", eButtonIsDown);
PyModule_AddIntConstant(m, "KEY_WAS_TRIGGERED", eButtonTriggered);
PyModule_AddIntConstant(m, "KEY_WAS_RELEASED", eButtonReleased);
@@ -5458,7 +5718,9 @@ initpybullet(void)
PyModule_AddIntConstant(m, "URDF_USE_INERTIA_FROM_FILE", URDF_USE_INERTIA_FROM_FILE);
PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION", URDF_USE_SELF_COLLISION);
PyModule_AddIntConstant(m, "MAX_RAY_INTERSECTION_BATCH_SIZE", MAX_RAY_INTERSECTION_BATCH_SIZE);
PyModule_AddIntConstant(m, "B3G_F1", B3G_F1);
PyModule_AddIntConstant(m, "B3G_F2", B3G_F2);
PyModule_AddIntConstant(m, "B3G_F3", B3G_F3);