virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld) { //this check is to prevent a crash, in case we removed all graphics instances, but there are still physics objects. //the check will be obsolete, once we have a better/safer way of synchronizing physics->graphics transforms if ( m_childGuiHelper->getRenderInterface()->getTotalNumInstances()>0) { m_childGuiHelper->syncPhysicsToGraphics(rbWorld); } }
virtual void resetCamera() { float dist = 8; float pitch = -45; float yaw = 32; float targetPos[3]={-0.33,-0.72,4.5}; m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); }
Tutorial(GUIHelperInterface* guiHelper, int tutorialIndex) :m_app(guiHelper->getAppInterface()), m_guiHelper(guiHelper), m_tutorialIndex(tutorialIndex), m_stage(0), m_counter(0), m_timeSeriesCanvas0(0), m_timeSeriesCanvas1(0) { int numBodies = 1; m_app->setUpAxis(1); m_app->m_renderer->enableBlend(true); switch (m_tutorialIndex) { case TUT_VELOCITY: { numBodies=10; m_timeSeriesCanvas0 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,256,"Constant Velocity"); m_timeSeriesCanvas0 ->setupTimeSeries(2,60, 0); m_timeSeriesCanvas0->addDataSource("X position (m)", 255,0,0); m_timeSeriesCanvas0->addDataSource("X velocity (m/s)", 0,0,255); m_timeSeriesCanvas0->addDataSource("dX/dt (m/s)", 0,0,0); break; } case TUT_ACCELERATION: { numBodies=10; m_timeSeriesCanvas1 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,256,512,"Constant Acceleration"); m_timeSeriesCanvas1 ->setupTimeSeries(50,60, 0); m_timeSeriesCanvas1->addDataSource("Y position (m)", 255,0,0); m_timeSeriesCanvas1->addDataSource("Y velocity (m/s)", 0,0,255); m_timeSeriesCanvas1->addDataSource("dY/dt (m/s)", 0,0,0); break; } case TUT_COLLISION: { numBodies=2; m_timeSeriesCanvas1 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,200,"Distance"); m_timeSeriesCanvas1 ->setupTimeSeries(1.5,60, 0); m_timeSeriesCanvas1->addDataSource("distance", 255,0,0); break; } case TUT_SOLVE_CONTACT_CONSTRAINT: { numBodies=2; m_timeSeriesCanvas1 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,200,"Collision Impulse"); m_timeSeriesCanvas1 ->setupTimeSeries(1.5,60, 0); m_timeSeriesCanvas1->addDataSource("Distance", 0,0,255); m_timeSeriesCanvas1->addDataSource("Impulse magnutide", 255,0,0); { SliderParams slider("Restitution",&gRestitution); slider.m_minVal=0; slider.m_maxVal=1; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { SliderParams slider("Mass A",&gMassA); slider.m_minVal=0; slider.m_maxVal=100; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { SliderParams slider("Mass B",&gMassB); slider.m_minVal=0; slider.m_maxVal=100; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } break; } default: { m_timeSeriesCanvas0 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,256,"Unknown"); m_timeSeriesCanvas0 ->setupTimeSeries(1,60, 0); } }; if (m_tutorialIndex==TUT_VELOCITY) { int boxId = m_app->registerCubeShape(100,1,100); b3Vector3 pos = b3MakeVector3(0,-3.5,0); b3Quaternion orn(0,0,0,1); b3Vector4 color = b3MakeVector4(1,1,1,1); b3Vector3 scaling = b3MakeVector3(1,1,1); m_app->m_renderer->registerGraphicsInstance(boxId,pos,orn,color,scaling); } for (int i=0;i<numBodies;i++) { m_bodies.push_back(new LWRigidBody()); } for (int i=0;i<m_bodies.size();i++) { m_bodies[i]->m_worldPose.m_position.setValue((i/4)*5,3,(i&3)*5); } { int textureIndex = -1; if (1) { int width,height,n; const char* filename = "data/cube.png"; const unsigned char* image=0; const char* prefix[]={"./","../","../../","../../../","../../../../"}; int numprefix = sizeof(prefix)/sizeof(const char*); for (int i=0;!image && i<numprefix;i++) { char relativeFileName[1024]; sprintf(relativeFileName,"%s%s",prefix[i],filename); image = stbi_load(relativeFileName, &width, &height, &n, 0); } b3Assert(image); if (image) { textureIndex = m_app->m_renderer->registerTexture(image,width,height); } } // int boxId = m_app->registerCubeShape(1,1,1,textureIndex); int boxId = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_HIGH, textureIndex); b3Vector4 color = b3MakeVector4(1,1,1,0.8); b3Vector3 scaling = b3MakeVector3(SPHERE_RADIUS,SPHERE_RADIUS,SPHERE_RADIUS); for (int i=0;i<m_bodies.size();i++) { m_bodies[i]->m_collisionShape.m_sphere.m_radius = SPHERE_RADIUS; m_bodies[i]->m_collisionShape.m_type = LW_SPHERE_TYPE; m_bodies[i]->m_graphicsIndex = m_app->m_renderer->registerGraphicsInstance(boxId,m_bodies[i]->m_worldPose.m_position, m_bodies[i]->m_worldPose.m_orientation,color,scaling); m_app->m_renderer->writeSingleInstanceTransformToCPU(m_bodies[i]->m_worldPose.m_position, m_bodies[i]->m_worldPose.m_orientation, m_bodies[i]->m_graphicsIndex); } } if (m_tutorialIndex == TUT_SOLVE_CONTACT_CONSTRAINT) { m_bodies[0]->m_invMass = gMassA? 1./gMassA : 0; m_bodies[0]->m_collisionShape.m_sphere.computeLocalInertia(gMassA,m_bodies[0]->m_localInertia); m_bodies[1]->m_invMass =gMassB? 1./gMassB : 0; m_bodies[1]->m_collisionShape.m_sphere.computeLocalInertia(gMassB,m_bodies[1]->m_localInertia); if (gMassA) m_bodies[0]->m_linearVelocity.setValue(0,0,1); if (gMassB) m_bodies[1]->m_linearVelocity.setValue(0,0,-1); } m_app->m_renderer->writeTransforms(); }
CollisionTutorialBullet2(GUIHelperInterface* guiHelper, int tutorialIndex) :m_app(guiHelper->getAppInterface()), m_guiHelper(guiHelper), m_tutorialIndex(tutorialIndex), m_collisionSdkHandle(0), m_collisionWorldHandle(0), m_stage(0), m_counter(0), m_timeSeriesCanvas0(0) { gTotalPoints = 0; m_app->setUpAxis(1); m_app->m_renderer->enableBlend(true); switch (m_tutorialIndex) { case TUT_SPHERE_PLANE_RTB3: case TUT_SPHERE_PLANE_BULLET2: { if (m_tutorialIndex==TUT_SPHERE_PLANE_BULLET2) { m_collisionSdkHandle = plCreateBullet2CollisionSdk(); } else { #ifndef DISABLE_REAL_TIME_BULLET3_COLLISION_SDK m_collisionSdkHandle = plCreateRealTimeBullet3CollisionSdk(); #endif //DISABLE_REAL_TIME_BULLET3_COLLISION_SDK } if (m_collisionSdkHandle) { int maxNumObjsCapacity=1024; int maxNumShapesCapacity=1024; int maxNumPairsCapacity=16384; btAlignedObjectArray<plCollisionObjectHandle> colliders; m_collisionWorldHandle = plCreateCollisionWorld(m_collisionSdkHandle,maxNumObjsCapacity,maxNumShapesCapacity,maxNumPairsCapacity); //create objects, do query etc { float radius = 1.f; void* userPointer = 0; { for (int j=0;j<sNumCompounds;j++) { plCollisionShapeHandle compoundShape = plCreateCompoundShape(m_collisionSdkHandle,m_collisionWorldHandle); for (int i=0;i<sNumSpheres;i++) { btVector3 childPos(i*1.5,0,0); btQuaternion childOrn(0,0,0,1); btVector3 scaling(radius,radius,radius); plCollisionShapeHandle childShape = plCreateSphereShape(m_collisionSdkHandle, m_collisionWorldHandle,radius); plAddChildShape(m_collisionSdkHandle,m_collisionWorldHandle,compoundShape, childShape,childPos,childOrn); //m_guiHelper->createCollisionObjectGraphicsObject(colObj,color); } if (m_tutorialIndex==TUT_SPHERE_PLANE_BULLET2) { btCollisionShape* colShape = (btCollisionShape*) compoundShape; m_guiHelper->createCollisionShapeGraphicsObject(colShape); } else { } { btVector3 pos(j*sNumSpheres*1.5,-2.4,0); btQuaternion orn(0,0,0,1); plCollisionObjectHandle colObjHandle = plCreateCollisionObject(m_collisionSdkHandle,m_collisionWorldHandle,userPointer, -1,compoundShape,pos,orn); if (m_tutorialIndex==TUT_SPHERE_PLANE_BULLET2) { btCollisionObject* colObj = (btCollisionObject*) colObjHandle; btVector4 color=sColors[j&3]; m_guiHelper->createCollisionObjectGraphicsObject(colObj,color); colliders.push_back(colObjHandle); plAddCollisionObject(m_collisionSdkHandle, m_collisionWorldHandle,colObjHandle); } } } } } { plCollisionShapeHandle colShape = plCreatePlaneShape(m_collisionSdkHandle, m_collisionWorldHandle,0,1,0,-3.5); btVector3 pos(0,0,0); btQuaternion orn(0,0,0,1); void* userPointer = 0; plCollisionObjectHandle colObj = plCreateCollisionObject(m_collisionSdkHandle,m_collisionWorldHandle,userPointer, 0,colShape,pos,orn); colliders.push_back(colObj); plAddCollisionObject(m_collisionSdkHandle, m_collisionWorldHandle,colObj); } int numContacts = plCollide(m_collisionSdkHandle,m_collisionWorldHandle,colliders[0],colliders[1],pointsOut,sPointCapacity); printf("numContacts = %d\n", numContacts); void* myUserPtr = 0; plWorldCollide(m_collisionSdkHandle,m_collisionWorldHandle,myNearCallback, myUserPtr); printf("total points=%d\n",gTotalPoints); //plRemoveCollisionObject(m_collisionSdkHandle,m_collisionWorldHandle,colObj); //plDeleteCollisionObject(m_collisionSdkHandle,colObj); //plDeleteShape(m_collisionSdkHandle,colShape); } /* m_timeSeriesCanvas0 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,256,"Constant Velocity"); m_timeSeriesCanvas0 ->setupTimeSeries(2,60, 0); m_timeSeriesCanvas0->addDataSource("X position (m)", 255,0,0); m_timeSeriesCanvas0->addDataSource("X velocity (m/s)", 0,0,255); m_timeSeriesCanvas0->addDataSource("dX/dt (m/s)", 0,0,0); */ break; } default: { m_timeSeriesCanvas0 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,256,"Unknown"); m_timeSeriesCanvas0 ->setupTimeSeries(1,60, 0); } }; { int boxId = m_app->registerCubeShape(100,0.01,100); b3Vector3 pos = b3MakeVector3(0,-3.5,0); b3Quaternion orn(0,0,0,1); b3Vector4 color = b3MakeVector4(1,1,1,1); b3Vector3 scaling = b3MakeVector3(1,1,1); m_app->m_renderer->registerGraphicsInstance(boxId,pos,orn,color,scaling); } { int textureIndex = -1; if (1) { int width,height,n; const char* filename = "data/cube.png"; const unsigned char* image=0; const char* prefix[]={"./","../","../../","../../../","../../../../"}; int numprefix = sizeof(prefix)/sizeof(const char*); for (int i=0;!image && i<numprefix;i++) { char relativeFileName[1024]; sprintf(relativeFileName,"%s%s",prefix[i],filename); image = stbi_load(relativeFileName, &width, &height, &n, 3); } b3Assert(image); if (image) { textureIndex = m_app->m_renderer->registerTexture(image,width,height); } } } m_app->m_renderer->writeTransforms(); }
bool Hinge2Vehicle::keyboardCallback(int key, int state) { bool handled = false; bool isShiftPressed = m_guiHelper->getAppInterface()->m_window->isModifierKeyPressed(B3G_SHIFT); if (state) { if (isShiftPressed) { switch (key) { case B3G_LEFT_ARROW : { m_liftHinge->setLimit(-M_PI/16.0f, M_PI/8.0f); m_liftHinge->enableAngularMotor(true, -0.1, maxMotorImpulse); handled = true; break; } case B3G_RIGHT_ARROW : { m_liftHinge->setLimit(-M_PI/16.0f, M_PI/8.0f); m_liftHinge->enableAngularMotor(true, 0.1, maxMotorImpulse); handled = true; break; } case B3G_UP_ARROW : { m_forkSlider->setLowerLinLimit(0.1f); m_forkSlider->setUpperLinLimit(3.9f); m_forkSlider->setPoweredLinMotor(true); m_forkSlider->setMaxLinMotorForce(maxMotorImpulse); m_forkSlider->setTargetLinMotorVelocity(1.0); handled = true; break; } case B3G_DOWN_ARROW : { m_forkSlider->setLowerLinLimit(0.1f); m_forkSlider->setUpperLinLimit(3.9f); m_forkSlider->setPoweredLinMotor(true); m_forkSlider->setMaxLinMotorForce(maxMotorImpulse); m_forkSlider->setTargetLinMotorVelocity(-1.0); handled = true; break; } } } else { switch (key) { case B3G_LEFT_ARROW : { handled = true; gVehicleSteering += steeringIncrement; if ( gVehicleSteering > steeringClamp) gVehicleSteering = steeringClamp; break; } case B3G_RIGHT_ARROW : { handled = true; gVehicleSteering -= steeringIncrement; if ( gVehicleSteering < -steeringClamp) gVehicleSteering = -steeringClamp; break; } case B3G_UP_ARROW : { handled = true; gEngineForce = maxEngineForce; gBreakingForce = 0.f; break; } case B3G_DOWN_ARROW : { handled = true; gEngineForce = -maxEngineForce; gBreakingForce = 0.f; break; } case B3G_F7: { handled = true; btDiscreteDynamicsWorld* world = (btDiscreteDynamicsWorld*)m_dynamicsWorld; world->setLatencyMotionStateInterpolation(!world->getLatencyMotionStateInterpolation()); printf("world latencyMotionStateInterpolation = %d\n", world->getLatencyMotionStateInterpolation()); break; } case B3G_F6: { handled = true; //switch solver (needs demo restart) useMCLPSolver = !useMCLPSolver; printf("switching to useMLCPSolver = %d\n", useMCLPSolver); delete m_solver; if (useMCLPSolver) { btDantzigSolver* mlcp = new btDantzigSolver(); //btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel; btMLCPSolver* sol = new btMLCPSolver(mlcp); m_solver = sol; } else { m_solver = new btSequentialImpulseConstraintSolver(); } m_dynamicsWorld->setConstraintSolver(m_solver); //exitPhysics(); //initPhysics(); break; } case B3G_F5: handled = true; m_useDefaultCamera = !m_useDefaultCamera; break; default: break; } } } else { switch (key) { case B3G_UP_ARROW: { lockForkSlider(); gEngineForce = 0.f; gBreakingForce = defaultBreakingForce; handled=true; break; } case B3G_DOWN_ARROW: { lockForkSlider(); gEngineForce = 0.f; gBreakingForce = defaultBreakingForce; handled=true; break; } case B3G_LEFT_ARROW: case B3G_RIGHT_ARROW: { lockLiftHinge(); handled=true; break; } default: break; } } return handled; }
//to be implemented by the demo void Hinge2Vehicle::renderScene() { m_guiHelper->syncPhysicsToGraphics(m_dynamicsWorld); #if 0 for (int i=0;i<m_vehicle->getNumWheels();i++) { //synchronize the wheels with the (interpolated) chassis worldtransform m_vehicle->updateWheelTransform(i,true); CommonRenderInterface* renderer = m_guiHelper->getRenderInterface(); if (renderer) { btTransform tr = m_vehicle->getWheelInfo(i).m_worldTransform; btVector3 pos=tr.getOrigin(); btQuaternion orn = tr.getRotation(); renderer->writeSingleInstanceTransformToCPU(pos,orn,m_wheelInstances[i]); } } #endif m_guiHelper->render(m_dynamicsWorld); btVector3 wheelColor(1,0,0); btVector3 worldBoundsMin,worldBoundsMax; getDynamicsWorld()->getBroadphase()->getBroadphaseAabb(worldBoundsMin,worldBoundsMax); #if 0 int lineWidth=400; int xStart = m_glutScreenWidth - lineWidth; int yStart = 20; if((getDebugMode() & btIDebugDraw::DBG_NoHelpText)==0) { setOrthographicProjection(); glDisable(GL_LIGHTING); glColor3f(0, 0, 0); char buf[124]; sprintf(buf,"SHIFT+Cursor Left/Right - rotate lift"); GLDebugDrawString(xStart,20,buf); yStart+=20; sprintf(buf,"SHIFT+Cursor UP/Down - fork up/down"); yStart+=20; GLDebugDrawString(xStart,yStart,buf); if (m_useDefaultCamera) { sprintf(buf,"F5 - camera mode (free)"); } else { sprintf(buf,"F5 - camera mode (follow)"); } yStart+=20; GLDebugDrawString(xStart,yStart,buf); yStart+=20; if (m_dynamicsWorld->getConstraintSolver()->getSolverType()==BT_MLCP_SOLVER) { sprintf(buf,"F6 - solver (direct MLCP)"); } else { sprintf(buf,"F6 - solver (sequential impulse)"); } GLDebugDrawString(xStart,yStart,buf); btDiscreteDynamicsWorld* world = (btDiscreteDynamicsWorld*) m_dynamicsWorld; if (world->getLatencyMotionStateInterpolation()) { sprintf(buf,"F7 - motionstate interpolation (on)"); } else { sprintf(buf,"F7 - motionstate interpolation (off)"); } yStart+=20; GLDebugDrawString(xStart,yStart,buf); sprintf(buf,"Click window for keyboard focus"); yStart+=20; GLDebugDrawString(xStart,yStart,buf); resetPerspectiveProjection(); glEnable(GL_LIGHTING); } #endif }
void Hinge2Vehicle::initPhysics() { m_guiHelper->setUpAxis(1); btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50)); m_collisionShapes.push_back(groundShape); m_collisionConfiguration = new btDefaultCollisionConfiguration(); m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); btVector3 worldMin(-1000,-1000,-1000); btVector3 worldMax(1000,1000,1000); m_broadphase = new btAxisSweep3(worldMin,worldMax); if (useMCLPSolver) { btDantzigSolver* mlcp = new btDantzigSolver(); //btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel; btMLCPSolver* sol = new btMLCPSolver(mlcp); m_solver = sol; } else { m_solver = new btSequentialImpulseConstraintSolver(); } m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration); if (useMCLPSolver) { m_dynamicsWorld ->getSolverInfo().m_minimumSolverBatchSize = 1;//for direct solver it is better to have a small A matrix } else { m_dynamicsWorld ->getSolverInfo().m_minimumSolverBatchSize = 128;//for direct solver, it is better to solve multiple objects together, small batches have high overhead } m_dynamicsWorld->getSolverInfo().m_numIterations = 100; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); //m_dynamicsWorld->setGravity(btVector3(0,0,0)); btTransform tr; tr.setIdentity(); tr.setOrigin(btVector3(0,-3,0)); //either use heightfield or triangle mesh //create ground object localCreateRigidBody(0,tr,groundShape); btCollisionShape* chassisShape = new btBoxShape(btVector3(1.f,0.5f,2.f)); m_collisionShapes.push_back(chassisShape); btCompoundShape* compound = new btCompoundShape(); m_collisionShapes.push_back(compound); btTransform localTrans; localTrans.setIdentity(); //localTrans effectively shifts the center of mass with respect to the chassis localTrans.setOrigin(btVector3(0,1,0)); compound->addChildShape(localTrans,chassisShape); { btCollisionShape* suppShape = new btBoxShape(btVector3(0.5f,0.1f,0.5f)); btTransform suppLocalTrans; suppLocalTrans.setIdentity(); //localTrans effectively shifts the center of mass with respect to the chassis suppLocalTrans.setOrigin(btVector3(0,1.0,2.5)); compound->addChildShape(suppLocalTrans, suppShape); } tr.setOrigin(btVector3(0,0.f,0)); btScalar chassisMass = 800; m_carChassis = localCreateRigidBody(chassisMass,tr,compound);//chassisShape); //m_carChassis->setDamping(0.2,0.2); //m_wheelShape = new btCylinderShapeX(btVector3(wheelWidth,wheelRadius,wheelRadius)); m_wheelShape = new btCylinderShapeX(btVector3(wheelWidth,wheelRadius,wheelRadius)); //const float position[4]={0,10,10,0}; //const float quaternion[4]={0,0,0,1}; //const float color[4]={0,1,0,1}; //const float scaling[4] = {1,1,1,1}; btVector3 wheelPos[4] = { btVector3(btScalar(-1.), btScalar(-0.25), btScalar(1.25)), btVector3(btScalar(1.), btScalar(-0.25), btScalar(1.25)), btVector3(btScalar(1.), btScalar(-0.25), btScalar(-1.25)), btVector3(btScalar(-1.), btScalar(-0.25), btScalar(-1.25)) }; for (int i=0;i<4;i++) { // create a Hinge2 joint // create two rigid bodies // static bodyA (parent) on top: btRigidBody* pBodyA = this->m_carChassis;//m_chassis;//createRigidBody( 0.0, tr, m_wheelShape); pBodyA->setActivationState(DISABLE_DEACTIVATION); // dynamic bodyB (child) below it : btTransform tr; tr.setIdentity(); tr.setOrigin(wheelPos[i]); btRigidBody* pBodyB = createRigidBody(10.0, tr, m_wheelShape); pBodyB->setFriction(1110); pBodyB->setActivationState(DISABLE_DEACTIVATION); // add some data to build constraint frames btVector3 parentAxis(0.f, 1.f, 0.f); btVector3 childAxis(1.f, 0.f, 0.f); btVector3 anchor = tr.getOrigin();//(0.f, 0.f, 0.f); btHinge2Constraint* pHinge2 = new btHinge2Constraint(*pBodyA, *pBodyB, anchor, parentAxis, childAxis); //m_guiHelper->get2dCanvasInterface(); pHinge2->setLowerLimit(-SIMD_HALF_PI * 0.5f); pHinge2->setUpperLimit( SIMD_HALF_PI * 0.5f); // add constraint to world m_dynamicsWorld->addConstraint(pHinge2, true); // draw constraint frames and limits for debugging { int motorAxis = 3; pHinge2->enableMotor(motorAxis,true); pHinge2->setMaxMotorForce(motorAxis,1000); pHinge2->setTargetVelocity(motorAxis,-1); } { int motorAxis = 5; pHinge2->enableMotor(motorAxis,true); pHinge2->setMaxMotorForce(motorAxis,1000); pHinge2->setTargetVelocity(motorAxis,0); } pHinge2->setDbgDrawSize(btScalar(5.f)); } { btCollisionShape* liftShape = new btBoxShape(btVector3(0.5f,2.0f,0.05f)); m_collisionShapes.push_back(liftShape); btTransform liftTrans; m_liftStartPos = btVector3(0.0f, 2.5f, 3.05f); liftTrans.setIdentity(); liftTrans.setOrigin(m_liftStartPos); m_liftBody = localCreateRigidBody(10,liftTrans, liftShape); btTransform localA, localB; localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0, M_PI_2, 0); localA.setOrigin(btVector3(0.0, 1.0, 3.05)); localB.getBasis().setEulerZYX(0, M_PI_2, 0); localB.setOrigin(btVector3(0.0, -1.5, -0.05)); m_liftHinge = new btHingeConstraint(*m_carChassis,*m_liftBody, localA, localB); // m_liftHinge->setLimit(-LIFT_EPS, LIFT_EPS); m_liftHinge->setLimit(0.0f, 0.0f); m_dynamicsWorld->addConstraint(m_liftHinge, true); btCollisionShape* forkShapeA = new btBoxShape(btVector3(1.0f,0.1f,0.1f)); m_collisionShapes.push_back(forkShapeA); btCompoundShape* forkCompound = new btCompoundShape(); m_collisionShapes.push_back(forkCompound); btTransform forkLocalTrans; forkLocalTrans.setIdentity(); forkCompound->addChildShape(forkLocalTrans, forkShapeA); btCollisionShape* forkShapeB = new btBoxShape(btVector3(0.1f,0.02f,0.6f)); m_collisionShapes.push_back(forkShapeB); forkLocalTrans.setIdentity(); forkLocalTrans.setOrigin(btVector3(-0.9f, -0.08f, 0.7f)); forkCompound->addChildShape(forkLocalTrans, forkShapeB); btCollisionShape* forkShapeC = new btBoxShape(btVector3(0.1f,0.02f,0.6f)); m_collisionShapes.push_back(forkShapeC); forkLocalTrans.setIdentity(); forkLocalTrans.setOrigin(btVector3(0.9f, -0.08f, 0.7f)); forkCompound->addChildShape(forkLocalTrans, forkShapeC); btTransform forkTrans; m_forkStartPos = btVector3(0.0f, 0.6f, 3.2f); forkTrans.setIdentity(); forkTrans.setOrigin(m_forkStartPos); m_forkBody = localCreateRigidBody(5, forkTrans, forkCompound); localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0, 0, M_PI_2); localA.setOrigin(btVector3(0.0f, -1.9f, 0.05f)); localB.getBasis().setEulerZYX(0, 0, M_PI_2); localB.setOrigin(btVector3(0.0, 0.0, -0.1)); m_forkSlider = new btSliderConstraint(*m_liftBody, *m_forkBody, localA, localB, true); m_forkSlider->setLowerLinLimit(0.1f); m_forkSlider->setUpperLinLimit(0.1f); // m_forkSlider->setLowerAngLimit(-LIFT_EPS); // m_forkSlider->setUpperAngLimit(LIFT_EPS); m_forkSlider->setLowerAngLimit(0.0f); m_forkSlider->setUpperAngLimit(0.0f); m_dynamicsWorld->addConstraint(m_forkSlider, true); btCompoundShape* loadCompound = new btCompoundShape(); m_collisionShapes.push_back(loadCompound); btCollisionShape* loadShapeA = new btBoxShape(btVector3(2.0f,0.5f,0.5f)); m_collisionShapes.push_back(loadShapeA); btTransform loadTrans; loadTrans.setIdentity(); loadCompound->addChildShape(loadTrans, loadShapeA); btCollisionShape* loadShapeB = new btBoxShape(btVector3(0.1f,1.0f,1.0f)); m_collisionShapes.push_back(loadShapeB); loadTrans.setIdentity(); loadTrans.setOrigin(btVector3(2.1f, 0.0f, 0.0f)); loadCompound->addChildShape(loadTrans, loadShapeB); btCollisionShape* loadShapeC = new btBoxShape(btVector3(0.1f,1.0f,1.0f)); m_collisionShapes.push_back(loadShapeC); loadTrans.setIdentity(); loadTrans.setOrigin(btVector3(-2.1f, 0.0f, 0.0f)); loadCompound->addChildShape(loadTrans, loadShapeC); loadTrans.setIdentity(); m_loadStartPos = btVector3(0.0f, 3.5f, 7.0f); loadTrans.setOrigin(m_loadStartPos); m_loadBody = localCreateRigidBody(loadMass, loadTrans, loadCompound); } resetForklift(); // setCameraDistance(26.f); m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); }
virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ) { m_childGuiHelper->resetCamera(camDist,pitch,yaw,camPosX,camPosY,camPosZ); }
virtual void setUpAxis(int axis) { m_childGuiHelper->setUpAxis(axis); }
virtual CommonGraphicsApp* getAppInterface() { return m_childGuiHelper->getAppInterface(); }
virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld) { m_childGuiHelper->createPhysicsDebugDrawer(rbWorld); }
virtual void render(const btDiscreteDynamicsWorld* rbWorld) { m_childGuiHelper->render(0); }
void ForkLiftDemo::initPhysics() { int upAxis = 1; m_guiHelper->setUpAxis(upAxis); btVector3 groundExtents(50,50,50); groundExtents[upAxis]=3; btCollisionShape* groundShape = new btBoxShape(groundExtents); m_collisionShapes.push_back(groundShape); m_collisionConfiguration = new btDefaultCollisionConfiguration(); m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); btVector3 worldMin(-1000,-1000,-1000); btVector3 worldMax(1000,1000,1000); m_overlappingPairCache = new btAxisSweep3(worldMin,worldMax); if (useMCLPSolver) { btDantzigSolver* mlcp = new btDantzigSolver(); //btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel; btMLCPSolver* sol = new btMLCPSolver(mlcp); m_constraintSolver = sol; } else { m_constraintSolver = new btSequentialImpulseConstraintSolver(); } m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_constraintSolver,m_collisionConfiguration); if (useMCLPSolver) { m_dynamicsWorld ->getSolverInfo().m_minimumSolverBatchSize = 1;//for direct solver it is better to have a small A matrix } else { m_dynamicsWorld ->getSolverInfo().m_minimumSolverBatchSize = 128;//for direct solver, it is better to solve multiple objects together, small batches have high overhead } m_dynamicsWorld->getSolverInfo().m_globalCfm = 0.00001; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); //m_dynamicsWorld->setGravity(btVector3(0,0,0)); btTransform tr; tr.setIdentity(); tr.setOrigin(btVector3(0,-3,0)); //either use heightfield or triangle mesh //create ground object localCreateRigidBody(0,tr,groundShape); btCollisionShape* chassisShape = new btBoxShape(btVector3(1.f,0.5f,2.f)); m_collisionShapes.push_back(chassisShape); btCompoundShape* compound = new btCompoundShape(); m_collisionShapes.push_back(compound); btTransform localTrans; localTrans.setIdentity(); //localTrans effectively shifts the center of mass with respect to the chassis localTrans.setOrigin(btVector3(0,1,0)); compound->addChildShape(localTrans,chassisShape); { btCollisionShape* suppShape = new btBoxShape(btVector3(0.5f,0.1f,0.5f)); btTransform suppLocalTrans; suppLocalTrans.setIdentity(); //localTrans effectively shifts the center of mass with respect to the chassis suppLocalTrans.setOrigin(btVector3(0,1.0,2.5)); compound->addChildShape(suppLocalTrans, suppShape); } tr.setOrigin(btVector3(0,0.f,0)); m_carChassis = localCreateRigidBody(800,tr,compound);//chassisShape); //m_carChassis->setDamping(0.2,0.2); m_wheelShape = new btCylinderShapeX(btVector3(wheelWidth,wheelRadius,wheelRadius)); m_guiHelper->createCollisionShapeGraphicsObject(m_wheelShape); int wheelGraphicsIndex = m_wheelShape->getUserIndex(); const float position[4]={0,10,10,0}; const float quaternion[4]={0,0,0,1}; const float color[4]={0,1,0,1}; const float scaling[4] = {1,1,1,1}; for (int i=0;i<4;i++) { m_wheelInstances[i] = m_guiHelper->registerGraphicsInstance(wheelGraphicsIndex, position, quaternion, color, scaling); } { btCollisionShape* liftShape = new btBoxShape(btVector3(0.5f,2.0f,0.05f)); m_collisionShapes.push_back(liftShape); btTransform liftTrans; m_liftStartPos = btVector3(0.0f, 2.5f, 3.05f); liftTrans.setIdentity(); liftTrans.setOrigin(m_liftStartPos); m_liftBody = localCreateRigidBody(10,liftTrans, liftShape); btTransform localA, localB; localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0, M_PI_2, 0); localA.setOrigin(btVector3(0.0, 1.0, 3.05)); localB.getBasis().setEulerZYX(0, M_PI_2, 0); localB.setOrigin(btVector3(0.0, -1.5, -0.05)); m_liftHinge = new btHingeConstraint(*m_carChassis,*m_liftBody, localA, localB); // m_liftHinge->setLimit(-LIFT_EPS, LIFT_EPS); m_liftHinge->setLimit(0.0f, 0.0f); m_dynamicsWorld->addConstraint(m_liftHinge, true); btCollisionShape* forkShapeA = new btBoxShape(btVector3(1.0f,0.1f,0.1f)); m_collisionShapes.push_back(forkShapeA); btCompoundShape* forkCompound = new btCompoundShape(); m_collisionShapes.push_back(forkCompound); btTransform forkLocalTrans; forkLocalTrans.setIdentity(); forkCompound->addChildShape(forkLocalTrans, forkShapeA); btCollisionShape* forkShapeB = new btBoxShape(btVector3(0.1f,0.02f,0.6f)); m_collisionShapes.push_back(forkShapeB); forkLocalTrans.setIdentity(); forkLocalTrans.setOrigin(btVector3(-0.9f, -0.08f, 0.7f)); forkCompound->addChildShape(forkLocalTrans, forkShapeB); btCollisionShape* forkShapeC = new btBoxShape(btVector3(0.1f,0.02f,0.6f)); m_collisionShapes.push_back(forkShapeC); forkLocalTrans.setIdentity(); forkLocalTrans.setOrigin(btVector3(0.9f, -0.08f, 0.7f)); forkCompound->addChildShape(forkLocalTrans, forkShapeC); btTransform forkTrans; m_forkStartPos = btVector3(0.0f, 0.6f, 3.2f); forkTrans.setIdentity(); forkTrans.setOrigin(m_forkStartPos); m_forkBody = localCreateRigidBody(5, forkTrans, forkCompound); localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0, 0, M_PI_2); localA.setOrigin(btVector3(0.0f, -1.9f, 0.05f)); localB.getBasis().setEulerZYX(0, 0, M_PI_2); localB.setOrigin(btVector3(0.0, 0.0, -0.1)); m_forkSlider = new btSliderConstraint(*m_liftBody, *m_forkBody, localA, localB, true); m_forkSlider->setLowerLinLimit(0.1f); m_forkSlider->setUpperLinLimit(0.1f); // m_forkSlider->setLowerAngLimit(-LIFT_EPS); // m_forkSlider->setUpperAngLimit(LIFT_EPS); m_forkSlider->setLowerAngLimit(0.0f); m_forkSlider->setUpperAngLimit(0.0f); m_dynamicsWorld->addConstraint(m_forkSlider, true); btCompoundShape* loadCompound = new btCompoundShape(); m_collisionShapes.push_back(loadCompound); btCollisionShape* loadShapeA = new btBoxShape(btVector3(2.0f,0.5f,0.5f)); m_collisionShapes.push_back(loadShapeA); btTransform loadTrans; loadTrans.setIdentity(); loadCompound->addChildShape(loadTrans, loadShapeA); btCollisionShape* loadShapeB = new btBoxShape(btVector3(0.1f,1.0f,1.0f)); m_collisionShapes.push_back(loadShapeB); loadTrans.setIdentity(); loadTrans.setOrigin(btVector3(2.1f, 0.0f, 0.0f)); loadCompound->addChildShape(loadTrans, loadShapeB); btCollisionShape* loadShapeC = new btBoxShape(btVector3(0.1f,1.0f,1.0f)); m_collisionShapes.push_back(loadShapeC); loadTrans.setIdentity(); loadTrans.setOrigin(btVector3(-2.1f, 0.0f, 0.0f)); loadCompound->addChildShape(loadTrans, loadShapeC); loadTrans.setIdentity(); m_loadStartPos = btVector3(0.0f, 3.5f, 7.0f); loadTrans.setOrigin(m_loadStartPos); m_loadBody = localCreateRigidBody(loadMass, loadTrans, loadCompound); } /// create vehicle { m_vehicleRayCaster = new btDefaultVehicleRaycaster(m_dynamicsWorld); m_vehicle = new btRaycastVehicle(m_tuning,m_carChassis,m_vehicleRayCaster); ///never deactivate the vehicle m_carChassis->setActivationState(DISABLE_DEACTIVATION); m_dynamicsWorld->addVehicle(m_vehicle); float connectionHeight = 1.2f; bool isFrontWheel=true; //choose coordinate system m_vehicle->setCoordinateSystem(rightIndex,upIndex,forwardIndex); btVector3 connectionPointCS0(CUBE_HALF_EXTENTS-(0.3*wheelWidth),connectionHeight,2*CUBE_HALF_EXTENTS-wheelRadius); m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel); connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),connectionHeight,2*CUBE_HALF_EXTENTS-wheelRadius); m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel); connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),connectionHeight,-2*CUBE_HALF_EXTENTS+wheelRadius); isFrontWheel = false; m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel); connectionPointCS0 = btVector3(CUBE_HALF_EXTENTS-(0.3*wheelWidth),connectionHeight,-2*CUBE_HALF_EXTENTS+wheelRadius); m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel); for (int i=0;i<m_vehicle->getNumWheels();i++) { btWheelInfo& wheel = m_vehicle->getWheelInfo(i); wheel.m_suspensionStiffness = suspensionStiffness; wheel.m_wheelsDampingRelaxation = suspensionDamping; wheel.m_wheelsDampingCompression = suspensionCompression; wheel.m_frictionSlip = wheelFriction; wheel.m_rollInfluence = rollInfluence; } } resetForklift(); // setCameraDistance(26.f); m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); }
virtual void initPhysics() { int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER; m_robotSim.setGuiHelper(m_guiHelper); bool connected = m_robotSim.connect(mode); b3Printf("robotSim connected = %d",connected); if ((m_options & eGRIPPER_GRASP)!=0) { { SliderParams slider("Vertical velocity",&sGripperVerticalVelocity); slider.m_minVal=-2; slider.m_maxVal=2; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { SliderParams slider("Closing velocity",&sGripperClosingTargetVelocity ); slider.m_minVal=-1; slider.m_maxVal=1; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { b3RobotSimulatorLoadUrdfFileArgs args; args.m_startPosition.setValue(0, 0, .107); args.m_startOrientation.setEulerZYX(0, 0, 0); args.m_useMultiBody = true; m_robotSim.loadURDF("cube_small.urdf", args); } { b3RobotSimulatorLoadFileResults results; m_robotSim.loadSDF("gripper/wsg50_with_r2d2_gripper.sdf",results); if (results.m_uniqueObjectIds.size()==1) { m_gripperIndex = results.m_uniqueObjectIds[0]; int numJoints = m_robotSim.getNumJoints(m_gripperIndex); b3Printf("numJoints = %d",numJoints); for (int i=0;i<numJoints;i++) { b3JointInfo jointInfo; m_robotSim.getJointInfo(m_gripperIndex,i,&jointInfo); b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName); } /* int fingerJointIndices[2]={1,3}; double fingerTargetPositions[2]={-0.04,0.04}; for (int i=0;i<2;i++) { b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD); controlArgs.m_targetPosition = fingerTargetPositions[i]; controlArgs.m_kp = 5.0; controlArgs.m_kd = 3.0; controlArgs.m_maxTorqueValue = 1.0; m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs); } */ int fingerJointIndices[3]={0,1,3}; double fingerTargetVelocities[3]={-0.2,.5,-.5}; double maxTorqueValues[3]={40.0,50.0,50.0}; for (int i=0;i<3;i++) { b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_targetVelocity = fingerTargetVelocities[i]; controlArgs.m_maxTorqueValue = maxTorqueValues[i]; controlArgs.m_kd = 1.; m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs); } } } if (1) { m_robotSim.loadURDF("plane.urdf"); } m_robotSim.setGravity(b3MakeVector3(0,0,-10)); m_robotSim.setNumSimulationSubSteps(4); } if ((m_options & eTWO_POINT_GRASP)!=0) { { b3RobotSimulatorLoadUrdfFileArgs args; args.m_startPosition.setValue(0, 0, .107); m_robotSim.loadURDF("cube_small.urdf", args); } { b3RobotSimulatorLoadUrdfFileArgs args; args.m_startPosition.setValue(0.068, 0.02, 0.11); m_robotSim.loadURDF("cube_gripper_left.urdf", args); b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_targetVelocity = -0.1; controlArgs.m_maxTorqueValue = 10.0; controlArgs.m_kd = 1.; m_robotSim.setJointMotorControl(1,0,controlArgs); } { b3RobotSimulatorLoadUrdfFileArgs args; args.m_startPosition.setValue(-0.068, 0.02, 0.11); m_robotSim.loadURDF("cube_gripper_right.urdf", args); b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_targetVelocity = 0.1; controlArgs.m_maxTorqueValue = 10.0; controlArgs.m_kd = 1.; m_robotSim.setJointMotorControl(2,0,controlArgs); } if (1) { m_robotSim.loadURDF("plane.urdf"); } m_robotSim.setGravity(b3MakeVector3(0,0,-10)); m_robotSim.setNumSimulationSubSteps(4); } if ((m_options & eONE_MOTOR_GRASP)!=0) { m_robotSim.setNumSolverIterations(150); { SliderParams slider("Vertical velocity",&sGripperVerticalVelocity); slider.m_minVal=-2; slider.m_maxVal=2; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { SliderParams slider("Closing velocity",&sGripperClosingTargetVelocity ); slider.m_minVal=-1; slider.m_maxVal=1; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { b3RobotSimulatorLoadUrdfFileArgs args; args.m_startPosition.setValue(0, -0.2, .47); args.m_startOrientation.setEulerZYX(SIMD_HALF_PI, 0, 0); m_robotSim.loadURDF("dinnerware/pan_tefal.urdf", args); } { b3RobotSimulatorLoadFileResults args; b3RobotSimulatorLoadFileResults results; m_robotSim.loadSDF("gripper/wsg50_one_motor_gripper_new.sdf",results); if (results.m_uniqueObjectIds.size()==1) { m_gripperIndex = results.m_uniqueObjectIds[0]; int numJoints = m_robotSim.getNumJoints(m_gripperIndex); b3Printf("numJoints = %d",numJoints); for (int i=0;i<numJoints;i++) { b3JointInfo jointInfo; m_robotSim.getJointInfo(m_gripperIndex,i,&jointInfo); b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName); } for (int i=0;i<8;i++) { b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_maxTorqueValue = 0.0; m_robotSim.setJointMotorControl(m_gripperIndex,i,controlArgs); } } } if (1) { m_robotSim.loadURDF("plane.urdf"); } m_robotSim.setGravity(b3MakeVector3(0,0,-10)); b3JointInfo revoluteJoint1; revoluteJoint1.m_parentFrame[0] = -0.055; revoluteJoint1.m_parentFrame[1] = 0; revoluteJoint1.m_parentFrame[2] = 0.02; revoluteJoint1.m_parentFrame[3] = 0; revoluteJoint1.m_parentFrame[4] = 0; revoluteJoint1.m_parentFrame[5] = 0; revoluteJoint1.m_parentFrame[6] = 1.0; revoluteJoint1.m_childFrame[0] = 0; revoluteJoint1.m_childFrame[1] = 0; revoluteJoint1.m_childFrame[2] = 0; revoluteJoint1.m_childFrame[3] = 0; revoluteJoint1.m_childFrame[4] = 0; revoluteJoint1.m_childFrame[5] = 0; revoluteJoint1.m_childFrame[6] = 1.0; revoluteJoint1.m_jointAxis[0] = 1.0; revoluteJoint1.m_jointAxis[1] = 0.0; revoluteJoint1.m_jointAxis[2] = 0.0; revoluteJoint1.m_jointType = ePoint2PointType; b3JointInfo revoluteJoint2; revoluteJoint2.m_parentFrame[0] = 0.055; revoluteJoint2.m_parentFrame[1] = 0; revoluteJoint2.m_parentFrame[2] = 0.02; revoluteJoint2.m_parentFrame[3] = 0; revoluteJoint2.m_parentFrame[4] = 0; revoluteJoint2.m_parentFrame[5] = 0; revoluteJoint2.m_parentFrame[6] = 1.0; revoluteJoint2.m_childFrame[0] = 0; revoluteJoint2.m_childFrame[1] = 0; revoluteJoint2.m_childFrame[2] = 0; revoluteJoint2.m_childFrame[3] = 0; revoluteJoint2.m_childFrame[4] = 0; revoluteJoint2.m_childFrame[5] = 0; revoluteJoint2.m_childFrame[6] = 1.0; revoluteJoint2.m_jointAxis[0] = 1.0; revoluteJoint2.m_jointAxis[1] = 0.0; revoluteJoint2.m_jointAxis[2] = 0.0; revoluteJoint2.m_jointType = ePoint2PointType; m_robotSim.createConstraint(1, 2, 1, 4, &revoluteJoint1); m_robotSim.createConstraint(1, 3, 1, 6, &revoluteJoint2); } if ((m_options & eGRASP_SOFT_BODY)!=0) { { SliderParams slider("Vertical velocity",&sGripperVerticalVelocity); slider.m_minVal=-2; slider.m_maxVal=2; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { SliderParams slider("Closing velocity",&sGripperClosingTargetVelocity ); slider.m_minVal=-1; slider.m_maxVal=1; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { b3RobotSimulatorLoadFileResults results; m_robotSim.loadSDF("gripper/wsg50_one_motor_gripper_new.sdf",results); if (results.m_uniqueObjectIds.size()==1) { m_gripperIndex = results.m_uniqueObjectIds[0]; int numJoints = m_robotSim.getNumJoints(m_gripperIndex); b3Printf("numJoints = %d",numJoints); for (int i=0;i<numJoints;i++) { b3JointInfo jointInfo; m_robotSim.getJointInfo(m_gripperIndex,i,&jointInfo); b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName); } for (int i=0;i<8;i++) { b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_maxTorqueValue = 0.0; m_robotSim.setJointMotorControl(m_gripperIndex,i,controlArgs); } } } { b3RobotSimulatorLoadUrdfFileArgs args; args.m_startPosition.setValue(0,0,-0.2); args.m_startOrientation.setEulerZYX(0,0,0); m_robotSim.loadURDF("plane.urdf", args); } m_robotSim.setGravity(b3MakeVector3(0,0,-10)); m_robotSim.loadBunny(0.1,0.1,0.02); b3JointInfo revoluteJoint1; revoluteJoint1.m_parentFrame[0] = -0.055; revoluteJoint1.m_parentFrame[1] = 0; revoluteJoint1.m_parentFrame[2] = 0.02; revoluteJoint1.m_parentFrame[3] = 0; revoluteJoint1.m_parentFrame[4] = 0; revoluteJoint1.m_parentFrame[5] = 0; revoluteJoint1.m_parentFrame[6] = 1.0; revoluteJoint1.m_childFrame[0] = 0; revoluteJoint1.m_childFrame[1] = 0; revoluteJoint1.m_childFrame[2] = 0; revoluteJoint1.m_childFrame[3] = 0; revoluteJoint1.m_childFrame[4] = 0; revoluteJoint1.m_childFrame[5] = 0; revoluteJoint1.m_childFrame[6] = 1.0; revoluteJoint1.m_jointAxis[0] = 1.0; revoluteJoint1.m_jointAxis[1] = 0.0; revoluteJoint1.m_jointAxis[2] = 0.0; revoluteJoint1.m_jointType = ePoint2PointType; b3JointInfo revoluteJoint2; revoluteJoint2.m_parentFrame[0] = 0.055; revoluteJoint2.m_parentFrame[1] = 0; revoluteJoint2.m_parentFrame[2] = 0.02; revoluteJoint2.m_parentFrame[3] = 0; revoluteJoint2.m_parentFrame[4] = 0; revoluteJoint2.m_parentFrame[5] = 0; revoluteJoint2.m_parentFrame[6] = 1.0; revoluteJoint2.m_childFrame[0] = 0; revoluteJoint2.m_childFrame[1] = 0; revoluteJoint2.m_childFrame[2] = 0; revoluteJoint2.m_childFrame[3] = 0; revoluteJoint2.m_childFrame[4] = 0; revoluteJoint2.m_childFrame[5] = 0; revoluteJoint2.m_childFrame[6] = 1.0; revoluteJoint2.m_jointAxis[0] = 1.0; revoluteJoint2.m_jointAxis[1] = 0.0; revoluteJoint2.m_jointAxis[2] = 0.0; revoluteJoint2.m_jointType = ePoint2PointType; m_robotSim.createConstraint(0, 2, 0, 4, &revoluteJoint1); m_robotSim.createConstraint(0, 3, 0, 6, &revoluteJoint2); } if ((m_options & eSOFTBODY_MULTIBODY_COUPLING)!=0) { { b3RobotSimulatorLoadUrdfFileArgs args; args.m_startPosition.setValue(0,1.0,2.0); args.m_startOrientation.setEulerZYX(0,0,1.57); args.m_forceOverrideFixedBase = false; args.m_useMultiBody = true; int kukaId = m_robotSim.loadURDF("kuka_iiwa/model_free_base.urdf", args); int numJoints = m_robotSim.getNumJoints(kukaId); b3Printf("numJoints = %d",numJoints); for (int i=0;i<numJoints;i++) { b3JointInfo jointInfo; m_robotSim.getJointInfo(kukaId,i,&jointInfo); b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName); b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_maxTorqueValue = 0.0; m_robotSim.setJointMotorControl(kukaId,i,controlArgs); } } { b3RobotSimulatorLoadUrdfFileArgs args; args.m_startPosition.setValue(0,0,0); args.m_startOrientation.setEulerZYX(0,0,0); args.m_forceOverrideFixedBase = true; args.m_useMultiBody = false; m_robotSim.loadURDF("plane.urdf", args); } m_robotSim.setGravity(b3MakeVector3(0,0,-10)); m_robotSim.loadBunny(0.5,10.0,0.1); } }
virtual CommonRenderInterface* getRenderInterface() { return m_childGuiHelper->getRenderInterface(); }