virtual void stepSimulation(float deltaTime) { #ifndef BT_NO_PROFILE CProfileManager::Reset(); #endif void* myUserPtr = 0; gTotalPoints = 0; numNearCallbacks = 0; { BT_PROFILE("plWorldCollide"); if (m_collisionSdkHandle && m_collisionWorldHandle) { plWorldCollide(m_collisionSdkHandle,m_collisionWorldHandle,myNearCallback, myUserPtr); } } #if 0 switch (m_tutorialIndex) { case TUT_SPHERE_SPHERE: { if (m_timeSeriesCanvas0) { float xPos = 0.f; float xVel = 1.f; m_timeSeriesCanvas0->insertDataAtCurrentTime(xPos,0,true); m_timeSeriesCanvas0->insertDataAtCurrentTime(xVel,1,true); } break; } default: { } }; #endif if (m_timeSeriesCanvas0) m_timeSeriesCanvas0->nextTick(); // m_app->m_renderer->writeSingleInstanceTransformToCPU(m_bodies[i]->m_worldPose.m_position, m_bodies[i]->m_worldPose.m_orientation, m_bodies[i]->m_graphicsIndex); m_app->m_renderer->writeTransforms(); #ifndef BT_NO_PROFILE CProfileManager::Increment_Frame_Counter(); #endif }
virtual void stepSimulation(float deltaTime) { switch (m_tutorialIndex) { case TUT_VELOCITY: { tutorial1Update(deltaTime); float xPos = m_bodies[0]->m_worldPose.m_position.x; float xVel = m_bodies[0]->m_linearVelocity.x; m_timeSeriesCanvas0->insertDataAtCurrentTime(xPos,0,true); m_timeSeriesCanvas0->insertDataAtCurrentTime(xVel,1,true); break; } case TUT_ACCELERATION: { tutorial2Update(deltaTime); float yPos = m_bodies[0]->m_worldPose.m_position.y; float yVel = m_bodies[0]->m_linearVelocity.y; m_timeSeriesCanvas1->insertDataAtCurrentTime(yPos,0,true); m_timeSeriesCanvas1->insertDataAtCurrentTime(yVel,1,true); break; } case TUT_COLLISION: { m_contactPoints.clear(); LWContactPoint contactPoint; tutorialCollisionUpdate(deltaTime, contactPoint); m_contactPoints.push_back(contactPoint); m_timeSeriesCanvas1->insertDataAtCurrentTime(contactPoint.m_distance,0,true); break; } case TUT_SOLVE_CONTACT_CONSTRAINT: { m_contactPoints.clear(); LWContactPoint contactPoint; tutorialSolveContactConstraintUpdate(deltaTime, contactPoint); m_contactPoints.push_back(contactPoint); if (contactPoint.m_distance<0) { m_bodies[0]->computeInvInertiaTensorWorld(); m_bodies[1]->computeInvInertiaTensorWorld(); b3Scalar appliedImpulse = resolveCollision(*m_bodies[0], *m_bodies[1], contactPoint ); m_timeSeriesCanvas1->insertDataAtCurrentTime(appliedImpulse,1,true); } else { m_timeSeriesCanvas1->insertDataAtCurrentTime(0.,1,true); } m_timeSeriesCanvas1->insertDataAtCurrentTime(contactPoint.m_distance,0,true); break; } default: { } }; if (m_timeSeriesCanvas0) m_timeSeriesCanvas0->nextTick(); if (m_timeSeriesCanvas1) m_timeSeriesCanvas1->nextTick(); for (int i=0;i<m_bodies.size();i++) { m_bodies[i]->integrateAcceleration(deltaTime); m_bodies[i]->integrateVelocity(deltaTime); m_app->m_renderer->writeSingleInstanceTransformToCPU(m_bodies[i]->m_worldPose.m_position, m_bodies[i]->m_worldPose.m_orientation, m_bodies[i]->m_graphicsIndex); } m_app->m_renderer->writeTransforms(); }
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(); }
void InverseDynamicsExample::stepSimulation(float deltaTime) { if(m_multiBody) { const int num_dofs=m_multiBody->getNumDofs(); btInverseDynamics::vecx nu(num_dofs), qdot(num_dofs), q(num_dofs),joint_force(num_dofs); btInverseDynamics::vecx pd_control(num_dofs); // compute joint forces from one of two control laws: // 1) "computed torque" control, which gives perfect, decoupled, // linear second order error dynamics per dof in case of a // perfect model and (and negligible time discretization effects) // 2) decoupled PD control per joint, without a model for(int dof=0;dof<num_dofs;dof++) { q(dof) = m_multiBody->getJointPos(dof); qdot(dof)= m_multiBody->getJointVel(dof); const btScalar qd_dot=0; const btScalar qd_ddot=0; if (m_timeSeriesCanvas) m_timeSeriesCanvas->insertDataAtCurrentTime(q[dof],dof,true); // pd_control is either desired joint torque for pd control, // or the feedback contribution to nu pd_control(dof) = kd*(qd_dot-qdot(dof)) + kp*(qd[dof]-q(dof)); // nu is the desired joint acceleration for computed torque control nu(dof) = qd_ddot + pd_control(dof); } if(useInverseModel) { // calculate joint forces corresponding to desired accelerations nu if (m_multiBody->hasFixedBase()) { if(-1 != m_inverseModel->calculateInverseDynamics(q,qdot,nu,&joint_force)) { //joint_force(dof) += damping*dot_q(dof); // use inverse model: apply joint force corresponding to // desired acceleration nu for(int dof=0;dof<num_dofs;dof++) { m_multiBody->addJointTorque(dof,joint_force(dof)); } } } else { //the inverse dynamics model represents the 6 DOFs of the base, unlike btMultiBody. //append some dummy values to represent the 6 DOFs of the base btInverseDynamics::vecx nu6(num_dofs+6), qdot6(num_dofs+6), q6(num_dofs+6),joint_force6(num_dofs+6); for (int i=0;i<num_dofs;i++) { nu6[6+i] = nu[i]; qdot6[6+i] = qdot[i]; q6[6+i] = q[i]; joint_force6[6+i] = joint_force[i]; } if(-1 != m_inverseModel->calculateInverseDynamics(q6,qdot6,nu6,&joint_force6)) { //joint_force(dof) += damping*dot_q(dof); // use inverse model: apply joint force corresponding to // desired acceleration nu for(int dof=0;dof<num_dofs;dof++) { m_multiBody->addJointTorque(dof,joint_force6(dof+6)); } } } } else { for(int dof=0;dof<num_dofs;dof++) { // no model: just apply PD control law m_multiBody->addJointTorque(dof,pd_control(dof)); } } } if (m_timeSeriesCanvas) m_timeSeriesCanvas->nextTick(); //todo: joint damping for btMultiBody, tune parameters // step the simulation if (m_dynamicsWorld) { // todo(thomas) check that this is correct: // want to advance by 10ms, with 1ms timesteps m_dynamicsWorld->stepSimulation(1e-3,0);//,1e-3); btAlignedObjectArray<btQuaternion> scratch_q; btAlignedObjectArray<btVector3> scratch_m; m_multiBody->forwardKinematics(scratch_q, scratch_m); #if 0 for (int i = 0; i < m_multiBody->getNumLinks(); i++) { //btVector3 pos = m_multiBody->getLink(i).m_cachedWorldTransform.getOrigin(); btTransform tr = m_multiBody->getLink(i).m_cachedWorldTransform; btVector3 pos = tr.getOrigin() - quatRotate(tr.getRotation(), m_multiBody->getLink(i).m_dVector); btVector3 localAxis = m_multiBody->getLink(i).m_axes[0].m_topVec; //printf("link %d: %f,%f,%f, local axis:%f,%f,%f\n", i, pos.x(), pos.y(), pos.z(), localAxis.x(), localAxis.y(), localAxis.z()); } #endif } }
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); }