diff --git a/docs/pybullet_quickstartguide.pdf b/docs/pybullet_quickstartguide.pdf index a46bbf128..41d4a60b2 100644 Binary files a/docs/pybullet_quickstartguide.pdf and b/docs/pybullet_quickstartguide.pdf differ diff --git a/examples/OpenGLWindow/SimpleCamera.cpp b/examples/OpenGLWindow/SimpleCamera.cpp index f0ba375d2..4b0b27164 100644 --- a/examples/OpenGLWindow/SimpleCamera.cpp +++ b/examples/OpenGLWindow/SimpleCamera.cpp @@ -71,6 +71,21 @@ void SimpleCamera::setVRCamera(const float viewMat[16], const float projectionMa } } +bool SimpleCamera::getVRCamera(float viewMat[16], float projectionMatrix[16]) +{ + if (m_data->m_enableVR) + { + for (int i=0;i<16;i++) + { + viewMat[i] = m_data->m_viewMatrixVR[i]; + projectionMatrix[i] = m_data->m_projectionMatrixVR[i]; + } + } + return false; +} + + + void SimpleCamera::disableVRCamera() { m_data->m_enableVR = false; diff --git a/examples/OpenGLWindow/SimpleCamera.h b/examples/OpenGLWindow/SimpleCamera.h index 7221f01a6..5a61a729f 100644 --- a/examples/OpenGLWindow/SimpleCamera.h +++ b/examples/OpenGLWindow/SimpleCamera.h @@ -15,6 +15,8 @@ struct SimpleCamera : public CommonCameraInterface virtual void getCameraViewMatrix(float m[16]) const; virtual void setVRCamera(const float viewMat[16], const float projectionMatrix[16]); + virtual bool getVRCamera(float viewMat[16], float projectionMatrix[16]); + virtual void setVRCameraOffsetTransform(const float offset[16]); virtual void disableVRCamera(); diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index e30c1519c..56fe09ee6 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2191,3 +2191,54 @@ void b3GetVREventsData(b3PhysicsClientHandle physClient, struct b3VREventsData* cl->getCachedVREvents(vrEventsData); } } + +b3SharedMemoryCommandHandle b3SetVRCameraStateCommandInit(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + + command->m_type = CMD_SET_VR_CAMERA_STATE; + command->m_updateFlags = 0; + + return (b3SharedMemoryCommandHandle)command; + +} + +int b3SetVRCameraRootPosition(b3SharedMemoryCommandHandle commandHandle, double rootPos[3]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_SET_VR_CAMERA_STATE); + command->m_updateFlags |= VR_CAMERA_ROOT_POSITION; + command->m_vrCameraStateArguments.m_rootPosition[0] = rootPos[0]; + command->m_vrCameraStateArguments.m_rootPosition[1] = rootPos[1]; + command->m_vrCameraStateArguments.m_rootPosition[2] = rootPos[2]; + return 0; + +} + +int b3SetVRCameraRootOrientation(b3SharedMemoryCommandHandle commandHandle, double rootOrn[4]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_SET_VR_CAMERA_STATE); + command->m_updateFlags |= VR_CAMERA_ROOT_ORIENTATION; + command->m_vrCameraStateArguments.m_rootOrientation[0] = rootOrn[0]; + command->m_vrCameraStateArguments.m_rootOrientation[1] = rootOrn[1]; + command->m_vrCameraStateArguments.m_rootOrientation[2] = rootOrn[2]; + return 0; +} + +int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_SET_VR_CAMERA_STATE); + command->m_updateFlags |= VR_CAMERA_ROOT_TRACKING_OBJECT; + command->m_vrCameraStateArguments.m_trackingObjectUniqueId = objectUniqueId; + return 0; +} + diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index b8b27dd53..2f553ea73 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -314,6 +314,12 @@ int b3LoadBunnySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, dou b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient); void b3GetVREventsData(b3PhysicsClientHandle physClient, struct b3VREventsData* vrEventsData); +b3SharedMemoryCommandHandle b3SetVRCameraStateCommandInit(b3PhysicsClientHandle physClient); +int b3SetVRCameraRootPosition(b3SharedMemoryCommandHandle commandHandle, double rootPos[3]); +int b3SetVRCameraRootOrientation(b3SharedMemoryCommandHandle commandHandle, double rootOrn[4]); +int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId); + + #ifdef __cplusplus diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 911a501f3..7f98b8dfb 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -44,11 +44,14 @@ bool gEnableRealTimeSimVR=false; bool gCreateDefaultRobotAssets = false; int gInternalSimFlags = 0; bool gResetSimulation = 0; -int gHuskyId = -1; -btTransform huskyTr = btTransform::getIdentity(); +int gVRTrackingObjectUniqueId = -1; +btTransform gVRTrackingObjectTr = btTransform::getIdentity(); int gCreateObjectSimVR = -1; int gEnableKukaControl = 0; +btVector3 gVRTeleportPos1(0,0,0); +btQuaternion gVRTeleportOrn(0, 0, 0,1); + btScalar simTimeScalingFactor = 1; btScalar gRhsClamp = 1.f; @@ -1369,6 +1372,32 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } #endif + case CMD_SET_VR_CAMERA_STATE: + { + + if (clientCmd.m_updateFlags & VR_CAMERA_ROOT_POSITION) + { + gVRTeleportPos1[0] = clientCmd.m_vrCameraStateArguments.m_rootPosition[0]; + gVRTeleportPos1[1] = clientCmd.m_vrCameraStateArguments.m_rootPosition[1]; + gVRTeleportPos1[2] = clientCmd.m_vrCameraStateArguments.m_rootPosition[2]; + } + if (clientCmd.m_updateFlags & VR_CAMERA_ROOT_ORIENTATION) + { + gVRTeleportOrn[0] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[0]; + gVRTeleportOrn[1] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[1]; + gVRTeleportOrn[2] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[2]; + gVRTeleportOrn[3] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[3]; + } + + if (clientCmd.m_updateFlags & VR_CAMERA_ROOT_TRACKING_OBJECT) + { + gVRTrackingObjectUniqueId = clientCmd.m_vrCameraStateArguments.m_trackingObjectUniqueId; + } + + serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; + hasStatus = true; + break; + } case CMD_REQUEST_VR_EVENTS_DATA: { serverStatusOut.m_sendVREvents.m_numVRControllerEvents = 0; @@ -4287,12 +4316,12 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec, const gSubStep = m_data->m_physicsDeltaTime; } - if (gHuskyId >= 0) + if (gVRTrackingObjectUniqueId >= 0) { - InternalBodyHandle* bodyHandle = m_data->getHandle(gHuskyId); + InternalBodyHandle* bodyHandle = m_data->getHandle(gVRTrackingObjectUniqueId); if (bodyHandle && bodyHandle->m_multiBody) { - huskyTr = bodyHandle->m_multiBody->getBaseWorldTransform(); + gVRTrackingObjectTr = bodyHandle->m_multiBody->getBaseWorldTransform(); } } @@ -4595,8 +4624,6 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets() //loadUrdf("knight.urdf", btVector3(-1.2, 0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("husky/husky.urdf", btVector3(2, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - gHuskyId = bodyId; - b3Printf("huskyId = %d", gHuskyId); m_data->m_huskyId = bodyId; m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10)); diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 889c3a5b8..994312986 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -26,9 +26,14 @@ //@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet! extern btVector3 gLastPickPos; -btVector3 gVRTeleportPos1(0,0,0); + +btVector3 gVRTeleportPosLocal(0,0,0); +btQuaternion gVRTeleportOrnLocal(0,0,0,1); + +extern btVector3 gVRTeleportPos1; +extern btQuaternion gVRTeleportOrn; btScalar gVRTeleportRotZ = 0; -btQuaternion gVRTeleportOrn(0, 0, 0,1); + extern btVector3 gVRGripperPos; extern btQuaternion gVRGripperOrn; extern btVector3 gVRController2Pos; @@ -57,9 +62,9 @@ static void saveCurrentSettingsVR() FILE* f = fopen(startFileNameVR, "w"); if (f) { - fprintf(f, "--camPosX= %f\n", gVRTeleportPos1[0]); - fprintf(f, "--camPosY= %f\n", gVRTeleportPos1[1]); - fprintf(f, "--camPosZ= %f\n", gVRTeleportPos1[2]); + fprintf(f, "--camPosX= %f\n", gVRTeleportPosLocal[0]); + fprintf(f, "--camPosY= %f\n", gVRTeleportPosLocal[1]); + fprintf(f, "--camPosZ= %f\n", gVRTeleportPosLocal[2]); fprintf(f, "--camRotZ= %f\n", gVRTeleportRotZ); fclose(f); } @@ -109,9 +114,9 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void if (message->at(1) == 16) { gVRTeleportRotZ= getParamf(-3.1415, 3.1415, message->at(2)); - gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ); + gVRTeleportOrnLocal = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ); saveCurrentSettingsVR(); -// b3Printf("gVRTeleportOrn rotZ = %f\n", gVRTeleportRotZ); +// b3Printf("gVRTeleportOrnLocal rotZ = %f\n", gVRTeleportRotZ); } if (message->at(1) == 32) @@ -123,9 +128,9 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void { if (message->at(1) == i) { - gVRTeleportPos1[i] = getParamf(-2, 2, message->at(2)); + gVRTeleportPosLocal[i] = getParamf(-2, 2, message->at(2)); saveCurrentSettingsVR(); -// b3Printf("gVRTeleportPos[%d] = %f\n", i,gVRTeleportPos1[i]); +// b3Printf("gVRTeleportPos[%d] = %f\n", i,gVRTeleportPosLocal[i]); } } @@ -1040,19 +1045,19 @@ public: setSharedMemoryKey(shmemKey); } - if (args.GetCmdLineArgument("camPosX", gVRTeleportPos1[0])) + if (args.GetCmdLineArgument("camPosX", gVRTeleportPosLocal[0])) { - printf("camPosX=%f\n", gVRTeleportPos1[0]); + printf("camPosX=%f\n", gVRTeleportPosLocal[0]); } - if (args.GetCmdLineArgument("camPosY", gVRTeleportPos1[1])) + if (args.GetCmdLineArgument("camPosY", gVRTeleportPosLocal[1])) { - printf("camPosY=%f\n", gVRTeleportPos1[1]); + printf("camPosY=%f\n", gVRTeleportPosLocal[1]); } - if (args.GetCmdLineArgument("camPosZ", gVRTeleportPos1[2])) + if (args.GetCmdLineArgument("camPosZ", gVRTeleportPosLocal[2])) { - printf("camPosZ=%f\n", gVRTeleportPos1[2]); + printf("camPosZ=%f\n", gVRTeleportPosLocal[2]); } float camRotZ = 0.f; @@ -1060,7 +1065,7 @@ public: { printf("camRotZ = %f\n", camRotZ); btQuaternion ornZ(btVector3(0, 0, 1), camRotZ); - gVRTeleportOrn = ornZ; + gVRTeleportOrnLocal = ornZ; } if (args.CheckCmdLineFlag("robotassets")) @@ -1458,8 +1463,8 @@ extern int gDroppedSimulationSteps; extern int gNumSteps; extern double gDtInSec; extern double gSubStep; -extern int gHuskyId; -extern btTransform huskyTr; +extern int gVRTrackingObjectUniqueId; +extern btTransform gVRTrackingObjectTr; struct LineSegment { @@ -1591,15 +1596,22 @@ void PhysicsServerExample::drawUserDebugLines() void PhysicsServerExample::renderScene() { + btTransform vrTrans; + gVRTeleportPos1 = gVRTeleportPosLocal; + gVRTeleportOrn = gVRTeleportOrnLocal; -#if 0 ///little VR test to follow/drive Husky vehicle - if (gHuskyId >= 0) + if (gVRTrackingObjectUniqueId >= 0) { - gVRTeleportPos1 = huskyTr.getOrigin(); - gVRTeleportOrn = huskyTr.getRotation(); + btTransform vrTrans; + vrTrans.setOrigin(gVRTeleportPosLocal); + vrTrans.setRotation(gVRTeleportOrnLocal); + + vrTrans = vrTrans * gVRTrackingObjectTr; + + gVRTeleportPos1 = vrTrans.getOrigin(); + gVRTeleportOrn = vrTrans.getRotation(); } -#endif B3_PROFILE("PhysicsServerExample::RenderScene"); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 4efee8023..cd22178f3 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -594,6 +594,26 @@ struct UserDebugDrawResultArgs int m_debugItemUniqueId; }; +struct SendVREvents +{ + int m_numVRControllerEvents; + b3VRControllerEvent m_controllerEvents[MAX_VR_CONTROLLERS]; +}; + +enum eVRCameraEnums +{ + VR_CAMERA_ROOT_POSITION=1, + VR_CAMERA_ROOT_ORIENTATION=2, + VR_CAMERA_ROOT_TRACKING_OBJECT=4 +}; + +struct VRCameraState +{ + double m_rootPosition[3]; + double m_rootOrientation[4]; + int m_trackingObjectUniqueId; +}; + struct SharedMemoryCommand { @@ -635,6 +655,7 @@ struct SharedMemoryCommand struct UserDebugDrawArgs m_userDebugDrawArgs; struct RequestRaycastIntersections m_requestRaycastIntersections; struct LoadBunnyArgs m_loadBunnyArguments; + struct VRCameraState m_vrCameraStateArguments; }; }; @@ -657,11 +678,8 @@ struct SendOverlappingObjectsArgs int m_numRemainingOverlappingObjects; }; -struct SendVREvents -{ - int m_numVRControllerEvents; - b3VRControllerEvent m_controllerEvents[MAX_VR_CONTROLLERS]; -}; + + diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 7eb9d37ad..a517659bb 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -46,6 +46,8 @@ enum EnumSharedMemoryClientCommand CMD_SET_SHADOW, CMD_USER_DEBUG_DRAW, CMD_REQUEST_VR_EVENTS_DATA, + CMD_SET_VR_CAMERA_STATE, + //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index da4cf300b..2fce9796f 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -2431,6 +2431,56 @@ static PyObject* pybullet_getMatrixFromQuaterion(PyObject* self, PyObject* args) }; +static PyObject* pybullet_setVRCameraState(PyObject* self, PyObject* args, PyObject *keywds) +{ + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + PyObject* rootPosObj=0; + PyObject* rootOrnObj=0; + int trackObjectUid=-2; + double rootPos[3]; + double rootOrn[4]; + + static char *kwlist[] = {"rootPosition", "rootOrientation", "trackObject", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|OOii", kwlist,&rootPosObj, &rootOrnObj, &trackObjectUid,&physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + commandHandle = b3SetVRCameraStateCommandInit(sm); + + if (pybullet_internalSetVectord(rootPosObj,rootPos)) + { + b3SetVRCameraRootPosition(commandHandle,rootPos); + } + if (pybullet_internalSetVector4d(rootOrnObj,rootOrn)) + { + b3SetVRCameraRootOrientation(commandHandle,rootOrn); + } + + if (trackObjectUid>=-1) + { + b3SetVRCameraTrackingObject(commandHandle,trackObjectUid); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + + Py_INCREF(Py_None); + return Py_None; + +} + + static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject *keywds) { b3SharedMemoryCommandHandle commandHandle; @@ -3732,10 +3782,10 @@ static PyObject* pybullet_renderImageObsolete(PyObject* self, PyObject* args) { static PyObject* pybullet_applyExternalForce(PyObject* self, PyObject* args,PyObject* keywds) { { - int objectUniqueId, linkIndex, flags; + int objectUniqueId=-1, linkIndex=-1, flags; double force[3]; double position[3] = {0.0, 0.0, 0.0}; - PyObject* forceObj, *posObj; + PyObject* forceObj=0, *posObj=0; b3SharedMemoryCommandHandle command; b3SharedMemoryStatusHandle statusHandle; @@ -4304,15 +4354,15 @@ static PyMethodDef SpamMethods[] = { "Set a single joint motor control mode and desired target value. There is " "no immediate state change, stepSimulation will process the motors."}, - {"applyExternalForce",(PyCFunction) pybullet_applyExternalForce, METH_VARARGS, + {"applyExternalForce",(PyCFunction) pybullet_applyExternalForce, METH_VARARGS| METH_KEYWORDS, "for objectUniqueId, linkIndex (-1 for base/root link), apply a force " "[x,y,z] at the a position [x,y,z], flag to select FORCE_IN_LINK_FRAME or " - "FORCE_IN_WORLD_FRAME coordinates"}, + "WORLD_FRAME coordinates"}, {"applyExternalTorque", (PyCFunction)pybullet_applyExternalTorque, METH_VARARGS| METH_KEYWORDS, "for objectUniqueId, linkIndex (-1 for base/root link) apply a torque " "[x,y,z] in Cartesian coordinates, flag to select TORQUE_IN_LINK_FRAME or " - "TORQUE_IN_WORLD_FRAME coordinates"}, + "WORLD_FRAME coordinates"}, {"renderImage", pybullet_renderImageObsolete, METH_VARARGS, "obsolete, please use getCameraImage and getViewProjectionMatrices instead" @@ -4420,6 +4470,11 @@ static PyMethodDef SpamMethods[] = { { "getVREvents", (PyCFunction)pybullet_getVREvents, METH_VARARGS | METH_KEYWORDS, "Get Virtual Reality events, for example to track VR controllers position/buttons" }, + { "setVRCameraState", (PyCFunction)pybullet_setVRCameraState, METH_VARARGS | METH_KEYWORDS, + "Set properties of the VR Camera such as its root transform " + "for teleporting or to track objects (camera inside a vehicle for example)." + }, + { "rayTest", (PyCFunction)pybullet_rayTest, METH_VARARGS | METH_KEYWORDS, "Cast a ray and return the first object hit, if any. " diff --git a/examples/pybullet/robotcontrol.py b/examples/pybullet/robotcontrol.py index e94d6d5ba..3541b9d9e 100644 --- a/examples/pybullet/robotcontrol.py +++ b/examples/pybullet/robotcontrol.py @@ -23,7 +23,7 @@ for joint in range (2,6) : for step in range (400): p.stepSimulation() -p.getContactPointData(husky) +p.getContactPoints(husky) p.disconnect()