Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

show inverse dynamics demo to use floating base

Browse Source
This commit is contained in:
Erwin Coumans
2015-12-02 09:33:12 -08:00
parent 5da9e37a05
commit 3dfebe0c45
2 changed files with 59 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

72
examples/InverseDynamics/InverseDynamicsExample.cpp
View File

@@ -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;
@@ -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;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));
}
}
}

9
examples/MultiBody/InvertedPendulumPDControl.cpp
View File

@@ -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;i<m_multiBody->getNumLinks();i++)