int b3ResourcePath::getExePath(char* path, int maxPathLenInBytes) { int numBytes = 0; #if __APPLE__ uint32_t bufsize = uint32_t(maxPathLenInBytes); if (_NSGetExecutablePath(path, &bufsize)!=0) { b3Warning("Cannot find executable path\n"); return false; } else { numBytes = strlen(path); } #else #ifdef _WIN32 //https://msdn.microsoft.com/en-us/library/windows/desktop/ms683197(v=vs.85).aspx HMODULE hModule = GetModuleHandle(NULL); numBytes = GetModuleFileName(hModule, path, maxPathLenInBytes); #else ///http://stackoverflow.com/questions/933850/how-to-find-the-location-of-the-executable-in-c numBytes = (int)readlink("/proc/self/exe", path, maxPathLenInBytes-1); if (numBytes > 0) { path[numBytes] = 0; } else { b3Warning("Cannot find executable path\n"); } #endif //_WIN32 #endif //__APPLE__ return numBytes; }
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"); } }
void PhysicsClientExample::initPhysics() { if (m_guiHelper && m_guiHelper->getParameterInterface()) { int upAxis = 2; m_guiHelper->setUpAxis(upAxis); createButtons(); } else { MyCallback(CMD_LOAD_URDF, true, this); MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this); MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this); MyCallback(CMD_RESET_SIMULATION,true,this); } m_selectedBody = -1; m_prevSelectedBody = -1; m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey); //m_physicsClientHandle = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY); //m_physicsClientHandle = b3ConnectPhysicsDirect(); if (!b3CanSubmitCommand(m_physicsClientHandle)) { b3Warning("Cannot connect to physics client"); } }
void RobotControlExample::prepareControlCommand(SharedMemoryCommand& command) { for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++) { command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 0; command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 0; } switch (m_option) { case ROBOT_VELOCITY_CONTROL: { command.m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_VELOCITY; for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++) { command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 0; command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 1000; } for (int i=0;i<m_numMotors;i++) { btScalar targetVel = m_motorTargetState[i].m_velTarget; int uIndex = m_motorTargetState[i].m_uIndex; command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel; } break; } case ROBOT_PD_CONTROL: { command.m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_POSITION_VELOCITY_PD; for (int i=0;i<m_numMotors;i++) { int uIndex = m_motorTargetState[i].m_uIndex; command.m_sendDesiredStateCommandArgument.m_Kp[uIndex] = m_motorTargetState[i].m_kp; command.m_sendDesiredStateCommandArgument.m_Kd[uIndex] = m_motorTargetState[i].m_kd; command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[uIndex] = 10000;//max force btScalar targetVel = m_motorTargetState[i].m_velTarget; command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel; int posIndex = m_motorTargetState[i].m_posIndex; btScalar targetPos = m_motorTargetState[i].m_posTarget; command.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex] = targetPos; } break; } default: { b3Warning("Unknown control mode in RobotControlExample::prepareControlCommand"); } }; }
bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface* guiHelper) { m_data->m_commandProcessor->setGuiHelper(guiHelper); bool allowCreation = true; if (m_data->m_isConnected) { b3Warning("connectSharedMemory, while already connected"); return m_data->m_isConnected; } int counter = 0; do { m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE,allowCreation); if (m_data->m_testBlock1) { int magicId =m_data->m_testBlock1->m_magicId; if (m_data->m_verboseOutput) { b3Printf("magicId = %d\n", magicId); } if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER) { InitSharedMemoryBlock(m_data->m_testBlock1); if (m_data->m_verboseOutput) { b3Printf("Created and initialized shared memory block\n"); } m_data->m_isConnected = true; } else { m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE); m_data->m_testBlock1 = 0; m_data->m_isConnected = false; } } else { b3Error("Cannot connect to shared memory"); m_data->m_isConnected = false; } } while (counter++ < 10 && !m_data->m_isConnected); if (!m_data->m_isConnected) { b3Error("Server cannot connect to shared memory.\n"); } return m_data->m_isConnected; }
void LoadMeshFromColladaAssimp(const char* relativeFileName, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances,btTransform& upAxisTrans, float& unitMeterScaling) { upAxisTrans.setIdentity(); unitMeterScaling=1; GLInstanceGraphicsShape* shape = 0; FILE* file = fopen(relativeFileName,"rb"); if (file) { int size=0; if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET)) { b3Warning("Error: Cannot access file to determine size of %s\n", relativeFileName); } else { if (size) { //printf("Open DAE file of %d bytes\n",size); Assimp::Importer importer; //importer.SetPropertyInteger(AI_CONFIG_PP_RVC_FLAGS, aiComponent_NORMALS | aiComponent_COLORS); importer.SetPropertyInteger(AI_CONFIG_PP_SBP_REMOVE, aiPrimitiveType_LINE | aiPrimitiveType_POINT); // importer.SetPropertyInteger(AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION, 1); aiScene const* scene = importer.ReadFile(relativeFileName, aiProcess_JoinIdenticalVertices | //aiProcess_RemoveComponent | aiProcess_SortByPType | aiProcess_Triangulate); if (scene) { shape = &visualShapes.expand(); shape->m_scaling[0] = 1; shape->m_scaling[1] = 1; shape->m_scaling[2] = 1; shape->m_scaling[3] = 1; int index = 0; shape->m_indices = new b3AlignedObjectArray<int>(); shape->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>(); aiMatrix4x4 ident; addMeshParts(scene, scene->mRootNode, shape, ident); shape->m_numIndices = shape->m_indices->size(); shape->m_numvertices = shape->m_vertices->size(); ColladaGraphicsInstance& instance = visualShapeInstances.expand(); instance.m_shapeIndex = visualShapes.size()-1; } } } } }
void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data, int len) { btAssert(len < SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); if (len >= SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE) { b3Warning("uploadBulletFileToSharedMemory %d exceeds max size %d\n", len, SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); } else { for (int i = 0; i < len; i++) { m_data->m_testBlock1->m_bulletStreamDataClientToServer[i] = data[i]; } } }
void b3GpuNarrowPhase::setObjectVelocityCpu(float* linVel, float* angVel, int bodyIndex) { if (bodyIndex>=0 && bodyIndex<m_data->m_bodyBufferCPU->size()) { m_data->m_bodyBufferCPU->at(bodyIndex).m_linVel.setValue(linVel[0],linVel[1],linVel[2]); m_data->m_bodyBufferCPU->at(bodyIndex).m_angVel.setValue(angVel[0],angVel[1],angVel[2]); } else { b3Warning("setObjectVelocityCpu out of range.\n"); } }
void b3GpuNarrowPhase::setObjectTransformCpu(float* position, float* orientation , int bodyIndex) { if (bodyIndex>=0 && bodyIndex<m_data->m_bodyBufferCPU->size()) { m_data->m_bodyBufferCPU->at(bodyIndex).m_pos.setValue(position[0],position[1],position[2]); m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.setValue(orientation[0],orientation[1],orientation[2],orientation[3]); } else { b3Warning("setObjectVelocityCpu out of range.\n"); } }
bool PhysicsClientSharedMemory::connect() { /// server always has to create and initialize shared memory bool allowCreation = false; m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory( m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE, allowCreation); if (m_data->m_testBlock1) { if (m_data->m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER) { b3Error("Error: please start server before client\n"); m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE); m_data->m_testBlock1 = 0; return false; } else { if (m_data->m_verboseOutput) { b3Printf("Connected to existing shared memory, status OK.\n"); } m_data->m_isConnected = true; } } else { b3Warning("Cannot connect to shared memory"); return false; } #if 0 if (m_data->m_isConnected) { //get all existing bodies and body info... SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; //now transfer the information of the individual objects etc. command.m_type = CMD_REQUEST_BODY_INFO; command.m_sdfRequestInfoArgs.m_bodyUniqueId = 37; submitClientCommand(command); int timeout = 1024 * 1024 * 1024; const SharedMemoryStatus* status = 0; while ((status == 0) && (timeout-- > 0)) { status = processServerStatus(); } //submitClientCommand(command); } #endif return true; }
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"); } }
bool b3GpuNarrowPhase::getObjectTransformFromCpu(float* position, float* orientation , int bodyIndex) const { if (bodyIndex>=0 && bodyIndex<m_data->m_bodyBufferCPU->size()) { position[0] = m_data->m_bodyBufferCPU->at(bodyIndex).m_pos.x; position[1] = m_data->m_bodyBufferCPU->at(bodyIndex).m_pos.y; position[2] = m_data->m_bodyBufferCPU->at(bodyIndex).m_pos.z; position[3] = 1.f;//or 1 orientation[0] = m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.x; orientation[1] = m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.y; orientation[2] = m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.z; orientation[3] = m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.w; return true; } b3Warning("getObjectTransformFromCpu out of range.\n"); return false; }
///todo(erwincoumans) refactor this: merge with PhysicsDirect::processBodyJointInfo void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd) { bParse::btBulletFile bf( &this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0], serverCmd.m_dataStreamArguments.m_streamChunkLength); bf.setFileDNAisMemoryDNA(); bf.parse(false); BodyJointInfoCache* bodyJoints = new BodyJointInfoCache; m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints); for (int i = 0; i < bf.m_multiBodies.size(); i++) { int flag = bf.getFlags(); if ((flag & bParse::FD_DOUBLE_PRECISION) != 0) { Bullet::btMultiBodyDoubleData* mb = (Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i]; bodyJoints->m_baseName = mb->m_baseName; addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); } else { Bullet::btMultiBodyFloatData* mb = (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i]; bodyJoints->m_baseName = mb->m_baseName; addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); } } if (bf.ok()) { if (m_data->m_verboseOutput) { b3Printf("Received robot description ok!\n"); } } else { b3Warning("Robot description not received"); } }
bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface* guiHelper) { m_data->m_guiHelper = guiHelper; bool allowCreation = true; bool allowConnectToExistingSharedMemory = false; if (m_data->m_isConnected) { b3Warning("connectSharedMemory, while already connected"); return m_data->m_isConnected; } m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE,allowCreation); if (m_data->m_testBlock1) { int magicId =m_data->m_testBlock1->m_magicId; b3Printf("magicId = %d\n", magicId); if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER) { InitSharedMemoryBlock(m_data->m_testBlock1); b3Printf("Created and initialized shared memory block\n"); m_data->m_isConnected = true; } else { b3Error("Server cannot connect to existing shared memory, disconnecting shared memory.\n"); m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE); m_data->m_testBlock1 = 0; m_data->m_isConnected = false; } } else { b3Error("Cannot connect to shared memory"); m_data->m_isConnected = false; } return m_data->m_isConnected; }
void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, btTransform& upAxisTransform, float& unitMeterScaling,int clientUpAxis) { // GLInstanceGraphicsShape* instance = 0; //usually COLLADA files don't have that many visual geometries/shapes visualShapes.reserve(MAX_VISUAL_SHAPES); float extraScaling = 1;//0.01; btHashMap<btHashString, int> name2ShapeIndex; b3FileUtils f; char filename[1024]; if (!f.findFile(relativeFileName,filename,1024)) { b3Warning("File not found: %s\n", filename); return; } TiXmlDocument doc(filename); if (!doc.LoadFile()) return; //We need units to be in meter, so apply a scaling using the asset/units meter unitMeterScaling=1; upAxisTransform.setIdentity(); //Also we can optionally compensate all transforms using the asset/up_axis as well as unit meter scaling getUnitMeterScalingAndUpAxisTransform(doc, upAxisTransform, unitMeterScaling,clientUpAxis); btMatrix4x4 ident; ident.setIdentity(); readLibraryGeometries(doc, visualShapes, name2ShapeIndex, extraScaling); readVisualSceneInstanceGeometries(doc, name2ShapeIndex, visualShapeInstances); }
void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut) { GLInstanceGraphicsShape* glmesh = 0; btConvexShape* convexColShape = 0; switch (visual->m_geometry.m_type) { case URDF_GEOM_CYLINDER: { btAlignedObjectArray<btVector3> vertices; //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); int numSteps = 32; for (int i = 0; i<numSteps; i++) { btScalar cylRadius = visual->m_geometry.m_cylinderRadius; btScalar cylLength = visual->m_geometry.m_cylinderLength; btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.); vertices.push_back(vert); vert[2] = -cylLength / 2.; vertices.push_back(vert); } btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); cylZShape->setMargin(0.001); convexColShape = cylZShape; break; } case URDF_GEOM_BOX: { btVector3 extents = visual->m_geometry.m_boxSize; btBoxShape* boxShape = new btBoxShape(extents*0.5f); //btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5); convexColShape = boxShape; convexColShape->setMargin(0.001); break; } case URDF_GEOM_SPHERE: { btScalar radius = visual->m_geometry.m_sphereRadius; btSphereShape* sphereShape = new btSphereShape(radius); convexColShape = sphereShape; convexColShape->setMargin(0.001); break; break; } case URDF_GEOM_MESH: { if (visual->m_name.length()) { //b3Printf("visual->name=%s\n", visual->m_name.c_str()); } if (1)//visual->m_geometry) { if (visual->m_geometry.m_meshFileName.length()) { const char* filename = visual->m_geometry.m_meshFileName.c_str(); //b3Printf("mesh->filename=%s\n", filename); char fullPath[1024]; int fileType = 0; char tmpPathPrefix[1024]; std::string xml_string; int maxPathLen = 1024; b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen); char visualPathPrefix[1024]; sprintf(visualPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix); sprintf(fullPath, "%s%s", urdfPathPrefix, filename); b3FileUtils::toLower(fullPath); if (strstr(fullPath, ".dae")) { fileType = MY_FILE_COLLADA; } if (strstr(fullPath, ".stl")) { fileType = MY_FILE_STL; } if (strstr(fullPath,".obj")) { fileType = MY_FILE_OBJ; } sprintf(fullPath, "%s%s", urdfPathPrefix, filename); FILE* f = fopen(fullPath, "rb"); if (f) { fclose(f); switch (fileType) { case MY_FILE_OBJ: { //glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); b3ImportMeshData meshData; if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fullPath, meshData)) { if (meshData.m_textureImage) { MyTexture2 texData; texData.m_width = meshData.m_textureWidth; texData.m_height = meshData.m_textureHeight; texData.textureData = meshData.m_textureImage; texturesOut.push_back(texData); } glmesh = meshData.m_gfxShape; } break; } case MY_FILE_STL: { glmesh = LoadMeshFromSTL(fullPath); break; } case MY_FILE_COLLADA: { btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes; btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances; btTransform upAxisTrans; upAxisTrans.setIdentity(); float unitMeterScaling = 1; int upAxis = 2; LoadMeshFromCollada(fullPath, visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, upAxis); glmesh = new GLInstanceGraphicsShape; // int index = 0; glmesh->m_indices = new b3AlignedObjectArray<int>(); glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>(); for (int i = 0; i<visualShapeInstances.size(); i++) { ColladaGraphicsInstance* instance = &visualShapeInstances[i]; GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex]; b3AlignedObjectArray<GLInstanceVertex> verts; verts.resize(gfxShape->m_vertices->size()); int baseIndex = glmesh->m_vertices->size(); for (int i = 0; i<gfxShape->m_vertices->size(); i++) { verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0]; verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1]; verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2]; verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0]; verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1]; verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0]; verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1]; verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2]; verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3]; } int curNumIndices = glmesh->m_indices->size(); int additionalIndices = gfxShape->m_indices->size(); glmesh->m_indices->resize(curNumIndices + additionalIndices); for (int k = 0; k<additionalIndices; k++) { glmesh->m_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex; } //compensate upAxisTrans and unitMeterScaling here btMatrix4x4 upAxisMat; upAxisMat.setIdentity(); // upAxisMat.setPureRotation(upAxisTrans.getRotation()); btMatrix4x4 unitMeterScalingMat; unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling)); btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform; //btMatrix4x4 worldMat = instance->m_worldTransform; int curNumVertices = glmesh->m_vertices->size(); int additionalVertices = verts.size(); glmesh->m_vertices->reserve(curNumVertices + additionalVertices); for (int v = 0; v<verts.size(); v++) { btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]); pos = worldMat*pos; verts[v].xyzw[0] = float(pos[0]); verts[v].xyzw[1] = float(pos[1]); verts[v].xyzw[2] = float(pos[2]); glmesh->m_vertices->push_back(verts[v]); } } glmesh->m_numIndices = glmesh->m_indices->size(); glmesh->m_numvertices = glmesh->m_vertices->size(); //glmesh = LoadMeshFromCollada(fullPath); break; } default: { b3Warning("Error: unsupported file type for Visual mesh: %s\n", fullPath); btAssert(0); } } if (glmesh && glmesh->m_vertices && (glmesh->m_numvertices>0)) { //apply the geometry scaling for (int i=0;i<glmesh->m_vertices->size();i++) { glmesh->m_vertices->at(i).xyzw[0] *= visual->m_geometry.m_meshScale[0]; glmesh->m_vertices->at(i).xyzw[1] *= visual->m_geometry.m_meshScale[1]; glmesh->m_vertices->at(i).xyzw[2] *= visual->m_geometry.m_meshScale[2]; } } else { b3Warning("issue extracting mesh from COLLADA/STL file %s\n", fullPath); } } else { b3Warning("mesh geometry not found %s\n", fullPath); } } } break; } default: { b3Warning("Error: unknown visual geometry type\n"); } } //if we have a convex, tesselate into localVertices/localIndices if ((glmesh==0) && convexColShape) { btShapeHull* hull = new btShapeHull(convexColShape); hull->buildHull(0.0); { // int strideInBytes = 9*sizeof(float); int numVertices = hull->numVertices(); int numIndices = hull->numIndices(); glmesh = new GLInstanceGraphicsShape; // int index = 0; glmesh->m_indices = new b3AlignedObjectArray<int>(); glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>(); for (int i = 0; i < numVertices; i++) { GLInstanceVertex vtx; btVector3 pos = hull->getVertexPointer()[i]; vtx.xyzw[0] = pos.x(); vtx.xyzw[1] = pos.y(); vtx.xyzw[2] = pos.z(); vtx.xyzw[3] = 1.f; pos.normalize(); vtx.normal[0] = pos.x(); vtx.normal[1] = pos.y(); vtx.normal[2] = pos.z(); vtx.uv[0] = 0.5f; vtx.uv[1] = 0.5f; glmesh->m_vertices->push_back(vtx); } btAlignedObjectArray<int> indices; for (int i = 0; i < numIndices; i++) { glmesh->m_indices->push_back(hull->getIndexPointer()[i]); } glmesh->m_numvertices = glmesh->m_vertices->size(); glmesh->m_numIndices = glmesh->m_indices->size(); } delete hull; delete convexColShape; convexColShape = 0; } if (glmesh && glmesh->m_numIndices>0 && glmesh->m_numvertices >0) { int baseIndex = verticesOut.size(); for (int i = 0; i < glmesh->m_indices->size(); i++) { indicesOut.push_back(glmesh->m_indices->at(i) + baseIndex); } for (int i = 0; i < glmesh->m_vertices->size(); i++) { GLInstanceVertex& v = glmesh->m_vertices->at(i); btVector3 vert(v.xyzw[0],v.xyzw[1],v.xyzw[2]); btVector3 vt = visualTransform*vert; v.xyzw[0] = vt[0]; v.xyzw[1] = vt[1]; v.xyzw[2] = vt[2]; btVector3 triNormal(v.normal[0],v.normal[1],v.normal[2]); triNormal = visualTransform.getBasis()*triNormal; v.normal[0] = triNormal[0]; v.normal[1] = triNormal[1]; v.normal[2] = triNormal[2]; verticesOut.push_back(v); } } delete glmesh; }
SimpleOpenGL3App::SimpleOpenGL3App( const char* title, int width,int height) { gApp = this; m_data = new SimpleInternalData; m_data->m_frameDumpPngFileName = 0; m_data->m_renderTexture = 0; m_data->m_ffmpegFile = 0; m_data->m_userPointer = 0; m_window = new b3gDefaultOpenGLWindow(); b3gWindowConstructionInfo ci; ci.m_title = title; ci.m_width = width; ci.m_height = height; m_window->createWindow(ci); m_window->setWindowTitle(title); b3Assert(glGetError() ==GL_NO_ERROR); glClearColor(0.9,0.9,1,1); m_window->startRendering(); b3Assert(glGetError() ==GL_NO_ERROR); #ifndef __APPLE__ #ifndef _WIN32 //some Linux implementations need the 'glewExperimental' to be true glewExperimental = GL_TRUE; #endif if (glewInit() != GLEW_OK) exit(1); // or handle the error in a nicer way if (!GLEW_VERSION_2_1) // check that the machine supports the 2.1 API. exit(1); // or handle the error in a nicer way #endif glGetError();//don't remove this call, it is needed for Ubuntu b3Assert(glGetError() ==GL_NO_ERROR); m_primRenderer = new GLPrimitiveRenderer(width,height); m_parameterInterface = 0; b3Assert(glGetError() ==GL_NO_ERROR); m_instancingRenderer = new GLInstancingRenderer(128*1024,4*1024*1024); m_instancingRenderer->init(); m_instancingRenderer->resize(width,height); b3Assert(glGetError() ==GL_NO_ERROR); m_instancingRenderer->InitShaders(); m_window->setMouseMoveCallback(b3DefaultMouseMoveCallback); m_window->setMouseButtonCallback(b3DefaultMouseButtonCallback); m_window->setKeyboardCallback(b3DefaultKeyboardCallback); m_window->setWheelCallback(b3DefaultWheelCallback); m_window->setResizeCallback(SimpleResizeCallback); TwGenerateDefaultFonts(); m_data->m_fontTextureId = BindFont(g_DefaultNormalFont); { m_data->m_renderCallbacks = new OpenGL2RenderCallbacks(m_primRenderer); m_data->m_fontStash = sth_create(512,512,m_data->m_renderCallbacks);//256,256);//,1024);//512,512); b3Assert(glGetError() ==GL_NO_ERROR); if (!m_data->m_fontStash) { b3Warning("Could not create stash"); //fprintf(stderr, "Could not create stash.\n"); } char* data2 = OpenSansData; unsigned char* data = (unsigned char*) data2; if (!(m_data->m_droidRegular = sth_add_font_from_memory(m_data->m_fontStash, data))) { b3Warning("error!\n"); } b3Assert(glGetError() ==GL_NO_ERROR); } }
virtual void reportWarning(const char* warning) { m_numWarnings++; b3Warning(warning); }
btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix) { btCollisionShape* shape = 0; switch (collision->m_geometry.m_type) { case URDF_GEOM_CYLINDER: { btScalar cylRadius = collision->m_geometry.m_cylinderRadius; btScalar cylLength = collision->m_geometry.m_cylinderLength; btAlignedObjectArray<btVector3> vertices; //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); int numSteps = 32; for (int i=0;i<numSteps;i++) { btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i)/numSteps)),cylRadius*btCos(SIMD_2_PI*(float(i)/numSteps)),cylLength/2.); vertices.push_back(vert); vert[2] = -cylLength/2.; vertices.push_back(vert); } btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); cylZShape->setMargin(0.001); cylZShape->initializePolyhedralFeatures(); //btConvexShape* cylZShape = new btConeShapeZ(cyl->radius,cyl->length);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.); //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents); shape = cylZShape; break; } case URDF_GEOM_BOX: { btVector3 extents = collision->m_geometry.m_boxSize; btBoxShape* boxShape = new btBoxShape(extents*0.5f); //btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5); shape = boxShape; shape ->setMargin(0.001); break; } case URDF_GEOM_SPHERE: { btScalar radius = collision->m_geometry.m_sphereRadius; btSphereShape* sphereShape = new btSphereShape(radius); shape = sphereShape; shape ->setMargin(0.001); break; break; } case URDF_GEOM_MESH: { if (collision->m_name.length()) { //b3Printf("collision->name=%s\n",collision->m_name.c_str()); } if (1) { if (collision->m_geometry.m_meshFileName.length()) { const char* filename = collision->m_geometry.m_meshFileName.c_str(); //b3Printf("mesh->filename=%s\n",filename); char fullPath[1024]; int fileType = 0; sprintf(fullPath,"%s%s",urdfPathPrefix,filename); b3FileUtils::toLower(fullPath); char tmpPathPrefix[1024]; int maxPathLen = 1024; b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen); char collisionPathPrefix[1024]; sprintf(collisionPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix); if (strstr(fullPath,".dae")) { fileType = FILE_COLLADA; } if (strstr(fullPath,".stl")) { fileType = FILE_STL; } if (strstr(fullPath,".obj")) { fileType = FILE_OBJ; } sprintf(fullPath,"%s%s",urdfPathPrefix,filename); FILE* f = fopen(fullPath,"rb"); if (f) { fclose(f); GLInstanceGraphicsShape* glmesh = 0; switch (fileType) { case FILE_OBJ: { glmesh = LoadMeshFromObj(fullPath,collisionPathPrefix); break; } case FILE_STL: { glmesh = LoadMeshFromSTL(fullPath); break; } case FILE_COLLADA: { btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes; btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances; btTransform upAxisTrans;upAxisTrans.setIdentity(); float unitMeterScaling=1; int upAxis = 2; LoadMeshFromCollada(fullPath, visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, upAxis ); glmesh = new GLInstanceGraphicsShape; // int index = 0; glmesh->m_indices = new b3AlignedObjectArray<int>(); glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>(); for (int i=0;i<visualShapeInstances.size();i++) { ColladaGraphicsInstance* instance = &visualShapeInstances[i]; GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex]; b3AlignedObjectArray<GLInstanceVertex> verts; verts.resize(gfxShape->m_vertices->size()); int baseIndex = glmesh->m_vertices->size(); for (int i=0;i<gfxShape->m_vertices->size();i++) { verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0]; verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1]; verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2]; verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0]; verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1]; verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0]; verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1]; verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2]; verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3]; } int curNumIndices = glmesh->m_indices->size(); int additionalIndices = gfxShape->m_indices->size(); glmesh->m_indices->resize(curNumIndices+additionalIndices); for (int k=0;k<additionalIndices;k++) { glmesh->m_indices->at(curNumIndices+k)=gfxShape->m_indices->at(k)+baseIndex; } //compensate upAxisTrans and unitMeterScaling here btMatrix4x4 upAxisMat; upAxisMat.setIdentity(); //upAxisMat.setPureRotation(upAxisTrans.getRotation()); btMatrix4x4 unitMeterScalingMat; unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling,unitMeterScaling,unitMeterScaling)); btMatrix4x4 worldMat = unitMeterScalingMat*instance->m_worldTransform*upAxisMat; //btMatrix4x4 worldMat = instance->m_worldTransform; int curNumVertices = glmesh->m_vertices->size(); int additionalVertices = verts.size(); glmesh->m_vertices->reserve(curNumVertices+additionalVertices); for(int v=0;v<verts.size();v++) { btVector3 pos(verts[v].xyzw[0],verts[v].xyzw[1],verts[v].xyzw[2]); pos = worldMat*pos; verts[v].xyzw[0] = float(pos[0]); verts[v].xyzw[1] = float(pos[1]); verts[v].xyzw[2] = float(pos[2]); glmesh->m_vertices->push_back(verts[v]); } } glmesh->m_numIndices = glmesh->m_indices->size(); glmesh->m_numvertices = glmesh->m_vertices->size(); //glmesh = LoadMeshFromCollada(fullPath); break; } default: { b3Warning("Unsupported file type in Collision: %s\n",fullPath); btAssert(0); } } if (glmesh && (glmesh->m_numvertices>0)) { //b3Printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath); //int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size()); //convex->setUserIndex(shapeId); btAlignedObjectArray<btVector3> convertedVerts; convertedVerts.reserve(glmesh->m_numvertices); for (int i=0;i<glmesh->m_numvertices;i++) { convertedVerts.push_back(btVector3(glmesh->m_vertices->at(i).xyzw[0],glmesh->m_vertices->at(i).xyzw[1],glmesh->m_vertices->at(i).xyzw[2])); } //btConvexHullShape* cylZShape = new btConvexHullShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, sizeof(GLInstanceVertex)); btConvexHullShape* cylZShape = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3)); //cylZShape->initializePolyhedralFeatures(); //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.); //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents); cylZShape->setMargin(0.001); shape = cylZShape; } else { b3Warning("issue extracting mesh from STL file %s\n", fullPath); } delete glmesh; } else { b3Warning("mesh geometry not found %s\n",fullPath); } } } break; } default: { b3Warning("Error: unknown visual geometry type\n"); } } return shape; }
void MotionThreadFunc(void* userPtr,void* lsMemory) { printf("MotionThreadFunc thread started\n"); MotionThreadLocalStorage* localStorage = (MotionThreadLocalStorage*) lsMemory; MotionArgs* args = (MotionArgs*) userPtr; int workLeft = true; b3Clock clock; clock.reset(); bool init = true; if (init) { args->m_cs->lock(); args->m_cs->setSharedParam(0,eMotionIsInitialized); args->m_cs->unlock(); double deltaTimeInSeconds = 0; do { deltaTimeInSeconds+= double(clock.getTimeMicroseconds())/1000000.; if (deltaTimeInSeconds<(1./5000.)) { skip++; skip1++; if (skip1>5) { b3Clock::usleep(250); } } else { skip1=0; //process special controller commands, such as //VR controller button press/release and controller motion for (int c=0;c<MAX_VR_CONTROLLERS;c++) { btVector3 from = args->m_vrControllerPos[c]; btMatrix3x3 mat(args->m_vrControllerOrn[c]); btScalar pickDistance = 1000.; btVector3 toX = from+mat.getColumn(0); btVector3 toY = from+mat.getColumn(1); btVector3 toZ = from+mat.getColumn(2)*pickDistance; if (args->m_isVrControllerTeleporting[c]) { args->m_isVrControllerTeleporting[c] = false; args->m_physicsServerPtr->pickBody(from,-toZ); args->m_physicsServerPtr->removePickingConstraint(); } if (!gCloseToKuka) { if (args->m_isVrControllerPicking[c]) { args->m_isVrControllerPicking[c] = false; args->m_isVrControllerDragging[c] = true; args->m_physicsServerPtr->pickBody(from,-toZ); //printf("PICK!\n"); } } if (args->m_isVrControllerDragging[c]) { args->m_physicsServerPtr->movePickedBody(from,-toZ); // printf("."); } if (args->m_isVrControllerReleasing[c]) { args->m_isVrControllerDragging[c] = false; args->m_isVrControllerReleasing[c] = false; args->m_physicsServerPtr->removePickingConstraint(); //printf("Release pick\n"); } } //don't simulate over a huge timestep if we had some interruption (debugger breakpoint etc) if (deltaTimeInSeconds>clampedDeltaTime) { deltaTimeInSeconds = clampedDeltaTime; b3Warning("Clamp deltaTime from %f to %f",deltaTimeInSeconds, clampedDeltaTime); } clock.reset(); args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds); deltaTimeInSeconds = 0; } args->m_physicsServerPtr->processClientCommands(); } while (args->m_cs->getSharedParam(0)!=eRequestTerminateMotion); } else { args->m_cs->lock(); args->m_cs->setSharedParam(0,eMotionInitializationFailed); args->m_cs->unlock(); } printf("finished, #skip = %d, skip1 = %d\n",skip,skip1); skip=0; skip1=0; //do nothing }
bool PhysicsServerSharedMemory::connectSharedMemory(struct GUIHelperInterface* guiHelper) { m_data->m_commandProcessor->setGuiHelper(guiHelper); bool allowCreation = true; bool allConnected = false; int numConnected = 0; int counter = 0; for (int block = 0; block < MAX_SHARED_MEMORY_BLOCKS; block++) { if (m_data->m_areConnected[block]) { allConnected = true; numConnected++; b3Warning("connectSharedMemory, while already connected"); continue; } do { m_data->m_testBlocks[block] = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(m_data->m_sharedMemoryKey + block, SHARED_MEMORY_SIZE, allowCreation); if (m_data->m_testBlocks[block]) { int magicId = m_data->m_testBlocks[block]->m_magicId; if (m_data->m_verboseOutput) { b3Printf("magicId = %d\n", magicId); } if (m_data->m_testBlocks[block]->m_magicId != SHARED_MEMORY_MAGIC_NUMBER) { InitSharedMemoryBlock(m_data->m_testBlocks[block]); if (m_data->m_verboseOutput) { b3Printf("Created and initialized shared memory block\n"); } m_data->m_areConnected[block] = true; numConnected++; } else { m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey + block, SHARED_MEMORY_SIZE); m_data->m_testBlocks[block] = 0; m_data->m_areConnected[block] = false; } } else { //b3Error("Cannot connect to shared memory"); m_data->m_areConnected[block] = false; } } while (counter++ < 10 && !m_data->m_areConnected[block]); if (!m_data->m_areConnected[block]) { b3Error("Server cannot connect to shared memory.\n"); } } allConnected = (numConnected == MAX_SHARED_MEMORY_BLOCKS); return allConnected; }
bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverStatus) { bool hasStatus = false; if (!m_data->m_testBlock1) { serverStatus.m_type = CMD_SHARED_MEMORY_NOT_INITIALIZED; return true; } if (!m_data->m_waitingForServer) { serverStatus.m_type = CMD_WAITING_FOR_CLIENT_COMMAND; return true; } if (m_data->m_testBlock1->m_numServerCommands> m_data->m_testBlock1->m_numProcessedServerCommands) { btAssert(m_data->m_testBlock1->m_numServerCommands==m_data->m_testBlock1->m_numProcessedServerCommands+1); const SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; hasStatus = true; serverStatus = serverCmd; EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type; //consume the command switch (serverCmd.m_type) { case CMD_CLIENT_COMMAND_COMPLETED: { if (m_data->m_verboseOutput) { b3Printf("Server completed command"); } break; } case CMD_URDF_LOADING_COMPLETED: { m_data->m_serverLoadUrdfOK = true; if (m_data->m_verboseOutput) { b3Printf("Server loading the URDF OK\n"); } if (serverCmd.m_dataStreamArguments.m_streamChunkLength>0) { bParse::btBulletFile* bf = new bParse::btBulletFile(this->m_data->m_testBlock1->m_bulletStreamDataServerToClient,serverCmd.m_dataStreamArguments.m_streamChunkLength); bf->setFileDNAisMemoryDNA(); bf->parse(false); m_data->m_robotMultiBodyData.push_back(bf); for (int i=0; i<bf->m_multiBodies.size(); i++) { int flag = bf->getFlags(); int qOffset = 7; int uOffset=6; if ((flag&bParse::FD_DOUBLE_PRECISION)!=0) { Bullet::btMultiBodyDoubleData* mb = (Bullet::btMultiBodyDoubleData*)bf->m_multiBodies[i]; if (mb->m_baseName) { if (m_data->m_verboseOutput) { b3Printf("mb->m_baseName = %s\n",mb->m_baseName); } } for (int link=0; link<mb->m_numLinks; link++) { { b3JointInfo info; info.m_flags = 0; info.m_qIndex = qOffset; info.m_uIndex = uOffset; info.m_linkIndex = link; if (mb->m_links[link].m_linkName) { if (m_data->m_verboseOutput) { b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName); } info.m_linkName = mb->m_links[link].m_linkName; } if (mb->m_links[link].m_jointName) { if (m_data->m_verboseOutput) { b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName); } info.m_jointName = mb->m_links[link].m_jointName; info.m_jointType = mb->m_links[link].m_jointType; } if ((mb->m_links[link].m_jointType == eRevoluteType)|| (mb->m_links[link].m_jointType == ePrismaticType)) { info.m_flags |= JOINT_HAS_MOTORIZED_POWER; } m_data->m_jointInfo.push_back(info); } qOffset+= mb->m_links[link].m_posVarCount; uOffset+= mb->m_links[link].m_dofCount; } } else { Bullet::btMultiBodyFloatData* mb = (Bullet::btMultiBodyFloatData*) bf->m_multiBodies[i]; if (mb->m_baseName) { if (m_data->m_verboseOutput) { b3Printf("mb->m_baseName = %s\n",mb->m_baseName); } } for (int link=0; link<mb->m_numLinks; link++) { { b3JointInfo info; info.m_flags = 0; info.m_qIndex = qOffset; info.m_uIndex = uOffset; if (mb->m_links[link].m_linkName) { if (m_data->m_verboseOutput) { b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName); } info.m_linkName = mb->m_links[link].m_linkName; } if (mb->m_links[link].m_jointName) { if (m_data->m_verboseOutput) { b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName); } info.m_jointName = mb->m_links[link].m_jointName; info.m_jointType = mb->m_links[link].m_jointType; } if ((mb->m_links[link].m_jointType == eRevoluteType)|| (mb->m_links[link].m_jointType == ePrismaticType)) { info.m_flags |= JOINT_HAS_MOTORIZED_POWER; } m_data->m_jointInfo.push_back(info); } qOffset+= mb->m_links[link].m_posVarCount; uOffset+= mb->m_links[link].m_dofCount; } } } if (bf->ok()) { if (m_data->m_verboseOutput) { b3Printf("Received robot description ok!\n"); } } else { b3Warning("Robot description not received"); } } break; } case CMD_DESIRED_STATE_RECEIVED_COMPLETED: { if (m_data->m_verboseOutput) { b3Printf("Server received desired state"); } break; } case CMD_STEP_FORWARD_SIMULATION_COMPLETED: { if (m_data->m_verboseOutput) { b3Printf("Server completed step simulation"); } break; } case CMD_URDF_LOADING_FAILED: { if (m_data->m_verboseOutput) { b3Printf("Server failed loading the URDF...\n"); } m_data->m_serverLoadUrdfOK = false; break; } case CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED: { if (m_data->m_verboseOutput) { b3Printf("Server received bullet data stream OK\n"); } break; } case CMD_BULLET_DATA_STREAM_RECEIVED_FAILED: { if (m_data->m_verboseOutput) { b3Printf("Server failed receiving bullet data stream\n"); } break; } case CMD_ACTUAL_STATE_UPDATE_COMPLETED: { if (m_data->m_verboseOutput) { b3Printf("Received actual state\n"); } SharedMemoryStatus& command = m_data->m_testBlock1->m_serverCommands[0]; int numQ = command.m_sendActualStateArgs.m_numDegreeOfFreedomQ; int numU = command.m_sendActualStateArgs.m_numDegreeOfFreedomU; if (m_data->m_verboseOutput) { b3Printf("size Q = %d, size U = %d\n", numQ,numU); } char msg[1024]; { sprintf(msg,"Q=["); for (int i=0; i<numQ; i++) { if (i<numQ-1) { sprintf(msg,"%s%f,",msg,command.m_sendActualStateArgs.m_actualStateQ[i]); } else { sprintf(msg,"%s%f",msg,command.m_sendActualStateArgs.m_actualStateQ[i]); } } sprintf(msg,"%s]",msg); } if (m_data->m_verboseOutput) { b3Printf(msg); } { sprintf(msg,"U=["); for (int i=0; i<numU; i++) { if (i<numU-1) { sprintf(msg,"%s%f,",msg,command.m_sendActualStateArgs.m_actualStateQdot[i]); } else { sprintf(msg,"%s%f",msg,command.m_sendActualStateArgs.m_actualStateQdot[i]); } } sprintf(msg,"%s]",msg); } if (m_data->m_verboseOutput) { b3Printf(msg); } if (m_data->m_verboseOutput) { b3Printf("\n"); } break; } case CMD_DEBUG_LINES_COMPLETED: { if (m_data->m_verboseOutput) { b3Printf("Success receiving %d debug lines",serverCmd.m_sendDebugLinesArgs.m_numDebugLines); } int numLines = serverCmd.m_sendDebugLinesArgs.m_numDebugLines; btScalar* linesFrom = (btScalar*)&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]; btScalar* linesTo = (btScalar*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+numLines*sizeof(btVector3)); btScalar* linesColor = (btScalar*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+2*numLines*sizeof(btVector3)); m_data->m_debugLinesFrom.resize(numLines); m_data->m_debugLinesTo.resize(numLines); m_data->m_debugLinesColor.resize(numLines); for (int i=0; i<numLines; i++) { btVector3 from(linesFrom[i*4],linesFrom[i*4+1],linesFrom[i*4+2]); btVector3 to(linesTo[i*4],linesTo[i*4+1],linesTo[i*4+2]); btVector3 color(linesColor[i*4],linesColor[i*4+1],linesColor[i*4+2]); m_data->m_debugLinesFrom[i] = from; m_data->m_debugLinesTo[i] = to; m_data->m_debugLinesColor[i] = color; } break; } case CMD_DEBUG_LINES_OVERFLOW_FAILED: { b3Warning("Error receiving debug lines"); m_data->m_debugLinesFrom.resize(0); m_data->m_debugLinesTo.resize(0); m_data->m_debugLinesColor.resize(0); break; } default: { b3Error("Unknown server status\n"); btAssert(0); } }; m_data->m_testBlock1->m_numProcessedServerCommands++; //we don't have more than 1 command outstanding (in total, either server or client) btAssert(m_data->m_testBlock1->m_numProcessedServerCommands == m_data->m_testBlock1->m_numServerCommands); if (m_data->m_testBlock1->m_numServerCommands == m_data->m_testBlock1->m_numProcessedServerCommands) { m_data->m_waitingForServer = false; } else { m_data->m_waitingForServer = true; } } else { if (m_data->m_verboseOutput) { b3Printf("m_numServerStatus = %d, processed = %d\n", m_data->m_testBlock1->m_numServerCommands, m_data->m_testBlock1->m_numProcessedServerCommands); } } return hasStatus; }
void b3GpuRaycast::castRaysHost(const b3AlignedObjectArray<b3RayInfo>& rays, b3AlignedObjectArray<b3RayHit>& hitResults, int numBodies,const struct b3RigidBodyData* bodies, int numCollidables,const struct b3Collidable* collidables, const struct b3GpuNarrowPhaseInternalData* narrowphaseData) { // return castRays(rays,hitResults,numBodies,bodies,numCollidables,collidables); B3_PROFILE("castRaysHost"); for (int r=0;r<rays.size();r++) { b3Vector3 rayFrom = rays[r].m_from; b3Vector3 rayTo = rays[r].m_to; float hitFraction = hitResults[r].m_hitFraction; int hitBodyIndex= -1; b3Vector3 hitNormal; for (int b=0;b<numBodies;b++) { const b3Vector3& pos = bodies[b].m_pos; const b3Quaternion& orn = bodies[b].m_quat; switch (collidables[bodies[b].m_collidableIdx].m_shapeType) { case SHAPE_SPHERE: { b3Scalar radius = collidables[bodies[b].m_collidableIdx].m_radius; if (sphere_intersect(pos, radius, rayFrom, rayTo,hitFraction)) { hitBodyIndex = b; b3Vector3 hitPoint; hitPoint.setInterpolate3(rays[r].m_from, rays[r].m_to,hitFraction); hitNormal = (hitPoint-bodies[b].m_pos).normalize(); } } case SHAPE_CONVEX_HULL: { b3Transform convexWorldTransform; convexWorldTransform.setIdentity(); convexWorldTransform.setOrigin(bodies[b].m_pos); convexWorldTransform.setRotation(bodies[b].m_quat); b3Transform convexWorld2Local = convexWorldTransform.inverse(); b3Vector3 rayFromLocal = convexWorld2Local(rayFrom); b3Vector3 rayToLocal = convexWorld2Local(rayTo); int shapeIndex = collidables[bodies[b].m_collidableIdx].m_shapeIndex; const b3ConvexPolyhedronData& poly = narrowphaseData->m_convexPolyhedra[shapeIndex]; if (rayConvex(rayFromLocal, rayToLocal,poly,narrowphaseData->m_convexFaces, hitFraction, hitNormal)) { hitBodyIndex = b; } break; } default: { static bool once=true; if (once) { once=false; b3Warning("Raytest: unsupported shape type\n"); } } } } if (hitBodyIndex>=0) { hitResults[r].m_hitFraction = hitFraction; hitResults[r].m_hitPoint.setInterpolate3(rays[r].m_from, rays[r].m_to,hitFraction); hitResults[r].m_hitNormal = hitNormal; hitResults[r].m_hitBody = hitBodyIndex; } } }
void b3GpuDynamicsWorld::addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies) { constraint->setUserConstraintPtr(0); m_constraints.push_back(constraint); switch (constraint->getConstraintType()) { case POINT2POINT_CONSTRAINT_TYPE: { btPoint2PointConstraint* p = (btPoint2PointConstraint*) constraint; int rbA = p->getRigidBodyA().getUserIndex(); int rbB = p->getRigidBodyB().getUserIndex(); btVector3 pivotInB = p->getPivotInB(); if (rbB<=0) { pivotInB = p->getRigidBodyA().getWorldTransform()*p->getPivotInA(); rbB = m_staticBody->getUserIndex(); } if (rbA>=0 && rbB>=0) { b3Point2PointConstraint* p2p = new b3Point2PointConstraint(rbA,rbB, (const b3Vector3&)p->getPivotInA(),(const b3Vector3&)pivotInB); p2p->setBreakingImpulseThreshold(p->getBreakingImpulseThreshold()); constraint->setUserConstraintPtr(p2p); m_rigidBodyPipeline->addConstraint(p2p); } else { b3Error("invalid body index in addConstraint,b3Point2PointConstraint\n"); } break; } case D6_CONSTRAINT_TYPE: { btGeneric6DofConstraint* dof2 = (btGeneric6DofConstraint*) constraint; const b3RigidBodyCL* bodiesCL = m_np->getBodiesCpu(); int rbA = dof2->getRigidBodyA().getUserIndex(); int rbB = dof2->getRigidBodyB().getUserIndex(); btTransform frameInA = dof2->getFrameOffsetB(); btTransform frameInB = dof2->getFrameOffsetB(); if (rbA<=0) { frameInA = dof2->getRigidBodyB().getWorldTransform()*dof2->getFrameOffsetB(); rbA = m_staticBody->getUserIndex(); } if (rbB<=0) { frameInB = dof2->getRigidBodyA().getWorldTransform()*dof2->getFrameOffsetA(); rbB = m_staticBody->getUserIndex(); } if (rbA>=0 && rbB>=0) { b3Generic6DofConstraint* dof3 = new b3Generic6DofConstraint(rbA,rbB,(b3Transform&)frameInA,(b3Transform&)frameInB,false,bodiesCL);//(();//(rbA,rbB, (const b3Vector3&)p->getPivotInA(),(const b3Vector3&)pivotInB); { btVector3 limit(0,0,0); dof2->getLinearLowerLimit(limit); dof3->setLinearLowerLimit((const b3Vector3&)limit); dof2->setLinearUpperLimit(limit); dof3->setLinearUpperLimit((const b3Vector3&)limit); dof2->setAngularLowerLimit(limit); dof3->setAngularLowerLimit((const b3Vector3&)limit); dof2->setAngularUpperLimit(limit); dof3->setAngularUpperLimit((const b3Vector3&)limit); /* for (int i=0;i<6;i++) { dof3->setParam(BT_CONSTRAINT_STOP_CFM,dof2->getParam(BT_CONSTRAINT_STOP_CFM,i),i); dof3->setParam(BT_CONSTRAINT_STOP_ERP,dof2->getParam(BT_CONSTRAINT_STOP_ERP,i),i); } */ dof3->setBreakingImpulseThreshold(dof2->getBreakingImpulseThreshold()); } // p2p->setBreakingImpulseThreshold(p->getBreakingImpulseThreshold()); constraint->setUserConstraintPtr(dof3); m_rigidBodyPipeline->addConstraint(dof3); } else { b3Error("invalid body index in addConstraint, btGeneric6DofConstraint.\n"); } // b3Generic6DofConstraint break; } default: b3Warning("Warning: b3GpuDynamicsWorld::addConstraint with unsupported constraint type\n"); }; }
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); } } } } }
bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& command) { if (!m_data->m_waitingForServer) { //process command { m_data->m_waitingForServer = true; switch (command.m_type) { case CMD_LOAD_URDF: { if (!m_data->m_serverLoadUrdfOK) { m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_LOAD_URDF; sprintf(m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_urdfFileName,"r2d2.urdf"); m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialPosition[0] = 0.0; m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialPosition[1] = 0.0; m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialPosition[2] = 0.0; m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[0] = 0.0; m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[1] = 0.0; m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[2] = 0.0; m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[3] = 1.0; m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useFixedBase = false; m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useMultiBody = true; m_data->m_testBlock1->m_numClientCommands++; b3Printf("Client created CMD_LOAD_URDF\n"); } else { b3Warning("Server already loaded URDF, no client command submitted\n"); } break; } case CMD_CREATE_BOX_COLLISION_SHAPE: { if (m_data->m_serverLoadUrdfOK) { b3Printf("Requesting create box collision shape\n"); m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_CREATE_BOX_COLLISION_SHAPE; m_data->m_testBlock1->m_numClientCommands++; } else { b3Warning("No URDF loaded\n"); } break; } case CMD_SEND_BULLET_DATA_STREAM: { b3Printf("Sending a Bullet Data Stream\n"); ///The idea is to pass a stream of chunks from client to server ///over shared memory. The server will process it ///Initially we will just copy an entire .bullet file into shared ///memory but we can also send individual chunks one at a time ///so it becomes a streaming solution ///In addition, we can make a separate API to create those chunks ///if needed, instead of using a 3d modeler or the Bullet SDK btSerializer char relativeFileName[1024]; const char* fileName = "slope.bullet"; bool fileFound = b3ResourcePath::findResourcePath(fileName,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) { fread(m_data->m_testBlock1->m_bulletStreamDataClientToServer, mFileLen, 1, fp); fclose(fp); m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_SEND_BULLET_DATA_STREAM; m_data->m_testBlock1->m_clientCommands[0].m_dataStreamArguments.m_streamChunkLength = mFileLen; m_data->m_testBlock1->m_numClientCommands++; b3Printf("Send bullet data stream command\n"); } 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", fileName); } break; } case CMD_REQUEST_ACTUAL_STATE: { if (m_data->m_serverLoadUrdfOK) { b3Printf("Requesting actual state\n"); m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_REQUEST_ACTUAL_STATE; m_data->m_testBlock1->m_numClientCommands++; } else { b3Warning("No URDF loaded\n"); } break; } case CMD_SEND_DESIRED_STATE: { if (m_data->m_serverLoadUrdfOK) { b3Printf("Sending desired state (pos, vel, torque)\n"); m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_SEND_DESIRED_STATE; //todo: expose a drop box in the GUI for this int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE; switch (controlMode) { case CONTROL_MODE_VELOCITY: { m_data->m_testBlock1->m_clientCommands[0].m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_VELOCITY; for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++) { m_data->m_testBlock1->m_desiredStateQdot[i] = 1; m_data->m_testBlock1->m_desiredStateForceTorque[i] = 100; } break; } case CONTROL_MODE_TORQUE: { m_data->m_testBlock1->m_clientCommands[0].m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_TORQUE; for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++) { m_data->m_testBlock1->m_desiredStateForceTorque[i] = 100; } break; } default: { b3Printf("Unknown control mode in client CMD_SEND_DESIRED_STATE"); btAssert(0); } } m_data->m_testBlock1->m_numClientCommands++; } else { b3Warning("Cannot send CMD_SEND_DESIRED_STATE, no URDF loaded\n"); } break; } case CMD_STEP_FORWARD_SIMULATION: { if (m_data->m_serverLoadUrdfOK) { m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_STEP_FORWARD_SIMULATION; m_data->m_testBlock1->m_clientCommands[0].m_stepSimulationArguments.m_deltaTimeInSeconds = 1./60.; m_data->m_testBlock1->m_numClientCommands++; b3Printf("client created CMD_STEP_FORWARD_SIMULATION %d\n", m_data->m_counter++); } else { b3Warning("No URDF loaded yet, no client CMD_STEP_FORWARD_SIMULATION submitted\n"); } break; } case CMD_SHUTDOWN: { m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_SHUTDOWN; m_data->m_testBlock1->m_numClientCommands++; m_data->m_serverLoadUrdfOK = false; b3Printf("client created CMD_SHUTDOWN\n"); break; } default: { b3Error("unknown command requested\n"); } } } } return true; }
SimpleOpenGL3App::SimpleOpenGL3App( const char* title, int width,int height) { gApp = this; m_data = new SimpleInternalData; m_window = new b3gDefaultOpenGLWindow(); b3gWindowConstructionInfo ci; ci.m_title = title; ci.m_width = width; ci.m_height = height; m_window->createWindow(ci); m_window->setWindowTitle(title); glClearColor(1,1,1,1); m_window->startRendering(); #ifndef __APPLE__ glewInit(); #endif m_primRenderer = new GLPrimitiveRenderer(width,height); m_instancingRenderer = new GLInstancingRenderer(128*1024,4*1024*1024); m_instancingRenderer->init(); m_instancingRenderer->resize(width,height); m_instancingRenderer->InitShaders(); m_window->setMouseMoveCallback(b3DefaultMouseMoveCallback); m_window->setMouseButtonCallback(b3DefaultMouseButtonCallback); m_window->setKeyboardCallback(b3DefaultKeyboardCallback); m_window->setWheelCallback(b3DefaultWheelCallback); m_window->setResizeCallback(SimpleResizeCallback); TwGenerateDefaultFonts(); m_data->m_fontTextureId = BindFont(g_DefaultNormalFont); { GLint err; int datasize; float sx,sy,dx,dy,lh; GLuint texture; m_data->m_renderCallbacks = new OpenGL2RenderCallbacks(m_primRenderer); m_data->m_fontStash = sth_create(512,512,m_data->m_renderCallbacks);//256,256);//,1024);//512,512); err = glGetError(); assert(err==GL_NO_ERROR); if (!m_data->m_fontStash) { b3Warning("Could not create stash"); //fprintf(stderr, "Could not create stash.\n"); } int droidRegular=0; char* data2 = OpenSansData; unsigned char* data = (unsigned char*) data2; if (!(m_data->m_droidRegular = sth_add_font_from_memory(m_data->m_fontStash, data))) { b3Warning("error!\n"); } err = glGetError(); assert(err==GL_NO_ERROR); } }
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); } } } } }
void* Win32SharedMemory::allocateSharedMemory(int key, int size, bool allowCreation) { { Win32SharedMemorySegment* seg = 0; int i=0; for (i=0;i<m_internalData->m_segments.size();i++) { if (m_internalData->m_segments[i].m_key == key) { seg = &m_internalData->m_segments[i]; break; } } if (seg) { b3Error("already created shared memory segment using same key"); return seg->m_buf; } } Win32SharedMemorySegment seg; seg.m_key = key; #ifdef UNICODE swprintf_s (seg.m_szName,TEXT("MyFileMappingObject%d"),key); #else sprintf(seg.m_szName,"MyFileMappingObject%d",key); #endif seg.m_hMapFile = OpenFileMapping( FILE_MAP_ALL_ACCESS, // read/write access FALSE, // do not inherit the name seg.m_szName); // name of mapping object if (seg.m_hMapFile==NULL) { if (allowCreation) { seg.m_hMapFile = CreateFileMapping( INVALID_HANDLE_VALUE, // use paging file NULL, // default security PAGE_READWRITE, // read/write access 0, // maximum object size (high-order DWORD) size, // maximum object size (low-order DWORD) seg.m_szName); // name of mapping object } else { b3Warning("Could not create file mapping object (%d).\n",GetLastError()); return 0; } } seg.m_buf = MapViewOfFile(seg.m_hMapFile, // handle to map object FILE_MAP_ALL_ACCESS, // read/write permission 0, 0, size); if (seg.m_buf == NULL) { b3Warning("Could not map view of file (%d).\n",GetLastError()); CloseHandle(seg.m_hMapFile); return 0; } m_internalData->m_segments.push_back(seg); return seg.m_buf; }
virtual void reportWarning(const char* warning) { b3Warning(warning); }