b3PhysicsClientHandle b3ConnectSharedMemory(int key) { PhysicsClientSharedMemory* cl = new PhysicsClientSharedMemory(); ///client should never create shared memory, only the server does cl->setSharedMemoryKey(key); cl->connect(); return (b3PhysicsClientHandle ) cl; }
void PhysicsClientExample::initPhysics() { if (m_guiHelper && m_guiHelper->getParameterInterface()) { int upAxis = 2; m_guiHelper->setUpAxis(upAxis); createButtons(); } else { /* m_userCommandRequests.push_back(CMD_LOAD_URDF); m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE); m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE); m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE); //m_userCommandRequests.push_back(CMD_SET_JOINT_FEEDBACK); m_userCommandRequests.push_back(CMD_CREATE_BOX_COLLISION_SHAPE); //m_userCommandRequests.push_back(CMD_CREATE_RIGID_BODY); m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION); m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE); m_userCommandRequests.push_back(CMD_SHUTDOWN); */ } if (!m_physicsClient.connect()) { b3Warning("Cannot connect to physics client"); } }
virtual void renderScene() { int numLines = m_physicsClient.getNumDebugLines(); const btVector3* fromLines = m_physicsClient.getDebugLinesFrom(); const btVector3* toLines = m_physicsClient.getDebugLinesTo(); const btVector3* colorLines = m_physicsClient.getDebugLinesColor(); int lineWidth = 1; if (1) { btAlignedObjectArray<btVector3FloatData> points; points.resize(numLines*2); btAlignedObjectArray<unsigned int> indices; indices.resize(numLines*2); for (int i=0;i<numLines;i++) { points[i*2].m_floats[0] = fromLines[i].x(); points[i*2].m_floats[1] = fromLines[i].y(); points[i*2].m_floats[2] = fromLines[i].z(); points[i*2+1].m_floats[0] = toLines[i].x(); points[i*2+1].m_floats[1] = toLines[i].y(); points[i*2+1].m_floats[2] = toLines[i].z(); indices[i*2] = i*2; indices[i*2+1] = i*2+1; } float color[4] = {1,1,0,1}; if (points.size() && indices.size()) { m_guiHelper->getRenderInterface()->drawLines(&points[0].m_floats[0],color,points.size(),sizeof(btVector3FloatData),&indices[0],indices.size(),lineWidth); } } else { for (int i=0;i<numLines;i++) { m_guiHelper->getRenderInterface()->drawLine(fromLines[i],toLines[i],colorLines[i],lineWidth); } } }
void RobotControlExample::stepSimulation(float deltaTime) { m_physicsServer.processClientCommands(); if (m_physicsClient.isConnected()) { SharedMemoryStatus status; bool hasStatus = m_physicsClient.processServerStatus(status); if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED) { for (int i=0;i<m_physicsClient.getNumJoints();i++) { b3JointInfo info; m_physicsClient.getJointInfo(i,info); b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex); if (info.m_flags & JOINT_HAS_MOTORIZED_POWER) { if (m_numMotors<MAX_NUM_MOTORS) { char motorName[1024]; sprintf(motorName,"%s q'", info.m_jointName); MyMotorInfo* motorInfo = &m_motorTargetVelocities[m_numMotors]; motorInfo->m_velTarget = 0.f; motorInfo->m_uIndex = info.m_uIndex; SliderParams slider(motorName,&motorInfo->m_velTarget); slider.m_minVal=-4; slider.m_maxVal=4; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); m_numMotors++; } } } } if (m_physicsClient.canSubmitCommand()) { if (m_userCommandRequests.size()) { b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size()); SharedMemoryCommand& cmd = m_userCommandRequests[0]; //a manual 'pop_front', we don't use 'remove' because it will re-order the commands for (int i=1;i<m_userCommandRequests.size();i++) { m_userCommandRequests[i-1] = m_userCommandRequests[i]; } m_userCommandRequests.pop_back(); m_physicsClient.submitClientCommand(cmd); } } } }
void RobotControlExample::initPhysics() { ///for this testing we use Z-axis up int upAxis = 2; m_guiHelper->setUpAxis(upAxis); createEmptyDynamicsWorld(); //todo: create a special debug drawer that will cache the lines, so we can send the debug info over the wire m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); btVector3 grav(0,0,0); grav[upAxis] = 0;//-9.8; this->m_dynamicsWorld->setGravity(grav); bool allowSharedMemoryInitialization = true; m_physicsServer.connectSharedMemory(allowSharedMemoryInitialization, m_dynamicsWorld,m_guiHelper); if (m_guiHelper && m_guiHelper->getParameterInterface()) { bool isTrigger = false; createButton("Load URDF",CMD_LOAD_URDF, isTrigger); createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger); createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger); createButton("Get State",CMD_REQUEST_ACTUAL_STATE, isTrigger); createButton("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger); createButton("Create Box Collider",CMD_CREATE_BOX_COLLISION_SHAPE,isTrigger); createButton("Set Physics Params",CMD_SEND_PHYSICS_SIMULATION_PARAMETERS,isTrigger); createButton("Init Pose",CMD_INIT_POSE,isTrigger); } else { /* m_userCommandRequests.push_back(CMD_LOAD_URDF); m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE); m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE); m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE); //m_userCommandRequests.push_back(CMD_SET_JOINT_FEEDBACK); m_userCommandRequests.push_back(CMD_CREATE_BOX_COLLISION_SHAPE); //m_userCommandRequests.push_back(CMD_CREATE_RIGID_BODY); m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION); m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE); m_userCommandRequests.push_back(CMD_SHUTDOWN); */ } if (!m_physicsClient.connect()) { b3Warning("Cannot eonnect to physics client"); } }
virtual bool isConnected() { return m_physicsClient.isConnected(); }
void PhysicsClientExample::stepSimulation(float deltaTime) { if (m_physicsClient.isConnected()) { SharedMemoryStatus status; bool hasStatus = m_physicsClient.processServerStatus(status); if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED) { for (int i=0;i<m_physicsClient.getNumJoints();i++) { b3JointInfo info; m_physicsClient.getJointInfo(i,info); b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex); } if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED) { for (int i=0;i<m_physicsClient.getNumJoints();i++) { b3JointInfo info; m_physicsClient.getJointInfo(i,info); b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex); if (info.m_flags & JOINT_HAS_MOTORIZED_POWER) { if (m_numMotors<MAX_NUM_MOTORS) { char motorName[1024]; sprintf(motorName,"%s q'", info.m_jointName); MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors]; motorInfo->m_velTarget = 0.f; motorInfo->m_uIndex = info.m_uIndex; SliderParams slider(motorName,&motorInfo->m_velTarget); slider.m_minVal=-4; slider.m_maxVal=4; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); m_numMotors++; } } } } } if (hasStatus && status.m_type ==CMD_DEBUG_LINES_COMPLETED) { SharedMemoryCommand command; if (status.m_sendDebugLinesArgs.m_numRemainingDebugLines>0) { //continue requesting debug lines for drawing command.m_type =CMD_REQUEST_DEBUG_LINES; command.m_requestDebugLinesArguments.m_debugMode = btIDebugDraw::DBG_DrawWireframe;//DBG_DrawConstraints; command.m_requestDebugLinesArguments.m_startingLineIndex = status.m_sendDebugLinesArgs.m_numDebugLines+status.m_sendDebugLinesArgs.m_startingLineIndex; enqueueCommand(command); } } if (m_physicsClient.canSubmitCommand()) { if (m_userCommandRequests.size()) { //b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size()); SharedMemoryCommand command = m_userCommandRequests[0]; //a manual 'pop_front', we don't use 'remove' because it will re-order the commands for (int i=1;i<m_userCommandRequests.size();i++) { m_userCommandRequests[i-1] = m_userCommandRequests[i]; } m_userCommandRequests.pop_back(); //for the CMD_RESET_SIMULATION we need to do something special: clear the GUI sliders if (command.m_type==CMD_RESET_SIMULATION) { m_guiHelper->getParameterInterface()->removeAllParameters(); m_numMotors=0; createButtons(); } m_physicsClient.submitClientCommand(command); } else { if (m_numMotors) { SharedMemoryCommand command; command.m_type =CMD_SEND_DESIRED_STATE; prepareControlCommand(command); enqueueCommand(command); command.m_type =CMD_STEP_FORWARD_SIMULATION; enqueueCommand(command); command.m_type = CMD_REQUEST_ACTUAL_STATE; enqueueCommand(command); } } } } }
virtual void setSharedMemoryKey(int key) { m_physicsClient.setSharedMemoryKey(key); }
void RobotControlExample::stepSimulation(float deltaTime) { m_physicsServer.processClientCommands(); if (m_physicsClient.isConnected()) { SharedMemoryStatus status; bool hasStatus = m_physicsClient.processServerStatus(status); if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED) { for (int i=0;i<m_physicsClient.getNumJoints();i++) { b3JointInfo info; m_physicsClient.getJointInfo(i,info); if (m_verboseOutput) { b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex); } if (info.m_flags & JOINT_HAS_MOTORIZED_POWER) { if (m_numMotors<MAX_NUM_MOTORS) { switch (m_option) { case ROBOT_VELOCITY_CONTROL: { char motorName[1024]; sprintf(motorName,"%s q'", info.m_jointName); MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors]; motorInfo->m_velTarget = 0.f; motorInfo->m_posTarget = 0.f; motorInfo->m_uIndex = info.m_uIndex; SliderParams slider(motorName,&motorInfo->m_velTarget); slider.m_minVal=-4; slider.m_maxVal=4; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); m_numMotors++; break; } case ROBOT_PD_CONTROL: { char motorName[1024]; MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors]; motorInfo->m_velTarget = 0.f; motorInfo->m_posTarget = 0.f; motorInfo->m_uIndex = info.m_uIndex; motorInfo->m_posIndex = info.m_qIndex; motorInfo->m_kp = 1; motorInfo->m_kd = 0; { sprintf(motorName,"%s kp", info.m_jointName); SliderParams slider(motorName,&motorInfo->m_kp); slider.m_minVal=0; slider.m_maxVal=1; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { sprintf(motorName,"%s q", info.m_jointName); SliderParams slider(motorName,&motorInfo->m_posTarget); slider.m_minVal=-SIMD_PI; slider.m_maxVal=SIMD_PI; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { sprintf(motorName,"%s kd", info.m_jointName); SliderParams slider(motorName,&motorInfo->m_kd); slider.m_minVal=0; slider.m_maxVal=1; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { sprintf(motorName,"%s q'", info.m_jointName); SliderParams slider(motorName,&motorInfo->m_velTarget); slider.m_minVal=-10; slider.m_maxVal=10; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } m_numMotors++; break; } default: { b3Warning("Unknown control mode in RobotControlExample::stepSimulation"); } }; } } } } if (m_physicsClient.canSubmitCommand()) { if (m_userCommandRequests.size()) { if (m_verboseOutput) { b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size()); } SharedMemoryCommand cmd = m_userCommandRequests[0]; //a manual 'pop_front', we don't use 'remove' because it will re-order the commands for (int i=1;i<m_userCommandRequests.size();i++) { m_userCommandRequests[i-1] = m_userCommandRequests[i]; } m_userCommandRequests.pop_back(); if (cmd.m_type == CMD_SEND_BULLET_DATA_STREAM) { char relativeFileName[1024]; bool fileFound = b3ResourcePath::findResourcePath(cmd.m_dataStreamArguments.m_bulletFileName,relativeFileName,1024); if (fileFound) { FILE *fp = fopen(relativeFileName, "rb"); if (fp) { fseek(fp, 0L, SEEK_END); int mFileLen = ftell(fp); fseek(fp, 0L, SEEK_SET); if (mFileLen<SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE) { char* data = (char*)malloc(mFileLen); fread(data, mFileLen, 1, fp); fclose(fp); cmd.m_dataStreamArguments.m_streamChunkLength = mFileLen; m_physicsClient.uploadBulletFileToSharedMemory(data,mFileLen); if (m_verboseOutput) { b3Printf("Loaded bullet data chunks into shared memory\n"); } free(data); } else { b3Warning("Bullet file size (%d) exceeds of streaming memory chunk size (%d)\n", mFileLen,SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); } } else { b3Warning("Cannot open file %s\n", relativeFileName); } } else { b3Warning("Cannot find file %s\n", cmd.m_dataStreamArguments.m_bulletFileName); } } m_physicsClient.submitClientCommand(cmd); } else { if (m_numMotors) { SharedMemoryCommand command; command.m_type =CMD_SEND_DESIRED_STATE; prepareControlCommand(command); enqueueCommand(command); command.m_type =CMD_STEP_FORWARD_SIMULATION; enqueueCommand(command); } } } } }
bool RobotControlExample::isConnected() { return m_physicsClient.isConnected(); }
void RobotControlExample::stepSimulation(float deltaTime) { m_physicsServer.processClientCommands(); if (m_physicsClient.isConnected()) { SharedMemoryStatus status; bool hasStatus = m_physicsClient.processServerStatus(status); if (hasStatus && status.m_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED) { //update sensor feedback: joint force/torque data and measured joint positions for (int i=0;i<m_numMotors;i++) { int jointIndex = m_motorTargetState[i].m_jointIndex; int positionIndex = m_motorTargetState[i].m_posIndex; int velocityIndex = m_motorTargetState[i].m_uIndex; m_motorTargetState[i].m_measuredJointPosition = status.m_sendActualStateArgs.m_actualStateQ[positionIndex]; m_motorTargetState[i].m_measuredJointVelocity = status.m_sendActualStateArgs.m_actualStateQdot[velocityIndex]; m_motorTargetState[i].m_measuredJointForce.setValue(status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex], status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+1], status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+2]); m_motorTargetState[i].m_measuredJointTorque.setValue(status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+3], status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+4], status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+5]); if (m_motorTargetState[i].m_measuredJointPosition>0.1) { m_motorTargetState[i].m_velTarget = -1.5; } else { m_motorTargetState[i].m_velTarget = 1.5; } b3Printf("Joint Force (Linear) [%s]=(%f,%f,%f)\n",m_motorTargetState[i].m_jointName.c_str(),m_motorTargetState[i].m_measuredJointForce.x(),m_motorTargetState[i].m_measuredJointForce.y(),m_motorTargetState[i].m_measuredJointForce.z()); b3Printf("Joint Torque (Angular) [%s]=(%f,%f,%f)\n",m_motorTargetState[i].m_jointName.c_str(),m_motorTargetState[i].m_measuredJointTorque.x(),m_motorTargetState[i].m_measuredJointTorque.y(),m_motorTargetState[i].m_measuredJointTorque.z()); } } if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED) { SharedMemoryCommand sensorCommand; sensorCommand.m_type = CMD_CREATE_SENSOR; sensorCommand.m_createSensorArguments.m_numJointSensorChanges = 0; for (int jointIndex=0;jointIndex<m_physicsClient.getNumJoints();jointIndex++) { b3JointInfo info; m_physicsClient.getJointInfo(jointIndex,info); if (m_verboseOutput) { b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex); } if (info.m_flags & JOINT_HAS_MOTORIZED_POWER) { if (m_numMotors<MAX_NUM_MOTORS) { switch (m_option) { case ROBOT_VELOCITY_CONTROL: { char motorName[1024]; sprintf(motorName,"%s q'", info.m_jointName); MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors]; motorInfo->m_jointName = info.m_jointName; motorInfo->m_velTarget = 0.f; motorInfo->m_posTarget = 0.f; motorInfo->m_uIndex = info.m_uIndex; SliderParams slider(motorName,&motorInfo->m_velTarget); slider.m_minVal=-4; slider.m_maxVal=4; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); m_numMotors++; break; } case ROBOT_PD_CONTROL: { char motorName[1024]; MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors]; motorInfo->m_jointName = info.m_jointName; motorInfo->m_velTarget = 0.f; motorInfo->m_posTarget = 0.f; motorInfo->m_uIndex = info.m_uIndex; motorInfo->m_posIndex = info.m_qIndex; motorInfo->m_kp = 1; motorInfo->m_kd = 0; { sprintf(motorName,"%s kp", info.m_jointName); SliderParams slider(motorName,&motorInfo->m_kp); slider.m_minVal=0; slider.m_maxVal=1; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { sprintf(motorName,"%s q", info.m_jointName); SliderParams slider(motorName,&motorInfo->m_posTarget); slider.m_minVal=-SIMD_PI; slider.m_maxVal=SIMD_PI; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { sprintf(motorName,"%s kd", info.m_jointName); SliderParams slider(motorName,&motorInfo->m_kd); slider.m_minVal=0; slider.m_maxVal=1; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { sprintf(motorName,"%s q'", info.m_jointName); SliderParams slider(motorName,&motorInfo->m_velTarget); slider.m_minVal=-10; slider.m_maxVal=10; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } m_numMotors++; break; } case ROBOT_PING_PONG_JOINT_FEEDBACK: { if (info.m_flags & JOINT_HAS_MOTORIZED_POWER) { if (m_numMotors<MAX_NUM_MOTORS) { MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors]; motorInfo->m_jointName = info.m_jointName; motorInfo->m_velTarget = 0.f; motorInfo->m_posTarget = 0.f; motorInfo->m_uIndex = info.m_uIndex; motorInfo->m_posIndex = info.m_qIndex; motorInfo->m_jointIndex = jointIndex; sensorCommand.m_createSensorArguments.m_jointIndex[sensorCommand.m_createSensorArguments.m_numJointSensorChanges] = jointIndex; sensorCommand.m_createSensorArguments.m_enableJointForceSensor[sensorCommand.m_createSensorArguments.m_numJointSensorChanges] = true; sensorCommand.m_createSensorArguments.m_numJointSensorChanges++; m_numMotors++; } } break; } default: { b3Warning("Unknown control mode in RobotControlExample::stepSimulation"); } }; } } } if (sensorCommand.m_createSensorArguments.m_numJointSensorChanges) { enqueueCommand(sensorCommand); } } if (m_physicsClient.canSubmitCommand()) { if (m_userCommandRequests.size()) { if (m_verboseOutput) { b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size()); } SharedMemoryCommand cmd = m_userCommandRequests[0]; //a manual 'pop_front', we don't use 'remove' because it will re-order the commands for (int i=1;i<m_userCommandRequests.size();i++) { m_userCommandRequests[i-1] = m_userCommandRequests[i]; } m_userCommandRequests.pop_back(); if (cmd.m_type == CMD_CREATE_SENSOR) { b3Printf("CMD_CREATE_SENSOR!\n"); } if (cmd.m_type == CMD_SEND_BULLET_DATA_STREAM) { char relativeFileName[1024]; bool fileFound = b3ResourcePath::findResourcePath(cmd.m_dataStreamArguments.m_bulletFileName,relativeFileName,1024); if (fileFound) { FILE *fp = fopen(relativeFileName, "rb"); if (fp) { fseek(fp, 0L, SEEK_END); int mFileLen = ftell(fp); fseek(fp, 0L, SEEK_SET); if (mFileLen<SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE) { char* data = (char*)malloc(mFileLen); fread(data, mFileLen, 1, fp); fclose(fp); cmd.m_dataStreamArguments.m_streamChunkLength = mFileLen; m_physicsClient.uploadBulletFileToSharedMemory(data,mFileLen); if (m_verboseOutput) { b3Printf("Loaded bullet data chunks into shared memory\n"); } free(data); } else { b3Warning("Bullet file size (%d) exceeds of streaming memory chunk size (%d)\n", mFileLen,SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); } } else { b3Warning("Cannot open file %s\n", relativeFileName); } } else { b3Warning("Cannot find file %s\n", cmd.m_dataStreamArguments.m_bulletFileName); } } m_physicsClient.submitClientCommand(cmd); } else { if (m_numMotors) { SharedMemoryCommand command; command.m_type =CMD_SEND_DESIRED_STATE; prepareControlCommand(command); enqueueCommand(command); command.m_type =CMD_STEP_FORWARD_SIMULATION; enqueueCommand(command); command.m_type = CMD_REQUEST_ACTUAL_STATE; enqueueCommand(command); } } } } }