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); } } }
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::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); } }
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<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<SceneState> *lm = (LogManager<SceneState> *)m_log; SceneState &state = lm->state(); for (unsigned int i=0; i<state.bodyStates.size(); i++){ const BodyState& bstate = state.bodyStates[i]; GLbody *glbody = dynamic_cast<GLbody *>(body(i).get()); glbody->setPosture(bstate.q, bstate.p, bstate.R); if (m_showSensors){ glbody->setSensorDrawCallback( boost::bind(&GLscene::drawSensorOutput, this, _1, _2)); }else{ glbody->setSensorDrawCallback(NULL); } } }
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; } } }
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); } }
size_t GLlink::draw(){ size_t ntri=0; glPushMatrix(); if (m_useAbsTransformToDraw){ glMultMatrixd(m_absTrans); }else{ glMultMatrixd(m_trans); glMultMatrixd(m_T_j); } if (m_drawMode != DM_COLLISION){ for (size_t i=0; i<m_shapes.size(); i++){ ntri += m_shapes[i]->draw(m_drawMode); } for (size_t i=0; i<m_cameras.size(); i++){ ntri += m_cameras[i]->draw(m_drawMode); } }else{ if (coldetModel && coldetModel->getNumTriangles()){ ntri = coldetModel->getNumTriangles(); Eigen::Vector3f n, v[3]; int vindex[3]; if (m_highlight){ float red[] = {1,0,0,1}; glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, red); }else{ float gray[] = {0.8,0.8,0.8,1}; glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, gray); } glBegin(GL_TRIANGLES); for (int i=0; i<coldetModel->getNumTriangles(); i++){ coldetModel->getTriangle(i, vindex[0], vindex[1], vindex[2]); for (int j=0; j<3; j++){ coldetModel->getVertex(vindex[j], v[j][0], v[j][1], v[j][2]); } n = (v[1]-v[0]).cross(v[2]-v[0]); n.normalize(); glNormal3fv(n.data()); for (int j=0; j<3; j++){ glVertex3fv(v[j].data()); } } glEnd(); } } for (size_t i=0; i<sensors.size(); i++){ Sensor *s = sensors[i]; double T[16]; for (int i=0; i<3; i++){ for (int j=0; j<3; j++){ T[i*4+j] = s->localR(j,i); } } T[12] = s->localPos[0]; T[13] = s->localPos[1]; T[14] = s->localPos[2]; T[3] = T[7] = T[11] = 0.0; T[15] = 1.0; glPushMatrix(); glMultMatrixd(T); GLbody *glb = dynamic_cast<GLbody *>(body); glb->drawSensor(s); glPopMatrix(); } for (size_t i=0; i<lights.size(); i++){ Light *l = lights[i]; int lid = GL_LIGHT0+l->id; if (!l->on){ glDisable(lid); continue; } double T[16]; for (int j=0; j<3; j++){ for (int k=0; k<3; k++){ T[j*4+k] = l->localR(k,j); } } T[12] = l->localPos[0]; T[13] = l->localPos[1]; T[14] = l->localPos[2]; T[3] = T[7] = T[11] = 0.0; T[15] = 1.0; glPushMatrix(); glMultMatrixd(T); glEnable(GL_LIGHTING); glEnable(lid); GLfloat pos[] = {0,0,0,1}; glLightfv(lid, GL_POSITION, pos); GLfloat color[] = {l->color[0], l->color[1], l->color[2], 1}; glLightfv(lid, GL_DIFFUSE, color); if (l->type == POINT){ glLightf(lid, GL_CONSTANT_ATTENUATION, l->attenuation[0]); glLightf(lid, GL_LINEAR_ATTENUATION, l->attenuation[1]); glLightf(lid, GL_QUADRATIC_ATTENUATION, l->attenuation[2]); }else if(l->type == SPOT){ glLightf(lid, GL_CONSTANT_ATTENUATION, l->attenuation[0]); glLightf(lid, GL_LINEAR_ATTENUATION, l->attenuation[1]); glLightf(lid, GL_QUADRATIC_ATTENUATION, l->attenuation[2]); glLightf(lid, GL_SPOT_EXPONENT, 20); glLightf(lid, GL_SPOT_CUTOFF, l->cutOffAngle*180/M_PI); GLfloat dir[] = {l->direction[0], l->direction[1], l->direction[2]}; glLightfv(lid, GL_SPOT_DIRECTION, dir); }else if(l->type == DIRECTIONAL){ GLfloat dir[] = {l->direction[0], l->direction[1], l->direction[2]}; glLightfv(lid, GL_SPOT_DIRECTION, dir); } glPopMatrix(); } if (m_showAxes){ glDisable(GL_LIGHTING); glBegin(GL_LINES); glColor3f(1,0,0); glVertex3f(0,0,0); glVertex3f(0.5, 0, 0); glColor3f(0,1,0); glVertex3f(0,0,0); glVertex3f(0, 0.5, 0); glColor3f(0,0,1); glVertex3f(0,0,0); glVertex3f(0, 0, 0.5); glEnd(); glEnable(GL_LIGHTING); } if (!m_useAbsTransformToDraw){ hrp::Link *l = child; while (l){ ntri += ((GLlink *)l)->draw(); l = l->sibling; } } glPopMatrix(); return ntri; }
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); } }