ForwardDynamicsABM::ForwardDynamicsABM(BodyPtr body) :
    ForwardDynamics(body),
    q0(body->numLinks()),
    dq0(body->numLinks()),
    dq(body->numLinks()),
    ddq(body->numLinks())
{

}
bool PoseProviderToBodyMotionConverter::convert(BodyPtr body, PoseProvider* provider, BodyMotion& motion)
{
    const double frameRate = motion.frameRate();
    const int beginningFrame = static_cast<int>(frameRate * std::max(provider->beginningTime(), lowerTime));
    const int endingFrame = static_cast<int>(frameRate * std::min(provider->endingTime(), upperTime));
    const int numJoints = body->numJoints();
    const int numLinksToPut = (allLinkPositionOutputMode ? body->numLinks() : 1);
    
    motion.setDimension(endingFrame + 1, numJoints, numLinksToPut, true);

    MultiValueSeq& qseq = *motion.jointPosSeq();
    MultiAffine3Seq& pseq = *motion.linkPosSeq();
    Vector3Seq& relZmpSeq = *motion.relativeZmpSeq();
    bool isZmpValid = false;

    Link* rootLink = body->rootLink();
    Link* baseLink = rootLink;

    shared_ptr<LinkTraverse> fkTraverse;
    if(allLinkPositionOutputMode){
        fkTraverse.reset(new LinkTraverse(baseLink, true, true));
    } else {
        fkTraverse.reset(new LinkPath(baseLink, rootLink));
    }

    // store the original state
    vector<double> orgq(numJoints);
    for(int i=0; i < numJoints; ++i){
        orgq[i] = body->joint(i)->q;
    }
    Affine3 orgp;
    orgp.translation() = rootLink->p;
    orgp.linear() = rootLink->R;

    std::vector< boost::optional<double> > jointPositions(numJoints);

    for(int frame = beginningFrame; frame <= endingFrame; ++frame){

        provider->seek(frame / frameRate);

        const int baseLinkIndex = provider->baseLinkIndex();
        if(baseLinkIndex >= 0){
            if(baseLinkIndex != baseLink->index){
                baseLink = body->link(baseLinkIndex);
                if(allLinkPositionOutputMode){
                    fkTraverse->find(baseLink, true, true);
                } else {
                    static_pointer_cast<LinkPath>(fkTraverse)->find(baseLink, rootLink);
                }
            }
            provider->getBaseLinkPosition(baseLink->p, baseLink->R);
        }

        MultiValueSeq::View qs = qseq.frame(frame);
        provider->getJointPositions(jointPositions);
        for(int i=0; i < numJoints; ++i){
            const optional<double>& q = jointPositions[i];
            qs[i] = q ? *q : 0.0;
            body->joint(i)->q = qs[i];
        }

        if(allLinkPositionOutputMode || baseLink != rootLink){
            fkTraverse->calcForwardKinematics();
        }

        for(int i=0; i < numLinksToPut; ++i){
            Affine3& p = pseq.at(frame, i);
            Link* link = body->link(i);
            p.translation() = link->p;
            p.linear() = link->R;
        }

        optional<Vector3> zmp = provider->zmp();
        if(zmp){
            relZmpSeq[frame].noalias() = rootLink->R.transpose() * (*zmp - rootLink->p);
            isZmpValid = true;
        }

    }

    if(!isZmpValid){
        //bodyMotionItem->clearRelativeZmpSeq();
    }

    // restore the original state
    for(int i=0; i < numJoints; ++i){
        body->joint(i)->q = orgq[i];
    }
    rootLink->p = orgp.translation();
    rootLink->R = orgp.linear();
    body->calcForwardKinematics();

    return true;
}
int KinematicFaultCheckerImpl::checkFaults
(BodyItem* bodyItem, BodyMotionItem* motionItem, std::ostream& os,
 bool checkPosition, bool checkVelocity, bool checkCollision, dynamic_bitset<> linkSelection,
 double beginningTime, double endingTime)
{
    numFaults = 0;

    BodyPtr body = bodyItem->body();
    BodyMotionPtr motion = motionItem->motion();
    MultiValueSeqPtr qseq = motion->jointPosSeq();;
    MultiSE3SeqPtr pseq = motion->linkPosSeq();
    
    if((!checkPosition && !checkVelocity && !checkCollision) || body->isStaticModel() || !qseq->getNumFrames()){
        return numFaults;
    }

    BodyState orgKinematicState;
    
    if(USE_DUPLICATED_BODY){
        body = body->clone();
    } else {
        bodyItem->storeKinematicState(orgKinematicState);
    }

    CollisionDetectorPtr collisionDetector;
    WorldItem* worldItem = bodyItem->findOwnerItem<WorldItem>();
    if(worldItem){
        collisionDetector = worldItem->collisionDetector()->clone();
    } else {
        int index = CollisionDetector::factoryIndex("AISTCollisionDetector");
        if(index >= 0){
            collisionDetector = CollisionDetector::create(index);
        } else {
            collisionDetector = CollisionDetector::create(0);
            os << _("A collision detector is not found. Collisions cannot be detected this time.") << endl;
        }
    }

    addBodyToCollisionDetector(*body, *collisionDetector);
    collisionDetector->makeReady();

    const int numJoints = std::min(body->numJoints(), qseq->numParts());
    const int numLinks = std::min(body->numLinks(), pseq->numParts());

    frameRate = motion->frameRate();
    double stepRatio2 = 2.0 / frameRate;
    angleMargin = radian(angleMarginSpin.value());
    translationMargin = translationMarginSpin.value();
    velocityLimitRatio = velocityLimitRatioSpin.value() / 100.0;

    int beginningFrame = std::max(0, (int)(beginningTime * frameRate));
    int endingFrame = std::min((motion->numFrames() - 1), (int)lround(endingTime * frameRate));

    lastPosFaultFrames.clear();
    lastPosFaultFrames.resize(numJoints, std::numeric_limits<int>::min());
    lastVelFaultFrames.clear();
    lastVelFaultFrames.resize(numJoints, std::numeric_limits<int>::min());
    lastCollisionFrames.clear();

    if(checkCollision){
        Link* root = body->rootLink();
        root->p().setZero();
        root->R().setIdentity();
    }
        
    for(int frame = beginningFrame; frame <= endingFrame; ++frame){

        int prevFrame = (frame == beginningFrame) ? beginningFrame : frame - 1;
        int nextFrame = (frame == endingFrame) ? endingFrame : frame + 1;

        for(int i=0; i < numJoints; ++i){
            Link* joint = body->joint(i);
            double q = qseq->at(frame, i);
            joint->q() = q;
            if(joint->index() >= 0 && linkSelection[joint->index()]){
                if(checkPosition){
                    bool fault = false;
                    if(joint->isRotationalJoint()){
                        fault = (q > (joint->q_upper() - angleMargin) || q < (joint->q_lower() + angleMargin));
                    } else if(joint->isSlideJoint()){
                        fault = (q > (joint->q_upper() - translationMargin) || q < (joint->q_lower() + translationMargin));
                    }
                    if(fault){
                        putJointPositionFault(frame, joint, os);
                    }
                }
                if(checkVelocity){
                    double dq = (qseq->at(nextFrame, i) - qseq->at(prevFrame, i)) / stepRatio2;
                    joint->dq() = dq;
                    if(dq > (joint->dq_upper() * velocityLimitRatio) || dq < (joint->dq_lower() * velocityLimitRatio)){
                        putJointVelocityFault(frame, joint, os);
                    }
                }
            }
        }

        if(checkCollision){

            Link* link = body->link(0);
            if(!pseq->empty())
            {
                const SE3& p = pseq->at(frame, 0);
                link->p() = p.translation();
                link->R() = p.rotation().toRotationMatrix();
            }
            else
            {
                link->p() = Vector3d(0., 0., 0.);
                link->R() = Matrix3d::Identity();
            }

            body->calcForwardKinematics();

            for(int i=1; i < numLinks; ++i){
                link = body->link(i);
                if(!pseq->empty())
                {
                    const SE3& p = pseq->at(frame, i);
                    link->p() = p.translation();
                    link->R() = p.rotation().toRotationMatrix();
                }
            }

            for(int i=0; i < numLinks; ++i){
                link = body->link(i);
                collisionDetector->updatePosition(i, link->position());
            }
            collisionDetector->detectCollisions(
                boost::bind(&KinematicFaultCheckerImpl::putSelfCollision, this, body.get(), frame, _1, boost::ref(os)));
        }
    }

    if(!USE_DUPLICATED_BODY){
        bodyItem->restoreKinematicState(orgKinematicState);
    }

    return numFaults;
}
Пример #4
0
/**
   @brief compute CoM Jacobian
   @param base link fixed to the environment
   @param J CoM Jacobian
   @note Link::wc must be computed by calcCM() before calling
*/
void calcCMJacobian(const BodyPtr& body, Link* base, Eigen::MatrixXd& J)
{
    // prepare subm, submwc

    const int nj = body->numJoints();
    vector<SubMass> subMasses(body->numLinks());
    Link* rootLink = body->rootLink();

    JointPath path;
    if(!base) {
        calcSubMass(rootLink, subMasses);
        J.resize(3, nj + 6);

    } else {
        path.setPath(rootLink, base);
        Link* skip = path.joint(0);
        SubMass& sub = subMasses[skip->index()];
        sub.m = rootLink->m();
        sub.mwc = rootLink->m() * rootLink->wc();

        for(Link* child = rootLink->child(); child; child = child->sibling()) {
            if(child != skip) {
                calcSubMass(child, subMasses);
                subMasses[skip->index()] += subMasses[child->index()];
            }
        }

        // assuming there is no branch between base and root
        for(int i=1; i < path.numJoints(); i++) {
            Link* joint = path.joint(i);
            const Link* parent = joint->parent();
            SubMass& sub = subMasses[joint->index()];
            sub.m = parent->m();
            sub.mwc = parent->m() * parent->wc();
            sub += subMasses[parent->index()];
        }

        J.resize(3, nj);
    }

    // compute Jacobian
    std::vector<int> sgn(nj, 1);
    for(int i=0; i < path.numJoints(); i++) {
        sgn[path.joint(i)->jointId()] = -1;
    }

    for(int i=0; i < nj; i++) {
        Link* joint = body->joint(i);
        if(joint->isRotationalJoint()) {
            const Vector3 omega = sgn[joint->jointId()] * joint->R() * joint->a();
            const SubMass& sub = subMasses[joint->index()];
            const Vector3 arm = (sub.mwc - sub.m * joint->p()) / body->mass();
            const Vector3 dp = omega.cross(arm);
            J.col(joint->jointId()) = dp;
        } else {
            std::cerr << "calcCMJacobian() : unsupported jointType("
                      << joint->jointType() << std::endl;
        }
    }

    if(!base) {
        const int c = nj;
        J.block(0, c, 3, 3).setIdentity();

        const Vector3 dp = subMasses[0].mwc / body->mass() - rootLink->p();

        J.block(0, c + 3, 3, 3) <<
                                0.0,  dp(2), -dp(1),
                                      -dp(2),    0.0,  dp(0),
                                      dp(1), -dp(0),    0.0;
    }
}