static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) { if (0==sm) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } ///request an image from a simulated camera, using a software renderer. struct b3CameraImageData imageData; PyObject* objViewMat,* objProjMat; int width, height; if (PyArg_ParseTuple(args, "iiOO", &width, &height, &objViewMat, &objProjMat)) { PyObject* seq; int i, len; PyObject* item; float viewMatrix[16]; float projectionMatrix[16]; int valid = 1; { seq = PySequence_Fast(objViewMat, "expected a sequence"); len = PySequence_Size(objViewMat); //printf("objViewMat size = %d\n", len); if (len==16) { if (PyList_Check(seq)) { for (i = 0; i < len; i++) { item = PyList_GET_ITEM(seq, i); viewMatrix[i] = PyFloat_AsDouble(item); float v = viewMatrix[i]; //printf("view %d to %f\n", i,v); } } else { for (i = 0; i < len; i++) { item = PyTuple_GET_ITEM(seq,i); viewMatrix[i] = PyFloat_AsDouble(item); } } } else { valid = 0; } } { seq = PySequence_Fast(objProjMat, "expected a sequence"); len = PySequence_Size(objProjMat); //printf("projMat len = %d\n", len); if (len==16) { if (PyList_Check(seq)) { for (i = 0; i < len; i++) { item = PyList_GET_ITEM(seq, i); projectionMatrix[i] = PyFloat_AsDouble(item); } } else { for (i = 0; i < len; i++) { item = PyTuple_GET_ITEM(seq,i); projectionMatrix[i] = PyFloat_AsDouble(item); } } } else { valid = 0; } } Py_DECREF(seq); { b3SharedMemoryCommandHandle command; command = b3InitRequestCameraImage(sm); if (valid) { //printf("set b3RequestCameraImageSetCameraMatrices\n"); b3RequestCameraImageSetCameraMatrices(command, viewMatrix, projectionMatrix); } b3RequestCameraImageSetPixelResolution(command,width,height); if (b3CanSubmitCommand(sm)) { b3SharedMemoryStatusHandle statusHandle; int statusType; statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); if (statusType==CMD_CAMERA_IMAGE_COMPLETED) { PyObject *item2; PyObject* pyResultList;//store 4 elements in this result: width, height, rgbData, depth b3GetCameraImageData(sm, &imageData); //todo: error handling if image size is 0 pyResultList = PyTuple_New(4); PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth)); PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight)); PyObject *pylistPos; PyObject* pylistDep; int i,j,p; //printf("image width = %d, height = %d\n", imageData.m_pixelWidth, imageData.m_pixelHeight); { PyObject *item; int bytesPerPixel = 4;//Red, Green, Blue, each 8 bit values int num=bytesPerPixel*imageData.m_pixelWidth*imageData.m_pixelHeight; pylistPos = PyTuple_New(num); pylistDep = PyTuple_New(imageData.m_pixelWidth*imageData.m_pixelHeight); for (i=0;i<imageData.m_pixelWidth;i++) { for (j=0;j<imageData.m_pixelHeight;j++) { int depIndex = i+j*imageData.m_pixelWidth; item = PyFloat_FromDouble(imageData.m_depthValues[depIndex]); PyTuple_SetItem(pylistDep, depIndex, item); for (p=0;p<bytesPerPixel;p++) { int pixelIndex = bytesPerPixel*(i+j*imageData.m_pixelWidth)+p; item = PyInt_FromLong(imageData.m_rgbColorData[pixelIndex]); PyTuple_SetItem(pylistPos, pixelIndex, item); } } } } PyTuple_SetItem(pyResultList, 2,pylistPos); PyTuple_SetItem(pyResultList, 3,pylistDep); return pyResultList; } } } } Py_INCREF(Py_None); return Py_None; }
void testSharedMemory(b3PhysicsClientHandle sm) { int i, dofCount , posVarCount, ret ,numJoints ; int sensorJointIndexLeft=-1; int sensorJointIndexRight=-1; const char* urdfFileName = "r2d2.urdf"; const char* sdfFileName = "kuka_iiwa/model.sdf"; double gravx=0, gravy=0, gravz=-9.8; double timeStep = 1./60.; double startPosX, startPosY,startPosZ; int imuLinkIndex = -1; int bodyIndex = -1; if (b3CanSubmitCommand(sm)) { { b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); b3SharedMemoryStatusHandle statusHandle; ret = b3PhysicsParamSetGravity(command, gravx,gravy, gravz); ret = b3PhysicsParamSetTimeStep(command, timeStep); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); } { b3SharedMemoryStatusHandle statusHandle; int statusType; int bodyIndicesOut[10];//MAX_SDF_BODIES = 10 int numJoints, numBodies; int bodyUniqueId; b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(sm, sdfFileName); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); ASSERT_EQ(statusType, CMD_SDF_LOADING_COMPLETED); numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, 10); ASSERT_EQ(numBodies,1); bodyUniqueId = bodyIndicesOut[0]; { { b3SharedMemoryStatusHandle statusHandle; int statusType; b3SharedMemoryCommandHandle command = b3InitRequestVisualShapeInformation(sm, bodyUniqueId); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED) { struct b3VisualShapeInformation vi; b3GetVisualShapeInformation(sm, &vi); } } } numJoints = b3GetNumJoints(sm,bodyUniqueId); ASSERT_EQ(numJoints,7); #if 0 b3Printf("numJoints: %d\n", numJoints); for (i=0;i<numJoints;i++) { struct b3JointInfo jointInfo; if (b3GetJointInfo(sm,bodyUniqueId, i,&jointInfo)) { b3Printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName); } } #endif { b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryCommandHandle commandHandle; double jointAngle = 0.f; int jointIndex; commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId); for (jointIndex=0;jointIndex<numJoints;jointIndex++) { b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, jointAngle); } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); } } { b3SharedMemoryStatusHandle statusHandle; int statusType; b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName); //setting the initial position, orientation and other arguments are optional startPosX =2; startPosY =0; startPosZ = 1; ret = b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); ASSERT_EQ(statusType, CMD_URDF_LOADING_COMPLETED); bodyIndex = b3GetStatusBodyIndex(statusHandle); } if (bodyIndex>=0) { numJoints = b3GetNumJoints(sm,bodyIndex); for (i=0;i<numJoints;i++) { struct b3JointInfo jointInfo; b3GetJointInfo(sm,bodyIndex, i,&jointInfo); // printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName); //pick the IMU link index based on torso name if (strstr(jointInfo.m_linkName,"base_link")) { imuLinkIndex = i; } //pick the joint index based on joint name if (strstr(jointInfo.m_jointName,"base_to_left_leg")) { sensorJointIndexLeft = i; } if (strstr(jointInfo.m_jointName,"base_to_right_leg")) { sensorJointIndexRight = i; } } if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0)) { b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm, bodyIndex); b3SharedMemoryStatusHandle statusHandle; if (imuLinkIndex>=0) { ret = b3CreateSensorEnableIMUForLink(command, imuLinkIndex, 1); } if (sensorJointIndexLeft>=0) { ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexLeft, 1); } if(sensorJointIndexRight>=0) { ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexRight, 1); } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); } } { b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryCommandHandle command = b3CreateBoxShapeCommandInit(sm); ret = b3CreateBoxCommandSetStartPosition(command, 0,0,-1); ret = b3CreateBoxCommandSetStartOrientation(command,0,0,0,1); ret = b3CreateBoxCommandSetHalfExtents(command, 10,10,1); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); ASSERT_EQ(b3GetStatusType(statusHandle), CMD_RIGID_BODY_CREATION_COMPLETED); } { int statusType; b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(sm,bodyIndex); b3SharedMemoryStatusHandle statusHandle; statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); ASSERT_EQ(statusType, CMD_ACTUAL_STATE_UPDATE_COMPLETED); if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED) { b3GetStatusActualState(statusHandle, 0, &posVarCount, &dofCount, 0, 0, 0, 0); ASSERT_EQ(posVarCount,15); ASSERT_EQ(dofCount,14); } } { #if 0 b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryCommandHandle command = b3JointControlCommandInit( sm, CONTROL_MODE_VELOCITY); for ( dofIndex=0;dofIndex<dofCount;dofIndex++) { b3JointControlSetDesiredVelocity(command,dofIndex,1); b3JointControlSetMaximumForce(command,dofIndex,100); } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); #endif } ///perform some simulation steps for testing for ( i=0;i<1000;i++) { b3SharedMemoryStatusHandle statusHandle; int statusType; if (b3CanSubmitCommand(sm)) { statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitStepSimulationCommand(sm)); statusType = b3GetStatusType(statusHandle); ASSERT_EQ(statusType, CMD_STEP_FORWARD_SIMULATION_COMPLETED); } else { break; } } { b3SharedMemoryCommandHandle command; b3SharedMemoryStatusHandle statusHandle; int width = 1024; int height = 1024; command = b3InitRequestCameraImage(sm); b3RequestCameraImageSetPixelResolution(command, width, height); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); } if (b3CanSubmitCommand(sm)) { b3SharedMemoryStatusHandle state = b3SubmitClientCommandAndWaitStatus(sm, b3RequestActualStateCommandInit(sm,bodyIndex)); if (sensorJointIndexLeft>=0) { struct b3JointSensorState sensorState; b3GetJointState(sm,state,sensorJointIndexLeft,&sensorState); b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexLeft, sensorState.m_jointForceTorque[0], sensorState.m_jointForceTorque[1], sensorState.m_jointForceTorque[2]); } if (sensorJointIndexRight>=0) { struct b3JointSensorState sensorState; b3GetJointState(sm,state,sensorJointIndexRight,&sensorState); b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexRight, sensorState.m_jointForceTorque[0], sensorState.m_jointForceTorque[1], sensorState.m_jointForceTorque[2]); } { b3SharedMemoryStatusHandle statusHandle; statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitResetSimulationCommand(sm)); ASSERT_EQ(b3GetStatusType(statusHandle), CMD_RESET_SIMULATION_COMPLETED); } } } b3DisconnectSharedMemory(sm); }
void PhysicsClientExample::prepareAndSubmitCommand(int commandId) { switch (commandId) { case CMD_LOAD_URDF: { b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf"); //setting the initial position, orientation and other arguments are optional double startPosX = 0; static double startPosY = 0; double startPosZ = 0; b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ); startPosY += 2.f; // ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_REQUEST_CAMERA_IMAGE_DATA: { ///request an image from a simulated camera, using a software renderer. b3SharedMemoryCommandHandle commandHandle = b3InitRequestCameraImage(m_physicsClientHandle); float viewMatrix[16]; float projectionMatrix[16]; this->m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix); this->m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix); b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix,projectionMatrix); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_CREATE_BOX_COLLISION_SHAPE: { b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle); b3CreateBoxCommandSetStartPosition(commandHandle,0,0,-3); b3CreateBoxCommandSetColorRGBA(commandHandle,0,0,1,1); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_CREATE_RIGID_BODY: { b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle); b3CreateBoxCommandSetStartPosition(commandHandle,0,0,0); b3CreateBoxCommandSetMass(commandHandle,1); b3CreateBoxCommandSetCollisionShapeType(commandHandle,COLLISION_SHAPE_TYPE_CYLINDER_Y); b3CreateBoxCommandSetColorRGBA(commandHandle,1,1,0,1); double radius = 0.2; double halfHeight = 0.5; b3CreateBoxCommandSetHalfExtents(commandHandle,radius,halfHeight,radius); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_REQUEST_ACTUAL_STATE: { if (m_selectedBody>=0) { b3SharedMemoryCommandHandle commandHandle = b3RequestActualStateCommandInit(m_physicsClientHandle,m_selectedBody); b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_physicsClientHandle, commandHandle); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody); for (int i = 0; i < numJoints; ++i) { struct b3JointSensorState sensorState; b3GetJointState(m_physicsClientHandle, statusHandle, i, &sensorState); b3Printf("Joint %d: %f", i, sensorState.m_jointMotorTorque); } } break; }; case CMD_INIT_POSE: { if (m_selectedBody>=0) { b3SharedMemoryCommandHandle commandHandle = b3CreatePoseCommandInit(m_physicsClientHandle,m_selectedBody); static int toggle = 0; double pos[3] = {0,0,0}; pos[toggle] = 2; toggle++; if (toggle>2) toggle=0; btQuaternion orn; orn.setValue(0,0,0,1); switch (toggle) { case 0: orn = btQuaternion(btVector3(1,0,0),SIMD_HALF_PI); break; case 1: orn = btQuaternion(btVector3(0,1,0),SIMD_HALF_PI); break; case 2: orn = btQuaternion(btVector3(0,0,1),SIMD_HALF_PI); break; default: orn.setValue(0,0,0,1); }; b3CreatePoseCommandSetBaseOrientation(commandHandle,orn[0],orn[1],orn[2],orn[3]); b3CreatePoseCommandSetBasePosition(commandHandle, pos[0],pos[1],pos[2]); int numJoints = b3GetNumJoints(m_physicsClientHandle,m_selectedBody); static double jointPos = SIMD_PI/2.f; for (int i=0;i<numJoints;i++) { b3JointInfo info; b3GetJointInfo(m_physicsClientHandle, m_selectedBody,i, &info); if ((info.m_jointType == 0) || (info.m_jointType == 1)) //revolute or prismatic { b3CreatePoseCommandSetJointPosition(m_physicsClientHandle,commandHandle,i,jointPos); } } jointPos += SIMD_PI/8.0; b3SubmitClientCommand(m_physicsClientHandle, commandHandle); } break; } case CMD_STEP_FORWARD_SIMULATION: { b3SharedMemoryCommandHandle commandHandle = b3InitStepSimulationCommand(m_physicsClientHandle); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_REQUEST_DEBUG_LINES: { b3SharedMemoryCommandHandle commandHandle = b3InitRequestDebugLinesCommand(m_physicsClientHandle, btIDebugDraw::DBG_DrawWireframe); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_SEND_DESIRED_STATE: { // b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_VELOCITY); b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_POSITION_VELOCITY_PD); prepareControlCommand(command); b3SubmitClientCommand(m_physicsClientHandle, command); break; } case CMD_RESET_SIMULATION: { b3SharedMemoryCommandHandle commandHandle = b3InitResetSimulationCommand(m_physicsClientHandle); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_SEND_BULLET_DATA_STREAM: { #if 0 //this worked, but needs C-API and a streaming options, similar to debug lines command.m_type = buttonId; cl->enqueueCommand(command); #endif break; } case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: { b3SharedMemoryCommandHandle commandHandle = b3InitPhysicsParamCommand(m_physicsClientHandle); b3PhysicsParamSetGravity(commandHandle, 0.0, 0.0, -9.8); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } default: { b3Error("Unknown buttonId"); btAssert(0); } }; }
void PhysicsClientExample::prepareAndSubmitCommand(int commandId) { switch (commandId) { case CMD_LOAD_URDF: { b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf"); //setting the initial position, orientation and other arguments are optional double startPosX = 0; static double startPosY = 0; double startPosZ = 0; b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX, startPosY, startPosZ); startPosY += 2.f; // ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_LOAD_SDF: { #ifdef BT_DEBUG b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "two_cubes.sdf"); #else b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "kitchens/1.sdf"); //two_cubes.sdf");//kitchens/1.sdf");//kuka_iiwa/model.sdf"); #endif b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_REQUEST_CAMERA_IMAGE_DATA: { ///request an image from a simulated camera, using a software renderer. b3SharedMemoryCommandHandle commandHandle = b3InitRequestCameraImage(m_physicsClientHandle); //b3RequestCameraImageSelectRenderer(commandHandle,ER_BULLET_HARDWARE_OPENGL); float viewMatrix[16]; float projectionMatrix[16]; m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix); m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix); b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix, projectionMatrix); b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth, camVisualizerHeight); float lightPos[3]; lightPos[0] = m_lightPos[0]; lightPos[1] = m_lightPos[1]; lightPos[2] = m_lightPos[2]; b3RequestCameraImageSetLightDirection(commandHandle, lightPos); b3RequestCameraImageSetLightSpecularCoeff(commandHandle, m_specularCoeff); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_CREATE_BOX_COLLISION_SHAPE: { b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle); b3CreateBoxCommandSetStartPosition(commandHandle, 0, 0, -1.5); b3CreateBoxCommandSetColorRGBA(commandHandle, 0, 0, 1, 1); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_CREATE_RIGID_BODY: { b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle); b3CreateBoxCommandSetStartPosition(commandHandle, 0, 0, 0); b3CreateBoxCommandSetMass(commandHandle, 1); b3CreateBoxCommandSetCollisionShapeType(commandHandle, COLLISION_SHAPE_TYPE_CYLINDER_Y); b3CreateBoxCommandSetColorRGBA(commandHandle, 1, 1, 0, 1); double radius = 0.2; double halfHeight = 0.5; b3CreateBoxCommandSetHalfExtents(commandHandle, radius, halfHeight, radius); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_REQUEST_ACTUAL_STATE: { if (m_selectedBody >= 0) { b3SharedMemoryCommandHandle commandHandle = b3RequestActualStateCommandInit(m_physicsClientHandle, m_selectedBody); b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_physicsClientHandle, commandHandle); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody); for (int i = 0; i < numJoints; ++i) { struct b3JointSensorState sensorState; b3GetJointState(m_physicsClientHandle, statusHandle, i, &sensorState); //b3Printf("Joint %d: %f", i, sensorState.m_jointMotorTorque); } } break; }; case CMD_INIT_POSE: { if (m_selectedBody >= 0) { b3SharedMemoryCommandHandle commandHandle = b3CreatePoseCommandInit(m_physicsClientHandle, m_selectedBody); static int toggle = 0; double pos[3] = {0, 0, 0}; pos[toggle] = 2; toggle++; if (toggle > 2) toggle = 0; btQuaternion orn; orn.setValue(0, 0, 0, 1); switch (toggle) { case 0: orn = btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI); break; case 1: orn = btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI); break; case 2: orn = btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI); break; default: orn.setValue(0, 0, 0, 1); }; b3CreatePoseCommandSetBaseOrientation(commandHandle, orn[0], orn[1], orn[2], orn[3]); b3CreatePoseCommandSetBasePosition(commandHandle, pos[0], pos[1], pos[2]); int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody); static double jointPos = SIMD_PI / 2.f; for (int i = 0; i < numJoints; i++) { b3JointInfo info; b3GetJointInfo(m_physicsClientHandle, m_selectedBody, i, &info); if ((info.m_jointType == 0) || (info.m_jointType == 1)) //revolute or prismatic { b3CreatePoseCommandSetJointPosition(m_physicsClientHandle, commandHandle, i, jointPos); } } jointPos += SIMD_PI / 8.0; b3SubmitClientCommand(m_physicsClientHandle, commandHandle); } break; } case CMD_STEP_FORWARD_SIMULATION: { b3SharedMemoryCommandHandle commandHandle = b3InitStepSimulationCommand(m_physicsClientHandle); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_REQUEST_DEBUG_LINES: { b3SharedMemoryCommandHandle commandHandle = b3InitRequestDebugLinesCommand(m_physicsClientHandle, btIDebugDraw::DBG_DrawWireframe); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_SEND_DESIRED_STATE: { if (m_selectedBody >= 0) { // b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, m_selectedBody, CONTROL_MODE_VELOCITY); b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_physicsClientHandle, m_selectedBody, CONTROL_MODE_POSITION_VELOCITY_PD); // b3Printf("prepare control command for body %d", m_selectedBody); prepareControlCommand(command); b3SubmitClientCommand(m_physicsClientHandle, command); } break; } case CMD_RESET_SIMULATION: { b3SharedMemoryCommandHandle commandHandle = b3InitResetSimulationCommand(m_physicsClientHandle); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_SEND_BULLET_DATA_STREAM: { #if 0 //this worked, but needs C-API and a streaming options, similar to debug lines command.m_type = buttonId; cl->enqueueCommand(command); #endif break; } case CMD_CUSTOM_SET_GRAVITY: { b3SharedMemoryCommandHandle commandHandle = b3InitPhysicsParamCommand(m_physicsClientHandle); b3PhysicsParamSetGravity(commandHandle, 0.0, 0.0, -9.8); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_CUSTOM_SET_REALTIME_SIMULATION: { b3SharedMemoryCommandHandle commandHandle = b3InitPhysicsParamCommand(m_physicsClientHandle); b3PhysicsParamSetRealTimeSimulation(commandHandle, 1); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_CALCULATE_INVERSE_DYNAMICS: { if (m_selectedBody >= 0) { btAlignedObjectArray<double> jointPositionsQ; btAlignedObjectArray<double> jointVelocitiesQdot; btAlignedObjectArray<double> jointAccelerations; int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody); if (numJoints) { b3Printf("Compute inverse dynamics for joint accelerations:"); jointPositionsQ.resize(numJoints); jointVelocitiesQdot.resize(numJoints); jointAccelerations.resize(numJoints); for (int i = 0; i < numJoints; i++) { jointAccelerations[i] = 100; b3Printf("Desired joint acceleration[%d]=%f", i, jointAccelerations[i]); } b3SharedMemoryCommandHandle commandHandle = b3CalculateInverseDynamicsCommandInit(m_physicsClientHandle, m_selectedBody, &jointPositionsQ[0], &jointVelocitiesQdot[0], &jointAccelerations[0]); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); } } break; } case CMD_REQUEST_CONTACT_POINT_INFORMATION: { b3SharedMemoryCommandHandle commandHandle = b3InitRequestContactPointInformation(m_physicsClientHandle); b3SetContactFilterBodyA(commandHandle, 0); b3SetContactFilterBodyB(commandHandle, 1); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_SAVE_WORLD: { b3SharedMemoryCommandHandle commandHandle = b3SaveWorldCommandInit(m_physicsClientHandle, "saveWorld.py"); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_REQUEST_VISUAL_SHAPE_INFO: { if (m_selectedBody >= 0) { //request visual shape information b3SharedMemoryCommandHandle commandHandle = b3InitRequestVisualShapeInformation(m_physicsClientHandle, m_selectedBody); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); } break; } case CMD_SET_SHADOW: { b3SharedMemoryCommandHandle commandHandle = b3InitRequestCameraImage(m_physicsClientHandle); float viewMatrix[16]; float projectionMatrix[16]; m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix); m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix); b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix, projectionMatrix); b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth, camVisualizerHeight); bool hasShadow = true; b3RequestCameraImageSetShadow(commandHandle, hasShadow); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_UPDATE_VISUAL_SHAPE: { int objectUniqueId = 0; int linkIndex = -1; int shapeIndex = -1; int textureIndex = -2; double rgbaColor[4] = {0.0, 1.0, 0.0, 1.0}; b3SharedMemoryCommandHandle commandHandle = b3InitUpdateVisualShape2(m_physicsClientHandle, objectUniqueId, linkIndex, shapeIndex); b3UpdateVisualShapeRGBAColor(commandHandle, rgbaColor); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } default: { b3Error("Unknown buttonId"); btAssert(0); } }; }