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