void PhysicsServerSharedMemory::disconnectSharedMemory(bool deInitializeSharedMemory) { //m_data->m_commandProcessor->deleteDynamicsWorld(); m_data->m_commandProcessor->setGuiHelper(0); if (m_data->m_verboseOutput) { b3Printf("releaseSharedMemory1\n"); } for (int block = 0; block < MAX_SHARED_MEMORY_BLOCKS; block++) { if (m_data->m_testBlocks[block]) { if (m_data->m_verboseOutput) { b3Printf("m_testBlock1\n"); } if (deInitializeSharedMemory) { m_data->m_testBlocks[block]->m_magicId = 0; if (m_data->m_verboseOutput) { b3Printf("De-initialized shared memory, magic id = %d\n", m_data->m_testBlocks[block]->m_magicId); } } btAssert(m_data->m_sharedMemory); 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; } }
void b3ChromeUtilsStopTimingsAndWriteJsonFile(const char* fileNamePrefix) { b3SetCustomEnterProfileZoneFunc(MyDummyEnterProfileZoneFunc); b3SetCustomLeaveProfileZoneFunc(MyDummyLeaveProfileZoneFunc); //also for Bullet 2.x API btSetCustomEnterProfileZoneFunc(MyDummyEnterProfileZoneFunc); btSetCustomLeaveProfileZoneFunc(MyDummyLeaveProfileZoneFunc); char fileName[1024]; static int fileCounter = 0; sprintf(fileName, "%s_%d.json", fileNamePrefix, fileCounter++); gTimingFile = fopen(fileName, "w"); if (gTimingFile) { fprintf(gTimingFile, "{\"traceEvents\":[\n"); //dump the content to file for (int i = 0; i < BT_QUICKPROF_MAX_THREAD_COUNT; i++) { if (gTimings[i].m_numTimings) { printf("Writing %d timings for thread %d\n", gTimings[i].m_numTimings, i); gTimings[i].flush(); } } fprintf(gTimingFile, "\n],\n\"displayTimeUnit\": \"ns\"}"); fclose(gTimingFile); } else { b3Printf("Error opening file"); b3Printf(fileName); } gTimingFile = 0; }
bool PhysicsClientSharedMemory::connect(bool allowSharedMemoryInitialization) { bool allowCreation = true; m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE, allowCreation); if (m_data->m_testBlock1) { if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER) { if (allowSharedMemoryInitialization) { InitSharedMemoryBlock(m_data->m_testBlock1); b3Printf("Created and initialized shared memory block"); m_data->m_isConnected = true; } else { b3Error("Error: please start server before client\n"); m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); m_data->m_testBlock1 = 0; return false; } } else { b3Printf("Connected to existing shared memory, status OK.\n"); m_data->m_isConnected = true; } } else { b3Error("Cannot connect to shared memory"); return false; } return true; }
void PhysicsServerSharedMemory::releaseSharedMemory() { if (m_data->m_verboseOutput) { b3Printf("releaseSharedMemory1\n"); } if (m_data->m_testBlock1) { if (m_data->m_verboseOutput) { b3Printf("m_testBlock1\n"); } m_data->m_testBlock1->m_magicId = 0; if (m_data->m_verboseOutput) { b3Printf("magic id = %d\n",m_data->m_testBlock1->m_magicId); } btAssert(m_data->m_sharedMemory); m_data->m_sharedMemory->releaseSharedMemory( m_data->m_sharedMemoryKey , SHARED_MEMORY_SIZE); } if (m_data->m_sharedMemory) { if (m_data->m_verboseOutput) { b3Printf("m_sharedMemory\n"); } if (m_data->m_ownsSharedMemory) { delete m_data->m_sharedMemory; } m_data->m_sharedMemory = 0; m_data->m_testBlock1 = 0; } }
static int gLoadMultiBodyFromUrdf(lua_State *L) { int argc = lua_gettop(L); if (argc==4) { if (!lua_isuserdata(L,1)) { std::cerr << "error: first argument to b3CreateRigidbody should be world"; return 0; } luaL_checktype(L,3, LUA_TTABLE); btVector3 pos = getLuaVectorArg(L,3); btQuaternion orn = getLuaQuaternionArg(L,4); btDiscreteDynamicsWorld* world = (btDiscreteDynamicsWorld*) lua_touserdata(L,1); if (world != sLuaDemo->m_dynamicsWorld) { std::cerr << "error: first argument expected to be a world"; return 0; } const char* fileName = lua_tostring(L,2); #if 1 BulletURDFImporter u2b(sLuaDemo->m_guiHelper); bool loadOk = u2b.loadURDF(fileName); if (loadOk) { b3Printf("loaded %s OK!", fileName); btTransform tr; tr.setIdentity(); tr.setOrigin(pos); tr.setRotation(orn); int rootLinkIndex = u2b.getRootLinkIndex(); // printf("urdf root link index = %d\n",rootLinkIndex); MyMultiBodyCreator creation(sLuaDemo->m_guiHelper); bool m_useMultiBody = true; ConvertURDF2Bullet(u2b,creation, tr,sLuaDemo->m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix()); btMultiBody* mb = creation.getBulletMultiBody(); if (mb) { lua_pushlightuserdata (L, mb); return 1; } } else { b3Printf("can't find %s",fileName); } #endif } return 0; }
void PhysicsServer::initPhysics() { createEmptyDynamicsWorld(); m_testBlock1 = (SharedMemoryExampleData*) m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); // btAssert(m_testBlock1); if (m_testBlock1) { // btAssert(m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER); if (m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER) { b3Printf("Warning: shared memory is already initialized, did you already spawn a server?\n"); } m_testBlock1->m_numClientCommands = 0; m_testBlock1->m_numServerCommands = 0; m_testBlock1->m_numProcessedClientCommands=0; m_testBlock1->m_numProcessedServerCommands=0; m_testBlock1->m_magicId = SHARED_MEMORY_MAGIC_NUMBER; b3Printf("Shared memory succesfully allocated\n"); } else { b3Error("Couldn't allocated shared memory, is it implemented on your operating system?\n"); } }
virtual void initPhysics() { ///create some graphics proxy for the tracking target ///the endeffector tries to track it using Inverse Kinematics { int sphereId = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_MEDIUM); b3Quaternion orn(0, 0, 0, 1); b3Vector4 color = b3MakeVector4(1., 0.3, 0.3, 1); b3Vector3 scaling = b3MakeVector3(.02, .02, .02); m_targetSphereInstance = m_app->m_renderer->registerGraphicsInstance(sphereId, m_targetPos, orn, color, scaling); } m_app->m_renderer->writeTransforms(); int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER; m_robotSim.setGuiHelper(m_guiHelper); bool connected = m_robotSim.connect(mode); // 0;//m_robotSim.connect(m_guiHelper); b3Printf("robotSim connected = %d",connected); { m_kukaIndex = m_robotSim.loadURDF("kuka_iiwa/model.urdf"); if (m_kukaIndex >=0) { int numJoints = m_robotSim.getNumJoints(m_kukaIndex); b3Printf("numJoints = %d",numJoints); for (int i=0;i<numJoints;i++) { b3JointInfo jointInfo; m_robotSim.getJointInfo(m_kukaIndex,i,&jointInfo); b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName); } /* int wheelJointIndices[4]={2,3,6,7}; int wheelTargetVelocities[4]={-10,-10,-10,-10}; for (int i=0;i<4;i++) { b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_targetVelocity = wheelTargetVelocities[i]; controlArgs.m_maxTorqueValue = 1e30; m_robotSim.setJointMotorControl(m_kukaIndex,wheelJointIndices[i],controlArgs); } */ } { m_robotSim.loadURDF("plane.urdf"); m_robotSim.setGravity(b3MakeVector3(0,0,0)); } } }
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); } } } }
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 MultiBodyConstraintFeedbackSetup::stepSimulation(float deltaTime) { //m_multiBody->addLinkForce(0,btVector3(100,100,100)); if (0)//m_once) { m_once=false; m_multiBody->addJointTorque(0, 10.0); btScalar torque = m_multiBody->getJointTorque(0); b3Printf("t = %f,%f,%f\n",torque,torque,torque);//[0],torque[1],torque[2]); } btScalar timeStep = 1./240.f; m_dynamicsWorld->stepSimulation(timeStep,0); static int count = 0; if ((count& 0x0f)==0) { if (m_motor) { float force = m_motor->getAppliedImpulse(0)/timeStep; b3Printf("motor applied force = %f\n", force); } for (int i=0;i<m_jointFeedbacks.size();i++) { b3Printf("F_reaction[%i] linear:%f,%f,%f, angular:%f,%f,%f", i, m_jointFeedbacks[i]->m_reactionForces.m_topVec[0], m_jointFeedbacks[i]->m_reactionForces.m_topVec[1], m_jointFeedbacks[i]->m_reactionForces.m_topVec[2], m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[0], m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[1], m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[2] ); } } count++; /* b3Printf("base angvel = %f,%f,%f",m_multiBody->getBaseOmega()[0], m_multiBody->getBaseOmega()[1], m_multiBody->getBaseOmega()[2] ); */ btScalar jointVel =m_multiBody->getJointVel(0); // b3Printf("child angvel = %f",jointVel); }
void TestHingeTorque::stepSimulation(float deltaTime) { if (0)//m_once) { m_once=false; btHingeConstraint* hinge = (btHingeConstraint*)m_dynamicsWorld->getConstraint(0); btRigidBody& bodyA = hinge->getRigidBodyA(); btTransform trA = bodyA.getWorldTransform(); btVector3 hingeAxisInWorld = trA.getBasis()*hinge->getFrameOffsetA().getBasis().getColumn(2); hinge->getRigidBodyA().applyTorque(-hingeAxisInWorld*10); hinge->getRigidBodyB().applyTorque(hingeAxisInWorld*10); } m_dynamicsWorld->stepSimulation(1./240,0); static int count = 0; if ((count& 0x0f)==0) { btRigidBody* base = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[0]); b3Printf("base angvel = %f,%f,%f",base->getAngularVelocity()[0], base->getAngularVelocity()[1], base->getAngularVelocity()[2]); btRigidBody* child = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[1]); b3Printf("child angvel = %f,%f,%f",child->getAngularVelocity()[0], child->getAngularVelocity()[1], child->getAngularVelocity()[2]); for (int i=0;i<m_jointFeedback.size();i++) { b3Printf("Applied force at the COM/Inertial frame B[%d]:(%f,%f,%f), torque B:(%f,%f,%f)\n", i, m_jointFeedback[i]->m_appliedForceBodyB.x(), m_jointFeedback[i]->m_appliedForceBodyB.y(), m_jointFeedback[i]->m_appliedForceBodyB.z(), m_jointFeedback[i]->m_appliedTorqueBodyB.x(), m_jointFeedback[i]->m_appliedTorqueBodyB.y(), m_jointFeedback[i]->m_appliedTorqueBodyB.z()); } } count++; //CommonRigidBodyBase::stepSimulation(deltaTime); }
void PosixSharedMemory::releaseSharedMemory(int key, int size) { #ifdef TEST_SHARED_MEMORY btSharedMemorySegment* 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 (0==seg) { b3Error("PosixSharedMemory::releaseSharedMemory: shared memory key not found"); return; } if (seg->m_sharedMemoryId < 0) { b3Error("PosixSharedMemory::releaseSharedMemory: shared memory id is not set"); } else { if (seg->m_createdSharedMemory) { int result = shmctl(seg->m_sharedMemoryId,IPC_RMID,0); if (result == -1) { b3Error("PosixSharedMemory::releaseSharedMemory: shmat returned -1"); } else { b3Printf("PosixSharedMemory::releaseSharedMemory removed shared memory"); } seg->m_createdSharedMemory = false; seg->m_sharedMemoryId = -1; } if (seg->m_sharedMemoryPtr) { shmdt(seg->m_sharedMemoryPtr); seg->m_sharedMemoryPtr = 0; b3Printf("PosixSharedMemory::releaseSharedMemory detached shared memory\n"); } } m_internalData->m_segments.removeAtIndex(i); #endif }
PhysicsServerExample::PhysicsServerExample(GUIHelperInterface* helper) :SharedMemoryCommon(helper), m_wantsShutdown(false) { b3Printf("Started PhysicsServer\n"); bool useServer = true; }
b3Scalar resolveCollision(LWRigidBody& bodyA, LWRigidBody& bodyB, LWContactPoint& contactPoint) { b3Assert(contactPoint.m_distance<=0); btScalar appliedImpulse = 0.f; b3Vector3 rel_pos1 = contactPoint.m_ptOnAWorld - bodyA.m_worldPose.m_position; b3Vector3 rel_pos2 = contactPoint.m_ptOnBWorld - bodyB.getPosition(); btScalar rel_vel = contactPoint.m_normalOnB.dot(bodyA.getVelocity(rel_pos1) - bodyB.getVelocity(rel_pos2)); if (rel_vel < -B3_EPSILON) { b3Vector3 temp1 = bodyA.m_invInertiaTensorWorld * rel_pos1.cross(contactPoint.m_normalOnB); b3Vector3 temp2 = bodyB.m_invInertiaTensorWorld * rel_pos2.cross(contactPoint.m_normalOnB); btScalar impulse = -(1.0f + gRestitution) * rel_vel / (bodyA.m_invMass + bodyB.m_invMass + contactPoint.m_normalOnB.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2))); b3Vector3 impulse_vector = contactPoint.m_normalOnB * impulse; b3Printf("impulse = %f\n", impulse); appliedImpulse = impulse; bodyA.applyImpulse(impulse_vector, rel_pos1); bodyB.applyImpulse(-impulse_vector, rel_pos2); } return appliedImpulse; }
void GpuConvexScene::setupScene() { m_raycaster = new b3GpuRaycast(m_clData->m_clContext,m_clData->m_clDevice,m_clData->m_clQueue); int index=0; createStaticEnvironment(); index+=createDynamicsObjects(); m_data->m_rigidBodyPipeline->writeAllInstancesToGpu(); float camPos[4]={0,0,0,0};//ci.arraySizeX,ci.arraySizeY/2,ci.arraySizeZ,0}; //float camPos[4]={1,12.5,1.5,0}; m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraTargetPosition(camPos[0],camPos[1],camPos[2]); m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraDistance(150); //m_instancingRenderer->setCameraYaw(85); m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraYaw(30); m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraPitch(225); m_guiHelper->getRenderInterface()->updateCamera(1);//>updateCamera(); char msg[1024]; int numInstances = index; sprintf(msg,"Num objects = %d",numInstances); b3Printf(msg); //if (ci.m_gui) // ci.m_gui->setStatusBarMessage(msg,true); }
PhysicsClientExample::~PhysicsClientExample() { if (m_physicsClientHandle) { b3ProcessServerStatus(m_physicsClientHandle); b3DisconnectSharedMemory(m_physicsClientHandle); } if (m_options == eCLIENTEXAMPLE_SERVER) { bool deInitializeSharedMemory = true; m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory); } if (m_canvas) { if (m_canvasRGBIndex >= 0) m_canvas->destroyCanvas(m_canvasRGBIndex); if (m_canvasDepthIndex >= 0) m_canvas->destroyCanvas(m_canvasDepthIndex); if (m_canvasSegMaskIndex >= 0) m_canvas->destroyCanvas(m_canvasSegMaskIndex); } b3Printf("~PhysicsClientExample\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 { b3Error("Cannot connect to shared memory"); return false; } return true; }
bool PhysicsServer::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn, bool useMultiBody, bool useFixedBase) { MyURDFImporter u2b(m_guiHelper); bool loadOk = u2b.loadURDF(fileName); if (loadOk) { b3Printf("loaded %s OK!", fileName); btTransform tr; tr.setIdentity(); tr.setOrigin(pos); tr.setRotation(orn); int rootLinkIndex = u2b.getRootLinkIndex(); // printf("urdf root link index = %d\n",rootLinkIndex); MyMultiBodyCreator creation(m_guiHelper); ConvertURDF2Bullet(u2b,creation, tr,m_dynamicsWorld,useMultiBody,u2b.getPathPrefix()); btMultiBody* mb = creation.getBulletMultiBody(); return true; } return false; }
PhysicsClientExample::PhysicsClientExample(GUIHelperInterface* helper) :SharedMemoryCommon(helper), m_wantsTermination(false), m_numMotors(0) { b3Printf("Started PhysicsClientExample\n"); }
PhysicsServerExample::PhysicsServerExample(GUIHelperInterface* helper) :SharedMemoryCommon(helper), m_wantsShutdown(false), m_isConnected(false) { b3Printf("Started PhysicsServer\n"); }
void* b3AlignedAllocInternal (size_t size, int alignment,int line,char* filename) { void *ret; char *real; b3g_totalBytesAlignedAllocs += size; b3g_numAlignedAllocs++; real = (char *)b3s_allocFunc(size + 2*sizeof(void *) + (alignment-1)); if (real) { ret = (void*) b3AlignPointer(real + 2*sizeof(void *), alignment); *((void **)(ret)-1) = (void *)(real); *((int*)(ret)-2) = size; } else { ret = (void *)(real);//?? } b3Printf("allocation#%d at address %x, from %s,line %d, size %d\n",b3g_numAlignedAllocs,real, filename,line,size); int* ptr = (int*)ret; *ptr = 12; return (ret); }
void b3GpuDynamicsWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const { b3AlignedObjectArray<b3RayInfo> rays; b3RayInfo ray; ray.m_from = (const b3Vector3&)rayFromWorld; ray.m_to = (const b3Vector3&)rayToWorld; rays.push_back(ray); b3AlignedObjectArray<b3RayHit> hitResults; b3RayHit hit; hit.m_hitFraction = 1.f; hitResults.push_back(hit); m_rigidBodyPipeline->castRays(rays,hitResults); b3Printf("hit = %f\n", hitResults[0].m_hitFraction); if (hitResults[0].m_hitFraction<1.f) { b3Assert(hitResults[0].m_hitBody >=0); b3Assert(hitResults[0].m_hitBody < m_collisionObjects.size()); b3Vector3 hitNormalLocal = hitResults[0].m_hitNormal; btCollisionObject* colObj = m_collisionObjects[hitResults[0].m_hitBody]; LocalRayResult rayResult(colObj,0,(btVector3&)hitNormalLocal,hitResults[0].m_hitFraction); rayResult.m_hitFraction = hitResults[0].m_hitFraction; resultCallback.addSingleResult(rayResult,true); } }
ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName) :CommonMultiBodyBase(helper) { m_data = new ImportUrdfInternalData; if (option==1) { m_useMultiBody = true; } else { m_useMultiBody = false; } static int count = 0; if (fileName) { setFileName(fileName); } else { gFileNameArray.clear(); //load additional urdf file names from file FILE* f = fopen("urdf_files.txt","r"); if (f) { int result; //warning: we don't avoid string buffer overflow in this basic example in fscanf char fileName[1024]; do { result = fscanf(f,"%s",fileName); b3Printf("urdf_files.txt entry %s",fileName); if (result==1) { gFileNameArray.push_back(fileName); } } while (result==1); fclose(f); } if (gFileNameArray.size()==0) { gFileNameArray.push_back("r2d2.urdf"); } int numFileNames = gFileNameArray.size(); if (count>=numFileNames) { count=0; } sprintf(m_fileName,"%s",gFileNameArray[count++].c_str()); } }
void PhysicsClientExample::createButtons() { bool isTrigger = false; if (m_guiHelper && m_guiHelper->getParameterInterface()) { m_guiHelper->getParameterInterface()->removeAllParameters(); 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("Create Cylinder Body",CMD_CREATE_RIGID_BODY,isTrigger); createButton("Reset Simulation",CMD_RESET_SIMULATION,isTrigger); createButton("Initialize Pose",CMD_INIT_POSE, isTrigger); createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger); if (m_physicsClientHandle && m_selectedBody>=0) { int numJoints = b3GetNumJoints(m_physicsClientHandle,m_selectedBody); for (int i=0;i<numJoints;i++) { b3JointInfo info; b3GetJointInfo(m_physicsClientHandle,m_selectedBody,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]; MyMotorInfo2* motorInfo = &m_motorTargetPositions[m_numMotors]; motorInfo->m_velTarget = 0.f; motorInfo->m_posTarget = 0.f; motorInfo->m_uIndex = info.m_uIndex; motorInfo->m_qIndex = info.m_qIndex; // SliderParams slider(motorName,&motorInfo->m_velTarget); // slider.m_minVal=-4; // slider.m_maxVal=4; SliderParams slider(motorName,&motorInfo->m_posTarget); slider.m_minVal=-4; slider.m_maxVal=4; if (m_guiHelper && m_guiHelper->getParameterInterface()) { m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } m_numMotors++; } } } } } }
static void cleanup(int signo) { if (!interrupted) { // this is the second time, we're hanging somewhere b3Printf("Aborting and deleting SharedMemoryCommon object"); delete sExampleBrowser; sleep(1); sExampleBrowser = 0; errx(EXIT_FAILURE, "aborted example on signal %d", signo); } else { b3Printf("no action"); exit(EXIT_FAILURE); } interrupted = true; warnx("caught signal %d", signo); }
virtual void exitPhysics() { b3Printf("exitPhysics, stopping threads"); bool blockingWait =false; int arg0,arg1; args.m_cs->lock(); //terminate all threads args.m_cs->setSharedParam(1,MAGIC_RESET_NUMBER); args.m_cs->unlock(); if (blockingWait) { for (int i=0;i<m_numThreads;i++) { m_threadSupport->waitForResponse(&arg0,&arg1); printf("finished waiting for response: %d %d\n", arg0,arg1); } } else { int numActiveThreads = m_numThreads; while (numActiveThreads) { if (m_threadSupport->isTaskCompleted(&arg0,&arg1,0)) { numActiveThreads--; printf("numActiveThreads = %d\n",numActiveThreads); } else { // printf("polling.."); } }; } delete m_threadSupport; b3Printf("Threads stopped"); for (int i=0;i<m_jobs.size();i++) { delete m_jobs[i]; } m_jobs.clear(); }
PhysicsClientExample::~PhysicsClientExample() { if (m_physicsClientHandle) { b3ProcessServerStatus(m_physicsClientHandle); b3DisconnectSharedMemory(m_physicsClientHandle); } b3Printf("~PhysicsClientExample\n"); }
void MultiPendulumExample::applyPendulumForce(btScalar pendulumForce){ if(pendulumForce != 0){ b3Printf("Apply %f to pendulum",pendulumForce); for (int i = 0; i < gDisplacedPendula; i++) { if (gDisplacedPendula >= 0 && gDisplacedPendula <= gPendulaQty) pendula[i]->applyCentralForce(btVector3(pendulumForce, 0, 0)); } } }
void enqueueCommand(const SharedMemoryCommand& orgCommand) { m_userCommandRequests.push_back(orgCommand); SharedMemoryCommand& cmd = m_userCommandRequests[m_userCommandRequests.size()-1]; cmd.m_sequenceNumber = m_sequenceNumberGenerator++; cmd.m_timeStamp = m_realtimeClock.getTimeMicroseconds(); b3Printf("User put command request %d on queue (queue length = %d)\n",cmd.m_type, m_userCommandRequests.size()); }
bool PhysicsDirect::processContactPointData(const struct SharedMemoryCommand& orgCommand) { SharedMemoryCommand command = orgCommand; const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; do { bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); b3Clock clock; double startTime = clock.getTimeInSeconds(); double timeOutInSeconds = m_data->m_timeOutInSeconds; while ((!hasStatus) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds)) { const SharedMemoryStatus* stat = processServerStatus(); if (stat) { hasStatus = true; } } m_data->m_hasStatus = hasStatus; if (hasStatus) { if (m_data->m_verboseOutput) { b3Printf("Contact Point Information Request OK\n"); } int startContactIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex; int numContactsCopied = serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; m_data->m_cachedContactPoints.resize(startContactIndex + numContactsCopied); b3ContactPointData* contactData = (b3ContactPointData*)&m_data->m_bulletStreamDataServerToClient[0]; for (int i = 0; i < numContactsCopied; i++) { m_data->m_cachedContactPoints[startContactIndex + i] = contactData[i]; } if (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints > 0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied) { m_data->m_hasStatus = false; command.m_type = CMD_REQUEST_CONTACT_POINT_INFORMATION; command.m_requestContactPointArguments.m_startingContactPointIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex + serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; command.m_requestContactPointArguments.m_objectAIndexFilter = -1; command.m_requestContactPointArguments.m_objectBIndexFilter = -1; } } } while (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints > 0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied); return m_data->m_hasStatus; }