add hand.py/hand.ino used to create this VR glove, using MuJoCo Haptix MPL hand (Apache 2 license)
https://www.youtube.com/watch?v=VMJyZtHQL50 added b3InitSyncBodyInfoCommand, to retrieve body info, when connecting to a server with existing bodies pybullet will call this b3InitSyncBodyInfoCommand automatically after connecting Avoid overriding the py.setVRCameraState setting in VR
This commit is contained in:
Erwin Coumans
32
examples/pybullet/hand.ino
Normal file
32
examples/pybullet/hand.ino
Normal file
@@ -0,0 +1,32 @@
|
||||
//arduino script for vr glove, sending analogue 'finger' readings
|
||||
//to be used with pybullet and hand.py
|
||||
|
||||
|
||||
int sensorPin0 = A0;
|
||||
int sensorPin1 = A1;
|
||||
int sensorPin2 = A2;
|
||||
int sensorPin3 = A3;
|
||||
|
||||
void setup() {
|
||||
// put your setup code here, to run once:
|
||||
Serial.begin(115200);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// put your main code here, to run repeatedly:
|
||||
int sensorValue0 = analogRead(sensorPin0);
|
||||
int sensorValue1 = analogRead(sensorPin1);
|
||||
int sensorValue2 = analogRead(sensorPin2);
|
||||
int sensorValue3 = analogRead(sensorPin3);
|
||||
|
||||
Serial.print(",");
|
||||
Serial.print(sensorValue0);
|
||||
Serial.print(",");
|
||||
Serial.print(sensorValue1);
|
||||
Serial.print(",");
|
||||
Serial.print(sensorValue2);
|
||||
Serial.print(",");
|
||||
Serial.print(sensorValue3);
|
||||
Serial.println(",");
|
||||
delay(10);
|
||||
}
|
||||
78
examples/pybullet/hand.py
Normal file
78
examples/pybullet/hand.py
Normal file
@@ -0,0 +1,78 @@
|
||||
#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)
|
||||
print(c)
|
||||
if (c<0):
|
||||
p.connect(p.GUI)
|
||||
|
||||
#load the MuJoCo MJCF hand
|
||||
objects = p.loadMJCF("MPL/mpl.xml")
|
||||
|
||||
hand=objects[0]
|
||||
#clamp in range 400-600
|
||||
minV = 400
|
||||
maxV = 600
|
||||
|
||||
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 (1.0-b)
|
||||
|
||||
ser = serial.Serial(port='COM13',baudrate=115200,parity=serial.PARITY_ODD,stopbits=serial.STOPBITS_TWO,bytesize=serial.SEVENBITS)
|
||||
if (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)
|
||||
p.setJointMotorControl2(hand,11,p.POSITION_CONTROL,thumb)
|
||||
p.setJointMotorControl2(hand,13,p.POSITION_CONTROL,thumb)
|
||||
|
||||
p.setJointMotorControl2(hand,17,p.POSITION_CONTROL,index)
|
||||
p.setJointMotorControl2(hand,19,p.POSITION_CONTROL,index)
|
||||
p.setJointMotorControl2(hand,21,p.POSITION_CONTROL,index)
|
||||
|
||||
p.setJointMotorControl2(hand,24,p.POSITION_CONTROL,middle)
|
||||
p.setJointMotorControl2(hand,26,p.POSITION_CONTROL,middle)
|
||||
p.setJointMotorControl2(hand,28,p.POSITION_CONTROL,middle)
|
||||
|
||||
p.setJointMotorControl2(hand,40,p.POSITION_CONTROL,pink)
|
||||
p.setJointMotorControl2(hand,42,p.POSITION_CONTROL,pink)
|
||||
p.setJointMotorControl2(hand,44,p.POSITION_CONTROL,pink)
|
||||
|
||||
ringpos = 0.5*(pink+middle)
|
||||
p.setJointMotorControl2(hand,32,p.POSITION_CONTROL,ringpos)
|
||||
p.setJointMotorControl2(hand,34,p.POSITION_CONTROL,ringpos)
|
||||
p.setJointMotorControl2(hand,36,p.POSITION_CONTROL,ringpos)
|
||||
|
||||
#print(middle)
|
||||
#print(pink)
|
||||
#print(index)
|
||||
#print(thumb)
|
||||
@@ -315,9 +315,25 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
|
||||
|
||||
if (freeIndex>=0)
|
||||
{
|
||||
b3SharedMemoryCommandHandle command;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
|
||||
|
||||
sPhysicsClients1[freeIndex] = sm;
|
||||
sNumPhysicsClients++;
|
||||
|
||||
command = b3InitSyncBodyInfoCommand(sm);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
#if 0
|
||||
if (statusType != CMD_BODY_INFO_COMPLETED) {
|
||||
PyErr_SetString(SpamError, "b3InitSyncBodyInfoCommand failed.");
|
||||
return NULL;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
}
|
||||
return PyInt_FromLong(freeIndex);
|
||||
}
|
||||
@@ -1989,24 +2005,29 @@ static PyObject* pybullet_getJointState(PyObject* self, PyObject* args,PyObject*
|
||||
pyListJointState = PyTuple_New(sensorStateSize);
|
||||
pyListJointForceTorque = PyTuple_New(forceTorqueSize);
|
||||
|
||||
b3GetJointState(sm, status_handle, jointIndex, &sensorState);
|
||||
if (b3GetJointState(sm, status_handle, jointIndex, &sensorState))
|
||||
{
|
||||
PyTuple_SetItem(pyListJointState, 0,
|
||||
PyFloat_FromDouble(sensorState.m_jointPosition));
|
||||
PyTuple_SetItem(pyListJointState, 1,
|
||||
PyFloat_FromDouble(sensorState.m_jointVelocity));
|
||||
|
||||
PyTuple_SetItem(pyListJointState, 0,
|
||||
PyFloat_FromDouble(sensorState.m_jointPosition));
|
||||
PyTuple_SetItem(pyListJointState, 1,
|
||||
PyFloat_FromDouble(sensorState.m_jointVelocity));
|
||||
for (j = 0; j < forceTorqueSize; j++) {
|
||||
PyTuple_SetItem(pyListJointForceTorque, j,
|
||||
PyFloat_FromDouble(sensorState.m_jointForceTorque[j]));
|
||||
}
|
||||
|
||||
for (j = 0; j < forceTorqueSize; j++) {
|
||||
PyTuple_SetItem(pyListJointForceTorque, j,
|
||||
PyFloat_FromDouble(sensorState.m_jointForceTorque[j]));
|
||||
}
|
||||
PyTuple_SetItem(pyListJointState, 2, pyListJointForceTorque);
|
||||
|
||||
PyTuple_SetItem(pyListJointState, 2, pyListJointForceTorque);
|
||||
PyTuple_SetItem(pyListJointState, 3,
|
||||
PyFloat_FromDouble(sensorState.m_jointMotorTorque));
|
||||
|
||||
PyTuple_SetItem(pyListJointState, 3,
|
||||
PyFloat_FromDouble(sensorState.m_jointMotorTorque));
|
||||
|
||||
return pyListJointState;
|
||||
return pyListJointState;
|
||||
} else
|
||||
{
|
||||
PyErr_SetString(SpamError, "getJointState failed (2).");
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -2070,54 +2091,55 @@ b3PhysicsClientHandle sm = 0;
|
||||
return NULL;
|
||||
}
|
||||
|
||||
b3GetLinkState(sm, status_handle, linkIndex, &linkState);
|
||||
if (b3GetLinkState(sm, status_handle, linkIndex, &linkState))
|
||||
{
|
||||
pyLinkStateWorldPosition = PyTuple_New(3);
|
||||
for (i = 0; i < 3; ++i) {
|
||||
PyTuple_SetItem(pyLinkStateWorldPosition, i,
|
||||
PyFloat_FromDouble(linkState.m_worldPosition[i]));
|
||||
}
|
||||
|
||||
pyLinkStateWorldPosition = PyTuple_New(3);
|
||||
for (i = 0; i < 3; ++i) {
|
||||
PyTuple_SetItem(pyLinkStateWorldPosition, i,
|
||||
PyFloat_FromDouble(linkState.m_worldPosition[i]));
|
||||
}
|
||||
pyLinkStateWorldOrientation = PyTuple_New(4);
|
||||
for (i = 0; i < 4; ++i) {
|
||||
PyTuple_SetItem(pyLinkStateWorldOrientation, i,
|
||||
PyFloat_FromDouble(linkState.m_worldOrientation[i]));
|
||||
}
|
||||
|
||||
pyLinkStateWorldOrientation = PyTuple_New(4);
|
||||
for (i = 0; i < 4; ++i) {
|
||||
PyTuple_SetItem(pyLinkStateWorldOrientation, i,
|
||||
PyFloat_FromDouble(linkState.m_worldOrientation[i]));
|
||||
}
|
||||
pyLinkStateLocalInertialPosition = PyTuple_New(3);
|
||||
for (i = 0; i < 3; ++i) {
|
||||
PyTuple_SetItem(pyLinkStateLocalInertialPosition, i,
|
||||
PyFloat_FromDouble(linkState.m_localInertialPosition[i]));
|
||||
}
|
||||
|
||||
pyLinkStateLocalInertialPosition = PyTuple_New(3);
|
||||
for (i = 0; i < 3; ++i) {
|
||||
PyTuple_SetItem(pyLinkStateLocalInertialPosition, i,
|
||||
PyFloat_FromDouble(linkState.m_localInertialPosition[i]));
|
||||
}
|
||||
pyLinkStateLocalInertialOrientation = PyTuple_New(4);
|
||||
for (i = 0; i < 4; ++i) {
|
||||
PyTuple_SetItem(pyLinkStateLocalInertialOrientation, i,
|
||||
PyFloat_FromDouble(linkState.m_localInertialOrientation[i]));
|
||||
}
|
||||
|
||||
pyLinkStateLocalInertialOrientation = PyTuple_New(4);
|
||||
for (i = 0; i < 4; ++i) {
|
||||
PyTuple_SetItem(pyLinkStateLocalInertialOrientation, i,
|
||||
PyFloat_FromDouble(linkState.m_localInertialOrientation[i]));
|
||||
}
|
||||
pyLinkStateWorldLinkFramePosition = PyTuple_New(3);
|
||||
for (i = 0; i < 3; ++i) {
|
||||
PyTuple_SetItem(pyLinkStateWorldLinkFramePosition , i,
|
||||
PyFloat_FromDouble(linkState.m_worldLinkFramePosition[i]));
|
||||
}
|
||||
|
||||
pyLinkStateWorldLinkFramePosition = PyTuple_New(3);
|
||||
for (i = 0; i < 3; ++i) {
|
||||
PyTuple_SetItem(pyLinkStateWorldLinkFramePosition , i,
|
||||
PyFloat_FromDouble(linkState.m_worldLinkFramePosition[i]));
|
||||
}
|
||||
|
||||
pyLinkStateWorldLinkFrameOrientation = PyTuple_New(4);
|
||||
for (i = 0; i < 4; ++i) {
|
||||
PyTuple_SetItem(pyLinkStateWorldLinkFrameOrientation, i,
|
||||
PyFloat_FromDouble(linkState.m_worldLinkFrameOrientation[i]));
|
||||
}
|
||||
pyLinkStateWorldLinkFrameOrientation = PyTuple_New(4);
|
||||
for (i = 0; i < 4; ++i) {
|
||||
PyTuple_SetItem(pyLinkStateWorldLinkFrameOrientation, i,
|
||||
PyFloat_FromDouble(linkState.m_worldLinkFrameOrientation[i]));
|
||||
}
|
||||
|
||||
|
||||
pyLinkState = PyTuple_New(6);
|
||||
PyTuple_SetItem(pyLinkState, 0, pyLinkStateWorldPosition);
|
||||
PyTuple_SetItem(pyLinkState, 1, pyLinkStateWorldOrientation);
|
||||
PyTuple_SetItem(pyLinkState, 2, pyLinkStateLocalInertialPosition);
|
||||
PyTuple_SetItem(pyLinkState, 3, pyLinkStateLocalInertialOrientation);
|
||||
PyTuple_SetItem(pyLinkState, 4, pyLinkStateWorldLinkFramePosition );
|
||||
PyTuple_SetItem(pyLinkState, 5, pyLinkStateWorldLinkFrameOrientation);
|
||||
pyLinkState = PyTuple_New(6);
|
||||
PyTuple_SetItem(pyLinkState, 0, pyLinkStateWorldPosition);
|
||||
PyTuple_SetItem(pyLinkState, 1, pyLinkStateWorldOrientation);
|
||||
PyTuple_SetItem(pyLinkState, 2, pyLinkStateLocalInertialPosition);
|
||||
PyTuple_SetItem(pyLinkState, 3, pyLinkStateLocalInertialOrientation);
|
||||
PyTuple_SetItem(pyLinkState, 4, pyLinkStateWorldLinkFramePosition );
|
||||
PyTuple_SetItem(pyLinkState, 5, pyLinkStateWorldLinkFrameOrientation);
|
||||
|
||||
return pyLinkState;
|
||||
return pyLinkState;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -7,6 +7,7 @@ import pybullet as p
|
||||
|
||||
CONTROLLER_ID = 0
|
||||
POSITION=1
|
||||
ORIENTATION=2
|
||||
BUTTONS=6
|
||||
|
||||
#assume that the VR physics server is already started before
|
||||
|
||||
76
examples/pybullet/vr_kuka_setup.py
Normal file
76
examples/pybullet/vr_kuka_setup.py
Normal file
@@ -0,0 +1,76 @@
|
||||
import pybullet as p
|
||||
p.connect(p.SHARED_MEMORY)
|
||||
p.resetSimulation()
|
||||
|
||||
objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
|
||||
objects = [p.loadURDF("samurai.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
|
||||
objects = [p.loadURDF("pr2_gripper.urdf", 0.500000,0.300006,0.700000,-0.000000,-0.000000,-0.000031,1.000000)]
|
||||
pr2_gripper = objects[0]
|
||||
print ("pr2_gripper=")
|
||||
print (pr2_gripper)
|
||||
|
||||
jointPositions=[ 0.550569, 0.000000, 0.549657, 0.000000 ]
|
||||
for jointIndex in range (p.getNumJoints(pr2_gripper)):
|
||||
p.resetJointState(pr2_gripper,jointIndex,jointPositions[jointIndex])
|
||||
|
||||
pr2_cid = p.createConstraint(pr2_gripper,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.2,0,0],[0.500000,0.300006,0.700000])
|
||||
print ("pr2_cid")
|
||||
print (pr2_cid)
|
||||
|
||||
objects = [p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000,-0.200000,0.600000,0.000000,0.000000,0.000000,1.000000)]
|
||||
kuka = objects[0]
|
||||
jointPositions=[ -0.000000, -0.000000, 0.000000, 1.570793, 0.000000, -1.036725, 0.000001 ]
|
||||
for jointIndex in range (p.getNumJoints(kuka)):
|
||||
p.resetJointState(kuka,jointIndex,jointPositions[jointIndex])
|
||||
|
||||
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.700000,0.000000,0.000000,0.000000,1.000000)]
|
||||
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.800000,0.000000,0.000000,0.000000,1.000000)]
|
||||
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.900000,0.000000,0.000000,0.000000,1.000000)]
|
||||
objects = p.loadSDF("gripper/wsg50_one_motor_gripper_new_free_base.sdf")
|
||||
kuka_gripper = objects[0]
|
||||
print ("kuka gripper=")
|
||||
print(kuka_gripper)
|
||||
|
||||
p.resetBasePositionAndOrientation(kuka_gripper,[0.923103,-0.200000,1.250036],[-0.000000,0.964531,-0.000002,-0.263970])
|
||||
jointPositions=[ 0.000000, -0.011130, -0.206421, 0.205143, -0.009999, 0.000000, -0.010055, 0.000000 ]
|
||||
for jointIndex in range (p.getNumJoints(kuka_gripper)):
|
||||
p.resetJointState(kuka_gripper,jointIndex,jointPositions[jointIndex])
|
||||
|
||||
|
||||
kuka_cid = p.createConstraint(kuka, 6, kuka_gripper,0,p.JOINT_FIXED, [0,0,0], [0,0,0.05],[0,0,0])
|
||||
|
||||
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("teddy_vhacd.urdf", 1.050000,-0.500000,0.700000,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)]
|
||||
objects = [p.loadURDF("duck_vhacd.urdf", 0.850000,-0.400000,0.900000,0.000000,0.000000,0.707107,0.707107)]
|
||||
objects = p.loadSDF("kiva_shelf/model.sdf")
|
||||
ob = objects[0]
|
||||
p.resetBasePositionAndOrientation(ob,[0.000000,1.000000,1.204500],[0.000000,0.000000,0.000000,1.000000])
|
||||
objects = [p.loadURDF("teddy_vhacd.urdf", -0.100000,0.600000,0.850000,0.000000,0.000000,0.000000,1.000000)]
|
||||
objects = [p.loadURDF("sphere_small.urdf", -0.100000,0.955006,1.169706,0.633232,-0.000000,-0.000000,0.773962)]
|
||||
objects = [p.loadURDF("cube_small.urdf", 0.300000,0.600000,0.850000,0.000000,0.000000,0.000000,1.000000)]
|
||||
objects = [p.loadURDF("table_square/table_square.urdf", -1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
|
||||
ob = objects[0]
|
||||
jointPositions=[ 0.000000 ]
|
||||
for jointIndex in range (p.getNumJoints(ob)):
|
||||
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
|
||||
|
||||
objects = [p.loadURDF("husky/husky.urdf", 2.000000,-5.000000,1.000000,0.000000,0.000000,0.000000,1.000000)]
|
||||
ob = objects[0]
|
||||
jointPositions=[ 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000 ]
|
||||
for jointIndex in range (p.getNumJoints(ob)):
|
||||
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
|
||||
|
||||
p.setGravity(0.000000,0.000000,0.000000)
|
||||
p.setGravity(0,0,-10)
|
||||
|
||||
p.stepSimulation()
|
||||
|
||||
p.disconnect()
|
||||
Reference in New Issue
Block a user