Exemplo n.º 1
0
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(); 
        }
    }
}
Exemplo n.º 2
0
/**
   \note v, dv, dw are not used in the forward dynamics, but are calculated
   for forward dynamics users.
*/
void ForwardDynamicsABM::calcABMPhase1(bool updateNonSpatialVariables)
{
    const LinkTraverse& traverse = body->linkTraverse();
    const int n = traverse.numLinks();

    for(int i=0; i < n; ++i){
        DyLink* link = static_cast<DyLink*>(traverse[i]);
        const DyLink* parent = link->parent();

        if(parent){
            
            switch(link->jointType()){
                
            case Link::ROTATIONAL_JOINT:
            {
                const Vector3 arm = parent->R() * link->b();
                link->R().noalias() = parent->R() * AngleAxisd(link->q(), link->a());
                link->p().noalias() = arm + parent->p();
                link->sw().noalias() = parent->R() * link->a();
                link->sv().noalias() = link->p().cross(link->sw());
                link->w().noalias() = link->dq() * link->sw() + parent->w();
                if(updateNonSpatialVariables){
                    link->dw().noalias() =
                        parent->dw() + link->dq() * parent->w().cross(link->sw()) + (link->ddq() * link->sw());
                    link->dv().noalias() =
                        parent->dv() + parent->w().cross(parent->w().cross(arm)) + parent->dw().cross(arm);
                }
                break;
            }
                
            case Link::SLIDE_JOINT:
                link->p().noalias() = parent->R() * (link->b() + link->q() * link->d()) + parent->p();
                link->R() = parent->R();
                link->sw().setZero();
                link->sv().noalias() = parent->R() * link->d();
                link->w() = parent->w();
                if(updateNonSpatialVariables){
                    link->dw() = parent->dw();
                    const Vector3 arm = parent->R() * link->b();
                    link->dv().noalias() =
                        parent->dv() + parent->w().cross(parent->w().cross(arm)) + parent->dw().cross(arm)
                        + 2.0 * link->dq() * parent->w().cross(link->sv()) + link->ddq() * link->sv();
                }
                break;
                
            case Link::FIXED_JOINT:
            default:
                link->p().noalias() = parent->R() * link->b() + parent->p();
                link->R() = parent->R();
                link->w() = parent->w();
                link->vo() = parent->vo();
                link->sw().setZero();
                link->sv().setZero();
                link->cv().setZero();
                link->cw().setZero();
                if(updateNonSpatialVariables){
                    link->dw() = parent->dw();
                    const Vector3 arm = parent->R() * link->b();
                    link->dv().noalias() = parent->dv() +
                        parent->w().cross(parent->w().cross(arm)) + parent->dw().cross(arm);
                }
                
                goto COMMON_CALCS_FOR_ALL_JOINT_TYPES;
            }
            
            // Common for ROTATE and SLIDE
            link->vo().noalias() = link->dq() * link->sv() + parent->vo();
            const Vector3 dsv = parent->w().cross(link->sv()) + parent->vo().cross(link->sw());
            const Vector3 dsw = parent->w().cross(link->sw());
            link->cv() = link->dq() * dsv;
            link->cw() = link->dq() * dsw;
        }
        
COMMON_CALCS_FOR_ALL_JOINT_TYPES:

        if(updateNonSpatialVariables){
            link->v().noalias() = link->vo() + link->w().cross(link->p());
        }
        link->wc().noalias() = link->R() * link->c() + link->p();
        
        // compute I^s (Eq.(6.24) of Kajita's textbook))
        const Matrix3 Iw = link->R() * link->I() * link->R().transpose();
        
        const double m = link->m();
        const Matrix3 c_hat = hat(link->wc());
        link->Iww().noalias() = m * c_hat * c_hat.transpose() + Iw;

        link->Ivv() <<
            m,  0.0, 0.0,
            0.0,  m,  0.0,
            0.0, 0.0,  m;
        
        link->Iwv() = m * c_hat;
        
        // compute P and L (Eq.(6.25) of Kajita's textbook)
        const Vector3 P = m * (link->vo() + link->w().cross(link->wc()));
        const Vector3 L = link->Iww() * link->w() + m * link->wc().cross(link->vo());
        
        link->pf().noalias() = link->w().cross(P);
        link->ptau().noalias() = link->vo().cross(P) + link->w().cross(L);
        
        const Vector3 fg = m * g;
        const Vector3 tg = link->wc().cross(fg);
        
        link->pf() -= fg;
        link->ptau() -= tg;
    }
}