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 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 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 } }