C++ 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 Body::initializeState()
{
    rootLink_->T() = rootLink_->Tb();

    rootLink_->v().setZero();
    rootLink_->w().setZero();
    rootLink_->dv().setZero();
    rootLink_->dw().setZero();
    
    const int n = linkTraverse_.numLinks();
    for(int i=0; i < n; ++i){
        Link* link = linkTraverse_[i];
        link->u() = 0.0;
        link->q() = 0.0;
        link->dq() = 0.0;
        link->ddq() = 0.0;
    }
 
    calcForwardKinematics(true, true);
    clearExternalForces();
    initializeDeviceStates();
}

void ODESimulatorItemImpl::addBody(ODEBody* odeBody)
{
    Body& body = *odeBody->body();

    Link* rootLink = body.rootLink();
    rootLink->v().setZero();
    rootLink->dv().setZero();
    rootLink->w().setZero();
    rootLink->dw().setZero();

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

    odeBody->createBody(this);
}

/**
   calculate the mass matrix using the unit vector method
   \todo replace the unit vector method here with a more efficient method
       
   The motion equation (dv != dvo)
   |       |   | dv   |   |    |   | fext      |
   | out_M | * | dw   | + | b1 | = | tauext    |
   |       |   |ddq   |   |    |   | u         |
*/
void calcMassMatrix(Body* body, const Vector3& g, Eigen::MatrixXd& out_M)
{
    const int nj = body->numJoints();
    Link* rootLink = body->rootLink();
    const int totaldof = rootLink->isFixedJoint() ? nj : nj + 6;

    out_M.resize(totaldof, totaldof);
        
    // preserve and clear the joint accelerations
    VectorXd ddqorg(nj);
    VectorXd uorg(nj);
    for(int i = 0; i < nj; ++i){
        Link* joint = body->joint(i);
        ddqorg[i] = joint->ddq();
        uorg  [i] = joint->u();
        joint->ddq() = 0.0;
    }

    // preserve and clear the root link acceleration
    const Vector3 dvorg = rootLink->dv();
    const Vector3 dworg  = rootLink->dw();

    //const Vector3 root_w_x_v = rootLink->w().cross(rootLink->vo() + rootLink->w().cross(rootLink->p()));

    rootLink->dv() = g;
    rootLink->dw().setZero();
	
    MatrixXd b1(totaldof, 1);
        
    setColumnOfMassMatrix(body, b1, 0);

    if(!rootLink->isFixedJoint()){
        for(int i=0; i < 3; ++i){
            rootLink->dv()[i] += 1.0;
            setColumnOfMassMatrix(body, out_M, i);
            rootLink->dv()[i] -= 0.0;
        }
        for(int i=0; i < 3; ++i){
            rootLink->dw()[i] = 1.0;
            setColumnOfMassMatrix(body, out_M, i + 3);
            rootLink->dw()[i] = 0.0;
        }
    }

    for(int i = 0; i < nj; ++i){
        Link* joint = body->joint(i);
        joint->ddq() = 1.0;
        int j = i + 6;
        setColumnOfMassMatrix(body, out_M, j);
        out_M(j, j) += joint->Jm2(); // motor inertia
        joint->ddq() = 0.0;
    }

    // subtract the constant term
    for(int i = 0; i < out_M.cols(); ++i){
        out_M.col(i) -= b1;
    }

    // recover state
    for(int i = 0; i < nj; ++i){
        Link* joint = body->joint(i);
        joint->ddq()  = ddqorg[i];
        joint->u()    = uorg  [i];
    }
    rootLink->dv() = dvorg;
    rootLink->dw() = dworg;
}

void LinkTraverse::calcForwardKinematics(bool calcVelocity, bool calcAcceleration) const
{
    Vector3 arm;
    int i;
    for(i=1; i <= numUpwardConnections; ++i){

        Link* link = links[i];
        const Link* child = links[i-1];

        switch(child->jointType()){

        case Link::ROTATIONAL_JOINT:
            link->R().noalias() = child->R() * AngleAxisd(child->q(), child->a()).inverse();
            arm.noalias() = link->R() * child->b();
            link->p().noalias() = child->p() - arm;

            if(calcVelocity){
                const Vector3 sw(link->R() * child->a());
                link->w() = child->w() - child->dq() * sw;
                link->v() = child->v() - link->w().cross(arm);
                
                if(calcAcceleration){
                    link->dw() = child->dw() - child->dq() * child->w().cross(sw) - (child->ddq() * sw);
                    link->dv() = child->dv() - child->w().cross(child->w().cross(arm)) - child->dw().cross(arm);
                }
            }
            break;
            
        case Link::SLIDE_JOINT:
            link->R() = child->R();
            arm.noalias() = link->R() * (child->b() + child->q() * child->d());
            link->p().noalias() = child->p() - arm;

            if(calcVelocity){
                const Vector3 sv(link->R() * child->d());
                link->w() = child->w();
                link->v().noalias() = child->v() - child->dq() * sv;

                if(calcAcceleration){
                    link->dw() = child->dw();
                    link->dv().noalias() =
                        child->dv() - child->w().cross(child->w().cross(arm)) - child->dw().cross(arm)
                        - 2.0 * child->dq() * child->w().cross(sv) - child->ddq() * sv;
                }
            }
            break;
            
        case Link::FIXED_JOINT:
        default:
            arm.noalias() = link->R() * child->b();
            link->R() = child->R();
            link->p().noalias() = child->p() - arm;

            if(calcVelocity){
                
                link->w() = child->w();
                link->v() = child->v() - link->w().cross(arm);
				
                if(calcAcceleration){
                    link->dw() = child->dw();
                    link->dv() = child->dv() - child->w().cross(child->w().cross(arm)) - child->dw().cross(arm);;
                }
            }
            break;
        }
    }

    const int n = links.size();
    for( ; i < n; ++i){
        
        Link* link = links[i];
        const Link* parent = link->parent();

        switch(link->jointType()){
            
        case Link::ROTATIONAL_JOINT:
            link->R().noalias() = parent->R() * AngleAxisd(link->q(), link->a());
            arm.noalias() = parent->R() * link->b();
            link->p().noalias() = parent->p() + arm;

            if(calcVelocity){
                const Vector3 sw(parent->R() * link->a());
                link->w().noalias() = parent->w() + sw * link->dq();
                link->v().noalias() = parent->v() + parent->w().cross(arm);

                if(calcAcceleration){
                    link->dw().noalias() = parent->dw() + link->dq() * parent->w().cross(sw) + (link->ddq() * sw);
                    link->dv().noalias() = parent->dv() + parent->w().cross(parent->w().cross(arm)) + parent->dw().cross(arm);
                }
            }
            break;
            
        case Link::SLIDE_JOINT:
            link->R() = parent->R();
            arm.noalias() = parent->R() * (link->b() + link->q() * link->d());
            link->p() = parent->p() + arm;

            if(calcVelocity){
                const Vector3 sv(parent->R() * link->d());
                link->w() = parent->w();
//.........这里部分代码省略.........