iCub-main
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties 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 #include <yarp/os/Log.h>
25 #include <yarp/os/LogStream.h>
26 
27 using namespace yarp::sig;
28 
29 // globals
30 Semaphore ODE_access(1);
31 #define CTRL_RAD2DEG (180.0/M_PI)
32 #define CTRL_DEG2RAD (M_PI/180.0)
33 
34 // locals
35 // NOTE that we use (long) instead of (clock_t), because on MacOS, (clock_t) is unsigned, while we need negative numbers
36 static long gl_frame_length = 1000/30; // update opengl and vision stream at 30 Hz
37 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)
38 static double dstep = 10.0/1000.0; // step size in ODE's dWorldStep in seconds
39 
40 static bool glrun; // draw gl
41 static bool simrun; // run simulator thread
42 
43 static int stop = 0;
44 static int v = 0;
45 
46 static float xyz[3];
47 static float hpr[8];
48 static float rez[3];
49 
50 static int contactPoint;
51 static int mouseDiffx, mouseDiffy;
52 static bool picking = false;
53 static float cam_rx = 0.0, cam_ry = 0.0;
54 
55 static int width = 640;
56 static int height = 480;
57 
60 static int mouse_ray_x;
61 static int mouse_ray_y;
62 static float *VAD;
63 static float *VAD2;
64 const dReal *pos;
65 static float angle_xref = 0.0f;
66 static float angle_yref = 25.0f;
67 static float ydistance = 10.0f;
68 static float xdistance = 0.0f;
69 static float view_xyz[3]; // position x,y,z
70 static float view_hpr[3]; // heading, pitch, roll (degrees)
71 static float view2_xyz[3];
72 static float view2_hpr[3];
73 static float zoom = 0;
74 static float xpos = 0, ypos = 0, zpos = 0, xrot = 0, yrot = 0, zrot = 0, angle=0.0;
75 static float lastx, lasty;
76 static float xrotrad = 0, yrotrad = 0;
77 static long startTime, finishTime;
79 static float test[3];
80 //static SDL_TimerID id;
81 static Uint32 colorkey;
82 static SDL_Surface *image;
83 static bool extractImages = false;
84 static VideoTexture *video = NULL;
85 static RobotStreamer *robot_streamer = NULL;
86 static RobotConfig *robot_config = NULL;
87 static bool eyeCams;
88 static const GLfloat light_position[] = { 0.0f, 5.0f, 5.0f, 0.0f };
89 
90 //camera calibration parameters
91 static int width_left;
92 static int width_right;
93 static int height_left;
94 static int height_right;
95 static double fov_left;
96 static double fov_right;
97 
98 
99 static int cameraSizeWidth;
100 static int cameraSizeHeight;
101 
102 
103 /* For every collision detected by ODE, contact joints (up to MAX_CONTACTS per collison) are created and a feedback structs may be associated with them - that will carry information about the contact.
104  * The number of collisions and contact joints may vary, but we allocate these as a static array for performance issues.
105  * (Allocating feedback structs at every simulation step would degrade simulation performance).
106  * If the MAX_DJOINT_FEEDBACKSTRUCTS was exceeded, contacts will still be saved for the purposes of whole_body_skin_emul,
107  * but the forces send to skinEvents will not be available.
108 */
109 #define MAX_DJOINT_FEEDBACKSTRUCTS 500
110 
112 static int nFeedbackStructs=0;
113 
114 static bool START_SELF_COLLISION_DETECTION = false; //we want to set this trigger on only after the robot is in in home pos -
115  //it's initial configuration is with arms inside the thighs
116 
117 
118 void OdeSdlSimulation::draw() {
119  OdeInit& odeinit = OdeInit::get();
120  odeinit._iCub->draw();
121  odeinit._wrld->draw();
122 }
123 
124 void OdeSdlSimulation::printStats() {
125  OdeInit& odeinit = OdeInit::get();
126 
127  finishTime = (long) clock() ;
128  duration += (double)(finishTime - startTime) / CLOCKS_PER_SEC ;
129  frames ++ ;
130  FPS = frames / duration ;
131  startTime = (long) clock() ;
132  odeinit.SimTime = duration;
133  //printf("duration: %.2lf\n",odeinit.SimTime);
134  static double starting_time_stamp = 0;
135  //test[0] = dBodyGetPosition(odeinit._iCub->body_cube[0])[0];
136  //test[1] = dBodyGetPosition(odeinit._iCub->body_cube[0])[1];
137  //test[2] = dBodyGetPosition(odeinit._iCub->body_cube[0])[2];
138  //printf("test[0] %f test[1] %f test[2] %f\n",test[0],test[1],test[2]);
139  if( duration - starting_time_stamp >= 1){
140  //printf("Frames: %.2lf Duration: %.2lf fps: %3.1f \n",frames,duration,FPS);
141  starting_time_stamp = duration;
142  }
143  //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]);
144  //drawText(text, textPos);
145 }
146 
147 void OdeSdlSimulation::handle_key_down(SDL_keysym* keysym) {
148  switch (keysym->sym)
149  {
150  case SDLK_e:
151  break;
152  case SDLK_r:
153  initViewpoint();
154  break;
155  case SDLK_t:
156  break;
157  case SDLK_y:
158  break;
159  case SDLK_SPACE:
160  printf("SPACEBAR pressed! Press spacebar again to disable/enable drawing.\n");
161  glrun = !glrun;
162  break;
163  default:
164  break;
165  }
166 }
167 
168 void OdeSdlSimulation::handle_mouse_motion(SDL_MouseMotionEvent* mousemotion) {
169  if (SDL_GetMouseState(NULL, NULL) & SDL_BUTTON(1)){// MOUSE LEFT BUTTON
170  //if (!picking){
171  //camera movement
172  angle_xref += (float)mousemotion->xrel; // 10.0f;
173  angle_yref += (float)mousemotion->yrel; // 10.0f;
174  mouseMovement(angle_xref,angle_yref);
175 
176  if (v<1){
177  //mouse_ray_x = mouse0_down_x;
178  //mouse_ray_y = mouse0_down_y;
179  }
180  /*mouseDiffx = mouse0_down_x - mouse_ray_x;
181  mouseDiffy = mouse0_down_y - mouse_ray_y;
182  mouse_ray_x = mouse0_down_x;
183  mouse_ray_y = mouse0_down_y;*/
184 
185  //VAD = ScreenToWorld(mouse0_down_x,mouse0_down_y,0);
186  //xpos = VAD[0];ypos = VAD[1];zpos = VAD[2];
187  //VAD2 =ScreenToWorld(mouse0_down_x,mouse0_down_y,1);
188  //xpos2 = VAD2[0];ypos2 = VAD2[1];zpos2 = VAD2[2];
189 
190  //if (i<1){ray = dCreateRay(space,100*100);}
191  //Origin[0] = xpos;
192  //Origin[1] = ypos;
193  //Origin[2] = zpos;
194  //Origin[3] = 0;
195  //Direction[0] = xpos2;
196  //Direction[1] = ypos2;
197  //Direction[2] = zpos2;
198  //Direction[3] = 0;
199  //dGeomRaySet(ray, Origin[0], Origin[1], Origin[2], Direction[0], Direction[1], Direction[2]);
200  //dGeomSetPosition(ray, xpos,ypos,zpos);
201  //i++;
202  }
203  if (SDL_GetMouseState(NULL, NULL) & SDL_BUTTON(3)){// MOUSE RIGHT BUTTON
204 
205  //xdistance -= mousemotion->xrel / 10.0f;
206  //ydistance -= mousemotion->yrel / 10.0f;
207  }
208 }
209 
210 void OdeSdlSimulation::process_events(void) {
211  OdeInit& odeinit = OdeInit::get();
212  SDL_Event event;
213 
214  Uint8 * keystate = SDL_GetKeyState(NULL);
215  if(keystate[SDLK_q]){xrot += 1 * 0.4f;if (xrot >360) xrot -= 360 * 0.1f;}
216  if(keystate[SDLK_z]){xrot -= 1 * 0.4f;if (xrot < -360) xrot += 360 * 0.1f;}
217  if(keystate[SDLK_w]){yrotrad = (yrot / 180 * 3.141592654f); xrotrad = (xrot / 180 * 3.141592654f);
218  xpos += float(sin(yrotrad))* 0.005f; ;zpos -= float(cos(yrotrad))* 0.005f; ypos -= float(sin(xrotrad))* 0.005f;}
219  if(keystate[SDLK_s]){yrotrad = (yrot / 180 * 3.141592654f); xrotrad = (xrot / 180 * 3.141592654f);
220  xpos -= float(sin(yrotrad))* 0.005f;zpos += float(cos(yrotrad))* 0.005f; ;ypos += float(sin(xrotrad))* 0.005f;}
221  if (keystate[SDLK_a]){yrotrad = (yrot / 180 * 3.141592654f);xpos -= float(cos(yrotrad)) * 0.008;zpos -= float(sin(yrotrad)) * 0.008; }
222  if (keystate[SDLK_d]){yrotrad = (yrot / 180 * 3.141592654f);xpos += float(cos(yrotrad)) * 0.008;zpos += float(sin(yrotrad)) * 0.008;}
223  if(keystate[SDLK_e]){zrot += 1 * 0.4f;if (zrot >360) zrot -= 360 * 0.4f;}
224  if(keystate[SDLK_c]){zrot -= 1 * 0.4f;if (zrot < -360) zrot += 360 * 0.4f;}
225 
226  if (keystate[SDLK_f]){ypos +=1 *0.005f;}
227  if (keystate[SDLK_v]){ypos -=1 *0.005f;}
228 
229  if(keystate[SDLK_1]){initViewpoint();}
230 
231  if (keystate[SDLK_5]){
232 
233  if ((odeinit._iCub->eyeLidRot) < 0.55) odeinit._iCub->eyeLidRot += 0.01;
234  cout<<odeinit._iCub->eyeLidRot<<endl;
235  }
236  if (keystate[SDLK_6]){
237  if ((odeinit._iCub->eyeLidRot) > 0.01) odeinit._iCub->eyeLidRot -= 0.01;
238  cout<<odeinit._iCub->eyeLidRot<<endl;
239  }
240  if (keystate[SDLK_h])
241  {
242  odeinit.sendHomePos();
243  }
244  /* Grab all the events off the queue. */
245  while (SDL_PollEvent(&event)){
246  switch (event.type)
247  {
248  case SDL_VIDEORESIZE:
249  width = event.resize.w;
250  height = event.resize.h;
251  SDL_SetVideoMode(width,height,16,SDL_OPENGL | SDL_RESIZABLE);
252  {
253  bool ok = setup_opengl(robot_config->getFinder());
254  if (!ok) {
255  odeinit.stop = true;
256  }
257  }
258  odeinit._iCub->reinitialized = true;
259  //draw_screen( );
260  break;
261  case SDL_KEYDOWN:
262  /* Handle key presses*/
263  handle_key_down(&event.key.keysym);
264  // SDL_GetKeyName(event.key.keysym.sym));
265  break;
266  break;
267  case SDL_MOUSEMOTION:
268  handle_mouse_motion(&event.motion);
269  mouse0_down_x = event.button.x;
270  mouse0_down_y = event.button.y;
271  break;
272  case SDL_QUIT:
273  /* Handle quit requests (like Ctrl-c). */
274  odeinit.stop = true;
275  break;
276 
277  case SDL_MOUSEBUTTONDOWN:
278  handle_mouse_motion(&event.motion);
279  switch (event.button.button)
280  {
281  case SDL_BUTTON_LEFT:
282  //deleteRay = false;
283  picking = false;
284  //printf(" Down\n");
285  break;
286  case SDL_BUTTON_MIDDLE:
287  break;
288  case SDL_BUTTON_RIGHT:
289  break;
290  default:
291  //this is not reached
292  break;
293  }
294  break;
295  break;
296  case SDL_MOUSEBUTTONUP:
297  switch (event.button.button)
298  {
299  case SDL_BUTTON_LEFT:
300  //printf(" up\n");
301  v=0;
302  break;
303  case SDL_BUTTON_MIDDLE:
304  //nothing
305  break;
306  case SDL_BUTTON_RIGHT:
307  //nothing
308  break;
309  default:
310  //this is not reached either
311  break;
312  }
313  break;
314  }
315  }
316 }
317 
318 void OdeSdlSimulation::nearCallback (void *data, dGeomID o1, dGeomID o2) {
319 
320  const double CONTACT_HEIGHT_TRESHOLD_METERS = 0.1; //for debugging or skin emulation purposes, assuming the robot is in contact with a flat ground (typically standing),
321  //the contacts generated between the robot and the ground that are always present can be ignored
322 
323  OdeInit& odeinit = OdeInit::get();
324 
325  assert(o1);
326  assert(o2);
327 
328  dSpaceID space1,space2;
329  dSpaceID superSpace1,superSpace2;
330  std::string geom1className("");
331  std::string geom2ClassName("");
332  std::string geom1name("");
333  std::string geom2name("");
334  bool geom1isiCubPart = false;
335  bool geom2isiCubPart = false;
336  bool geom1isTorsoOrArm = false;
337  bool geom2isTorsoOrArm = false;
338  int subLevel1;
339  //determine the indentation level for the printouts based on the sublevel in the hiearchy of spaces
340  string indentString("");
341  std::map<dGeomID,string>::iterator geom1namesIt;
342  std::map<dGeomID,string>::iterator geom2namesIt;
343 
344  if (dGeomIsSpace(o1)){
345  space1 = (dSpaceID)o1;
346  } else {
347  space1 = dGeomGetSpace(o1);
348  indentString = indentString + " --- "; //extra indentation level because it is a geom in that space
349  }
350  subLevel1 = dSpaceGetSublevel(space1);
351  for (int i=1;i<=subLevel1;i++){ //start from i=1, for sublevel==0 we don't add any indentation
352  indentString = indentString + " --- ";
353  }
354 
355  if (MY_VERBOSITY > 2) printf("%s nearCallback()\n",indentString.c_str());
356 
357  if (dGeomIsSpace(o1)){
358  space1 = (dSpaceID)o1;
359  if (MY_VERBOSITY > 2){
360  printf("%s Object nr. 1: %s, sublevel: %d, contained within: %s, nr. geoms: %d. \n",indentString.c_str(),odeinit._iCub->dSpaceNames[space1].c_str(),dSpaceGetSublevel(space1),odeinit._iCub->dSpaceNames[dGeomGetSpace(o1)].c_str(),dSpaceGetNumGeoms(space1));
361  }
362  }
363  else{ //it's a geom
364  getGeomClassName(dGeomGetClass(o1),geom1className);
365  superSpace1 = dGeomGetSpace(o1);
366  geom1namesIt = odeinit._iCub->dGeomNames.find(o1);
367  if (geom1namesIt != odeinit._iCub->dGeomNames.end()){
368  geom1name = geom1namesIt->second;
369  if (MY_VERBOSITY > 2) printf("%s Object nr. 1: geom: %s, class: %s, contained within %s (sublevel %d).\n",indentString.c_str(),geom1name.c_str(),geom1className.c_str(),odeinit._iCub->dSpaceNames[superSpace1].c_str(),dSpaceGetSublevel(superSpace1));
370  }
371  else{
372  if (MY_VERBOSITY > 2) printf("%s Object nr. 1: A geom, ID: %p, class: %s, contained within %s (sublevel %d).\n",indentString.c_str(),o1,geom1className.c_str(),odeinit._iCub->dSpaceNames[superSpace1].c_str(),dSpaceGetSublevel(superSpace1));
373  }
374  }
375 
376  if (dGeomIsSpace(o2)){
377  space2 = (dSpaceID)o2;
378  if (MY_VERBOSITY > 2){
379  printf("%s Object nr. 2: %s, sublevel: %d, contained within: %s, nr. geoms: %d. \n",indentString.c_str(),odeinit._iCub->dSpaceNames[space2].c_str(),dSpaceGetSublevel(space2),odeinit._iCub->dSpaceNames[dGeomGetSpace(o2)].c_str(),dSpaceGetNumGeoms(space2));
380  }
381  } else {
382  getGeomClassName(dGeomGetClass(o2),geom2ClassName);
383  superSpace2 = dGeomGetSpace(o2);
384  geom2namesIt = odeinit._iCub->dGeomNames.find(o2);
385  if (geom2namesIt != odeinit._iCub->dGeomNames.end()){
386  geom2name = geom2namesIt->second;
387  if (MY_VERBOSITY > 2) printf("%s Object nr. 2: geom: %s, class: %s, contained within %s (sublevel %d).\n",indentString.c_str(),geom2name.c_str(),geom2ClassName.c_str(),odeinit._iCub->dSpaceNames[superSpace2].c_str(),dSpaceGetSublevel(superSpace2));
388  }
389  else{
390  if (MY_VERBOSITY > 2) printf("%s Object nr. 2: A geom, ID: %p, class: %s, contained within %s (sublevel %d).\n",indentString.c_str(),o2,geom2ClassName.c_str(),odeinit._iCub->dSpaceNames[superSpace2].c_str(),dSpaceGetSublevel(superSpace2));
391  }
392  }
393 
394  // if at least one of the geoms is a space, we need to go deeper -> recursive calls
395  if (dGeomIsSpace(o1) || dGeomIsSpace(o2)){
396  if (dGeomIsSpace(o1) && dGeomIsSpace(o2)){ //if both are spaces, we exclude special combinations from the checking
397  if (((space1 == odeinit._iCub->iCubHeadSpace) && (space2 == odeinit._iCub->iCubTorsoSpace)) || ((space1 == odeinit._iCub->iCubTorsoSpace) && (space2 == odeinit._iCub->iCubHeadSpace))){
398  if (MY_VERBOSITY > 2) printf("%s Ignoring head vs. torso collision space checking.\n",indentString.c_str());
399  //these are unnecessary geoms to check, moreover 2 of these were colliding while not connected by a joint
400  }
401  else if (((space1 == odeinit._iCub->iCubLegsSpace) && (space2 == odeinit._iCub->iCubTorsoSpace)) || ((space1 == odeinit._iCub->iCubTorsoSpace) && (space2 == odeinit._iCub->iCubLegsSpace))){
402  if (MY_VERBOSITY > 2) printf("%s Ignoring legs vs. torso collision space checking.\n",indentString.c_str());
403  //these are unnecessary geoms to check - it always check collisions of geoms connected by a joint
404  }
405  else{
406  dSpaceCollide2(o1,o2,data,&nearCallback);
407  }
408  }
409  else{
410  dSpaceCollide2(o1,o2,data,&nearCallback);
411  }
412  //}
413  //if (dGeomIsSpace(o2)){
414  // dSpaceCollide2(o2,o1,data,&nearCallback); //start the recursion from the other end
415  //}
416  return;
417  }
418  /* Note we do not want to test intersections within a space,
419  * only between spaces. Therefore, we do not call dSpaceCollide ((dSpaceID)o1, data, &nearCallback) and the same for o2 */
420 
421  /* if we made it up to here, it means we have two geoms (not spaces) o1, o2 from two different spaces and we should handle their collision */
422 
423  dBodyID b1 = dGeomGetBody(o1);
424  dBodyID b2 = dGeomGetBody(o2);
425  if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)){
426  if (MY_VERBOSITY > 2) printf("%s Collision ignored: the bodies of o1 and o2 are connected by a joint.\n",indentString.c_str());
427  return;
428  }
429  // list of self-collisions to ignore
430  if (selfCollisionOnIgnoreList(geom1name,geom2name)){
431  if (MY_VERBOSITY > 2){
432  printf("%s geom: %s (class: %s, contained within %s) AND geom: %s (class: %s, contained within %s).\n",indentString.c_str(),geom1name.c_str(),geom1className.c_str(),odeinit._iCub->dSpaceNames[superSpace1].c_str(),geom2name.c_str(),geom2ClassName.c_str(),odeinit._iCub->dSpaceNames[superSpace2].c_str());
433  printf("%s Collision ignored (ignore list).\n",indentString.c_str());
434  }
435  return;
436  }
437 
438  if (MY_VERBOSITY > 2) printf("%s Collision candidate. Preparing contact joints.\n",indentString.c_str());
439  dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box
440  int i;
441  for (i=0; i<MAX_CONTACTS; i++) {
442  contact[i].surface.mode = dContactSlip1| dContactSlip2| dContactBounce | dContactSoftCFM;
443  contact[i].surface.mu = dInfinity;
444  contact[i].surface.mu2 = 0;
445  contact[i].surface.bounce = 0.01;
446  contact[i].surface.bounce_vel = 0.01;
447  contact[i].surface.slip1 = (dReal)0.000001;
448  contact[i].surface.slip2 = (dReal)0.000001;
449  contact[i].surface.soft_cfm = 0.0001;
450  }
451  int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,sizeof(dContact));
452  if (numc > 0){
453  if (MY_VERBOSITY > 2) printf("%s Collision suspect confirmed. There are %d contacts - creating joints.\n",indentString.c_str(),numc);
454  dMatrix3 RI;
455  dRSetIdentity (RI);
456  if(contact[0].geom.pos[1]>CONTACT_HEIGHT_TRESHOLD_METERS){ //non-foot contact
457  if (MY_VERBOSITY > 1){
458  printf("%s ****** non-ground COLLISION, %d contacts - creating joints************************************************************\n",indentString.c_str(),numc);
459  printf("%s geom: %s (%s, contained within %s) AND geom: %s (%s, contained within %s).\n",indentString.c_str(),geom1name.c_str(),geom1className.c_str(),odeinit._iCub->dSpaceNames[superSpace1].c_str(),geom2name.c_str(),geom2ClassName.c_str(),odeinit._iCub->dSpaceNames[superSpace2].c_str());
460  }
461  }
462  for (i=0; i<numc; i++) {
463  if (MY_VERBOSITY > 2) printf("%s Contact joint nr. %d (index:%d): at (%f,%f,%f), depth: %f \n",indentString.c_str(),i+1,i,contact[i].geom.pos[0],contact[i].geom.pos[1],contact[i].geom.pos[2],contact[i].geom.depth);
464  dJointID c = dJointCreateContact (odeinit.world,odeinit.contactgroup,contact+i);
465  dJointAttach (c,b1,b2);
466  // if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);
467  // check if the bodies are touch sensitive.
468  if (odeinit._iCub->actSkinEmul == "off"){ //this is the old implementation - hands (fingers and palm) are checked for touch
469  bool b1isTouchSensitive = isBodyTouchSensitive (b1);
470  bool b2isTouchSensitive = isBodyTouchSensitive (b2);
471  // if any of the bodies are touch sensitive...
472  if (b1isTouchSensitive || b2isTouchSensitive) {
473  // ... add a feedback structure to the contact joint.
474  if (MY_VERBOSITY > 2) printf("%s Adding tactile feedback for emulating finger/palm skin to this one (ODE joint feedback counter: %d).\n",indentString.c_str(),nFeedbackStructs);
475  dJointSetFeedback (c, &(touchSensorFeedbacks[nFeedbackStructs]));
477  }
478  }
479  else { //whole_body_skin_emul ~ actSkinEmul is on
480  /* here we treat all bodies belonging to the icub as touch sensitive
481  * we want to know if the geom is part of the iCub - that is its superSpace is one of the iCub subspaces*/
482 
483  if ((superSpace1 == odeinit._iCub->iCubHeadSpace) || (superSpace1 == odeinit._iCub->iCubLegsSpace)){
484  geom1isiCubPart = true;
485  }
486  else if ((superSpace1==odeinit._iCub->iCubTorsoSpace) || (superSpace1==odeinit._iCub->iCubLeftArmSpace) || (superSpace1== odeinit._iCub->iCubRightArmSpace)){
487  geom1isiCubPart = true;
488  geom1isTorsoOrArm = true;
489  }
490  // || (superSpace1 == iCub){ - this should never happen here - in the self-collision mode, the iCub space contains only subspaces - no geoms directly
491 
492  if ((superSpace2 == odeinit._iCub->iCubHeadSpace) || (superSpace2 == odeinit._iCub->iCubLegsSpace)){
493  geom2isiCubPart = true;
494  }
495  else if ((superSpace2==odeinit._iCub->iCubTorsoSpace) || (superSpace2==odeinit._iCub->iCubLeftArmSpace) || (superSpace2== odeinit._iCub->iCubRightArmSpace)){
496  geom2isiCubPart = true;
497  geom2isTorsoOrArm = true;
498  }
499 
500  // if (geom1isiCubPart || geom2isiCubPart){ //we don't have the legs and head implemented yet - these don't have skin in the real robot - but legs will -> should do that
501  if ( geom1isTorsoOrArm || geom2isTorsoOrArm){
502  if (MY_VERBOSITY > 2) printf("%s Adding tactile feedback for whole-body skinContact to this contact (ODE joint feedback counter: %d).\n",indentString.c_str(),nFeedbackStructs);
504  printf("Warning: out of contact joint feedback structures for ODE (exceeded %d) - some contact joints will not have info about forces stored\n.",MAX_DJOINT_FEEDBACKSTRUCTS);
505  }
506  else{
507  dJointSetFeedback (c, &(touchSensorFeedbacks[nFeedbackStructs]));
508  nFeedbackStructs++;
509  }
510  OdeInit::contactOnSkin_t contactOnSkin, contactOnSkin2;
511  if (geom1isiCubPart){
512  contactOnSkin.body_geom_space_id = superSpace1;
513  contactOnSkin.body_geom_id = o1;
514  contactOnSkin.body_index = 1;
515  contactOnSkin.contact_geom = contact[i].geom;
516  contactOnSkin.contact_joint = c;
517  odeinit.listOfSkinContactInfos.push_back(contactOnSkin);
518  }
519  if (geom2isiCubPart){
520  contactOnSkin2.body_geom_space_id = superSpace2;
521  contactOnSkin2.body_geom_id = o2;
522  contactOnSkin2.body_index = 2;
523  contactOnSkin2.contact_geom = contact[i].geom;
524  contactOnSkin2.contact_joint = c;
525  odeinit.listOfSkinContactInfos.push_back(contactOnSkin2);
526  }
527  }
528  else {
529  if (MY_VERBOSITY > 2) printf("%s Ignoring skin contact - so far only arms and torso are implemented.\n",indentString.c_str());
530  }
531  } //whole_body_skin_emul ~ actSkinEmul is on
532  if (MY_VERBOSITY > 2) printf("\n");
533  } // for numc - contacts
534  } // if (numc > 0)
535  else{
536  if (MY_VERBOSITY > 2) printf("%s Collision suspect NOT confirmed. There were %d contacts.\n",indentString.c_str(),numc);
537  }
538 }
539 
540 
541 bool OdeSdlSimulation::selfCollisionOnIgnoreList(string geom1_string, string geom2_string)
542 {
543  if ( ( (geom1_string.compare("upper right arm cover")==0) && (geom2_string.compare("torsoGeom[5]")==0) ) || ( (geom2_string.compare("upper right arm cover")==0) && (geom1_string.compare("torsoGeom[5]")==0) ) ){
544  return true;
545  }
546  if ( ( (geom1_string.compare("upper right arm cover")==0) && (geom2_string.compare("torso cover")==0) ) || ( (geom2_string.compare("upper right arm cover")==0) && (geom1_string.compare("torso cover")==0) ) ){
547  return true;
548  }
549 
550  if ( ( (geom1_string.compare("upper left arm cover")==0) && (geom2_string.compare("torsoGeom[4]")==0) ) || ( (geom2_string.compare("upper left arm cover")==0) && (geom1_string.compare("torsoGeom[4]")==0) ) ){
551  return true;
552  }
553  if ( ( (geom1_string.compare("upper left arm cover")==0) && (geom2_string.compare("torso cover")==0) ) || ( (geom2_string.compare("upper left arm cover")==0) && (geom1_string.compare("torso cover")==0) ) ){
554  return true;
555  }
556 
557  return false;
558 }
559 
560 // returns true if the body with the bodyID is a touch-sensitive body, returns false otherwise.
561 bool OdeSdlSimulation::isBodyTouchSensitive (dBodyID bodyID) {
562  OdeInit& odeinit = OdeInit::get();
563 
564  // check the smaller hand parts if the left hand is active.
565  if (odeinit._iCub->actLHand == "on") {
566  if (bodyID == odeinit._iCub->body[10]) {
567  return true;
568  } else if (bodyID == odeinit._iCub->body[30]) {
569  return true;
570  } else if (bodyID == odeinit._iCub->body[24]) {
571  return true;
572  } else if (bodyID == odeinit._iCub->body[25]) {
573  return true;
574  } else if (bodyID == odeinit._iCub->lhandfingers3) {
575  return true;
576  }
577  } else { // check the whole left hand body if the hand is not active.
578  if (bodyID == odeinit._iCub->l_hand) {
579  return true;
580  }
581  }
582 
583  // check the smaller hand parts if the right hand is active.
584  if (odeinit._iCub->actRHand == "on") {
585  if (bodyID == odeinit._iCub->body[11]) {
586  return true;
587  } else if (bodyID == odeinit._iCub->body[49]) {
588  return true;
589  } else if (bodyID == odeinit._iCub->body[43]) {
590  return true;
591  } else if (bodyID == odeinit._iCub->body[44]) {
592  return true;
593  } else if (bodyID == odeinit._iCub->rhandfingers3) {
594  return true;
595  }
596  } else { // check the whole right hand body if the hand is not active.
597  if (bodyID == odeinit._iCub->r_hand) {
598  return true;
599  }
600  }
601 
602  return false;
603 }
604 
605 // this is a function to mimic the sensor data from the physical icub fingertip/palm sensors
606 //but the palm cover is not being checked here
607 void OdeSdlSimulation::inspectHandTouch_icubSensors(Bottle& reportLeft, Bottle& reportRight, bool boolean) {
608  OdeInit& odeinit = OdeInit::get();
609  reportLeft.clear();
610  reportRight.clear();
611  int indicesLeft[6] = {24, 25, 26, 27, 30, 10};
612  int indicesRight[6] = {43, 44, 45, 46, 49, 11};
613 
614  if (odeinit._iCub->actLHand == "on" && odeinit._iCub->actRHand == "on" ){
615  double resultLeft=0, resultRight = 0;
616  for (int x = 0; x < 6; x++){
617  if (boolean){
618  resultLeft = odeinit._iCub->checkTouchSensor( indicesLeft[x] );
619  resultRight = odeinit._iCub->checkTouchSensor( indicesRight[x] );
620  }
621  else{
622  resultLeft = odeinit._iCub->checkTouchSensor_continuousValued( indicesLeft[x] );
623  resultRight = odeinit._iCub->checkTouchSensor_continuousValued( indicesRight[x] );
624  }
625 
626  if (x < 5){ //five fingers
627  for (int i = 0; i < 12; i++){
628  reportLeft.addDouble(resultLeft * 255);
629  reportRight.addDouble(resultRight * 255);
630  }
631  }
632  if (x == 5){
633  for (int y = 0; y<3; y++){
634  for (int i = 0; i < 12; i++){
635  reportLeft.addDouble(0.0);
636  reportRight.addDouble(0.0);
637  }
638  }
639  //these are palm taxels
640  for (int y = 0; y<4; y++){
641  for (int i = 0; i < 12; i++){
642  reportLeft.addDouble(resultLeft * 255);
643  reportRight.addDouble(resultRight * 255);
644  }
645  }
646  for (int y = 0; y<4; y++){
647  for (int i = 0; i < 12; i++){
648  reportLeft.addDouble(0.0);
649  reportRight.addDouble(0.0);
650  }
651  }
652  }
653  }
654  }//end lhand on rhand on
655  else if (odeinit._iCub->actLHand == "on" && odeinit._iCub->actRHand == "off" ){
656  double resultLeft=0, resultRight = 0;
657  for (int x = 0; x < 6; x++){
658  if (boolean){
659  resultLeft = odeinit._iCub->checkTouchSensor( indicesLeft[x] );
660  resultRight = odeinit._iCub->checkTouchSensor( odeinit._iCub->r_hand );
661  }
662  else{
663  resultLeft = odeinit._iCub->checkTouchSensor_continuousValued( indicesLeft[x] );
664  resultRight = odeinit._iCub->checkTouchSensor_continuousValued(odeinit._iCub->r_hand);
665  }
666  if (x < 5){
667  for (int i = 0; i < 12; i++){
668  reportLeft.addDouble(resultLeft * 255);
669  reportRight.addDouble(resultRight * 255);
670  }
671  }
672  if (x == 5){
673  for (int y = 0; y<3; y++){
674  for (int i = 0; i < 12; i++){
675  reportLeft.addDouble(0.0);
676  reportRight.addDouble(0.0);
677  }
678  }
679  for (int y = 0; y<4; y++){
680  for (int i = 0; i < 12; i++){
681  reportLeft.addDouble(resultLeft * 255);
682  reportRight.addDouble(resultRight * 255);
683  }
684  }
685  for (int y = 0; y<4; y++){
686  for (int i = 0; i < 12; i++){
687  reportLeft.addDouble(0.0);
688  reportRight.addDouble(0.0);
689  }
690  }
691  }
692  }
693  }//end lhand on rhand off
694  else if (odeinit._iCub->actRHand == "on" && odeinit._iCub->actLHand == "off" ){
695  double resultLeft=0, resultRight = 0;
696  for (int x = 0; x < 6; x++){
697  if (boolean){
698  resultLeft = odeinit._iCub->checkTouchSensor( odeinit._iCub->l_hand );
699  resultRight = odeinit._iCub->checkTouchSensor( indicesRight[x] );
700  }
701  else{
702  resultLeft = odeinit._iCub->checkTouchSensor_continuousValued( odeinit._iCub->l_hand );
703  resultRight = odeinit._iCub->checkTouchSensor_continuousValued( indicesRight[x] );
704  }
705 
706  if (x < 5){
707  for (int i = 0; i < 12; i++){
708  reportLeft.addDouble(resultLeft * 255);
709  reportRight.addDouble(resultRight * 255);
710  }
711  }
712  if (x == 5){
713  for (int y = 0; y<3; y++){
714  for (int i = 0; i < 12; i++){
715  reportLeft.addDouble(0.0);
716  reportRight.addDouble(0.0);
717  }
718  }
719  for (int y = 0; y<4; y++){
720  for (int i = 0; i < 12; i++){
721  reportLeft.addDouble(resultLeft * 255);
722  reportRight.addDouble(resultRight * 255);
723  }
724  }
725  for (int y = 0; y<4; y++){
726  for (int i = 0; i < 12; i++){
727  reportLeft.addDouble(0.0);
728  reportRight.addDouble(0.0);
729  }
730  }
731  }
732  }
733  }//end lhand off rhand on
734  else{//both off
735  for (int x = 0; x < 6; x++){
736  double resultLeft=0, resultRight = 0;
737  if (boolean){
738  resultLeft = odeinit._iCub->checkTouchSensor( odeinit._iCub->l_hand );
739  resultRight = odeinit._iCub->checkTouchSensor( odeinit._iCub->r_hand );
740  }
741  else{
742  resultLeft = odeinit._iCub->checkTouchSensor_continuousValued( odeinit._iCub->l_hand );
743  resultRight = odeinit._iCub->checkTouchSensor_continuousValued(odeinit._iCub->r_hand);
744  }
745 
746  if (x < 5){
747  for (int i = 0; i < 12; i++){
748  reportLeft.addDouble(resultLeft * 255);
749  reportRight.addDouble(resultRight * 255);
750  }
751  }
752  if (x == 5){
753  for (int y = 0; y<3; y++){
754  for (int i = 0; i < 12; i++){
755  reportLeft.addDouble(0.0);
756  reportRight.addDouble(0.0);
757  }
758  }
759  for (int y = 0; y<4; y++){
760  for (int i = 0; i < 12; i++){
761  reportLeft.addDouble(resultLeft * 255);
762  reportRight.addDouble(resultRight * 255);
763  }
764  }
765  for (int y = 0; y<4; y++){
766  for (int i = 0; i < 12; i++){
767  reportLeft.addDouble(0.0);
768  reportRight.addDouble(0.0);
769  }
770  }
771  }
772  }
773  }//end both off
774 }
775 
776 
777 void OdeSdlSimulation::getAngles(const dReal *m, float& z, float& y, float& x) {
778  const dReal eps = 0.00001;
779 
780  y = -asin(m[2]);
781  float c = cos(y);
782 
783  if (fabs(c)>eps) {
784  x = atan2(-m[6]/c,m[10]/c);
785  z = atan2(-m[1]/c,m[0]/c);
786  } else {
787  x = 0;
788  z = -atan2(m[4],m[5]);
789  }
790  x *= -180/M_PI;
791  y *= 180/M_PI;
792  z *= 180/M_PI;
793 }
794 
795 void OdeSdlSimulation::initViewpoint() {
796  xpos = 0;
797  ypos = 1;
798  zpos = 1;
799  xrot = 25;
800  yrot = 0;
801  zrot = 0;
802 }
803 
804 void OdeSdlSimulation::mouseMovement(float x, float y) {
805  float diffx = x-lastx; //check the difference between the current x and the last x position
806  float diffy = y-lasty; //check the difference between the current y and the last y position
807  lastx =x; //set lastx to the current x position
808  lasty =y; //set lasty to the current y position
809  xrot += (float) diffy; //set the xrot to xrot with the addition of the difference in the y position
810  yrot += (float) diffx; //set the xrot to yrot with the addition of the difference in the x position
811 }
812 
813 void OdeSdlSimulation::draw_screen() {
814  OdeInit& odeinit = OdeInit::get();
815 
816  glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT); // refresh opengl
817 
818  if (extractImages || odeinit._iCub->actVision == "on"){
820  }
821 
822  glViewport(0,0,width,height);
823  glMatrixMode (GL_PROJECTION);
824  glLoadIdentity();
825  gluPerspective( 75, (float)width/height, 0.01, 100.0 );
826  glMatrixMode (GL_MODELVIEW);
827  glLoadIdentity();
828  glLightfv(GL_LIGHT0, GL_POSITION, light_position);
829  glRotatef (xrot, 1,0,0);
830  glRotatef (yrot, 0,1,0);
831  glRotatef (zrot, 0,0,1);
832  glTranslated(-xpos,-ypos,-zpos);
833 
834  // set up any video textures
835 
836  if (video!=0)
837  DrawVideo(video);
838 
839  //draw the ground
840  glColor3d(0.5,0.5,1);
841  glPushMatrix();
842  glRotatef(90.0,1,0,0);
843  glRotatef(180.0,0,1,0);
844  DrawGround(false);
845  glPopMatrix();
846  glDisable(GL_TEXTURE_2D);
847  draw();
848  glEnable(GL_TEXTURE_2D);
849  drawSkyDome(0,0,0,50,50,50); // Draw the Skybox
850  SDL_GL_SwapBuffers();// Swap Buffers
851 }
852 
853 
854 
855 void OdeSdlSimulation::retreiveInertialData(Bottle& inertialReport) {
856  OdeInit& odeinit = OdeInit::get();
857  static dReal OldLinearVel[3], LinearVel[3], LinearAccel[3];
858  inertialReport.clear();
859 
860  //get euler angles from quaternions
861  dQuaternion angles;
862  dGeomGetQuaternion( odeinit._iCub->inertialGeom, angles );
863  dReal w, x, y, z;
864  w = angles[0];
865  x = angles[1];
866  y = angles[2];
867  z = angles[3];
868 
869  double sqw = w * w;
870  double sqx = x * x;
871  double sqy = y * y;
872  double sqz = z * z;
873  float roll, pitch, yaw;
874 
875  double unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
876  double test = x*y + z*w;
877  if (test > 0.499*unit) { // singularity at north pole
878  roll = 2 * atan2(x,w);
879  pitch = M_PI/2;
880  yaw = 0;
881  return;
882  }
883  if (test < -0.499*unit) { // singularity at south pole
884  roll = -2 * atan2(x,w);
885  pitch = -M_PI/2;
886  yaw = 0;
887  return;
888  }
889  roll =(float) ( atan2(2.0*y*w-2*x*z , sqx - sqy - sqz + sqw) ); //z
890  pitch = (float) (atan2(2.0*x*w-2*y*z , -sqx + sqy - sqz + sqw) );//x
891  yaw = asin(2*test/unit);//y
892 
893  //roll = dBodyGetRotation(odeinit._iCub->head)[4]; // was 1
894  //pitch = dBodyGetRotation(odeinit._iCub->head)[6];
895  //yaw = dBodyGetRotation(odeinit._iCub->head)[2];
896 
897  //Add Euler angles roll pitch yaw
898  inertialReport.addDouble( -yaw * 180/M_PI);// yaw
899  inertialReport.addDouble( -pitch * 180/M_PI);// pitch
900  inertialReport.addDouble( roll * 180/M_PI);// roll
901 
902  /*//in order to calculate linear acceleration (make sure of body) Inertial Measurement Unit IMU
903  LinearVel[0] = dBodyGetLinearVel(odeinit._iCub->inertialBody)[0];
904  LinearVel[1] = dBodyGetLinearVel(odeinit._iCub->inertialBody)[1];
905  LinearVel[2] = dBodyGetLinearVel(odeinit._iCub->inertialBody)[2];
907  LinearAccel[0] = ( LinearVel[0] - OldLinearVel[0] ) / 0.02;
908  LinearAccel[1] = ( LinearVel[1] - OldLinearVel[1] ) / 0.02;
909  LinearAccel[2] = ( LinearVel[2] - OldLinearVel[2] ) / 0.02;
910  OldLinearVel[0] = LinearVel[0];
911  OldLinearVel[1] = LinearVel[1];
912  OldLinearVel[2] = LinearVel[2];*/
913 
915  Vector grav,grav1,grav2,grav3;
916  grav.resize(3);
917  grav1.resize(3);
918  grav2.resize(3);
919  grav3.resize(3);
920  double theta;
921 
922  grav[0]=0;
923  grav[1]=0;
924  grav[2]=9.81;
925 
926  theta = pitch;
927  grav1[0]=grav[0]*cos(theta)+grav[2]*sin(theta);
928  grav1[1]=grav[1];
929  grav1[2]=grav[0]*(-sin(theta))+grav[2]*cos(theta);
930 
931  theta = yaw;
932  grav2[0]=grav1[0];
933  grav2[1]=grav1[1]*cos(theta)+grav1[2]*(-sin(theta));
934  grav2[2]=grav1[1]*sin(theta)+grav1[2]*cos(theta);
935 
936  theta = roll;
937  grav3[0]=grav2[0]*cos(theta)+grav2[1]*(-sin(theta));
938  grav3[1]=grav2[0]*sin(theta)+grav2[1]*cos(theta);
939  grav3[2]=grav2[2];
940 
941  inertialReport.addDouble( grav3[0] );
942  inertialReport.addDouble( grav3[1] );
943  inertialReport.addDouble( grav3[2] );
944 
945  //Add angular velocity
946  inertialReport.addDouble(-dBodyGetAngularVel(odeinit._iCub->inertialBody)[2]*CTRL_RAD2DEG);
947  inertialReport.addDouble(-dBodyGetAngularVel(odeinit._iCub->inertialBody)[0]*CTRL_RAD2DEG);
948  inertialReport.addDouble( dBodyGetAngularVel(odeinit._iCub->inertialBody)[1]*CTRL_RAD2DEG);
949 
950  //Add magnetic fields
951  inertialReport.addDouble(0.0);
952  inertialReport.addDouble(0.0);
953  inertialReport.addDouble(0.0);
954 }
955 
956 int OdeSdlSimulation::thread_ode(void *unused) {
957  //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
958  double cpms = 1e3 / CLOCKS_PER_SEC;
959  long lastOdeProcess = (long) (clock()*cpms);
960  double avg_ode_step_length = 0.0;
961  long count = 0;
962  simrun = true;
963  double timeCache = ode_step_length;
964  long lastTimeCacheUpdate = (long) (clock()*cpms);
965  double alpha = 0.99;
966  // if realTime=true when delays occur the simulation tries to recover by running more steps in a row
967  // if realTime=false the simulation executes the simulation steps with a fixed rate irregardless of delays
968  bool realTime = true;
969  long temp;
970 
971  while (simrun) {
972  temp = (long) (clock()*cpms);
973  timeCache += temp - lastTimeCacheUpdate;
974  lastTimeCacheUpdate = temp;
975  while(timeCache < ode_step_length){
976  SDL_Delay((unsigned int)(ode_step_length-timeCache));
977  temp = (long) (clock()*cpms);
978  timeCache += temp - lastTimeCacheUpdate;
979  lastTimeCacheUpdate = temp;
980  }
981 
982  /*if(timeCache >= 2.0*ode_step_length)
983  printf("Simulation delay: running %d steps in a row to recover.\n", (int)(timeCache/ode_step_length));*/
984 
985  while(timeCache >= ode_step_length){
986  count++;
987  lastOdeProcess = (long) (clock()*cpms);
988  ODE_process(1, (void*)1);
989  avg_ode_step_length = alpha*avg_ode_step_length + (1.0-alpha)*((long) (clock()*cpms) -lastOdeProcess);
990 
991  if(realTime)
992  timeCache -= ode_step_length;
993  else
994  timeCache = 0.0;
995 
996  // check if the desired timestep is achieved, if not, print a warning msg
997  if(count % (10000/ode_step_length)==0){
998  if(avg_ode_step_length >= ode_step_length+1)
999  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",
1000  ode_step_length, avg_ode_step_length);
1001  else if(avg_ode_step_length <= ode_step_length-1)
1002  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",
1003  ode_step_length, avg_ode_step_length);
1004  }
1005  }
1006  }
1007  return(0);
1008 }
1009 
1010 Uint32 OdeSdlSimulation::ODE_process(Uint32 interval, void *param) {
1011  OdeInit& odeinit = OdeInit::get();
1012  //static clock_t startTimeODE= clock(), finishTimeODE= clock();
1013  //startTimeODE = clock();
1014 
1015  odeinit.mutex.wait();
1016  nFeedbackStructs=0;
1017 
1018  if (MY_VERBOSITY > 2) printf("\n ***info code collision detection ***\n");
1019  if (MY_VERBOSITY > 2) printf("OdeSdlSimulation::ODE_process: dSpaceCollide(odeinit.space,0,&nearCallback): will test iCub space against the rest of the world (e.g. ground).\n");
1020  dSpaceCollide(odeinit.space,0,&nearCallback); //determines which pairs of geoms in a space may potentially intersect, and calls a callback function with each candidate pair
1021  if (odeinit._iCub->actSelfCol == "on"){
1023  if (MY_VERBOSITY > 2){
1024  printf("OdeSdlSimulation::ODE_process: dSpaceCollide(odeinit._iCub->iCub,0,&nearCallback): will test iCub subspaces against each other.\n");
1025  }
1026  dSpaceCollide(odeinit._iCub->iCub,0,&nearCallback); //determines which pairs of geoms in a space may potentially intersect, and calls a callback function with each candidate pair
1027  }
1028  }
1029  if (MY_VERBOSITY > 2) printf("***END OF info code collision detection ***\n");
1030 
1031  dWorldStep(odeinit.world, dstep);
1032  // do 1 TIMESTEP in controllers (ok to run at same rate as ODE: 1 iteration takes about 300 times less computation time than dWorldStep)
1033  for (int ipart = 0; ipart<MAX_PART; ipart++) {
1034  if (odeinit._controls[ipart] != NULL) {
1035  odeinit._controls[ipart]->jointStep();
1036  }
1037  }
1038  odeinit.sync = true;
1039  odeinit.mutex.post();
1040 
1041  if (odeinit._iCub->actSkinEmul == "off"){
1043  Bottle reportLeft;
1044  Bottle reportRight;
1045  bool boolean = true;
1046  if (odeinit._iCub->actPressure == "on"){
1047  boolean = false;
1048  }
1049  inspectHandTouch_icubSensors(reportLeft, reportRight, boolean);//inspectBodyTouch_continuousValued(report);
1050 
1052  robot_streamer->sendTouchLeftHand( reportLeft );
1053 
1055  robot_streamer->sendTouchRightHand( reportRight );
1056  }
1057  }
1058  else{ // actSkinEmul == "on"
1063  //these are emulating the skin in the covers on the iCub body. For the moment, whole skin part will be activated if it was touched
1064  inspectWholeBodyContactsAndSendTouch();
1065  }
1066  if(robot_streamer->shouldSendSkinEvents()){ //note that these are generated here for any body parts - not only those that have tactile sensors in the real robot
1067  // the contacts can be visualized using the icubGui (not skinGui)
1068  skinContactList mySkinContactList;
1069  if (! odeinit.listOfSkinContactInfos.empty()){
1070  if ((odeinit._iCub->actHead=="off") || (odeinit._iCub->actTorso=="off") || (odeinit._iCub->actLArm=="off") || (odeinit._iCub->actRArm=="off")){
1071  mySkinContactList.clear();
1072  printf("Warning: With self-collisions on but head/torso/left_arm/right_arm off, the skinContactList can't be created.\n");
1073  }
1074  else{
1075  processWholeBodyCollisions(mySkinContactList);
1076  }
1077  }
1078  robot_streamer->sendSkinEvents(mySkinContactList); //we send even if empty
1079  }
1080  odeinit.listOfSkinContactInfos.clear();
1081  }
1082 
1083  dJointGroupEmpty (odeinit.contactgroup);
1084 
1086  Bottle inertialReport;
1087  retreiveInertialData(inertialReport);
1088  robot_streamer->sendInertial(inertialReport);
1089  }
1090 
1091  //go and check if torques are needed
1093 
1094  odeinit._iCub->setJointControlAction();
1095 
1096  //finishTimeODE = clock() ;
1097  //SPS();
1098  //printf("ODE=%lf\n",(double)(finishTimeODE - startTimeODE) / CLOCKS_PER_SEC);
1099  return(interval);
1100 }
1101 
1102 
1103 /*int OdeSdlSimulation::thread_func(void *unused) {
1104  // this needs to be kept synchronized with the timestep in
1105  // dWorldStep, in order to get correct world clock time
1106  // --paulfitz
1107  int delay = 50;
1108  id = SDL_AddTimer( delay, &OdeSdlSimulation::ODE_process, (void*)1);
1109 
1110  return(0);
1111 }
1112 */
1113 /*
1114  static void SPS()
1115  {
1116  static float sps = 0.0f;
1117  static float previousTime = 0.0f;
1118  static int currentsps;
1119  static char strSPS[60] = {0};
1120 
1121  float currentTime = (GetTickCount() * 0.001f);
1122 
1123  ++sps; // Increment the SPS counter
1124 
1125  if( currentTime - previousTime > 1.0f ){
1126  previousTime = currentTime;
1127  currentsps = int(sps);
1128  printf("current SPS: %d\n",currentsps);
1129  sps = 0.0f;
1130  }
1131  }
1132 */
1133 
1134 void OdeSdlSimulation::sighandler(int sig) {
1135  OdeInit& odeinit = OdeInit::get();
1136  odeinit.stop = true;
1137  cout << "\nCAUGHT Ctrl-c" << endl;
1138 }
1139 
1140 void OdeSdlSimulation::simLoop(int h,int w) {
1141  fprintf(stdout, "***** OdeSdlSimulation::simLoop \n");
1142  OdeInit& odeinit = OdeInit::get();
1143 
1144  SDL_Init(SDL_INIT_TIMER | SDL_GL_ACCELERATED_VISUAL);
1145  SDL_SetVideoMode(h,w,32,SDL_OPENGL | SDL_RESIZABLE);// | SDL_SWSURFACE| SDL_ANYFORMAT); // on init
1146 
1147  dAllocateODEDataForThread(dAllocateMaskAll);
1148  ConstString logo = robot_config->getFinder().findFile("logo");
1149 
1150  image = SDL_LoadBMP(robot_config->getFinder().findFile(logo.c_str()).c_str());
1151  SDL_WM_SetIcon(image,0);
1152  SDL_FreeSurface(image);
1153  SDL_WM_SetCaption("iCub Simulator", "image");
1154 
1155  //SDL_Thread *thread;
1156  SDL_Thread *ode_thread = SDL_CreateThread(thread_ode, NULL);
1157  //thread = SDL_CreateThread(thread_func, NULL);
1158 
1159  if ( ode_thread == NULL ) {
1160  yError("Unable to create thread: %s\n", SDL_GetError());
1161  return;
1162  }
1163 
1164  initViewpoint();
1165  bool ok = setup_opengl(robot_config->getFinder());
1166  if (!ok) return;
1167  startTime = (long) clock();
1168  odeinit.stop = false;
1169 
1170  yarp::os::signal(yarp::os::YARP_SIGINT, sighandler);
1171  yarp::os::signal(yarp::os::YARP_SIGTERM, sighandler);
1172 
1173  glrun = true;
1174  odeinit._wrld->WAITLOADING = false;
1175  odeinit._wrld->static_model = false;
1176  long prevTime = (long) clock();
1177  long timeLeft;
1178 
1179  if (odeinit._iCub->actStartHomePos == "on"){
1180  odeinit.sendHomePos();
1181  }
1182  if (odeinit._iCub->actSelfCol == "on") {
1183  if (odeinit._iCub->actStartHomePos == "on"){
1184  Time::delay(2.0); //we want to set this trigger on only after the robot is in home pos -
1185  //it's initial configuration is with arms inside the thighs - generating many self-collisions
1187  }
1188  else{
1189  fprintf(stdout, "Warning: the robot is not starting from HomePos and self-collision mode is on. The initial posture is already self-colliding.\n");
1191  }
1192  }
1193 
1194  while(!odeinit.stop) {
1195  /* Process incoming events. */
1196  process_events();
1197  /* Draw the screen. */
1198  if ( !odeinit._wrld->WAITLOADING ){
1199  if (glrun) {
1200  odeinit.mutexTexture.wait();
1201  draw_screen();
1202  odeinit.mutexTexture.post();
1203  // check for framerate
1204  timeLeft = (prevTime - (long) clock()) + gl_frame_length;
1205  //cout << "check for framerate " << timeLeft << endl;
1206  if (timeLeft > 0)
1207  { // if there is still time left in this frame, just wait
1208  SDL_Delay(timeLeft);
1209  }
1210  prevTime = (long) clock();
1211  } else {
1212  SDL_Delay(100);
1213  }
1214  }
1215  else{
1216  glFinish();
1217  glFlush();
1218  //make sure it can also be done for static objects
1219  if (odeinit._wrld->static_model){
1220  odeinit._wrld->loadTexture(odeinit._wrld->texture, odeinit._wrld->s_modelTexture[odeinit._wrld->s_MODEL_NUM-1]);
1221  }else{
1222  odeinit._wrld->loadTexture(odeinit._wrld->texture, odeinit._wrld->modelTexture[odeinit._wrld->MODEL_NUM-1]);
1223  }
1224  odeinit._wrld->WAITLOADING = false;
1225  odeinit._wrld->static_model = false;
1226  }
1227  }
1228  printf("\n\nStopping SDL and ODE threads...\n");
1229  //stop the timer
1230  //SDL_RemoveTimer(id);
1231  //Stop the thread
1232  //SDL_KillThread( thread );
1233  simrun = false;
1234  //SDL_WaitThread( thread, NULL );
1235  SDL_WaitThread( ode_thread, NULL );
1236  //SDL_Quit();
1237 }
1238 
1239 void OdeSdlSimulation::drawView(bool left, bool right, bool wide) {
1240  OdeInit& odeinit = OdeInit::get();
1241  const dReal *pos;
1242  const dReal *rot;
1243  glViewport(0,0,cameraSizeWidth,cameraSizeHeight);
1244  glMatrixMode (GL_PROJECTION);
1245 
1246  if (left){
1247  glLoadIdentity();
1248  gluPerspective( fov_left, (float) width_left/height_left, 0.04, 100.0 );
1249  pos = dGeomGetPosition(odeinit._iCub->Leye1_geom);
1250  rot = dGeomGetRotation(odeinit._iCub->Leye1_geom);
1251  glMatrixMode (GL_MODELVIEW);
1252  glLoadIdentity();
1253  glLightfv(GL_LIGHT0, GL_POSITION, light_position);
1254  gluLookAt(
1255  pos[0],
1256  pos[1],
1257  pos[2],
1258  pos[0] + rot[2],
1259  pos[1] + rot[6],
1260  pos[2] + rot[10],
1261  -rot[4], 1, 0
1262  );
1263  }
1264  if (right){
1265  glLoadIdentity();
1266  gluPerspective( fov_right, (float) width_right/height_right, 0.04, 100.0 );//55.8
1267  pos = dGeomGetPosition(odeinit._iCub->Reye1_geom);
1268  rot = dGeomGetRotation(odeinit._iCub->Reye1_geom);
1269  glMatrixMode (GL_MODELVIEW);
1270  glLoadIdentity();
1271  glLightfv(GL_LIGHT0, GL_POSITION, light_position);
1272  gluLookAt(
1273  pos[0],
1274  pos[1],
1275  pos[2],
1276  pos[0] + rot[2],
1277  pos[1] + rot[6],
1278  pos[2] + rot[10],
1279  -rot[4], 1, 0
1280  );
1281  }
1282  if (wide){
1283  glLoadIdentity();
1284  gluPerspective( 55.8, (float) cameraSizeWidth/cameraSizeHeight, 0.04, 100.0 );//here nothing to do with cameras
1285  glMatrixMode (GL_MODELVIEW);
1286  glLoadIdentity();
1287  glLightfv(GL_LIGHT0, GL_POSITION, light_position);
1288  glRotatef (xrot, 1,0,0);
1289  glRotatef (yrot, 0,1,0);
1290  glTranslated(-xpos,-ypos,-zpos);
1291  }
1292 
1293  //draw the ground
1294  glColor3d(0.5,0.5,1);
1295  glEnable(GL_TEXTURE_2D);
1296  glPushMatrix();
1297  glRotatef(90.0,1,0,0);
1298  glRotatef(180.0,0,1,0);
1299  DrawGround(false);
1300  glPopMatrix();
1301  glDisable(GL_TEXTURE_2D);
1302  draw();//robot
1303  glEnable(GL_TEXTURE_2D);
1304  drawSkyDome(0,0,0,50,50,50); // Draw the Skybox
1305 }
1306 
1308  glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT); // refresh opengl
1309 }
1310 
1312 }
1313 
1315  RobotConfig *config) {
1316  OdeInit& odeinit = OdeInit::get();
1317  if (video!=NULL) {
1318  fprintf(stderr, "Only one Simulation object allowed\n");
1319  yarp::os::exit(1);
1320  }
1321  robot_streamer = streamer;
1322  robot_config = config;
1323 
1324  ode_step_length = config->getWorldTimestep();
1325  dstep = ode_step_length*1e-3;
1326 
1327  video = new VideoTexture;
1328  string moduleName = odeinit.getName();
1329  video->setName( moduleName );
1330  odeinit._iCub->eyeLidsPortName = moduleName;
1331  Property options;
1332 
1333  //get the camera calibration parameters
1334  string camcalib_context=robot_config->getFinder().check("camcalib_context",
1335  Value("cameraCalibration")).asString().c_str();
1336  string camcalib_file=robot_config->getFinder().check("camcalib_file",
1337  Value("icubSimEyes.ini")).asString().c_str();
1338 
1339  ResourceFinder rf_camcalib;
1340  rf_camcalib.setVerbose();
1341  rf_camcalib.setDefaultContext(camcalib_context.c_str());
1342  rf_camcalib.setDefaultConfigFile(camcalib_file.c_str());
1343  rf_camcalib.configure(0,NULL);
1344 
1345  //left
1346  Bottle &bCalibLeft=rf_camcalib.findGroup("CAMERA_CALIBRATION_LEFT");
1347  width_left=bCalibLeft.check("w",Value(320)).asInt();
1348  height_left=bCalibLeft.check("h",Value(240)).asInt();
1349 
1352 
1353  double focal_length_left=bCalibLeft.check("fy",Value(257.34)).asDouble();
1354  fov_left=2*atan2((double)height_left,2*focal_length_left)*180.0/M_PI;
1355 
1356  //right
1357  Bottle &bCalibRight=rf_camcalib.findGroup("CAMERA_CALIBRATION_RIGHT");
1358  width_right=bCalibRight.check("w",Value(320)).asInt();
1359  height_right=bCalibRight.check("h",Value(240)).asInt();
1360 
1361  double focal_length_right=bCalibRight.check("fy",Value(257.34)).asDouble();
1362  fov_right=2*atan2((double)height_right,2*focal_length_right)*180.0/M_PI;
1363  //--------------------------------------//
1364 
1365 
1366  ConstString videoconf = robot_config->getFinder().findFile("video");
1367  options.fromConfigFile(videoconf.c_str());
1368 
1369  Bottle textures = *options.find("textures").asList();
1370  for (int i=0; i<textures.size(); i++) {
1371  ConstString name = textures.get(i).asString();
1372  printf("Adding video texture %s\n", name.c_str());
1373  video->add(options.findGroup(name.c_str()));
1374  }
1375 }
1376 
1378  delete video;
1379 }
1380 
1381 
1383  OdeInit& odeinit = OdeInit::get();
1384  if (reset) {
1385  odeinit.sync = false;
1386  }
1387  return odeinit.sync;
1388 }
1389 
1390 
1391 bool OdeSdlSimulation::getTrqData(Bottle data) {
1392  OdeInit& odeinit = OdeInit::get();
1393  for (int s=0; s<data.size(); s++){
1394  odeinit._iCub->torqueData[s] = data.get(s).asDouble();
1395  //fprintf(stdout,"torques... %lf \n",odeinit._iCub->torqueData[s]);
1396  }
1397  return true;
1398 }
1399 
1400 
1401 
1402 bool OdeSdlSimulation::getImage(ImageOf<PixelRgb>& target) {
1403  int w = cameraSizeWidth;
1404  int h = cameraSizeHeight;
1405  int p = 3;
1406 
1407  char *buf=new char[w * h * p];
1408  glReadPixels( 0, 0, w, h, GL_RGB, GL_UNSIGNED_BYTE, buf);
1409  ImageOf<PixelRgb> img;
1410  img.setQuantum(1);
1411  img.setExternal(buf,w,h);
1412 
1413  // inefficient flip!
1414  target.resize(img);
1415  int ww = img.width();
1416  int hh = img.height();
1417  for (int x=0; x<ww; x++) {
1418  for (int y=0; y<hh; y++) {
1419  target(x,y) = img(x,hh-1-y);
1420  }
1421  }
1422  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
1423  delete[] buf;
1424  return true;
1425 }
1426 
1427 
1428 
1429 void OdeSdlSimulation::processWholeBodyCollisions(skinContactList& skin_contact_list)
1430 {
1431  SkinPart skinPart; // id of the part of the skin (e.g. left_hand, right_forearm, left_upper_arm)
1432  BodyPart bodyPart; // id of the body part
1433  bool skinCoverFlag;
1434  Vector geoCenter_SIM_FoR_forHomo(4,0.0), normal_SIM_FoR_forHomo(4,0.0);
1435  Vector force_SIM_FoR_forHomo(4,0.0), moment_SIM_FoR_forHomo(4,0.0);
1436  Vector v1(4,0.0); //auxilliary vector
1437  Vector geoCenter_link_FoR(3,0.0), normal_link_FoR(3,0.0);
1438  Vector force_link_FoR(3,0.0), moment_link_FoR(3,0.0);
1439  double forceOnBody_magnitude;
1440  double left_arm_encoders[16], right_arm_encoders[16], torso_encoders[3], head_encoders[6];
1441  Vector left_arm_for_iKin(10,0.0), right_arm_for_iKin(10,0.0), inertial_for_iKin(6,0.0);
1442  Matrix T_root_to_link = yarp::math::zeros(4,4);
1443  Matrix T_link_to_root = yarp::math::zeros(4,4);
1444  std::vector<unsigned int> taxel_list;
1445 
1446  taxel_list.push_back(1); // we will always emulate one activated "taxel" per contact joint - say taxel "1"
1447  OdeInit& odeinit = OdeInit::get();
1448  odeinit._controls[PART_ARM_LEFT]->getEncodersRaw(left_arm_encoders);
1449  odeinit._controls[PART_ARM_RIGHT]->getEncodersRaw(right_arm_encoders);
1450  odeinit._controls[PART_TORSO]->getEncodersRaw(torso_encoders);
1451  odeinit._controls[PART_HEAD]->getEncodersRaw(head_encoders); //first three are probably neck joints, then the eyes
1452 
1453  for (int j=0;j<TORSO_DOF;j++){
1454  left_arm_for_iKin(j)=torso_encoders[j]; //first 3 joints - 0 to 2 - in iKin arm are torso joints
1455  right_arm_for_iKin(j)=torso_encoders[j];
1456  inertial_for_iKin(j)=torso_encoders[j];
1457  }
1458  for (int l=0;l<7;l++){
1459  left_arm_for_iKin(l+TORSO_DOF) = left_arm_encoders[l]; // then we put seven arm joints (we ignore the rest of the encoders up to 16 - these are fingers)
1460  right_arm_for_iKin(l+TORSO_DOF) = right_arm_encoders[l];
1461  }
1462  for (int m=0;m<3;m++){
1463  inertial_for_iKin(m+TORSO_DOF) = head_encoders[m]; //we put the second three - the neck joints and ignore the rest of head_encoders (the eyes)
1464  }
1465 
1466  odeinit._iCub->iKinLeftArm.setAng(left_arm_for_iKin);
1467  odeinit._iCub->iKinRightArm.setAng(right_arm_for_iKin);
1468  odeinit._iCub->iKinInertialSensor.setAng(inertial_for_iKin);
1469 
1470  if (MY_VERBOSITY > 0) printf("OdeSdlSimulation::processWholeBodyCollisions:There were %lu iCub collisions to process. \n", odeinit.listOfSkinContactInfos.size());
1471  for (list<OdeInit::contactOnSkin_t>::iterator it = odeinit.listOfSkinContactInfos.begin(); it!=odeinit.listOfSkinContactInfos.end(); it++){
1472  geoCenter_SIM_FoR_forHomo.zero(); geoCenter_SIM_FoR_forHomo(3)=1.0; //setting the extra row to 1 - for multiplication by homogenous rototransl. matrix
1473  normal_SIM_FoR_forHomo.zero(); normal_SIM_FoR_forHomo(3)=1.0;
1474  force_SIM_FoR_forHomo.zero(); force_SIM_FoR_forHomo(3)=1.0;
1475  moment_SIM_FoR_forHomo.zero(); moment_SIM_FoR_forHomo(3)=1.0;
1476  geoCenter_link_FoR.zero();normal_link_FoR.zero();
1477  moment_link_FoR.zero();force_link_FoR.zero();
1478  forceOnBody_magnitude=0.0;
1479  skinPart = SKIN_PART_UNKNOWN; bodyPart = BODY_PART_UNKNOWN; skinCoverFlag = false;
1480  T_root_to_link.zero(); T_link_to_root.zero();
1481 
1482  odeinit._iCub->getSkinAndBodyPartFromSpaceAndGeomID((*it).body_geom_space_id,(*it).body_geom_id,skinPart,bodyPart,skinCoverFlag);
1483  for (int i=0;i<3;i++){
1484  geoCenter_SIM_FoR_forHomo(i)= (*it).contact_geom.pos[i]; //in global (i.e. simulator) coordinates
1485  normal_SIM_FoR_forHomo(i) = (*it).contact_geom.normal[i];
1486  }
1487  dJointFeedback * fb = dJointGetFeedback ((*it).contact_joint);
1488  if (fb==NULL){
1489  printf("Warning:OdeSdlSimulation::processWholeBodyCollisions: This joint (at %d skin part) has no feedback structure defined - contact force not available: setting to -1.\n",skinPart);
1490  forceOnBody_magnitude = -1;
1491  }
1492  else{
1493  //printf("OdeSdlSimulation::processWholeBodyCollisions: joint feedback structure:\n.");
1494  //printf("f1 force vector in simulator FoR: %f %f %f \n",fb->f1[0],fb->f1[1],fb->f1[2]); // assuming it is global ODE FoR ~ simulator FoR
1495  //printf("f2 force vector: %f %f %f \n",fb->f2[0],fb->f2[1],fb->f2[2]);
1496  //f2 force vector has same magnitude but opposite direction than f1
1497  for(int k=0;k<3;k++){
1498  if((*it).body_index == 1){
1499  force_SIM_FoR_forHomo(k)=fb->f1[k];
1500  moment_SIM_FoR_forHomo(k)=fb->t1[k];
1501  }
1502  else if((*it).body_index == 2){
1503  force_SIM_FoR_forHomo(k)=fb->f2[k];
1504  moment_SIM_FoR_forHomo(k)=fb->t2[k];
1505  }
1506  else{
1507  yError("OdeSdlSimulation::processWholeBodyCollisions: unexpected body_index for colliding body: %d.\n",(*it).body_index);
1508  }
1509  }
1510  forceOnBody_magnitude=sqrt(force_SIM_FoR_forHomo(0)*force_SIM_FoR_forHomo(0) + force_SIM_FoR_forHomo(1)*force_SIM_FoR_forHomo(1)
1511  + force_SIM_FoR_forHomo(2)*force_SIM_FoR_forHomo(2));
1512  }
1513 
1514  //Let's do all the transformations
1515  //Assuming, dJointFeedback and contact_geom data from ODE are in global ODE frame; the contact_geom.pos is the position; the contact_geom.normal and the dJointFeedback
1516  // vectors (f1, m1) are originating from the global origin, i.e. they need to be translated to contact_geom.pos;
1517  //see the post in ode-users group "dJointFeedback and dContactGeom reference frame", 6.12.2013local FoR of the contact point;
1518 
1519  switch(bodyPart){
1520  case LEFT_ARM:
1521  T_root_to_link = odeinit._iCub->iKinLeftArm.getH(SkinPart_2_LinkNum[skinPart].linkNum + TORSO_DOF);
1522  //e.g. skinPart LEFT_UPPER_ARM gives link number 2, which means we ask iKin for getH(2+3), which gives us FoR 6 - at the first elbow joint, which is the FoR for the upper arm
1523  break;
1524  case RIGHT_ARM:
1525  T_root_to_link = odeinit._iCub->iKinRightArm.getH(SkinPart_2_LinkNum[skinPart].linkNum + TORSO_DOF);
1526  break;
1527  case TORSO:
1528  T_root_to_link = odeinit._iCub->iKinInertialSensor.getH(SkinPart_2_LinkNum[skinPart].linkNum);
1529  // SkinPart_2_LinkNum[SKIN_FRONT_TORSO].linkNum is 2, this should give us the FoR 3 - the first neck joint which is the expected torso FoR
1530  //- check " SKIN torso 2" in iCub/main/app/iCubGui/skeleton.ini
1531  //- importantly, this needs to be the iKinInertialSensor, not the iKin Arm;
1532  break;
1533  default:
1534  if (MY_VERBOSITY > 0) printf("OdeSdlSimulation::processWholeBodyCollisions: FoR transforms to BODY PART %d not implemented yet\n",bodyPart);
1535  continue;
1536  }
1537  T_link_to_root = SE3inv(T_root_to_link);
1538 
1539  v1.zero();
1540  v1 = T_link_to_root * (odeinit._iCub->H_r2w) * geoCenter_SIM_FoR_forHomo; //first transform to robot coordinates, then transform to local FoR of respective body part
1541  geoCenter_link_FoR = v1.subVector(0,2); //strip the last one away
1542 
1543  v1.zero();
1544  v1 = T_link_to_root * (odeinit._iCub->H_r2w) * normal_SIM_FoR_forHomo;
1545  normal_link_FoR = v1.subVector(0,2);
1546 
1547  v1.zero();
1548  v1 = T_link_to_root * (odeinit._iCub->H_r2w) * force_SIM_FoR_forHomo;
1549  force_link_FoR = v1.subVector(0,2);
1550 
1551  v1.zero();
1552  v1 = T_link_to_root * (odeinit._iCub->H_r2w) * moment_SIM_FoR_forHomo;
1553  moment_link_FoR = v1.subVector(0,2);
1554 
1555  //Note that the normal, force, and moment are just carrying the orientation (and apart from the normal also magnitude) - they will still need to be translated to the
1556  //appropariate CoP / geoCenter to make the arrow to the taxel
1557  //Note also the dJointFeedback force vector does not necessarily point along the normal at the contact point (which points into the colliding body) - as is a sum of the
1558  //forces along the normal and frictional forces perpendicular to the normal
1559  //alternatively, I could just take the magnitude from the force and send the normal as the direction
1560 
1561  //printf("Contact coordinates in ODE / SIM FoR: %s\n",geoCenter_SIM_FoR_forHomo.subVector(0,2).toString().c_str());
1562  Vector temp_v4(4,0.0);
1563  temp_v4 = (odeinit._iCub->H_r2w) * geoCenter_SIM_FoR_forHomo;
1564  //printf("Contact coordinates in robot root FoR: %s\n",temp_v4.subVector(0,2).toString().c_str());
1565  //printf("Left arm for iKin:\n %s \n",left_arm_for_iKin.toString().c_str());
1566  //printf("Rototranslation matrix root to link:\n %s\n",T_root_to_link.toString().c_str());
1567  //printf("Contact coordinates in link FoR: %s\n",geoCenter_link_FoR.toString().c_str());
1568  /*for (int l=0;l<2;l++){
1569  geoCenter_link_FoR(l)=0.0;
1570  force_link_FoR(l)=1.0;
1571  normal_link_FoR(l)=1.0;
1572  moment_link_FoR(l)=1.0;
1573  } */
1574  //forceOnBody_magnitude=10.0;
1575 
1576  skinContact c(bodyPart, skinPart, getLinkNum(skinPart), geoCenter_link_FoR, geoCenter_link_FoR,taxel_list, forceOnBody_magnitude, normal_link_FoR,force_link_FoR,moment_link_FoR);
1577  //we have only one source of information - the contact as detected by ODE - therefore, we take the coordinates and set them both to CoP
1578  //(which is supposed to come from the dynamic estimation, and as geoCenter - from skin; Similarly, we derive the pressure directly from the force vector from ODE.
1579  if (MY_VERBOSITY > 0) printf("Creating skin contact as follows: %s.\n",c.toString().c_str());
1580  skin_contact_list.push_back(c);
1581  } // odeinit.listOfSkinContactInfos - iterator
1582 }
1583 
1584 void OdeSdlSimulation::inspectWholeBodyContactsAndSendTouch()
1585 {
1586 
1587  std::map<SkinPart,bool> skinPartsTouched;
1588  SkinPart skinPart; // id of the part of the skin - from skinDynLib/common.h
1589  BodyPart bodyPart; // id of the body part
1590  bool skinCoverFlag = false;
1591  bool left_hand_notCover_touched = false;
1592  bool right_hand_notCover_touched = false;
1593  OdeInit& odeinit = OdeInit::get();
1594 
1595  skinPartsTouched[SKIN_LEFT_FOREARM]=false; skinPartsTouched[SKIN_LEFT_UPPER_ARM]=false;
1596  skinPartsTouched[SKIN_RIGHT_FOREARM]=false; skinPartsTouched[SKIN_RIGHT_FOREARM]=false;
1597  skinPartsTouched[SKIN_FRONT_TORSO]=false;
1598 
1599  for (list<OdeInit::contactOnSkin_t>::iterator it = odeinit.listOfSkinContactInfos.begin(); it!=odeinit.listOfSkinContactInfos.end(); it++){
1600  skinPart = SKIN_PART_UNKNOWN; bodyPart = BODY_PART_UNKNOWN; skinCoverFlag = false;
1601  odeinit._iCub->getSkinAndBodyPartFromSpaceAndGeomID((*it).body_geom_space_id,(*it).body_geom_id,skinPart,bodyPart,skinCoverFlag);
1602  //We emulate the skin of the iCub - covers + fingertips; the rest of the geoms will only be processed by the skinEvents
1603  if(skinCoverFlag){
1604  //if it was a cover that was touched, we will emulate the skin there - this includes the palm cover
1605  skinPartsTouched[skinPart]=true; //if the skinPart comes back as UNKNOWN, this is fine, we set it to true
1606  }
1607  else if(skinPart == SKIN_LEFT_HAND) {
1608  left_hand_notCover_touched = true; //these need special treatment because for the fingertip, we check the geoms, there are no covers there
1609  }
1610  else if(skinPart == SKIN_RIGHT_HAND) {
1611  right_hand_notCover_touched = true; //these need special treatment because for the fingertip, we check the geoms, there are no covers there
1612  }
1613  }
1614 
1615  //rewritten based on original inspectTouch_icubSensors, the output of actual pressure values is discontinued; the collisions with palm cover are added
1616  // the palm cover replaces sensing in the palm body
1618  Bottle bottleLeftHand;
1619  if (left_hand_notCover_touched || skinPartsTouched[SKIN_LEFT_HAND]){
1620  if (left_hand_notCover_touched){ // some part other than the palm cover was touched, need to check fingertips
1621  int bodyIndicesLeft[5] = {24, 25, 26, 27, 30};
1622  double resultLeft=0;
1623  for (int x = 0; x < 5; x++){
1624  if (odeinit._iCub->actLHand == "on"){
1625  resultLeft = odeinit._iCub->checkTouchSensor(bodyIndicesLeft[x]);
1626  }
1627  else{
1628  resultLeft = odeinit._iCub->checkTouchSensor(odeinit._iCub->l_hand);
1629  }
1630  //now filling up the bottle - the port output where 1-60 are taxels of fingertips (12 each);
1631  for (int i = 0; i < 12; i++){
1632  bottleLeftHand.addDouble(resultLeft * 255);
1633  }
1634  }
1635  }
1636  else{ //we fill the fingertip positions with 0s
1637  for (int i = 0; i < 60; i++){
1638  bottleLeftHand.addDouble(0.0);
1639  }
1640  }
1641  //now filling up the bottle - the port output: 61-96 zeros;
1642  for (int y = 0; y<36; y++){ //positions 61-96
1643  bottleLeftHand.addDouble(0.0);
1644  }
1645  if(skinPartsTouched[SKIN_LEFT_HAND]){ //palm cover
1646  //97-144 palm taxels (inside these, 107, 119, 131, and 139 are thermal pads ~ 0s);
1647  for (int y = 0; y<48; y++){
1648  bottleLeftHand.addDouble(255); //we ignore the thermal pad positions which should be 0s for now
1649  }
1650  }
1651  else{
1652  for (int y = 0; y<48; y++){
1653  bottleLeftHand.addDouble(0.0); //we ignore the thermal pad positions which should be 0s for now
1654  }
1655  }
1656  //filling the rest: 145-192 zeros.
1657  for (int y = 0; y<48; y++){
1658  bottleLeftHand.addDouble(0.0);
1659  }
1660  }
1661  else{
1662  bottleLeftHand = Bottle(odeinit._iCub->emptySkinActivationHand);
1663  }
1664  robot_streamer->sendTouchLeftHand(bottleLeftHand);
1665  }
1666 
1667  //rewritten based on original inspectTouch_icubSensors, the output of actual pressure values is discontinued; the collisions with palm cover are added
1668  // the palm cover replaces sensing in the palm body
1670  Bottle bottleRightHand;
1671  if (right_hand_notCover_touched || skinPartsTouched[SKIN_RIGHT_HAND]){
1672  if (right_hand_notCover_touched){ // some part other than the palm cover was touched, need to check fingertips
1673  int bodyIndicesRight[5] = {43, 44, 45, 46, 49};
1674  double resultRight=0;
1675  for (int x = 0; x < 5; x++){
1676  if (odeinit._iCub->actRHand == "on"){
1677  resultRight = odeinit._iCub->checkTouchSensor(bodyIndicesRight[x]);
1678  }
1679  else{
1680  resultRight = odeinit._iCub->checkTouchSensor(odeinit._iCub->r_hand);
1681  }
1682  //now filling up the bottle - the port output where 1-60 are taxels of fingertips (12 each);
1683  for (int i = 0; i < 12; i++){
1684  bottleRightHand.addDouble(resultRight * 255);
1685  }
1686  }
1687  }
1688  else{ //we fill the fingertip positions with 0s
1689  for (int i = 0; i < 60; i++){
1690  bottleRightHand.addDouble(0.0);
1691  }
1692  }
1693  //now filling up the bottle - the port output: 61-96 zeros;
1694  for (int y = 0; y<36; y++){ //positions 61-96
1695  bottleRightHand.addDouble(0.0);
1696  }
1697  if(skinPartsTouched[SKIN_RIGHT_HAND]){ //palm cover
1698  //97-144 palm taxels (inside these, 107, 119, 131, and 139 are thermal pads ~ 0s);
1699  for (int y = 0; y<48; y++){
1700  bottleRightHand.addDouble(255); //we ignore the thermal pad positions which should be 0s for now
1701  }
1702  }
1703  else{
1704  for (int y = 0; y<48; y++){
1705  bottleRightHand.addDouble(0.0); //we ignore the thermal pad positions which should be 0s for now
1706  }
1707  }
1708  //filling the rest: 145-192 zeros.
1709  for (int y = 0; y<48; y++){
1710  bottleRightHand.addDouble(0.0);
1711  }
1712  }
1713  else{
1714  bottleRightHand = Bottle(odeinit._iCub->emptySkinActivationHand);
1715  }
1716  // printf("bottleRightHand: %s \n",bottleRightHand.toString().c_str());
1717  robot_streamer->sendTouchRightHand(bottleRightHand);
1718  }
1719 
1721  Bottle bottleLeftArm;
1722  if (skinPartsTouched[SKIN_LEFT_UPPER_ARM]){
1723  bottleLeftArm = Bottle(odeinit._iCub->fullSkinActivationUpperArm);
1724  }
1725  else{
1726  bottleLeftArm = Bottle(odeinit._iCub->emptySkinActivationUpperArm);
1727  }
1728  robot_streamer->sendTouchLeftArm(bottleLeftArm);
1729  }
1731  Bottle bottleLeftForearm;
1732  if(skinPartsTouched[SKIN_LEFT_FOREARM]){
1733  bottleLeftForearm = Bottle(odeinit._iCub->fullSkinActivationForearm);
1734  // printf("bottleLeftForearm: %s \n",bottleLeftForearm.toString().c_str());
1735  }
1736  else{
1737  bottleLeftForearm = Bottle(odeinit._iCub->emptySkinActivationForearm);
1738  }
1739  robot_streamer->sendTouchLeftForearm(bottleLeftForearm);
1740  }
1742  Bottle bottleRightArm;
1743  if (skinPartsTouched[SKIN_RIGHT_UPPER_ARM]){
1744  bottleRightArm = Bottle(odeinit._iCub->fullSkinActivationUpperArm);
1745  }
1746  else{
1747  bottleRightArm = Bottle(odeinit._iCub->emptySkinActivationUpperArm);
1748  }
1749  robot_streamer->sendTouchRightArm(bottleRightArm);
1750  }
1752  Bottle bottleRightForearm;
1753  if(skinPartsTouched[SKIN_RIGHT_FOREARM]){
1754  bottleRightForearm = Bottle(odeinit._iCub->fullSkinActivationForearm);
1755  }
1756  else{
1757  bottleRightForearm = Bottle(odeinit._iCub->emptySkinActivationForearm);
1758  }
1759  robot_streamer->sendTouchRightForearm(bottleRightForearm);
1760  }
1762  Bottle bottleTorso;
1763  if(skinPartsTouched[SKIN_FRONT_TORSO]){
1764  bottleTorso = Bottle(odeinit._iCub->fullSkinActivationTorso);
1765  }
1766  else{
1767  bottleTorso = Bottle(odeinit._iCub->emptySkinActivationTorso);
1768  }
1769  robot_streamer->sendTouchTorso(bottleTorso);
1770  }
1771 }
1772 
1773 
1774 //Auxiliary function to print class of geom - according to section 9.5 of ODE manual
1775 std::string OdeSdlSimulation::getGeomClassName(const int geom_class,std::string & s)
1776 {
1777  switch(geom_class){
1778  case 0:
1779  s = "sphere";
1780  break;
1781  case 1:
1782  s = "box";
1783  break;
1784  case 2:
1785  s = "capsule";
1786  break;
1787  case 3:
1788  s = "cylinder";
1789  break;
1790  case 4:
1791  s = "plane";
1792  break;
1793  case 8:
1794  s= "triangle mesh";
1795  break;
1796  case 10:
1797  case 11:
1798  s = "simple space";
1799  break;
1800  case 12:
1801  s="hash space";
1802  break;
1803  default:
1804  s ="unknown type";
1805  break;
1806  }
1807  return s;
1808 
1809 }
static float hpr[8]
Definition: iCub_Sim.cpp:47
static float lasty
Definition: iCub_Sim.cpp:75
const Skin_2_Link SkinPart_2_LinkNum[SKIN_PART_SIZE]
Definition: common.h:117
static int mouseDiffy
Definition: iCub_Sim.cpp:51
static bool eyeCams
Definition: iCub_Sim.cpp:87
static SDL_Surface * image
Definition: iCub_Sim.cpp:82
#define MAX_PART
iCub::iKin::iCubInertialSensor iKinInertialSensor
Definition: iCub.h:272
virtual bool getTrqData(Bottle data)
Definition: iCub_Sim.cpp:1391
struct timeval prevTime[BOARD_NUM]
virtual bool shouldSendInertial()=0
static RobotConfig * robot_config
Definition: iCub_Sim.cpp:86
double checkTouchSensor_continuousValued(int bodyToCheck)
Definition: iCub.cpp:108
yarp::os::Semaphore mutex
Definition: OdeInit.h:64
yarp::os::Semaphore mutexTexture
Definition: OdeInit.h:65
static float * VAD
Definition: iCub_Sim.cpp:62
static int width_left
Definition: iCub_Sim.cpp:91
virtual void sendTouchRightHand(yarp::os::Bottle &report)=0
static int mouse1_down_y
Definition: iCub_Sim.cpp:59
#define PART_ARM_RIGHT
Class representing a list of external contacts acting on the iCub' skin.
virtual void sendTouchLeftForearm(yarp::os::Bottle &report)=0
static double duration
Definition: iCub_Sim.cpp:78
bool sync
Definition: OdeInit.h:69
dSpaceID body_geom_space_id
Definition: OdeInit.h:77
iCub::iKin::iCubArm iKinRightArm
Definition: iCub.h:271
dBodyID inertialBody
Definition: iCub.h:96
static dJointFeedback touchSensorFeedbacks[MAX_DJOINT_FEEDBACKSTRUCTS]
Definition: iCub_Sim.cpp:111
OdeSdlSimulation()
Constructor.
Definition: iCub_Sim.cpp:1311
void DrawVideo(VideoTexture *video)
Definition: rendering.cpp:455
virtual void sendInertial(yarp::os::Bottle &report)=0
void getSkinAndBodyPartFromSpaceAndGeomID(const dSpaceID geomSpaceID, const dGeomID geomID, SkinPart &skinPart, BodyPart &bodyPart, bool &skinCoverFlag)
Definition: iCub.cpp:4176
virtual void sendSkinEvents(iCub::skinDynLib::skinContactList &skinContactListReport)=0
#define LEFT_ARM
void DrawGround(bool wireframe)
Definition: rendering.cpp:121
string eyeLidsPortName
Definition: iCub.h:86
int MODEL_NUM
Definition: world.h:174
static long startTime
Definition: iCub_Sim.cpp:77
virtual void checkTorques()=0
bool WAITLOADING
Definition: world.h:223
#define PART_ARM_LEFT
bool static_model
Definition: world.h:224
static const GLfloat light_position[]
Definition: iCub_Sim.cpp:88
static int height_right
Definition: iCub_Sim.cpp:94
dSpaceID iCubRightArmSpace
Definition: iCub.h:89
static double dstep
Definition: iCub_Sim.cpp:38
ICubSim * _iCub
Definition: OdeInit.h:66
static float zoom
Definition: iCub_Sim.cpp:73
static VideoTexture * video
Definition: iCub_Sim.cpp:84
virtual bool shouldSendTouchLeftForearm()=0
static long ode_step_length
Definition: iCub_Sim.cpp:37
void setName(string module)
dBodyID l_hand
Definition: iCub.h:163
const dReal * pos
Definition: iCub_Sim.cpp:64
static float view2_hpr[3]
Definition: iCub_Sim.cpp:72
dSpaceID iCubLeftArmSpace
Definition: iCub.h:89
static int stop
Definition: iCub_Sim.cpp:43
dBodyID r_hand
Definition: iCub.h:163
static float ydistance
Definition: iCub_Sim.cpp:67
zeros(2, 2) eye(2
string getName()
Definition: OdeInit.h:88
dWorldID world
Definition: OdeInit.h:57
static float ypos
Definition: iCub_Sim.cpp:74
Matrix H_r2w
Definition: iCub.h:284
static void sighandler(int)
Definition: main.cpp:152
static float angle
Definition: iCub_Sim.cpp:74
ConstString actSkinEmul
Definition: iCub.h:80
bool add(const char *port, int textureIndex)
Definition: VideoTexture.h:78
static float yrot
Definition: iCub_Sim.cpp:74
static float zrot
Definition: iCub_Sim.cpp:74
static RobotStreamer * robot_streamer
Definition: iCub_Sim.cpp:85
yarp::os::ConstString texture
Definition: world.h:295
static int cameraSizeWidth
Definition: iCub_Sim.cpp:99
static double frames
Definition: iCub_Sim.cpp:78
Semaphore ODE_access(1)
list< contactOnSkin_t > listOfSkinContactInfos
Definition: OdeInit.h:82
ConstString actCoversCol
Definition: iCub.h:80
#define MAX_DJOINT_FEEDBACKSTRUCTS
Definition: iCub_Sim.cpp:109
int modelTexture[100]
Definition: world.h:261
virtual bool shouldSendTouchLeftArm()=0
static int width
Definition: iCub_Sim.cpp:55
static float view_xyz[3]
Definition: iCub_Sim.cpp:69
bool reinitialized
Definition: iCub.h:84
tuple config
Definition: icubapp.py:262
virtual yarp::os::ResourceFinder & getFinder()=0
static float yrotrad
Definition: iCub_Sim.cpp:76
ConstString actVision
Definition: iCub.h:80
virtual bool shouldSendTouchTorso()=0
#define MAX_CONTACTS
Definition: iCub.h:68
dSpaceID iCubHeadSpace
Definition: iCub.h:89
void drawView(bool left, bool right, bool wide)
Render the requested view.
Definition: iCub_Sim.cpp:1239
tuple name
Definition: manager.py:710
Bottle fullSkinActivationUpperArm
Definition: iCub.h:280
dSpaceID space
Definition: OdeInit.h:58
static float xrot
Definition: iCub_Sim.cpp:74
iCubSimulationControl ** _controls
Definition: OdeInit.h:72
virtual void sendTouchTorso(yarp::os::Bottle &report)=0
virtual bool shouldSendSkinEvents()=0
static double seconds
Definition: iCub_Sim.cpp:78
yarp::sig::Matrix getH(const unsigned int i, const bool allLink=false)
Returns the rigid roto-translation matrix from the root reference frame to the ith frame in Denavit-H...
Definition: iKinFwd.cpp:752
fprintf(fid,'\n')
static float view2_xyz[3]
Definition: iCub_Sim.cpp:71
dSpaceID iCubLegsSpace
Definition: iCub.h:89
static int v
Definition: iCub_Sim.cpp:44
static float xrotrad
Definition: iCub_Sim.cpp:76
void sendHomePos()
Definition: OdeInit.cpp:125
static OdeInit & get()
Definition: OdeInit.cpp:162
virtual void sendTouchRightForearm(yarp::os::Bottle &report)=0
static bool picking
Definition: iCub_Sim.cpp:52
static long gl_frame_length
Definition: iCub_Sim.cpp:36
Bottle fullSkinActivationForearm
Definition: iCub.h:279
static long finishTime
Definition: iCub_Sim.cpp:77
static const int TORSO_DOF
Definition: common.h:194
static int height
Definition: iCub_Sim.cpp:56
dJointID contact_joint
Definition: OdeInit.h:80
Bottle emptySkinActivationForearm
Definition: iCub.h:276
static bool extractImages
Definition: iCub_Sim.cpp:83
virtual int getWorldTimestep()=0
static int mouse0_down_x
Definition: iCub_Sim.cpp:58
This file is responsible for the initialisation of the world parameters that are controlled by ODE...
ConstString actSelfCol
Definition: iCub.h:80
ODE state information.
Definition: OdeInit.h:54
virtual bool shouldSendTouchRightHand()=0
int s_modelTexture[100]
Definition: world.h:262
#define M_PI
Definition: math.h:53
static int nFeedbackStructs
Definition: iCub_Sim.cpp:112
static float lastx
Definition: iCub_Sim.cpp:75
virtual bool getImage(yarp::sig::ImageOf< yarp::sig::PixelRgb > &target)
Definition: iCub_Sim.cpp:1402
ConstString actTorso
Definition: iCub.h:80
static double FPS
Definition: iCub_Sim.cpp:78
static float zpos
Definition: iCub_Sim.cpp:74
static int mouse0_down_y
Definition: iCub_Sim.cpp:59
Bottle emptySkinActivationHand
Definition: iCub.h:275
void draw()
Definition: world.cpp:62
#define CTRL_RAD2DEG
Definition: iCub_Sim.cpp:31
Bottle emptySkinActivationUpperArm
Definition: iCub.h:277
void clearBuffer()
Signal that we're done with a view.
Definition: iCub_Sim.cpp:1307
dGeomID inertialGeom
Definition: iCub.h:97
static float test[3]
Definition: iCub_Sim.cpp:79
double torqueData[100]
Definition: iCub.h:237
void simLoop(int h, int w)
Run the simulation.
Definition: iCub_Sim.cpp:1140
static Uint32 colorkey
Definition: iCub_Sim.cpp:81
static float xpos
Definition: iCub_Sim.cpp:74
virtual void sendTouchLeftHand(yarp::os::Bottle &report)=0
static float xdistance
Definition: iCub_Sim.cpp:68
static bool simrun
Definition: iCub_Sim.cpp:41
#define MY_VERBOSITY
Definition: iCub_Sim.h:61
float eyeLidRot
Definition: iCub.h:85
dBodyID body[50]
Definition: iCub.h:117
dJointGroupID contactgroup
Definition: OdeInit.h:59
static float angle_xref
Definition: iCub_Sim.cpp:65
dContactGeom contact_geom
Definition: OdeInit.h:79
static int contactPoint
Definition: iCub_Sim.cpp:50
void setJointControlAction()
Set the control action for all the joints, that can be either a velocity command or a torque command...
Definition: iCub.cpp:192
dGeomID Leye1_geom
Definition: iCub.h:177
~OdeSdlSimulation()
Destructor.
Definition: iCub_Sim.cpp:1377
static float view_hpr[3]
Definition: iCub_Sim.cpp:70
void drawSkyDome(float x, float y, float z, float width, float height, float length)
Definition: rendering.cpp:158
dGeomID Reye1_geom
Definition: iCub.h:182
worldSim * _wrld
Definition: OdeInit.h:67
void loadTexture(yarp::os::ConstString texture, int numTexture)
Definition: world.cpp:172
virtual bool shouldSendTouchRightForearm()=0
#define RIGHT_ARM
static float * VAD2
Definition: iCub_Sim.cpp:63
static bool glrun
Definition: iCub_Sim.cpp:40
ConstString actRArm
Definition: iCub.h:80
ConstString actStartHomePos
Definition: iCub.h:80
static double fov_right
Definition: iCub_Sim.cpp:96
ConstString actPressure
Definition: iCub.h:80
int s_MODEL_NUM
Definition: world.h:175
virtual bool getEncodersRaw(double *encs)
static double TimestepManager
Definition: iCub_Sim.cpp:78
dSpaceID iCub
Definition: iCub.h:88
bool checkSync(bool reset=false)
Definition: iCub_Sim.cpp:1382
bool stop
Definition: OdeInit.h:68
static int width_right
Definition: iCub_Sim.cpp:92
virtual bool shouldSendTouchLeftHand()=0
static int mouse_ray_x
Definition: iCub_Sim.cpp:60
Class representing an external contact acting on the iCub' skin.
Definition: skinContact.h:49
iCub::iKin::iCubArm iKinLeftArm
Definition: iCub.h:271
double SimTime
Definition: OdeInit.h:56
#define PART_TORSO
static int height_left
Definition: iCub_Sim.cpp:93
static int mouse_ray_y
Definition: iCub_Sim.cpp:61
static int mouse1_down_x
Definition: iCub_Sim.cpp:58
static int cameraSizeHeight
Definition: iCub_Sim.cpp:100
#define PART_HEAD
dBodyID lhandfingers3
Definition: iCub.h:133
virtual void sendTouchLeftArm(yarp::os::Bottle &report)=0
ConstString actHead
Definition: iCub.h:80
virtual bool shouldSendTouchRightArm()=0
form input data form the output data fill the input data for i
Definition: acquireData.m:52
std::map< dSpaceID, string > dSpaceNames
Definition: iCub.h:91
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:95
int getLinkNum(SkinPart s)
Get the link number associated to the specified skin part.
Definition: common.cpp:83
ConstString actRHand
Definition: iCub.h:80
void draw()
Definition: iCub.cpp:318
static float angle_yref
Definition: iCub_Sim.cpp:66
static float xyz[3]
Definition: iCub_Sim.cpp:46
static float cam_rx
Definition: iCub_Sim.cpp:53
Definition: main.cpp:145
bool setup_opengl(ResourceFinder &finder)
Definition: rendering.cpp:63
Bottle fullSkinActivationTorso
Definition: iCub.h:281
void init(RobotStreamer *streamer, RobotConfig *config)
Initialization.
Definition: iCub_Sim.cpp:1314
std::map< dGeomID, string > dGeomNames
Definition: iCub.h:92
static float cam_ry
Definition: iCub_Sim.cpp:53
yarp::sig::Matrix SE3inv(const yarp::sig::Matrix &H, unsigned int verbose=0)
Returns the inverse of a 4 by 4 rototranslational matrix.
ConstString actLArm
Definition: iCub.h:80
static bool START_SELF_COLLISION_DETECTION
Definition: iCub_Sim.cpp:114
yarp::sig::Vector setAng(const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i].
static int mouseDiffx
Definition: iCub_Sim.cpp:51
ConstString actLHand
Definition: iCub.h:80
dBodyID rhandfingers3
Definition: iCub.h:145
virtual void sendTouchRightArm(yarp::os::Bottle &report)=0
bool checkTouchSensor(int bodyToCheck)
Definition: iCub.cpp:142
virtual void sendVision()=0
Bottle emptySkinActivationTorso
Definition: iCub.h:278
static float rez[3]
Definition: iCub_Sim.cpp:48
dSpaceID iCubTorsoSpace
Definition: iCub.h:89