util/monitor/GLscene.cpp
Go to the documentation of this file.
1 #include <cstdio>
2 #include <iostream>
3 #include <fstream>
4 #ifdef __APPLE__
5 #include <GLUT/glut.h>
6 #else
7 #include <GL/glut.h>
8 #endif
9 #include <sys/time.h>
10 #include "hrpsys/util/GLcamera.h"
11 #include "hrpsys/util/GLlink.h"
12 #include "hrpsys/util/GLbody.h"
13 #include "hrpsys/util/LogManager.h"
14 #include "TimedRobotState.h"
15 #include "GLscene.h"
16 
17 using namespace OpenHRP;
18 using namespace hrp;
19 
20 void drawString2(const char *str)
21 {
22  for (unsigned int i=0; i<strlen(str); i++){
23  glutBitmapCharacter(GLUT_BITMAP_8_BY_13, str[i]);
24  }
25 }
26 
27 void white(){
28  glColor3d(1.0,1.0,1.0);
29 }
30 
31 void red(){
32  glColor3d(1.0,0.0,0.0);
33 }
34 
35 void yellow(){
36  glColor3d(1.0,1.0,0.0);
37 }
38 
39 void green(){
40  glColor3d(0.0,1.0,0.0);
41 }
42 
43 void blue(){
44  glColor3d(0.0,0.0,1.0);
45 }
46 
47 void black(){
48  glColor3d(0.0,0.0,0.0);
49 }
50 
51 bool isCalibrated(int s) {
52  return s & OpenHRP::RobotHardwareService::CALIB_STATE_MASK;
53 }
54 bool isPowerOn(int s) {
55  return s & OpenHRP::RobotHardwareService::POWER_STATE_MASK;
56 }
57 bool isServoOn(int s) {
58  return s & OpenHRP::RobotHardwareService::SERVO_STATE_MASK;
59 }
60 int servoAlarm(int s){
61  return (s & OpenHRP::RobotHardwareService::SERVO_ALARM_MASK)>>OpenHRP::RobotHardwareService::SERVO_ALARM_SHIFT;
62 }
63 int temperature(int s){
64  return (s & OpenHRP::RobotHardwareService::DRIVER_TEMP_MASK)>>OpenHRP::RobotHardwareService::DRIVER_TEMP_SHIFT;
65 }
66 
68 {
69  if (m_log->index()<0) return;
70 
72  = (LogManager<TimedRobotState> *)m_log;
73  GLbody *glbody = dynamic_cast<GLbody *>(body(0).get());
74  OpenHRP::StateHolderService::Command &com = lm->state().command;
75  if (com.baseTransform.length() == 12){
76  double *tform = com.baseTransform.get_buffer();
77  glbody->setPosition(tform);
78  glbody->setRotation(tform+3);
79  hrp::Link *root = glbody->rootLink();
80  root->p << tform[0], tform[1], tform[2];
81  root->R << tform[3], tform[4], tform[5],
82  tform[6], tform[7], tform[8],
83  tform[9], tform[10], tform[11];
84  }
85  if (com.jointRefs.length() == glbody->numJoints()){
86  for (unsigned int i=0; i<glbody->numJoints(); i++){
87  GLlink *j = (GLlink *)glbody->joint(i);
88  if (j){
89  j->q = com.jointRefs[i];
90  j->setQ(com.jointRefs[i]);
91  }
92  }
93  }
94  glbody->calcForwardKinematics();
96  for (unsigned int i=0; i<glbody->numLinks(); i++){
97  ((GLlink *)glbody->link(i))->highlight(false);
98  }
99  for (size_t i=0; i<m_pairs.size(); i++){
100  if (m_pairs[i]->checkCollision()){
101  ((GLlink *)m_pairs[i]->link(0))->highlight(true);
102  ((GLlink *)m_pairs[i]->link(1))->highlight(true);
103  std::cout << m_pairs[i]->link(0)->name << "<->" << m_pairs[i]->link(1)->name << std::endl;
104  }
105  }
106 }
107 
108 void GLscene::showStatus()
109 {
110  if (m_log->index()<0) return;
111 
113  = (LogManager<TimedRobotState> *)m_log;
114  OpenHRP::RobotHardwareService::RobotState &rstate = lm->state().state;
115 
116  if (m_showingStatus){
117  GLbody *glbody = dynamic_cast<GLbody *>(body(0).get());
118 #define HEIGHT_STEP 12
119  int width = m_width - 410;
120  int height = m_height-HEIGHT_STEP;
121  char buf[256];
122  for (unsigned int i=0; i<glbody->numJoints(); i++){
123  hrp::Link *l = glbody->joint(i);
124  if (l){
125  int ss = rstate.servoState[i][0];
126  int x = width;
127  // joint ID
128  sprintf(buf, "%2d",i);
129  if (!isCalibrated(ss)){
130  yellow();
131  }else if(isServoOn(ss)){
132  red();
133  }
134  glRasterPos2f(x, height);
135  drawString2(buf);
136  white();
137  x += 8*3;
138  // power status
139  if (isPowerOn(ss)) blue();
140  glRasterPos2f(x, height);
141  drawString2("o");
142  if (isPowerOn(ss)) white();
143  x += 8*2;
144  // joint name, current angle, command angle and torque
145  sprintf(buf, "%13s %8.3f %8.3f %6.1f",
146  l->name.c_str(),
147  (i<rstate.angle.length())?(rstate.angle[i]*180/M_PI):0,
148  (i<rstate.command.length())?(rstate.command[i]*180/M_PI):0,
149  (i<rstate.torque.length())?(rstate.torque[i]*180/M_PI):0);
150  glRasterPos2f(x, height);
151  drawString2(buf);
152  x += 8*(14+9+9+7);
153  // servo alarms
154 
155  sprintf(buf, "%03x", servoAlarm(ss));
156  glRasterPos2f(x, height);
157  drawString2(buf);
158  x += 8*4;
159  // driver temperature
160  int temp = temperature(ss);
161  if (!temp){
162  sprintf(buf, "--");
163  }else{
164  sprintf(buf, "%2d", temp);
165  }
166  if (temp >= 60) red();
167  glRasterPos2f(x, height);
168  drawString2(buf);
169  if (temp >= 60) white();
170  x += 8*3;
171 
172  height -= HEIGHT_STEP;
173  }
174  }
175  if (rstate.accel.length()){
176  glRasterPos2f(width, height);
177  height -= HEIGHT_STEP;
178  drawString2("acc:");
179  for (unsigned int i=0; i<rstate.accel.length(); i++){
180  sprintf(buf, " %8.4f %8.4f %8.4f",
181  rstate.accel[i][0], rstate.accel[i][1], rstate.accel[i][2]);
182  glRasterPos2f(width, height);
183  height -= HEIGHT_STEP;
184  drawString2(buf);
185  }
186  }
187  if (rstate.rateGyro.length()){
188  glRasterPos2f(width, height);
189  height -= HEIGHT_STEP;
190  drawString2("rate:");
191  for (unsigned int i=0; i<rstate.rateGyro.length(); i++){
192  sprintf(buf, " %8.4f %8.4f %8.4f",
193  rstate.rateGyro[i][0], rstate.rateGyro[i][1], rstate.rateGyro[i][2]);
194  glRasterPos2f(width, height);
195  height -= HEIGHT_STEP;
196  drawString2(buf);
197  }
198  }
199  if (rstate.force.length()){
200  glRasterPos2f(width, height);
201  height -= HEIGHT_STEP;
202  drawString2("force/torque:");
203  for (unsigned int i=0; i<rstate.force.length(); i++){
204  sprintf(buf, " %6.1f %6.1f %6.1f %6.2f %6.2f %6.2f",
205  rstate.force[i][0],
206  rstate.force[i][1],
207  rstate.force[i][2],
208  rstate.force[i][3],
209  rstate.force[i][4],
210  rstate.force[i][5]);
211  glRasterPos2f(width, height);
212  height -= HEIGHT_STEP;
213  drawString2(buf);
214  }
215  }
216  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
217  glEnable(GL_BLEND);
218  glColor4f(0.0,0.0,0.0, 0.5);
219  if (m_showSlider){
220  glRectf(width,SLIDER_AREA_HEIGHT,m_width,m_height);
221  }else{
222  glRectf(width,0,m_width,m_height);
223  }
224  glDisable(GL_BLEND);
225  }else{
226  // !m_showingRobotState
227  bool servo=false, power=false;
228  for (unsigned int i=0; i<rstate.servoState.length(); i++){
229  if (isServoOn(rstate.servoState[i][0])) servo = true;
230  if (isPowerOn(rstate.servoState[i][0])) power = true;
231  }
232  struct timeval tv;
233  gettimeofday(&tv, NULL);
234  double dt = tv.tv_sec + tv.tv_usec/1e6 - lm->time(m_log->length()-1);
235  if (dt < 1.0) green(); else black();
236  glRectf(m_width-115,m_height-45,m_width-85,m_height-15);
237  if (power) blue(); else black();
238  glRectf(m_width- 80,m_height-45,m_width-50,m_height-15);
239  if (servo) red(); else black();
240  glRectf(m_width- 45,m_height-45,m_width-15,m_height-15);
241  }
242 }
243 
244 void printMatrix(double mat[16])
245 {
246  for (int i=0; i<4; i++){
247  for (int j=0; j<4; j++){
248  printf("%6.3f ", mat[j*4+i]);
249  }
250  printf("\n");
251  }
252 }
253 
254 static void drawCross(const Vector3& p)
255 {
256 #define LINE_HLEN 0.5
257  float v[3];
258  v[0] = p[0]+LINE_HLEN; v[1] = p[1]; v[2] = p[2]+0.001;
259  glVertex3fv(v);
260  v[0] = p[0]-LINE_HLEN;
261  glVertex3fv(v);
262  v[0] = p[0]; v[1] = p[1]+LINE_HLEN; v[2] = p[2]+0.001;
263  glVertex3fv(v);
264  v[1] = p[1]-LINE_HLEN;
265  glVertex3fv(v);
266  v[0] = p[0]; v[1] = p[1]; v[2] = p[2]+0.001;
267  glVertex3fv(v);
268  v[2] = p[2]+LINE_HLEN;
269  glVertex3fv(v);
270 }
271 
273 {
274  if (m_log->index()<0) return;
275 
277  = (LogManager<TimedRobotState> *)m_log;
278  OpenHRP::StateHolderService::Command &com = lm->state().command;
279 
280  if (com.zmp.length() != 3) return;
281 
282  Vector3 relZmp(com.zmp[0], com.zmp[1], com.zmp[2]);
283  hrp::Link *root = body(0)->rootLink();
284  Vector3 absZmp = root->R*relZmp + root->p;
285  glBegin(GL_LINES);
286  glColor3f(1,0,1);
287  drawCross(absZmp);
288  if (m_showCoMonFloor){
289  Vector3 projectedCom = body(0)->calcCM();
290  projectedCom[2] = 0;
291  glColor3f(0,1,1);
292  drawCross(projectedCom);
293  }
294  glEnd();
295 }
296 
297 
298 void GLscene::setCollisionCheckPairs(const std::vector<hrp::ColdetLinkPairPtr> &i_pairs)
299 {
300  m_pairs = i_pairs;
301 }
302 
303 void GLscene::showCoMonFloor(bool flag)
304 {
305  m_showCoMonFloor = flag;
306 }
void yellow()
Link * rootLink() const
#define LINE_HLEN
double time(int i)
Definition: LogManager.h:64
void black()
unsigned int numJoints() const
void setPosition(double x, double y, double z)
Definition: GLbody.cpp:27
unsigned int numLinks() const
void setCollisionCheckPairs(const std::vector< hrp::ColdetLinkPairPtr > &i_pairs)
static void drawCross(const Vector3 &p)
void drawAdditionalLines()
void updateScene()
#define HEIGHT_STEP
Link * link(int index) const
void showCoMonFloor(bool flag)
png_uint_32 i
int temperature(int s)
png_infop png_uint_32 * width
Definition: GLbody.h:11
OpenHRP::RobotHardwareService::RobotState state
void setRotation(const double *R)
Definition: GLbody.cpp:37
void printMatrix(double mat[16])
OpenHRP::vector3 Vector3
int gettimeofday(struct timeval *tv, struct timezone *tz)
void blue()
void updateLinkColdetModelPositions()
png_infop png_uint_32 png_uint_32 * height
bool isPowerOn(int s)
void drawString2(const char *str)
bool isServoOn(int s)
png_bytep buf
static std::vector< int > servo
Definition: iob.cpp:17
T & state()
Definition: LogManager.h:140
#define M_PI
OpenHRP::StateHolderService::Command command
std::string sprintf(char const *__restrict fmt,...)
static std::vector< int > power
Definition: iob.cpp:16
void white()
int servoAlarm(int s)
#define SLIDER_AREA_HEIGHT
Definition: GLsceneBase.h:27
void green()
Link * joint(int id) const
void red()
void calcForwardKinematics(bool calcVelocity=false, bool calcAcceleration=false)
bool isCalibrated(int s)


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:20