示例#1
0
bool PoseSeqItem::updateKeyPosesWithBalancedTrajectories(std::ostream& os)
{
    BodyMotionPtr motion = bodyMotionItem()->motion();
    MultiValueSeqPtr qseq = motion->jointPosSeq();
    MultiSE3SeqPtr pseq = motion->linkPosSeq();

    double length = seq->endingTime();
    
    if(qseq->timeLength() < length || pseq->timeLength() < length){
        os << "Length of the interpolated trajectories is shorter than key pose sequence.";
        return false;
    }
    if(pseq->numParts() < ownerBodyItem->body()->numLinks()){
        os << "Not all link positions are available. Please do interpolate with \"Put all link positions\"";
        return false;
    }

    beginEditing();
    
    for(PoseSeq::iterator p = seq->begin(); p != seq->end(); ++p){
        PosePtr pose = p->get<Pose>();
        if(pose){

            seq->beginPoseModification(p);
            
            int nj = pose->numJoints();
            int frame = qseq->frameOfTime(p->time());
            MultiValueSeq::Frame q = qseq->frame(frame);
            for(int i=0; i < nj; ++i){
                if(pose->isJointValid(i)){
                    pose->setJointPosition(i, q[i]);
                }
            }
            MultiSE3Seq::Frame pos = pseq->frame(frame);
            for(Pose::LinkInfoMap::iterator q = pose->ikLinkBegin(); q != pose->ikLinkEnd(); ++q){
                int linkIndex = q->first;
                Pose::LinkInfo& info = q->second;
                const Vector3& p = pos[linkIndex].translation();
                // only update horizontal position
                info.p[0] = p[0];
                info.p[1] = p[1];
            }

            seq->endPoseModification(p);
        }
    }

    endEditing();

    updateInterpolation();
    
    return true;
}
void OnlineViewerServerImpl::resetLogItem(BodyItemInfo* info, BodyMotionItem* newLogItem)
{
    info->logItemConnections.disconnect();
    
    info->logItem = newLogItem;

    if(newLogItem){
        BodyMotionPtr motion = newLogItem->motion();
        motion->jointPosSeq()->setDimension(0, 0);
        motion->linkPosSeq()->setNumParts(info->bodyItem->body()->numLinks());
        motion->setFrameRate(timeBar->frameRate());

        info->logItemConnections.add(
            newLogItem->sigPositionChanged().connect(
                boost::bind(&OnlineViewerServerImpl::resetLogItem, this, info, (BodyMotionItem*)0)));
        info->logItemConnections.add(
            newLogItem->sigNameChanged().connect(
                boost::bind(&OnlineViewerServerImpl::resetLogItem, this, info, (BodyMotionItem*)0)));
    }
}
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;
}