void GLscene::updateScene()
{ 
    if (m_log->index()<0) return;

    LogManager<OpenHRP::WorldState> *lm 
        = (LogManager<OpenHRP::WorldState> *)m_log;
    OpenHRP::WorldState &state = lm->state();
    for (unsigned int i=0; i<state.characterPositions.length(); i++){
        const CharacterPosition& cpos = state.characterPositions[i];
        std::string cname(cpos.characterName);
        GLbody *glbody = dynamic_cast<GLbody *>(body(cname).get());
        if (!glbody) {
            //std::cerr << "can't find a body named " << cname << std::endl;
            continue;
        }
        for (unsigned int j=0; j<cpos.linkPositions.length(); j++){
            const LinkPosition &lp = cpos.linkPositions[j];
            double T[] = {lp.R[0], lp.R[3], lp.R[6],0,
                          lp.R[1], lp.R[4], lp.R[7],0,
                          lp.R[2], lp.R[5], lp.R[8],0,
                          lp.p[0], lp.p[1], lp.p[2],1};
#if 0
            for (int i=0; i<4; i++){
                for (int j=0; j<4; j++){
                    printf("%6.3f ", T[i*4+j]);
                }
                printf("\n");
            }
            printf("\n");
#endif
            ((GLlink *)glbody->link(j))->setAbsTransform(T);
        }
    }
}
示例#2
0
void GLscene::showStatus()
{
    char buf[256];

    GLbody *glbody = dynamic_cast<GLbody *>(body(0).get());
    int width = m_width - 220;
#define HEIGHT_STEP 12
    int height = m_height-HEIGHT_STEP;
    int x = width;

    for (int i=0; i<glbody->numLinks(); i++){
        hrp::Link *l = glbody->link(i);
        if (l){
            sprintf(buf, "%13s %4d tris",
                    l->name.c_str(),
                    l->coldetModel->getNumTriangles());
            glRasterPos2f(x, height);
            drawString(buf);
            height -= HEIGHT_STEP;
        }
    }

    if (m_log->index()<0) return;

    LogManager<OpenHRP::CollisionDetectorService::CollisionState> *lm
        = (LogManager<OpenHRP::CollisionDetectorService::CollisionState> *)m_log;
    OpenHRP::CollisionDetectorService::CollisionState &co = lm->state();

    height -= HEIGHT_STEP;

    x = width - 34;
    sprintf(buf, "Number of pair     %8d",  co.lines.length());
    glRasterPos2f(x, height);
    drawString(buf);
    height -= HEIGHT_STEP;

    sprintf(buf, "Calc Time [msec]   %8.3f",  co.computation_time);
    glRasterPos2f(x, height);
    drawString(buf);
    height -= HEIGHT_STEP;

    sprintf(buf, "Recover Time[msec] %8.3f",  co.recover_time);
    glRasterPos2f(x, height);
    drawString(buf);
    height -= HEIGHT_STEP;

    sprintf(buf, "Safe Posture       %8s",  co.safe_posture?"true":"false");
    glRasterPos2f(x, height);
    drawString(buf);
    height -= HEIGHT_STEP;

    sprintf(buf, "Loop for check     %8d",  co.loop_for_check);
    glRasterPos2f(x, height);
    drawString(buf);
    height -= HEIGHT_STEP;

}
void GLscene::updateScene()
{
    if (m_log->index()<0) return;

    LogManager<TimedRobotState> *lm 
        = (LogManager<TimedRobotState> *)m_log;
    GLbody *glbody = dynamic_cast<GLbody *>(body(0).get());
    OpenHRP::StateHolderService::Command &com = lm->state().command;
    if (com.baseTransform.length() == 12){
        double *tform = com.baseTransform.get_buffer();
        glbody->setPosition(tform);
        glbody->setRotation(tform+3);
        hrp::Link *root = glbody->rootLink();
        root->p << tform[0], tform[1], tform[2];
        root->R << tform[3], tform[4], tform[5],
            tform[6], tform[7], tform[8],
            tform[9], tform[10], tform[11];
    }
    if (com.jointRefs.length() == glbody->numJoints()){
        for (int i=0; i<glbody->numJoints(); i++){
            GLlink *j = (GLlink *)glbody->joint(i);
            if (j){
                j->q = com.jointRefs[i];
                j->setQ(com.jointRefs[i]);
            }
        }
    }
    glbody->calcForwardKinematics();
    glbody->updateLinkColdetModelPositions();
    for (int i=0; i<glbody->numLinks(); i++){
        ((GLlink *)glbody->link(i))->highlight(false);
    }
    for (size_t i=0; i<m_pairs.size(); i++){
        if (m_pairs[i]->checkCollision()){
            ((GLlink *)m_pairs[i]->link(0))->highlight(true);
            ((GLlink *)m_pairs[i]->link(1))->highlight(true);
            std::cout << m_pairs[i]->link(0)->name << "<->" << m_pairs[i]->link(1)->name << std::endl;
        }
    }
}
示例#4
0
void GLscene::showStatus()
{
    if (m_log->index()<0) return;

    LogManager<SceneState> *lm 
        = (LogManager<SceneState> *)m_log;
    SceneState &state = lm->state();

    if (m_showingStatus){
        GLbody *glbody = NULL;
        BodyState *bstate = NULL;
        for (unsigned int i=0; i<numBodies(); i++){
            if (body(i)->numJoints()){
                glbody = dynamic_cast<GLbody *>(body(i).get());
                bstate = &state.bodyStates[i];
                break;
            }
        }
        if (!glbody) return;
#define HEIGHT_STEP 12
        int width = m_width - 350;
        int height = m_height-HEIGHT_STEP;
        char buf[256];
        double q[glbody->numJoints()];
        for (int i=0; i<glbody->numLinks(); i++){
            Link* l = glbody->link(i);
            if (l->jointId >= 0) q[l->jointId] = bstate->q[i];
        }
        for (int i=0; i<glbody->numJoints(); i++){
            GLlink *l = (GLlink *)glbody->joint(i);
            if (l){
                sprintf(buf, "%2d %15s %8.3f", i, l->name.c_str(),
                        q[i]*180/M_PI);
                glRasterPos2f(width, height);
                height -= HEIGHT_STEP;
                drawString2(buf);
            }
        }
        if (bstate->acc.size()){
            glRasterPos2f(width, height);
            height -= HEIGHT_STEP;
            drawString2("acc:");
            for (unsigned int i=0; i<bstate->acc.size(); i++){
                sprintf(buf, "  %8.4f %8.4f %8.4f",
                        bstate->acc[i][0], bstate->acc[i][1], bstate->acc[i][2]);
                glRasterPos2f(width, height);
                height -= HEIGHT_STEP;
                drawString2(buf);
            }
        }
        if (bstate->rate.size()){
            glRasterPos2f(width, height);
            height -= HEIGHT_STEP;
            drawString2("rate:");
            for (unsigned int i=0; i<bstate->rate.size(); i++){
                sprintf(buf, "  %8.4f %8.4f %8.4f",
                        bstate->rate[i][0], bstate->rate[i][1], bstate->rate[i][2]);
                glRasterPos2f(width, height);
                height -= HEIGHT_STEP;
                drawString2(buf);
            }
        }
        if (bstate->force.size()){
            glRasterPos2f(width, height);
            height -= HEIGHT_STEP;
            drawString2("force/torque:");
            for (unsigned int i=0; i<bstate->force.size(); i++){
                sprintf(buf, "  %6.1f %6.1f %6.1f %6.2f %6.2f %6.2f",
                        bstate->force[i][0], 
                        bstate->force[i][1], 
                        bstate->force[i][2],
                        bstate->force[i][3], 
                        bstate->force[i][4], 
                        bstate->force[i][5]);
                glRasterPos2f(width, height);
                height -= HEIGHT_STEP;
                drawString2(buf);
            }
        }
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
        glEnable(GL_BLEND);
        glColor4f(0.0,0.0,0.0, 0.5);
        if (m_showSlider){
            glRectf(width,SLIDER_AREA_HEIGHT,m_width,m_height);
        }else{
            glRectf(width,0,m_width,m_height);
        }
        glDisable(GL_BLEND);
    }
}