Пример #1
0
void GLscene::updateScene()
{
    if (m_log->index()<0) return;

#ifdef USE_COLLISION_STATE
    LogManager<OpenHRP::CollisionDetectorService::CollisionState> *lm
        = (LogManager<OpenHRP::CollisionDetectorService::CollisionState> *)m_log;
    GLbody *glbody = dynamic_cast<GLbody *>(body(0).get());
    OpenHRP::CollisionDetectorService::CollisionState &co = lm->state();
    if (co.angle.length() == glbody->numJoints()){
        for (int i=0; i<glbody->numJoints(); i++){
            GLlink *j = (GLlink *)glbody->joint(i);
            if (j){
                j->setQ(co.angle[i]);
            }
        }
    }
#else
    LogManager<TimedPosture> *lm 
        = (LogManager<TimedPosture> *)m_log;
    GLbody *glbody = dynamic_cast<GLbody *>(body(0).get());
    TimedPosture &ts = lm->state();
    if (ts.posture.size() == glbody->numJoints()){
        for (int i=0; i<glbody->numJoints(); i++){
            GLlink *j = (GLlink *)glbody->joint(i);
            if (j){
                j->setQ(ts.posture[i]);
            }
        }
    }
#endif
}
Пример #2
0
void GLscene::showStatus()
{
    if (m_log->index()<0) return;

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

    if (m_showingStatus){
        GLbody *glbody = NULL;
        OpenHRP::RobotState *rstate = NULL;
        for (unsigned int i=0; i<numBodies(); i++){
            if (body(i)->numJoints()){
                glbody = dynamic_cast<GLbody *>(body(i).get());
                rstate = &sstate.states[i];
                break;
            }
        }
#define HEIGHT_STEP 12
        int width = m_width - 410;
        int height = m_height-HEIGHT_STEP;
        char buf[256];
        for (unsigned int i=0; i<glbody->numJoints(); i++){
            hrp::Link *l = glbody->joint(i);
            if (l){
                int x = width;
                // joint ID
                sprintf(buf, "%2d",i);
                glRasterPos2f(x, height);
                drawString2(buf);
                white();
                x += 8*3;
                // joint name, current angle
                sprintf(buf, "%13s %8.3f", 
                        l->name.c_str(), 
                        rstate->q[i]*180/M_PI);
                glRasterPos2f(x, height);
                drawString2(buf);
                x += 8*(14+9);
                
                height -= HEIGHT_STEP;
            }
        }
        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);
    }
}
Пример #3
0
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<TimedRobotState> *lm 
        = (LogManager<TimedRobotState> *)m_log;
    OpenHRP::RobotHardwareService::RobotState &rstate = lm->state().state;

    if (m_showingStatus){
        GLbody *glbody = dynamic_cast<GLbody *>(body(0).get());
#define HEIGHT_STEP 12
        int width = m_width - 410;
        int height = m_height-HEIGHT_STEP;
        char buf[256];
        for (int i=0; i<glbody->numJoints(); i++){
            hrp::Link *l = glbody->joint(i);
            if (l){
                int ss = rstate.servoState[i][0];
                int x = width;
                // joint ID
                sprintf(buf, "%2d",i);
                if (!isCalibrated(ss)){
                    yellow();
                }else if(isServoOn(ss)){
                    red();
                }
                glRasterPos2f(x, height);
                drawString2(buf);
                white();
                x += 8*3;
                // power status
                if (isPowerOn(ss)) blue();
                glRasterPos2f(x, height);
                drawString2("o");
                if (isPowerOn(ss)) white();
                x += 8*2;
                // joint name, current angle, command angle and torque
                sprintf(buf, "%13s %8.3f %8.3f %6.1f", 
                        l->name.c_str(), 
                        rstate.angle[i]*180/M_PI,
                        rstate.command[i]*180/M_PI,
                        rstate.torque[i]*180/M_PI);
                glRasterPos2f(x, height);
                drawString2(buf);
                x += 8*(14+9+9+7);
                // servo alarms
                sprintf(buf, "%03x", servoAlarm(ss));
                glRasterPos2f(x, height);
                drawString2(buf);
                x += 8*4;
                // driver temperature
                int temp = temperature(ss);
                if (!temp){
                    sprintf(buf, "--", temp);
                }else{
                    sprintf(buf, "%2d", temp);
                }
                if (temp >= 60) red();
                glRasterPos2f(x, height);
                drawString2(buf);
                if (temp >= 60) white();
                x += 8*3;
                
                height -= HEIGHT_STEP;
            }
        }
        if (rstate.accel.length()){
            glRasterPos2f(width, height);
            height -= HEIGHT_STEP;
            drawString2("acc:");
            for (unsigned int i=0; i<rstate.accel.length(); i++){
                sprintf(buf, "  %8.4f %8.4f %8.4f",
                        rstate.accel[i][0], rstate.accel[i][1], rstate.accel[i][2]);
                glRasterPos2f(width, height);
                height -= HEIGHT_STEP;
                drawString2(buf);
            }
        }
        if (rstate.rateGyro.length()){
            glRasterPos2f(width, height);
            height -= HEIGHT_STEP;
            drawString2("rate:");
            for (unsigned int i=0; i<rstate.rateGyro.length(); i++){
                sprintf(buf, "  %8.4f %8.4f %8.4f",
                        rstate.rateGyro[i][0], rstate.rateGyro[i][1], rstate.rateGyro[i][2]);
                glRasterPos2f(width, height);
                height -= HEIGHT_STEP;
                drawString2(buf);
            }
        }
        if (rstate.force.length()){
            glRasterPos2f(width, height);
            height -= HEIGHT_STEP;
            drawString2("force/torque:");
            for (unsigned int i=0; i<rstate.force.length(); i++){
                sprintf(buf, "  %6.1f %6.1f %6.1f %6.2f %6.2f %6.2f",
                        rstate.force[i][0], 
                        rstate.force[i][1], 
                        rstate.force[i][2],
                        rstate.force[i][3], 
                        rstate.force[i][4], 
                        rstate.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);
    }else{
        // !m_showingRobotState
        bool servo=false, power=false;
        for (unsigned int i=0; i<rstate.servoState.length(); i++){
            if (isServoOn(rstate.servoState[i][0])) servo = true;
            if (isPowerOn(rstate.servoState[i][0])) power = true;
        }
        struct timeval tv;
        gettimeofday(&tv, NULL);
        double dt = tv.tv_sec + tv.tv_usec/1e6 - lm->time(m_log->length()-1);
        if (dt < 1.0) green(); else black();
        glRectf(m_width-115,m_height-45,m_width-85,m_height-15);
        if (power) blue(); else black();
        glRectf(m_width- 80,m_height-45,m_width-50,m_height-15);
        if (servo) red(); else black();
        glRectf(m_width- 45,m_height-45,m_width-15,m_height-15);
    }
}
Пример #5
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);
    }
}