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();
}
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;
}
/* ********************************************************************************************* */
int main(int argc, char* argv[]) {
// Create Left Leg skeleton
SkeletonDynamics LeftLegSkel;
// Pointers to be used during the Skeleton building
Matrix3d inertiaMatrix;
inertiaMatrix << 0, 0, 0, 0, 0, 0, 0, 0, 0;
double mass = 1.0;
// ***** BodyNode 1: Left Hip Yaw (LHY) ***** *
BodyNodeDynamics* node = (BodyNodeDynamics*) LeftLegSkel.createBodyNode("LHY");
Joint* joint = new Joint(NULL, node, "LHY");
add_XyzRpy(joint, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
add_DOF(&LeftLegSkel, joint, 0.0, 0.0, M_PI, DOF_YAW);
Shape* shape = new ShapeBox(Vector3d(0.3, 0.3, 1.0));
node->setVisualizationShape(shape);
node->setCollisionShape(shape);
node->setMass(mass);
LeftLegSkel.addNode(node);
// ***** BodyNode 2: Left Hip Roll (LHR) whose parent is: LHY *****
BodyNodeDynamics* parent_node = (BodyNodeDynamics*) LeftLegSkel.getNode("LHY");
node = (BodyNodeDynamics*) LeftLegSkel.createBodyNode("LHR");
joint = new Joint(parent_node, node, "LHR");
add_XyzRpy(joint, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0);
add_DOF(&LeftLegSkel, joint, 0.0, 0.0, M_PI, DOF_ROLL);
shape = new ShapeBox(Vector3d(0.3, 0.3, 1.0));
shape->setOffset(Vector3d(0.0, 0.0, 0.5));
node->setLocalCOM(shape->getOffset());
node->setMass(mass);
node->setVisualizationShape(shape);
node->setCollisionShape(shape);
LeftLegSkel.addNode(node);
// ***** BodyNode 3: Left Hip Pitch (LHP) whose parent is: LHR *****
parent_node = (BodyNodeDynamics*) LeftLegSkel.getNode("LHR");
node = (BodyNodeDynamics*) LeftLegSkel.createBodyNode("LHP");
joint = new Joint(parent_node, node, "LHP");
add_XyzRpy(joint, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0);
add_DOF(&LeftLegSkel, joint, 0.0, 0.0, M_PI, DOF_ROLL);
shape = new ShapeBox(Vector3d(0.3, 0.3, 1.0));
shape->setOffset(Vector3d(0.0, 0.0, 0.5));
node->setLocalCOM(shape->getOffset());
node->setMass(mass);
Shape* shape1 = new ShapeEllipsoid(Vector3d(0.3, 0.3, 1.0));
shape1->setOffset(Vector3d(0.0, 0.0, 0.5));
node->setVisualizationShape(shape1);
node->setCollisionShape(shape);
LeftLegSkel.addNode(node);
// Initialize the skeleton
LeftLegSkel.initSkel();
// Window stuff
MyWindow window(&LeftLegSkel);
glutInit(&argc, argv);
window.initWindow(640, 480, "Skeleton example");
glutMainLoop();
return 0;
}