C++ DyLink::dvo方法代码示例

void ForwardDynamicsABM::calcMotionWithEulerMethod()
{
    calcABMLastHalf();

    if(!sensorHelper.forceSensors().empty()){
        updateForceSensors();
    }

    DyLink* root = body->rootLink();

    if(!root->isFixedJoint()){
        root->dv() =
            root->dvo() - root->p().cross(root->dw())
            + root->w().cross(root->vo() + root->w().cross(root->p()));
        Position T;
        SE3exp(T, root->T(), root->w(), root->vo(), timeStep);
        root->T() = T;
        root->vo() += root->dvo() * timeStep;
        root->w()  += root->dw()  * timeStep;
    }

    int n = body->numLinks();
    for(int i=1; i < n; ++i){
        DyLink* link = body->link(i);
        link->q()  += link->dq()  * timeStep;
        link->dq() += link->ddq() * timeStep;
    }
}

void ForwardDynamicsABM::integrateRungeKuttaOneStep(double r, double dt)
{
    DyLink* root = body->rootLink();

    if(!root->isFixedJoint()){

        SE3exp(root->T(), T0, root->w(), root->vo(), dt);
        root->vo().noalias() = vo0 + root->dvo() * dt;
        root->w().noalias()  = w0  + root->dw()  * dt;

        vo  += r * root->vo();
        w   += r * root->w();
        dvo += r * root->dvo();
        dw  += r * root->dw();
    }

    int n = body->numLinks();
    for(int i=1; i < n; ++i){

        DyLink* link = body->link(i);

        link->q()  =  q0[i] + dt * link->dq();
        link->dq() = dq0[i] + dt * link->ddq();

        dq[i]  += r * link->dq();
        ddq[i] += r * link->ddq();
    }
}

void AISTSimulatorItemImpl::addBody(AISTSimBody* simBody)
{
    DyBody* body = static_cast<DyBody*>(simBody->body());

    DyLink* rootLink = body->rootLink();
    rootLink->v().setZero();
    rootLink->dv().setZero();
    rootLink->w().setZero();
    rootLink->dw().setZero();
    rootLink->vo().setZero();
    rootLink->dvo().setZero();

    for(int i=0; i < body->numLinks(); ++i){
        Link* link = body->link(i);
        link->u() = 0.0;
        link->dq() = 0.0;
        link->ddq() = 0.0;
    }
    
    body->clearExternalForces();
    body->calcForwardKinematics(true, true);

    int bodyIndex;
    if(dynamicsMode.is(AISTSimulatorItem::HG_DYNAMICS)){
        ForwardDynamicsCBMPtr cbm = make_shared_aligned<ForwardDynamicsCBM>(body);
        cbm->setHighGainModeForAllJoints();
        bodyIndex = world.addBody(body, cbm);
    } else {
        bodyIndex = world.addBody(body);
    }
    bodyIndexMap[body] = bodyIndex;
}

void ForwardDynamicsABM::calcABMPhase3()
{
    const LinkTraverse& traverse = body->linkTraverse();

    DyLink* root = static_cast<DyLink*>(traverse[0]);

    if(root->isFreeJoint()){

        // - | Ivv  trans(Iwv) | * | dvo | = | pf   |
        //   | Iwv     Iww     |   | dw  |   | ptau |

        Eigen::Matrix<double, 6, 6> M;
        M << root->Ivv(), root->Iwv().transpose(),
            root->Iwv(), root->Iww();
        
        Eigen::Matrix<double, 6, 1> f;
        f << root->pf(),
            root->ptau();
        f *= -1.0;

        Eigen::Matrix<double, 6, 1> a(M.colPivHouseholderQr().solve(f));

        root->dvo() = a.head<3>();
        root->dw() = a.tail<3>();

    } else {
        root->dvo().setZero();
        root->dw().setZero();
    }

    const int n = traverse.numLinks();
    for(int i=1; i < n; ++i){
        DyLink* link = static_cast<DyLink*>(traverse[i]);
        const DyLink* parent = link->parent();
        if(!link->isFixedJoint()){
            link->ddq() = (link->uu() - (link->hhv().dot(parent->dvo()) + link->hhw().dot(parent->dw()))) / link->dd();
            link->dvo().noalias() = parent->dvo() + link->cv() + link->sv() * link->ddq();
            link->dw().noalias()  = parent->dw()  + link->cw() + link->sw() * link->ddq();
        }else{
            link->ddq() = 0.0;
            link->dvo() = parent->dvo();
            link->dw()  = parent->dw(); 
        }
    }
}

void ForwardDynamicsABM::calcMotionWithRungeKuttaMethod()
{
    DyLink* root = body->rootLink();

    if(!root->isFixedJoint()){
        T0 = root->T();
        vo0 = root->vo();
        w0  = root->w();
    }

    vo.setZero();
    w.setZero();
    dvo.setZero();
    dw.setZero();

    int n = body->numLinks();
    for(int i=1; i < n; ++i){
        DyLink* link = body->link(i);
        q0[i]  = link->q();
        dq0[i] = link->dq();
        dq[i]  = 0.0;
        ddq[i] = 0.0;
    }

    calcABMLastHalf();

    if(!sensorHelper.forceSensors().empty()){
        updateForceSensors();
    }

    integrateRungeKuttaOneStep(1.0 / 6.0, timeStep / 2.0);
    calcABMPhase1();
    calcABMPhase2();
    calcABMPhase3();

    integrateRungeKuttaOneStep(2.0 / 6.0, timeStep / 2.0);
    calcABMPhase1();
    calcABMPhase2();
    calcABMPhase3();

    integrateRungeKuttaOneStep(2.0 / 6.0, timeStep);
    calcABMPhase1();
    calcABMPhase2();
    calcABMPhase3();

    if(!root->isFixedJoint()){
        SE3exp(root->T(), T0, w0, vo0, timeStep);
        root->vo() = vo0 + (dvo + root->dvo() / 6.0) * timeStep;
        root->w()  = w0  + (dw  + root->dw()  / 6.0) * timeStep;
    }

    for(int i=1; i < n; ++i){
        DyLink* link = body->link(i);
        link->q()  =  q0[i] + ( dq[i] + link->dq()  / 6.0) * timeStep;
        link->dq() = dq0[i] + (ddq[i] + link->ddq() / 6.0) * timeStep;
    }
}