Example #1
0
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);
}
Example #2
0
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;
}
Example #3
0
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;
}