void MyWindow::initDyn()
{
mDofs.resize(mSkels.size());
mDofVels.resize(mSkels.size());
for (unsigned int i = 0; i < mSkels.size(); i++) {
mDofs[i].resize(mSkels[i]->getNumDofs());
mDofVels[i].resize(mSkels[i]->getNumDofs());
mDofs[i].setZero();
mDofVels[i].setZero();
}
mDofs[0][1] = -2.9; // ground level
// default standing pose
mDofs[1][1] = -0.1;
mDofs[1][6] = 0.2; // left hip
mDofs[1][9] = -0.5; // left knee
mDofs[1][10] = 0.3; // left ankle
mDofs[1][13] = 0.2; // right hip
mDofs[1][16] = -0.5; // right knee
mDofs[1][17] = 0.3; // right ankle
mDofs[1][21] = -0.1; // lower back
mDofs[1][28] = 0.5; // left shoulder
mDofs[1][34] = -0.5; // right shoulder
for (unsigned int i = 0; i < mSkels.size(); i++) {
mSkels[i]->initDynamics();
mSkels[i]->setPose(mDofs[i], false, false);
// compute dynamics here because computation of control force at first iteration needs to access mass matrix
mSkels[i]->computeDynamics(mGravity, mDofVels[i], false);
}
mSkels[0]->setImmobileState(true);
mController = new Controller(mSkels[1], mConstraintHandle, mTimeStep);
for (int i = 0; i < mSkels[1]->getNumDofs(); i++)
mController->setDesiredDof(i, mController->getSkel()->getDof(i)->getValue());
// initialize constraint on the hand
mConstraintHandle = new ConstraintDynamics(mSkels, mTimeStep);
BodyNodeDynamics *bd = (BodyNodeDynamics*)mSkels[1]->getNode("fullbody1_h_hand_left");
PointConstraint *point1 = new PointConstraint(bd, bd->getLocalCOM(), bd->getWorldCOM(), 1);
mConstraintHandle->addConstraint(point1);
bd = (BodyNodeDynamics*)mSkels[1]->getNode("fullbody1_h_hand_right");
PointConstraint *point2 = new PointConstraint(bd, bd->getLocalCOM(), bd->getWorldCOM(), 1);
mConstraintHandle->addConstraint(point2);
}
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;
}