void PhysicsMgr::start() { createEmptyDynamicsWorld(); ///create a few basic rigid bodies btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.), btScalar(50.), btScalar(50.))); m_collisionShapes.push_back(groundShape); btTransform groundTransform; groundTransform.setIdentity(); groundTransform.setOrigin(btVector3(0, -50, 0)); { btScalar mass(0.); createRigidBodyInternal(mass, groundTransform, groundShape, btVector4(0, 0, 1, 1)); } { //create a few dynamic rigidbodies // Re-using the same collision is better for memory usage and performance btBoxShape* colShape = createBoxShape(btVector3(.1, .1, .1)); //btCollisionShape* colShape = new btSphereShape(btScalar(1.)); m_collisionShapes.push_back(colShape); /// Create Dynamic Objects btTransform startTransform; startTransform.setIdentity(); btScalar mass(1.f); //rigidbody is dynamic if and only if mass is non zero, otherwise static bool isDynamic = (mass != 0.f); btVector3 localInertia(0, 0, 0); if (isDynamic) colShape->calculateLocalInertia(mass, localInertia); //int index = 0; //for (int k = 0; k < ARRAY_SIZE_Y; k++) //{ // for (int i = 0; i < ARRAY_SIZE_X; i++) // { // for (int j = 0; j < ARRAY_SIZE_Z; j++) // { // startTransform.setOrigin(btVector3( // btScalar(0.2*i), // btScalar(2 + .2*k), // btScalar(0.2*j))); // auto body = createRigidBody(mass, startTransform, colShape, btVector4(1, 0, 0, 1)); // body->setUserIndex(index); // index++; // } // } //} } }
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"); } }
void ConstraintPhysicsSetup::initPhysics() { m_guiHelper->setUpAxis(1); createEmptyDynamicsWorld(); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); int mode = btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawConstraints + btIDebugDraw::DBG_DrawConstraintLimits; m_dynamicsWorld->getDebugDrawer()->setDebugMode(mode); { SliderParams slider("target vel", &targetVel); slider.m_minVal = -4; slider.m_maxVal = 4; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { SliderParams slider("max impulse", &maxImpulse); slider.m_minVal = 0; slider.m_maxVal = 1000; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { SliderParams slider("actual vel", &actualHingeVelocity); slider.m_minVal = -4; slider.m_maxVal = 4; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } val = 1.f; { SliderParams slider("angle", &val); slider.m_minVal = -720; slider.m_maxVal = 720; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { // create a door using hinge constraint attached to the world btCollisionShape* pDoorShape = new btBoxShape(btVector3(2.0f, 5.0f, 0.2f)); m_collisionShapes.push_back(pDoorShape); btTransform doorTrans; doorTrans.setIdentity(); doorTrans.setOrigin(btVector3(-5.0f, -2.0f, 0.0f)); btRigidBody* pDoorBody = createRigidBody(1.0, doorTrans, pDoorShape); pDoorBody->setActivationState(DISABLE_DEACTIVATION); const btVector3 btPivotA(10.f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside spDoorHinge = new btHingeAccumulatedAngleConstraint(*pDoorBody, btPivotA, btAxisA); m_dynamicsWorld->addConstraint(spDoorHinge); spDoorHinge->setDbgDrawSize(btScalar(5.f)); } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); }
void MultipleBoxesExample::initPhysics() { m_guiHelper->setUpAxis(1); createEmptyDynamicsWorld(); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); if (m_dynamicsWorld->getDebugDrawer()) m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints); ///create a few basic rigid bodies btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.))); m_collisionShapes.push_back(groundShape); btTransform groundTransform; groundTransform.setIdentity(); groundTransform.setOrigin(btVector3(0,-50,0)); { btScalar mass(0.); createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1)); } { //create a few dynamic rigidbodies // Re-using the same collision is better for memory usage and performance btBoxShape* colShape = createBoxShape(btVector3(1,1,1)); m_collisionShapes.push_back(colShape); /// Create Dynamic Objects btTransform startTransform; startTransform.setIdentity(); btScalar mass(1.f); //rigidbody is dynamic if and only if mass is non zero, otherwise static bool isDynamic = (mass != 0.f); btVector3 localInertia(0,0,0); if (isDynamic) colShape->calculateLocalInertia(mass,localInertia); for(int i=0;i<TOTAL_BOXES;++i) { startTransform.setOrigin(btVector3( btScalar(0), btScalar(20+i*2), btScalar(0))); createRigidBody(mass,startTransform,colShape); } } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); }
void CoordinateFrameDemoPhysicsSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) { createEmptyDynamicsWorld(); m_dynamicsWorld->setGravity(btVector3(0,0,0)); gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld); m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints); m_dynamicsWorld->getDebugDrawer()->setDebugMode(m_dynamicsWorld->getDebugDrawer()->getDebugMode() + btIDebugDraw::DBG_DrawFrames); btScalar sqr2 = btSqrt(2); btVector3 tetraVerts[] = { btVector3(1.f, 0.f, -1/sqr2), btVector3(-1.f, 0.f, -1/sqr2), btVector3(0, 1.f, 1/sqr2), btVector3(0, -1.f, 1/sqr2), }; { //create a few dynamic rigidbodies // Re-using the same collision is better for memory usage and performance btCompoundShape* hull = new btCompoundShape(); btConvexHullShape* childHull = new btConvexHullShape(&tetraVerts[0].getX(),sizeof(tetraVerts)/sizeof(btVector3),sizeof(btVector3)); childHull->initializePolyhedralFeatures(); btTransform childTrans; childTrans.setIdentity(); childTrans.setOrigin(btVector3(2,0,0)); hull->addChildShape(childTrans,childHull); gfxBridge.createCollisionShapeGraphicsObject(hull); m_collisionShapes.push_back(hull); /// Create Dynamic Objects btTransform startTransform; startTransform.setIdentity(); btScalar mass(1.f); //rigidbody is dynamic if and only if mass is non zero, otherwise static bool isDynamic = (mass != 0.f); btVector3 localInertia(0,0,0); if (isDynamic) hull->calculateLocalInertia(mass,localInertia); startTransform.setOrigin(btVector3(0,0,0)); btRigidBody* body = createRigidBody(mass,startTransform,hull); gfxBridge.createRigidBodyGraphicsObject(body, btVector3(1, 1, 0)); } }
PhysicsServerSharedMemory::PhysicsServerSharedMemory() { m_data = new PhysicsServerInternalData(); #ifdef _WIN32 m_data->m_sharedMemory = new Win32SharedMemoryServer(); #else m_data->m_sharedMemory = new PosixSharedMemory(); #endif createEmptyDynamicsWorld(); }
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"); } }
void PhysicsServerExample::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); m_isConnected = m_physicsServer.connectSharedMemory( m_dynamicsWorld,m_guiHelper); }
void InverseDynamicsExample::initPhysics() { //roboticists like Z up int upAxis = 2; m_guiHelper->setUpAxis(upAxis); createEmptyDynamicsWorld(); btVector3 gravity(0,0,0); // gravity[upAxis]=-9.8; m_dynamicsWorld->setGravity(gravity); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); { SliderParams slider("Kp",&kp); slider.m_minVal=0; slider.m_maxVal=2000; if (m_guiHelper->getParameterInterface()) m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { SliderParams slider("Kd",&kd); slider.m_minVal=0; slider.m_maxVal=50; if (m_guiHelper->getParameterInterface()) m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } if (m_option == BT_ID_PROGRAMMATICALLY) { ButtonParams button("toggle inverse model",0,true); button.m_callback = toggleUseInverseModel; m_guiHelper->getParameterInterface()->registerButtonParameter(button); } switch (m_option) { case BT_ID_LOAD_URDF: { BulletURDFImporter u2b(m_guiHelper,0,1); bool loadOk = u2b.loadURDF("kuka_iiwa/model.urdf");// lwr / kuka.urdf"); if (loadOk) { int rootLinkIndex = u2b.getRootLinkIndex(); b3Printf("urdf root link index = %d\n",rootLinkIndex); MyMultiBodyCreator creation(m_guiHelper); btTransform identityTrans; identityTrans.setIdentity(); ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,true,u2b.getPathPrefix()); for (int i = 0; i < u2b.getNumAllocatedCollisionShapes(); i++) { m_collisionShapes.push_back(u2b.getAllocatedCollisionShape(i)); } m_multiBody = creation.getBulletMultiBody(); if (m_multiBody) { //kuka without joint control/constraints will gain energy explode soon due to timestep/integrator //temporarily set some extreme damping factors until we have some joint control or constraints m_multiBody->setAngularDamping(0*0.99); m_multiBody->setLinearDamping(0*0.99); b3Printf("Root link name = %s",u2b.getLinkName(u2b.getRootLinkIndex()).c_str()); } } break; } case BT_ID_PROGRAMMATICALLY: { btTransform baseWorldTrans; baseWorldTrans.setIdentity(); m_multiBody = createInvertedPendulumMultiBody(m_dynamicsWorld, m_guiHelper, baseWorldTrans, false); break; } default: { b3Error("Unknown option in InverseDynamicsExample::initPhysics"); b3Assert(0); } }; if(m_multiBody) { { if (m_guiHelper->getAppInterface() && m_guiHelper->getParameterInterface()) { m_timeSeriesCanvas = new TimeSeriesCanvas(m_guiHelper->getAppInterface()->m_2dCanvasInterface,512,230, "Joint Space Trajectory"); m_timeSeriesCanvas ->setupTimeSeries(3,100, 0); } } // construct inverse model btInverseDynamics::btMultiBodyTreeCreator id_creator; if (-1 == id_creator.createFromBtMultiBody(m_multiBody, false)) { b3Error("error creating tree\n"); } else { m_inverseModel = btInverseDynamics::CreateMultiBodyTree(id_creator); } // add joint target controls qd.resize(m_multiBody->getNumDofs()); qd_name.resize(m_multiBody->getNumDofs()); q_name.resize(m_multiBody->getNumDofs()); if (m_timeSeriesCanvas && m_guiHelper->getParameterInterface()) { for(std::size_t dof=0;dof<qd.size();dof++) { qd[dof] = 0; char tmp[25]; sprintf(tmp,"q_desired[%lu]",dof); qd_name[dof] = tmp; SliderParams slider(qd_name[dof].c_str(),&qd[dof]); slider.m_minVal=-3.14; slider.m_maxVal=3.14; sprintf(tmp,"q[%lu]",dof); q_name[dof] = tmp; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); btVector4 color = sJointCurveColors[dof&7]; m_timeSeriesCanvas->addDataSource(q_name[dof].c_str(), color[0]*255,color[1]*255,color[2]*255); } } } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); }
void RigidBodyFromObjExample::initPhysics() { m_guiHelper->setUpAxis(1); createEmptyDynamicsWorld(); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); //if (m_dynamicsWorld->getDebugDrawer()) // m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints); ///create a few basic rigid bodies btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.))); m_collisionShapes.push_back(groundShape); btTransform groundTransform; groundTransform.setIdentity(); groundTransform.setOrigin(btVector3(0,-50,0)); { btScalar mass(0.); createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1)); } //load our obj mesh const char* fileName = "teddy.obj";//sphere8.obj";//sponza_closed.obj";//sphere8.obj"; char relativeFileName[1024]; if (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024)) { char pathPrefix[1024]; b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); } GLInstanceGraphicsShape* glmesh = LoadMeshFromObj(relativeFileName, ""); printf("[INFO] Obj loaded: Extracted %d verticed from obj file [%s]\n", glmesh->m_numvertices, fileName); const GLInstanceVertex& v = glmesh->m_vertices->at(0); btConvexHullShape* shape = new btConvexHullShape((const btScalar*)(&(v.xyzw[0])), glmesh->m_numvertices, sizeof(GLInstanceVertex)); float scaling[4] = {0.1,0.1,0.1,1}; btVector3 localScaling(scaling[0],scaling[1],scaling[2]); shape->setLocalScaling(localScaling); if (m_options & OptimizeConvexObj) { shape->optimizeConvexHull(); } if (m_options & ComputePolyhedralFeatures) { shape->initializePolyhedralFeatures(); } //shape->setMargin(0.001); m_collisionShapes.push_back(shape); btTransform startTransform; startTransform.setIdentity(); btScalar mass(1.f); bool isDynamic = (mass != 0.f); btVector3 localInertia(0,0,0); if (isDynamic) shape->calculateLocalInertia(mass,localInertia); float color[4] = {1,1,1,1}; float orn[4] = {0,0,0,1}; float pos[4] = {0,3,0,0}; btVector3 position(pos[0],pos[1],pos[2]); startTransform.setOrigin(position); btRigidBody* body = createRigidBody(mass,startTransform,shape); bool useConvexHullForRendering = ((m_options & ObjUseConvexHullForRendering)!=0); if (!useConvexHullForRendering) { int shapeId = m_guiHelper->registerGraphicsShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, &glmesh->m_indices->at(0), glmesh->m_numIndices, B3_GL_TRIANGLES, -1); shape->setUserIndex(shapeId); int renderInstance = m_guiHelper->registerGraphicsInstance(shapeId,pos,orn,color,scaling); body->setUserIndex(renderInstance); } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); }
void BasicExample::initPhysics() { m_guiHelper->setUpAxis(1); createEmptyDynamicsWorld(); //m_dynamicsWorld->setGravity(btVector3(0,0,0)); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); if (m_dynamicsWorld->getDebugDrawer()) m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints); ///create a few basic rigid bodies btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.))); //groundShape->initializePolyhedralFeatures(); //btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50); m_collisionShapes.push_back(groundShape); btTransform groundTransform; groundTransform.setIdentity(); groundTransform.setOrigin(btVector3(0,-50,0)); { btScalar mass(0.); createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1)); } { //create a few dynamic rigidbodies // Re-using the same collision is better for memory usage and performance btBoxShape* colShape = createBoxShape(btVector3(.1,.1,.1)); //btCollisionShape* colShape = new btSphereShape(btScalar(1.)); m_collisionShapes.push_back(colShape); /// Create Dynamic Objects btTransform startTransform; startTransform.setIdentity(); btScalar mass(1.f); //rigidbody is dynamic if and only if mass is non zero, otherwise static bool isDynamic = (mass != 0.f); btVector3 localInertia(0,0,0); if (isDynamic) colShape->calculateLocalInertia(mass,localInertia); for (int k=0;k<ARRAY_SIZE_Y;k++) { for (int i=0;i<ARRAY_SIZE_X;i++) { for(int j = 0;j<ARRAY_SIZE_Z;j++) { startTransform.setOrigin(btVector3( btScalar(0.2*i), btScalar(2+.2*k), btScalar(0.2*j))); createRigidBody(mass,startTransform,colShape); } } } } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); }
void SimpleJointExample::initPhysics() { m_guiHelper->setUpAxis(1); createEmptyDynamicsWorld(); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); if (m_dynamicsWorld->getDebugDrawer()) m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints); ///create a few basic rigid bodies btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.))); m_collisionShapes.push_back(groundShape); btTransform groundTransform; groundTransform.setIdentity(); groundTransform.setOrigin(btVector3(0,-50,0)); { btScalar mass(0.); createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1)); } { //create a few dynamic rigidbodies // Re-using the same collision is better for memory usage and performance btBoxShape* colShape = createBoxShape(btVector3(1,1,1)); m_collisionShapes.push_back(colShape); /// Create Dynamic Objects btTransform startTransform; startTransform.setIdentity(); btScalar mass(1.f); //rigidbody is dynamic if and only if mass is non zero, otherwise static bool isDynamic = (mass != 0.f); btVector3 localInertia(0,0,0); if (isDynamic) colShape->calculateLocalInertia(mass,localInertia); startTransform.setOrigin(btVector3( btScalar(0), btScalar(10), btScalar(0))); btRigidBody* dynamicBox = createRigidBody(mass,startTransform,colShape); //create a static rigid body mass = 0; startTransform.setOrigin(btVector3( btScalar(0), btScalar(20), btScalar(0))); btRigidBody* staticBox = createRigidBody(mass,startTransform,colShape); //create a simple p2pjoint constraint btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*dynamicBox, *staticBox, btVector3(0,3,0), btVector3(0,0,0)); p2p->m_setting.m_damping = 0.0625; p2p->m_setting.m_impulseClamp = 0.95; m_dynamicsWorld->addConstraint(p2p); } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); }
void ImportMJCFSetup::initPhysics() { m_guiHelper->setUpAxis(m_upAxis); createEmptyDynamicsWorld(); //MuJoCo uses a slightly different collision filter mode, use the FILTER_GROUPAMASKB_OR_GROUPBMASKA2 //@todo also use the modified collision filter for raycast and other collision related queries m_filterCallback->m_filterMode = FILTER_GROUPAMASKB_OR_GROUPBMASKA2; //m_dynamicsWorld->getSolverInfo().m_numIterations = 50; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); m_dynamicsWorld->getDebugDrawer()->setDebugMode( btIDebugDraw::DBG_DrawConstraints + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawAabb); //+btIDebugDraw::DBG_DrawConstraintLimits); if (m_guiHelper->getParameterInterface()) { SliderParams slider("Gravity", &m_grav); slider.m_minVal = -10; slider.m_maxVal = 10; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } int flags = 0; b3BulletDefaultFileIO fileIO; BulletMJCFImporter importer(m_guiHelper, 0, &fileIO, flags); MyMJCFLogger logger; bool result = importer.loadMJCF(m_fileName, &logger); if (result) { btTransform rootTrans; rootTrans.setIdentity(); for (int m = 0; m < importer.getNumModels(); m++) { importer.activateModel(m); // normally used with PhysicsServerCommandProcessor that allocates unique ids to multibodies, // emulate this behavior here: importer.setBodyUniqueId(m); btMultiBody* mb = 0; //todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user //int rootLinkIndex = importer.getRootLinkIndex(); //b3Printf("urdf root link index = %d\n",rootLinkIndex); MyMultiBodyCreator creation(m_guiHelper); rootTrans.setIdentity(); importer.getRootTransformInWorld(rootTrans); ConvertURDF2Bullet(importer, creation, rootTrans, m_dynamicsWorld, m_useMultiBody, importer.getPathPrefix(), CUF_USE_MJCF); mb = creation.getBulletMultiBody(); if (mb) { std::string* name = new std::string(importer.getLinkName( importer.getRootLinkIndex())); m_nameMemory.push_back(name); #ifdef TEST_MULTIBODY_SERIALIZATION s->registerNameForPointer(name->c_str(), name->c_str()); #endif //TEST_MULTIBODY_SERIALIZATION mb->setBaseName(name->c_str()); mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR); //create motors for each btMultiBody joint int numLinks = mb->getNumLinks(); for (int i = 0; i < numLinks; i++) { int mbLinkIndex = i; int urdfLinkIndex = creation.m_mb2urdfLink[mbLinkIndex]; std::string* jointName = new std::string(importer.getJointName(urdfLinkIndex)); std::string* linkName = new std::string(importer.getLinkName(urdfLinkIndex).c_str()); #ifdef TEST_MULTIBODY_SERIALIZATION s->registerNameForPointer(jointName->c_str(), jointName->c_str()); s->registerNameForPointer(linkName->c_str(), linkName->c_str()); #endif //TEST_MULTIBODY_SERIALIZATION m_nameMemory.push_back(jointName); m_nameMemory.push_back(linkName); mb->getLinkCollider(i)->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR); mb->getLink(i).m_linkName = linkName->c_str(); mb->getLink(i).m_jointName = jointName->c_str(); m_data->m_mb = mb; if (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eRevolute || mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::ePrismatic) { if (m_data->m_numMotors < MAX_NUM_MOTORS) { char motorName[1024]; sprintf(motorName, "%s q ", jointName->c_str()); btScalar* motorPos = &m_data->m_motorTargetPositions[m_data->m_numMotors]; *motorPos = 0.f; SliderParams slider(motorName, motorPos); slider.m_minVal = -4; slider.m_maxVal = 4; slider.m_clampToIntegers = false; slider.m_clampToNotches = false; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); float maxMotorImpulse = 5.f; btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb, mbLinkIndex, 0, 0, maxMotorImpulse); motor->setErp(0.1); //motor->setMaxAppliedImpulse(0); m_data->m_jointMotors[m_data->m_numMotors] = motor; m_dynamicsWorld->addMultiBodyConstraint(motor); m_data->m_numMotors++; } } } } else { // not multibody if (1) { //create motors for each generic joint int num6Dof = creation.getNum6DofConstraints(); for (int i = 0; i < num6Dof; i++) { btGeneric6DofSpring2Constraint* c = creation.get6DofConstraint(i); if (c->getUserConstraintPtr()) { GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)c->getUserConstraintPtr(); if ((jointInfo->m_urdfJointType == URDFRevoluteJoint) || (jointInfo->m_urdfJointType == URDFPrismaticJoint) || (jointInfo->m_urdfJointType == URDFContinuousJoint)) { int urdfLinkIndex = jointInfo->m_urdfIndex; std::string jointName = importer.getJointName(urdfLinkIndex); char motorName[1024]; sprintf(motorName, "%s q'", jointName.c_str()); btScalar* motorVel = &m_data->m_motorTargetPositions[m_data->m_numMotors]; *motorVel = 0.f; SliderParams slider(motorName, motorVel); slider.m_minVal = -4; slider.m_maxVal = 4; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); m_data->m_generic6DofJointMotors[m_data->m_numMotors] = c; bool motorOn = true; c->enableMotor(jointInfo->m_jointAxisIndex, motorOn); c->setMaxMotorForce(jointInfo->m_jointAxisIndex, 10000); c->setTargetVelocity(jointInfo->m_jointAxisIndex, 0); m_data->m_numMotors++; } } } } } } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); } }
void InclinedPlaneExample::initPhysics() { { // create slider to change the ramp tilt SliderParams slider("Ramp Tilt",&gTilt); slider.m_minVal=0; slider.m_maxVal=SIMD_PI/2.0f; slider.m_clampToNotches = false; slider.m_callback = onRampInclinationChanged; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { // create slider to change the ramp friction SliderParams slider("Ramp Friction",&gRampFriction); slider.m_minVal=0; slider.m_maxVal=10; slider.m_clampToNotches = false; slider.m_callback = onRampFrictionChanged; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { // create slider to change the ramp restitution SliderParams slider("Ramp Restitution",&gRampRestitution); slider.m_minVal=0; slider.m_maxVal=1; slider.m_clampToNotches = false; slider.m_callback = onRampRestitutionChanged; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { // create slider to change the box friction SliderParams slider("Box Friction",&gBoxFriction); slider.m_minVal=0; slider.m_maxVal=10; slider.m_clampToNotches = false; slider.m_callback = onBoxFrictionChanged; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { // create slider to change the box restitution SliderParams slider("Box Restitution",&gBoxRestitution); slider.m_minVal=0; slider.m_maxVal=1; slider.m_clampToNotches = false; slider.m_callback = onBoxRestitutionChanged; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { // create slider to change the sphere friction SliderParams slider("Sphere Friction",&gSphereFriction); slider.m_minVal=0; slider.m_maxVal=10; slider.m_clampToNotches = false; slider.m_callback = onSphereFrictionChanged; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { // create slider to change the sphere rolling friction SliderParams slider("Sphere Rolling Friction",&gSphereRollingFriction); slider.m_minVal=0; slider.m_maxVal=10; slider.m_clampToNotches = false; slider.m_callback = onSphereRestitutionChanged; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { // create slider to change the sphere rolling friction SliderParams slider("Sphere Spinning",&gSphereSpinningFriction); slider.m_minVal=0; slider.m_maxVal=2; slider.m_clampToNotches = false; slider.m_callback = onSphereRestitutionChanged; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { // create slider to change the sphere restitution SliderParams slider("Sphere Restitution",&gSphereRestitution); slider.m_minVal=0; slider.m_maxVal=1; slider.m_clampToNotches = false; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } m_guiHelper->setUpAxis(1); // set Y axis as up axis createEmptyDynamicsWorld(); // create debug drawer m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); if (m_dynamicsWorld->getDebugDrawer()) m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints); { // create a static ground btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.))); m_collisionShapes.push_back(groundShape); btTransform groundTransform; groundTransform.setIdentity(); groundTransform.setOrigin(btVector3(0,-50,0)); btScalar mass(0.); createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1)); } { //create a static inclined plane btBoxShape* inclinedPlaneShape = createBoxShape(btVector3(btScalar(20.),btScalar(1.),btScalar(10.))); m_collisionShapes.push_back(inclinedPlaneShape); btTransform startTransform; startTransform.setIdentity(); // position the inclined plane above ground startTransform.setOrigin(btVector3( btScalar(0), btScalar(15), btScalar(0))); btQuaternion incline; incline.setRotation(btVector3(0,0,1),gTilt); startTransform.setRotation(incline); btScalar mass(0.); ramp = createRigidBody(mass,startTransform,inclinedPlaneShape); ramp->setFriction(gRampFriction); ramp->setRestitution(gRampRestitution); } { //create a cube above the inclined plane btBoxShape* boxShape = createBoxShape(btVector3(1,1,1)); m_collisionShapes.push_back(boxShape); btTransform startTransform; startTransform.setIdentity(); btScalar boxMass(1.f); startTransform.setOrigin( btVector3(btScalar(0), btScalar(20), btScalar(2))); gBox = createRigidBody(boxMass, startTransform, boxShape); gBox->forceActivationState(DISABLE_DEACTIVATION); // to prevent the box on the ramp from disabling gBox->setFriction(gBoxFriction); gBox->setRestitution(gBoxRestitution); } { //create a sphere above the inclined plane btSphereShape* sphereShape = new btSphereShape(btScalar(1)); m_collisionShapes.push_back(sphereShape); btTransform startTransform; startTransform.setIdentity(); btScalar sphereMass(1.f); startTransform.setOrigin( btVector3(btScalar(0), btScalar(20), btScalar(4))); gSphere = createRigidBody(sphereMass, startTransform, sphereShape); gSphere->forceActivationState(DISABLE_DEACTIVATION); // to prevent the sphere on the ramp from disabling gSphere->setFriction(gSphereFriction); gSphere->setRestitution(gSphereRestitution); gSphere->setRollingFriction(gSphereRollingFriction); gSphere->setSpinningFriction(gSphereSpinningFriction); } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); }
void MultiPendulumExample::initPhysics() { // Setup your physics scene { // create a slider to change the number of pendula SliderParams slider("Number of Pendula", &gPendulaQty); slider.m_minVal = 1; slider.m_maxVal = 50; slider.m_clampToIntegers = true; m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider); } { // create a slider to change the number of displaced pendula SliderParams slider("Number of Displaced Pendula", &gDisplacedPendula); slider.m_minVal = 0; slider.m_maxVal = 49; slider.m_clampToIntegers = true; m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider); } { // create a slider to change the pendula restitution SliderParams slider("Pendula Restitution", &gPendulaRestitution); slider.m_minVal = 0; slider.m_maxVal = 1; slider.m_clampToNotches = false; slider.m_callback = onMultiPendulaRestitutionChanged; m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider); } { // create a slider to change the pendulum length SliderParams slider("Pendula Length", &gCurrentPendulumLength); slider.m_minVal = 0; slider.m_maxVal = 49; slider.m_clampToNotches = false; slider.m_callback = onMultiPendulaLengthChanged; m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider); } { // create a slider to change the force to displace the lowest pendulum SliderParams slider("Displacement force", &gDisplacementForce); slider.m_minVal = 0.1; slider.m_maxVal = 200; slider.m_clampToNotches = false; m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider); } { // create a slider to apply the force by slider SliderParams slider("Apply displacement force", &gForceScalar); slider.m_minVal = -1; slider.m_maxVal = 1; slider.m_clampToNotches = false; m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider); } m_guiHelper->setUpAxis(1); createEmptyDynamicsWorld(); // create a debug drawer m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); if (m_dynamicsWorld->getDebugDrawer()) m_dynamicsWorld->getDebugDrawer()->setDebugMode( btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawConstraints + btIDebugDraw::DBG_DrawConstraintLimits); { // create the multipendulum starting at the indicated position below and where each pendulum has the following mass btScalar pendulumMass(1.f); btVector3 position(0.0f,15.0f,0.0f); // initial top-most pendulum position // Re-using the same collision is better for memory usage and performance btSphereShape* pendulumShape = new btSphereShape(gSphereRadius); m_collisionShapes.push_back(pendulumShape); // create multi-pendulum createMultiPendulum(pendulumShape, floor(gPendulaQty), position, gInitialPendulumLength, pendulumMass); } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); }
void BridgeExample::initPhysics() { m_guiHelper->setUpAxis(1); createEmptyDynamicsWorld(); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); if (m_dynamicsWorld->getDebugDrawer()) m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints); ///create a few basic rigid bodies btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.))); m_collisionShapes.push_back(groundShape); btTransform groundTransform; groundTransform.setIdentity(); groundTransform.setOrigin(btVector3(0,-50,0)); { btScalar mass(0.); createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1)); } //create two fixed boxes to hold the planks { //create a few dynamic rigidbodies // Re-using the same collision is better for memory usage and performance btScalar plankWidth = 0.4; btScalar plankHeight = 0.2; btScalar plankBreadth = 1; btScalar plankOffset = plankWidth; //distance between two planks btScalar bridgeWidth = plankWidth*TOTAL_PLANKS + plankOffset*(TOTAL_PLANKS-1); btScalar bridgeHeight = 5; btScalar halfBridgeWidth = bridgeWidth*0.5f; btBoxShape* colShape = createBoxShape(btVector3(plankWidth,plankHeight,plankBreadth)); m_collisionShapes.push_back(colShape); /// Create Dynamic Objects btTransform startTransform; startTransform.setIdentity(); btScalar mass(1.f); //rigidbody is dynamic if and only if mass is non zero, otherwise static bool isDynamic = (mass != 0.f); btVector3 localInertia(0,0,0); if (isDynamic) colShape->calculateLocalInertia(mass,localInertia); //create a set of boxes to represent bridge btAlignedObjectArray<btRigidBody*> boxes; int lastBoxIndex = TOTAL_PLANKS-1; for(int i=0;i<TOTAL_PLANKS;++i) { float t = float(i)/lastBoxIndex; t = -(t*2-1.0f) * halfBridgeWidth; startTransform.setOrigin(btVector3( btScalar(t), bridgeHeight, btScalar(0) ) ); boxes.push_back(createRigidBody((i==0 || i==lastBoxIndex)?0:mass,startTransform,colShape)); } //add N-1 spring constraints for(int i=0;i<TOTAL_PLANKS-1;++i) { btRigidBody* b1 = boxes[i]; btRigidBody* b2 = boxes[i+1]; btPoint2PointConstraint* leftSpring = new btPoint2PointConstraint(*b1, *b2, btVector3(-0.5,0,-0.5), btVector3(0.5,0,-0.5)); m_dynamicsWorld->addConstraint(leftSpring); btPoint2PointConstraint* rightSpring = new btPoint2PointConstraint(*b1, *b2, btVector3(-0.5,0,0.5), btVector3(0.5,0,0.5)); m_dynamicsWorld->addConstraint(rightSpring); } } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); }
void TestHingeTorque::initPhysics() { int upAxis = 1; m_guiHelper->setUpAxis(upAxis); createEmptyDynamicsWorld(); m_dynamicsWorld->getSolverInfo().m_splitImpulse = false; m_dynamicsWorld->setGravity(btVector3(0,0,-10)); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); int mode = btIDebugDraw::DBG_DrawWireframe +btIDebugDraw::DBG_DrawConstraints +btIDebugDraw::DBG_DrawConstraintLimits; m_dynamicsWorld->getDebugDrawer()->setDebugMode(mode); { // create a door using hinge constraint attached to the world int numLinks = 2; bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals bool canSleep = false; bool selfCollide = false; btVector3 linkHalfExtents(0.05, 0.37, 0.1); btVector3 baseHalfExtents(0.05, 0.37, 0.1); btBoxShape* baseBox = new btBoxShape(baseHalfExtents); btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f); btTransform baseWorldTrans; baseWorldTrans.setIdentity(); baseWorldTrans.setOrigin(basePosition); //mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm //init the base btVector3 baseInertiaDiag(0.f, 0.f, 0.f); float baseMass = 0.f; float linkMass = 1.f; btRigidBody* base = createRigidBody(baseMass,baseWorldTrans,baseBox); m_dynamicsWorld->removeRigidBody(base); base->setDamping(0,0); m_dynamicsWorld->addRigidBody(base,collisionFilterGroup,collisionFilterMask); btBoxShape* linkBox1 = new btBoxShape(linkHalfExtents); btSphereShape* linkSphere = new btSphereShape(radius); btRigidBody* prevBody = base; for (int i=0;i<numLinks;i++) { btTransform linkTrans; linkTrans = baseWorldTrans; linkTrans.setOrigin(basePosition-btVector3(0,linkHalfExtents[1]*2.f*(i+1),0)); btCollisionShape* colOb = 0; if (i==0) { colOb = linkBox1; } else { colOb = linkSphere; } btRigidBody* linkBody = createRigidBody(linkMass,linkTrans,colOb); m_dynamicsWorld->removeRigidBody(linkBody); m_dynamicsWorld->addRigidBody(linkBody,collisionFilterGroup,collisionFilterMask); linkBody->setDamping(0,0); btTypedConstraint* con = 0; if (i==0) { //create a hinge constraint btVector3 pivotInA(0,-linkHalfExtents[1],0); btVector3 pivotInB(0,linkHalfExtents[1],0); btVector3 axisInA(1,0,0); btVector3 axisInB(1,0,0); bool useReferenceA = true; btHingeConstraint* hinge = new btHingeConstraint(*prevBody,*linkBody, pivotInA,pivotInB, axisInA,axisInB,useReferenceA); con = hinge; } else { btTransform pivotInA(btQuaternion::getIdentity(),btVector3(0, -radius, 0)); //par body's COM to cur body's COM offset btTransform pivotInB(btQuaternion::getIdentity(),btVector3(0, radius, 0)); //cur body's COM to cur body's PIV offset btGeneric6DofSpring2Constraint* fixed = new btGeneric6DofSpring2Constraint(*prevBody, *linkBody, pivotInA,pivotInB); fixed->setLinearLowerLimit(btVector3(0,0,0)); fixed->setLinearUpperLimit(btVector3(0,0,0)); fixed->setAngularLowerLimit(btVector3(0,0,0)); fixed->setAngularUpperLimit(btVector3(0,0,0)); con = fixed; } btAssert(con); if (con) { btJointFeedback* fb = new btJointFeedback(); m_jointFeedback.push_back(fb); con->setJointFeedback(fb); m_dynamicsWorld->addConstraint(con,true); } prevBody = linkBody; } } if (1) { btVector3 groundHalfExtents(1,1,0.2); groundHalfExtents[upAxis]=1.f; btBoxShape* box = new btBoxShape(groundHalfExtents); box->initializePolyhedralFeatures(); btTransform start; start.setIdentity(); btVector3 groundOrigin(-0.4f, 3.f, 0.f); btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f); btQuaternion groundOrn(btVector3(0,1,0),0.25*SIMD_PI); groundOrigin[upAxis] -=.5; groundOrigin[2]-=0.6; start.setOrigin(groundOrigin); // start.setRotation(groundOrn); btRigidBody* body = createRigidBody(0,start,box); body->setFriction(0); } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); }
void PhysicsServerSharedMemory::processClientCommands() { if (m_data->m_isConnected && m_data->m_testBlock1) { ///we ignore overflow of integer for now if (m_data->m_testBlock1->m_numClientCommands> m_data->m_testBlock1->m_numProcessedClientCommands) { //until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands btAssert(m_data->m_testBlock1->m_numClientCommands==m_data->m_testBlock1->m_numProcessedClientCommands+1); const SharedMemoryCommand& clientCmd =m_data->m_testBlock1->m_clientCommands[0]; m_data->m_testBlock1->m_numProcessedClientCommands++; //no timestamp yet int timeStamp = 0; //consume the command switch (clientCmd.m_type) { case CMD_SEND_BULLET_DATA_STREAM: { b3Printf("Processed CMD_SEND_BULLET_DATA_STREAM length %d",clientCmd.m_dataStreamArguments.m_streamChunkLength); btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); m_data->m_worldImporters.push_back(worldImporter); bool completedOk = worldImporter->loadFileFromMemory(m_data->m_testBlock1->m_bulletStreamDataClientToServer,clientCmd.m_dataStreamArguments.m_streamChunkLength); if (completedOk) { SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); m_data->submitServerStatus(status); } else { SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_FAILED,clientCmd.m_sequenceNumber,timeStamp); m_data->submitServerStatus(status); } break; } case CMD_LOAD_URDF: { //at the moment, we only load 1 urdf / robot if (m_data->m_urdfLinkNameMapper.size()) { SharedMemoryStatus& status = m_data->createServerStatus(CMD_URDF_LOADING_FAILED,clientCmd.m_sequenceNumber,timeStamp); m_data->submitServerStatus(status); break; } const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments; b3Printf("Processed CMD_LOAD_URDF:%s", urdfArgs.m_urdfFileName); btAssert((clientCmd.m_updateFlags&URDF_ARGS_FILE_NAME) !=0); btAssert(urdfArgs.m_urdfFileName); btVector3 initialPos(0,0,0); btQuaternion initialOrn(0,0,0,1); if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_POSITION) { initialPos[0] = urdfArgs.m_initialPosition[0]; initialPos[1] = urdfArgs.m_initialPosition[1]; initialPos[2] = urdfArgs.m_initialPosition[2]; } if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_ORIENTATION) { initialOrn[0] = urdfArgs.m_initialOrientation[0]; initialOrn[1] = urdfArgs.m_initialOrientation[1]; initialOrn[2] = urdfArgs.m_initialOrientation[2]; initialOrn[3] = urdfArgs.m_initialOrientation[3]; } bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? urdfArgs.m_useMultiBody : true; bool useFixedBase = (clientCmd.m_updateFlags & URDF_ARGS_USE_FIXED_BASE) ? urdfArgs.m_useFixedBase: false; //load the actual URDF and send a report: completed or failed bool completedOk = loadUrdf(urdfArgs.m_urdfFileName, initialPos,initialOrn, useMultiBody, useFixedBase); SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; if (completedOk) { if (m_data->m_urdfLinkNameMapper.size()) { serverCmd.m_dataStreamArguments.m_streamChunkLength = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize(); } m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); SharedMemoryStatus& status = m_data->createServerStatus(CMD_URDF_LOADING_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); m_data->submitServerStatus(status); } else { SharedMemoryStatus& status = m_data->createServerStatus(CMD_URDF_LOADING_FAILED,clientCmd.m_sequenceNumber,timeStamp); m_data->submitServerStatus(status); } break; } case CMD_CREATE_SENSOR: { b3Printf("Processed CMD_CREATE_SENSOR"); if (m_data->m_dynamicsWorld->getNumMultibodies()>0) { btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0); btAssert(mb); for (int i=0;i<clientCmd.m_createSensorArguments.m_numJointSensorChanges;i++) { int jointIndex = clientCmd.m_createSensorArguments.m_jointIndex[i]; if (clientCmd.m_createSensorArguments.m_enableJointForceSensor[i]) { if (mb->getLink(jointIndex).m_jointFeedback) { b3Warning("CMD_CREATE_SENSOR: sensor for joint [%d] already enabled", jointIndex); } else { btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback(); fb->m_reactionForces.setZero(); mb->getLink(jointIndex).m_jointFeedback = fb; m_data->m_multiBodyJointFeedbacks.push_back(fb); }; } else { if (mb->getLink(jointIndex).m_jointFeedback) { m_data->m_multiBodyJointFeedbacks.remove(mb->getLink(jointIndex).m_jointFeedback); delete mb->getLink(jointIndex).m_jointFeedback; mb->getLink(jointIndex).m_jointFeedback=0; } else { b3Warning("CMD_CREATE_SENSOR: cannot perform sensor removal request, no sensor on joint [%d]", jointIndex); }; } } } else { b3Warning("No btMultiBody in the world. btRigidBody/btTypedConstraint sensor not hooked up yet"); } #if 0 //todo(erwincoumans) here is some sample code to hook up a force/torque sensor for btTypedConstraint/btRigidBody /* for (int i=0;i<m_data->m_dynamicsWorld->getNumConstraints();i++) { btTypedConstraint* c = m_data->m_dynamicsWorld->getConstraint(i); btJointFeedback* fb = new btJointFeedback(); m_data->m_jointFeedbacks.push_back(fb); c->setJointFeedback(fb); } */ #endif SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); m_data->submitServerStatus(serverCmd); break; } case CMD_SEND_DESIRED_STATE: { b3Printf("Processed CMD_SEND_DESIRED_STATE"); if (m_data->m_dynamicsWorld->getNumMultibodies()>0) { btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0); btAssert(mb); switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) { case CONTROL_MODE_TORQUE: { b3Printf("Using CONTROL_MODE_TORQUE"); mb->clearForcesAndTorques(); int torqueIndex = 0; btVector3 f(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[0], clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[1], clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[2]); btVector3 t(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[3], clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[4], clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[5]); torqueIndex+=6; mb->addBaseForce(f); mb->addBaseTorque(t); for (int link=0;link<mb->getNumLinks();link++) { for (int dof=0;dof<mb->getLink(link).m_dofCount;dof++) { double torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex]; mb->addJointTorqueMultiDof(link,dof,torque); torqueIndex++; } } break; } case CONTROL_MODE_VELOCITY: { b3Printf("Using CONTROL_MODE_VELOCITY"); int numMotors = 0; //find the joint motors and apply the desired velocity and maximum force/torque if (m_data->m_dynamicsWorld->getNumMultibodies()>0) { btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0); int dofIndex = 6;//skip the 3 linear + 3 angular degree of freedom entries of the base for (int link=0;link<mb->getNumLinks();link++) { if (supportsJointMotor(mb,link)) { btMultiBodyJointMotor** motorPtr = m_data->m_multiBodyJointMotorMap[link]; if (motorPtr) { btMultiBodyJointMotor* motor = *motorPtr; btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex]; motor->setVelocityTarget(desiredVelocity); btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime; motor->setMaxAppliedImpulse(maxImp); numMotors++; } } dofIndex += mb->getLink(link).m_dofCount; } } break; } default: { b3Printf("m_controlMode not implemented yet"); break; } } } SharedMemoryStatus& status = m_data->createServerStatus(CMD_DESIRED_STATE_RECEIVED_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); m_data->submitServerStatus(status); break; } case CMD_REQUEST_ACTUAL_STATE: { b3Printf("Sending the actual state (Q,U)"); if (m_data->m_dynamicsWorld->getNumMultibodies()>0) { btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0); SharedMemoryStatus& serverCmd = m_data->createServerStatus(CMD_ACTUAL_STATE_UPDATE_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); serverCmd.m_sendActualStateArgs.m_bodyUniqueId = 0; int totalDegreeOfFreedomQ = 0; int totalDegreeOfFreedomU = 0; //always add the base, even for static (non-moving objects) //so that we can easily move the 'fixed' base when needed //do we don't use this conditional "if (!mb->hasFixedBase())" { btTransform tr; tr.setOrigin(mb->getBasePos()); tr.setRotation(mb->getWorldToBaseRot().inverse()); //base position in world space, carthesian serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0]; serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1]; serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2]; //base orientation, quaternion x,y,z,w, in world space, carthesian serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1]; serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2]; serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3]; totalDegreeOfFreedomQ +=7;//pos + quaternion //base linear velocity (in world space, carthesian) serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = mb->getBaseVel()[0]; serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = mb->getBaseVel()[1]; serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = mb->getBaseVel()[2]; //base angular velocity (in world space, carthesian) serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = mb->getBaseOmega()[0]; serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = mb->getBaseOmega()[1]; serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = mb->getBaseOmega()[2]; totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF } for (int l=0;l<mb->getNumLinks();l++) { for (int d=0;d<mb->getLink(l).m_posVarCount;d++) { serverCmd.m_sendActualStateArgs.m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d]; } for (int d=0;d<mb->getLink(l).m_dofCount;d++) { serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d]; } if (0 == mb->getLink(l).m_jointFeedback) { for (int d=0;d<6;d++) { serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+d]=0; } } else { btVector3 sensedForce = mb->getLink(l).m_jointFeedback->m_reactionForces.getLinear(); btVector3 sensedTorque = mb->getLink(l).m_jointFeedback->m_reactionForces.getLinear(); serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+0] = sensedForce[0]; serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+1] = sensedForce[1]; serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+2] = sensedForce[2]; serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+3] = sensedTorque[0]; serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+4] = sensedTorque[1]; serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+5] = sensedTorque[2]; } } serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; m_data->submitServerStatus(serverCmd); } else { b3Warning("Request state but no multibody available"); SharedMemoryStatus& serverCmd = m_data->createServerStatus(CMD_ACTUAL_STATE_UPDATE_FAILED,clientCmd.m_sequenceNumber,timeStamp); m_data->submitServerStatus(serverCmd); } break; } case CMD_STEP_FORWARD_SIMULATION: { b3Printf("Step simulation request"); m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime); SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_STEP_FORWARD_SIMULATION_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); m_data->submitServerStatus(serverCmd); break; } case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: { if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_GRAVITY) { btVector3 grav(clientCmd.m_physSimParamArgs.m_gravityAcceleration[0], clientCmd.m_physSimParamArgs.m_gravityAcceleration[1], clientCmd.m_physSimParamArgs.m_gravityAcceleration[2]); this->m_data->m_dynamicsWorld->setGravity(grav); b3Printf("Updated Gravity: %f,%f,%f",grav[0],grav[1],grav[2]); } SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); m_data->submitServerStatus(serverCmd); break; }; case CMD_INIT_POSE: { b3Printf("Server Init Pose not implemented yet"); ///@todo: implement this m_data->m_dynamicsWorld->setGravity(btVector3(0,0,0)); SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); m_data->submitServerStatus(serverCmd); break; } case CMD_RESET_SIMULATION: { //clean up all data m_data->m_guiHelper->getRenderInterface()->removeAllInstances(); deleteDynamicsWorld(); createEmptyDynamicsWorld(); SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); m_data->submitServerStatus(serverCmd); break; } case CMD_CREATE_BOX_COLLISION_SHAPE: { btVector3 halfExtents(1,1,1); if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_HALF_EXTENTS) { halfExtents = btVector3( clientCmd.m_createBoxShapeArguments.m_halfExtentsX, clientCmd.m_createBoxShapeArguments.m_halfExtentsY, clientCmd.m_createBoxShapeArguments.m_halfExtentsZ); } btTransform startTrans; startTrans.setIdentity(); if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_POSITION) { startTrans.setOrigin(btVector3( clientCmd.m_createBoxShapeArguments.m_initialPosition[0], clientCmd.m_createBoxShapeArguments.m_initialPosition[1], clientCmd.m_createBoxShapeArguments.m_initialPosition[2])); } if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_ORIENTATION) { b3Printf("test\n"); startTrans.setRotation(btQuaternion( clientCmd.m_createBoxShapeArguments.m_initialOrientation[0], clientCmd.m_createBoxShapeArguments.m_initialOrientation[1], clientCmd.m_createBoxShapeArguments.m_initialOrientation[2], clientCmd.m_createBoxShapeArguments.m_initialOrientation[3])); } btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); m_data->m_worldImporters.push_back(worldImporter); btCollisionShape* shape = worldImporter->createBoxShape(halfExtents); btScalar mass = 0.f; bool isDynamic = (mass>0); worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0); m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); m_data->submitServerStatus(serverCmd); break; } default: { b3Error("Unknown command encountered"); SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_UNKNOWN_COMMAND_FLUSHED,clientCmd.m_sequenceNumber,timeStamp); m_data->submitServerStatus(serverCmd); } }; } } }
void GyroscopicSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) { gfxBridge.setUpAxis(2); createEmptyDynamicsWorld(); m_dynamicsWorld->setGravity(btVector3(0, 0, 0)); gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld); btVector3 positions[5] = { btVector3( -10, 8,4), btVector3( -5, 8,4), btVector3( 0, 8,4), btVector3( 5, 8,4), btVector3( 10, 8,4), }; for (int i = 0; i<5; i++) { btCylinderShapeZ* pin = new btCylinderShapeZ(btVector3(0.1,0.1, 0.2)); btBoxShape* box = new btBoxShape(btVector3(1,0.1,0.1)); box->setMargin(0.01); pin->setMargin(0.01); btCompoundShape* compound = new btCompoundShape(); compound->addChildShape(btTransform::getIdentity(), pin); btTransform offsetBox(btMatrix3x3::getIdentity(),btVector3(0,0,0.2)); compound->addChildShape(offsetBox, box); btScalar masses[2] = {0.3,0.1}; btVector3 localInertia; btTransform principal; compound->calculatePrincipalAxisTransform(masses,principal,localInertia); btRigidBody* body = new btRigidBody(1, 0, compound, localInertia); btTransform tr; tr.setIdentity(); tr.setOrigin(positions[i]); body->setCenterOfMassTransform(tr); body->setAngularVelocity(btVector3(0, 0.1, 10));//51)); //body->setLinearVelocity(btVector3(3, 0, 0)); body->setFriction(btSqrt(1)); m_dynamicsWorld->addRigidBody(body); body->setFlags(gyroflags[i]); m_dynamicsWorld->getSolverInfo().m_maxGyroscopicForce = 10.f; body->setDamping(0.0000f, 0.000f); } { //btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(0.5))); btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0, 0, 1), 0); m_collisionShapes.push_back(groundShape); btTransform groundTransform; groundTransform.setIdentity(); groundTransform.setOrigin(btVector3(0, 0, 0)); btRigidBody* groundBody; groundBody = createRigidBody(0, groundTransform, groundShape); groundBody->setFriction(btSqrt(2)); } gfxBridge.autogenerateGraphicsObjects(m_dynamicsWorld); }