Vector3d Controller2::evalLinMomentum(const VectorXd& _dofVel) {
MatrixXd J(MatrixXd::Zero(3, mSkel->getNumDofs()));
for (int i = 0; i < mSkel->getNumNodes(); i++) {
BodyNodeDynamics *node = (BodyNodeDynamics*)mSkel->getNode(i);
MatrixXd localJ = node->getJacobianLinear() * node->getMass();
for (int j = 0; j < node->getNumDependentDofs(); j++) {
int index = node->getDependentDof(j);
J.col(index) += localJ.col(j);
}
}
Vector3d cDot = J * _dofVel;
return cDot / mSkel->getMass();
}
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;
}