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