iCub-main
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
iCub_Sim.cpp
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 /*
4 * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
5 * Author: Paul Fitzpatrick, Vadim Tikhanoff, Martin Peniak
6 * email: paulfitz@alum.mit.edu, vadim.tikhanoff@iit.it, martin.peniak@plymouth.ac.uk
7 * website: www.robotcub.org
8 * Permission is granted to copy, distribute, and/or modify this program
9 * under the terms of the GNU General Public License, version 2 or any
10 * later version published by the Free Software Foundation.
11 *
12 * A copy of the license can be found at
13 * http://www.robotcub.org/icub/license/gpl.txt
14 *
15 * This program is distributed in the hope that it will be useful, but
16 * WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
18 * Public License for more details
19 */
20 
21 #include "iCub_Sim.h"
22 
23 #include "OdeInit.h"
24 
25 using namespace yarp::sig;
26 
27 // globals
28 Semaphore ODE_access(1);
29 #define CTRL_RAD2DEG (180.0/M_PI)
30 #define CTRL_DEG2RAD (M_PI/180.0)
31 
32 // locals
33 // NOTE that we use (long) instead of (clock_t), because on MacOS, (clock_t) is unsigned, while we need negative numbers
34 static long gl_frame_length = 1000/30; // update opengl and vision stream at 30 Hz
35 static long ode_step_length = 10; // target duration of the ODE step in CPU time (set to 0 to go as fast as possible, set to dstep*1000 to go realtime)
36 static double dstep = 10.0/1000.0; // step size in ODE's dWorldStep in seconds
37 
38 static bool glrun; // draw gl
39 static bool simrun; // run simulator thread
40 
41 static int stop = 0;
42 static int v = 0;
43 
44 static float xyz[3];
45 static float hpr[8];
46 static float rez[3];
47 
48 static int contactPoint;
49 static int mouseDiffx, mouseDiffy;
50 static bool picking = false;
51 static float cam_rx = 0.0, cam_ry = 0.0;
52 
53 static int width = 640;
54 static int height = 480;
55 
58 static int mouse_ray_x;
59 static int mouse_ray_y;
60 static float *VAD;
61 static float *VAD2;
62 const dReal *pos;
63 static float angle_xref = 0.0f;
64 static float angle_yref = 25.0f;
65 static float ydistance = 10.0f;
66 static float xdistance = 0.0f;
67 static float view_xyz[3]; // position x,y,z
68 static float view_hpr[3]; // heading, pitch, roll (degrees)
69 static float view2_xyz[3];
70 static float view2_hpr[3];
71 static float zoom = 0;
72 static float xpos = 0, ypos = 0, zpos = 0, xrot = 0, yrot = 0, zrot = 0, angle=0.0;
73 static float lastx, lasty;
74 static float xrotrad = 0, yrotrad = 0;
75 static long startTime, finishTime;
77 static float test[3];
78 //static SDL_TimerID id;
79 static Uint32 colorkey;
80 static SDL_Surface *image;
81 static bool extractImages = false;
82 static VideoTexture *video = NULL;
83 static RobotStreamer *robot_streamer = NULL;
84 static RobotConfig *robot_config = NULL;
85 static bool eyeCams;
86 static const GLfloat light_position[] = { 0.0f, 5.0f, 5.0f, 0.0f };
87 
88 //camera calibration parameters
89 static int width_left;
90 static int width_right;
91 static int height_left;
92 static int height_right;
93 static double fov_left;
94 static double fov_right;
95 
96 
97 static int cameraSizeWidth;
98 static int cameraSizeHeight;
99 
100 
101 // # of touch sensors
102 #define N_TOUCH_SENSORS 12
103 // allocate feedback structs as a static array. We don't allocate feedback structs
104 // at every simulationstep since memory allocation at every step would degrade simulation performance.
106 
107 static int nFeedbackStructs=0;
108 
109 void OdeSdlSimulation::draw() {
110  OdeInit& odeinit = OdeInit::get();
111  odeinit._iCub->draw();
112  odeinit._wrld->draw();
113 }
114 
115 void OdeSdlSimulation::printStats() {
116  OdeInit& odeinit = OdeInit::get();
117 
118  finishTime = (long) clock() ;
119  duration += (double)(finishTime - startTime) / CLOCKS_PER_SEC ;
120  frames ++ ;
121  FPS = frames / duration ;
122  startTime = (long) clock() ;
123  odeinit.SimTime = duration;
124  //printf("duration: %.2lf\n",odeinit.SimTime);
125  static double starting_time_stamp = 0;
126  //test[0] = dBodyGetPosition(odeinit._iCub->body_cube[0])[0];
127  //test[1] = dBodyGetPosition(odeinit._iCub->body_cube[0])[1];
128  //test[2] = dBodyGetPosition(odeinit._iCub->body_cube[0])[2];
129  //printf("test[0] %f test[1] %f test[2] %f\n",test[0],test[1],test[2]);
130  if( duration - starting_time_stamp >= 1){
131  //printf("Frames: %.2lf Duration: %.2lf fps: %3.1f \n",frames,duration,FPS);
132  starting_time_stamp = duration;
133  }
134  //printf("%lf %lf %lf %lf %lf %lf\n", odeinit._iCub->ra_speed[0],odeinit._iCub->ra_speed[1],odeinit._iCub->ra_speed[2],odeinit._iCub->ra_speed[3],odeinit._iCub->ra_speed[4],odeinit._iCub->ra_speed[5]);
135  //drawText(text, textPos);
136 }
137 
138 void OdeSdlSimulation::handle_key_down(SDL_keysym* keysym) {
139  switch (keysym->sym)
140  {
141  case SDLK_e:
142  break;
143  case SDLK_r:
144  initViewpoint();
145  break;
146  case SDLK_t:
147  break;
148  case SDLK_y:
149  break;
150  case SDLK_SPACE:
151  printf("SPACEBAR pressed! Press spacebar again to disable/enable drawing.\n");
152  glrun = !glrun;
153  break;
154  default:
155  break;
156  }
157 }
158 
159 void OdeSdlSimulation::handle_mouse_motion(SDL_MouseMotionEvent* mousemotion) {
160  if (SDL_GetMouseState(NULL, NULL) & SDL_BUTTON(1)){// MOUSE LEFT BUTTON
161  //if (!picking){
162  //camera movement
163  angle_xref += (float)mousemotion->xrel; // 10.0f;
164  angle_yref += (float)mousemotion->yrel; // 10.0f;
165  mouseMovement(angle_xref,angle_yref);
166 
167  if (v<1){
168  //mouse_ray_x = mouse0_down_x;
169  //mouse_ray_y = mouse0_down_y;
170  }
171  /*mouseDiffx = mouse0_down_x - mouse_ray_x;
172  mouseDiffy = mouse0_down_y - mouse_ray_y;
173  mouse_ray_x = mouse0_down_x;
174  mouse_ray_y = mouse0_down_y;*/
175 
176  //VAD = ScreenToWorld(mouse0_down_x,mouse0_down_y,0);
177  //xpos = VAD[0];ypos = VAD[1];zpos = VAD[2];
178  //VAD2 =ScreenToWorld(mouse0_down_x,mouse0_down_y,1);
179  //xpos2 = VAD2[0];ypos2 = VAD2[1];zpos2 = VAD2[2];
180 
181  //if (i<1){ray = dCreateRay(space,100*100);}
182  //Origin[0] = xpos;
183  //Origin[1] = ypos;
184  //Origin[2] = zpos;
185  //Origin[3] = 0;
186  //Direction[0] = xpos2;
187  //Direction[1] = ypos2;
188  //Direction[2] = zpos2;
189  //Direction[3] = 0;
190  //dGeomRaySet(ray, Origin[0], Origin[1], Origin[2], Direction[0], Direction[1], Direction[2]);
191  //dGeomSetPosition(ray, xpos,ypos,zpos);
192  //i++;
193  }
194  if (SDL_GetMouseState(NULL, NULL) & SDL_BUTTON(3)){// MOUSE RIGHT BUTTON
195 
196  //xdistance -= mousemotion->xrel / 10.0f;
197  //ydistance -= mousemotion->yrel / 10.0f;
198  }
199 }
200 
201 void OdeSdlSimulation::process_events(void) {
202  OdeInit& odeinit = OdeInit::get();
203  SDL_Event event;
204 
205  Uint8 * keystate = SDL_GetKeyState(NULL);
206  if(keystate[SDLK_q]){xrot += 1 * 0.4f;if (xrot >360) xrot -= 360 * 0.1f;}
207  if(keystate[SDLK_z]){xrot -= 1 * 0.4f;if (xrot < -360) xrot += 360 * 0.1f;}
208  if(keystate[SDLK_w]){yrotrad = (yrot / 180 * 3.141592654f); xrotrad = (xrot / 180 * 3.141592654f);
209  xpos += float(sin(yrotrad))* 0.005f; ;zpos -= float(cos(yrotrad))* 0.005f; ypos -= float(sin(xrotrad))* 0.005f;}
210  if(keystate[SDLK_s]){yrotrad = (yrot / 180 * 3.141592654f); xrotrad = (xrot / 180 * 3.141592654f);
211  xpos -= float(sin(yrotrad))* 0.005f;zpos += float(cos(yrotrad))* 0.005f; ;ypos += float(sin(xrotrad))* 0.005f;}
212  if (keystate[SDLK_a]){yrotrad = (yrot / 180 * 3.141592654f);xpos -= float(cos(yrotrad)) * 0.008;zpos -= float(sin(yrotrad)) * 0.008; }
213  if (keystate[SDLK_d]){yrotrad = (yrot / 180 * 3.141592654f);xpos += float(cos(yrotrad)) * 0.008;zpos += float(sin(yrotrad)) * 0.008;}
214  if(keystate[SDLK_e]){zrot += 1 * 0.4f;if (zrot >360) zrot -= 360 * 0.4f;}
215  if(keystate[SDLK_c]){zrot -= 1 * 0.4f;if (zrot < -360) zrot += 360 * 0.4f;}
216 
217  if (keystate[SDLK_f]){ypos +=1 *0.005f;}
218  if (keystate[SDLK_v]){ypos -=1 *0.005f;}
219 
220  if(keystate[SDLK_1]){initViewpoint();}
221 
222  if (keystate[SDLK_5]){
223 
224  if ((odeinit._iCub->eyeLidRot) < 0.55) odeinit._iCub->eyeLidRot += 0.01;
225  cout<<odeinit._iCub->eyeLidRot<<endl;
226  }
227  if (keystate[SDLK_6]){
228  if ((odeinit._iCub->eyeLidRot) > 0.01) odeinit._iCub->eyeLidRot -= 0.01;
229  cout<<odeinit._iCub->eyeLidRot<<endl;
230  }
231  if (keystate[SDLK_h])
232  {
233  odeinit.sendHomePos();
234  }
235  /* Grab all the events off the queue. */
236  while (SDL_PollEvent(&event)){
237  switch (event.type)
238  {
239  case SDL_VIDEORESIZE:
240  width = event.resize.w;
241  height = event.resize.h;
242  SDL_SetVideoMode(width,height,16,SDL_OPENGL | SDL_RESIZABLE);
243  {
244  bool ok = setup_opengl(robot_config->getFinder());
245  if (!ok) {
246  odeinit.stop = true;
247  }
248  }
249  odeinit._iCub->reinitialized = true;
250  //draw_screen( );
251  break;
252  case SDL_KEYDOWN:
253  /* Handle key presses*/
254  handle_key_down(&event.key.keysym);
255  // SDL_GetKeyName(event.key.keysym.sym));
256  break;
257  break;
258  case SDL_MOUSEMOTION:
259  handle_mouse_motion(&event.motion);
260  mouse0_down_x = event.button.x;
261  mouse0_down_y = event.button.y;
262  break;
263  case SDL_QUIT:
264  /* Handle quit requests (like Ctrl-c). */
265  odeinit.stop = true;
266  break;
267 
268  case SDL_MOUSEBUTTONDOWN:
269  handle_mouse_motion(&event.motion);
270  switch (event.button.button)
271  {
272  case SDL_BUTTON_LEFT:
273  //deleteRay = false;
274  picking = false;
275  //printf(" Down\n");
276  break;
277  case SDL_BUTTON_MIDDLE:
278  break;
279  case SDL_BUTTON_RIGHT:
280  break;
281  default:
282  //this is not reached
283  break;
284  }
285  break;
286  break;
287  case SDL_MOUSEBUTTONUP:
288  switch (event.button.button)
289  {
290  case SDL_BUTTON_LEFT:
291  //printf(" up\n");
292  v=0;
293  break;
294  case SDL_BUTTON_MIDDLE:
295  //nothing
296  break;
297  case SDL_BUTTON_RIGHT:
298  //nothing
299  break;
300  default:
301  //this is not reached either
302  break;
303  }
304  break;
305  }
306  }
307 }
308 
309 void OdeSdlSimulation::nearCallback (void *data, dGeomID o1, dGeomID o2) {
310  OdeInit& odeinit = OdeInit::get();
311 
312  assert(o1);
313  assert(o2);
314  if (dGeomIsSpace(o1) || dGeomIsSpace(o2)){
315  // colliding a space with something
316  dSpaceCollide2(o1,o2,data,&nearCallback);
317  // Note we do not want to test intersections within a space,
318  // only between spaces.
319  return;
320  }
321  int i;
322  // if (o1->body && o2->body) return;
323 
324  // exit without doing anything if the two bodies are connected by a joint
325  dBodyID b1 = dGeomGetBody(o1);
326  dBodyID b2 = dGeomGetBody(o2);
327  if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
328 
329  dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box
330  for (i=0; i<MAX_CONTACTS; i++) {
331  contact[i].surface.mode = dContactSlip1| dContactSlip2| dContactBounce | dContactSoftCFM;
332  contact[i].surface.mu = dInfinity;
333  contact[i].surface.mu2 = 0;
334  contact[i].surface.bounce = 0.01;
335  contact[i].surface.bounce_vel = 0.01;
336  contact[i].surface.slip1 = (dReal)0.000001;
337  contact[i].surface.slip2 = (dReal)0.000001;
338  contact[i].surface.soft_cfm = 0.0001;
339  }
340  if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
341  sizeof(dContact))) {
342  dMatrix3 RI;
343  dRSetIdentity (RI);
344  for (i=0; i<numc; i++) {
345  dJointID c = dJointCreateContact (odeinit.world,odeinit.contactgroup,contact+i);
346  dJointAttach (c,b1,b2);
347  // if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);
348  // check if the bodies are touch sensitive.
349  bool b1isTouchSensitive = isBodyTouchSensitive (b1);
350  bool b2isTouchSensitive = isBodyTouchSensitive (b2);
351  // if any of the bodies are touch sensitive...
352  if (b1isTouchSensitive || b2isTouchSensitive) {
353  // ... add a feedback structure to the contact joint.
354  dJointSetFeedback (c, &(touchSensorFeedbacks[nFeedbackStructs]));
356  }
357  //fprintf(stdout,"colllliiiissssiiiiooon: %d %d\n", dGeomGetClass (o1), dGeomGetClass (o2));
358  }
359  }
360 }
361 /*static void nearCallback (void *data, dGeomID o1, dGeomID o2){
362  assert(o1);
363  assert(o2);
364  if (dGeomIsSpace(o1) || dGeomIsSpace(o2)){
365  // colliding a space with something
366  dSpaceCollide2(o1,o2,data,&nearCallback);
367  // Note we do not want to test intersections within a space,
368  // only between spaces.
369  return;
370  }
371  int i;
372  // exit without doing anything if the two bodies are connected by a joint
373  dBodyID b1 = dGeomGetBody(o1);
374  dBodyID b2 = dGeomGetBody(o2);
375  if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) {printf("testing space %p %p\n", b1,b1); return;}
376 
377  const int N = 5;
378  dContact contact[N]; // up to MAX_CONTACTS
379  for (i=0; i<N; i++) {
380  contact[i].surface.mode = 0;//dContactSlip1| dContactSlip2| dContactApprox1;
381  contact[i].surface.mu = dInfinity;
382  //contact[i].surface.mu2 = 0;
383  //contact[i].surface.slip1 = (dReal)0.0001;
384  //contact[i].surface.slip2 = (dReal)0.0001;
385  //contact[i].surface.soft_cfm = 0.1;
386  }
387  if (int numc = dCollide (o1,o2,5,&contact[0].geom,
388  sizeof(dContact))) {
389  /*dMatrix3 RI;
390  dRSetIdentity (RI);
391  for (i=0; i<numc; i++) {
392  dJointID c = dJointCreateContact (odeinit.world,odeinit.contactgroup,contact+i);
393  dJointAttach (c,b1,b2);
394  //dsSetColor (1.0,0.0,0.0); dsDrawBox (contact[i].geom.pos,RI,ss);
395  }
396  }
397  }
398 */
399 
400 // returns true if the body with the bodyID is a touch-sensitive body, returns false otherwise.
401 bool OdeSdlSimulation::isBodyTouchSensitive (dBodyID bodyID) {
402  OdeInit& odeinit = OdeInit::get();
403 
404  // check the smaller hand parts if the left hand is active.
405  if (odeinit._iCub->actLHand == "on") {
406  if (bodyID == odeinit._iCub->body[10]) {
407  return true;
408  } else if (bodyID == odeinit._iCub->body[30]) {
409  return true;
410  } else if (bodyID == odeinit._iCub->body[24]) {
411  return true;
412  } else if (bodyID == odeinit._iCub->body[25]) {
413  return true;
414  } else if (bodyID == odeinit._iCub->lhandfingers3) {
415  return true;
416  }
417  } else { // check the whole left hand body if the hand is not active.
418  if (bodyID == odeinit._iCub->l_hand) {
419  return true;
420  }
421  }
422 
423  // check the smaller hand parts if the right hand is active.
424  if (odeinit._iCub->actRHand == "on") {
425  if (bodyID == odeinit._iCub->body[11]) {
426  return true;
427  } else if (bodyID == odeinit._iCub->body[49]) {
428  return true;
429  } else if (bodyID == odeinit._iCub->body[43]) {
430  return true;
431  } else if (bodyID == odeinit._iCub->body[44]) {
432  return true;
433  } else if (bodyID == odeinit._iCub->rhandfingers3) {
434  return true;
435  }
436  } else { // check the whole right hand body if the hand is not active.
437  if (bodyID == odeinit._iCub->r_hand) {
438  return true;
439  }
440  }
441 
442  return false;
443 }
444 
445 // this is a function to mimic the sensor data from the physical icub fingetip/palm sensors
446 void OdeSdlSimulation::inspectBodyTouch_icubSensors(Bottle& reportLeft, Bottle& reportRight, bool boolean) {
447  OdeInit& odeinit = OdeInit::get();
448  reportLeft.clear();
449  reportRight.clear();
450  int indicesLeft[6] = {24, 25, 26, 27, 30, 10};
451  int indicesRight[6] = {43, 44, 45, 46, 49, 11};
452 
453  if (odeinit._iCub->actLHand == "on" && odeinit._iCub->actRHand == "on" ){
454  double resultLeft=0, resultRight = 0;
455  for (int x = 0; x < 6; x++){
456  if (boolean){
457  resultLeft = odeinit._iCub->checkTouchSensor( indicesLeft[x] );
458  resultRight = odeinit._iCub->checkTouchSensor( indicesRight[x] );
459  }
460  else{
461  resultLeft = odeinit._iCub->checkTouchSensor_continuousValued( indicesLeft[x] );
462  resultRight = odeinit._iCub->checkTouchSensor_continuousValued( indicesRight[x] );
463  }
464 
465  if (x < 5){
466  for (int i = 0; i < 12; i++){
467  reportLeft.addDouble(resultLeft * 255);
468  reportRight.addDouble(resultRight * 255);
469  }
470  }
471  if (x == 5){
472  for (int y = 0; y<3; y++){
473  for (int i = 0; i < 12; i++){
474  reportLeft.addDouble(0.0);
475  reportRight.addDouble(0.0);
476  }
477  }
478  for (int y = 0; y<4; y++){
479  for (int i = 0; i < 12; i++){
480  reportLeft.addDouble(resultLeft * 255);
481  reportRight.addDouble(resultRight * 255);
482  }
483  }
484  for (int y = 0; y<4; y++){
485  for (int i = 0; i < 12; i++){
486  reportLeft.addDouble(0.0);
487  reportRight.addDouble(0.0);
488  }
489  }
490  }
491  }
492  }//end lhand on rhand on
493  else if (odeinit._iCub->actLHand == "on" && odeinit._iCub->actRHand == "off" ){
494  double resultLeft=0, resultRight = 0;
495  for (int x = 0; x < 6; x++){
496  if (boolean){
497  resultLeft = odeinit._iCub->checkTouchSensor( indicesLeft[x] );
498  resultRight = odeinit._iCub->checkTouchSensor( odeinit._iCub->r_hand );
499  }
500  else{
501  resultLeft = odeinit._iCub->checkTouchSensor_continuousValued( indicesLeft[x] );
502  resultRight = odeinit._iCub->checkTouchSensor_continuousValued(odeinit._iCub->r_hand);
503  }
504  if (x < 5){
505  for (int i = 0; i < 12; i++){
506  reportLeft.addDouble(resultLeft * 255);
507  reportRight.addDouble(resultRight * 255);
508  }
509  }
510  if (x == 5){
511  for (int y = 0; y<3; y++){
512  for (int i = 0; i < 12; i++){
513  reportLeft.addDouble(0.0);
514  reportRight.addDouble(0.0);
515  }
516  }
517  for (int y = 0; y<4; y++){
518  for (int i = 0; i < 12; i++){
519  reportLeft.addDouble(resultLeft * 255);
520  reportRight.addDouble(resultRight * 255);
521  }
522  }
523  for (int y = 0; y<4; y++){
524  for (int i = 0; i < 12; i++){
525  reportLeft.addDouble(0.0);
526  reportRight.addDouble(0.0);
527  }
528  }
529  }
530  }
531  }//end lhand on rhand off
532  else if (odeinit._iCub->actRHand == "on" && odeinit._iCub->actLHand == "off" ){
533  double resultLeft=0, resultRight = 0;
534  for (int x = 0; x < 6; x++){
535  if (boolean){
536  resultLeft = odeinit._iCub->checkTouchSensor( odeinit._iCub->l_hand );
537  resultRight = odeinit._iCub->checkTouchSensor( indicesRight[x] );
538  }
539  else{
540  resultLeft = odeinit._iCub->checkTouchSensor_continuousValued( odeinit._iCub->l_hand );
541  resultRight = odeinit._iCub->checkTouchSensor_continuousValued( indicesRight[x] );
542  }
543 
544  if (x < 5){
545  for (int i = 0; i < 12; i++){
546  reportLeft.addDouble(resultLeft * 255);
547  reportRight.addDouble(resultRight * 255);
548  }
549  }
550  if (x == 5){
551  for (int y = 0; y<3; y++){
552  for (int i = 0; i < 12; i++){
553  reportLeft.addDouble(0.0);
554  reportRight.addDouble(0.0);
555  }
556  }
557  for (int y = 0; y<4; y++){
558  for (int i = 0; i < 12; i++){
559  reportLeft.addDouble(resultLeft * 255);
560  reportRight.addDouble(resultRight * 255);
561  }
562  }
563  for (int y = 0; y<4; y++){
564  for (int i = 0; i < 12; i++){
565  reportLeft.addDouble(0.0);
566  reportRight.addDouble(0.0);
567  }
568  }
569  }
570  }
571  }//end lhand off rhand on
572  else{//both off
573  for (int x = 0; x < 6; x++){
574  double resultLeft=0, resultRight = 0;
575  if (boolean){
576  resultLeft = odeinit._iCub->checkTouchSensor( odeinit._iCub->l_hand );
577  resultRight = odeinit._iCub->checkTouchSensor( odeinit._iCub->r_hand );
578  }
579  else{
580  resultLeft = odeinit._iCub->checkTouchSensor_continuousValued( odeinit._iCub->l_hand );
581  resultRight = odeinit._iCub->checkTouchSensor_continuousValued(odeinit._iCub->r_hand);
582  }
583 
584  if (x < 5){
585  for (int i = 0; i < 12; i++){
586  reportLeft.addDouble(resultLeft * 255);
587  reportRight.addDouble(resultRight * 255);
588  }
589  }
590  if (x == 5){
591  for (int y = 0; y<3; y++){
592  for (int i = 0; i < 12; i++){
593  reportLeft.addDouble(0.0);
594  reportRight.addDouble(0.0);
595  }
596  }
597  for (int y = 0; y<4; y++){
598  for (int i = 0; i < 12; i++){
599  reportLeft.addDouble(resultLeft * 255);
600  reportRight.addDouble(resultRight * 255);
601  }
602  }
603  for (int y = 0; y<4; y++){
604  for (int i = 0; i < 12; i++){
605  reportLeft.addDouble(0.0);
606  reportRight.addDouble(0.0);
607  }
608  }
609  }
610  }
611  }//end both off
612 }
613 
614 
615 void OdeSdlSimulation::getAngles(const dReal *m, float& z, float& y, float& x) {
616  const dReal eps = 0.00001;
617 
618  y = -asin(m[2]);
619  float c = cos(y);
620 
621  if (fabs(c)>eps) {
622  x = atan2(-m[6]/c,m[10]/c);
623  z = atan2(-m[1]/c,m[0]/c);
624  } else {
625  x = 0;
626  z = -atan2(m[4],m[5]);
627  }
628  x *= -180/M_PI;
629  y *= 180/M_PI;
630  z *= 180/M_PI;
631 }
632 
633 void OdeSdlSimulation::initViewpoint() {
634  xpos = 0;
635  ypos = 1;
636  zpos = 1;
637  xrot = 25;
638  yrot = 0;
639  zrot = 0;
640 }
641 
642 void OdeSdlSimulation::mouseMovement(float x, float y) {
643  float diffx = x-lastx; //check the difference between the current x and the last x position
644  float diffy = y-lasty; //check the difference between the current y and the last y position
645  lastx =x; //set lastx to the current x position
646  lasty =y; //set lasty to the current y position
647  xrot += (float) diffy; //set the xrot to xrot with the addition of the difference in the y position
648  yrot += (float) diffx; //set the xrot to yrot with the addition of the difference in the x position
649 }
650 
651 void OdeSdlSimulation::draw_screen() {
652  OdeInit& odeinit = OdeInit::get();
653 
654  glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT); // refresh opengl
655 
656  if (extractImages || odeinit._iCub->actVision == "on"){
658  }
659 
660  glViewport(0,0,width,height);
661  glMatrixMode (GL_PROJECTION);
662  glLoadIdentity();
663  gluPerspective( 75, (float)width/height, 0.01, 100.0 );
664  glMatrixMode (GL_MODELVIEW);
665  glLoadIdentity();
666  glLightfv(GL_LIGHT0, GL_POSITION, light_position);
667  glRotatef (xrot, 1,0,0);
668  glRotatef (yrot, 0,1,0);
669  glRotatef (zrot, 0,0,1);
670  glTranslated(-xpos,-ypos,-zpos);
671 
672  // set up any video textures
673 
674  if (video!=0)
675  DrawVideo(video);
676 
677  //draw the ground
678  glColor3d(0.5,0.5,1);
679  glPushMatrix();
680  glRotatef(90.0,1,0,0);
681  glRotatef(180.0,0,1,0);
682  DrawGround(false);
683  glPopMatrix();
684  glDisable(GL_TEXTURE_2D);
685  draw();
686  glEnable(GL_TEXTURE_2D);
687  drawSkyDome(0,0,0,50,50,50); // Draw the Skybox
688  SDL_GL_SwapBuffers();// Swap Buffers
689 }
690 
691 
692 
693 void OdeSdlSimulation::retreiveInertialData(Bottle& inertialReport) {
694  OdeInit& odeinit = OdeInit::get();
695  static dReal OldLinearVel[3], LinearVel[3], LinearAccel[3];
696  inertialReport.clear();
697 
698  //get euler angles from quaternions
699  dQuaternion angles;
700  dGeomGetQuaternion( odeinit._iCub->inertialGeom, angles );
701  dReal w, x, y, z;
702  w = angles[0];
703  x = angles[1];
704  y = angles[2];
705  z = angles[3];
706 
707  double sqw = w * w;
708  double sqx = x * x;
709  double sqy = y * y;
710  double sqz = z * z;
711  float roll, pitch, yaw;
712 
713  double unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
714  double test = x*y + z*w;
715  if (test > 0.499*unit) { // singularity at north pole
716  roll = 2 * atan2(x,w);
717  pitch = M_PI/2;
718  yaw = 0;
719  return;
720  }
721  if (test < -0.499*unit) { // singularity at south pole
722  roll = -2 * atan2(x,w);
723  pitch = -M_PI/2;
724  yaw = 0;
725  return;
726  }
727  roll =(float) ( atan2(2.0*y*w-2*x*z , sqx - sqy - sqz + sqw) ); //z
728  pitch = (float) (atan2(2.0*x*w-2*y*z , -sqx + sqy - sqz + sqw) );//x
729  yaw = asin(2*test/unit);//y
730 
731  //roll = dBodyGetRotation(odeinit._iCub->head)[4]; // was 1
732  //pitch = dBodyGetRotation(odeinit._iCub->head)[6];
733  //yaw = dBodyGetRotation(odeinit._iCub->head)[2];
734 
735  //Add Euler angles roll pitch yaw
736  inertialReport.addDouble( -yaw * 180/M_PI);// yaw
737  inertialReport.addDouble( -pitch * 180/M_PI);// pitch
738  inertialReport.addDouble( roll * 180/M_PI);// roll
739 
740  /*//in order to calculate linear acceleration (make sure of body) Inertial Measurement Unit IMU
741  LinearVel[0] = dBodyGetLinearVel(odeinit._iCub->inertialBody)[0];
742  LinearVel[1] = dBodyGetLinearVel(odeinit._iCub->inertialBody)[1];
743  LinearVel[2] = dBodyGetLinearVel(odeinit._iCub->inertialBody)[2];
745  LinearAccel[0] = ( LinearVel[0] - OldLinearVel[0] ) / 0.02;
746  LinearAccel[1] = ( LinearVel[1] - OldLinearVel[1] ) / 0.02;
747  LinearAccel[2] = ( LinearVel[2] - OldLinearVel[2] ) / 0.02;
748  OldLinearVel[0] = LinearVel[0];
749  OldLinearVel[1] = LinearVel[1];
750  OldLinearVel[2] = LinearVel[2];*/
751 
753  Vector grav,grav1,grav2,grav3;
754  grav.resize(3);
755  grav1.resize(3);
756  grav2.resize(3);
757  grav3.resize(3);
758  double theta;
759 
760  grav[0]=0;
761  grav[1]=0;
762  grav[2]=9.81;
763 
764  theta = pitch;
765  grav1[0]=grav[0]*cos(theta)+grav[2]*sin(theta);
766  grav1[1]=grav[1];
767  grav1[2]=grav[0]*(-sin(theta))+grav[2]*cos(theta);
768 
769  theta = yaw;
770  grav2[0]=grav1[0];
771  grav2[1]=grav1[1]*cos(theta)+grav1[2]*(-sin(theta));
772  grav2[2]=grav1[1]*sin(theta)+grav1[2]*cos(theta);
773 
774  theta = roll;
775  grav3[0]=grav2[0]*cos(theta)+grav2[1]*(-sin(theta));
776  grav3[1]=grav2[0]*sin(theta)+grav2[1]*cos(theta);
777  grav3[2]=grav2[2];
778 
779  inertialReport.addDouble( grav3[0] );
780  inertialReport.addDouble( grav3[1] );
781  inertialReport.addDouble( grav3[2] );
782 
783  //Add angular velocity
784  inertialReport.addDouble(-dBodyGetAngularVel(odeinit._iCub->inertialBody)[2]*CTRL_RAD2DEG);
785  inertialReport.addDouble(-dBodyGetAngularVel(odeinit._iCub->inertialBody)[0]*CTRL_RAD2DEG);
786  inertialReport.addDouble( dBodyGetAngularVel(odeinit._iCub->inertialBody)[1]*CTRL_RAD2DEG);
787 
788  //Add magnetic fields
789  inertialReport.addDouble(0.0);
790  inertialReport.addDouble(0.0);
791  inertialReport.addDouble(0.0);
792 }
793 
794 int OdeSdlSimulation::thread_ode(void *unused) {
795  //SLD_AddTimer freezes the system if delay is too short. Instead use a while loop that waits if there was time left after the computation of ODE_process
796  double cpms = 1e3 / CLOCKS_PER_SEC;
797  long lastOdeProcess = (long) (clock()*cpms);
798  double avg_ode_step_length = 0.0;
799  long count = 0;
800  simrun = true;
801  double timeCache = ode_step_length;
802  long lastTimeCacheUpdate = (long) (clock()*cpms);
803  double alpha = 0.99;
804  // if realTime=true when delays occur the simulation tries to recover by running more steps in a row
805  // if realTime=false the simulation executes the simulation steps with a fixed rate irregardless of delays
806  bool realTime = true;
807  long temp;
808 
809  while (simrun) {
810  temp = (long) (clock()*cpms);
811  timeCache += temp - lastTimeCacheUpdate;
812  lastTimeCacheUpdate = temp;
813  while(timeCache < ode_step_length){
814  SDL_Delay((unsigned int)(ode_step_length-timeCache));
815  temp = (long) (clock()*cpms);
816  timeCache += temp - lastTimeCacheUpdate;
817  lastTimeCacheUpdate = temp;
818  }
819 
820  /*if(timeCache >= 2.0*ode_step_length)
821  printf("Simulation delay: running %d steps in a row to recover.\n", (int)(timeCache/ode_step_length));*/
822 
823  while(timeCache >= ode_step_length){
824  count++;
825  lastOdeProcess = (long) (clock()*cpms);
826  ODE_process(1, (void*)1);
827  avg_ode_step_length = alpha*avg_ode_step_length + (1.0-alpha)*((long) (clock()*cpms) -lastOdeProcess);
828 
829  if(realTime)
830  timeCache -= ode_step_length;
831  else
832  timeCache = 0.0;
833 
834  // check if the desired timestep is achieved, if not, print a warning msg
835  if(count % (10000/ode_step_length)==0){
836  if(avg_ode_step_length >= ode_step_length+1)
837  printf("WARNING: the simulation is too slow to run in real-time, you should increase the timestep in ode_params.ini (current value: %ld, suggested value: %.0f)\n",
838  ode_step_length, avg_ode_step_length);
839  else if(avg_ode_step_length <= ode_step_length-1)
840  printf("INFO: you could get a more accurate dynamics simulation by decreasing the timestep in ode_params.ini (current value: %ld, suggested value: %.0f)\n",
841  ode_step_length, avg_ode_step_length);
842  }
843  }
844  }
845  return(0);
846 }
847 
848 Uint32 OdeSdlSimulation::ODE_process(Uint32 interval, void *param) {
849  OdeInit& odeinit = OdeInit::get();
850  //static clock_t startTimeODE= clock(), finishTimeODE= clock();
851  //startTimeODE = clock();
852 
853  odeinit.mutex.wait();
855  dSpaceCollide(odeinit.space,0,&nearCallback);
856  dWorldStep(odeinit.world, dstep);
857  // do 1 TIMESTEP in controllers (ok to run at same rate as ODE: 1 iteration takes about 300 times less computation time than dWorldStep)
858  for (int ipart = 0; ipart<MAX_PART; ipart++) {
859  if (odeinit._controls[ipart] != NULL) {
860  odeinit._controls[ipart]->jointStep();
861  }
862  }
863  odeinit.sync = true;
864  odeinit.mutex.post();
865 
867  Bottle reportLeft;
868  Bottle reportRight;
869  bool boolean = true;
870  if (odeinit._iCub->actPressure == "on")
871  boolean = false;
872 
873  inspectBodyTouch_icubSensors(reportLeft, reportRight, boolean);//inspectBodyTouch_continuousValued(report);
874 
876  robot_streamer->sendTouchLeft( reportLeft );
877 
879  robot_streamer->sendTouchRight( reportRight );
880  }
881 
882  dJointGroupEmpty (odeinit.contactgroup);
883 
885  Bottle inertialReport;
886  retreiveInertialData(inertialReport);
887  robot_streamer->sendInertial(inertialReport);
888  }
889 
890  //go and check if torques are needed
892 
893  odeinit._iCub->setJointControlAction();
894 
895  //finishTimeODE = clock() ;
896  //SPS();
897  //printf("ODE=%lf\n",(double)(finishTimeODE - startTimeODE) / CLOCKS_PER_SEC);
898  return(interval);
899 }
900 
901 
902 /*int OdeSdlSimulation::thread_func(void *unused) {
903  // this needs to be kept synchronized with the timestep in
904  // dWorldStep, in order to get correct world clock time
905  // --paulfitz
906  int delay = 50;
907  id = SDL_AddTimer( delay, &OdeSdlSimulation::ODE_process, (void*)1);
908 
909  return(0);
910 }
911 */
912 /*
913  static void SPS()
914  {
915  static float sps = 0.0f;
916  static float previousTime = 0.0f;
917  static int currentsps;
918  static char strSPS[60] = {0};
919 
920  float currentTime = (GetTickCount() * 0.001f);
921 
922  ++sps; // Increment the SPS counter
923 
924  if( currentTime - previousTime > 1.0f ){
925  previousTime = currentTime;
926  currentsps = int(sps);
927  printf("current SPS: %d\n",currentsps);
928  sps = 0.0f;
929  }
930  }
931 */
932 
933 void OdeSdlSimulation::sighandler(int sig) {
934  OdeInit& odeinit = OdeInit::get();
935  odeinit.stop = true;
936  cout << "\nCAUGHT Ctrl-c" << endl;
937 }
938 
939 void OdeSdlSimulation::simLoop(int h,int w) {
940  OdeInit& odeinit = OdeInit::get();
941 
942  SDL_Init(SDL_INIT_TIMER | SDL_GL_ACCELERATED_VISUAL);
943  SDL_SetVideoMode(h,w,32,SDL_OPENGL | SDL_RESIZABLE);// | SDL_SWSURFACE| SDL_ANYFORMAT); // on init
944 
945  dAllocateODEDataForThread(dAllocateMaskAll);
946  ConstString logo = robot_config->getFinder().findFile("logo");
947 
948  image = SDL_LoadBMP(robot_config->getFinder().findFile(logo.c_str()).c_str());
949  SDL_WM_SetIcon(image,0);
950  SDL_FreeSurface(image);
951  SDL_WM_SetCaption("iCub Simulator", "image");
952 
953  //SDL_Thread *thread;
954  SDL_Thread *ode_thread = SDL_CreateThread(thread_ode, NULL);
955  //thread = SDL_CreateThread(thread_func, NULL);
956 
957  if ( ode_thread == NULL ) {
958  fprintf(stderr, "Unable to create thread: %s\n", SDL_GetError());
959  return;
960  }
961 
962  initViewpoint();
963  bool ok = setup_opengl(robot_config->getFinder());
964  if (!ok) return;
965  startTime = (long) clock();
966  odeinit.stop = false;
967 
968  yarp::os::signal(yarp::os::YARP_SIGINT, sighandler);
969  yarp::os::signal(yarp::os::YARP_SIGTERM, sighandler);
970 
971  glrun = true;
972  odeinit._wrld->WAITLOADING = false;
973  odeinit._wrld->static_model = false;
974  long prevTime = (long) clock();
975  long timeLeft;
976 
977  if (odeinit._iCub->actStartHomePos == "on")
978  odeinit.sendHomePos();
979 
980  while(!odeinit.stop) {
981  /* Process incoming events. */
982  process_events();
983  /* Draw the screen. */
984  if ( !odeinit._wrld->WAITLOADING ){
985  if (glrun) {
986  odeinit.mutexTexture.wait();
987  draw_screen();
988  odeinit.mutexTexture.post();
989  // check for framerate
990  timeLeft = (prevTime - (long) clock()) + gl_frame_length;
991  //cout << "check for framerate " << timeLeft << endl;
992  if (timeLeft > 0)
993  { // if there is still time left in this frame, just wait
994  SDL_Delay(timeLeft);
995  }
996  prevTime = (long) clock();
997  } else {
998  SDL_Delay(100);
999  }
1000  }
1001  else{
1002  glFinish();
1003  glFlush();
1004  //make sure it can also be done for static objects
1005  if (odeinit._wrld->static_model){
1006  odeinit._wrld->loadTexture(odeinit._wrld->texture, odeinit._wrld->s_modelTexture[odeinit._wrld->s_MODEL_NUM-1]);
1007  }else{
1008  odeinit._wrld->loadTexture(odeinit._wrld->texture, odeinit._wrld->modelTexture[odeinit._wrld->MODEL_NUM-1]);
1009  }
1010  odeinit._wrld->WAITLOADING = false;
1011  odeinit._wrld->static_model = false;
1012  }
1013  }
1014  printf("\n\nStopping SDL and ODE threads...\n");
1015  //stop the timer
1016  //SDL_RemoveTimer(id);
1017  //Stop the thread
1018  //SDL_KillThread( thread );
1019  simrun = false;
1020  //SDL_WaitThread( thread, NULL );
1021  SDL_WaitThread( ode_thread, NULL );
1022  //SDL_Quit();
1023 }
1024 
1025 void OdeSdlSimulation::drawView(bool left, bool right, bool wide) {
1026  OdeInit& odeinit = OdeInit::get();
1027  const dReal *pos;
1028  const dReal *rot;
1029  glViewport(0,0,cameraSizeWidth,cameraSizeHeight);
1030  glMatrixMode (GL_PROJECTION);
1031 
1032  if (left){
1033  glLoadIdentity();
1034  gluPerspective( fov_left, (float) width_left/height_left, 0.04, 100.0 );
1035  pos = dGeomGetPosition(odeinit._iCub->Leye1_geom);
1036  rot = dGeomGetRotation(odeinit._iCub->Leye1_geom);
1037  glMatrixMode (GL_MODELVIEW);
1038  glLoadIdentity();
1039  glLightfv(GL_LIGHT0, GL_POSITION, light_position);
1040  gluLookAt(
1041  pos[0],
1042  pos[1],
1043  pos[2],
1044  pos[0] + rot[2],
1045  pos[1] + rot[6],
1046  pos[2] + rot[10],
1047  -rot[4], 1, 0
1048  );
1049  }
1050  if (right){
1051  glLoadIdentity();
1052  gluPerspective( fov_right, (float) width_right/height_right, 0.04, 100.0 );//55.8
1053  pos = dGeomGetPosition(odeinit._iCub->Reye1_geom);
1054  rot = dGeomGetRotation(odeinit._iCub->Reye1_geom);
1055  glMatrixMode (GL_MODELVIEW);
1056  glLoadIdentity();
1057  glLightfv(GL_LIGHT0, GL_POSITION, light_position);
1058  gluLookAt(
1059  pos[0],
1060  pos[1],
1061  pos[2],
1062  pos[0] + rot[2],
1063  pos[1] + rot[6],
1064  pos[2] + rot[10],
1065  -rot[4], 1, 0
1066  );
1067  }
1068  if (wide){
1069  glLoadIdentity();
1070  gluPerspective( 55.8, (float) cameraSizeWidth/cameraSizeHeight, 0.04, 100.0 );//here nothing to do with cameras
1071  glMatrixMode (GL_MODELVIEW);
1072  glLoadIdentity();
1073  glLightfv(GL_LIGHT0, GL_POSITION, light_position);
1074  glRotatef (xrot, 1,0,0);
1075  glRotatef (yrot, 0,1,0);
1076  glTranslated(-xpos,-ypos,-zpos);
1077  }
1078 
1079  //draw the ground
1080  glColor3d(0.5,0.5,1);
1081  glEnable(GL_TEXTURE_2D);
1082  glPushMatrix();
1083  glRotatef(90.0,1,0,0);
1084  glRotatef(180.0,0,1,0);
1085  DrawGround(false);
1086  glPopMatrix();
1087  glDisable(GL_TEXTURE_2D);
1088  draw();//robot
1089  glEnable(GL_TEXTURE_2D);
1090  drawSkyDome(0,0,0,50,50,50); // Draw the Skybox
1091 }
1092 
1094  glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT); // refresh opengl
1095 }
1096 
1098 }
1099 
1101  RobotConfig *config) {
1102  OdeInit& odeinit = OdeInit::get();
1103  if (video!=NULL) {
1104  fprintf(stderr, "Only one Simulation object allowed\n");
1105  yarp::os::exit(1);
1106  }
1107  robot_streamer = streamer;
1108  robot_config = config;
1109 
1110  ode_step_length = config->getWorldTimestep();
1111  dstep = ode_step_length*1e-3;
1112 
1113  video = new VideoTexture;
1114  string moduleName = odeinit.getName();
1115  video->setName( moduleName );
1116  odeinit._iCub->eyeLidsPortName = moduleName;
1117  Property options;
1118 
1119  //get the camera calibration parameters
1120  string camcalib_context=robot_config->getFinder().check("camcalib_context",
1121  Value("cameraCalibration")).asString().c_str();
1122  string camcalib_file=robot_config->getFinder().check("camcalib_file",
1123  Value("icubSimEyes.ini")).asString().c_str();
1124 
1125  ResourceFinder rf_camcalib;
1126  rf_camcalib.setVerbose();
1127  rf_camcalib.setDefaultContext(camcalib_context.c_str());
1128  rf_camcalib.setDefaultConfigFile(camcalib_file.c_str());
1129  rf_camcalib.configure(0,NULL);
1130 
1131  //left
1132  Bottle &bCalibLeft=rf_camcalib.findGroup("CAMERA_CALIBRATION_LEFT");
1133  width_left=bCalibLeft.check("w",Value(320)).asInt();
1134  height_left=bCalibLeft.check("h",Value(240)).asInt();
1135 
1138 
1139  double focal_length_left=bCalibLeft.check("fy",Value(257.34)).asDouble();
1140  fov_left=2*atan2((double)height_left,2*focal_length_left)*180.0/M_PI;
1141 
1142  //right
1143  Bottle &bCalibRight=rf_camcalib.findGroup("CAMERA_CALIBRATION_RIGHT");
1144  width_right=bCalibRight.check("w",Value(320)).asInt();
1145  height_right=bCalibRight.check("h",Value(240)).asInt();
1146 
1147  double focal_length_right=bCalibRight.check("fy",Value(257.34)).asDouble();
1148  fov_right=2*atan2((double)height_right,2*focal_length_right)*180.0/M_PI;
1149  //--------------------------------------//
1150 
1151 
1152  ConstString videoconf = robot_config->getFinder().findFile("video");
1153  options.fromConfigFile(videoconf.c_str());
1154 
1155  Bottle textures = *options.find("textures").asList();
1156  for (int i=0; i<textures.size(); i++) {
1157  ConstString name = textures.get(i).asString();
1158  printf("Adding video texture %s\n", name.c_str());
1159  video->add(options.findGroup(name.c_str()));
1160  }
1161 }
1162 
1164  delete video;
1165 }
1166 
1167 
1169  OdeInit& odeinit = OdeInit::get();
1170  if (reset) {
1171  odeinit.sync = false;
1172  }
1173  return odeinit.sync;
1174 }
1175 
1176 
1177 bool OdeSdlSimulation::getTrqData(Bottle data) {
1178  OdeInit& odeinit = OdeInit::get();
1179  for (int s=0; s<data.size(); s++){
1180  odeinit._iCub->torqueData[s] = data.get(s).asDouble();
1181  //fprintf(stdout,"torques... %lf \n",odeinit._iCub->torqueData[s]);
1182  }
1183  return true;
1184 }
1185 
1186 
1187 
1188 bool OdeSdlSimulation::getImage(ImageOf<PixelRgb>& target) {
1189  int w = cameraSizeWidth;
1190  int h = cameraSizeHeight;
1191  int p = 3;
1192 
1193  char *buf=new char[w * h * p];
1194  glReadPixels( 0, 0, w, h, GL_RGB, GL_UNSIGNED_BYTE, buf);
1195  ImageOf<PixelRgb> img;
1196  img.setQuantum(1);
1197  img.setExternal(buf,w,h);
1198 
1199  // inefficient flip!
1200  target.resize(img);
1201  int ww = img.width();
1202  int hh = img.height();
1203  for (int x=0; x<ww; x++) {
1204  for (int y=0; y<hh; y++) {
1205  target(x,y) = img(x,hh-1-y);
1206  }
1207  }
1208  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
1209  delete[] buf;
1210  return true;
1211 }
static float hpr[8]
Definition: iCub_Sim.cpp:45
static float lasty
Definition: iCub_Sim.cpp:73
static int mouseDiffy
Definition: iCub_Sim.cpp:49
static bool eyeCams
Definition: iCub_Sim.cpp:85
static SDL_Surface * image
Definition: iCub_Sim.cpp:80
#define MAX_PART
virtual bool getTrqData(Bottle data)
Definition: iCub_Sim.cpp:1177
struct timeval prevTime[BOARD_NUM]
virtual bool shouldSendInertial()=0
static RobotConfig * robot_config
Definition: iCub_Sim.cpp:84
double checkTouchSensor_continuousValued(int bodyToCheck)
Definition: iCub.cpp:105
yarp::os::Semaphore mutex
Definition: OdeInit.h:62
yarp::os::Semaphore mutexTexture
Definition: OdeInit.h:63
static float * VAD
Definition: iCub_Sim.cpp:60
static int width_left
Definition: iCub_Sim.cpp:89
static int mouse1_down_y
Definition: iCub_Sim.cpp:57
static double duration
Definition: iCub_Sim.cpp:76
bool sync
Definition: OdeInit.h:67
dBodyID inertialBody
Definition: iCub.h:80
OdeSdlSimulation()
Constructor.
Definition: iCub_Sim.cpp:1097
void DrawVideo(VideoTexture *video)
Definition: rendering.cpp:455
virtual void sendInertial(yarp::os::Bottle &report)=0
void DrawGround(bool wireframe)
Definition: rendering.cpp:121
string eyeLidsPortName
Definition: iCub.h:75
int MODEL_NUM
Definition: world.h:174
static long startTime
Definition: iCub_Sim.cpp:75
virtual bool shouldSendTouchLeft()=0
virtual void checkTorques()=0
#define N_TOUCH_SENSORS
Definition: iCub_Sim.cpp:102
bool WAITLOADING
Definition: world.h:223
bool static_model
Definition: world.h:224
static const GLfloat light_position[]
Definition: iCub_Sim.cpp:86
static int height_right
Definition: iCub_Sim.cpp:92
static double dstep
Definition: iCub_Sim.cpp:36
ICubSim * _iCub
Definition: OdeInit.h:64
static float zoom
Definition: iCub_Sim.cpp:71
static VideoTexture * video
Definition: iCub_Sim.cpp:82
static long ode_step_length
Definition: iCub_Sim.cpp:35
void setName(string module)
dBodyID l_hand
Definition: iCub.h:147
const dReal * pos
Definition: iCub_Sim.cpp:62
static float view2_hpr[3]
Definition: iCub_Sim.cpp:70
static int stop
Definition: iCub_Sim.cpp:41
dBodyID r_hand
Definition: iCub.h:147
static float ydistance
Definition: iCub_Sim.cpp:65
string getName()
Definition: OdeInit.h:75
dWorldID world
Definition: OdeInit.h:55
static float ypos
Definition: iCub_Sim.cpp:72
static float angle
Definition: iCub_Sim.cpp:72
bool add(const char *port, int textureIndex)
Definition: VideoTexture.h:78
static float yrot
Definition: iCub_Sim.cpp:72
static float zrot
Definition: iCub_Sim.cpp:72
static RobotStreamer * robot_streamer
Definition: iCub_Sim.cpp:83
yarp::os::ConstString texture
Definition: world.h:295
static int cameraSizeWidth
Definition: iCub_Sim.cpp:97
static double frames
Definition: iCub_Sim.cpp:76
Semaphore ODE_access(1)
int modelTexture[100]
Definition: world.h:261
static int width
Definition: iCub_Sim.cpp:53
static float view_xyz[3]
Definition: iCub_Sim.cpp:67
bool reinitialized
Definition: iCub.h:73
tuple config
Definition: icubapp.py:262
virtual yarp::os::ResourceFinder & getFinder()=0
static float yrotrad
Definition: iCub_Sim.cpp:74
ConstString actVision
Definition: iCub.h:69
#define MAX_CONTACTS
Definition: iCub.h:57
void drawView(bool left, bool right, bool wide)
Render the requested view.
Definition: iCub_Sim.cpp:1025
tuple name
Definition: manager.py:710
dSpaceID space
Definition: OdeInit.h:56
static float xrot
Definition: iCub_Sim.cpp:72
iCubSimulationControl ** _controls
Definition: OdeInit.h:70
static double seconds
Definition: iCub_Sim.cpp:76
fprintf(fid,'\n')
static float view2_xyz[3]
Definition: iCub_Sim.cpp:69
static int v
Definition: iCub_Sim.cpp:42
static float xrotrad
Definition: iCub_Sim.cpp:74
virtual void sendTouchLeft(yarp::os::Bottle &report)=0
void sendHomePos()
Definition: OdeInit.cpp:107
static OdeInit & get()
Definition: OdeInit.cpp:144
static bool picking
Definition: iCub_Sim.cpp:50
static long gl_frame_length
Definition: iCub_Sim.cpp:34
static long finishTime
Definition: iCub_Sim.cpp:75
static int height
Definition: iCub_Sim.cpp:54
static bool extractImages
Definition: iCub_Sim.cpp:81
virtual int getWorldTimestep()=0
static int mouse0_down_x
Definition: iCub_Sim.cpp:56
This file is responsible for the initialisation of the world parameters that are controlled by ODE...
ODE state information.
Definition: OdeInit.h:52
int s_modelTexture[100]
Definition: world.h:262
#define M_PI
Definition: math.h:53
static int nFeedbackStructs
Definition: iCub_Sim.cpp:107
static float lastx
Definition: iCub_Sim.cpp:73
virtual bool getImage(yarp::sig::ImageOf< yarp::sig::PixelRgb > &target)
Definition: iCub_Sim.cpp:1188
static double FPS
Definition: iCub_Sim.cpp:76
static float zpos
Definition: iCub_Sim.cpp:72
static int mouse0_down_y
Definition: iCub_Sim.cpp:57
virtual bool shouldSendTouchRight()=0
void draw()
Definition: world.cpp:62
#define CTRL_RAD2DEG
Definition: iCub_Sim.cpp:29
void clearBuffer()
Signal that we're done with a view.
Definition: iCub_Sim.cpp:1093
dGeomID inertialGeom
Definition: iCub.h:81
static float test[3]
Definition: iCub_Sim.cpp:77
double torqueData[100]
Definition: iCub.h:221
static dJointFeedback touchSensorFeedbacks[MAX_CONTACTS *N_TOUCH_SENSORS]
Definition: iCub_Sim.cpp:105
void simLoop(int h, int w)
Run the simulation.
Definition: iCub_Sim.cpp:939
static Uint32 colorkey
Definition: iCub_Sim.cpp:79
static float xpos
Definition: iCub_Sim.cpp:72
static float xdistance
Definition: iCub_Sim.cpp:66
static bool simrun
Definition: iCub_Sim.cpp:39
float eyeLidRot
Definition: iCub.h:74
dBodyID body[50]
Definition: iCub.h:101
dJointGroupID contactgroup
Definition: OdeInit.h:57
static float angle_xref
Definition: iCub_Sim.cpp:63
static int contactPoint
Definition: iCub_Sim.cpp:48
void setJointControlAction()
Set the control action for all the joints, that can be either a velocity command or a torque command...
Definition: iCub.cpp:189
dGeomID Leye1_geom
Definition: iCub.h:161
~OdeSdlSimulation()
Destructor.
Definition: iCub_Sim.cpp:1163
static float view_hpr[3]
Definition: iCub_Sim.cpp:68
void drawSkyDome(float x, float y, float z, float width, float height, float length)
Definition: rendering.cpp:158
dGeomID Reye1_geom
Definition: iCub.h:166
worldSim * _wrld
Definition: OdeInit.h:65
void loadTexture(yarp::os::ConstString texture, int numTexture)
Definition: world.cpp:172
static float * VAD2
Definition: iCub_Sim.cpp:61
static bool glrun
Definition: iCub_Sim.cpp:38
ConstString actStartHomePos
Definition: iCub.h:69
static double fov_right
Definition: iCub_Sim.cpp:94
ConstString actPressure
Definition: iCub.h:69
int s_MODEL_NUM
Definition: world.h:175
static double TimestepManager
Definition: iCub_Sim.cpp:76
bool checkSync(bool reset=false)
Definition: iCub_Sim.cpp:1168
bool stop
Definition: OdeInit.h:66
static int width_right
Definition: iCub_Sim.cpp:90
static int mouse_ray_x
Definition: iCub_Sim.cpp:58
double SimTime
Definition: OdeInit.h:54
static int height_left
Definition: iCub_Sim.cpp:91
static int mouse_ray_y
Definition: iCub_Sim.cpp:59
static int mouse1_down_x
Definition: iCub_Sim.cpp:56
static int cameraSizeHeight
Definition: iCub_Sim.cpp:98
dBodyID lhandfingers3
Definition: iCub.h:117
form input data form the output data fill the input data for i
Definition: acquireData.m:52
This class controls the simulation speed using dWorldstep for "exact" calculations, the collisions between objects/spaces and the rendering functions.
static double fov_left
Definition: iCub_Sim.cpp:93
ConstString actRHand
Definition: iCub.h:69
void draw()
Definition: iCub.cpp:315
static float angle_yref
Definition: iCub_Sim.cpp:64
static float xyz[3]
Definition: iCub_Sim.cpp:44
static float cam_rx
Definition: iCub_Sim.cpp:51
void sighandler(int sig)
Definition: main_window.cpp:46
bool setup_opengl(ResourceFinder &finder)
Definition: rendering.cpp:63
void init(RobotStreamer *streamer, RobotConfig *config)
Initialization.
Definition: iCub_Sim.cpp:1100
static float cam_ry
Definition: iCub_Sim.cpp:51
virtual void sendTouchRight(yarp::os::Bottle &report)=0
static int mouseDiffx
Definition: iCub_Sim.cpp:49
ConstString actLHand
Definition: iCub.h:69
dBodyID rhandfingers3
Definition: iCub.h:129
bool checkTouchSensor(int bodyToCheck)
Definition: iCub.cpp:139
virtual void sendVision()=0
static float rez[3]
Definition: iCub_Sim.cpp:46