예제 #1
0
void JointSliderViewImpl::updateJointPositions()
{
    BodyPtr body = currentBodyItem->body();
    for(size_t i=0; i < activeJointIds.size(); ++i){
        int jointId = activeJointIds[i];
        jointSliders[i]->updatePosition(body->joint(jointId));
    }
}
예제 #2
0
void JointSliderViewImpl::onUnitChanged()
{
    BodyPtr body = currentBodyItem->body();
    for(size_t i=0; i < activeJointIds.size(); ++i){
        int jointId = activeJointIds[i];
        jointSliders[jointId]->initialize(body->joint(jointId));
    }
}
    virtual bool initialize() {

        if(!qseq){
            string filename = getNativePathString(
                boost::filesystem::path(shareDirectory())
                / "motion" / "SR1" / "SR1WalkPattern3.yaml");

            BodyMotion motion;
            if(!motion.loadStandardYAMLformat(filename)){
                os() << motion.seqMessage() << endl;
                return false;
            }
            qseq = motion.jointPosSeq();
            if(qseq->numFrames() == 0){
                os() << "Empty motion data." << endl;
                return false;
            }
            timeStep_ = qseq->getTimeStep();
        }

        if(fabs(timeStep() - timeStep_) > 1.0e-6){
            os() << "Time step must be " << timeStep_ << "." << endl;;
            return false;
        }

        body = ioBody();

        numJoints = body->numJoints();
        if(numJoints != qseq->numParts()){
            os() << "The number of joints must be " << qseq->numParts() << endl;
            return false;
        }

        qold.resize(numJoints);
        VectorXd q0(numJoints);
        VectorXd q1(numJoints);
        for(int i=0; i < numJoints; ++i){
            qold[i] = q0[i] = body->joint(i)->q();
        }
        MultiValueSeq::Frame frame = qseq->frame(0);
        for(int i=0; i < numJoints; ++i){
            q1[i] = frame[i];
        }
        interpolator.clear();
        interpolator.appendSample(0.0, q0);
        interpolator.appendSample(2.0, q1);
        interpolator.update();

        qref_old = interpolator.interpolate(0.0);

        currentFrame = 0;

        phase = 0;
        time = 0.0;

        return true;
    }
 virtual void output() override
 {
     if(ready){
         u.length(ioBody->numJoints());
         for(int i=0; i < ioBody->numJoints(); ++i){
             u[i] = ioBody->joint(i)->u();
         }
         dynamicsSimulator->setCharacterAllLinkData(
             characterName.c_str(), DynamicsSimulator::JOINT_TORQUE, u);
     }
 }
 virtual void input() override
 {
     if(ready){
         dynamicsSimulator->getCharacterAllLinkData(
             characterName.c_str(), DynamicsSimulator::JOINT_VALUE, q);
         int n = std::min((int)q->length(), ioBody->numJoints());
         for(int i=0; i < n; ++i){
             ioBody->joint(i)->q() = q[i];
         }
     }
 }
예제 #6
0
파일: BodyState.cpp 프로젝트: fkanehiro/etc
void BodyState::set(BodyPtr i_body)
{
    Link *root = i_body->rootLink();
    p = root->p;
    R = root->R;
    q.resize(i_body->numJoints());
    for (int i=0; i<i_body->numJoints(); i++){
        Link *joint =  i_body->joint(i);
        if (joint){
            q[i] = joint->q;
        }
    }
}
    virtual bool control()
        {
            BodyPtr io = ioBody();

            if(cnt < 1000){
                io->joint(0)->u() = 0.0;
                io->joint(1)->u() = 0.0;

            } else if(cnt < 3000){
                io->joint(0)->u() = 1.5;
                io->joint(1)->u() = 1.5;	

            } else if(cnt < 4000){
                io->joint(0)->u() =  1.0;
                io->joint(1)->u() = -1.0;	
            } else {
                io->joint(0)->u() = 1.0;	
                io->joint(1)->u() = 1.0;	
            }
            cnt++;
        
            return true;
        }
    virtual bool control() {

        switch(phase){
        case 0 :
            qref = interpolator.interpolate(time);
            if(time > interpolator.domainUpper()){
                phase = 1;
            }
            break;
        case 1:
            if(currentFrame < qseq->numFrames()){
                MultiValueSeq::Frame frame = qseq->frame(currentFrame++);
                for(int i=0; i < numJoints; ++i){
                    qref[i] = frame[i];
                }
            }else{
                phase = 2;
            }
            break;
        case 2 :
            qref = interpolator.interpolate(time);
        }

        for(int i=0; i < body->numJoints(); ++i){
            Link* joint = body->joint(i);
            double q = joint->q();
            double dq_ref = (qref[i] - qref_old[i]) / timeStep_;
            double dq = (q - qold[i]) / timeStep_;
            joint->u() = (qref[i] - q) * pgain[i] + (dq_ref - dq) * dgain[i];
            qold[i] = q;
        }
        qref_old = qref;

        time += timeStep_;

        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;
}
    virtual bool control() {

        switch(phase){
        case 0 :
            qref = interpolator.interpolate(time);
            if(time > interpolator.domainUpper()){
                phase = 1;
            }
            break;
        case 1:
            if(currentFrame < qseq->numFrames()){
                MultiValueSeq::Frame frame = qseq->frame(currentFrame++);
                for(int i=0; i < numJoints; ++i){
                    qref[i] = frame[i];
                }
            }else{
                interpolator.clear();
                interpolator.appendSample(time, qref);
                VectorXd q1(numJoints);
                q1 = qref;
                q1[rarm_shoulder_r] = -0.4;
                q1[rarm_shoulder_p] = 0.75;
                q1[rarm_elbow] = -2.0;
                interpolator.appendSample(time + 3.0, q1);
                q1[rarm_elbow] = -1.57;
                q1[rarm_shoulder_p] = -0.2;
                q1[rarm_wrist_r] = 1.5;
                interpolator.appendSample(time + 5.0, q1);
                q1[rarm_elbow] = -1.3;
                q1[rarm_wrist_y] = -0.24;
                interpolator.appendSample(time + 6.0, q1);
                interpolator.update();
                qref = interpolator.interpolate(time);
                phase = 2;
            }
            break;
        case 2 :
            qref = interpolator.interpolate(time);
            if(time > interpolator.domainUpper()){
                interpolator.clear();
                interpolator.appendSample(time, qref);
                VectorXd q1(numJoints);
                q1 = qref;
                q1[rarm_wrist_y] = 0.0;
                q1[rarm_shoulder_r] = 0.1;
                interpolator.appendSample(time + 5.0, q1);
                interpolator.update();
                qref = interpolator.interpolate(time);
                phase = 3;
            }
            break;
        case 3:
            qref = interpolator.interpolate(time);
            if( rhsensor->F()[1] < -2 ) { 
                interpolator.clear();
                interpolator.appendSample(time, qref);
                VectorXd q1(numJoints);
                q1 = qref;
                q1[rarm_wrist_r] = -0.3;;
                interpolator.appendSample(time + 2.0, q1);
                interpolator.appendSample(time + 2.5, q1);
                q1[rarm_shoulder_p] = -0.13;
                q1[rarm_elbow] = -1.8;
                interpolator.appendSample(time + 3.5, q1);
                interpolator.update();
                qref = interpolator.interpolate(time);
                phase = 4;
            }
            break;
        case 4 :
            qref = interpolator.interpolate(time);
        }

        for(int i=0; i < body->numJoints(); ++i){
            Link* joint = body->joint(i);
            double q = joint->q();
            double dq_ref = (qref[i] - qref_old[i]) / timeStep_;
            double dq = (q - qold[i]) / timeStep_;
            joint->u() = (qref[i] - q) * pgain[i] + (dq_ref - dq) * dgain[i];
            qold[i] = q;
        }
        qref_old = qref;

        time += timeStep_;

        return true;
    }
예제 #11
0
파일: findpath3.cpp 프로젝트: fkanehiro/etc
int main(int argc, char *argv[])
{
    srand((unsigned)time(NULL));

    const char *robotURL = NULL;
    std::vector<std::string> obstacleURL;
    std::vector<Vector3> obstacleP;
    std::vector<Vector3> obstacleRpy;
    for(int i = 1 ; i < argc; i++) {
        if (strcmp(argv[i], "-robot") == 0) {
            robotURL = argv[++i];
        } else if (strcmp(argv[i], "-obstacle") == 0) {
            obstacleURL.push_back(argv[++i]);
            Vector3 p, rpy;
            for (int j=0; j<3; j++) p[j] = atof(argv[++i]);
            obstacleP.push_back(p);
            for (int j=0; j<3; j++) rpy[j] = atof(argv[++i]);
            obstacleRpy.push_back(rpy);
        }
    }
    if (robotURL == NULL) {
        std::cerr << "please specify URL of VRML model by -robot option"
                  << std::endl;
        return 1;
    }


    BodyPtr robot = new Body();
    loadBodyFromModelLoader(robot, robotURL, argc, argv, true);

    problem prob;
    prob.addRobot("robot", robotURL, robot);
    std::vector<BodyPtr> obstacles;
    for (unsigned int i=0; i<obstacleURL.size(); i++) {
        char buf[20];
        sprintf(buf, "obstacle%02d", i);
        obstacles.push_back(prob.addObstacle(buf, obstacleURL[i]));
    }

    // This must be called after all bodies are added
    prob.initOLV(argc, argv);

    PathEngine::Configuration::size(4+6+6);
    PathEngine::Configuration::bounds(0,  0.2, 0.8); // body z
    PathEngine::Configuration::bounds(1, -0.5, 0.5); // body roll
    PathEngine::Configuration::bounds(2, -0.0, 0.5); // body pitch
    PathEngine::Configuration::bounds(3, -0.5, 0.5); // body yaw

    int arm;
    PathEngine::Configuration goalCfg;
    std::ifstream ifs("goal.txt");
    ifs >> arm;
    for (unsigned int i=0; i<PathEngine::Configuration::size(); i++) {
        ifs >> goalCfg[i];
    }

#if 1
    PathEngine::Configuration::weight(0) = 0.1; // z

    PathEngine::Configuration::weight(1) = 1;  // roll
    PathEngine::Configuration::weight(2) = 1;  // pitch
    PathEngine::Configuration::weight(3) = 1;  // yaw

    PathEngine::Configuration::weight(4) = 0.8;
    PathEngine::Configuration::weight(5) = 0.6;
    PathEngine::Configuration::weight(6) = 0.4;
    PathEngine::Configuration::weight(7) = 0.3;
    PathEngine::Configuration::weight(8) = 0.2;
    PathEngine::Configuration::weight(9) = 0.1;

    PathEngine::Configuration::weight(10) = 0.8;
    PathEngine::Configuration::weight(11) = 0.6;
    PathEngine::Configuration::weight(12) = 0.4;
    PathEngine::Configuration::weight(13) = 0.3;
    PathEngine::Configuration::weight(14) = 0.2;
    PathEngine::Configuration::weight(15) = 0.1;
#endif

    JointPathPtr armPath[2];
    Link *chest = robot->link("CHEST_JOINT1");
    Link *wrist[2] = {robot->link("RARM_JOINT5"),
                      robot->link("LARM_JOINT5")
                     };
    for (int k=0; k<2; k++) {
        armPath[k] = robot->getJointPath(chest, wrist[k]);
        for (int i=0; i<armPath[k]->numJoints(); i++) {
            Link *j = armPath[k]->joint(i);
            PathEngine::Configuration::bounds(4+k*6+i, j->llimit, j->ulimit);
        }
    }

    PathEngine::PathPlanner *planner = prob.planner();
    planner->setMobilityName("OmniWheel");
    planner->setAlgorithmName("RRT");
    PathEngine::RRT *rrt = (PathEngine::RRT *)planner->getAlgorithm();
    rrt->extendFromGoal(true);
    planner->getAlgorithm()->setProperty("eps", "0.1");
    planner->getAlgorithm()->setProperty("max-trials", "50000");
    planner->getAlgorithm()->setProperty("interpolation-distance", "0.05");
    prob.initCollisionCheckPairs();
    prob.initPlanner();

    for (unsigned int i=0; i<obstacles.size(); i++) {
        Link *root = obstacles[i]->rootLink();
        root->p = obstacleP[i];
        root->R = rotFromRpy(obstacleRpy[i]);
        root->coldetModel->setPosition(root->R, root->p);
        obstacles[i]->calcForwardKinematics();
    }

    // set halfconf
    dvector halfconf;
    halfconf.setZero(robot->numJoints());
#define ToRad(x) ((x)*M_PI/180)
    halfconf[2] = halfconf[ 8] = ToRad(-40);
    halfconf[3] = halfconf[ 9] = ToRad( 78);
    halfconf[4] = halfconf[10] = ToRad(-38);
    double leg_link_len1=0, leg_link_len2=0;
    halfconf[16] = halfconf[23] = ToRad(20);
    halfconf[17] = ToRad(-10);
    halfconf[24] = -halfconf[17];
    halfconf[19] = halfconf[26] = ToRad(-30);
    if (robot->modelName() == "HRP2") {
        halfconf[20] = ToRad(80);
        halfconf[27] = -halfconf[20];
        halfconf[22] = halfconf[29] = -1.0;
    }
    leg_link_len1 = leg_link_len2 = 0.3;
    double waistHeight = leg_link_len1*cos(halfconf[2])
                         + leg_link_len2*cos(halfconf[4])
                         + 0.105;
    robot->rootLink()->p(2) = waistHeight;
    for (int i=0; i<robot->numJoints(); i++) {
        robot->joint(i)->q = halfconf[i];
    }
    robot->calcForwardKinematics();

    myCfgSetter setter(robot);

    PathEngine::Configuration startCfg;
    startCfg[0] = robot->rootLink()->p[2];
    startCfg[1] = startCfg[2] = startCfg[3] = 0;
    for (int j=0; j<2; j++) {
        for (int i=0; i<armPath[j]->numJoints(); i++) {
            startCfg[4+j*6+i] = armPath[j]->joint(i)->q;
        }
    }
    planner->setStartConfiguration(startCfg);
    planner->setGoalConfiguration(goalCfg);
    planner->setApplyConfigFunc(boost::bind(&myCfgSetter::set, &setter,
                                            _1, _2));
    planner->setConfiguration(startCfg);
    prob.updateOLV();
    planner->setConfiguration(goalCfg);
    prob.updateOLV();
#if 0
    int earm;
    ifs >> earm;
    while (!ifs.eof()) {
        PathEngine::Configuration cfg;
        for (unsigned int i=0; i<PathEngine::Configuration::size(); i++) {
            ifs >> cfg[i];
        }
        if (arm == earm) {
            rrt->addExtraGoal(cfg);
            std::cout << "added an extra goal" << std::endl;
            planner->setConfiguration(cfg);
            prob.updateOLV();
        }
        ifs >> earm;
    }
#endif

    CustomCD cd(robot, "hrp2.shape", "hrp2.pairs",
                obstacles[0], "plant.pc");
    prob.planner()->setCollisionDetector(&cd);

    struct timeval tv1, tv2;
    gettimeofday(&tv1, NULL);
    // plan
    bool ret = planner->calcPath();
    if (ret) {
        for (int i=0; i<5; i++) {
            planner->optimize("Shortcut");
            planner->optimize("RandomShortcut");
        }
    }
    gettimeofday(&tv2, NULL);
    if (ret) {
        const std::vector<PathEngine::Configuration>& postures
            = planner->getWayPoints();
        std::ofstream ofs("path.txt");
        ofs << arm << std::endl;
        for (unsigned int i=0; i<postures.size(); i++) {
            planner->setConfiguration(postures[i]);
            for (int j=0; j<3; j++) {
                ofs << robot->rootLink()->p[j] << " ";
            }
            for (int j=0; j<3; j++) {
                for (int k=0; k<3; k++) {
                    ofs << robot->rootLink()->R(j,k) << " ";
                }
            }
            for (int j=0; j<robot->numJoints(); j++) {
                ofs << robot->joint(j)->q << " ";
            }
            ofs << std::endl;
            prob.updateOLV();
        }
    } else {
        PathEngine::Roadmap *roadmap = planner->getRoadmap();
        for (unsigned int i=0; i<roadmap->nNodes(); i++) {
            PathEngine::RoadmapNode *node = roadmap->node(i);
            planner->setConfiguration(node->position());
            prob.updateOLV();
        }
    }
    double time = (tv2.tv_sec - tv1.tv_sec) + ((double)(tv2.tv_usec - tv1.tv_usec))/1e6;
    std::cout << "total time = " << time << "[s]" << std::endl;
    double cdtime = prob.planner()->timeCollisionCheck();
    std::cout << "time spent in collision detection:"
              << cdtime << "[s](" << cdtime/time*100 << "[%]), "
              << cdtime*1e3/prob.planner()->countCollisionCheck() << "[ms/call]" << std::endl;
    std::cout << "profile:";
    setter.profile();

    return 0;
}
예제 #12
0
 virtual bool control()
     {
         BodyPtr io = ioBody();
         io->joint(0)->u() = -1.0;
         return true;
     }
예제 #13
0
파일: main.cpp 프로젝트: fkanehiro/etc
int main(int argc, char *argv[])
{
    OnlineViewer_var olv;
    olv = hrp::getOnlineViewer(argc, argv);
    olv->load("robot", url);
    olv->clearLog();

    BodyPtr body = BodyPtr(new Body());
    loadBodyFromModelLoader(body, url, argc, argv, true);
    body->setName("robot");
    convertToConvexHull(body);

    WorldState wstate;
    wstate.time = 0.0;
    wstate.characterPositions.length(1);
    setupCharacterPosition(wstate.characterPositions[0], body);
    wstate.collisions.length(3);

    // ワイヤの固定部
    std::vector<Anchor> anchors;
    anchors.push_back(Anchor(body->link("CHEST_Y"), Vector3(-0.03, 0.1,0.35)));
    anchors.push_back(Anchor(body->link("R_HIP_P"), Vector3(-0.08,0,-0.2)));
    anchors.push_back(Anchor(body->link("CHEST_Y"), Vector3(-0.03,-0.1,0.35)));
    anchors.push_back(Anchor(body->link("L_HIP_P"), Vector3(-0.08,0,-0.2)));

    wstate.collisions.length(anchors.size()+1);
    for (size_t i=1; i<wstate.collisions.length(); i++){
        setupFrame(wstate.collisions[i], 0.05);
    }

    // ワイヤと接触するリンクセットその1
    std::vector<Link *> linkset1;
    linkset1.push_back(body->link("CHEST_Y"));
    linkset1.push_back(body->link("WAIST"));
    linkset1.push_back(body->link("R_HIP_P"));

    // ワイヤと接触するリンクセットその2
    std::vector<Link *> linkset2;
    linkset2.push_back(body->link("CHEST_Y"));
    linkset2.push_back(body->link("WAIST"));
    linkset2.push_back(body->link("L_HIP_P"));

    // ワイヤ(両端点及びリンクセット)の定義
    std::vector<String> strings;
    strings.push_back(String(&anchors[0], &anchors[1], linkset1));
    strings.push_back(String(&anchors[2], &anchors[3], linkset2));


    std::ifstream ifs(logfile);
    if (!ifs.is_open()){
        std::cerr << "failed to open the log file:" << logfile << std::endl;
        return 1;
    }
    char buf[1024];
    ifs.getline(buf, 1024); // skip header
    double q;

    while(1){
        for (int i=0; i<body->numJoints(); i++){
            ifs >> q;
            if (ifs.eof()) return 0;
            Link *j = body->joint(i);
            if (j) j->q = q;
        }
        ifs.getline(buf, 1024); // skip rest of the line
        body->calcForwardKinematics();

        for (size_t i=0; i<strings.size(); i++){ 
            if (!strings[i].update()){
                std::cerr << "failed to update string state" << std::endl;
            }
        }
        // update body
        updateCharacterPosition(wstate.characterPositions[0], body);
        // update anchors
        for (size_t i=0; i<anchors.size(); i++){
            updateFrame(wstate.collisions[i+1], 
                        anchors[i].position(), Matrix33::Identity());
        }
        // update strings
        size_t n=0, index=0;
        for (size_t i=0; i<strings.size(); i++){
            n += strings[i].polyLine.lines.size();
        }
        wstate.collisions[0].points.length(n);
        for (size_t i=0; i<strings.size(); i++){
            const std::vector<LineSegment>& lines = strings[i].polyLine.lines;
            for (size_t i=0; i<lines.size(); i++){
                updateLineSegment(wstate.collisions[0].points[index++],
                                  lines[i].first, lines[i].second);
            }
        }
        olv->update(wstate);
        wstate.time += 0.005;
        std::cout << wstate.time << " ";
        for (size_t i=0; i<strings.size(); i++){
            std::cout << strings[i].length() << " ";
        }
        std::cout << std::endl;
    }
    return 0;
}
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;
}
예제 #15
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;
    }
}
예제 #16
0
void JointSliderViewImpl::updateSliderGrid()
{
    if(!currentBodyItem){
        initializeSliders(0);

    } else {

        BodyPtr body = currentBodyItem->body();
        int numJoints = body->numJoints();
        
        if(!showAllToggle.isChecked()){
            const boost::dynamic_bitset<>& linkSelection =
                LinkSelectionView::mainInstance()->linkSelection(currentBodyItem);
            activeJointIds.clear();
            for(int i=0; i < numJoints; ++i){
                Link* joint = body->joint(i);
                if(joint->isValid() && linkSelection[joint->index()]){
                    activeJointIds.push_back(i);
                }
            }
        } else {
            activeJointIds.resize(numJoints);
            for(int i=0; i < numJoints; ++i){
                activeJointIds[i] = i;
            }
        }

        int n = activeJointIds.size();
        
        initializeSliders(n);

        int nColumns = numColumnsSpin.value();
        bool isLabelAtLeft = labelOnLeftToggle.isChecked();
        int nUnitColumns, nGridColumns;
        if(isLabelAtLeft){
            nUnitColumns = 7;
            nGridColumns = nColumns * nUnitColumns;
        } else {
            nUnitColumns = 6;
            nGridColumns = nColumns * nUnitColumns;
        }

        int row = 0;
        int col = 0;

        for(int i=0; i < n; ++i){

            SliderUnit* unit = jointSliders[i];

            unit->initialize(body->joint(activeJointIds[i]));
            
            if(!isLabelAtLeft){
                sliderGrid.addWidget(&unit->nameLabel, row, col, 1, nUnitColumns);
                sliderGrid.addWidget(&unit->idLabel, row + 1, col);
                attachSliderUnits(unit, row + 1, col + 1);
                col += nUnitColumns;
                if(col == nGridColumns){
                    col = 0;
                    row += 2;
                }
            } else {
                sliderGrid.addWidget(&unit->idLabel,row, col);
                sliderGrid.addWidget(&unit->nameLabel,row, col + 1);
                attachSliderUnits(unit, row, col + 2);
                col += nUnitColumns;
                if(col == nGridColumns){
                    col = 0;
                    row += 1;
                }
            }
        }
    }
}