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;
}
}
}
