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