diff --git a/examples/InverseDynamics/InverseDynamicsExample.cpp b/examples/InverseDynamics/InverseDynamicsExample.cpp index 0ff239fd1..2c6a8a893 100644 --- a/examples/InverseDynamics/InverseDynamicsExample.cpp +++ b/examples/InverseDynamics/InverseDynamicsExample.cpp @@ -43,7 +43,7 @@ subject to the following restrictions: // as parameters and callbacks static btScalar kp =10*10; static btScalar kd = 2*10; -static bool useInverseModel = true; +static bool useInverseModel = false; static std::vector qd; static std::vector qd_name; static std::vector q_name; @@ -110,7 +110,7 @@ InverseDynamicsExample::~InverseDynamicsExample() } //todo(erwincoumans) Quick hack, reference to InvertedPendulumPDControl implementation. Will create a separate header/source file for this. -btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GUIHelperInterface* guiHelper, const btTransform& baseWorldTrans); +btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GUIHelperInterface* guiHelper, const btTransform& baseWorldTrans, bool fixedBase); void InverseDynamicsExample::initPhysics() { @@ -120,7 +120,7 @@ void InverseDynamicsExample::initPhysics() createEmptyDynamicsWorld(); btVector3 gravity(0,0,0); - gravity[upAxis]=-9.8; + // gravity[upAxis]=-9.8; m_dynamicsWorld->setGravity(gravity); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); @@ -176,7 +176,7 @@ void InverseDynamicsExample::initPhysics() { btTransform baseWorldTrans; baseWorldTrans.setIdentity(); - m_multiBody = createInvertedPendulumMultiBody(m_dynamicsWorld, m_guiHelper, baseWorldTrans); + m_multiBody = createInvertedPendulumMultiBody(m_dynamicsWorld, m_guiHelper, baseWorldTrans, false); break; } default: @@ -255,19 +255,57 @@ void InverseDynamicsExample::stepSimulation(float deltaTime) nu(dof) = qd_ddot + pd_control(dof); } - // calculate joint forces corresponding to desired accelerations nu - if(-1 != m_inverseModel->calculateInverseDynamics(q,qdot,nu,&joint_force)) { - for(int dof=0;dofaddJointTorque(dof,joint_force(dof)); - } else { - // no model: just apply PD control law - m_multiBody->addJointTorque(dof,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;dofaddJointTorque(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;icalculateInverseDynamics(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;dofaddJointTorque(dof,joint_force6(dof+6)); + } + } + + } + + + } else + { + for(int dof=0;dofaddJointTorque(dof,pd_control(dof)); + } } } diff --git a/examples/MultiBody/InvertedPendulumPDControl.cpp b/examples/MultiBody/InvertedPendulumPDControl.cpp index 9c58e115a..d31b113d4 100644 --- a/examples/MultiBody/InvertedPendulumPDControl.cpp +++ b/examples/MultiBody/InvertedPendulumPDControl.cpp @@ -32,7 +32,7 @@ public: float dist = 5; float pitch = 270; float yaw = 21; - float targetPos[3]={-1.34,3.4,-0.44}; + float targetPos[3]={-1.34,1.4,3.44}; m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); } @@ -55,7 +55,7 @@ InvertedPendulumPDControl::~InvertedPendulumPDControl() extern bool gJointFeedbackInWorldSpace; extern bool gJointFeedbackInJointFrame; -btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GUIHelperInterface* guiHelper, const btTransform& baseWorldTrans) +btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GUIHelperInterface* guiHelper, const btTransform& baseWorldTrans, bool fixedBase) { btVector4 colors[4] = { @@ -66,7 +66,6 @@ btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GU }; int curColor = 0; - bool fixedBase = true; bool damping = false; bool gyro = false; int numLinks = 2; @@ -80,7 +79,7 @@ btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GU //mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm //init the base btVector3 baseInertiaDiag(0.f, 0.f, 0.f); - float baseMass = 0.f; + float baseMass = fixedBase ? 0.f : 10.f; if(baseMass) { @@ -356,7 +355,7 @@ void InvertedPendulumPDControl::initPhysics() btTransform baseWorldTrans; baseWorldTrans.setIdentity(); baseWorldTrans.setOrigin(btVector3(1,2,3)); - m_multiBody = createInvertedPendulumMultiBody(m_dynamicsWorld, m_guiHelper, baseWorldTrans); + m_multiBody = createInvertedPendulumMultiBody(m_dynamicsWorld, m_guiHelper, baseWorldTrans, true); //for (int i=pMultiBody->getNumLinks()-1;i>=0;i--)// for (int i=0;igetNumLinks();i++)