util/simulator/GLscene.cpp
Go to the documentation of this file.
1 #include <cstdio>
2 #include <iostream>
3 #include <fstream>
4 #include <sys/time.h>
5 #ifdef __APPLE__
6 #include <GLUT/glut.h>
7 #else
8 #include <GL/glut.h>
9 #endif
10 #include <boost/bind.hpp>
11 #include <hrpModel/Sensor.h>
12 #include "hrpsys/util/GLcamera.h"
13 #include "hrpsys/util/GLlink.h"
14 #include "hrpsys/util/GLbody.h"
15 #include "hrpsys/util/LogManager.h"
16 #include "SceneState.h"
17 #include "GLscene.h"
18 
19 using namespace OpenHRP;
20 using namespace hrp;
21 
23  GLsceneBase(i_log),
24  m_showSensors(false),
25  m_showCollision(true)
26 {
27 }
28 
30 {
31  if (m_log->index()<0) return;
32 
35  SceneState &state = lm->state();
36 
37  for (unsigned int i=0; i<state.bodyStates.size(); i++){
38  const BodyState& bstate = state.bodyStates[i];
39  GLbody *glbody = dynamic_cast<GLbody *>(body(i).get());
40  glbody->setPosture(bstate.q, bstate.p, bstate.R);
41  if (m_showSensors){
42  glbody->setSensorDrawCallback(
43  boost::bind(&GLscene::drawSensorOutput, this, _1, _2));
44  }else{
45  glbody->setSensorDrawCallback(NULL);
46  }
47  }
48 }
49 
50 static void drawString2(const char *str)
51 {
52  for (unsigned int i=0; i<strlen(str); i++){
53  glutBitmapCharacter(GLUT_BITMAP_8_BY_13, str[i]);
54  }
55 }
56 
58 {
59  if (!m_showCollision || m_log->index()<0) return;
60 
63  SceneState &state = lm->state();
64 
65  glBegin(GL_LINES);
66  glColor3f(1,0,0);
67  double e[3];
68  const std::vector<CollisionInfo> &cs = state.collisions;
69  for (unsigned int i=0; i<cs.size(); i++){
70  for (int k=0; k<3; k++){
71  e[k] = cs[i].position[k] + cs[i].normal[k]*(cs[i].idepth*10+0.1);
72  }
73  glVertex3dv(cs[i].position);
74  glVertex3dv(e);
75  }
76  glEnd();
77 }
78 
80 {
81  if (m_log->index()<0) return;
82 
85  SceneState &state = lm->state();
86 
87  if (m_showingStatus){
88  GLbody *glbody = NULL;
89  BodyState *bstate = NULL;
90  for (unsigned int i=0; i<numBodies(); i++){
91  if (body(i)->numJoints()){
92  glbody = dynamic_cast<GLbody *>(body(i).get());
93  bstate = &state.bodyStates[i];
94  break;
95  }
96  }
97  if (!glbody) return;
98 #define HEIGHT_STEP 12
99  int width = m_width - 350;
101  char buf[256];
102  double q[glbody->numJoints()];
103  for (unsigned int i=0; i<glbody->numLinks(); i++){
104  Link* l = glbody->link(i);
105  if (l->jointId >= 0) q[l->jointId] = bstate->q[i];
106  }
107  for (unsigned int i=0; i<glbody->numJoints(); i++){
108  GLlink *l = (GLlink *)glbody->joint(i);
109  if (l){
110  sprintf(buf, "%2d %15s %8.3f", i, l->name.c_str(),
111  q[i]*180/M_PI);
112  glRasterPos2f(width, height);
113  height -= HEIGHT_STEP;
114  drawString2(buf);
115  }
116  }
117  if (bstate->acc.size()){
118  glRasterPos2f(width, height);
119  height -= HEIGHT_STEP;
120  drawString2("acc:");
121  for (unsigned int i=0; i<bstate->acc.size(); i++){
122  sprintf(buf, " %8.4f %8.4f %8.4f",
123  bstate->acc[i][0], bstate->acc[i][1], bstate->acc[i][2]);
124  glRasterPos2f(width, height);
125  height -= HEIGHT_STEP;
126  drawString2(buf);
127  }
128  }
129  if (bstate->rate.size()){
130  glRasterPos2f(width, height);
131  height -= HEIGHT_STEP;
132  drawString2("rate:");
133  for (unsigned int i=0; i<bstate->rate.size(); i++){
134  sprintf(buf, " %8.4f %8.4f %8.4f",
135  bstate->rate[i][0], bstate->rate[i][1], bstate->rate[i][2]);
136  glRasterPos2f(width, height);
137  height -= HEIGHT_STEP;
138  drawString2(buf);
139  }
140  }
141  if (bstate->force.size()){
142  glRasterPos2f(width, height);
143  height -= HEIGHT_STEP;
144  drawString2("force/torque:");
145  for (unsigned int i=0; i<bstate->force.size(); i++){
146  sprintf(buf, " %6.1f %6.1f %6.1f %6.2f %6.2f %6.2f",
147  bstate->force[i][0],
148  bstate->force[i][1],
149  bstate->force[i][2],
150  bstate->force[i][3],
151  bstate->force[i][4],
152  bstate->force[i][5]);
153  glRasterPos2f(width, height);
154  height -= HEIGHT_STEP;
155  drawString2(buf);
156  }
157  }
158  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
159  glEnable(GL_BLEND);
160  glColor4f(0.0,0.0,0.0, 0.5);
161  if (m_showSlider){
162  glRectf(width,SLIDER_AREA_HEIGHT,m_width,m_height);
163  }else{
164  glRectf(width,0,m_width,m_height);
165  }
166  glDisable(GL_BLEND);
167  }
168 }
169 
170 
172 {
173  if (m_log->index()<0) return;
174 
177  SceneState &sstate = lm->state();
178  if (bodyIndex(body->name())<0){
179  std::cerr << "invalid bodyIndex(" << bodyIndex(body->name())
180  << ") for " << body->name() << std::endl;
181  return;
182  }
183  const BodyState &state = sstate.bodyStates[bodyIndex(body->name())];
184 
185  if (sensor->type == Sensor::RANGE){
186  RangeSensor *range = dynamic_cast<RangeSensor *>(sensor);
187  const std::vector<double> distances = state.range[sensor->id];
188  if (distances.empty()) return;
189  int scan_half = (int)(range->scanAngle/2/range->scanStep);
190  double th;
191  Vector3 v;
192  v[1] = 0.0;
193  glDisable(GL_LIGHTING);
194  glBegin(GL_LINES);
195  glColor3f(1,0,0);
196  for (int i = -scan_half,j=0; i<= scan_half; i++,j++){
197  th = i*range->scanStep;
198  double d = distances[j] ? distances[j] : range->maxDistance;
199  v[0] = -d*sin(th);
200  v[2] = -d*cos(th);
201  glVertex3f(0,0,0); glVertex3f(v[0], v[1], v[2]);
202  }
203  glEnd();
204  glEnable(GL_LIGHTING);
205  }else if(sensor->type == Sensor::VISION){
206  VisionSensor *v = dynamic_cast<VisionSensor *>(sensor);
207  double far = v->far, near = v->near;
208  glDisable(GL_LIGHTING);
209  glColor3f(1,1,1);
210  glBegin(GL_LINES);
211  double t = tan(v->fovy/2);
212  double xf = t*far*v->width/v->height, yf = t*far;
213  glVertex3f( xf, yf, -far); glVertex3f(-xf, yf, -far);
214  glVertex3f(-xf, yf, -far); glVertex3f(-xf, -yf, -far);
215  glVertex3f(-xf, -yf, -far); glVertex3f( xf, -yf, -far);
216  glVertex3f( xf, -yf, -far); glVertex3f( xf, yf, -far);
217  double xn = t*near*v->width/v->height, yn = t*near;
218  glVertex3f( xn, yn, -near); glVertex3f(-xn, yn, -near);
219  glVertex3f(-xn, yn, -near); glVertex3f(-xn, -yn, -near);
220  glVertex3f(-xn, -yn, -near); glVertex3f( xn, -yn, -near);
221  glVertex3f( xn, -yn, -near); glVertex3f( xn, yn, -near);
222  glVertex3f( xn, yn, -near); glVertex3f( xf, yf, -far);
223  glVertex3f(-xn, yn, -near); glVertex3f(-xf, yf, -far);
224  glVertex3f(-xn, -yn, -near); glVertex3f(-xf, -yf, -far);
225  glVertex3f( xn, -yn, -near); glVertex3f( xf, -yf, -far);
226  glEnd();
227  if (v->imageType == VisionSensor::DEPTH
228  || v->imageType == VisionSensor::COLOR_DEPTH
229  || v->imageType == VisionSensor::MONO_DEPTH){
230  bool colored = v->imageType == VisionSensor::COLOR_DEPTH;
231  glBegin(GL_POINTS);
232  float *ptr = (float *)&v->depth[0];
233  for (unsigned int i=0; i<v->depth.size()/16; i++){
234  glVertex3f(ptr[0], ptr[1], ptr[2]);
235  if (colored){
236  ptr += 3;
237  unsigned char *rgb = (unsigned char *)ptr;
238  glColor3f(rgb[0]/255.0, rgb[1]/255.0, rgb[2]/255.0);
239  ptr++;
240  }else{
241  ptr += 4;
242  }
243  }
244  glEnd();
245  }
246 
247  glEnable(GL_LIGHTING);
248  }
249 }
250 
251 void GLscene::showSensors(bool flag)
252 {
253  m_showSensors = flag;
254 }
255 
257 {
258  return m_showSensors;
259 }
260 
261 void GLscene::showCollision(bool flag)
262 {
263  m_showCollision = flag;
264 }
265 
267 {
268  return m_showCollision;
269 }
d
GLscene()
Definition: GLmodel.cpp:347
int bodyIndex(const std::string &name)
hrp::dvector q
Definition: BodyState.h:10
std::vector< hrp::Vector3 > acc
Definition: BodyState.h:13
state
png_voidp ptr
void drawAdditionalLines()
void updateScene()
static void drawString2(const char *str)
#define for
std::vector< CollisionInfo > collisions
Definition: SceneState.h:22
png_uint_32 i
png_infop png_uint_32 * width
#define RANGE(d, th)
virtual int index()=0
Definition: GLbody.h:11
#define HEIGHT_STEP
GLbody * body(unsigned int i_rank)
Definition: GLmodel.cpp:442
OpenHRP::vector3 Vector3
png_infop png_uint_32 png_uint_32 * height
def j(str, encoding="cp932")
const std::string & name()
std::vector< std::vector< double > > range
Definition: BodyState.h:16
unsigned int numBodies() const
Definition: GLmodel.cpp:437
LogManagerBase * m_log
Definition: GLsceneBase.h:76
t
bool m_showSlider
Definition: GLsceneBase.h:70
png_bytep buf
bool m_showingStatus
Definition: GLsceneBase.h:70
Link * joint(int id) const
Link * link(int index) const
std::vector< hrp::Vector3 > rate
Definition: BodyState.h:14
T & state()
Definition: LogManager.h:140
position
#define M_PI
void drawSensorOutput(hrp::Body *i_body, hrp::Sensor *i_sensor)
std::vector< unsigned char > depth
typedef int
void setPosture(const double *i_angles)
Definition: GLbody.cpp:21
std::string sprintf(char const *__restrict fmt,...)
unsigned int numJoints() const
std::vector< BodyState > bodyStates
Definition: SceneState.h:21
hrp::Matrix33 R
Definition: BodyState.h:12
hrp::Vector3 p
Definition: BodyState.h:11
std::vector< hrp::dvector6, Eigen::aligned_allocator< hrp::dvector6 > > force
Definition: BodyState.h:15
unsigned int numLinks() const
#define SLIDER_AREA_HEIGHT
Definition: GLsceneBase.h:22
ImageType imageType
void setSensorDrawCallback(boost::function2< void, hrp::Body *, hrp::Sensor * > f)
Definition: GLbody.cpp:84


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Thu May 6 2021 02:41:50