iCub-main
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 #include <set>
27 
28 using namespace yarp::sig;
29 
30 // globals
31 Semaphore ODE_access(1);
32 #define CTRL_RAD2DEG (180.0/M_PI)
33 #define CTRL_DEG2RAD (M_PI/180.0)
34 
35 // locals
36 // NOTE that we use (long) instead of (clock_t), because on MacOS, (clock_t) is unsigned, while we need negative numbers
37 static long gl_frame_length = 1000/30; // update opengl and vision stream at 30 Hz
38 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)
39 static double dstep = 10.0/1000.0; // step size in ODE's dWorldStep in seconds
40 
41 static bool glrun; // draw gl
42 static bool simrun; // run simulator thread
43 
44 static int stop = 0;
45 static int v = 0;
46 
47 static float xyz[3];
48 static float hpr[8];
49 static float rez[3];
50 
51 static int contactPoint;
52 static int mouseDiffx, mouseDiffy;
53 static bool picking = false;
54 static float cam_rx = 0.0, cam_ry = 0.0;
55 
56 static int width = 640;
57 static int height = 480;
58 
61 static int mouse_ray_x;
62 static int mouse_ray_y;
63 static float *VAD;
64 static float *VAD2;
65 const dReal *pos;
66 static float angle_xref = 0.0f;
67 static float angle_yref = 25.0f;
68 static float ydistance = 10.0f;
69 static float xdistance = 0.0f;
70 static float view_xyz[3]; // position x,y,z
71 static float view_hpr[3]; // heading, pitch, roll (degrees)
72 static float view2_xyz[3];
73 static float view2_hpr[3];
74 static float zoom = 0;
75 static float xpos = 0, ypos = 0, zpos = 0, xrot = 0, yrot = 0, zrot = 0, angle=0.0;
76 static float lastx, lasty;
77 static float xrotrad = 0, yrotrad = 0;
78 static long startTime, finishTime;
80 static float test[3];
81 //static SDL_TimerID id;
82 static Uint32 colorkey;
83 static SDL_Surface *image;
84 static bool extractImages = false;
85 static VideoTexture *video = NULL;
86 static RobotStreamer *robot_streamer = NULL;
87 static RobotConfig *robot_config = NULL;
88 static bool eyeCams;
89 static const GLfloat light_position[] = { 0.0f, 5.0f, 5.0f, 0.0f };
90 
91 //camera calibration parameters
92 static int width_left;
93 static int width_right;
94 static int height_left;
95 static int height_right;
96 static double fov_left;
97 static double fov_right;
98 
99 static int cameraSizeWidth;
100 static int cameraSizeHeight;
101 
105  std::set<unsigned int> taxelsTouched;
106 };
107 
108 static std::map<SkinPart,contactICubSkinEmul_t> contactICubSkinEmulMap;
109 
110 /* 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.
111  * The number of collisions and contact joints may vary, but we allocate these as a static array for performance issues.
112  * (Allocating feedback structs at every simulation step would degrade simulation performance).
113  * If the MAX_DJOINT_FEEDBACKSTRUCTS was exceeded, contacts will still be saved for the purposes of whole_body_skin_emul,
114  * but the forces send to skinEvents will not be available.
115 */
116 #define MAX_DJOINT_FEEDBACKSTRUCTS 500
117 
119 static int nFeedbackStructs=0;
120 
121 static bool START_SELF_COLLISION_DETECTION = false; //we want to set this trigger on only after the robot is in in home pos -
122  //it's initial configuration is with arms inside the thighs
123 static const double EXTRA_MARGIN_FOR_TAXEL_POSITION_M = 0.03; //0.03 //for skin emulation we get the coordinates of the collision and contact with skin cover from ODE;
124 //after transforming to local reference frame of respective skin part, we emulate which set of taxels would get activated at that position;
125 //however, with errors in the position, we need an extra margin, so the contact falls onto some taxels
126 static const double MORE_EXTRA_MARGIN_FOR_TAXEL_POSITION_M = 0.01; //0.01;
127 
128 void OdeSdlSimulation::draw() {
129  OdeInit& odeinit = OdeInit::get();
130  odeinit._iCub->draw();
131  odeinit._wrld->draw();
132 }
133 
134 void OdeSdlSimulation::printStats() {
135  OdeInit& odeinit = OdeInit::get();
136 
137  finishTime = (long) clock() ;
138  duration += (double)(finishTime - startTime) / CLOCKS_PER_SEC ;
139  frames ++ ;
140  FPS = frames / duration ;
141  startTime = (long) clock() ;
142  odeinit.SimTime = duration;
143  //yDebug("duration: %.2lf\n",odeinit.SimTime);
144  static double starting_time_stamp = 0;
145  //test[0] = dBodyGetPosition(odeinit._iCub->body_cube[0])[0];
146  //test[1] = dBodyGetPosition(odeinit._iCub->body_cube[0])[1];
147  //test[2] = dBodyGetPosition(odeinit._iCub->body_cube[0])[2];
148  //yDebug("test[0] %f test[1] %f test[2] %f\n",test[0],test[1],test[2]);
149  if( duration - starting_time_stamp >= 1){
150  //yDebug("Frames: %.2lf Duration: %.2lf fps: %3.1f \n",frames,duration,FPS);
151  starting_time_stamp = duration;
152  }
153  //yDebug("%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]);
154  //drawText(text, textPos);
155 }
156 
157 void OdeSdlSimulation::handle_key_down(SDL_keysym* keysym) {
158  switch (keysym->sym)
159  {
160  case SDLK_e:
161  break;
162  case SDLK_r:
163  initViewpoint();
164  break;
165  case SDLK_t:
166  break;
167  case SDLK_y:
168  break;
169  case SDLK_SPACE:
170  yInfo("SPACEBAR pressed! Press spacebar again to disable/enable drawing.\n");
171  glrun = !glrun;
172  break;
173  default:
174  break;
175  }
176 }
177 
178 void OdeSdlSimulation::handle_mouse_motion(SDL_MouseMotionEvent* mousemotion) {
179  if (SDL_GetMouseState(NULL, NULL) & SDL_BUTTON(1)){// MOUSE LEFT BUTTON
180  //if (!picking){
181  //camera movement
182  angle_xref += (float)mousemotion->xrel; // 10.0f;
183  angle_yref += (float)mousemotion->yrel; // 10.0f;
184  mouseMovement(angle_xref,angle_yref);
185 
186  if (v<1){
187  //mouse_ray_x = mouse0_down_x;
188  //mouse_ray_y = mouse0_down_y;
189  }
190  /*mouseDiffx = mouse0_down_x - mouse_ray_x;
191  mouseDiffy = mouse0_down_y - mouse_ray_y;
192  mouse_ray_x = mouse0_down_x;
193  mouse_ray_y = mouse0_down_y;*/
194 
195  //VAD = ScreenToWorld(mouse0_down_x,mouse0_down_y,0);
196  //xpos = VAD[0];ypos = VAD[1];zpos = VAD[2];
197  //VAD2 =ScreenToWorld(mouse0_down_x,mouse0_down_y,1);
198  //xpos2 = VAD2[0];ypos2 = VAD2[1];zpos2 = VAD2[2];
199 
200  //if (i<1){ray = dCreateRay(space,100*100);}
201  //Origin[0] = xpos;
202  //Origin[1] = ypos;
203  //Origin[2] = zpos;
204  //Origin[3] = 0;
205  //Direction[0] = xpos2;
206  //Direction[1] = ypos2;
207  //Direction[2] = zpos2;
208  //Direction[3] = 0;
209  //dGeomRaySet(ray, Origin[0], Origin[1], Origin[2], Direction[0], Direction[1], Direction[2]);
210  //dGeomSetPosition(ray, xpos,ypos,zpos);
211  //i++;
212  }
213  if (SDL_GetMouseState(NULL, NULL) & SDL_BUTTON(3)){// MOUSE RIGHT BUTTON
214 
215  //xdistance -= mousemotion->xrel / 10.0f;
216  //ydistance -= mousemotion->yrel / 10.0f;
217  }
218 }
219 
220 void OdeSdlSimulation::process_events(void) {
221  OdeInit& odeinit = OdeInit::get();
222  SDL_Event event;
223 
224  Uint8 * keystate = SDL_GetKeyState(NULL);
225  if(keystate[SDLK_q]){xrot += 1 * 0.4f;if (xrot >360) xrot -= 360 * 0.1f;}
226  if(keystate[SDLK_z]){xrot -= 1 * 0.4f;if (xrot < -360) xrot += 360 * 0.1f;}
227  if(keystate[SDLK_w]){yrotrad = (yrot / 180 * 3.141592654f); xrotrad = (xrot / 180 * 3.141592654f);
228  xpos += float(sin(yrotrad))* 0.005f; ;zpos -= float(cos(yrotrad))* 0.005f; ypos -= float(sin(xrotrad))* 0.005f;}
229  if(keystate[SDLK_s]){yrotrad = (yrot / 180 * 3.141592654f); xrotrad = (xrot / 180 * 3.141592654f);
230  xpos -= float(sin(yrotrad))* 0.005f;zpos += float(cos(yrotrad))* 0.005f; ;ypos += float(sin(xrotrad))* 0.005f;}
231  if (keystate[SDLK_a]){yrotrad = (yrot / 180 * 3.141592654f);xpos -= float(cos(yrotrad)) * 0.008;zpos -= float(sin(yrotrad)) * 0.008; }
232  if (keystate[SDLK_d]){yrotrad = (yrot / 180 * 3.141592654f);xpos += float(cos(yrotrad)) * 0.008;zpos += float(sin(yrotrad)) * 0.008;}
233  if(keystate[SDLK_e]){zrot += 1 * 0.4f;if (zrot >360) zrot -= 360 * 0.4f;}
234  if(keystate[SDLK_c]){zrot -= 1 * 0.4f;if (zrot < -360) zrot += 360 * 0.4f;}
235 
236  if (keystate[SDLK_f]){ypos +=1 *0.005f;}
237  if (keystate[SDLK_v]){ypos -=1 *0.005f;}
238 
239  if(keystate[SDLK_1]){initViewpoint();}
240 
241  if (keystate[SDLK_5]){
242 
243  if ((odeinit._iCub->eyeLidRot) < 0.55) odeinit._iCub->eyeLidRot += 0.01;
244  yDebug()<<odeinit._iCub->eyeLidRot;
245  }
246  if (keystate[SDLK_6]){
247  if ((odeinit._iCub->eyeLidRot) > 0.01) odeinit._iCub->eyeLidRot -= 0.01;
248  yDebug()<<odeinit._iCub->eyeLidRot;
249  }
250  if (keystate[SDLK_h])
251  {
252  odeinit.sendHomePos();
253  }
254  /* Grab all the events off the queue. */
255  while (SDL_PollEvent(&event)){
256  switch (event.type)
257  {
258  case SDL_VIDEORESIZE:
259  width = event.resize.w;
260  height = event.resize.h;
261  SDL_SetVideoMode(width,height,16,SDL_OPENGL | SDL_RESIZABLE);
262  {
263  bool ok = setup_opengl(robot_config->getFinder());
264  if (!ok) {
265  odeinit.stop = true;
266  }
267  }
268  odeinit._iCub->reinitialized = true;
269  //draw_screen( );
270  break;
271  case SDL_KEYDOWN:
272  /* Handle key presses*/
273  handle_key_down(&event.key.keysym);
274  // SDL_GetKeyName(event.key.keysym.sym));
275  break;
276  break;
277  case SDL_MOUSEMOTION:
278  handle_mouse_motion(&event.motion);
279  mouse0_down_x = event.button.x;
280  mouse0_down_y = event.button.y;
281  break;
282  case SDL_QUIT:
283  /* Handle quit requests (like Ctrl-c). */
284  odeinit.stop = true;
285  break;
286 
287  case SDL_MOUSEBUTTONDOWN:
288  handle_mouse_motion(&event.motion);
289  switch (event.button.button)
290  {
291  case SDL_BUTTON_LEFT:
292  //deleteRay = false;
293  picking = false;
294  //yDebug(" Down\n");
295  break;
296  case SDL_BUTTON_MIDDLE:
297  break;
298  case SDL_BUTTON_RIGHT:
299  break;
300  default:
301  //this is not reached
302  break;
303  }
304  break;
305  break;
306  case SDL_MOUSEBUTTONUP:
307  switch (event.button.button)
308  {
309  case SDL_BUTTON_LEFT:
310  //yDebug(" up\n");
311  v=0;
312  break;
313  case SDL_BUTTON_MIDDLE:
314  //nothing
315  break;
316  case SDL_BUTTON_RIGHT:
317  //nothing
318  break;
319  default:
320  //this is not reached either
321  break;
322  }
323  break;
324  }
325  }
326 }
327 
328 void OdeSdlSimulation::nearCallback (void *data, dGeomID o1, dGeomID o2) {
329 
330  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),
331  //the contacts generated between the robot and the ground that are always present can be ignored
332 
333  OdeInit& odeinit = OdeInit::get();
334 
335  assert(o1);
336  assert(o2);
337 
338  dSpaceID space1,space2;
339  dSpaceID superSpace1,superSpace2;
340  std::string geom1className("");
341  std::string geom2ClassName("");
342  std::string geom1name("");
343  std::string geom2name("");
344  bool geom1isiCubPart = false;
345  bool geom2isiCubPart = false;
346  bool geom1isTorsoOrArm = false;
347  bool geom2isTorsoOrArm = false;
348  int subLevel1;
349  //determine the indentation level for the printouts based on the sublevel in the hiearchy of spaces
350  string indentString("");
351  std::map<dGeomID,string>::iterator geom1namesIt;
352  std::map<dGeomID,string>::iterator geom2namesIt;
353 
354  if (dGeomIsSpace(o1)){
355  space1 = (dSpaceID)o1;
356  } else {
357  space1 = dGeomGetSpace(o1);
358  indentString = indentString + " --- "; //extra indentation level because it is a geom in that space
359  }
360  subLevel1 = dSpaceGetSublevel(space1);
361  for (int i=1;i<=subLevel1;i++){ //start from i=1, for sublevel==0 we don't add any indentation
362  indentString = indentString + " --- ";
363  }
364 
365  if (odeinit.verbosity > 3) yDebug("%s nearCallback()\n",indentString.c_str());
366 
367  if (dGeomIsSpace(o1)){
368  space1 = (dSpaceID)o1;
369  if (odeinit.verbosity > 3){
370  yDebug("%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));
371  }
372  }
373  else{ //it's a geom
374  getGeomClassName(dGeomGetClass(o1),geom1className);
375  superSpace1 = dGeomGetSpace(o1);
376  geom1namesIt = odeinit._iCub->dGeomNames.find(o1);
377  if (geom1namesIt != odeinit._iCub->dGeomNames.end()){
378  geom1name = geom1namesIt->second;
379  if (odeinit.verbosity > 3) yDebug("%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));
380  }
381  else{
382  if (odeinit.verbosity > 3) yDebug("%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));
383  }
384  }
385 
386  if (dGeomIsSpace(o2)){
387  space2 = (dSpaceID)o2;
388  if (odeinit.verbosity > 3){
389  yDebug("%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));
390  }
391  } else {
392  getGeomClassName(dGeomGetClass(o2),geom2ClassName);
393  superSpace2 = dGeomGetSpace(o2);
394  geom2namesIt = odeinit._iCub->dGeomNames.find(o2);
395  if (geom2namesIt != odeinit._iCub->dGeomNames.end()){
396  geom2name = geom2namesIt->second;
397  if (odeinit.verbosity > 3) yDebug("%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));
398  }
399  else{
400  if (odeinit.verbosity > 3) yDebug("%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));
401  }
402  }
403 
404  // if at least one of the geoms is a space, we need to go deeper -> recursive calls
405  if (dGeomIsSpace(o1) || dGeomIsSpace(o2)){
406  if (dGeomIsSpace(o1) && dGeomIsSpace(o2)){ //if both are spaces, we exclude special combinations from the checking
407  if (((space1 == odeinit._iCub->iCubHeadSpace) && (space2 == odeinit._iCub->iCubTorsoSpace)) || ((space1 == odeinit._iCub->iCubTorsoSpace) && (space2 == odeinit._iCub->iCubHeadSpace))){
408  if (odeinit.verbosity > 3) yDebug("%s Ignoring head vs. torso collision space checking.\n",indentString.c_str());
409  //these are unnecessary geoms to check, moreover 2 of these were colliding while not connected by a joint
410  }
411  else if (((space1 == odeinit._iCub->iCubLegsSpace) && (space2 == odeinit._iCub->iCubTorsoSpace)) || ((space1 == odeinit._iCub->iCubTorsoSpace) && (space2 == odeinit._iCub->iCubLegsSpace))){
412  if (odeinit.verbosity > 3) yDebug("%s Ignoring legs vs. torso collision space checking.\n",indentString.c_str());
413  //these are unnecessary geoms to check - it always check collisions of geoms connected by a joint
414  }
415  else{
416  dSpaceCollide2(o1,o2,data,&nearCallback);
417  }
418  }
419  else{
420  dSpaceCollide2(o1,o2,data,&nearCallback);
421  }
422  //}
423  //if (dGeomIsSpace(o2)){
424  // dSpaceCollide2(o2,o1,data,&nearCallback); //start the recursion from the other end
425  //}
426  return;
427  }
428  /* Note we do not want to test intersections within a space,
429  * only between spaces. Therefore, we do not call dSpaceCollide ((dSpaceID)o1, data, &nearCallback) and the same for o2 */
430 
431  /* 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 */
432 
433  dBodyID b1 = dGeomGetBody(o1);
434  dBodyID b2 = dGeomGetBody(o2);
435  if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)){
436  if (odeinit.verbosity > 3) yDebug("%s Collision ignored: the bodies of o1 and o2 are connected by a joint.\n",indentString.c_str());
437  return;
438  }
439  // list of self-collisions to ignore
440  if (selfCollisionOnIgnoreList(geom1name,geom2name)){
441  if (odeinit.verbosity > 3){
442  yDebug("%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());
443  yDebug("%s Collision ignored (ignore list).\n",indentString.c_str());
444  }
445  return;
446  }
447 
448  if (odeinit.verbosity > 3) yDebug("%s Collision candidate. Preparing contact joints.\n",indentString.c_str());
449  dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box
450  int i;
451  for (i=0; i<MAX_CONTACTS; i++) {
452  contact[i].surface.mode = dContactSlip1| dContactSlip2| dContactBounce | dContactSoftCFM;
453  contact[i].surface.mu = odeinit.contactFrictionCoefficient;
454  contact[i].surface.mu2 = odeinit.contactFrictionCoefficient;
455  contact[i].surface.bounce = 0.01;
456  contact[i].surface.bounce_vel = 0.01;
457  contact[i].surface.slip1 = (dReal)0.000001;
458  contact[i].surface.slip2 = (dReal)0.000001;
459  contact[i].surface.soft_cfm = 0.0001;
460  }
461  int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,sizeof(dContact));
462  if (numc > 0){
463  if (odeinit.verbosity > 3) yDebug("%s Collision suspect confirmed. There are %d contacts - creating joints.\n",indentString.c_str(),numc);
464  dMatrix3 RI;
465  dRSetIdentity (RI);
466  if(contact[0].geom.pos[1]>CONTACT_HEIGHT_TRESHOLD_METERS){ //non-foot contact
467  if (odeinit.verbosity > 2){
468  yDebug("%s ****** non-ground COLLISION, %d contacts - creating joints************************************************************\n",indentString.c_str(),numc);
469  yDebug("%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());
470  }
471  }
472  for (i=0; i<numc; i++) {
473  if (odeinit.verbosity > 4) yDebug("%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);
474  dJointID c = dJointCreateContact (odeinit.world,odeinit.contactgroup,contact+i);
475  dJointAttach (c,b1,b2);
476  // if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);
477  // check if the bodies are touch sensitive.
478  if (odeinit._iCub->actSkinEmul == "off"){ //this is the old implementation - hands (fingers and palm) are checked for touch
479  bool b1isTouchSensitive = isBodyTouchSensitive (b1);
480  bool b2isTouchSensitive = isBodyTouchSensitive (b2);
481  // if any of the bodies are touch sensitive...
482  if (b1isTouchSensitive || b2isTouchSensitive) {
483  // ... add a feedback structure to the contact joint.
484  if (odeinit.verbosity > 2) yDebug("%s Adding tactile feedback for emulating finger/palm skin to this one (ODE joint feedback counter: %d).\n",indentString.c_str(),nFeedbackStructs);
485  dJointSetFeedback (c, &(touchSensorFeedbacks[nFeedbackStructs]));
487  }
488  }
489  else { //whole_body_skin_emul ~ actSkinEmul is on
490  /* here we treat all bodies belonging to the icub as touch sensitive
491  * we want to know if the geom is part of the iCub - that is its superSpace is one of the iCub subspaces*/
492 
493  if ((superSpace1 == odeinit._iCub->iCubHeadSpace) || (superSpace1 == odeinit._iCub->iCubLegsSpace)){
494  geom1isiCubPart = true;
495  }
496  else if ((superSpace1==odeinit._iCub->iCubTorsoSpace) || (superSpace1==odeinit._iCub->iCubLeftArmSpace) || (superSpace1== odeinit._iCub->iCubRightArmSpace)){
497  geom1isiCubPart = true;
498  geom1isTorsoOrArm = true;
499  }
500  // || (superSpace1 == iCub){ - this should never happen here - in the self-collision mode, the iCub space contains only subspaces - no geoms directly
501 
502  if ((superSpace2 == odeinit._iCub->iCubHeadSpace) || (superSpace2 == odeinit._iCub->iCubLegsSpace)){
503  geom2isiCubPart = true;
504  }
505  else if ((superSpace2==odeinit._iCub->iCubTorsoSpace) || (superSpace2==odeinit._iCub->iCubLeftArmSpace) || (superSpace2== odeinit._iCub->iCubRightArmSpace)){
506  geom2isiCubPart = true;
507  geom2isTorsoOrArm = true;
508  }
509 
510  // 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
511  if ( geom1isTorsoOrArm || geom2isTorsoOrArm){
512  if (odeinit.verbosity > 3) yDebug("%s Adding tactile feedback for whole-body skinContact to this contact (ODE joint feedback counter: %d).\n",indentString.c_str(),nFeedbackStructs);
514  yWarning("out of contact joint feedback structures for ODE (exceeded %d) - some contact joints will not have info about forces stored\n.",MAX_DJOINT_FEEDBACKSTRUCTS);
515  }
516  else{
517  dJointSetFeedback (c, &(touchSensorFeedbacks[nFeedbackStructs]));
518  nFeedbackStructs++;
519  }
520  OdeInit::contactOnSkin_t contactOnSkin, contactOnSkin2;
521  if (geom1isiCubPart){
522  contactOnSkin.body_geom_space_id = superSpace1;
523  contactOnSkin.body_geom_id = o1;
524  contactOnSkin.body_index = 1;
525  contactOnSkin.contact_geom = contact[i].geom;
526  contactOnSkin.contact_joint = c;
527  odeinit.listOfSkinContactInfos.push_back(contactOnSkin);
528  }
529  if (geom2isiCubPart){
530  contactOnSkin2.body_geom_space_id = superSpace2;
531  contactOnSkin2.body_geom_id = o2;
532  contactOnSkin2.body_index = 2;
533  contactOnSkin2.contact_geom = contact[i].geom;
534  contactOnSkin2.contact_joint = c;
535  odeinit.listOfSkinContactInfos.push_back(contactOnSkin2);
536  }
537  }
538  else {
539  if (odeinit.verbosity > 3) yDebug("%s Ignoring skin contact - so far only arms and torso are implemented.\n",indentString.c_str());
540  }
541  } //whole_body_skin_emul ~ actSkinEmul is on
542  if (odeinit.verbosity > 3) yDebug("\n");
543  } // for numc - contacts
544  } // if (numc > 0)
545  else{
546  if (odeinit.verbosity > 3) yDebug("%s Collision suspect NOT confirmed. There were %d contacts.\n",indentString.c_str(),numc);
547  }
548 }
549 
550 
551 bool OdeSdlSimulation::selfCollisionOnIgnoreList(string geom1_string, string geom2_string)
552 {
554  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) ) ){
555  return true;
556  }
557  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) ) ){
558  return true;
559  }
560  if ( ( (geom1_string.compare("geom[2]")==0) && (geom2_string.compare("torso cover")==0) ) || ( (geom2_string.compare("geom[2]")==0) && (geom1_string.compare("torso cover")==0) ) ){
561  return true;
562  } //geom[2] is the cylinder in at shoulder joint (when it is "on" - part activated, it may collide ; when off (different geom name), it will not go into the torso, so no need to handle this)
563 
564  if ( ( (geom1_string.compare("geom[4]")==0) && (geom2_string.compare("torso cover")==0) ) || ( (geom2_string.compare("geom[4]")==0) && (geom1_string.compare("torso cover")==0) ) ){
565  return true;
566  } //geom[4] is the cylinder in upper left arm (similarly, no need to test for the version with part off (ICubSim::initLeftArmOff))
567  if ( ( (geom1_string.compare("geom[4]")==0) && (geom2_string.compare("torsoGeom[5]")==0) ) || ( (geom2_string.compare("geom[4]")==0) && (geom1_string.compare("torsoGeom[5]")==0) ) ){
568  return true;
569  } //upper arm cylinder colliding with torso box
570 
572  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) ) ){
573  return true;
574  }
575  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) ) ){
576  return true;
577  }
578  if ( ( (geom1_string.compare("geom[3]")==0) && (geom2_string.compare("torso cover")==0) ) || ( (geom2_string.compare("geom[3]")==0) && (geom1_string.compare("torso cover")==0) ) ){
579  return true;
580  } //geom[3] is the cylinder in at shoulder joint (when it is "on" - part activated, it may collide ; when off (different geom name), it will not go into the torso, so no need to handle this)
581  if ( ( (geom1_string.compare("geom[5]")==0) && (geom2_string.compare("torso cover")==0) ) || ( (geom2_string.compare("geom[5]")==0) && (geom1_string.compare("torso cover")==0) ) ){
582  return true;
583  } //geom[5] is the cylinder in upper right arm (similarly, no need to test for the version with part off (ICubSim::initRightArmOff))
584  if ( ( (geom1_string.compare("geom[5]")==0) && (geom2_string.compare("torsoGeom[5]")==0) ) || ( (geom2_string.compare("geom[5]")==0) && (geom1_string.compare("torsoGeom[5]")==0) ) ){
585  return true;
586  } //upper arm cylinder colliding with torso box
587 
589  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) ) ){
590  return true;
591  }
592  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) ) ){
593  return true;
594  }
595  if ( ( (geom1_string.compare("geom[2]")==0) && (geom2_string.compare("torso cover")==0) ) || ( (geom2_string.compare("geom[2]")==0) && (geom1_string.compare("torso cover")==0) ) ){
596  return true;
597  } //geom[2] is the cylinder in at shoulder joint (when it is "on" - part activated, it may collide ; when off (different geom name), it will not go into the torso, so no need to handle this)
598 
599  if ( ( (geom1_string.compare("geom[4]")==0) && (geom2_string.compare("torso cover")==0) ) || ( (geom2_string.compare("geom[4]")==0) && (geom1_string.compare("torso cover")==0) ) ){
600  return true;
601  } //geom[4] is the cylinder in upper left arm (similarly, no need to test for the version with part off (ICubSim::initLeftArmOff))
602  if ( ( (geom1_string.compare("geom[4]")==0) && (geom2_string.compare("torsoGeom[5]")==0) ) || ( (geom2_string.compare("geom[4]")==0) && (geom1_string.compare("torsoGeom[5]")==0) ) ){
603  return true;
604  } //upper arm cylinder colliding with torso box
605 
606 
607  return false;
608 }
609 
610 // returns true if the body with the bodyID is a touch-sensitive body, returns false otherwise.
611 bool OdeSdlSimulation::isBodyTouchSensitive (dBodyID bodyID) {
612  OdeInit& odeinit = OdeInit::get();
613 
614  // check the smaller hand parts if the left hand is active.
615  if (odeinit._iCub->actLHand == "on") {
616  if (bodyID == odeinit._iCub->body[10]) {
617  return true;
618  } else if (bodyID == odeinit._iCub->body[30]) {
619  return true;
620  } else if (bodyID == odeinit._iCub->body[24]) {
621  return true;
622  } else if (bodyID == odeinit._iCub->body[25]) {
623  return true;
624  } else if (bodyID == odeinit._iCub->lhandfingers3) {
625  return true;
626  }
627  } else { // check the whole left hand body if the hand is not active.
628  if (bodyID == odeinit._iCub->l_hand) {
629  return true;
630  }
631  }
632 
633  // check the smaller hand parts if the right hand is active.
634  if (odeinit._iCub->actRHand == "on") {
635  if (bodyID == odeinit._iCub->body[11]) {
636  return true;
637  } else if (bodyID == odeinit._iCub->body[49]) {
638  return true;
639  } else if (bodyID == odeinit._iCub->body[43]) {
640  return true;
641  } else if (bodyID == odeinit._iCub->body[44]) {
642  return true;
643  } else if (bodyID == odeinit._iCub->rhandfingers3) {
644  return true;
645  }
646  } else { // check the whole right hand body if the hand is not active.
647  if (bodyID == odeinit._iCub->r_hand) {
648  return true;
649  }
650  }
651 
652  return false;
653 }
654 
655 // this is a function to mimic the sensor data from the physical icub fingertip/palm sensors
656 //but the palm cover is not being checked here
657 void OdeSdlSimulation::inspectHandTouch_icubSensors(Bottle& reportLeft, Bottle& reportRight, bool boolean) {
658  OdeInit& odeinit = OdeInit::get();
659  reportLeft.clear();
660  reportRight.clear();
661  int indicesLeft[6] = {24, 25, 26, 27, 30, 10};
662  int indicesRight[6] = {43, 44, 45, 46, 49, 11};
663 
664  if (odeinit._iCub->actLHand == "on" && odeinit._iCub->actRHand == "on" ){
665  double resultLeft=0, resultRight = 0;
666  for (int x = 0; x < 6; x++){
667  if (boolean){
668  resultLeft = odeinit._iCub->checkTouchSensor( indicesLeft[x] );
669  resultRight = odeinit._iCub->checkTouchSensor( indicesRight[x] );
670  }
671  else{
672  resultLeft = odeinit._iCub->checkTouchSensor_continuousValued( indicesLeft[x] );
673  resultRight = odeinit._iCub->checkTouchSensor_continuousValued( indicesRight[x] );
674  }
675 
676  if (x < 5){ //five fingers
677  for (int i = 0; i < 12; i++){
678  reportLeft.addDouble(resultLeft * 255);
679  reportRight.addDouble(resultRight * 255);
680  }
681  }
682  if (x == 5){
683  for (int y = 0; y<3; y++){
684  for (int i = 0; i < 12; i++){
685  reportLeft.addDouble(0.0);
686  reportRight.addDouble(0.0);
687  }
688  }
689  //these are palm taxels
690  for (int y = 0; y<4; y++){
691  for (int i = 0; i < 12; i++){
692  reportLeft.addDouble(resultLeft * 255);
693  reportRight.addDouble(resultRight * 255);
694  }
695  }
696  for (int y = 0; y<4; y++){
697  for (int i = 0; i < 12; i++){
698  reportLeft.addDouble(0.0);
699  reportRight.addDouble(0.0);
700  }
701  }
702  }
703  }
704  }//end lhand on rhand on
705  else if (odeinit._iCub->actLHand == "on" && odeinit._iCub->actRHand == "off" ){
706  double resultLeft=0, resultRight = 0;
707  for (int x = 0; x < 6; x++){
708  if (boolean){
709  resultLeft = odeinit._iCub->checkTouchSensor( indicesLeft[x] );
710  resultRight = odeinit._iCub->checkTouchSensor( odeinit._iCub->r_hand );
711  }
712  else{
713  resultLeft = odeinit._iCub->checkTouchSensor_continuousValued( indicesLeft[x] );
714  resultRight = odeinit._iCub->checkTouchSensor_continuousValued(odeinit._iCub->r_hand);
715  }
716  if (x < 5){
717  for (int i = 0; i < 12; i++){
718  reportLeft.addDouble(resultLeft * 255);
719  reportRight.addDouble(resultRight * 255);
720  }
721  }
722  if (x == 5){
723  for (int y = 0; y<3; y++){
724  for (int i = 0; i < 12; i++){
725  reportLeft.addDouble(0.0);
726  reportRight.addDouble(0.0);
727  }
728  }
729  for (int y = 0; y<4; y++){
730  for (int i = 0; i < 12; i++){
731  reportLeft.addDouble(resultLeft * 255);
732  reportRight.addDouble(resultRight * 255);
733  }
734  }
735  for (int y = 0; y<4; y++){
736  for (int i = 0; i < 12; i++){
737  reportLeft.addDouble(0.0);
738  reportRight.addDouble(0.0);
739  }
740  }
741  }
742  }
743  }//end lhand on rhand off
744  else if (odeinit._iCub->actRHand == "on" && odeinit._iCub->actLHand == "off" ){
745  double resultLeft=0, resultRight = 0;
746  for (int x = 0; x < 6; x++){
747  if (boolean){
748  resultLeft = odeinit._iCub->checkTouchSensor( odeinit._iCub->l_hand );
749  resultRight = odeinit._iCub->checkTouchSensor( indicesRight[x] );
750  }
751  else{
752  resultLeft = odeinit._iCub->checkTouchSensor_continuousValued( odeinit._iCub->l_hand );
753  resultRight = odeinit._iCub->checkTouchSensor_continuousValued( indicesRight[x] );
754  }
755 
756  if (x < 5){
757  for (int i = 0; i < 12; i++){
758  reportLeft.addDouble(resultLeft * 255);
759  reportRight.addDouble(resultRight * 255);
760  }
761  }
762  if (x == 5){
763  for (int y = 0; y<3; y++){
764  for (int i = 0; i < 12; i++){
765  reportLeft.addDouble(0.0);
766  reportRight.addDouble(0.0);
767  }
768  }
769  for (int y = 0; y<4; y++){
770  for (int i = 0; i < 12; i++){
771  reportLeft.addDouble(resultLeft * 255);
772  reportRight.addDouble(resultRight * 255);
773  }
774  }
775  for (int y = 0; y<4; y++){
776  for (int i = 0; i < 12; i++){
777  reportLeft.addDouble(0.0);
778  reportRight.addDouble(0.0);
779  }
780  }
781  }
782  }
783  }//end lhand off rhand on
784  else{//both off
785  for (int x = 0; x < 6; x++){
786  double resultLeft=0, resultRight = 0;
787  if (boolean){
788  resultLeft = odeinit._iCub->checkTouchSensor( odeinit._iCub->l_hand );
789  resultRight = odeinit._iCub->checkTouchSensor( odeinit._iCub->r_hand );
790  }
791  else{
792  resultLeft = odeinit._iCub->checkTouchSensor_continuousValued( odeinit._iCub->l_hand );
793  resultRight = odeinit._iCub->checkTouchSensor_continuousValued(odeinit._iCub->r_hand);
794  }
795 
796  if (x < 5){
797  for (int i = 0; i < 12; i++){
798  reportLeft.addDouble(resultLeft * 255);
799  reportRight.addDouble(resultRight * 255);
800  }
801  }
802  if (x == 5){
803  for (int y = 0; y<3; y++){
804  for (int i = 0; i < 12; i++){
805  reportLeft.addDouble(0.0);
806  reportRight.addDouble(0.0);
807  }
808  }
809  for (int y = 0; y<4; y++){
810  for (int i = 0; i < 12; i++){
811  reportLeft.addDouble(resultLeft * 255);
812  reportRight.addDouble(resultRight * 255);
813  }
814  }
815  for (int y = 0; y<4; y++){
816  for (int i = 0; i < 12; i++){
817  reportLeft.addDouble(0.0);
818  reportRight.addDouble(0.0);
819  }
820  }
821  }
822  }
823  }//end both off
824 }
825 
826 
827 void OdeSdlSimulation::getAngles(const dReal *m, float& z, float& y, float& x) {
828  const dReal eps = 0.00001;
829 
830  y = -asin(m[2]);
831  float c = cos(y);
832 
833  if (fabs(c)>eps) {
834  x = atan2(-m[6]/c,m[10]/c);
835  z = atan2(-m[1]/c,m[0]/c);
836  } else {
837  x = 0;
838  z = -atan2(m[4],m[5]);
839  }
840  x *= -180/M_PI;
841  y *= 180/M_PI;
842  z *= 180/M_PI;
843 }
844 
845 void OdeSdlSimulation::initViewpoint() {
846  xpos = 0;
847  ypos = 1;
848  zpos = 1;
849  xrot = 25;
850  yrot = 0;
851  zrot = 0;
852 }
853 
854 void OdeSdlSimulation::mouseMovement(float x, float y) {
855  float diffx = x-lastx; //check the difference between the current x and the last x position
856  float diffy = y-lasty; //check the difference between the current y and the last y position
857  lastx =x; //set lastx to the current x position
858  lasty =y; //set lasty to the current y position
859  xrot += (float) diffy; //set the xrot to xrot with the addition of the difference in the y position
860  yrot += (float) diffx; //set the xrot to yrot with the addition of the difference in the x position
861 }
862 
863 void OdeSdlSimulation::draw_screen() {
864  OdeInit& odeinit = OdeInit::get();
865 
866  glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT); // refresh opengl
867 
868  if (extractImages || odeinit._iCub->actVision == "on"){
869  robot_streamer->sendVision();
870  }
871 
872  glViewport(0,0,width,height);
873  glMatrixMode (GL_PROJECTION);
874  glLoadIdentity();
875  gluPerspective( 75, (float)width/height, 0.01, 100.0 );
876  glMatrixMode (GL_MODELVIEW);
877  glLoadIdentity();
878  glLightfv(GL_LIGHT0, GL_POSITION, light_position);
879  glRotatef (xrot, 1,0,0);
880  glRotatef (yrot, 0,1,0);
881  glRotatef (zrot, 0,0,1);
882  glTranslated(-xpos,-ypos,-zpos);
883 
884  // set up any video textures
885 
886  if (video!=0)
887  DrawVideo(video);
888 
889  //draw the ground
890  glColor3d(0.5,0.5,1);
891  glPushMatrix();
892  glRotatef(90.0,1,0,0);
893  glRotatef(180.0,0,1,0);
894  DrawGround(false);
895  glPopMatrix();
896  glDisable(GL_TEXTURE_2D);
897  draw();
898  glEnable(GL_TEXTURE_2D);
899  drawSkyDome(0,0,0,50,50,50); // Draw the Skybox
900  SDL_GL_SwapBuffers();// Swap Buffers
901 }
902 
903 
904 
905 void OdeSdlSimulation::retreiveInertialData(Bottle& inertialReport) {
906  OdeInit& odeinit = OdeInit::get();
907  static dReal OldLinearVel[3], LinearVel[3], LinearAccel[3];
908  inertialReport.clear();
909 
910  //get euler angles from quaternions
911  dQuaternion angles;
912  dGeomGetQuaternion( odeinit._iCub->inertialGeom, angles );
913  dReal w, x, y, z;
914  w = angles[0];
915  x = angles[1];
916  y = angles[2];
917  z = angles[3];
918 
919  double sqw = w * w;
920  double sqx = x * x;
921  double sqy = y * y;
922  double sqz = z * z;
923  float roll, pitch, yaw;
924 
925  double unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
926  double test = x*y + z*w;
927  if (test > 0.499*unit) { // singularity at north pole
928  roll = 2 * atan2(x,w);
929  pitch = M_PI/2;
930  yaw = 0;
931  return;
932  }
933  if (test < -0.499*unit) { // singularity at south pole
934  roll = -2 * atan2(x,w);
935  pitch = -M_PI/2;
936  yaw = 0;
937  return;
938  }
939  roll =(float) ( atan2(2.0*y*w-2*x*z , sqx - sqy - sqz + sqw) ); //z
940  pitch = (float) (atan2(2.0*x*w-2*y*z , -sqx + sqy - sqz + sqw) );//x
941  yaw = asin(2*test/unit);//y
942 
943  //roll = dBodyGetRotation(odeinit._iCub->head)[4]; // was 1
944  //pitch = dBodyGetRotation(odeinit._iCub->head)[6];
945  //yaw = dBodyGetRotation(odeinit._iCub->head)[2];
946 
947  //Add Euler angles roll pitch yaw
948  inertialReport.addDouble( -yaw * 180/M_PI);// yaw
949  inertialReport.addDouble( -pitch * 180/M_PI);// pitch
950  inertialReport.addDouble( roll * 180/M_PI);// roll
951 
952  /*//in order to calculate linear acceleration (make sure of body) Inertial Measurement Unit IMU
953  LinearVel[0] = dBodyGetLinearVel(odeinit._iCub->inertialBody)[0];
954  LinearVel[1] = dBodyGetLinearVel(odeinit._iCub->inertialBody)[1];
955  LinearVel[2] = dBodyGetLinearVel(odeinit._iCub->inertialBody)[2];
957  LinearAccel[0] = ( LinearVel[0] - OldLinearVel[0] ) / 0.02;
958  LinearAccel[1] = ( LinearVel[1] - OldLinearVel[1] ) / 0.02;
959  LinearAccel[2] = ( LinearVel[2] - OldLinearVel[2] ) / 0.02;
960  OldLinearVel[0] = LinearVel[0];
961  OldLinearVel[1] = LinearVel[1];
962  OldLinearVel[2] = LinearVel[2];*/
963 
965  Vector grav,grav1,grav2,grav3;
966  grav.resize(3);
967  grav1.resize(3);
968  grav2.resize(3);
969  grav3.resize(3);
970  double theta;
971 
972  grav[0]=0;
973  grav[1]=0;
974  grav[2]=9.81;
975 
976  theta = pitch;
977  grav1[0]=grav[0]*cos(theta)+grav[2]*sin(theta);
978  grav1[1]=grav[1];
979  grav1[2]=grav[0]*(-sin(theta))+grav[2]*cos(theta);
980 
981  theta = yaw;
982  grav2[0]=grav1[0];
983  grav2[1]=grav1[1]*cos(theta)+grav1[2]*(-sin(theta));
984  grav2[2]=grav1[1]*sin(theta)+grav1[2]*cos(theta);
985 
986  theta = roll;
987  grav3[0]=grav2[0]*cos(theta)+grav2[1]*(-sin(theta));
988  grav3[1]=grav2[0]*sin(theta)+grav2[1]*cos(theta);
989  grav3[2]=grav2[2];
990 
991  inertialReport.addDouble( grav3[0] );
992  inertialReport.addDouble( grav3[1] );
993  inertialReport.addDouble( grav3[2] );
994 
995  //Add angular velocity
996  inertialReport.addDouble(-dBodyGetAngularVel(odeinit._iCub->inertialBody)[2]*CTRL_RAD2DEG);
997  inertialReport.addDouble(-dBodyGetAngularVel(odeinit._iCub->inertialBody)[0]*CTRL_RAD2DEG);
998  inertialReport.addDouble( dBodyGetAngularVel(odeinit._iCub->inertialBody)[1]*CTRL_RAD2DEG);
999 
1000  //Add magnetic fields
1001  inertialReport.addDouble(0.0);
1002  inertialReport.addDouble(0.0);
1003  inertialReport.addDouble(0.0);
1004 }
1005 
1006 int OdeSdlSimulation::thread_ode(void *unused) {
1007  //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
1008  double cpms = 1e3 / CLOCKS_PER_SEC;
1009  long lastOdeProcess = (long) (clock()*cpms);
1010  double avg_ode_step_length = 0.0;
1011  long count = 0;
1012  simrun = true;
1013  double timeCache = ode_step_length;
1014  long lastTimeCacheUpdate = (long) (clock()*cpms);
1015  double alpha = 0.99;
1016  // if realTime=true when delays occur the simulation tries to recover by running more steps in a row
1017  // if realTime=false the simulation executes the simulation steps with a fixed rate irregardless of delays
1018  bool realTime = true;
1019  long temp;
1020 
1021  while (simrun) {
1022  temp = (long) (clock()*cpms);
1023  timeCache += temp - lastTimeCacheUpdate;
1024  lastTimeCacheUpdate = temp;
1025  while(timeCache < ode_step_length){
1026  SDL_Delay((unsigned int)(ode_step_length-timeCache));
1027  temp = (long) (clock()*cpms);
1028  timeCache += temp - lastTimeCacheUpdate;
1029  lastTimeCacheUpdate = temp;
1030  }
1031 
1032  /*if(timeCache >= 2.0*ode_step_length)
1033  yWarning("Simulation delay: running %d steps in a row to recover.\n", (int)(timeCache/ode_step_length));*/
1034 
1035  while(timeCache >= ode_step_length){
1036  count++;
1037  lastOdeProcess = (long) (clock()*cpms);
1038  ODE_process(1, (void*)1);
1039  avg_ode_step_length = alpha*avg_ode_step_length + (1.0-alpha)*((long) (clock()*cpms) -lastOdeProcess);
1040 
1041  if(realTime)
1042  timeCache -= ode_step_length;
1043  else
1044  timeCache = 0.0;
1045 
1046  // check if the desired timestep is achieved, if not, print a warning msg
1047  if(count % (10000/ode_step_length)==0){
1048  if(avg_ode_step_length >= ode_step_length+1)
1049  yWarning("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",
1050  ode_step_length, avg_ode_step_length);
1051  else if(avg_ode_step_length <= ode_step_length-1)
1052  yWarning("you could get a more accurate dynamics simulation by decreasing the timestep in ode_params.ini (current value: %ld, suggested value: %.0f)\n",
1053  ode_step_length, avg_ode_step_length);
1054  }
1055  }
1056  }
1057  return(0);
1058 }
1059 
1060 Uint32 OdeSdlSimulation::ODE_process(Uint32 interval, void *param) {
1061  OdeInit& odeinit = OdeInit::get();
1062  //static clock_t startTimeODE= clock(), finishTimeODE= clock();
1063  //startTimeODE = clock();
1064 
1065  odeinit.mutex.wait();
1066  nFeedbackStructs=0;
1067 
1068  if (odeinit.verbosity > 3) yDebug("\n ***info code collision detection ***");
1069  if (odeinit.verbosity > 3) yDebug("OdeSdlSimulation::ODE_process: dSpaceCollide(odeinit.space,0,&nearCallback): will test iCub space against the rest of the world (e.g. ground).\n");
1070  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
1071  if (odeinit._iCub->actSelfCol == "on"){
1073  if (odeinit.verbosity > 3){
1074  yDebug("OdeSdlSimulation::ODE_process: dSpaceCollide(odeinit._iCub->iCub,0,&nearCallback): will test iCub subspaces against each other.");
1075  }
1076  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
1077  }
1078  }
1079  if (odeinit.verbosity > 3) yDebug("***END OF info code collision detection\n ***");
1080 
1081  dWorldStep(odeinit.world, dstep);
1082  // do 1 TIMESTEP in controllers (ok to run at same rate as ODE: 1 iteration takes about 300 times less computation time than dWorldStep)
1083  for (int ipart = 0; ipart<MAX_PART; ipart++) {
1084  if (odeinit._controls[ipart] != NULL) {
1085  odeinit._controls[ipart]->jointStep();
1086  }
1087  }
1088  odeinit.sync = true;
1089  odeinit.mutex.post();
1090 
1091  if (odeinit._iCub->actSkinEmul == "off"){
1092  if ( robot_streamer->shouldSendTouchLeftHand() || robot_streamer->shouldSendTouchRightHand() ) {
1093  Bottle reportLeft;
1094  Bottle reportRight;
1095  bool boolean = true;
1096  if (odeinit._iCub->actPressure == "on"){
1097  boolean = false;
1098  }
1099  inspectHandTouch_icubSensors(reportLeft, reportRight, boolean);//inspectBodyTouch_continuousValued(report);
1100 
1101  if ( robot_streamer->shouldSendTouchLeftHand() )
1102  robot_streamer->sendTouchLeftHand( reportLeft );
1103 
1104  if ( robot_streamer->shouldSendTouchRightHand() )
1105  robot_streamer->sendTouchRightHand( reportRight );
1106  }
1107  }
1108  else{ // actSkinEmul == "on"
1109  if(robot_streamer->shouldSendSkinEvents() || (robot_streamer->shouldSendTouchLeftHand() || robot_streamer->shouldSendTouchRightHand() ||
1110  robot_streamer->shouldSendTouchLeftArm() || robot_streamer->shouldSendTouchLeftForearm() ||
1111  robot_streamer->shouldSendTouchRightArm() || robot_streamer->shouldSendTouchRightForearm() ||
1112  robot_streamer->shouldSendTouchTorso())){
1113  if (! odeinit.listOfSkinContactInfos.empty()){ //if someone is reading AND there are contacts to process
1114  if (odeinit.verbosity > 2) yDebug("OdeSdlSimulation::ODE_process():There were %lu iCub collisions to process.", odeinit.listOfSkinContactInfos.size());
1115  inspectWholeBodyContactsAndSendTouch();
1116  }
1117  else{ //someone is reading but no contacts, we send empty lists
1118  if(robot_streamer->shouldSendSkinEvents()){
1119  skinContactList emptySkinContactList;
1120  emptySkinContactList.clear();
1121  robot_streamer->sendSkinEvents(emptySkinContactList);
1122  }
1123  if(robot_streamer->shouldSendTouchLeftHand()){
1124  Bottle bottleLeftHand = Bottle(odeinit._iCub->emptySkinActivationHand);
1125  robot_streamer->sendTouchLeftHand(bottleLeftHand);
1126  }
1127  if(robot_streamer->shouldSendTouchRightHand()){
1128  Bottle bottleRightHand = Bottle(odeinit._iCub->emptySkinActivationHand);
1129  robot_streamer->sendTouchRightHand(bottleRightHand);
1130  }
1131  if(robot_streamer->shouldSendTouchLeftArm()){
1132  Bottle bottleLeftArm = Bottle(odeinit._iCub->emptySkinActivationUpperArm);
1133  robot_streamer->sendTouchLeftArm(bottleLeftArm);
1134  }
1135  if(robot_streamer->shouldSendTouchLeftForearm()){
1136  Bottle bottleLeftForearm = Bottle(odeinit._iCub->emptySkinActivationForearm);
1137  robot_streamer->sendTouchLeftForearm(bottleLeftForearm);
1138  }
1139  if(robot_streamer->shouldSendTouchRightArm()){
1140  Bottle bottleRightArm = Bottle(odeinit._iCub->emptySkinActivationUpperArm);
1141  robot_streamer->sendTouchRightArm(bottleRightArm);
1142  }
1143  if(robot_streamer->shouldSendTouchRightForearm()){
1144  Bottle bottleRightForearm = Bottle(odeinit._iCub->emptySkinActivationForearm);
1145  robot_streamer->sendTouchRightForearm(bottleRightForearm);
1146  }
1147  if(robot_streamer->shouldSendTouchTorso()){
1148  Bottle bottleTorso = Bottle(odeinit._iCub->emptySkinActivationTorso);
1149  robot_streamer->sendTouchTorso(bottleTorso);
1150  }
1151  }
1152  }
1153  odeinit.listOfSkinContactInfos.clear();
1154  if(odeinit.verbosity > 4){
1155  yDebug("contactICubSkinEmulMap before resetting:");
1156  printContactICubSkinEmulMap();
1157  }
1158  resetContactICubSkinEmulMap();
1159  if(odeinit.verbosity > 4){
1160  yDebug("contactICubSkinEmulMap after resetting:");
1161  printContactICubSkinEmulMap();
1162  }
1163  }
1164 
1165  dJointGroupEmpty (odeinit.contactgroup);
1166 
1167  if (robot_streamer->shouldSendInertial()) {
1168  Bottle inertialReport;
1169  retreiveInertialData(inertialReport);
1170  robot_streamer->sendInertial(inertialReport);
1171  }
1172 
1173  //go and check if torques are needed
1174  robot_streamer->checkTorques();
1175 
1176  odeinit._iCub->setJointControlAction();
1177 
1178  //finishTimeODE = clock() ;
1179  //SPS();
1180  //yDebug("ODE=%lf\n",(double)(finishTimeODE - startTimeODE) / CLOCKS_PER_SEC);
1181  return(interval);
1182 }
1183 
1184 
1185 /*int OdeSdlSimulation::thread_func(void *unused) {
1186  // this needs to be kept synchronized with the timestep in
1187  // dWorldStep, in order to get correct world clock time
1188  // --paulfitz
1189  int delay = 50;
1190  id = SDL_AddTimer( delay, &OdeSdlSimulation::ODE_process, (void*)1);
1191 
1192  return(0);
1193 }
1194 */
1195 /*
1196  static void SPS()
1197  {
1198  static float sps = 0.0f;
1199  static float previousTime = 0.0f;
1200  static int currentsps;
1201  static char strSPS[60] = {0};
1202 
1203  float currentTime = (GetTickCount() * 0.001f);
1204 
1205  ++sps; // Increment the SPS counter
1206 
1207  if( currentTime - previousTime > 1.0f ){
1208  previousTime = currentTime;
1209  currentsps = int(sps);
1210  yDebug("current SPS: %d\n",currentsps);
1211  sps = 0.0f;
1212  }
1213  }
1214 */
1215 
1216 void OdeSdlSimulation::sighandler(int sig) {
1217  OdeInit& odeinit = OdeInit::get();
1218  odeinit.stop = true;
1219  yInfo() << "\nCAUGHT Ctrl-c";
1220 }
1221 
1222 void OdeSdlSimulation::simLoop(int h,int w) {
1223  yDebug("***** OdeSdlSimulation::simLoop \n");
1224  OdeInit& odeinit = OdeInit::get();
1225 
1226  SDL_Init(SDL_INIT_TIMER | SDL_GL_ACCELERATED_VISUAL);
1227  SDL_SetVideoMode(h,w,32,SDL_OPENGL | SDL_RESIZABLE);// | SDL_SWSURFACE| SDL_ANYFORMAT); // on init
1228 
1229  dAllocateODEDataForThread(dAllocateMaskAll);
1230  ConstString logo = robot_config->getFinder().findFile("logo");
1231 
1232  image = SDL_LoadBMP(robot_config->getFinder().findFile(logo.c_str()).c_str());
1233  SDL_WM_SetIcon(image,0);
1234  SDL_FreeSurface(image);
1235  SDL_WM_SetCaption("iCub Simulator", "image");
1236 
1237  //SDL_Thread *thread;
1238  SDL_Thread *ode_thread = SDL_CreateThread(thread_ode, NULL);
1239  //thread = SDL_CreateThread(thread_func, NULL);
1240 
1241  if ( ode_thread == NULL ) {
1242  yError("Unable to create thread: %s\n", SDL_GetError());
1243  return;
1244  }
1245 
1246  initViewpoint();
1247  bool ok = setup_opengl(robot_config->getFinder());
1248  if (!ok) return;
1249  startTime = (long) clock();
1250  odeinit.stop = false;
1251 
1252  yarp::os::signal(yarp::os::YARP_SIGINT, sighandler);
1253  yarp::os::signal(yarp::os::YARP_SIGTERM, sighandler);
1254 
1255  glrun = true;
1256  odeinit._wrld->WAITLOADING = false;
1257  odeinit._wrld->static_model = false;
1258  long prevTime = (long) clock();
1259  long timeLeft;
1260 
1261  if (odeinit._iCub->actStartHomePos == "on"){
1262  odeinit.sendHomePos();
1263  }
1264  if (odeinit._iCub->actSelfCol == "on") {
1265  if (odeinit._iCub->actStartHomePos == "on"){
1266  Time::delay(2.0); //we want to set this trigger on only after the robot is in home pos -
1267  //it's initial configuration is with arms inside the thighs - generating many self-collisions
1269  }
1270  else{
1271  yWarning("the robot is not starting from HomePos and self-collision mode is on. The initial posture is already self-colliding.\n");
1273  }
1274  }
1275 
1276  while(!odeinit.stop) {
1277  /* Process incoming events. */
1278  process_events();
1279  /* Draw the screen. */
1280  if ( !odeinit._wrld->WAITLOADING ){
1281  if (glrun) {
1282  odeinit.mutexTexture.wait();
1283  draw_screen();
1284  odeinit.mutexTexture.post();
1285  // check for framerate
1286  timeLeft = (prevTime - (long) clock()) + gl_frame_length;
1287  //yDebug() << "check for framerate " << timeLeft;
1288  if (timeLeft > 0)
1289  { // if there is still time left in this frame, just wait
1290  SDL_Delay(timeLeft);
1291  }
1292  prevTime = (long) clock();
1293  } else {
1294  SDL_Delay(100);
1295  }
1296  }
1297  else{
1298  glFinish();
1299  glFlush();
1300  //make sure it can also be done for static objects
1301  if (odeinit._wrld->static_model){
1302  odeinit._wrld->loadTexture(odeinit._wrld->texture, odeinit._wrld->s_modelTexture[odeinit._wrld->s_MODEL_NUM-1]);
1303  }else{
1304  odeinit._wrld->loadTexture(odeinit._wrld->texture, odeinit._wrld->modelTexture[odeinit._wrld->MODEL_NUM-1]);
1305  }
1306  odeinit._wrld->WAITLOADING = false;
1307  odeinit._wrld->static_model = false;
1308  }
1309  }
1310  yInfo("Stopping SDL and ODE threads...");
1311  //stop the timer
1312  //SDL_RemoveTimer(id);
1313  //Stop the thread
1314  //SDL_KillThread( thread );
1315  simrun = false;
1316  //SDL_WaitThread( thread, NULL );
1317  SDL_WaitThread( ode_thread, NULL );
1318  //SDL_Quit();
1319 }
1320 
1321 void OdeSdlSimulation::drawView(bool left, bool right, bool wide) {
1322  OdeInit& odeinit = OdeInit::get();
1323  const dReal *pos;
1324  const dReal *rot;
1325  glViewport(0,0,cameraSizeWidth,cameraSizeHeight);
1326  glMatrixMode (GL_PROJECTION);
1327 
1328  if (left){
1329  glLoadIdentity();
1330  gluPerspective( fov_left, (float) width_left/height_left, 0.04, 100.0 );
1331  pos = dGeomGetPosition(odeinit._iCub->Leye1_geom);
1332  rot = dGeomGetRotation(odeinit._iCub->Leye1_geom);
1333  glMatrixMode (GL_MODELVIEW);
1334  glLoadIdentity();
1335  glLightfv(GL_LIGHT0, GL_POSITION, light_position);
1336  gluLookAt(
1337  pos[0],
1338  pos[1],
1339  pos[2],
1340  pos[0] + rot[2],
1341  pos[1] + rot[6],
1342  pos[2] + rot[10],
1343  -rot[4], 1, 0
1344  );
1345  }
1346  if (right){
1347  glLoadIdentity();
1348  gluPerspective( fov_right, (float) width_right/height_right, 0.04, 100.0 );//55.8
1349  pos = dGeomGetPosition(odeinit._iCub->Reye1_geom);
1350  rot = dGeomGetRotation(odeinit._iCub->Reye1_geom);
1351  glMatrixMode (GL_MODELVIEW);
1352  glLoadIdentity();
1353  glLightfv(GL_LIGHT0, GL_POSITION, light_position);
1354  gluLookAt(
1355  pos[0],
1356  pos[1],
1357  pos[2],
1358  pos[0] + rot[2],
1359  pos[1] + rot[6],
1360  pos[2] + rot[10],
1361  -rot[4], 1, 0
1362  );
1363  }
1364  if (wide){
1365  glLoadIdentity();
1366  gluPerspective( 55.8, (float) cameraSizeWidth/cameraSizeHeight, 0.04, 100.0 );//here nothing to do with cameras
1367  glMatrixMode (GL_MODELVIEW);
1368  glLoadIdentity();
1369  glLightfv(GL_LIGHT0, GL_POSITION, light_position);
1370  glRotatef (xrot, 1,0,0);
1371  glRotatef (yrot, 0,1,0);
1372  glTranslated(-xpos,-ypos,-zpos);
1373  }
1374 
1375  //draw the ground
1376  glColor3d(0.5,0.5,1);
1377  glEnable(GL_TEXTURE_2D);
1378  glPushMatrix();
1379  glRotatef(90.0,1,0,0);
1380  glRotatef(180.0,0,1,0);
1381  DrawGround(false);
1382  glPopMatrix();
1383  glDisable(GL_TEXTURE_2D);
1384  draw();//robot
1385  glEnable(GL_TEXTURE_2D);
1386  drawSkyDome(0,0,0,50,50,50); // Draw the Skybox
1387 }
1388 
1390  glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT); // refresh opengl
1391 }
1392 
1394 }
1395 
1397  RobotConfig *config) {
1398  OdeInit& odeinit = OdeInit::get();
1399  if (video!=NULL) {
1400  yError("Only one Simulation object allowed\n");
1401  yarp::os::exit(1);
1402  }
1403  robot_streamer = streamer;
1404  robot_config = config;
1405 
1406  ode_step_length = config->getWorldTimestep();
1407  dstep = ode_step_length*1e-3;
1408 
1409  video = new VideoTexture;
1410  string moduleName = odeinit.getName();
1411  video->setName( moduleName );
1412  odeinit._iCub->eyeLidsPortName = moduleName;
1413  Property options;
1414 
1415  //get the camera calibration parameters
1416  string camcalib_context=robot_config->getFinder().check("camcalib_context",
1417  Value("cameraCalibration")).asString().c_str();
1418  string camcalib_file=robot_config->getFinder().check("camcalib_file",
1419  Value("icubSimEyes.ini")).asString().c_str();
1420 
1421  ResourceFinder rf_camcalib;
1422  rf_camcalib.setVerbose();
1423  rf_camcalib.setDefaultContext(camcalib_context.c_str());
1424  rf_camcalib.setDefaultConfigFile(camcalib_file.c_str());
1425  rf_camcalib.configure(0,NULL);
1426 
1427  //left
1428  Bottle &bCalibLeft=rf_camcalib.findGroup("CAMERA_CALIBRATION_LEFT");
1429  width_left=bCalibLeft.check("w",Value(320)).asInt();
1430  height_left=bCalibLeft.check("h",Value(240)).asInt();
1431 
1434 
1435  double focal_length_left=bCalibLeft.check("fy",Value(257.34)).asDouble();
1436  fov_left=2*atan2((double)height_left,2*focal_length_left)*180.0/M_PI;
1437 
1438  //right
1439  Bottle &bCalibRight=rf_camcalib.findGroup("CAMERA_CALIBRATION_RIGHT");
1440  width_right=bCalibRight.check("w",Value(320)).asInt();
1441  height_right=bCalibRight.check("h",Value(240)).asInt();
1442 
1443  double focal_length_right=bCalibRight.check("fy",Value(257.34)).asDouble();
1444  fov_right=2*atan2((double)height_right,2*focal_length_right)*180.0/M_PI;
1445  //--------------------------------------//
1446 
1447 
1448  ConstString videoconf = robot_config->getFinder().findFile("video");
1449  options.fromConfigFile(videoconf.c_str());
1450 
1451  Bottle textures = *options.find("textures").asList();
1452  for (int i=0; i<textures.size(); i++) {
1453  ConstString name = textures.get(i).asString();
1454  yInfo("Adding video texture %s\n", name.c_str());
1455  video->add(options.findGroup(name.c_str()));
1456  }
1457 
1458  initContactICubSkinEmulMap();
1459 
1460 };
1461 
1462 void OdeSdlSimulation::initContactICubSkinEmulMap(void)
1463 {
1464 
1465  contactICubSkinEmul_t skin_emul_struct;
1466 
1467  //SKIN_LEFT_HAND
1468  skin_emul_struct.coverTouched = false; //for the hand, this comprises also fingertips - they are treated like covers
1469  skin_emul_struct.indivTaxelResolution = true;
1470  contactICubSkinEmulMap[SKIN_LEFT_HAND]=skin_emul_struct;
1471 
1472  //SKIN_LEFT_FOREARM
1473  skin_emul_struct.coverTouched = false;
1474  skin_emul_struct.indivTaxelResolution = true;
1475  contactICubSkinEmulMap[SKIN_LEFT_FOREARM]=skin_emul_struct;
1476 
1477  //SKIN_LEFT_UPPER_ARM
1478  skin_emul_struct.coverTouched = false;
1479  skin_emul_struct.indivTaxelResolution = false;
1480  contactICubSkinEmulMap[SKIN_LEFT_UPPER_ARM]=skin_emul_struct;
1481 
1482  //SKIN_RIGHT_HAND
1483  skin_emul_struct.coverTouched = false; //for the hand, this comprises also fingertips - they are treated like covers
1484  skin_emul_struct.indivTaxelResolution = true;
1485  contactICubSkinEmulMap[SKIN_RIGHT_HAND]=skin_emul_struct;
1486 
1487  //SKIN_RIGHT_FOREARM
1488  skin_emul_struct.coverTouched = false;
1489  skin_emul_struct.indivTaxelResolution = true;
1490  contactICubSkinEmulMap[SKIN_RIGHT_FOREARM]=skin_emul_struct;
1491 
1492  //SKIN_RIGHT_UPPER_ARM
1493  skin_emul_struct.coverTouched = false;
1494  skin_emul_struct.indivTaxelResolution = false;
1495  contactICubSkinEmulMap[SKIN_RIGHT_UPPER_ARM]=skin_emul_struct;
1496 
1497  //SKIN_FRONT_TORSO
1498  skin_emul_struct.coverTouched = false;
1499  skin_emul_struct.indivTaxelResolution = false;
1500  contactICubSkinEmulMap[SKIN_FRONT_TORSO]=skin_emul_struct;
1501 
1502  //LEFT_LEG_UPPER
1503  skin_emul_struct.coverTouched = false;
1504  skin_emul_struct.indivTaxelResolution = false;
1505  contactICubSkinEmulMap[LEFT_LEG_UPPER]=skin_emul_struct;
1506 
1507  //LEFT_LEG_LOWER
1508  skin_emul_struct.coverTouched = false;
1509  skin_emul_struct.indivTaxelResolution = false;
1510  contactICubSkinEmulMap[LEFT_LEG_LOWER]=skin_emul_struct;
1511 
1512  //LEFT_FOOT
1513  skin_emul_struct.coverTouched = false;
1514  skin_emul_struct.indivTaxelResolution = false;
1515  contactICubSkinEmulMap[LEFT_FOOT]=skin_emul_struct;
1516 
1517  //RIGHT_LEG_UPPER
1518  skin_emul_struct.coverTouched = false;
1519  skin_emul_struct.indivTaxelResolution = false;
1520  contactICubSkinEmulMap[RIGHT_LEG_UPPER]=skin_emul_struct;
1521 
1522  //RIGHT_LEG_LOWER
1523  skin_emul_struct.coverTouched = false;
1524  skin_emul_struct.indivTaxelResolution = false;
1525  contactICubSkinEmulMap[RIGHT_LEG_LOWER]=skin_emul_struct;
1526 
1527  //RIGHT_FOOT
1528  skin_emul_struct.coverTouched = false;
1529  skin_emul_struct.indivTaxelResolution = false;
1530  contactICubSkinEmulMap[RIGHT_FOOT]=skin_emul_struct;
1531 
1532 
1533 }
1534 
1535 void OdeSdlSimulation::resetContactICubSkinEmulMap(void)
1536 {
1537 
1538  //SKIN_LEFT_HAND
1539  contactICubSkinEmulMap[SKIN_LEFT_HAND].coverTouched=false;
1540  contactICubSkinEmulMap[SKIN_LEFT_HAND].taxelsTouched.clear();
1541 
1542  //SKIN_LEFT_FOREARM
1543  contactICubSkinEmulMap[SKIN_LEFT_FOREARM].coverTouched=false;
1544  contactICubSkinEmulMap[SKIN_LEFT_FOREARM].taxelsTouched.clear();
1545 
1546  //SKIN_LEFT_UPPER_ARM
1547  contactICubSkinEmulMap[SKIN_LEFT_UPPER_ARM].coverTouched=false;
1548  contactICubSkinEmulMap[SKIN_LEFT_UPPER_ARM].taxelsTouched.clear();
1549 
1550  //SKIN_RIGHT_HAND
1551  contactICubSkinEmulMap[SKIN_RIGHT_HAND].coverTouched=false;
1552  contactICubSkinEmulMap[SKIN_RIGHT_HAND].taxelsTouched.clear();
1553 
1554  //SKIN_RIGHT_FOREARM
1555  contactICubSkinEmulMap[SKIN_RIGHT_FOREARM].coverTouched=false;
1556  contactICubSkinEmulMap[SKIN_RIGHT_FOREARM].taxelsTouched.clear();
1557 
1558  //SKIN_RIGHT_UPPER_ARM
1559  contactICubSkinEmulMap[SKIN_RIGHT_UPPER_ARM].coverTouched=false;
1560  contactICubSkinEmulMap[SKIN_RIGHT_UPPER_ARM].taxelsTouched.clear();
1561 
1562  //SKIN_FRONT_TORSO
1563  contactICubSkinEmulMap[SKIN_FRONT_TORSO].coverTouched=false;
1564  contactICubSkinEmulMap[SKIN_FRONT_TORSO].taxelsTouched.clear();
1565 
1566  //LEFT_LEG_UPPER
1567  contactICubSkinEmulMap[LEFT_LEG_UPPER].coverTouched=false;
1568  contactICubSkinEmulMap[LEFT_LEG_UPPER].taxelsTouched.clear();
1569 
1570  //LEFT_LEG_LOWER
1571  contactICubSkinEmulMap[LEFT_LEG_LOWER].coverTouched=false;
1572  contactICubSkinEmulMap[LEFT_LEG_LOWER].taxelsTouched.clear();
1573 
1574  //LEFT_FOOT
1575  contactICubSkinEmulMap[LEFT_FOOT].coverTouched=false;
1576  contactICubSkinEmulMap[LEFT_FOOT].taxelsTouched.clear();
1577 
1578  //RIGHT_LEG_UPPER
1579  contactICubSkinEmulMap[RIGHT_LEG_UPPER].coverTouched=false;
1580  contactICubSkinEmulMap[RIGHT_LEG_UPPER].taxelsTouched.clear();
1581 
1582  //RIGHT_LEG_LOWER
1583  contactICubSkinEmulMap[RIGHT_LEG_LOWER].coverTouched=false;
1584  contactICubSkinEmulMap[RIGHT_LEG_LOWER].taxelsTouched.clear();
1585 
1586  //RIGHT_FOOT
1587  contactICubSkinEmulMap[RIGHT_FOOT].coverTouched=false;
1588  contactICubSkinEmulMap[RIGHT_FOOT].taxelsTouched.clear();
1589 }
1590 
1591 void OdeSdlSimulation::printContactICubSkinEmulMap(void)
1592 {
1593  std::set<unsigned int> taxels_touched;
1594  yDebug("OdeSdlSimulation::printContactICubSkinEmulMap");
1595  for (std::map<SkinPart,contactICubSkinEmul_t>::const_iterator it=contactICubSkinEmulMap.begin(); it!=contactICubSkinEmulMap.end(); ++it){
1596  yDebug("key: %d, %s,cover touched: %d, indivTaxelResolution: %d, list of taxel IDs:", it->first,SkinPart_s[it->first].c_str(),it->second.coverTouched,it->second.indivTaxelResolution);
1597  taxels_touched = it->second.taxelsTouched;
1598  for (std::set<unsigned int>::const_iterator taxel_it = taxels_touched.begin(); taxel_it!=taxels_touched.end(); ++taxel_it){
1599  yDebug("%d ",*taxel_it);
1600  }
1601  }
1602 }
1603 
1604 
1606  delete video;
1607 }
1608 
1609 
1611  OdeInit& odeinit = OdeInit::get();
1612  if (reset) {
1613  odeinit.sync = false;
1614  }
1615  return odeinit.sync;
1616 }
1617 
1618 
1619 bool OdeSdlSimulation::getTrqData(Bottle data) {
1620  OdeInit& odeinit = OdeInit::get();
1621  for (int s=0; s<data.size(); s++){
1622  odeinit._iCub->torqueData[s] = data.get(s).asDouble();
1623  //yDebug(stdout,"torques... %lf \n",odeinit._iCub->torqueData[s]);
1624  }
1625  return true;
1626 }
1627 
1628 
1629 
1630 bool OdeSdlSimulation::getImage(ImageOf<PixelRgb>& target) {
1631  int w = cameraSizeWidth;
1632  int h = cameraSizeHeight;
1633  int p = 3;
1634 
1635  char *buf=new char[w * h * p];
1636  glReadPixels( 0, 0, w, h, GL_RGB, GL_UNSIGNED_BYTE, buf);
1637  ImageOf<PixelRgb> img;
1638  img.setQuantum(1);
1639  img.setExternal(buf,w,h);
1640 
1641  // inefficient flip!
1642  target.resize(img);
1643  int ww = img.width();
1644  int hh = img.height();
1645  for (int x=0; x<ww; x++) {
1646  for (int y=0; y<hh; y++) {
1647  target(x,y) = img(x,hh-1-y);
1648  }
1649  }
1650  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
1651  delete[] buf;
1652  return true;
1653 }
1654 
1655 void OdeSdlSimulation::inspectWholeBodyContactsAndSendTouch()
1656 {
1657  //SkinDynLib enums
1658  SkinPart skinPart; // id of the part of the skin (e.g. SKIN_LEFT_FOREARM; from skinDynLib/common.h)
1659  BodyPart bodyPart; // id of the body part
1660  HandPart handPart; // id of the hand part - INDEX, MIDDLE, RING, LITTLE, THUMB, PALM, ALL_HAND_PARTS, HAND_PART_SIZE
1661 
1662  //coordinate transformations for skinEvents and for emulating ind. taxel groups per skin part
1663  Vector geoCenter_SIM_FoR_forHomo(4,0.0), normal_SIM_FoR_forHomo(4,0.0);
1664  Vector force_SIM_FoR_forHomo(4,0.0), moment_SIM_FoR_forHomo(4,0.0);
1665  Vector v1(4,0.0); //auxilliary vector
1666  Vector geoCenter_link_FoR(3,0.0), normal_link_FoR(3,0.0);
1667  Vector force_link_FoR(3,0.0), moment_link_FoR(3,0.0);
1668  double forceOnBody_magnitude;
1669  double left_arm_encoders[16], right_arm_encoders[16], torso_encoders[3], head_encoders[6];
1670  Vector left_arm_for_iKin(10,0.0), right_arm_for_iKin(10,0.0), inertial_for_iKin(6,0.0);
1671  Matrix T_root_to_link = yarp::math::zeros(4,4);
1672  Matrix T_link_to_root = yarp::math::zeros(4,4);
1673  std::vector<unsigned int> taxel_list;
1674  bool upper_body_transforms_available = false;
1675 
1676  bool skinCoverFlag = false;
1677  bool fingertipFlag = true;
1678  OdeInit& odeinit = OdeInit::get();
1679  skinContactList mySkinContactList;
1680  mySkinContactList.clear();
1681 
1682  if ((odeinit._iCub->actHead=="off") || (odeinit._iCub->actTorso=="off") || (odeinit._iCub->actLArm=="off") || (odeinit._iCub->actRArm=="off")){
1683  upper_body_transforms_available = false;
1684  yWarning("With self-collisions on but head/torso/left_arm/right_arm off, the upper body transforms are unavailable and skinContactList can't be created.");
1685  }
1686  else{
1687  upper_body_transforms_available = true;
1688 
1689  odeinit._controls[PART_ARM_LEFT]->getEncodersRaw(left_arm_encoders);
1690  odeinit._controls[PART_ARM_RIGHT]->getEncodersRaw(right_arm_encoders);
1691  odeinit._controls[PART_TORSO]->getEncodersRaw(torso_encoders);
1692  odeinit._controls[PART_HEAD]->getEncodersRaw(head_encoders); //first three are probably neck joints, then the eyes
1693  for (int j=0;j<TORSO_DOF;j++){
1694  left_arm_for_iKin(j)=torso_encoders[j]; //first 3 joints - 0 to 2 - in iKin arm are torso joints
1695  right_arm_for_iKin(j)=torso_encoders[j];
1696  inertial_for_iKin(j)=torso_encoders[j];
1697  }
1698  for (int l=0;l<7;l++){
1699  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)
1700  right_arm_for_iKin(l+TORSO_DOF) = right_arm_encoders[l];
1701  }
1702  for (int m=0;m<3;m++){
1703  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)
1704  }
1705  odeinit._iCub->iKinLeftArm.setAng(left_arm_for_iKin);
1706  odeinit._iCub->iKinRightArm.setAng(right_arm_for_iKin);
1707  odeinit._iCub->iKinInertialSensor.setAng(inertial_for_iKin);
1708  }
1709 
1710  if (odeinit.verbosity > 4) yDebug("OdeSdlSimulation::inspectWholeBodyContactsAndSendTouch:There were %lu iCub collisions to process.", odeinit.listOfSkinContactInfos.size());
1711  //main loop through all the contacts
1712  for (list<OdeInit::contactOnSkin_t>::iterator it = odeinit.listOfSkinContactInfos.begin(); it!=odeinit.listOfSkinContactInfos.end(); it++){
1713  skinPart = SKIN_PART_UNKNOWN; bodyPart = BODY_PART_UNKNOWN; handPart = ALL_HAND_PARTS; skinCoverFlag = false; fingertipFlag = false;
1714  taxel_list.clear();
1715  odeinit._iCub->getSkinAndBodyPartFromSpaceAndGeomID((*it).body_geom_space_id,(*it).body_geom_id,skinPart,bodyPart,handPart,skinCoverFlag,fingertipFlag);
1716  if(upper_body_transforms_available){
1717  geoCenter_SIM_FoR_forHomo.zero(); geoCenter_SIM_FoR_forHomo(3)=1.0; //setting the extra row to 1 - for multiplication by homogenous rototransl. matrix
1718  normal_SIM_FoR_forHomo.zero(); normal_SIM_FoR_forHomo(3)=1.0;
1719  force_SIM_FoR_forHomo.zero(); force_SIM_FoR_forHomo(3)=1.0;
1720  moment_SIM_FoR_forHomo.zero(); moment_SIM_FoR_forHomo(3)=1.0;
1721  geoCenter_link_FoR.zero();normal_link_FoR.zero();
1722  moment_link_FoR.zero();force_link_FoR.zero();
1723  forceOnBody_magnitude=0.0;
1724  T_root_to_link.zero(); T_link_to_root.zero();
1725  for (int i=0;i<3;i++){
1726  geoCenter_SIM_FoR_forHomo(i)= (*it).contact_geom.pos[i]; //in global (i.e. simulator) coordinates
1727  normal_SIM_FoR_forHomo(i) = (*it).contact_geom.normal[i];
1728  }
1729  dJointFeedback * fb = dJointGetFeedback ((*it).contact_joint);
1730  if (fb==NULL){
1731  yDebug("Warning:OdeSdlSimulation::inspectWholeBodyContactsAndSendTouch: This joint (at %d skin part) has no feedback structure defined - contact force not available: setting to -1.",skinPart);
1732  forceOnBody_magnitude = -1;
1733  }
1734  else{
1735  //yDebug("OdeSdlSimulation::processWholeBodyCollisions: joint feedback structure:\n.");
1736  //yDebug("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
1737  //yDebug("f2 force vector: %f %f %f \n",fb->f2[0],fb->f2[1],fb->f2[2]);
1738  //f2 force vector has same magnitude but opposite direction than f1
1739  for(int k=0;k<3;k++){
1740  if((*it).body_index == 1){
1741  force_SIM_FoR_forHomo(k)=fb->f1[k];
1742  moment_SIM_FoR_forHomo(k)=fb->t1[k];
1743  }
1744  else if((*it).body_index == 2){
1745  force_SIM_FoR_forHomo(k)=fb->f2[k];
1746  moment_SIM_FoR_forHomo(k)=fb->t2[k];
1747  }
1748  else{
1749  yError("OdeSdlSimulation::inspectWholeBodyContactsAndSendTouch: unexpected body_index for colliding body: %d.\n",(*it).body_index);
1750  }
1751  }
1752  forceOnBody_magnitude=sqrt(force_SIM_FoR_forHomo(0)*force_SIM_FoR_forHomo(0) + force_SIM_FoR_forHomo(1)*force_SIM_FoR_forHomo(1)
1753  + force_SIM_FoR_forHomo(2)*force_SIM_FoR_forHomo(2));
1754  }
1755  //Let's do all the transformations
1756  //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
1757  // vectors (f1, m1) are originating from the global origin, i.e. they need to be translated to contact_geom.pos;
1758  //see the post in ode-users group "dJointFeedback and dContactGeom reference frame", 6.12.2013; local FoR of the contact point;
1759 
1760  switch(bodyPart){
1761  case LEFT_ARM:
1762  T_root_to_link = odeinit._iCub->iKinLeftArm.getH(SkinPart_2_LinkNum[skinPart].linkNum + TORSO_DOF);
1763  //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
1764  break;
1765  case RIGHT_ARM:
1766  T_root_to_link = odeinit._iCub->iKinRightArm.getH(SkinPart_2_LinkNum[skinPart].linkNum + TORSO_DOF);
1767  break;
1768  case TORSO:
1769  T_root_to_link = odeinit._iCub->iKinInertialSensor.getH(SkinPart_2_LinkNum[skinPart].linkNum);
1770  // 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
1771  //- check " SKIN torso 2" in iCub/main/app/iCubGui/skeleton.ini
1772  //- importantly, this needs to be the iKinInertialSensor, not the iKin Arm;
1773  break;
1774  default:
1775  if (odeinit.verbosity > 0) yDebug("OdeSdlSimulation::processWholeBodyCollisions: FoR transforms to BODY PART %d not implemented yet\n",bodyPart);
1776  continue;
1777  }
1778  T_link_to_root = SE3inv(T_root_to_link);
1779 
1780  v1.zero();
1781  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
1782  geoCenter_link_FoR = v1.subVector(0,2); //strip the last one away
1783 
1784  v1.zero();
1785  v1 = T_link_to_root * (odeinit._iCub->H_r2w) * normal_SIM_FoR_forHomo;
1786  normal_link_FoR = v1.subVector(0,2);
1787 
1788  v1.zero();
1789  v1 = T_link_to_root * (odeinit._iCub->H_r2w) * force_SIM_FoR_forHomo;
1790  force_link_FoR = v1.subVector(0,2);
1791 
1792  v1.zero();
1793  v1 = T_link_to_root * (odeinit._iCub->H_r2w) * moment_SIM_FoR_forHomo;
1794  moment_link_FoR = v1.subVector(0,2);
1795 
1796  //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
1797  //appropariate CoP / geoCenter to make the arrow to the taxel
1798  //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
1799  //forces along the normal and frictional forces perpendicular to the normal
1800  //alternatively, I could just take the magnitude from the force and send the normal as the direction
1801 
1802  //yDebug("Contact coordinates in ODE / SIM FoR: %s\n",geoCenter_SIM_FoR_forHomo.subVector(0,2).toString().c_str());
1803  Vector temp_v4(4,0.0);
1804  temp_v4 = (odeinit._iCub->H_r2w) * geoCenter_SIM_FoR_forHomo;
1805  //yDebug("Contact coordinates in robot root FoR: %s\n",temp_v4.subVector(0,2).toString().c_str());
1806  //yDebug("Left arm for iKin:\n %s \n",left_arm_for_iKin.toString().c_str());
1807  //yDebug("Rototranslation matrix root to link:\n %s\n",T_root_to_link.toString().c_str());
1808  //yDebug("Contact coordinates in link FoR: %s\n",geoCenter_link_FoR.toString().c_str());
1809  /*for (int l=0;l<2;l++){ geoCenter_link_FoR(l)=0.0; force_link_FoR(l)=1.0; normal_link_FoR(l)=1.0; moment_link_FoR(l)=1.0;
1810  } */
1811  //forceOnBody_magnitude=10.0;
1812  if (contactICubSkinEmulMap[skinPart].indivTaxelResolution && (skinCoverFlag || fingertipFlag)){ //indiv taxels get emulated only on covers - where the actual skin is
1813  if(skinCoverFlag){
1814  mapPositionIntoTaxelList(skinPart,geoCenter_link_FoR,taxel_list);
1815  }
1816  else if(fingertipFlag){
1817  mapFingertipIntoTaxelList(handPart,taxel_list);
1818  }
1819  }
1820  else{
1821  taxel_list.push_back(FAKE_TAXEL_ID); // we will emulate one non-existent activated "taxel" per contact joint - say taxel "10000"
1822  }
1823  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);
1824  //we have only one source of information - the contact as detected by ODE - therefore, we take the coordinates and set them both to CoP
1825  //(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.
1826  if (odeinit.verbosity > 4) yDebug("Creating skin contact as follows: %s.\n",c.toString().c_str());
1827  mySkinContactList.push_back(c);
1828  } //if(upper_body_transforms_available){
1829  // here we collect the info for emulating the skin ports (compensated tactile ports)
1830  if(skinCoverFlag || fingertipFlag){
1831  //if it was a cover (including palm cover) or fingertip that was touched, we will append the taxels touched to respective contactICubSkinEmulMap
1832  contactICubSkinEmulMap[skinPart].coverTouched = true;
1833  if (contactICubSkinEmulMap[skinPart].indivTaxelResolution){
1834  if (!taxel_list.empty()){
1835  unsigned int first_taxel_in_list = taxel_list[0];
1836  if (first_taxel_in_list != FAKE_TAXEL_ID){
1837  for (std::vector<unsigned int>::const_iterator it = taxel_list.begin() ; it != taxel_list.end(); ++it){
1838  contactICubSkinEmulMap[skinPart].taxelsTouched.insert(*it); //inserting the taxel IDs into the set
1839  }
1840  }
1841  }
1842  }
1843  }
1844  } //cycle through odeinit.listOfSkinContactInfos
1845 
1846  //all contacts have been processed, now we produce the output
1847 
1848  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
1849  // the contacts can be visualized using the icubGui (not skinGui)
1850  robot_streamer->sendSkinEvents(mySkinContactList); //we send even if empty
1851  }
1852 
1853  //for hands, this is now done differently than in the original inspectTouch_icubSensors, where finger bodies were inspected, whether they have contact joints attached to them
1854  // the palm cover replaces sensing in the palm body
1855  //now all info about contacts has come from cycling through the odeinit.listOfSkinContactInfos above and it has beem filled into appropriate structs
1856  //the output of actual pressure values is discontinued;
1857  int y=0;
1858  if(robot_streamer->shouldSendTouchLeftHand()){
1859  Bottle bottleLeftHand;
1860  if (contactICubSkinEmulMap[SKIN_LEFT_HAND].coverTouched){
1861  //prepare the bottle
1862  //first 60 are fingers
1863  if (contactICubSkinEmulMap[SKIN_LEFT_HAND].indivTaxelResolution){
1864  for (y = 0; y<=59; y++){
1865  if (contactICubSkinEmulMap[SKIN_LEFT_HAND].taxelsTouched.count(y)){ // if element (taxel ID) is in the set, count returns 1
1866  bottleLeftHand.addDouble(255.0);
1867  }
1868  else{
1869  bottleLeftHand.addDouble(0.0);
1870  }
1871  }
1872  }
1873  else{ //we fill them all
1874  for (y = 0; y<=59; y++){
1875  bottleLeftHand.addDouble(255); //we ignore the thermal pad positions which should be 0s for now
1876  }
1877  }
1878  //zero padding - the port output: 61-96 zeros; taxel IDs 60-95
1879  for (y = 60; y<=95; y++){
1880  bottleLeftHand.addDouble(0.0);
1881  }
1882 
1883  //pam - positions 97-144 palm taxels; taxel IDs have index by one lower (inside these, IDs 107, 119, 131, and 139 are thermal pads ~ 0s);
1884  if (contactICubSkinEmulMap[SKIN_LEFT_HAND].indivTaxelResolution){
1885  for (y = 96; y<=143; y++){
1886  if (contactICubSkinEmulMap[SKIN_LEFT_HAND].taxelsTouched.count(y)){ // if element (taxel ID) is in the set, count returns 1
1887  bottleLeftHand.addDouble(255.0);
1888  }
1889  else{
1890  bottleLeftHand.addDouble(0.0);
1891  }
1892  }
1893  }
1894  else{ //we fill the whole palm
1895  for (int y = 96; y<=143; y++){
1896  bottleLeftHand.addDouble(255.0); //we ignore the thermal pad positions, which should be 0s, for now
1897  }
1898  }
1899  //filling the rest: 145-192 zeros. IDs: 144-191
1900  for (int y = 144; y<=191; y++){
1901  bottleLeftHand.addDouble(0.0);
1902  }
1903  }
1904  else{
1905  bottleLeftHand = Bottle(odeinit._iCub->emptySkinActivationHand);
1906  }
1907  robot_streamer->sendTouchLeftHand(bottleLeftHand);
1908  }
1909 
1910 
1911  if(robot_streamer->shouldSendTouchRightHand()){
1912  Bottle bottleRightHand;
1913  if (contactICubSkinEmulMap[SKIN_RIGHT_HAND].coverTouched){
1914  //prepare the bottle
1915  //first 60 are fingers
1916  if (contactICubSkinEmulMap[SKIN_RIGHT_HAND].indivTaxelResolution){
1917  for (y = 0; y<=59; y++){
1918  if (contactICubSkinEmulMap[SKIN_RIGHT_HAND].taxelsTouched.count(y)){ // if element (taxel ID) is in the set, count returns 1
1919  bottleRightHand.addDouble(255.0);
1920  }
1921  else{
1922  bottleRightHand.addDouble(0.0);
1923  }
1924  }
1925  }
1926  else{ //we fill them all
1927  for (y = 0; y<=59; y++){
1928  bottleRightHand.addDouble(255); //we ignore the thermal pad positions which should be 0s for now
1929  }
1930  }
1931  //zero padding - the port output: 61-96 zeros; taxel IDs 60-95
1932  for (y = 60; y<=95; y++){
1933  bottleRightHand.addDouble(0.0);
1934  }
1935 
1936  //pam - positions 97-144 palm taxels; taxel IDs have index by one lower (inside these, IDs 107, 119, 131, and 139 are thermal pads ~ 0s);
1937  if (contactICubSkinEmulMap[SKIN_RIGHT_HAND].indivTaxelResolution){
1938  for (y = 96; y<=143; y++){
1939  if (contactICubSkinEmulMap[SKIN_RIGHT_HAND].taxelsTouched.count(y)){ // if element (taxel ID) is in the set, count returns 1
1940  bottleRightHand.addDouble(255.0);
1941  }
1942  else{
1943  bottleRightHand.addDouble(0.0);
1944  }
1945  }
1946  }
1947  else{ //we fill the whole palm
1948  for (int y = 96; y<=143; y++){
1949  bottleRightHand.addDouble(255.0); //we ignore the thermal pad positions, which should be 0s, for now
1950  }
1951  }
1952  //filling the rest: 145-192 zeros. IDs: 144-191
1953  for (int y = 144; y<=191; y++){
1954  bottleRightHand.addDouble(0.0);
1955  }
1956  }
1957  else{
1958  bottleRightHand = Bottle(odeinit._iCub->emptySkinActivationHand);
1959  }
1960  // yDebug("bottleRightHand: %s \n",bottleRightHand.toString().c_str());
1961  // yDebug("bottleRightHand: %s \n",bottleRightHand.toString().c_str());
1962  robot_streamer->sendTouchRightHand(bottleRightHand);
1963  }
1964 
1965 
1966  if(robot_streamer->shouldSendTouchLeftArm()){
1967  Bottle bottleLeftArm;
1968  if (contactICubSkinEmulMap[SKIN_LEFT_UPPER_ARM].coverTouched){
1969  if (contactICubSkinEmulMap[SKIN_LEFT_UPPER_ARM].indivTaxelResolution){
1970  for (int y = 0; y<=767; y++){
1971  if (contactICubSkinEmulMap[SKIN_LEFT_UPPER_ARM].taxelsTouched.count(y)){ // if element (taxel ID) is in the set, count returns 1
1972  bottleLeftArm.addDouble(255.0);
1973  }
1974  else{
1975  bottleLeftArm.addDouble(0.0);
1976  }
1977  }
1978  }
1979  else{ //we fill the whole upper arm
1980  bottleLeftArm = Bottle(odeinit._iCub->fullSkinActivationUpperArm);
1981  }
1982  }
1983  else{
1984  bottleLeftArm = Bottle(odeinit._iCub->emptySkinActivationUpperArm);
1985  }
1986  robot_streamer->sendTouchLeftArm(bottleLeftArm);
1987  }
1988  if(robot_streamer->shouldSendTouchLeftForearm()){
1989  Bottle bottleLeftForearm;
1990  if (contactICubSkinEmulMap[SKIN_LEFT_FOREARM].coverTouched){
1991  if (contactICubSkinEmulMap[SKIN_LEFT_FOREARM].indivTaxelResolution){
1992  for (int y = 0; y<=383; y++){
1993  if (contactICubSkinEmulMap[SKIN_LEFT_FOREARM].taxelsTouched.count(y)){ // if element (taxel ID) is in the set, count returns 1
1994  bottleLeftForearm.addDouble(255.0);
1995  }
1996  else{
1997  bottleLeftForearm.addDouble(0.0);
1998  }
1999  }
2000  }
2001  else{ //we fill the whole forearm
2002  bottleLeftForearm = Bottle(odeinit._iCub->fullSkinActivationForearm);
2003  }
2004  }
2005  else{
2006  bottleLeftForearm = Bottle(odeinit._iCub->emptySkinActivationForearm);
2007  }
2008  robot_streamer->sendTouchLeftForearm(bottleLeftForearm);
2009  }
2010  if(robot_streamer->shouldSendTouchRightArm()){
2011  Bottle bottleRightArm;
2012  if (contactICubSkinEmulMap[SKIN_RIGHT_UPPER_ARM].coverTouched){
2013  if (contactICubSkinEmulMap[SKIN_RIGHT_UPPER_ARM].indivTaxelResolution){
2014  for (int y = 0; y<=767; y++){
2015  if (contactICubSkinEmulMap[SKIN_RIGHT_UPPER_ARM].taxelsTouched.count(y)){ // if element (taxel ID) is in the set, count returns 1
2016  bottleRightArm.addDouble(255.0);
2017  }
2018  else{
2019  bottleRightArm.addDouble(0.0);
2020  }
2021  }
2022  }
2023  else{ //we fill the whole upper arm
2024  bottleRightArm = Bottle(odeinit._iCub->fullSkinActivationUpperArm);
2025  }
2026  }
2027  else{
2028  bottleRightArm = Bottle(odeinit._iCub->emptySkinActivationUpperArm);
2029  }
2030  robot_streamer->sendTouchRightArm(bottleRightArm);
2031  }
2032  if(robot_streamer->shouldSendTouchRightForearm()){
2033  Bottle bottleRightForearm;
2034  if (contactICubSkinEmulMap[SKIN_RIGHT_FOREARM].coverTouched){
2035  if (contactICubSkinEmulMap[SKIN_RIGHT_FOREARM].indivTaxelResolution){
2036  for (int y = 0; y<=383; y++){
2037  if (contactICubSkinEmulMap[SKIN_RIGHT_FOREARM].taxelsTouched.count(y)){ // if element (taxel ID) is in the set, count returns 1
2038  bottleRightForearm.addDouble(255.0);
2039  }
2040  else{
2041  bottleRightForearm.addDouble(0.0);
2042  }
2043  }
2044  }
2045  else{ //we fill the whole forearm
2046  bottleRightForearm = Bottle(odeinit._iCub->fullSkinActivationForearm);
2047  }
2048  }
2049  else{
2050  bottleRightForearm = Bottle(odeinit._iCub->emptySkinActivationForearm);
2051  }
2052  robot_streamer->sendTouchRightForearm(bottleRightForearm);
2053  }
2054  if(robot_streamer->shouldSendTouchTorso()){
2055  Bottle bottleTorso;
2056  if (contactICubSkinEmulMap[SKIN_FRONT_TORSO].coverTouched){
2057  if (contactICubSkinEmulMap[SKIN_FRONT_TORSO].indivTaxelResolution){
2058  for (int y = 0; y<=767; y++){
2059  if (contactICubSkinEmulMap[SKIN_FRONT_TORSO].taxelsTouched.count(y)){ // if element (taxel ID) is in the set, count returns 1
2060  bottleTorso.addDouble(255.0);
2061  }
2062  else{
2063  bottleTorso.addDouble(0.0);
2064  }
2065  }
2066  }
2067  else{ //we fill the whole torso
2068  bottleTorso = Bottle(odeinit._iCub->fullSkinActivationTorso);
2069  }
2070  }
2071  else{
2072  bottleTorso = Bottle(odeinit._iCub->emptySkinActivationTorso);
2073  }
2074  robot_streamer->sendTouchTorso(bottleTorso);
2075  }
2076 }
2077 
2078 
2079 void OdeSdlSimulation::mapPositionIntoTaxelList(const SkinPart skin_part,const Vector geo_center_link_FoR,std::vector<unsigned int>& list_of_taxels){
2080 
2081 
2082  // EXTRA_MARGIN_FOR_TAXEL_POSITION_M = 0.03; //for skin emulation we get the coordinates of the collision and contact with skin cover from ODE;
2083  //after transforming to local reference frame of respective skin part, we emulate which set of taxels would get activated at that position;
2084  //however, with errors in the position, we need an extra margin, so the contact falls onto some taxels
2085  switch (skin_part){
2086  case SKIN_LEFT_HAND:
2087  if ((geo_center_link_FoR[0]<0.003+MORE_EXTRA_MARGIN_FOR_TAXEL_POSITION_M) && (geo_center_link_FoR[0]>-0.014) && (geo_center_link_FoR[1]>-0.026-EXTRA_MARGIN_FOR_TAXEL_POSITION_M-1.5*MORE_EXTRA_MARGIN_FOR_TAXEL_POSITION_M) && (geo_center_link_FoR[1]<-0.0055)){
2088  list_of_taxels.push_back(121);list_of_taxels.push_back(122);list_of_taxels.push_back(123);
2089  list_of_taxels.push_back(124);list_of_taxels.push_back(125);list_of_taxels.push_back(126);
2090  list_of_taxels.push_back(127);list_of_taxels.push_back(128);
2091  //list_of_taxels.push_back();list_of_taxels.push_back();list_of_taxels.push_back();
2092  }
2093  else if ((geo_center_link_FoR[0]<0.003+MORE_EXTRA_MARGIN_FOR_TAXEL_POSITION_M) && (geo_center_link_FoR[0]>-0.014) && (geo_center_link_FoR[1]>-0.0055) && (geo_center_link_FoR[1]<0.01)){
2094  list_of_taxels.push_back(96);list_of_taxels.push_back(97);list_of_taxels.push_back(98);
2095  list_of_taxels.push_back(99);list_of_taxels.push_back(102);list_of_taxels.push_back(103);
2096  list_of_taxels.push_back(120);list_of_taxels.push_back(129);list_of_taxels.push_back(130);
2097  }
2098  else if ((geo_center_link_FoR[0]<0.003+MORE_EXTRA_MARGIN_FOR_TAXEL_POSITION_M) && (geo_center_link_FoR[0]>-0.014) && (geo_center_link_FoR[1]>0.01) && (geo_center_link_FoR[1]<0.03+EXTRA_MARGIN_FOR_TAXEL_POSITION_M) ){
2099  list_of_taxels.push_back(100);list_of_taxels.push_back(101);list_of_taxels.push_back(104);
2100  list_of_taxels.push_back(105);list_of_taxels.push_back(106);list_of_taxels.push_back(113);
2101  list_of_taxels.push_back(116);list_of_taxels.push_back(117);
2102  }
2103  else if ((geo_center_link_FoR[0]<-0.014) && (geo_center_link_FoR[0]>-0.024) && (geo_center_link_FoR[1]>0.0-EXTRA_MARGIN_FOR_TAXEL_POSITION_M-2*MORE_EXTRA_MARGIN_FOR_TAXEL_POSITION_M) && (geo_center_link_FoR[1]<0.03+EXTRA_MARGIN_FOR_TAXEL_POSITION_M) ){
2104  list_of_taxels.push_back(108);list_of_taxels.push_back(109);list_of_taxels.push_back(110);
2105  list_of_taxels.push_back(111);list_of_taxels.push_back(112);list_of_taxels.push_back(114);
2106  list_of_taxels.push_back(115);list_of_taxels.push_back(118); list_of_taxels.push_back(142);
2107  list_of_taxels.push_back(143);
2108  }
2109  else if ((geo_center_link_FoR[0]<-0.024) && (geo_center_link_FoR[0]>-0.04-EXTRA_MARGIN_FOR_TAXEL_POSITION_M-2.0*MORE_EXTRA_MARGIN_FOR_TAXEL_POSITION_M) && (geo_center_link_FoR[1]>0.0-EXTRA_MARGIN_FOR_TAXEL_POSITION_M-2*MORE_EXTRA_MARGIN_FOR_TAXEL_POSITION_M) && (geo_center_link_FoR[1]<0.03+EXTRA_MARGIN_FOR_TAXEL_POSITION_M) ){
2110  list_of_taxels.push_back(132);list_of_taxels.push_back(133);list_of_taxels.push_back(134);
2111  list_of_taxels.push_back(135);list_of_taxels.push_back(136);list_of_taxels.push_back(137);
2112  list_of_taxels.push_back(138);list_of_taxels.push_back(140); list_of_taxels.push_back(141);
2113  }
2114  else{
2115  yWarning("OdeSdlSimulation::mapPositionIntoTaxelList: WARNING: contact at part: %d, coordinates: %f %f %f, but no taxels asigned to this position. \n",skin_part,geo_center_link_FoR[0],geo_center_link_FoR[1],geo_center_link_FoR[2]);
2116  }
2117  break;
2118  case SKIN_RIGHT_HAND:
2119  if ((geo_center_link_FoR[0]<0.003+MORE_EXTRA_MARGIN_FOR_TAXEL_POSITION_M) && (geo_center_link_FoR[0]>-0.014) && (geo_center_link_FoR[1]>-0.026-EXTRA_MARGIN_FOR_TAXEL_POSITION_M-1.5*MORE_EXTRA_MARGIN_FOR_TAXEL_POSITION_M) && (geo_center_link_FoR[1]<-0.0055)){
2120  list_of_taxels.push_back(120);list_of_taxels.push_back(121);list_of_taxels.push_back(122);
2121  list_of_taxels.push_back(123);list_of_taxels.push_back(124);list_of_taxels.push_back(125);
2122  list_of_taxels.push_back(126);list_of_taxels.push_back(128);
2123  //list_of_taxels.push_back();list_of_taxels.push_back();list_of_taxels.push_back();
2124  }
2125  else if ((geo_center_link_FoR[0]<0.003+MORE_EXTRA_MARGIN_FOR_TAXEL_POSITION_M) && (geo_center_link_FoR[0]>-0.014) && (geo_center_link_FoR[1]>-0.0055) && (geo_center_link_FoR[1]<0.01)){
2126  list_of_taxels.push_back(99);list_of_taxels.push_back(102);list_of_taxels.push_back(103);
2127  list_of_taxels.push_back(104);list_of_taxels.push_back(105);list_of_taxels.push_back(106);
2128  list_of_taxels.push_back(127);list_of_taxels.push_back(129);list_of_taxels.push_back(130);
2129  }
2130  else if ((geo_center_link_FoR[0]<0.003+MORE_EXTRA_MARGIN_FOR_TAXEL_POSITION_M) && (geo_center_link_FoR[0]>-0.014) && (geo_center_link_FoR[1]>0.01) && (geo_center_link_FoR[1]<0.03+EXTRA_MARGIN_FOR_TAXEL_POSITION_M) ){
2131  list_of_taxels.push_back(96);list_of_taxels.push_back(97);list_of_taxels.push_back(98);
2132  list_of_taxels.push_back(100);list_of_taxels.push_back(101);list_of_taxels.push_back(110);
2133  list_of_taxels.push_back(111);list_of_taxels.push_back(112);
2134  }
2135  else if ((geo_center_link_FoR[0]<-0.014) && (geo_center_link_FoR[0]>-0.024) && (geo_center_link_FoR[1]>0.0-EXTRA_MARGIN_FOR_TAXEL_POSITION_M-2*MORE_EXTRA_MARGIN_FOR_TAXEL_POSITION_M) && (geo_center_link_FoR[1]<0.03+EXTRA_MARGIN_FOR_TAXEL_POSITION_M) ){
2136  list_of_taxels.push_back(108);list_of_taxels.push_back(109);list_of_taxels.push_back(113);
2137  list_of_taxels.push_back(114);list_of_taxels.push_back(115);list_of_taxels.push_back(116);
2138  list_of_taxels.push_back(117);list_of_taxels.push_back(118); list_of_taxels.push_back(142);
2139  list_of_taxels.push_back(143);
2140  }
2141  else if ((geo_center_link_FoR[0]<-0.024) && (geo_center_link_FoR[0]>-0.040-EXTRA_MARGIN_FOR_TAXEL_POSITION_M-2.0*MORE_EXTRA_MARGIN_FOR_TAXEL_POSITION_M) && (geo_center_link_FoR[1]>0.0-EXTRA_MARGIN_FOR_TAXEL_POSITION_M-2*MORE_EXTRA_MARGIN_FOR_TAXEL_POSITION_M) && (geo_center_link_FoR[1]<0.03+EXTRA_MARGIN_FOR_TAXEL_POSITION_M) ){
2142  list_of_taxels.push_back(132);list_of_taxels.push_back(133);list_of_taxels.push_back(134);
2143  list_of_taxels.push_back(135);list_of_taxels.push_back(136);list_of_taxels.push_back(137);
2144  list_of_taxels.push_back(138);list_of_taxels.push_back(140); list_of_taxels.push_back(141);
2145  }
2146  else{
2147  yWarning("OdeSdlSimulation::mapPositionIntoTaxelList: WARNING: contact at part: %d, coordinates: %f %f %f, but no taxels asigned to this position. \n",skin_part,geo_center_link_FoR[0],geo_center_link_FoR[1],geo_center_link_FoR[2]);
2148  }
2149  break;
2150  case SKIN_LEFT_FOREARM:
2151  //upper small patch (7 triangles)
2152  if((geo_center_link_FoR[0]>-0.0326) && (geo_center_link_FoR[0]<0.0326) && (geo_center_link_FoR[1]>-0.0328) && (geo_center_link_FoR[1]<0.0039) && (geo_center_link_FoR[2]>-0.0338) && (geo_center_link_FoR[2]<0)){
2153  //triangle taxel IDs 288-299
2154  pushTriangleToTaxelList(288,list_of_taxels);
2155  //triangle 300-311
2156  pushTriangleToTaxelList(300,list_of_taxels);
2157  //triangle 348-359
2158  pushTriangleToTaxelList(348,list_of_taxels);
2159  }
2160  else if((geo_center_link_FoR[0]>-0.0345) && (geo_center_link_FoR[0]<0) && (geo_center_link_FoR[1]>-0.0687) && (geo_center_link_FoR[1]<-0.0328) && (geo_center_link_FoR[2]>-0.0369) && (geo_center_link_FoR[2]<0)){
2161  //triangle 204:215
2162  pushTriangleToTaxelList(204,list_of_taxels);
2163  //triangle 336:347
2164  pushTriangleToTaxelList(336,list_of_taxels);
2165  }
2166  else if((geo_center_link_FoR[0]>0) && (geo_center_link_FoR[0]<0.0345) && (geo_center_link_FoR[1]>-0.0687) && (geo_center_link_FoR[1]<0.0345) && (geo_center_link_FoR[2]>-0.0328) && (geo_center_link_FoR[2]<0)){
2167  //triangle 252:263
2168  pushTriangleToTaxelList(252,list_of_taxels);
2169  //triangle 312:323
2170  pushTriangleToTaxelList(312,list_of_taxels);
2171  }
2172 
2174 
2175  //lower patch - big (16 triangles)
2176  else if((geo_center_link_FoR[0]>-0.0375) && (geo_center_link_FoR[0]<0) && (geo_center_link_FoR[1]>-0.0716) && (geo_center_link_FoR[1]<-0.0298) && (geo_center_link_FoR[2]>0.0281) && (geo_center_link_FoR[2]<0.0484)){
2177  //triangle nr. 12 in CAD, taxel IDs 132:143
2178  pushTriangleToTaxelList(132,list_of_taxels);
2179  //triangle 16 168:179
2180  pushTriangleToTaxelList(168,list_of_taxels);
2181  }
2182  else if((geo_center_link_FoR[0]>-0.0375) && (geo_center_link_FoR[0]<0) && (geo_center_link_FoR[1]>-0.1081) && (geo_center_link_FoR[1]<-0.0716) && (geo_center_link_FoR[2]>0.0343) && (geo_center_link_FoR[2]<0.0526)){
2183  //triangle 3, 156:167
2184  pushTriangleToTaxelList(156,list_of_taxels);
2185  //triangle 8, 144:155
2186  pushTriangleToTaxelList(144,list_of_taxels);
2187  }
2188  else if((geo_center_link_FoR[0]>-0.0375) && (geo_center_link_FoR[0]<0) && (geo_center_link_FoR[1]>-0.1133) && (geo_center_link_FoR[1]<-0.0716) && (geo_center_link_FoR[2]>0) && (geo_center_link_FoR[2]<0.0343)){
2189  //triangle 4, 24:35
2190  pushTriangleToTaxelList(24,list_of_taxels);
2191  //triangle 6, 12:23
2192  pushTriangleToTaxelList(12,list_of_taxels);
2193  }
2194  else if((geo_center_link_FoR[0]>-0.0375) && (geo_center_link_FoR[0]<0) && (geo_center_link_FoR[1]>-0.0716) && (geo_center_link_FoR[1]<-0.0318) && (geo_center_link_FoR[2]>0) && (geo_center_link_FoR[2]<0.0281)){
2195  //triangle 10, 0:11
2196  pushTriangleToTaxelList(0,list_of_taxels);
2197  //triangle 14, 180:191
2198  pushTriangleToTaxelList(180,list_of_taxels);
2199  }
2200  else if((geo_center_link_FoR[0]>0) && (geo_center_link_FoR[0]<0.0375) && (geo_center_link_FoR[1]>-0.0716) && (geo_center_link_FoR[1]<-0.0298) && (geo_center_link_FoR[2]>0.0281) && (geo_center_link_FoR[2]<0.0484)){
2201  //triangle 11, 120:131
2202  pushTriangleToTaxelList(120,list_of_taxels);
2203  //triangle 15, 60:71
2204  pushTriangleToTaxelList(60,list_of_taxels);
2205  }
2206  else if((geo_center_link_FoR[0]>0) && (geo_center_link_FoR[0]<0.0375) && (geo_center_link_FoR[1]>-0.1081) && (geo_center_link_FoR[1]<-0.0716) && (geo_center_link_FoR[2]>0.0343) && (geo_center_link_FoR[2]<0.0526)){
2207  //triangle 2, 96:107
2208  pushTriangleToTaxelList(96,list_of_taxels);
2209  //triangle 7, 108:119
2210  pushTriangleToTaxelList(108,list_of_taxels);
2211  }
2212  else if((geo_center_link_FoR[0]>0) && (geo_center_link_FoR[0]<0.0375) && (geo_center_link_FoR[1]>-0.1133) && (geo_center_link_FoR[1]<-0.0716) && (geo_center_link_FoR[2]>0) && (geo_center_link_FoR[2]<0.0343)){
2213  //triangle 1, 84:95
2214  pushTriangleToTaxelList(84,list_of_taxels);
2215  //triangle 5, 72:83
2216  pushTriangleToTaxelList(72,list_of_taxels);
2217  }
2218  else if((geo_center_link_FoR[0]>0) && (geo_center_link_FoR[0]<0.0375) && (geo_center_link_FoR[1]>-0.0716) && (geo_center_link_FoR[1]<-0.0318) && (geo_center_link_FoR[2]>0) && (geo_center_link_FoR[2]<0.0281)){
2219  //triangle 9, 36:47
2220  pushTriangleToTaxelList(36,list_of_taxels);
2221  //triangle 13, 48:59
2222  pushTriangleToTaxelList(48,list_of_taxels);
2223  }
2224  else{
2225  yWarning("OdeSdlSimulation::mapPositionIntoTaxelList: WARNING: contact at part: %d, coordinates: %f %f %f, but no taxels asigned to this position. \n",skin_part,geo_center_link_FoR[0],geo_center_link_FoR[1],geo_center_link_FoR[2]);
2226  }
2227  break;
2228  case SKIN_RIGHT_FOREARM:
2229  //upper small patch (7 triangles)
2230  if((geo_center_link_FoR[0]>-0.0326) && (geo_center_link_FoR[0]<0.0326) && (geo_center_link_FoR[1]<0.0328) && (geo_center_link_FoR[1]>-0.0039) && (geo_center_link_FoR[2]<0.0338) && (geo_center_link_FoR[2]>0)){
2231  //triangle taxel IDs 288-299
2232  pushTriangleToTaxelList(288,list_of_taxels);
2233  //triangle 300-311
2234  pushTriangleToTaxelList(300,list_of_taxels);
2235  //triangle 348-359
2236  pushTriangleToTaxelList(348,list_of_taxels);
2237  }
2238  else if((geo_center_link_FoR[0]>-0.0345) && (geo_center_link_FoR[0]<0) && (geo_center_link_FoR[1]<0.0687) && (geo_center_link_FoR[1]>0.0328) && (geo_center_link_FoR[2]<0.0369) && (geo_center_link_FoR[2]>0)){
2239  //triangle 204:215
2240  pushTriangleToTaxelList(204,list_of_taxels);
2241  //triangle 336:347
2242  pushTriangleToTaxelList(336,list_of_taxels);
2243  }
2244  else if((geo_center_link_FoR[0]>0) && (geo_center_link_FoR[0]<0.0345) && (geo_center_link_FoR[1]<0.0687) && (geo_center_link_FoR[1]>0.0345) && (geo_center_link_FoR[2]<0.0328) && (geo_center_link_FoR[2]>0)){
2245  //triangle 252:263
2246  pushTriangleToTaxelList(252,list_of_taxels);
2247  //triangle 312:323
2248  pushTriangleToTaxelList(312,list_of_taxels);
2249  }
2250 
2252 
2254 
2255  else if((geo_center_link_FoR[0]>-0.0375) && (geo_center_link_FoR[0]<0) && (geo_center_link_FoR[1]<0.0716) && (geo_center_link_FoR[1]<0.0298) && (geo_center_link_FoR[2]<-0.0281) && (geo_center_link_FoR[2]>-0.0484)){
2256  //triangle nr. 12 in CAD, taxel IDs 132:143
2257  pushTriangleToTaxelList(132,list_of_taxels);
2258  //triangle 16 168:179
2259  pushTriangleToTaxelList(168,list_of_taxels);
2260  }
2261 
2262  else if((geo_center_link_FoR[0]>-0.0375) && (geo_center_link_FoR[0]<0) && (geo_center_link_FoR[1]<0.1081) && (geo_center_link_FoR[1]>0.0716) && (geo_center_link_FoR[2]<-0.0343) && (geo_center_link_FoR[2]>-0.0526)){
2263  //triangle 3, 156:167
2264  pushTriangleToTaxelList(156,list_of_taxels);
2265  //triangle 8, 144:155
2266  pushTriangleToTaxelList(144,list_of_taxels);
2267  }
2268 
2269  else if((geo_center_link_FoR[0]>-0.0375) && (geo_center_link_FoR[0]<0) && (geo_center_link_FoR[1]<0.1133) && (geo_center_link_FoR[1]>0.0716) && (geo_center_link_FoR[2]<-0) && (geo_center_link_FoR[2]>-0.0343)){
2270  //triangle 4, 24:35
2271  pushTriangleToTaxelList(24,list_of_taxels);
2272  //triangle 6, 12:23
2273  pushTriangleToTaxelList(12,list_of_taxels);
2274  }
2275 
2276  else if((geo_center_link_FoR[0]>-0.0375) && (geo_center_link_FoR[0]<0) && (geo_center_link_FoR[1]<0.0716) && (geo_center_link_FoR[1]>0.0318) && (geo_center_link_FoR[2]<0) && (geo_center_link_FoR[2]>-0.0281)){
2277  //triangle 10, 0:11
2278  pushTriangleToTaxelList(0,list_of_taxels);
2279  //triangle 14, 180:191
2280  pushTriangleToTaxelList(180,list_of_taxels);
2281  }
2282 
2283  else if((geo_center_link_FoR[0]>0) && (geo_center_link_FoR[0]<0.0375) && (geo_center_link_FoR[1]<0.0716) && (geo_center_link_FoR[1]>0.0298) && (geo_center_link_FoR[2]<-0.0281) && (geo_center_link_FoR[2]>-0.0484)){
2284  //triangle 11, 120:131
2285  pushTriangleToTaxelList(120,list_of_taxels);
2286  //triangle 15, 60:71
2287  pushTriangleToTaxelList(60,list_of_taxels);
2288  }
2289 
2290  else if((geo_center_link_FoR[0]>0) && (geo_center_link_FoR[0]<0.0375) && (geo_center_link_FoR[1]<0.1081) && (geo_center_link_FoR[1]>0.0716) && (geo_center_link_FoR[2]<-0.0343) && (geo_center_link_FoR[2]>-0.0526)){
2291  //triangle 2, 96:107
2292  pushTriangleToTaxelList(96,list_of_taxels);
2293  //triangle 7, 108:119
2294  pushTriangleToTaxelList(108,list_of_taxels);
2295  }
2296 
2297  else if((geo_center_link_FoR[0]>0) && (geo_center_link_FoR[0]<0.0375) && (geo_center_link_FoR[1]<0.1133) && (geo_center_link_FoR[1]>0.0716) && (geo_center_link_FoR[2]<0) && (geo_center_link_FoR[2]>-0.0343)){
2298  //triangle 1, 84:95
2299  pushTriangleToTaxelList(84,list_of_taxels);
2300  //triangle 5, 72:83
2301  pushTriangleToTaxelList(72,list_of_taxels);
2302  }
2303 
2304  else if((geo_center_link_FoR[0]>0) && (geo_center_link_FoR[0]<0.0375) && (geo_center_link_FoR[1]<0.0716) && (geo_center_link_FoR[1]>0.0318) && (geo_center_link_FoR[2]<0) && (geo_center_link_FoR[2]>-0.0281)){
2305  //triangle 9, 36:47
2306  pushTriangleToTaxelList(36,list_of_taxels);
2307  //triangle 13, 48:59
2308  pushTriangleToTaxelList(48,list_of_taxels);
2309  }
2310  else{
2311  yWarning("OdeSdlSimulation::mapPositionIntoTaxelList: WARNING: contact at part: %d, coordinates: %f %f %f, but no taxels asigned to this position. \n",skin_part,geo_center_link_FoR[0],geo_center_link_FoR[1],geo_center_link_FoR[2]);
2312  }
2313  break;
2314 
2315 
2316  default:
2317  yWarning("OdeSdlSimulation::mapPositionIntoTaxelList: WARNING: contact at part: %d, but no taxel resolution implemented for this skin part. \n",skin_part);
2318  }
2319 
2320 // if (odeinit.verbosity > 2) {
2321 // yDebug("OdeSdlSimulation::mapPositionIntoTaxelList: contact at part: %d, coordinates: %f %f %f. \n",skin_part,geo_center_link_FoR[0],geo_center_link_FoR[1],geo_center_link_FoR[2]);
2322 // yDebug(" Taxel list: ");
2323 // for (std::vector<unsigned int>::const_iterator it = list_of_taxels.begin() ; it != list_of_taxels.end(); ++it){
2324 // yDebug("%d,",*it);
2325 // }
2326 // yDebug("\n");
2327 // }
2328  return;
2329 }
2330 
2331 //pushes taxel IDs of whole triangle into list_of_taxels, starting from startingTaxelID and skipping 7th and 11th taxels (thermal pads)
2332 void OdeSdlSimulation::pushTriangleToTaxelList(const int startingTaxelID,std::vector<unsigned int>& list_of_taxels)
2333 {
2334  int i = startingTaxelID;
2335  for (i=startingTaxelID;i<startingTaxelID+6;i++){
2336  list_of_taxels.push_back(i);
2337  }
2338  //skipping 7th and 11th taxel - thermal pads
2339  for (i = startingTaxelID + 7; i < startingTaxelID + 10; i++){
2340  list_of_taxels.push_back(i);
2341  }
2342  list_of_taxels.push_back(startingTaxelID+11);
2343 }
2344 
2345 void OdeSdlSimulation::mapFingertipIntoTaxelList(const HandPart hand_part,std::vector<unsigned int>& list_of_taxels)
2346 {
2347  int i=0;
2348  switch(hand_part)
2349  {
2350  case INDEX:
2351  for(i=0; i<=11; i++){
2352  list_of_taxels.push_back(i);
2353  }
2354  break;
2355  case MIDDLE:
2356  for(i=12; i<=23; i++){
2357  list_of_taxels.push_back(i);
2358  }
2359  break;
2360  case RING:
2361  for(i=24; i<=35; i++){
2362  list_of_taxels.push_back(i);
2363  }
2364  break;
2365  case LITTLE:
2366  for(i=36; i<=47; i++){
2367  list_of_taxels.push_back(i);
2368  }
2369  break;
2370  case THUMB:
2371  for(i=48; i<=59; i++){
2372  list_of_taxels.push_back(i);
2373  }
2374  break;
2375  default:
2376  printf("Warning: OdeSdlSimulation::mapFingertipIntoTaxelList: unexpected HandPart: %d. Pushing fake taxel ID \n",hand_part);
2377  list_of_taxels.push_back(FAKE_TAXEL_ID);
2378 
2379  }
2380 
2381 
2382 }
2383 
2384 //Auxiliary function to print class of geom - according to section 9.5 of ODE manual
2385 std::string OdeSdlSimulation::getGeomClassName(const int geom_class,std::string & s)
2386 {
2387  switch(geom_class){
2388  case 0:
2389  s = "sphere";
2390  break;
2391  case 1:
2392  s = "box";
2393  break;
2394  case 2:
2395  s = "capsule";
2396  break;
2397  case 3:
2398  s = "cylinder";
2399  break;
2400  case 4:
2401  s = "plane";
2402  break;
2403  case 8:
2404  s= "triangle mesh";
2405  break;
2406  case 10:
2407  case 11:
2408  s = "simple space";
2409  break;
2410  case 12:
2411  s="hash space";
2412  break;
2413  default:
2414  s ="unknown type";
2415  break;
2416  }
2417  return s;
2418 
2419 }
static float hpr[8]
Definition: iCub_Sim.cpp:48
static float lasty
Definition: iCub_Sim.cpp:76
const Skin_2_Link SkinPart_2_LinkNum[SKIN_PART_SIZE]
Definition: common.h:117
static int mouseDiffy
Definition: iCub_Sim.cpp:52
static bool eyeCams
Definition: iCub_Sim.cpp:88
static SDL_Surface * image
Definition: iCub_Sim.cpp:83
#define MAX_PART
iCub::iKin::iCubInertialSensor iKinInertialSensor
Definition: iCub.h:272
const std::string SkinPart_s[]
Definition: common.h:63
virtual bool getTrqData(Bottle data)
Definition: iCub_Sim.cpp:1619
struct timeval prevTime[BOARD_NUM]
virtual bool shouldSendInertial()=0
static RobotConfig * robot_config
Definition: iCub_Sim.cpp:87
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:63
static int width_left
Definition: iCub_Sim.cpp:92
virtual void sendTouchRightHand(yarp::os::Bottle &report)=0
static int mouse1_down_y
Definition: iCub_Sim.cpp:60
#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 const double MORE_EXTRA_MARGIN_FOR_TAXEL_POSITION_M
Definition: iCub_Sim.cpp:126
static double duration
Definition: iCub_Sim.cpp:79
bool sync
Definition: OdeInit.h:69
dSpaceID body_geom_space_id
Definition: OdeInit.h:79
iCub::iKin::iCubArm iKinRightArm
Definition: iCub.h:271
dBodyID inertialBody
Definition: iCub.h:96
static dJointFeedback touchSensorFeedbacks[MAX_DJOINT_FEEDBACKSTRUCTS]
Definition: iCub_Sim.cpp:118
OdeSdlSimulation()
Constructor.
Definition: iCub_Sim.cpp:1393
void DrawVideo(VideoTexture *video)
Definition: rendering.cpp:457
virtual void sendInertial(yarp::os::Bottle &report)=0
int verbosity
Definition: OdeInit.h:71
virtual void sendSkinEvents(iCub::skinDynLib::skinContactList &skinContactListReport)=0
#define LEFT_ARM
void DrawGround(bool wireframe)
Definition: rendering.cpp:123
string eyeLidsPortName
Definition: iCub.h:86
int MODEL_NUM
Definition: world.h:174
static long startTime
Definition: iCub_Sim.cpp:78
virtual void checkTorques()=0
void getSkinAndBodyPartFromSpaceAndGeomID(const dSpaceID geomSpaceID, const dGeomID geomID, SkinPart &skinPart, BodyPart &bodyPart, HandPart &handPart, bool &skinCoverFlag, bool &fingertipFlag)
Definition: iCub.cpp:4215
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:89
static int height_right
Definition: iCub_Sim.cpp:95
dSpaceID iCubRightArmSpace
Definition: iCub.h:89
static double dstep
Definition: iCub_Sim.cpp:39
ICubSim * _iCub
Definition: OdeInit.h:66
static float zoom
Definition: iCub_Sim.cpp:74
static VideoTexture * video
Definition: iCub_Sim.cpp:85
virtual bool shouldSendTouchLeftForearm()=0
static long ode_step_length
Definition: iCub_Sim.cpp:38
void setName(string module)
dBodyID l_hand
Definition: iCub.h:163
const dReal * pos
Definition: iCub_Sim.cpp:65
static float view2_hpr[3]
Definition: iCub_Sim.cpp:73
dSpaceID iCubLeftArmSpace
Definition: iCub.h:89
static int stop
Definition: iCub_Sim.cpp:44
dBodyID r_hand
Definition: iCub.h:163
static float ydistance
Definition: iCub_Sim.cpp:68
zeros(2, 2) eye(2
string getName()
Definition: OdeInit.h:90
dWorldID world
Definition: OdeInit.h:57
static float ypos
Definition: iCub_Sim.cpp:75
Matrix H_r2w
Definition: iCub.h:284
static float angle
Definition: iCub_Sim.cpp:75
ConstString actSkinEmul
Definition: iCub.h:80
std::set< unsigned int > taxelsTouched
Definition: iCub_Sim.cpp:105
bool add(const char *port, int textureIndex)
Definition: VideoTexture.h:78
static float yrot
Definition: iCub_Sim.cpp:75
static float zrot
Definition: iCub_Sim.cpp:75
static RobotStreamer * robot_streamer
Definition: iCub_Sim.cpp:86
yarp::os::ConstString texture
Definition: world.h:295
static int cameraSizeWidth
Definition: iCub_Sim.cpp:99
static double frames
Definition: iCub_Sim.cpp:79
Semaphore ODE_access(1)
list< contactOnSkin_t > listOfSkinContactInfos
Definition: OdeInit.h:84
#define MAX_DJOINT_FEEDBACKSTRUCTS
Definition: iCub_Sim.cpp:116
int modelTexture[100]
Definition: world.h:261
virtual bool shouldSendTouchLeftArm()=0
static int width
Definition: iCub_Sim.cpp:56
static float view_xyz[3]
Definition: iCub_Sim.cpp:70
double contactFrictionCoefficient
Definition: OdeInit.h:74
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:77
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:1321
tuple name
Definition: manager.py:710
Definition: main.cpp:147
Bottle fullSkinActivationUpperArm
Definition: iCub.h:280
dSpaceID space
Definition: OdeInit.h:58
static float xrot
Definition: iCub_Sim.cpp:75
iCubSimulationControl ** _controls
Definition: OdeInit.h:73
virtual void sendTouchTorso(yarp::os::Bottle &report)=0
virtual bool shouldSendSkinEvents()=0
static double seconds
Definition: iCub_Sim.cpp:79
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
static float view2_xyz[3]
Definition: iCub_Sim.cpp:72
dSpaceID iCubLegsSpace
Definition: iCub.h:89
static int v
Definition: iCub_Sim.cpp:45
static float xrotrad
Definition: iCub_Sim.cpp:77
void sendHomePos()
Definition: OdeInit.cpp:133
static OdeInit & get()
Definition: OdeInit.cpp:170
virtual void sendTouchRightForearm(yarp::os::Bottle &report)=0
static bool picking
Definition: iCub_Sim.cpp:53
static long gl_frame_length
Definition: iCub_Sim.cpp:37
Bottle fullSkinActivationForearm
Definition: iCub.h:279
static long finishTime
Definition: iCub_Sim.cpp:78
static const int TORSO_DOF
Definition: common.h:194
static int height
Definition: iCub_Sim.cpp:57
dJointID contact_joint
Definition: OdeInit.h:82
Bottle emptySkinActivationForearm
Definition: iCub.h:276
static bool extractImages
Definition: iCub_Sim.cpp:84
virtual int getWorldTimestep()=0
static int mouse0_down_x
Definition: iCub_Sim.cpp:59
This file is responsible for the initialisation of the world parameters that are controlled by ODE...
for j
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:119
static float lastx
Definition: iCub_Sim.cpp:76
virtual bool getImage(yarp::sig::ImageOf< yarp::sig::PixelRgb > &target)
Definition: iCub_Sim.cpp:1630
ConstString actTorso
Definition: iCub.h:80
static double FPS
Definition: iCub_Sim.cpp:79
static float zpos
Definition: iCub_Sim.cpp:75
static int mouse0_down_y
Definition: iCub_Sim.cpp:60
Bottle emptySkinActivationHand
Definition: iCub.h:275
void draw()
Definition: world.cpp:63
#define CTRL_RAD2DEG
Definition: iCub_Sim.cpp:32
Bottle emptySkinActivationUpperArm
Definition: iCub.h:277
void clearBuffer()
Signal that we're done with a view.
Definition: iCub_Sim.cpp:1389
dGeomID inertialGeom
Definition: iCub.h:97
static float test[3]
Definition: iCub_Sim.cpp:80
double torqueData[100]
Definition: iCub.h:237
static const double EXTRA_MARGIN_FOR_TAXEL_POSITION_M
Definition: iCub_Sim.cpp:123
void simLoop(int h, int w)
Run the simulation.
Definition: iCub_Sim.cpp:1222
static Uint32 colorkey
Definition: iCub_Sim.cpp:82
static float xpos
Definition: iCub_Sim.cpp:75
virtual void sendTouchLeftHand(yarp::os::Bottle &report)=0
static float xdistance
Definition: iCub_Sim.cpp:69
static bool simrun
Definition: iCub_Sim.cpp:42
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:66
dContactGeom contact_geom
Definition: OdeInit.h:81
static int contactPoint
Definition: iCub_Sim.cpp:51
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:1605
static float view_hpr[3]
Definition: iCub_Sim.cpp:71
void drawSkyDome(float x, float y, float z, float width, float height, float length)
Definition: rendering.cpp:160
dGeomID Reye1_geom
Definition: iCub.h:182
worldSim * _wrld
Definition: OdeInit.h:67
void loadTexture(yarp::os::ConstString texture, int numTexture)
Definition: world.cpp:173
virtual bool shouldSendTouchRightForearm()=0
#define RIGHT_ARM
static float * VAD2
Definition: iCub_Sim.cpp:64
static bool glrun
Definition: iCub_Sim.cpp:41
ConstString actRArm
Definition: iCub.h:80
ConstString actStartHomePos
Definition: iCub.h:80
static double fov_right
Definition: iCub_Sim.cpp:97
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:79
dSpaceID iCub
Definition: iCub.h:88
bool checkSync(bool reset=false)
Definition: iCub_Sim.cpp:1610
bool stop
Definition: OdeInit.h:68
static int width_right
Definition: iCub_Sim.cpp:93
virtual bool shouldSendTouchLeftHand()=0
static int mouse_ray_x
Definition: iCub_Sim.cpp:61
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:94
static int mouse_ray_y
Definition: iCub_Sim.cpp:62
static int mouse1_down_x
Definition: iCub_Sim.cpp:59
static int cameraSizeHeight
Definition: iCub_Sim.cpp:100
#define PART_HEAD
dBodyID lhandfingers3
Definition: iCub.h:133
#define FAKE_TAXEL_ID
Definition: iCub_Sim.h:61
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:96
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:67
static float xyz[3]
Definition: iCub_Sim.cpp:47
static float cam_rx
Definition: iCub_Sim.cpp:54
static std::map< SkinPart, contactICubSkinEmul_t > contactICubSkinEmulMap
Definition: iCub_Sim.cpp:108
bool setup_opengl(ResourceFinder &finder)
Definition: rendering.cpp:65
Bottle fullSkinActivationTorso
Definition: iCub.h:281
void init(RobotStreamer *streamer, RobotConfig *config)
Initialization.
Definition: iCub_Sim.cpp:1396
std::map< dGeomID, string > dGeomNames
Definition: iCub.h:92
static float cam_ry
Definition: iCub_Sim.cpp:54
ConstString actLArm
Definition: iCub.h:80
static bool START_SELF_COLLISION_DETECTION
Definition: iCub_Sim.cpp:121
yarp::sig::Vector setAng(const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i].
static void sighandler(int)
Definition: main.cpp:152
static int mouseDiffx
Definition: iCub_Sim.cpp:52
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:49
dSpaceID iCubTorsoSpace
Definition: iCub.h:89