Vector3d Controller2::evalAngMomentum(const VectorXd& _dofVel) {
Vector3d c = mSkel->getWorldCOM();
Vector3d sum = Vector3d::Zero();
Vector3d temp = Vector3d::Zero();
for (int i = 0; i < mSkel->getNumNodes(); i++) {
BodyNodeDynamics *node = (BodyNodeDynamics*)mSkel->getNode(i);
node->evalVelocity(_dofVel);
node->evalOmega(_dofVel);
sum += node->getInertia() * node->mOmega;
sum += node->getMass() * (node->getWorldCOM() - c).cross(node->mVel);
}
return sum;
}
VectorXd Controller2::adjustAngMomentum(VectorXd _deltaMomentum, VectorXd _controlledAxis) {
int nDof = mSkel->getNumDofs();
double mass = mSkel->getMass();
Matrix3d c = makeSkewSymmetric(mSkel->getWorldCOM());
MatrixXd A(MatrixXd::Zero(6, nDof));
MatrixXd Jv(MatrixXd::Zero(3, nDof));
MatrixXd Jw(MatrixXd::Zero(3, nDof));
for (int i = 0; i < mSkel->getNumNodes(); i++) {
BodyNodeDynamics *node = (BodyNodeDynamics*)mSkel->getNode(i);
Matrix3d c_i = makeSkewSymmetric(node->getWorldCOM());
double m_i = node->getMass();
MatrixXd localJv = node->getJacobianLinear();
MatrixXd localJw = node->getJacobianAngular();
Jv.setZero();
Jw.setZero();
for (int j = 0; j < node->getNumDependentDofs(); j++) {
int dofIndex = node->getDependentDof(j);
Jv.col(dofIndex) += localJv.col(j);
Jw.col(dofIndex) += localJw.col(j);
}
A.block(0, 0, 3, nDof) += m_i * Jv / mass;
A.block(3, 0, 3, nDof) += m_i * (c_i - c) * Jv + node->getInertia() * Jw;
}
for ( int i = 0; i < 6; i++)
A.col(i).setZero(); // try to realize momentum without using root acceleration
for (int i = 6; i < 20; i++)
A.col(i) *= 0;
MatrixXd aggregateMat(_controlledAxis.size(), nDof);
for (int i = 0; i < _controlledAxis.size(); i++) {
aggregateMat.row(i) = A.row(_controlledAxis[i]);
}
VectorXd deltaQdot = aggregateMat.transpose() * (aggregateMat * aggregateMat.transpose()).inverse() * _deltaMomentum;
return deltaQdot;
}