Skip to content

Commit

Permalink
show inverse dynamics demo to use floating base
Browse files Browse the repository at this point in the history
  • Loading branch information
Erwin Coumans committed Dec 2, 2015
1 parent 5da9e37 commit 3dfebe0
Show file tree
Hide file tree
Showing 2 changed files with 59 additions and 22 deletions.
72 changes: 55 additions & 17 deletions examples/InverseDynamics/InverseDynamicsExample.cpp
View file
Expand Up @@ -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<btScalar> qd;
static std::vector<std::string> qd_name;
static std::vector<std::string> q_name;
Expand Down Expand Up @@ -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()
{
Expand All @@ -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);
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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;dof<num_dofs;dof++) {
if(useInverseModel) {
//joint_force(dof) += damping*dot_q(dof);
// use inverse model: apply joint force corresponding to
// desired acceleration nu
m_multiBody->addJointTorque(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;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] = qdot6[i];
q6[6+i] = q6[i];
joint_force6[6+i] = joint_force6[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));
}
}
}

Expand Down
9 changes: 4 additions & 5 deletions examples/MultiBody/InvertedPendulumPDControl.cpp
View file
Expand Up @@ -32,7 +32,7 @@ struct InvertedPendulumPDControl : public CommonMultiBodyBase
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]);
}

Expand All @@ -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] =
{
Expand All @@ -66,7 +66,6 @@ btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GU
};
int curColor = 0;

bool fixedBase = true;
bool damping = false;
bool gyro = false;
int numLinks = 2;
Expand All @@ -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)
{
Expand Down Expand Up @@ -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;i<m_multiBody->getNumLinks();i++)
Expand Down

0 comments on commit 3dfebe0

Please sign in to comment.