RigidBody::RigidBody( const Vector2s& v, const scalar& omega, const VectorXs& vertices, const VectorXs& masses, const scalar& radius )
: m_M(-1.0)
//, m_masses(masses)
, m_I(-1.0)
, m_vertices(vertices)
, m_r(radius)
, m_X(-1.0,-1.0)
, m_theta(0.0)
, m_V(v)
, m_omega(omega)
// theta == 0 => identity matrix
, m_R(Matrix2s::Identity())
, m_F(0.0,0.0)
, m_tau(0.0)
{
m_M = computeTotalMass(masses);
m_X = computeCenterOfMass(vertices,masses);
m_I = computeMomentOfInertia(vertices,masses);
assert( (masses.array()>0.0).all() );
assert( m_M > 0.0 );
assert( m_I > 0.0 );
assert( m_vertices.size() >= 2 );
assert( m_r >= 0.0 );
// Translate the rigid body so the body space center of mass is the origin
for( int i = 0; i < m_vertices.size()/2; ++i ) m_vertices.segment<2>(2*i) -= m_X;
}
scalar RigidBody::computeMomentOfInertia( const VectorXs& vertices, const VectorXs& masses ) const
{
assert( vertices.size()%2 == 0 );
assert( 2*masses.size() == vertices.size() );
// TODO: By using a partial reduction from Eigen, we could make this a single line of vectorized code :).
Vector2s cm = computeCenterOfMass(vertices,masses);
scalar I = 0.0;
for( int i = 0; i < masses.size(); ++i ) I += masses(i)*(vertices.segment<2>(2*i)-cm).squaredNorm();
return I;
}
void Destructulon::update(int elapsedTime) {
// BALANCE CONTROLLER
// ==================
// Step 1.1: Compute the COM in world coordinate system
btVector3 comInWorld = computeCenterOfMass();
m_positionCOM = comInWorld;
if (m_showCOM) { // Visualize COM
btTransform transform;
m_COM->getMotionState()->getWorldTransform(transform);
transform.setOrigin(comInWorld);
m_COM->getMotionState()->setWorldTransform(transform);
}
// Step 1.2: Update pose only if Destructulon did not fall
if (m_hasFallen) {
if (((btHingeConstraint*)m_joints[Destructulon::JOINT_ANKLE])->getEnableAngularMotor()) { // ragdoll is fallen
for(int i = 0; i < Destructulon::JOINT_COUNT; i++)
{
m_joints[i]->setEnabled(false);
}
((btHingeConstraint*)m_joints[Destructulon::JOINT_ANKLE])->enableMotor(false);
((btHingeConstraint*)m_joints[Destructulon::JOINT_KNEE])->enableMotor(false);
}
return;
}
if (elapsedTime - lastChange > 10) { // Update balance control only every 10 ms
lastChange = elapsedTime;
btVector3 COM, COA;
COA = m_bodies[BODYPART_FOOT]->getCenterOfMassPosition();
COM = computeCenterOfMass();
btTransform trFoot = m_bodies[Destructulon::BODYPART_FOOT]->getWorldTransform();
btQuaternion fRot = trFoot.getRotation();
btScalar mag = sqrt(fRot.w()*fRot.w() + fRot.y()*fRot.y());
btScalar yaw = 2*acos(fRot.w()/mag); //... I think...
btTransform yawRot; yawRot.setIdentity();
yawRot.setRotation(btQuaternion(yaw,0,0));
yawRot = yawRot.inverse();
COM = yawRot * COM;
COA = yawRot * COA;
((btHingeConstraint*)m_joints[Destructulon::JOINT_ANKLE])->setMotorTarget( btScalar( COM.z() - COA.z() ) * 20.0f );
((btHingeConstraint*)m_joints[Destructulon::JOINT_KNEE])->setMotorTarget( btScalar( COA.x() - COM.x() ) * 25.0f );
//=================== TODO ===================//
// Step 2: Describe the ground projected CSP in world coordinate system
// ANKLE
// -----
// Step 3.1: Describe the ground projected CSP in foot coordinate system
// Step 3.2: Describe the ground projected COM in foot coordinate system
// Step 3.3: Calculate the balance error solveable by an ankle rotation (inverted pendulum model)
// Step 3.4: Feed the error to the PD controller and apply resulting 'torque' (here angular motor velocity)
// (Conversion between error to torque/motor velocity done by gains in PD controller)
// KNEE
// ----
// Step 4.1: Describe the ground projected CSP in lower leg coordinate system
// Step 4.2: Describe the ground projected COM in lower leg coordinate system
// Step 4.3: Calculate the balance error solveable by a knee rotation (inverted pendulum model)
// Step 4.4: Feed the error to the PD controller and apply resulting 'torque' (here angular motor velocity)
// (Conversion between error to torque/motor velocity done by gains in PD controller)
//===========================================//
}
}
