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