void MultiBodyConstraintFeedbackSetup::stepSimulation(float deltaTime) { //m_multiBody->addLinkForce(0,btVector3(100,100,100)); if (0)//m_once) { m_once=false; m_multiBody->addJointTorque(0, 10.0); btScalar torque = m_multiBody->getJointTorque(0); b3Printf("t = %f,%f,%f\n",torque,torque,torque);//[0],torque[1],torque[2]); } btScalar timeStep = 1./240.f; m_dynamicsWorld->stepSimulation(timeStep,0); static int count = 0; if ((count& 0x0f)==0) { if (m_motor) { float force = m_motor->getAppliedImpulse(0)/timeStep; b3Printf("motor applied force = %f\n", force); } for (int i=0;i<m_jointFeedbacks.size();i++) { b3Printf("F_reaction[%i] linear:%f,%f,%f, angular:%f,%f,%f", i, m_jointFeedbacks[i]->m_reactionForces.m_topVec[0], m_jointFeedbacks[i]->m_reactionForces.m_topVec[1], m_jointFeedbacks[i]->m_reactionForces.m_topVec[2], m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[0], m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[1], m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[2] ); } } count++; /* b3Printf("base angvel = %f,%f,%f",m_multiBody->getBaseOmega()[0], m_multiBody->getBaseOmega()[1], m_multiBody->getBaseOmega()[2] ); */ btScalar jointVel =m_multiBody->getJointVel(0); // b3Printf("child angvel = %f",jointVel); }
void Pendulum::stepSimulation(float deltaTime) { m_multiBody->addJointTorque(0, 20.0); #ifdef USE_GTEST m_dynamicsWorld->stepSimulation(1./1000.0,0); #else m_dynamicsWorld->stepSimulation(deltaTime); #endif btVector3 from = m_multiBody->getBaseWorldTransform().getOrigin(); btVector3 to = m_multiBody->getLink(0).m_collider->getWorldTransform().getOrigin(); btVector4 color(1,0,0,1); if (m_guiHelper->getRenderInterface()) { m_guiHelper->getRenderInterface()->drawLine(from,to,color,btScalar(1)); } }
void MultiBodySoftContact::stepSimulation(float deltaTime) { if (0)//m_once) { m_once=false; m_multiBody->addJointTorque(0, 10.0); btScalar torque = m_multiBody->getJointTorque(0); b3Printf("t = %f,%f,%f\n",torque,torque,torque);//[0],torque[1],torque[2]); } m_dynamicsWorld->stepSimulation(deltaTime); }
void InvertedPendulumPDControl::stepSimulation(float deltaTime) { static btScalar offset = -0.1*SIMD_PI; m_frameCount++; if ((m_frameCount&0xff)==0 ) { offset = -offset; } btScalar target= SIMD_PI+offset; qDesiredArray.resize(0); qDesiredArray.resize(m_multiBody->getNumLinks(),target); for (int joint = 0; joint<m_multiBody->getNumLinks(); joint++) { int dof1 = 0; btScalar qActual = m_multiBody->getJointPosMultiDof(joint)[dof1]; btScalar qdActual = m_multiBody->getJointVelMultiDof(joint)[dof1]; btScalar positionError = (qDesiredArray[joint]-qActual); double desiredVelocity = 0; btScalar velocityError = (desiredVelocity-qdActual); btScalar force = kp * positionError + kd*velocityError; btClamp(force,-maxForce,maxForce); m_multiBody->addJointTorque(joint, force); } if (m_frameCount==100) { const char* gPngFileName = "pendulum"; if (gPngFileName) { //printf("gPngFileName=%s\n",gPngFileName); sprintf(fileName,"%s%d.png",gPngFileName,m_frameCount); b3Printf("Made screenshot %s",fileName); this->m_guiHelper->getAppInterface()->dumpNextFrameToPng(fileName); } } m_dynamicsWorld->stepSimulation(1./60.,0);//240,0); static int count = 0; if ((count& 0x0f)==0) { #if 0 for (int i=0; i<m_jointFeedbacks.size(); i++) { b3Printf("F_reaction[%i] linear:%f,%f,%f, angular:%f,%f,%f", i, m_jointFeedbacks[i]->m_reactionForces.m_topVec[0], m_jointFeedbacks[i]->m_reactionForces.m_topVec[1], m_jointFeedbacks[i]->m_reactionForces.m_topVec[2], m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[0], m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[1], m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[2] ); } #endif } count++; /* b3Printf("base angvel = %f,%f,%f",m_multiBody->getBaseOmega()[0], m_multiBody->getBaseOmega()[1], m_multiBody->getBaseOmega()[2] ); */ btScalar jointVel =m_multiBody->getJointVel(0); // b3Printf("child angvel = %f",jointVel); }
void TestJointTorqueSetup::stepSimulation(float deltaTime) { m_multiBody->addJointTorque(0, 10.0); m_dynamicsWorld->stepSimulation(deltaTime); }
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); }