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

add hand.py/hand.ino used to create this VR glove, using MuJoCo Haptix MPL hand (Apache 2 license)

Browse Source 
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
2017-01-20 18:13:24 -08:00
parent d81d62a70b
commit 64957ece9f
40 changed files with 837 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -535,7 +535,7 @@ b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandl
return (b3SharedMemoryCommandHandle) command;
}
void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, b3JointSensorState *state)
int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, b3JointSensorState *state)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
@@ -544,23 +544,32 @@ void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandl
if (bodyIndex>=0)
{
b3JointInfo info;
b3GetJointInfo(physClient, bodyIndex,jointIndex, &info);
state->m_jointPosition = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex];
state->m_jointVelocity = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex];
for (int ii(0); ii < 6; ++ii) {
state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii];
bool result = b3GetJointInfo(physClient, bodyIndex,jointIndex, &info);
if (result)
{
state->m_jointPosition = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex];
state->m_jointVelocity = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex];
for (int ii(0); ii < 6; ++ii) {
state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii];
}
state->m_jointMotorTorque = status->m_sendActualStateArgs.m_jointMotorForce[jointIndex];
return 1;
}
state->m_jointMotorTorque = status->m_sendActualStateArgs.m_jointMotorForce[jointIndex];
}
return 0;
}
void b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, b3LinkState *state)
int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, b3LinkState *state)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
int bodyIndex = status->m_sendActualStateArgs.m_bodyUniqueId;
b3Assert(bodyIndex>=0);
if (bodyIndex>=0)
b3Assert(linkIndex >= 0);
int numJoints = b3GetNumJoints(physClient,bodyIndex);
b3Assert(linkIndex < numJoints);
if ((bodyIndex>=0) && (linkIndex >= 0) && linkIndex < numJoints)
{
b3Transform wlf,com,inertial;
@@ -590,7 +599,9 @@ void b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle
{
state->m_worldLinkFrameOrientation[i] = wlfOrn[i];
}
return 1;
}
return 0;
}
b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient)
@@ -1259,6 +1270,18 @@ void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastI
}
///If you re-connected to an existing server, or server changed otherwise, sync the body info
b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type =CMD_SYNC_BODY_INFO;
return (b3SharedMemoryCommandHandle) command;
}
b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode)
{

7
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -57,6 +57,9 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
const double* actualStateQdot[],
const double* jointReactionForces[]);
///If you re-connected to an existing server, or server changed otherwise, sync the body info
b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient);
///return the total number of bodies in the simulation
int b3GetNumBodies(b3PhysicsClientHandle physClient);
@@ -293,8 +296,8 @@ int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle c
int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable);
b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId);
void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state);
void b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, struct b3LinkState *state);
int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state);
int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, struct b3LinkState *state);
b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldY, double rayFromWorldZ,

11
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -811,6 +811,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
b3Warning("createConstraint failed");
break;
}
case CMD_ACTUAL_STATE_UPDATE_FAILED:
{
b3Warning("request actual state failed");
break;
}
case CMD_SYNC_BODY_INFO_COMPLETED:
{
break;
}
default: {
b3Error("Unknown server status %d\n", serverCmd.m_type);
btAssert(0);
@@ -830,7 +839,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
}
if ((serverCmd.m_type == CMD_SDF_LOADING_COMPLETED) || (serverCmd.m_type == CMD_MJCF_LOADING_COMPLETED))
if ((serverCmd.m_type == CMD_SDF_LOADING_COMPLETED) || (serverCmd.m_type == CMD_MJCF_LOADING_COMPLETED) || (serverCmd.m_type == CMD_SYNC_BODY_INFO_COMPLETED))
{
int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
if (numBodies>0)

1
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -670,6 +670,7 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
break;
}
case CMD_SYNC_BODY_INFO_COMPLETED:
case CMD_MJCF_LOADING_COMPLETED:
case CMD_SDF_LOADING_COMPLETED:
{

29
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -423,6 +423,12 @@ struct PhysicsServerCommandProcessorInternalData
btAlignedObjectArray<InternalBodyHandle> m_bodyHandles;
int m_numUsedHandles; // number of active handles
int m_firstFreeHandle; // free handles list
int getNumHandles() const
{
return m_bodyHandles.size();
}
InternalBodyHandle* getHandle(int handle)
{
btAssert(handle>=0);
@@ -1741,6 +1747,23 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
break;
}
case CMD_SYNC_BODY_INFO:
{
int numHandles = m_data->getNumHandles();
int actualNumBodies = 0;
for (int i=0;i<numHandles;i++)
{
InteralBodyData* body = m_data->getHandle(i);
if (body && (body->m_multiBody || body->m_rigidBody))
{
serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[actualNumBodies++] = i;
}
}
serverStatusOut.m_sdfLoadedArgs.m_numBodies = actualNumBodies;
serverStatusOut.m_type = CMD_SYNC_BODY_INFO_COMPLETED;
hasStatus = true;
break;
}
case CMD_REQUEST_BODY_INFO:
{
const SdfRequestInfoArgs& sdfInfoArgs = clientCmd.m_sdfRequestInfoArgs;
@@ -3672,7 +3695,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
if (userConstraintPtr->m_rbConstraint)
{
//todo
}
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = -1;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
@@ -4367,7 +4390,7 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
}
btVector3 gVRGripperPos(0.6, 0.4, 0.7);
btVector3 gVRGripperPos(0.7, 0.3, 0.7);
btQuaternion gVRGripperOrn(0,0,0,1);
btVector3 gVRController2Pos(0,0,0.2);
btQuaternion gVRController2Orn(0,0,0,1);
@@ -4764,7 +4787,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
loadUrdf("husky/husky.urdf", btVector3(2, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_huskyId = bodyId;
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
}

4
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -1671,8 +1671,8 @@ void PhysicsServerExample::drawUserDebugLines()
void PhysicsServerExample::renderScene()
{
btTransform vrTrans;
gVRTeleportPos1 = gVRTeleportPosLocal;
gVRTeleportOrn = gVRTeleportOrnLocal;
//gVRTeleportPos1 = gVRTeleportPosLocal;
//gVRTeleportOrn = gVRTeleportOrnLocal;
///little VR test to follow/drive Husky vehicle
if (gVRTrackingObjectUniqueId >= 0)

2
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -33,7 +33,7 @@
#define MAX_SDF_FILENAME_LENGTH 1024
#define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH
#define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM
#define MAX_SDF_BODIES 500
#define MAX_SDF_BODIES 512
struct TmpFloat3
{

4
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -47,7 +47,7 @@ enum EnumSharedMemoryClientCommand
CMD_USER_DEBUG_DRAW,
CMD_REQUEST_VR_EVENTS_DATA,
CMD_SET_VR_CAMERA_STATE,
CMD_SYNC_BODY_INFO,
//don't go beyond this command!
CMD_MAX_CLIENT_COMMANDS,
@@ -115,6 +115,8 @@ enum EnumSharedMemoryServerStatus
CMD_USER_CONSTRAINT_FAILED,
CMD_REQUEST_VR_EVENTS_DATA_COMPLETED,
CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED,
CMD_SYNC_BODY_INFO_COMPLETED,
CMD_SYNC_BODY_INFO_FAILED,
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
CMD_MAX_SERVER_COMMANDS
};

3
examples/StandaloneMain/hellovr_opengl_main.cpp
View File

@@ -2246,12 +2246,13 @@ int main(int argc, char *argv[])
if (sExample)
{
//until we have a proper VR gui, always assume we want the hard-coded default robot assets
#if 0
char* newargv[2];
char* t0 = (char*)"--robotassets";
newargv[0] = t0;
newargv[1] = t0;
sExample->processCommandLineArgs(2,newargv);
#endif
sExample->processCommandLineArgs(argc,argv);
}

32
examples/pybullet/hand.ino Normal file
View 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
View 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)

130
examples/pybullet/pybullet.c
View File

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

1
examples/pybullet/vrEvent.py
View File

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