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; }