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

    LogManager<OpenHRP::SceneState> *lm 
        = (LogManager<OpenHRP::SceneState> *)m_log;
    OpenHRP::SceneState &ss = lm->state();
    for (unsigned int i=0; i<ss.states.length(); i++){
        OpenHRP::RobotState &rs = ss.states[i];
        GLbody *glbody = dynamic_cast<GLbody *>(body(i).get());
        glbody->setPosition(rs.basePose.position.x,
                            rs.basePose.position.y,
                            rs.basePose.position.z);
        glbody->setRotation(rs.basePose.orientation.r,
                            rs.basePose.orientation.p,
                            rs.basePose.orientation.y);
        glbody->setPosture(rs.q.get_buffer());
    }
}
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;
        }
    }
}