void MultiBodyConstraintFeedbackSetup::stepSimulation(float deltaTime)
{
//m_multiBody->addLinkForce(0,btVector3(100,100,100));
if (0)//m_once)
{
m_once=false;
m_multiBody->addJointTorque(0, 10.0);
btScalar torque = m_multiBody->getJointTorque(0);
b3Printf("t = %f,%f,%f\n",torque,torque,torque);//[0],torque[1],torque[2]);
}
btScalar timeStep = 1./240.f;
m_dynamicsWorld->stepSimulation(timeStep,0);
static int count = 0;
if ((count& 0x0f)==0)
{
if (m_motor)
{
float force = m_motor->getAppliedImpulse(0)/timeStep;
b3Printf("motor applied force = %f\n", force);
}
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]
);
}
}
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);
}
void Pendulum::stepSimulation(float deltaTime)
{
m_multiBody->addJointTorque(0, 20.0);
#ifdef USE_GTEST
m_dynamicsWorld->stepSimulation(1./1000.0,0);
#else
m_dynamicsWorld->stepSimulation(deltaTime);
#endif
btVector3 from = m_multiBody->getBaseWorldTransform().getOrigin();
btVector3 to = m_multiBody->getLink(0).m_collider->getWorldTransform().getOrigin();
btVector4 color(1,0,0,1);
if (m_guiHelper->getRenderInterface())
{
m_guiHelper->getRenderInterface()->drawLine(from,to,color,btScalar(1));
}
}
void MultiBodySoftContact::stepSimulation(float deltaTime)
{
if (0)//m_once)
{
m_once=false;
m_multiBody->addJointTorque(0, 10.0);
btScalar torque = m_multiBody->getJointTorque(0);
b3Printf("t = %f,%f,%f\n",torque,torque,torque);//[0],torque[1],torque[2]);
}
m_dynamicsWorld->stepSimulation(deltaTime);
}
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);
}
void TestJointTorqueSetup::stepSimulation(float deltaTime)
{
m_multiBody->addJointTorque(0, 10.0);
m_dynamicsWorld->stepSimulation(deltaTime);
}
void InverseDynamicsExample::stepSimulation(float deltaTime)
{
if(m_multiBody) {
const int num_dofs=m_multiBody->getNumDofs();
btInverseDynamics::vecx nu(num_dofs), qdot(num_dofs), q(num_dofs),joint_force(num_dofs);
btInverseDynamics::vecx pd_control(num_dofs);
// compute joint forces from one of two control laws:
// 1) "computed torque" control, which gives perfect, decoupled,
// linear second order error dynamics per dof in case of a
// perfect model and (and negligible time discretization effects)
// 2) decoupled PD control per joint, without a model
for(int dof=0;dof<num_dofs;dof++) {
q(dof) = m_multiBody->getJointPos(dof);
qdot(dof)= m_multiBody->getJointVel(dof);
const btScalar qd_dot=0;
const btScalar qd_ddot=0;
if (m_timeSeriesCanvas)
m_timeSeriesCanvas->insertDataAtCurrentTime(q[dof],dof,true);
// pd_control is either desired joint torque for pd control,
// or the feedback contribution to nu
pd_control(dof) = kd*(qd_dot-qdot(dof)) + kp*(qd[dof]-q(dof));
// nu is the desired joint acceleration for computed torque control
nu(dof) = qd_ddot + 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] = qdot[i];
q6[6+i] = q[i];
joint_force6[6+i] = joint_force[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));
}
}
}
if (m_timeSeriesCanvas)
m_timeSeriesCanvas->nextTick();
//todo: joint damping for btMultiBody, tune parameters
// step the simulation
if (m_dynamicsWorld)
{
// todo(thomas) check that this is correct:
// want to advance by 10ms, with 1ms timesteps
m_dynamicsWorld->stepSimulation(1e-3,0);//,1e-3);
btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m;
m_multiBody->forwardKinematics(scratch_q, scratch_m);
#if 0
for (int i = 0; i < m_multiBody->getNumLinks(); i++)
{
//btVector3 pos = m_multiBody->getLink(i).m_cachedWorldTransform.getOrigin();
btTransform tr = m_multiBody->getLink(i).m_cachedWorldTransform;
btVector3 pos = tr.getOrigin() - quatRotate(tr.getRotation(), m_multiBody->getLink(i).m_dVector);
btVector3 localAxis = m_multiBody->getLink(i).m_axes[0].m_topVec;
//printf("link %d: %f,%f,%f, local axis:%f,%f,%f\n", i, pos.x(), pos.y(), pos.z(), localAxis.x(), localAxis.y(), localAxis.z());
}
#endif
}
}
void InverseDynamicsExample::initPhysics()
{
//roboticists like Z up
int upAxis = 2;
m_guiHelper->setUpAxis(upAxis);
createEmptyDynamicsWorld();
btVector3 gravity(0,0,0);
// gravity[upAxis]=-9.8;
m_dynamicsWorld->setGravity(gravity);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
{
SliderParams slider("Kp",&kp);
slider.m_minVal=0;
slider.m_maxVal=2000;
if (m_guiHelper->getParameterInterface())
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
SliderParams slider("Kd",&kd);
slider.m_minVal=0;
slider.m_maxVal=50;
if (m_guiHelper->getParameterInterface())
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
if (m_option == BT_ID_PROGRAMMATICALLY)
{
ButtonParams button("toggle inverse model",0,true);
button.m_callback = toggleUseInverseModel;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
switch (m_option)
{
case BT_ID_LOAD_URDF:
{
BulletURDFImporter u2b(m_guiHelper,0,1);
bool loadOk = u2b.loadURDF("kuka_iiwa/model.urdf");// lwr / kuka.urdf");
if (loadOk)
{
int rootLinkIndex = u2b.getRootLinkIndex();
b3Printf("urdf root link index = %d\n",rootLinkIndex);
MyMultiBodyCreator creation(m_guiHelper);
btTransform identityTrans;
identityTrans.setIdentity();
ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,true,u2b.getPathPrefix());
for (int i = 0; i < u2b.getNumAllocatedCollisionShapes(); i++)
{
m_collisionShapes.push_back(u2b.getAllocatedCollisionShape(i));
}
m_multiBody = creation.getBulletMultiBody();
if (m_multiBody)
{
//kuka without joint control/constraints will gain energy explode soon due to timestep/integrator
//temporarily set some extreme damping factors until we have some joint control or constraints
m_multiBody->setAngularDamping(0*0.99);
m_multiBody->setLinearDamping(0*0.99);
b3Printf("Root link name = %s",u2b.getLinkName(u2b.getRootLinkIndex()).c_str());
}
}
break;
}
case BT_ID_PROGRAMMATICALLY:
{
btTransform baseWorldTrans;
baseWorldTrans.setIdentity();
m_multiBody = createInvertedPendulumMultiBody(m_dynamicsWorld, m_guiHelper, baseWorldTrans, false);
break;
}
default:
{
b3Error("Unknown option in InverseDynamicsExample::initPhysics");
b3Assert(0);
}
};
if(m_multiBody) {
{
if (m_guiHelper->getAppInterface() && m_guiHelper->getParameterInterface())
{
m_timeSeriesCanvas = new TimeSeriesCanvas(m_guiHelper->getAppInterface()->m_2dCanvasInterface,512,230, "Joint Space Trajectory");
m_timeSeriesCanvas ->setupTimeSeries(3,100, 0);
}
}
// construct inverse model
btInverseDynamics::btMultiBodyTreeCreator id_creator;
if (-1 == id_creator.createFromBtMultiBody(m_multiBody, false)) {
b3Error("error creating tree\n");
} else {
m_inverseModel = btInverseDynamics::CreateMultiBodyTree(id_creator);
}
// add joint target controls
qd.resize(m_multiBody->getNumDofs());
qd_name.resize(m_multiBody->getNumDofs());
q_name.resize(m_multiBody->getNumDofs());
if (m_timeSeriesCanvas && m_guiHelper->getParameterInterface())
{
for(std::size_t dof=0;dof<qd.size();dof++)
{
qd[dof] = 0;
char tmp[25];
sprintf(tmp,"q_desired[%lu]",dof);
qd_name[dof] = tmp;
SliderParams slider(qd_name[dof].c_str(),&qd[dof]);
slider.m_minVal=-3.14;
slider.m_maxVal=3.14;
sprintf(tmp,"q[%lu]",dof);
q_name[dof] = tmp;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
btVector4 color = sJointCurveColors[dof&7];
m_timeSeriesCanvas->addDataSource(q_name[dof].c_str(), color[0]*255,color[1]*255,color[2]*255);
}
}
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
