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) //===========================================// } }