void InvertedPendulumPDControl::stepSimulation(float deltaTime)
{
static btScalar offset = -0.1*SIMD_PI;
m_frameCount++;
if ((m_frameCount&0xff)==0 )
{
offset = -offset;
}
btScalar target= SIMD_PI+offset;
qDesiredArray.resize(0);
qDesiredArray.resize(m_multiBody->getNumLinks(),target);
for (int joint = 0; joint<m_multiBody->getNumLinks(); joint++)
{
int dof1 = 0;
btScalar qActual = m_multiBody->getJointPosMultiDof(joint)[dof1];
btScalar qdActual = m_multiBody->getJointVelMultiDof(joint)[dof1];
btScalar positionError = (qDesiredArray[joint]-qActual);
double desiredVelocity = 0;
btScalar velocityError = (desiredVelocity-qdActual);
btScalar force = kp * positionError + kd*velocityError;
btClamp(force,-maxForce,maxForce);
m_multiBody->addJointTorque(joint, force);
}
if (m_frameCount==100)
{
const char* gPngFileName = "pendulum";
if (gPngFileName)
{
//printf("gPngFileName=%s\n",gPngFileName);
sprintf(fileName,"%s%d.png",gPngFileName,m_frameCount);
b3Printf("Made screenshot %s",fileName);
this->m_guiHelper->getAppInterface()->dumpNextFrameToPng(fileName);
}
}
m_dynamicsWorld->stepSimulation(1./60.,0);//240,0);
static int count = 0;
if ((count& 0x0f)==0)
{
#if 0
for (int i=0; i<m_jointFeedbacks.size(); i++)
{
b3Printf("F_reaction[%i] linear:%f,%f,%f, angular:%f,%f,%f",
i,
m_jointFeedbacks[i]->m_reactionForces.m_topVec[0],
m_jointFeedbacks[i]->m_reactionForces.m_topVec[1],
m_jointFeedbacks[i]->m_reactionForces.m_topVec[2],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[0],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[1],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[2]
);
}
#endif
}
count++;
/*
b3Printf("base angvel = %f,%f,%f",m_multiBody->getBaseOmega()[0],
m_multiBody->getBaseOmega()[1],
m_multiBody->getBaseOmega()[2]
);
*/
btScalar jointVel =m_multiBody->getJointVel(0);
// b3Printf("child angvel = %f",jointVel);
}
