diff --git a/data/MPL/LICENSE.txt b/data/MPL/LICENSE.txt new file mode 100644 index 000000000..a89a42bce --- /dev/null +++ b/data/MPL/LICENSE.txt @@ -0,0 +1,19 @@ + \ No newline at end of file diff --git a/data/MPL/MPL.xml b/data/MPL/MPL.xml new file mode 100644 index 000000000..5800a1200 --- /dev/null +++ b/data/MPL/MPL.xml @@ -0,0 +1,473 @@ + + + + + diff --git a/data/MPL/mesh/index0.STL b/data/MPL/mesh/index0.STL new file mode 100644 index 000000000..1189fb830 Binary files /dev/null and b/data/MPL/mesh/index0.STL differ diff --git a/data/MPL/mesh/index1.STL b/data/MPL/mesh/index1.STL new file mode 100644 index 000000000..9c7a277da Binary files /dev/null and b/data/MPL/mesh/index1.STL differ diff --git a/data/MPL/mesh/index2.STL b/data/MPL/mesh/index2.STL new file mode 100644 index 000000000..94e5b4d4f Binary files /dev/null and b/data/MPL/mesh/index2.STL differ diff --git a/data/MPL/mesh/index3.STL b/data/MPL/mesh/index3.STL new file mode 100644 index 000000000..acdcd807e Binary files /dev/null and b/data/MPL/mesh/index3.STL differ diff --git a/data/MPL/mesh/middle0.STL b/data/MPL/mesh/middle0.STL new file mode 100644 index 000000000..1189fb830 Binary files /dev/null and b/data/MPL/mesh/middle0.STL differ diff --git a/data/MPL/mesh/middle1.STL b/data/MPL/mesh/middle1.STL new file mode 100644 index 000000000..b29f6e640 Binary files /dev/null and b/data/MPL/mesh/middle1.STL differ diff --git a/data/MPL/mesh/middle2.STL b/data/MPL/mesh/middle2.STL new file mode 100644 index 000000000..0ae92882a Binary files /dev/null and b/data/MPL/mesh/middle2.STL differ diff --git a/data/MPL/mesh/middle3.STL b/data/MPL/mesh/middle3.STL new file mode 100644 index 000000000..acdcd807e Binary files /dev/null and b/data/MPL/mesh/middle3.STL differ diff --git a/data/MPL/mesh/palm.STL b/data/MPL/mesh/palm.STL new file mode 100644 index 000000000..7a4e9d0ae Binary files /dev/null and b/data/MPL/mesh/palm.STL differ diff --git a/data/MPL/mesh/pinky0.STL b/data/MPL/mesh/pinky0.STL new file mode 100644 index 000000000..109b68199 Binary files /dev/null and b/data/MPL/mesh/pinky0.STL differ diff --git a/data/MPL/mesh/pinky1.STL b/data/MPL/mesh/pinky1.STL new file mode 100644 index 000000000..8e2e98211 Binary files /dev/null and b/data/MPL/mesh/pinky1.STL differ diff --git a/data/MPL/mesh/pinky2.STL b/data/MPL/mesh/pinky2.STL new file mode 100644 index 000000000..8413cc85b Binary files /dev/null and b/data/MPL/mesh/pinky2.STL differ diff --git a/data/MPL/mesh/pinky3.STL b/data/MPL/mesh/pinky3.STL new file mode 100644 index 000000000..4e539d732 Binary files /dev/null and b/data/MPL/mesh/pinky3.STL differ diff --git a/data/MPL/mesh/ring0.STL b/data/MPL/mesh/ring0.STL new file mode 100644 index 000000000..1189fb830 Binary files /dev/null and b/data/MPL/mesh/ring0.STL differ diff --git a/data/MPL/mesh/ring1.STL b/data/MPL/mesh/ring1.STL new file mode 100644 index 000000000..b29f6e640 Binary files /dev/null and b/data/MPL/mesh/ring1.STL differ diff --git a/data/MPL/mesh/ring2.STL b/data/MPL/mesh/ring2.STL new file mode 100644 index 000000000..0ae92882a Binary files /dev/null and b/data/MPL/mesh/ring2.STL differ diff --git a/data/MPL/mesh/ring3.STL b/data/MPL/mesh/ring3.STL new file mode 100644 index 000000000..acdcd807e Binary files /dev/null and b/data/MPL/mesh/ring3.STL differ diff --git a/data/MPL/mesh/thumb0.STL b/data/MPL/mesh/thumb0.STL new file mode 100644 index 000000000..6764cb45c Binary files /dev/null and b/data/MPL/mesh/thumb0.STL differ diff --git a/data/MPL/mesh/thumb1.STL b/data/MPL/mesh/thumb1.STL new file mode 100644 index 000000000..a13de7c3e Binary files /dev/null and b/data/MPL/mesh/thumb1.STL differ diff --git a/data/MPL/mesh/thumb2.STL b/data/MPL/mesh/thumb2.STL new file mode 100644 index 000000000..3555fbd93 Binary files /dev/null and b/data/MPL/mesh/thumb2.STL differ diff --git a/data/MPL/mesh/thumb3.STL b/data/MPL/mesh/thumb3.STL new file mode 100644 index 000000000..2e2699eaf Binary files /dev/null and b/data/MPL/mesh/thumb3.STL differ diff --git a/data/MPL/mesh/wristx.STL b/data/MPL/mesh/wristx.STL new file mode 100644 index 000000000..b9b81e611 Binary files /dev/null and b/data/MPL/mesh/wristx.STL differ diff --git a/data/MPL/mesh/wristy.STL b/data/MPL/mesh/wristy.STL new file mode 100644 index 000000000..cbd3fdbfd Binary files /dev/null and b/data/MPL/mesh/wristy.STL differ diff --git a/data/MPL/mesh/wristz.STL b/data/MPL/mesh/wristz.STL new file mode 100644 index 000000000..a13bed861 Binary files /dev/null and b/data/MPL/mesh/wristz.STL differ diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index ab18dbe0a..37e7cb78b 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -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) { diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 4a2333749..54f1ad1e1 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -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, diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 5b3f3ac81..df2aed6a7 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -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) diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 1fc760482..07dc561af 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -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: { diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 6c0f23b4f..af2d5c2d8 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -423,6 +423,12 @@ struct PhysicsServerCommandProcessorInternalData btAlignedObjectArray 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;igetHandle(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)); } diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 487f46e98..07b980dcb 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -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) diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index f87e44c0c..1c85c3b7d 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -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 { diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 571aa05e7..e8350560a 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -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 }; diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp index 71196c11d..7ef40476c 100644 --- a/examples/StandaloneMain/hellovr_opengl_main.cpp +++ b/examples/StandaloneMain/hellovr_opengl_main.cpp @@ -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); } diff --git a/examples/pybullet/hand.ino b/examples/pybullet/hand.ino new file mode 100644 index 000000000..faf5b70bc --- /dev/null +++ b/examples/pybullet/hand.ino @@ -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); +} diff --git a/examples/pybullet/hand.py b/examples/pybullet/hand.py new file mode 100644 index 000000000..d0428c555 --- /dev/null +++ b/examples/pybullet/hand.py @@ -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 (vmaxV): + 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) \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 07af59b6d..c717df3f1 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -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; + } } } diff --git a/examples/pybullet/vrEvent.py b/examples/pybullet/vrEvent.py index 32eb1edd9..beb369b1d 100644 --- a/examples/pybullet/vrEvent.py +++ b/examples/pybullet/vrEvent.py @@ -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 diff --git a/examples/pybullet/vr_kuka_setup.py b/examples/pybullet/vr_kuka_setup.py new file mode 100644 index 000000000..5a404cd46 --- /dev/null +++ b/examples/pybullet/vr_kuka_setup.py @@ -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()