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 // as parameters and callbacks
static btScalar kp =10*10; static btScalar kp =10*10;
static btScalar kd = 2*10; static btScalar kd = 2*10;
static bool useInverseModel = true; static bool useInverseModel = false;
static std::vector<btScalar> qd; static std::vector<btScalar> qd;
static std::vector<std::string> qd_name; static std::vector<std::string> qd_name;
static std::vector<std::string> q_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. //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() void InverseDynamicsExample::initPhysics()
{ {
@@ -120,7 +120,7 @@ void InverseDynamicsExample::initPhysics()
createEmptyDynamicsWorld(); createEmptyDynamicsWorld();
btVector3 gravity(0,0,0); btVector3 gravity(0,0,0);
gravity[upAxis]=-9.8; // gravity[upAxis]=-9.8;
m_dynamicsWorld->setGravity(gravity); m_dynamicsWorld->setGravity(gravity);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
@@ -176,7 +176,7 @@ void InverseDynamicsExample::initPhysics()
{ {
btTransform baseWorldTrans; btTransform baseWorldTrans;
baseWorldTrans.setIdentity(); baseWorldTrans.setIdentity();
m_multiBody = createInvertedPendulumMultiBody(m_dynamicsWorld, m_guiHelper, baseWorldTrans); m_multiBody = createInvertedPendulumMultiBody(m_dynamicsWorld, m_guiHelper, baseWorldTrans, false);
break; break;
} }
default: default:
@@ -255,19 +255,57 @@ void InverseDynamicsExample::stepSimulation(float deltaTime)
nu(dof) = qd_ddot + pd_control(dof); nu(dof) = qd_ddot + pd_control(dof);
} }
// calculate joint forces corresponding to desired accelerations nu if(useInverseModel)
if(-1 != m_inverseModel->calculateInverseDynamics(q,qdot,nu,&joint_force)) { {
for(int dof=0;dof<num_dofs;dof++) { // calculate joint forces corresponding to desired accelerations nu
if(useInverseModel) { if (m_multiBody->hasFixedBase())
//joint_force(dof) += damping*dot_q(dof); {
// use inverse model: apply joint force corresponding to if(-1 != m_inverseModel->calculateInverseDynamics(q,qdot,nu,&joint_force))
// desired acceleration nu {
m_multiBody->addJointTorque(dof,joint_force(dof)); //joint_force(dof) += damping*dot_q(dof);
} else { // use inverse model: apply joint force corresponding to
// no model: just apply PD control law // desired acceleration nu
m_multiBody->addJointTorque(dof,pd_control(dof));
} 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 dist = 5;
float pitch = 270; float pitch = 270;
float yaw = 21; 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]); m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
} }
@@ -55,7 +55,7 @@ InvertedPendulumPDControl::~InvertedPendulumPDControl()
extern bool gJointFeedbackInWorldSpace; extern bool gJointFeedbackInWorldSpace;
extern bool gJointFeedbackInJointFrame; 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] = btVector4 colors[4] =
{ {
@@ -66,7 +66,6 @@ btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GU
}; };
int curColor = 0; int curColor = 0;
bool fixedBase = true;
bool damping = false; bool damping = false;
bool gyro = false; bool gyro = false;
int numLinks = 2; 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 //mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
//init the base //init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f); btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 0.f; float baseMass = fixedBase ? 0.f : 10.f;
if(baseMass) if(baseMass)
{ {
@@ -356,7 +355,7 @@ void InvertedPendulumPDControl::initPhysics()
btTransform baseWorldTrans; btTransform baseWorldTrans;
baseWorldTrans.setIdentity(); baseWorldTrans.setIdentity();
baseWorldTrans.setOrigin(btVector3(1,2,3)); 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=pMultiBody->getNumLinks()-1;i>=0;i--)//
for (int i=0;i<m_multiBody->getNumLinks();i++) for (int i=0;i<m_multiBody->getNumLinks();i++)