GLscene.cpp
Go to the documentation of this file.
00001 #include <cstdio>
00002 #include <iostream>
00003 #include <fstream>
00004 #ifdef __APPLE__
00005 #include <GLUT/glut.h>
00006 #else
00007 #include <GL/glut.h>
00008 #endif
00009 #include <sys/time.h>
00010 #include "hrpsys/util/GLcamera.h"
00011 #include "hrpsys/util/GLlink.h"
00012 #include "hrpsys/util/GLbody.h"
00013 #include "hrpsys/util/LogManager.h"
00014 #include "TimedRobotState.h"
00015 #include "GLscene.h"
00016 
00017 using namespace OpenHRP;
00018 using namespace hrp;
00019 
00020 void drawString2(const char *str)
00021 {
00022     for (unsigned int i=0; i<strlen(str); i++){
00023         glutBitmapCharacter(GLUT_BITMAP_8_BY_13, str[i]);
00024     }
00025 }
00026 
00027 void white(){
00028     glColor3d(1.0,1.0,1.0);
00029 }
00030 
00031 void red(){
00032     glColor3d(1.0,0.0,0.0);
00033 }
00034 
00035 void yellow(){
00036     glColor3d(1.0,1.0,0.0);
00037 }
00038 
00039 void green(){
00040     glColor3d(0.0,1.0,0.0);
00041 }
00042 
00043 void blue(){
00044     glColor3d(0.0,0.0,1.0);
00045 }
00046 
00047 void black(){
00048     glColor3d(0.0,0.0,0.0);
00049 }
00050 
00051 bool isCalibrated(int s) { 
00052     return s & OpenHRP::RobotHardwareService::CALIB_STATE_MASK; 
00053 }
00054 bool isPowerOn(int s) { 
00055     return s & OpenHRP::RobotHardwareService::POWER_STATE_MASK; 
00056 }
00057 bool isServoOn(int s) { 
00058     return s & OpenHRP::RobotHardwareService::SERVO_STATE_MASK; 
00059 }
00060 int servoAlarm(int s){
00061     return (s & OpenHRP::RobotHardwareService::SERVO_ALARM_MASK)>>OpenHRP::RobotHardwareService::SERVO_ALARM_SHIFT; 
00062 }
00063 int temperature(int s){
00064     return (s & OpenHRP::RobotHardwareService::DRIVER_TEMP_MASK)>>OpenHRP::RobotHardwareService::DRIVER_TEMP_SHIFT; 
00065 }
00066 
00067 void GLscene::updateScene()
00068 {
00069     if (m_log->index()<0) return;
00070 
00071     LogManager<TimedRobotState> *lm 
00072         = (LogManager<TimedRobotState> *)m_log;
00073     GLbody *glbody = dynamic_cast<GLbody *>(body(0).get());
00074     OpenHRP::StateHolderService::Command &com = lm->state().command;
00075     if (com.baseTransform.length() == 12){
00076         double *tform = com.baseTransform.get_buffer();
00077         glbody->setPosition(tform);
00078         glbody->setRotation(tform+3);
00079         hrp::Link *root = glbody->rootLink();
00080         root->p << tform[0], tform[1], tform[2];
00081         root->R << tform[3], tform[4], tform[5],
00082             tform[6], tform[7], tform[8],
00083             tform[9], tform[10], tform[11];
00084     }
00085     if (com.jointRefs.length() == glbody->numJoints()){
00086         for (unsigned int i=0; i<glbody->numJoints(); i++){
00087             GLlink *j = (GLlink *)glbody->joint(i);
00088             if (j){
00089                 j->q = com.jointRefs[i];
00090                 j->setQ(com.jointRefs[i]);
00091             }
00092         }
00093     }
00094     glbody->calcForwardKinematics();
00095     glbody->updateLinkColdetModelPositions();
00096     for (unsigned int i=0; i<glbody->numLinks(); i++){
00097         ((GLlink *)glbody->link(i))->highlight(false);
00098     }
00099     for (size_t i=0; i<m_pairs.size(); i++){
00100         if (m_pairs[i]->checkCollision()){
00101             ((GLlink *)m_pairs[i]->link(0))->highlight(true);
00102             ((GLlink *)m_pairs[i]->link(1))->highlight(true);
00103             std::cout << m_pairs[i]->link(0)->name << "<->" << m_pairs[i]->link(1)->name << std::endl;
00104         }
00105     }
00106 }
00107 
00108 void GLscene::showStatus()
00109 {
00110     if (m_log->index()<0) return;
00111 
00112     LogManager<TimedRobotState> *lm 
00113         = (LogManager<TimedRobotState> *)m_log;
00114     OpenHRP::RobotHardwareService::RobotState &rstate = lm->state().state;
00115 
00116     if (m_showingStatus){
00117         GLbody *glbody = dynamic_cast<GLbody *>(body(0).get());
00118 #define HEIGHT_STEP 12
00119         int width = m_width - 410;
00120         int height = m_height-HEIGHT_STEP;
00121         char buf[256];
00122         for (unsigned int i=0; i<glbody->numJoints(); i++){
00123             hrp::Link *l = glbody->joint(i);
00124             if (l){
00125                 int ss = rstate.servoState[i][0];
00126                 int x = width;
00127                 // joint ID
00128                 sprintf(buf, "%2d",i);
00129                 if (!isCalibrated(ss)){
00130                     yellow();
00131                 }else if(isServoOn(ss)){
00132                     red();
00133                 }
00134                 glRasterPos2f(x, height);
00135                 drawString2(buf);
00136                 white();
00137                 x += 8*3;
00138                 // power status
00139                 if (isPowerOn(ss)) blue();
00140                 glRasterPos2f(x, height);
00141                 drawString2("o");
00142                 if (isPowerOn(ss)) white();
00143                 x += 8*2;
00144                 // joint name, current angle, command angle and torque
00145                 sprintf(buf, "%13s %8.3f %8.3f %6.1f", 
00146                         l->name.c_str(), 
00147                         (i<rstate.angle.length())?(rstate.angle[i]*180/M_PI):0,
00148                         (i<rstate.command.length())?(rstate.command[i]*180/M_PI):0,
00149                         (i<rstate.torque.length())?(rstate.torque[i]*180/M_PI):0);
00150                 glRasterPos2f(x, height);
00151                 drawString2(buf);
00152                 x += 8*(14+9+9+7);
00153                 // servo alarms
00154 
00155                 sprintf(buf, "%03x", servoAlarm(ss));
00156                 glRasterPos2f(x, height);
00157                 drawString2(buf);
00158                 x += 8*4;
00159                 // driver temperature
00160                 int temp = temperature(ss);
00161                 if (!temp){
00162                     sprintf(buf, "--");
00163                 }else{
00164                     sprintf(buf, "%2d", temp);
00165                 }
00166                 if (temp >= 60) red();
00167                 glRasterPos2f(x, height);
00168                 drawString2(buf);
00169                 if (temp >= 60) white();
00170                 x += 8*3;
00171                 
00172                 height -= HEIGHT_STEP;
00173             }
00174         }
00175         if (rstate.accel.length()){
00176             glRasterPos2f(width, height);
00177             height -= HEIGHT_STEP;
00178             drawString2("acc:");
00179             for (unsigned int i=0; i<rstate.accel.length(); i++){
00180                 sprintf(buf, "  %8.4f %8.4f %8.4f",
00181                         rstate.accel[i][0], rstate.accel[i][1], rstate.accel[i][2]);
00182                 glRasterPos2f(width, height);
00183                 height -= HEIGHT_STEP;
00184                 drawString2(buf);
00185             }
00186         }
00187         if (rstate.rateGyro.length()){
00188             glRasterPos2f(width, height);
00189             height -= HEIGHT_STEP;
00190             drawString2("rate:");
00191             for (unsigned int i=0; i<rstate.rateGyro.length(); i++){
00192                 sprintf(buf, "  %8.4f %8.4f %8.4f",
00193                         rstate.rateGyro[i][0], rstate.rateGyro[i][1], rstate.rateGyro[i][2]);
00194                 glRasterPos2f(width, height);
00195                 height -= HEIGHT_STEP;
00196                 drawString2(buf);
00197             }
00198         }
00199         if (rstate.force.length()){
00200             glRasterPos2f(width, height);
00201             height -= HEIGHT_STEP;
00202             drawString2("force/torque:");
00203             for (unsigned int i=0; i<rstate.force.length(); i++){
00204                 sprintf(buf, "  %6.1f %6.1f %6.1f %6.2f %6.2f %6.2f",
00205                         rstate.force[i][0], 
00206                         rstate.force[i][1], 
00207                         rstate.force[i][2],
00208                         rstate.force[i][3], 
00209                         rstate.force[i][4], 
00210                         rstate.force[i][5]);
00211                 glRasterPos2f(width, height);
00212                 height -= HEIGHT_STEP;
00213                 drawString2(buf);
00214             }
00215         }
00216         glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
00217         glEnable(GL_BLEND);
00218         glColor4f(0.0,0.0,0.0, 0.5);
00219         if (m_showSlider){
00220             glRectf(width,SLIDER_AREA_HEIGHT,m_width,m_height);
00221         }else{
00222             glRectf(width,0,m_width,m_height);
00223         }
00224         glDisable(GL_BLEND);
00225     }else{
00226         // !m_showingRobotState
00227         bool servo=false, power=false;
00228         for (unsigned int i=0; i<rstate.servoState.length(); i++){
00229             if (isServoOn(rstate.servoState[i][0])) servo = true;
00230             if (isPowerOn(rstate.servoState[i][0])) power = true;
00231         }
00232         struct timeval tv;
00233         gettimeofday(&tv, NULL);
00234         double dt = tv.tv_sec + tv.tv_usec/1e6 - lm->time(m_log->length()-1);
00235         if (dt < 1.0) green(); else black();
00236         glRectf(m_width-115,m_height-45,m_width-85,m_height-15);
00237         if (power) blue(); else black();
00238         glRectf(m_width- 80,m_height-45,m_width-50,m_height-15);
00239         if (servo) red(); else black();
00240         glRectf(m_width- 45,m_height-45,m_width-15,m_height-15);
00241     }
00242 }
00243 
00244 void printMatrix(double mat[16])
00245 {
00246     for (int i=0; i<4; i++){
00247         for (int j=0; j<4; j++){
00248             printf("%6.3f ", mat[j*4+i]);
00249         }
00250         printf("\n");
00251     }
00252 }
00253 
00254 static void drawCross(const Vector3& p)
00255 {
00256 #define LINE_HLEN 0.5
00257     float v[3];
00258     v[0] = p[0]+LINE_HLEN; v[1] = p[1]; v[2] = p[2]+0.001;
00259     glVertex3fv(v);
00260     v[0] = p[0]-LINE_HLEN;
00261     glVertex3fv(v);
00262     v[0] = p[0]; v[1] = p[1]+LINE_HLEN; v[2] = p[2]+0.001;
00263     glVertex3fv(v);
00264     v[1] = p[1]-LINE_HLEN;
00265     glVertex3fv(v);
00266     v[0] = p[0]; v[1] = p[1]; v[2] = p[2]+0.001;
00267     glVertex3fv(v);
00268     v[2] = p[2]+LINE_HLEN;
00269     glVertex3fv(v);
00270 }
00271 
00272 void GLscene::drawAdditionalLines()
00273 {
00274     if (m_log->index()<0) return;
00275 
00276     LogManager<TimedRobotState> *lm 
00277         = (LogManager<TimedRobotState> *)m_log;
00278     OpenHRP::StateHolderService::Command &com = lm->state().command;
00279 
00280     if (com.zmp.length() != 3) return;
00281 
00282     Vector3 relZmp(com.zmp[0], com.zmp[1], com.zmp[2]);
00283     hrp::Link *root = body(0)->rootLink();
00284     Vector3 absZmp = root->R*relZmp + root->p;
00285     glBegin(GL_LINES);
00286     glColor3f(1,0,1);
00287     drawCross(absZmp);
00288     if (m_showCoMonFloor){
00289         Vector3 projectedCom = body(0)->calcCM();
00290         projectedCom[2] = 0;
00291         glColor3f(0,1,1);
00292         drawCross(projectedCom);
00293     }
00294     glEnd();
00295 }
00296 
00297 
00298 void GLscene::setCollisionCheckPairs(const std::vector<hrp::ColdetLinkPairPtr> &i_pairs)
00299 {
00300     m_pairs = i_pairs;
00301 }
00302 
00303 void GLscene::showCoMonFloor(bool flag)
00304 {
00305     m_showCoMonFloor = flag;
00306 }


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:55