iCub-main
iCub.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: Vadim Tikhanoff
6 * email: vadim.tikhanoff@iit.it
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 
28 #include <cstdio>
29 #include <cstring>
30 #include <yarp/os/Bottle.h>
31 #include <yarp/os/LogStream.h>
32 #include <yarp/math/Math.h>
33 #include <iCub/iKin/iKinFwd.h>
34 #include <iCub/ctrl/math.h>
35 #include <string>
36 #include <iostream>
37 #include <fstream>
38 #include <sstream>
39 
40 #include "iCub.h"
41 #include "EyeLidsController.h"
42 
43 #include "MS3D.h"
44 #include "xloader.h"
45 
46 using std::string;
47 using std::cout;
48 using std::endl;
49 
50 using namespace iCub::iKin;
51 using namespace iCub::ctrl;
52 using namespace yarp::math;
53 
54 #ifndef CTRL_DEG2RAD
55 #define CTRL_DEG2RAD M_PI/180.0
56 #endif
57 #ifndef CTRL_RAD2DEG
58 #define CTRL_RAD2DEG 180.0/M_PI
59 #endif
60 
64 
65 
67 }
68 
69 // The maximum force that the simulated touch sensor can sense.
70 // Upper limit for the touch sensor is currently set as 100N. It can safely be changed.
71 #define TOUCH_SENSOR_MAX_FORCE 100.0
72 
73 // This function returns the touch sensor value in the range 0-1.
74 // Input fb is the feedback structure of the contactJoint on a touch sensitive body.
75 double getTouchSensorValue (dJointFeedback* fb) {
76 
77  double forceOnBody = sqrt(fb->f1[0]*fb->f1[0] + fb->f1[1]*fb->f1[1] + fb->f1[2]*fb->f1[2]);
78 
79  // If the force on the touch sensor is more than it can sense, set the force to the maximum value.
80  if (forceOnBody > TOUCH_SENSOR_MAX_FORCE) {
81  forceOnBody = TOUCH_SENSOR_MAX_FORCE;
82  }
83  // normalize the touch sensor output to the range 0-1.
84  forceOnBody = forceOnBody / TOUCH_SENSOR_MAX_FORCE;
85 
86  return forceOnBody;
87 }
88 
89 //function to check the activation of the sensor on the selected body... - this is for the case of the respective hand body part off
91  dJointID j;
92  double result = 0;
93  j = dBodyGetJoint(id, 0);
94 
95  if(dJointGetType(j) == dJointTypeContact) {
96  // get the feedback from the joint.
97  dJointFeedback * fb = dJointGetFeedback (j);
98  // get the sensor value.
99  result = getTouchSensorValue(fb);
100  }
101 
102  return result;
103 }
104 
105 //function to check the activation of the sensor on the selected body... - this is for the case of the respective hand body part on
107  dJointID j;
108  double result = 0;
109 
110  if (bodyToCheck == 26 || bodyToCheck == 27 || bodyToCheck == 45 || bodyToCheck == 46)
111  {
112  if (bodyToCheck == 26 || bodyToCheck == 27){
113  j = dBodyGetJoint(lhandfingers3, 0);
114  } else {
115  j = dBodyGetJoint(rhandfingers3, 0);
116  }
117  } else {
118  j = dBodyGetJoint(body[bodyToCheck], 0);
119  }
120  if(dJointGetType(j) == dJointTypeContact) {
121  // get the feedback from the joint.
122  dJointFeedback * fb = dJointGetFeedback (j);
123  // get the sensor value.
124  result = getTouchSensorValue(fb);
125  }
126 
127  return result;
128 }
129 
130 //function to check the activation of the sensor on the selected body...
131 bool ICubSim::checkTouchSensor(dBodyID id){
132  dJointID j;
133  bool result = false;
134  j = dBodyGetJoint(id, 0);
135  if(dJointGetType(j) == dJointTypeContact) {
136  result = true;
137  }
138  return result;
139 }
140 bool ICubSim::checkTouchSensor(int bodyToCheck) {
141  dJointID j;
142  bool result = false;
143 
144  if (bodyToCheck == 26 || bodyToCheck == 27 || bodyToCheck == 45 || bodyToCheck == 46)
145  {
146  if (bodyToCheck == 26 || bodyToCheck == 27){
147  j = dBodyGetJoint(lhandfingers3, 0);
148  }
149  else{
150  j = dBodyGetJoint(rhandfingers3, 0);
151  }
152  }
153 
154  else{
155  j = dBodyGetJoint(body[bodyToCheck], 0);
156  }
157  if(dJointGetType(j) == dJointTypeContact) {
158  result = true;
159  }
160 
161  return result;
162 }
163 
165  int x;
166  for(x = 0; x < 10; x++) {
167  LLeg_speed[x] = 0.0;
168  RLeg_speed[x] = 0.0;
169  LLeg_torques[x] = 0.0;
170  RLeg_torques[x] = 0.0;
171  }
172 
173  for(x=0; x<7; x++){
174  Torso_speed[x] = 0.0;
175  Torso_torques[x] = 0.0;
176  }
177 
178  for(x = 0; x < 25; x++) {
179  la_speed[x] = 0.0;
180  la_speed1[x] = 0.0;
181  la_torques[x] = 0.0;
182  ra_speed[x] = 0.0;
183  ra_speed1[x] = 0.0;
184  ra_torques[x] = 0.0;
185  h_speed[x] = 0.0;
186  h_torques[x] = 0.0;
187  }
188 }
189 
191  OdeInit& odeInit = OdeInit::get();
192  int cm[25];
193  memset(cm, VOCAB_CM_VELOCITY, sizeof(int) * 25);
194  /*for(int i=0;i<5;i++){
195  yDebug("LAJoint %d angle: %.3f\t", i, 180.0/M_PI*dJointGetHingeAngle(LAjoints[i]));
196  yDebug("vel_d: %.3f\t", 180.0/M_PI*dJointGetHingeParam(LAjoints[i], dParamVel));
197  yDebug("fmax: %.1f\t", dJointGetHingeParam(LAjoints[i], dParamFMax));
198  yDebug("torque_d: %.1f\n", la_torques[i]);
199  }*/
200  /*for(int i=0;i<5;i++){
201  yDebug("RAJoint %d angle: %.1f\t", i, 180.0/M_PI*dJointGetHingeAngle(RAjoints[i]));
202  yDebug("vel_d: %.1f\t", 180.0/M_PI*dJointGetHingeParam(RAjoints[i], dParamVel));
203  yDebug("fmax: %.1f\t", dJointGetHingeParam(RAjoints[i], dParamFMax));
204  yDebug("torque_d: %.1f\n", ra_torques[i]);
205  }*/
206  /*for(int i=0;i<3;i++){
207  yDebug("TorsoJoint %d angle: %.1f\t", i, 180.0/M_PI*dJointGetHingeAngle(Torsojoints[i]));
208  yDebug("vel_d: %.1f\t", 180.0/M_PI*dJointGetHingeParam(Torsojoints[i], dParamVel));
209  yDebug("fmax: %.1f\t", dJointGetHingeParam(Torsojoints[i], dParamFMax));
210  yDebug("torque_d: %.1f\n", Torso_torques[i]);
211  }*/
212 
213  // BodyPart: 1 left arm, 2 right arm, 3 head, 4 left leg, 5 right leg, 6 torso
214  // LEFT LEG
215  int bodyPart = 4;
216  if(odeInit._controls[bodyPart])
217  odeInit._controls[bodyPart]->getControlModesRaw(cm);
218  for (int x=0; x<6;x++){
219  if(cm[x]==VOCAB_CM_TORQUE)
220  dJointAddHingeTorque(LLegjoints[x], LLeg_torques[x]);
221  else
222  dJointSetHingeParam(LLegjoints[x], dParamVel, LLeg_speed[x]);
223  }
224  // RIGHT LEG
225  bodyPart = 5;
226  if(odeInit._controls[bodyPart])
227  odeInit._controls[bodyPart]->getControlModesRaw(cm);
228  else
229  memset(cm, VOCAB_CM_VELOCITY, sizeof(int) * 25);
230  for (int x=0; x<6;x++){
231  if(cm[x]==VOCAB_CM_TORQUE)
232  dJointAddHingeTorque(RLegjoints[x], RLeg_torques[x]);
233  else
234  dJointSetHingeParam(RLegjoints[x], dParamVel, RLeg_speed[x]);
235  }
236  // TORSO
237  bodyPart = 6;
238  if(odeInit._controls[bodyPart])
239  odeInit._controls[bodyPart]->getControlModesRaw(cm);
240  else
241  memset(cm, VOCAB_CM_VELOCITY, sizeof(int) * 25);
242  for (int x=0; x<5; x++){
243  if(cm[x]==VOCAB_CM_TORQUE)
244  dJointAddHingeTorque(Torsojoints[x], Torso_torques[x]);
245  else
246  dJointSetHingeParam(Torsojoints[x], dParamVel, Torso_speed[x]);
247  }
248  // LEFT ARM
249  bodyPart = 1;
250  if(odeInit._controls[bodyPart])
251  odeInit._controls[bodyPart]->getControlModesRaw(cm);
252  else
253  memset(cm, VOCAB_CM_VELOCITY, sizeof(int) * 25);
254  for (int x=0; x<5; x++){
255  if(cm[x]==VOCAB_CM_TORQUE){
256  dJointAddHingeTorque(LAjoints[x], la_torques[x]);
257  }
258  else
259  dJointSetHingeParam(LAjoints[x], dParamVel, la_speed[x]);
260  }
261 
262  // RIGHT ARM
263  bodyPart = 2;
264  if(odeInit._controls[bodyPart])
265  odeInit._controls[bodyPart]->getControlModesRaw(cm);
266  else
267  memset(cm, VOCAB_CM_VELOCITY, sizeof(int) * 25);
268  for (int x=0; x<5; x++){
269  if(cm[x]==VOCAB_CM_TORQUE){
270  dJointAddHingeTorque(RAjoints[x], ra_torques[x]);
271  }
272  else{
273  dJointSetHingeParam(RAjoints[x], dParamVel, ra_speed[x]);
274  }
275  }
276 
277  for (int x=5; x<6;x++){//for the hand
278  dJointSetUniversalParam(LAjoints[x],dParamVel, la_speed[x]);
279  dJointSetUniversalParam(LAjoints[x], dParamVel2, la_speed1[x]);
280  dJointSetUniversalParam(RAjoints[x],dParamVel, ra_speed[x]);
281  dJointSetUniversalParam(RAjoints[x], dParamVel2, ra_speed1[x]);
282  }
283  for (int x=6; x<25;x++){
284  if (x!=9 && x!=13 && x!=17 && x!=21 && x!=22){
285  dJointSetHingeParam(LAjoints[x], dParamVel, la_speed[x]);
286  dJointSetHingeParam(RAjoints[x], dParamVel, ra_speed[x]);
287  }
288  }
289  for (int x=22; x<23;x++){//for the hands
290  dJointSetUniversalParam(LAjoints[x], dParamVel, la_speed[x]);
291  dJointSetUniversalParam(LAjoints[x], dParamVel2, la_speed1[x]);
292  dJointSetUniversalParam(RAjoints[x], dParamVel, ra_speed[x]);
293  dJointSetUniversalParam(RAjoints[x], dParamVel2, ra_speed1[x]);
294  }
295 
296  // HEAD
297  bodyPart = 3;
298  if(odeInit._controls[bodyPart])
299  odeInit._controls[bodyPart]->getControlModesRaw(cm);
300  else
301  memset(cm, VOCAB_CM_VELOCITY, sizeof(int) * 25);
302  for (int x=0; x<6; x++){
303  if(cm[x]==VOCAB_CM_TORQUE)
304  dJointAddHingeTorque(Hjoints[x], h_torques[x]);
305  else
306  dJointSetHingeParam(Hjoints[x], dParamVel, h_speed[x]);
307  }
308 
309  /*dMatrix3 R;
310  dRFromAxisAndAngle(R,1,0,0,eyeLids.eyeLidsRotation);
311  dBodySetRotation(topEyeLid,R);
312  dRFromAxisAndAngle(R,1,0,0,-eyeLids.eyeLidsRotation);
313  dBodySetRotation(bottomEyeLid,R);*/
314 }
315 
317  if (reinitialized)
318  {
319  glFinish();
320  glFlush();
321  if(actHeadCover == "on")
322  {
323  iCubHeadModel->reloadTextures();
324  topEyeLidModel->reloadTextures();
325  bottomEyeLidModel->reloadTextures();
326  }
327 
328  if (model_ThreeD_obj.size()){
329  model_ThreeD_obj[(*model_ThreeD_obj.begin()).first].reloadTexture(textureName[0], modelTexture[0]);
330  }
331  reinitialized = false;
332  }
333 
334  if (actScreen == "on")
335  {
336  glColor3d(1.0,1.0,1.0);
337  glPushMatrix(); //opengl: glPushMatrix pushes the current matrix stack down by one, duplicating the current matrix. That is, after a glPushMatrix call, the matrix on top of the stack is identical to the one below it.
338  LDEsetM(dGeomGetPosition(screenGeom),dGeomGetRotation(screenGeom)); //this makes a homo transformation matrix out of the pos and ori and multiplies the opengl "current" matrix with it and sets the current matrix to the new one
339  DrawBox(1.0,1.0,0.001,false,textured,15);glPopMatrix();
340  }
341 
342  //glColor3d(1.0,0.0,1.0);
343  //glPushMatrix();LDEsetM(dGeomGetPosition(geom_cube[0]),dGeomGetRotation(geom_cube[0]));DrawBox(0.1,2.005,0.1,false,false,0);glPopMatrix();
344 
345  if (actLegs == "off"){
346  glColor3d(1.0,1.0,1.0);
347  if (actLegsCovers == "on")
348  {
349  glPushMatrix();
350  if (actCoversCol == "on"){ //in this case, the covers are placeable geoms initialized in initCovers() - we can retrieve their coordinates
351  //left leg
352  LDEsetM(dGeomGetPosition(model_ThreeD_obj["leftFoot"].geom),dGeomGetRotation(model_ThreeD_obj["leftFoot"].geom));
353  DrawX( model_trimesh["leftFoot"], modelTexture[0]);
354  glPopMatrix();
355 
356  glPushMatrix();
357  LDEsetM(dGeomGetPosition(model_ThreeD_obj["lowerLeftLeg"].geom),dGeomGetRotation(model_ThreeD_obj["lowerLeftLeg"].geom));
358  DrawX( model_trimesh["lowerLeftLeg"], modelTexture[0]);
359  glPopMatrix();
360 
361  glPushMatrix();
362  LDEsetM(dGeomGetPosition(model_ThreeD_obj["upperLeftLeg"].geom),dGeomGetRotation(model_ThreeD_obj["upperLeftLeg"].geom));
363  DrawX( model_trimesh["upperLeftLeg"], modelTexture[0]);
364  glPopMatrix();
365 
366  //right leg
367  glPushMatrix();
368  LDEsetM(dGeomGetPosition(model_ThreeD_obj["rightFoot"].geom),dGeomGetRotation(model_ThreeD_obj["rightFoot"].geom));
369  DrawX( model_trimesh["rightFoot"], modelTexture[0]);
370  glPopMatrix();
371 
372  glPushMatrix();
373  LDEsetM(dGeomGetPosition(model_ThreeD_obj["lowerRightLeg"].geom),dGeomGetRotation(model_ThreeD_obj["lowerRightLeg"].geom));
374  DrawX( model_trimesh["lowerRightLeg"], modelTexture[0]);
375  glPopMatrix();
376 
377  glPushMatrix();
378  LDEsetM(dGeomGetPosition(model_ThreeD_obj["upperRightLeg"].geom),dGeomGetRotation(model_ThreeD_obj["upperRightLeg"].geom));
379  DrawX( model_trimesh["upperRightLeg"], modelTexture[0]);
380  glPopMatrix();
381  } else { // covers are not placeable geoms but only "eye-candy" - coordinates need to be retrieved from other geoms
382  //left leg
383  LDEsetM(dGeomGetPosition(l_leg1_geom),dGeomGetRotation(l_leg1_geom)); //DRAW THE MODEL
384  DrawX( model_trimesh["leftFoot"], modelTexture[0]);
385  glPopMatrix();
386 
387  glPushMatrix();
388  LDEsetM(dGeomGetPosition(l_leg3_geom),dGeomGetRotation(l_leg3_geom)); //DRAW THE MODEL
389  DrawX( model_trimesh["lowerLeftLeg"], modelTexture[0]);
390  glPopMatrix();
391 
392  glPushMatrix();
393  LDEsetM(dGeomGetPosition(l_leg6_geom),dGeomGetRotation(l_leg5_geom)); //DRAW THE MODEL
394  DrawX( model_trimesh["upperLeftLeg"], modelTexture[0]);
395  glPopMatrix();
396 
397  //right leg
398  glPushMatrix();
399  LDEsetM(dGeomGetPosition(r_leg1_geom),dGeomGetRotation(r_leg1_geom)); //DRAW THE MODEL
400  DrawX( model_trimesh["rightFoot"], modelTexture[0]);
401  glPopMatrix();
402 
403  glPushMatrix();
404  LDEsetM(dGeomGetPosition(r_leg3_geom),dGeomGetRotation(r_leg3_geom)); //DRAW THE MODEL
405  DrawX( model_trimesh["lowerRightLeg"], modelTexture[0]);
406  glPopMatrix();
407 
408  glPushMatrix();
409  LDEsetM(dGeomGetPosition(r_leg6_geom),dGeomGetRotation(r_leg5_geom)); //DRAW THE MODEL
410  DrawX( model_trimesh["upperRightLeg"], modelTexture[0]);
411  glPopMatrix();
412  }
413  } //leg covers on
414 
415  // left leg
416  if ((actSelfCol == "on") || (actCoversCol == "on")){ //the foot "box" is drawn - in this regime, we want to draw all objects that are relevant for the simulation
417  glColor3d(0.9,0.9,0.9);
418  glPushMatrix();LDEsetM(dGeomGetPosition(l_leg0_geom),dGeomGetRotation(l_leg0_geom));
419  DrawBox(0.054,0.004,0.13,false,textured,2);glPopMatrix();//Taken from ODE use Y, Z, X
420  }
421  else {
422  if (actLegsCovers == "off") //if covers are off, the foot "box" is drawn; if they are on, it is not drawn as it is slightly longer and is visually not ideal
423  {
424  glColor3d(0.9,0.9,0.9);
425  glPushMatrix();LDEsetM(dGeomGetPosition(l_leg0_geom),dGeomGetRotation(l_leg0_geom));
426  DrawBox(0.054,0.004,0.13,false,textured,2);glPopMatrix();//Taken from ODE use Y, Z, X
427  }
428  }
429 
430  //the rest is drawn irrespective whether covers are on
431  glColor3d(0.5,0.5,0.5);
432  glPushMatrix(); LDEsetM(dGeomGetPosition(l_leg1_geom),dGeomGetRotation(l_leg1_geom));
433  DrawCylinder(0.027,0.095,false,textured,2);glPopMatrix();
434 
435  glPushMatrix(); LDEsetM(dGeomGetPosition(l_leg2_geom),dGeomGetRotation(l_leg2_geom));
436  DrawCylinder(0.0245,0.063,false,textured,2);glPopMatrix();
437 
438  glColor3d(1.0,1.0,1.0);
439  glPushMatrix(); LDEsetM(dGeomGetPosition(l_leg3_geom),dGeomGetRotation(l_leg3_geom));
440  DrawCylinder(0.0315,fabs(jP_leftLeg[2][2]-jP_leftLeg[1][2]),false,textured,2);glPopMatrix();
441 
442  glColor3d(0.5,0.5,0.5);
443  glPushMatrix(); LDEsetM(dGeomGetPosition(l_leg4_geom),dGeomGetRotation(l_leg4_geom));
444  DrawCylinder(0.0315,0.077,false,textured,2);glPopMatrix();
445  glColor3d(1.0,1.0,1.0);
446  glPushMatrix(); LDEsetM(dGeomGetPosition(l_leg5_geom),dGeomGetRotation(l_leg5_geom));
447  DrawCylinder(0.034,0.224,false,textured,2);glPopMatrix();
448 
449  glColor3d(0.5,0.5,0.5);
450  glPushMatrix(); LDEsetM(dGeomGetPosition(l_leg6_geom),dGeomGetRotation(l_leg6_geom));
451  DrawCylinder(0.031,0.075,false,textured,2);glPopMatrix();
452  glColor3d(1.0,1.0,1.0);
453  glPushMatrix(); LDEsetM(dGeomGetPosition(l_leg7_geom),dGeomGetRotation(l_leg7_geom));
454  DrawCylinder(0.038,0.013,false,textured,2);glPopMatrix();
455 
456 
457  // right leg
458  if ((actSelfCol == "on") || (actCoversCol == "on")){ //the foot "box" is drawn - in this regime, we want to draw all objects that are relevant for the simulation
459  glColor3d(0.9,0.9,0.9);
460  glPushMatrix();LDEsetM(dGeomGetPosition(r_leg0_geom),dGeomGetRotation(r_leg0_geom));
461  DrawBox(0.054,0.004,0.13,false,textured,2);glPopMatrix();//Taken from ODE use Y, Z, X
462  }
463  else {
464  if (actLegsCovers == "off") //if covers are off, the foot "box" is drawn; if they are on, it is not drawn as it is slightly longer and is visually not ideal
465  {
466  glColor3d(0.9,0.9,0.9);
467  glPushMatrix();LDEsetM(dGeomGetPosition(r_leg0_geom),dGeomGetRotation(r_leg0_geom));
468  DrawBox(0.054,0.004,0.13,false,textured,2);glPopMatrix();//Taken from ODE use Y, Z, X
469  }
470  }
471  //the rest is drawn irrespective whether covers are on
472  glColor3d(0.5,0.5,0.5);
473  glPushMatrix(); LDEsetM(dGeomGetPosition(r_leg1_geom),dGeomGetRotation(r_leg1_geom));
474  DrawCylinder(0.027,0.095,false,textured,2);glPopMatrix();
475 
476  glPushMatrix(); LDEsetM(dGeomGetPosition(r_leg2_geom),dGeomGetRotation(r_leg2_geom));
477  DrawCylinder(0.0245,0.063,false,textured,2);glPopMatrix();
478 
479  glColor3d(1.0,1.0,1.0);
480  glPushMatrix(); LDEsetM(dGeomGetPosition(r_leg3_geom),dGeomGetRotation(r_leg3_geom));
481  DrawCylinder(0.0315,fabs(jP_rightLeg[2][2]-jP_rightLeg[1][2]),false,textured,2);glPopMatrix();
482 
483  glColor3d(0.5,0.5,0.5);
484  glPushMatrix(); LDEsetM(dGeomGetPosition(r_leg4_geom),dGeomGetRotation(r_leg4_geom));
485  DrawCylinder(0.0315,0.077,false,textured,2);glPopMatrix();
486 
487  glColor3d(1.0,1.0,1.0);
488  glPushMatrix(); LDEsetM(dGeomGetPosition(r_leg5_geom),dGeomGetRotation(r_leg5_geom));
489  DrawCylinder(0.034,0.224,false,textured,2);glPopMatrix();
490 
491  glColor3d(0.5,0.5,0.5);
492  glPushMatrix(); LDEsetM(dGeomGetPosition(r_leg6_geom),dGeomGetRotation(r_leg6_geom));
493  DrawCylinder(0.031,0.075,false,textured,2);glPopMatrix();
494 
495  glColor3d(1.0,1.0,1.0);
496  glPushMatrix(); LDEsetM(dGeomGetPosition(r_leg7_geom),dGeomGetRotation(r_leg7_geom));
497  DrawCylinder(0.038,0.013,false,textured,2);glPopMatrix();
498 
499  }else{ //(actLegs == "on")
500 
501  // left leg
502  if ((actSelfCol == "on") || (actCoversCol == "on")){ //the foot "box" is drawn - in this regime, we want to draw all objects that are relevant for the simulation
503  glColor3d(1.0,1.0,1.0);
504  glPushMatrix();LDEsetM(dBodyGetPosition(leftLeg[0]),dBodyGetRotation(leftLeg[0]));
505  DrawBox(0.054,0.004,0.13,false,textured,2);glPopMatrix();//Taken from ODE use Y, Z, X
506  }
507  else {
508  if (actLegsCovers == "off") //if covers are off, the foot "box" is drawn; if they are on, it is not drawn as it is slightly longer and is visually not ideal
509  {
510  glColor3d(1.0,1.0,1.0);
511  glPushMatrix();LDEsetM(dBodyGetPosition(leftLeg[0]),dBodyGetRotation(leftLeg[0]));
512  DrawBox(0.054,0.004,0.13,false,textured,2);glPopMatrix();//Taken from ODE use Y, Z, X
513  }
514  }
515 
516  if (actLegsCovers == "on")
517  {
518  glColor3d(1.0,1.0,1.0);
519  if (actCoversCol == "on"){ //in this case, the covers are placeable geoms initialized in initCovers() - we can retrieve their coordinates
520 
521  glPushMatrix();
522  LDEsetM(dGeomGetPosition(model_ThreeD_obj["leftFoot"].geom),dGeomGetRotation(model_ThreeD_obj["leftFoot"].geom));
523  DrawX( model_trimesh["leftFoot"], modelTexture[0]);
524  glPopMatrix();
525 
526  glPushMatrix();
527  LDEsetM(dGeomGetPosition(model_ThreeD_obj["lowerLeftLeg"].geom),dGeomGetRotation(model_ThreeD_obj["lowerLeftLeg"].geom));
528  DrawX( model_trimesh["lowerLeftLeg"], modelTexture[0]);
529  glPopMatrix();
530 
531  glPushMatrix();
532  LDEsetM(dGeomGetPosition(model_ThreeD_obj["upperLeftLeg"].geom),dGeomGetRotation(model_ThreeD_obj["upperLeftLeg"].geom));
533  DrawX( model_trimesh["upperLeftLeg"], modelTexture[0]);
534  glPopMatrix();
535 
536  //right leg
537  glPushMatrix();
538  LDEsetM(dGeomGetPosition(model_ThreeD_obj["rightFoot"].geom),dGeomGetRotation(model_ThreeD_obj["rightFoot"].geom));
539  DrawX( model_trimesh["rightFoot"], modelTexture[0]);
540  glPopMatrix();
541 
542  glPushMatrix();
543  LDEsetM(dGeomGetPosition(model_ThreeD_obj["lowerRightLeg"].geom),dGeomGetRotation(model_ThreeD_obj["lowerRightLeg"].geom));
544  DrawX( model_trimesh["lowerRightLeg"], modelTexture[0]);
545  glPopMatrix();
546 
547  glPushMatrix();
548  LDEsetM(dGeomGetPosition(model_ThreeD_obj["upperRightLeg"].geom),dGeomGetRotation(model_ThreeD_obj["upperRightLeg"].geom));
549  DrawX( model_trimesh["upperRightLeg"], modelTexture[0]);
550  glPopMatrix();
551  } else { // covers are not placeable geoms but only "eye-candy" - coordinates need to be retrieved from other geoms
552 
553  glPushMatrix();
554  LDEsetM(dBodyGetPosition(leftLeg[1]),dBodyGetRotation(leftLeg[0])); //DRAW THE MODEL
555  DrawX( model_trimesh["leftFoot"], modelTexture[0]);
556  glPopMatrix();
557 
558  glPushMatrix();
559  LDEsetM(dGeomGetPosition(leftLeg_2_2),dGeomGetRotation(leftLeg_2_2)); //DRAW THE MODEL
560  DrawX( model_trimesh["lowerLeftLeg"], modelTexture[0]);
561  glPopMatrix();
562 
563  glPushMatrix();
564  LDEsetM(dGeomGetPosition(leftLeg_4_2),dGeomGetRotation(leftLeg_3_2)); //DRAW THE MODEL
565  DrawX( model_trimesh["upperLeftLeg"], modelTexture[0]);
566  glPopMatrix();
567 
568  //right leg
569  glPushMatrix();
570  LDEsetM(dBodyGetPosition(rightLeg[1]),dBodyGetRotation(rightLeg[0])); //DRAW THE MODEL
571  DrawX( model_trimesh["rightFoot"], modelTexture[0]);
572  glPopMatrix();
573 
574  glPushMatrix();
575  LDEsetM(dGeomGetPosition(rightLeg_2_2),dGeomGetRotation(rightLeg_2_2)); //DRAW THE MODEL
576  //glTranslatef(0.0, 0.0,0.5*fabs(jP_leftArm[5][2] - jP_leftArm[4][2]));
577  DrawX( model_trimesh["lowerRightLeg"], modelTexture[0]);
578  glPopMatrix();
579 
580  glPushMatrix();
581  LDEsetM(dGeomGetPosition(rightLeg_4_2),dGeomGetRotation(rightLeg_3_2)); //DRAW THE MODEL
582  //glTranslatef(0.0, 0.0,0.5*fabs(jP_leftArm[5][2] - jP_leftArm[4][2]));
583  DrawX( model_trimesh["upperRightLeg"], modelTexture[0]);
584  glPopMatrix();
585  }
586  }
587 
588  glColor3d(0.5,0.5,0.5);
589  glPushMatrix(); LDEsetM(dBodyGetPosition(leftLeg[1]),dBodyGetRotation(leftLeg[1]));
590  DrawCylinder(0.027,0.095,false,textured,2);glPopMatrix(); //foot joint
591 
592  glPushMatrix(); LDEsetM(dGeomGetPosition(leftLeg_2_1),dGeomGetRotation(leftLeg_2_1));
593  DrawCylinder(0.0245,0.063,false,textured,2);glPopMatrix(); //foot joint
594  glColor3d(1.0,1.0,1.0);
595  glPushMatrix(); LDEsetM(dGeomGetPosition(leftLeg_2_2),dGeomGetRotation(leftLeg_2_2));
596  DrawCylinder(0.0315,fabs(jP_leftLeg[2][2]-jP_leftLeg[1][2]),false,textured,2);glPopMatrix(); //tibia
597 
598  glColor3d(0.5,0.5,0.5);
599  glPushMatrix(); LDEsetM(dGeomGetPosition(leftLeg_3_1),dGeomGetRotation(leftLeg_3_1));
600  DrawCylinder(0.0315,0.077,false,textured,2);glPopMatrix(); //knee
601  glColor3d(1.0,1.0,1.0);
602  glPushMatrix(); LDEsetM(dGeomGetPosition(leftLeg_3_2),dGeomGetRotation(leftLeg_3_2));
603  DrawCylinder(0.034,fabs(jP_leftLeg[3][2]-jP_leftLeg[2][2]),false,textured,2);glPopMatrix(); //thigh
604 
605  //glPushMatrix(); LDEsetM(dGeomGetPosition(leftLeg_4_1),dGeomGetRotation(leftLeg_4_1));
606  //DrawSphere(0.017,false,false);glPopMatrix();
607  glColor3d(0.5,0.5,0.5);
608  glPushMatrix(); LDEsetM(dGeomGetPosition(leftLeg_4_2),dGeomGetRotation(leftLeg_4_2));
609  DrawCylinder(0.031,0.075,false,textured,2);glPopMatrix();
610 
611  glColor3d(1.0,1.0,1.0);
612  glPushMatrix(); LDEsetM(dBodyGetPosition(leftLeg[5]),dBodyGetRotation(leftLeg[5]));
613  DrawCylinder(0.038,0.013,false,textured,2);glPopMatrix();
614 
615 
616  //right leg
617  if ((actSelfCol == "on") || (actCoversCol == "on")){ //the foot "box" is drawn - in this regime, we want to draw all objects that are relevant for the simulation
618  glColor3d(0.9,0.9,0.9);
619  glPushMatrix();LDEsetM(dBodyGetPosition(rightLeg[0]),dBodyGetRotation(rightLeg[0]));
620  DrawBox(0.054,0.004,0.13,false,textured,2);glPopMatrix();//Taken from ODE use Y, Z, X
621  }
622  else {
623  if (actLegsCovers == "off") //if covers are off, the foot "box" is drawn; if they are on, it is not drawn as it is slightly longer and is visually not ideal
624  {
625  glColor3d(0.9,0.9,0.9);
626  glPushMatrix();LDEsetM(dBodyGetPosition(rightLeg[0]),dBodyGetRotation(rightLeg[0]));
627  DrawBox(0.054,0.004,0.13,false,textured,2);glPopMatrix();//Taken from ODE use Y, Z, X
628  }
629  }
630 
631  glColor3d(0.5,0.5,0.5);
632  glPushMatrix(); LDEsetM(dBodyGetPosition(rightLeg[1]),dBodyGetRotation(rightLeg[1]));
633  DrawCylinder(0.027,0.095,false,textured,2);glPopMatrix();
634  glPushMatrix(); LDEsetM(dGeomGetPosition(rightLeg_2_1),dGeomGetRotation(rightLeg_2_1));
635  DrawCylinder(0.0245,0.063,false,textured,2);glPopMatrix();
636  glColor3d(1.0,1.0,1.0);
637  glPushMatrix(); LDEsetM(dGeomGetPosition(rightLeg_2_2),dGeomGetRotation(rightLeg_2_2));
638  DrawCylinder(0.0315,fabs(jP_rightLeg[2][2]-jP_rightLeg[1][2]),false,textured,2);glPopMatrix();
639 
640  glColor3d(0.5,0.5,0.5);
641  glPushMatrix(); LDEsetM(dGeomGetPosition(rightLeg_3_1),dGeomGetRotation(rightLeg_3_1));
642  DrawCylinder(0.0315,0.077,false,textured,2);glPopMatrix();
643  glColor3d(1.0,1.0,1.0);
644  glPushMatrix(); LDEsetM(dGeomGetPosition(rightLeg_3_2),dGeomGetRotation(rightLeg_3_2));
645  DrawCylinder(0.034,fabs(jP_rightLeg[3][2]-jP_rightLeg[2][2]),false,textured,2);glPopMatrix();
646  //glPushMatrix(); LDEsetM(dGeomGetPosition(rightLeg_4_1),dGeomGetRotation(rightLeg_4_1));
647  //DrawSphere(0.017,false,false);glPopMatrix();
648 
649  glColor3d(0.5,0.5,0.5);
650  glPushMatrix(); LDEsetM(dGeomGetPosition(rightLeg_4_2),dGeomGetRotation(rightLeg_4_2));
651  DrawCylinder(0.031,0.075,false,textured,2);glPopMatrix();
652  glColor3d(1.0,1.0,1.0);
653  glPushMatrix(); LDEsetM(dBodyGetPosition(rightLeg[5]),dBodyGetRotation(rightLeg[5]));
654  DrawCylinder(0.038,0.013,false,textured,2);glPopMatrix();
655  }
656  if (actTorso == "off"){
657  glColor3d(1.0,1.0,1.0);
658  if (actTorsoCovers == "on")
659  {
660  if (actCoversCol == "on"){ //in this case, the covers are placeable geoms initialized in initCovers() - we can retrieve their coordinates
661  glPushMatrix();
662  LDEsetM(dGeomGetPosition(model_ThreeD_obj["waist"].geom),dGeomGetRotation(model_ThreeD_obj["waist"].geom)); //DRAW THE MODEL
663  DrawX( model_trimesh["waist"], modelTexture[0]);
664  glPopMatrix();
665 
666  glPushMatrix();
667  LDEsetM(dGeomGetPosition(model_ThreeD_obj["torso"].geom),dGeomGetRotation(model_ThreeD_obj["torso"].geom)); //DRAW THE MODEL
668  DrawX( model_trimesh["torso"], modelTexture[0]);
669  glPopMatrix();
670  } else { // covers are not placeable geoms but only "eye-candy" - coordinates need to be retrieved from other geoms
671  glPushMatrix();
672  LDEsetM(dGeomGetPosition(torso1_geom),dGeomGetRotation(torso1_geom)); //DRAW THE MODEL
673  DrawX( model_trimesh["waist"], modelTexture[0]);
674  glPopMatrix();
675 
676  glPushMatrix();
677  if (actHead == "on")
678  LDEsetM(dBodyGetPosition(neck[0]),dGeomGetRotation(torso3_geom)); //DRAW THE MODEL
679  else
680  LDEsetM(dGeomGetPosition(neck0_geom),dGeomGetRotation(torso3_geom));
681 
682  DrawX( model_trimesh["torso"], modelTexture[0]);
683  glPopMatrix();
684  }
685  }
686  glColor3d(1.0,1.0,1.0);
687  glPushMatrix(); LDEsetM(dGeomGetPosition(torso0_geom),dGeomGetRotation(torso0_geom));
688  DrawBox(0.0470,fabs((jP_torso[0][2]-0.031/*torso pitch cyl rad*/)-(jP_leftLeg[5][2]-0.031/*leg cyl rad*/)),0.064,false,textured,2);glPopMatrix();
689 
690  glPushMatrix();LDEsetM(dGeomGetPosition(torso1_geom),dGeomGetRotation(torso1_geom));
691  //DrawBox(0.176,0.063,0.127,false,textured,2);glPopMatrix();
692  DrawCylinder(0.031,fabs(jP_leftLeg[3][1]-jP_rightLeg[3][1]),false,textured,2);glPopMatrix();
693  glColor3d(0.5,0.5,0.5);
694  glPushMatrix(); LDEsetM(dGeomGetPosition(torso2_geom),dGeomGetRotation(torso2_geom));
695  DrawCylinder(0.031,0.097,false,textured,2);glPopMatrix();
696 
697  glPushMatrix(); LDEsetM(dGeomGetPosition(torso3_geom),dGeomGetRotation(torso3_geom));
698  DrawCylinder(0.04,0.0274,false,textured,2);glPopMatrix();
699  glColor3d(1.0,1.0,1.0);
700 
701 
702  if ((actSelfCol == "on") || (actCoversCol == "on")){ //the torso "boxes" are drawn - in this regime, we want to draw all objects that are relevant for the simulation
703  glPushMatrix();LDEsetM(dBodyGetPosition(torso[4]),dBodyGetRotation(torso[4]));
704  DrawBox(fabs(jP_leftArm[1][1]-jP_torso[2][1])-0.011-0.5*0.059 /*clavicule height + half shoulder height*/,fabs((jP_head[0][2]-0.015/*head pitch cyl rad*/)-(jP_torso[2][2]+0.031+0.0274/*torso roll cyl rad + torso yaw cyl height*/))/*0.118*/,0.109,false,textured,2);glPopMatrix();
705 
706  glPushMatrix();LDEsetM(dBodyGetPosition(torso[5]),dBodyGetRotation(torso[5]));
707  DrawBox(fabs(jP_rightArm[1][1]-jP_torso[2][1])-0.011-0.5*0.059 /*clavicule height + half shoulder height*/,fabs((jP_head[0][2]-0.015/*head pitch cyl rad*/)-(jP_torso[2][2]+0.031+0.0274/*torso roll cyl rad + torso yaw cyl height*/))/*0.118*/,0.109,false,textured,2);glPopMatrix();
708  }
709  else {
710  if (actTorsoCovers == "off") //if covers are off, the torso "boxes" are drawn; if they are on, they are not drawn as the cover visually fills up the space
711  {
712  glPushMatrix();LDEsetM(dBodyGetPosition(torso[4]),dBodyGetRotation(torso[4]));
713  DrawBox(fabs(jP_leftArm[1][1]-jP_torso[2][1])-0.011-0.5*0.059 /*clavicule height + half shoulder height*/,fabs((jP_head[0][2]-0.015/*head pitch cyl rad*/)-(jP_torso[2][2]+0.031+0.0274/*torso roll cyl rad + torso yaw cyl height*/))/*0.118*/,0.109,false,textured,2);glPopMatrix();
714 
715  glPushMatrix();LDEsetM(dBodyGetPosition(torso[5]),dBodyGetRotation(torso[5]));
716  DrawBox(fabs(jP_rightArm[1][1]-jP_torso[2][1])-0.011-0.5*0.059 /*clavicule height + half shoulder height*/,fabs((jP_head[0][2]-0.015/*head pitch cyl rad*/)-(jP_torso[2][2]+0.031+0.0274/*torso roll cyl rad + torso yaw cyl height*/))/*0.118*/,0.109,false,textured,2);glPopMatrix();
717  }
718  }
719 
720 
721  } // if (actTorso == "off")
722  else // (actTorso == "on")
723  {
724  glColor3d(1.0,1.0,1.0);
725  if (actTorsoCovers == "on")
726  {
727  if (actCoversCol == "on"){ //in this case, the covers are placeable geoms initialized in initCovers() - we can retrieve their coordinates
728  glPushMatrix();
729  LDEsetM(dGeomGetPosition(model_ThreeD_obj["waist"].geom),dGeomGetRotation(model_ThreeD_obj["waist"].geom));
730  DrawX( model_trimesh["waist"], modelTexture[0]);
731  glPopMatrix(); //takes the current matrix back out
732 
733  glPushMatrix();
734  LDEsetM(dGeomGetPosition(model_ThreeD_obj["torso"].geom),dGeomGetRotation(model_ThreeD_obj["torso"].geom));
735  DrawX( model_trimesh["torso"], modelTexture[0]);
736  glPopMatrix();
737  } else { // covers are not placeable geoms but only "eye-candy" - coordinates need to be retrieved from other geoms
738  glPushMatrix();
739  LDEsetM(dBodyGetPosition(torso[1]),dBodyGetRotation(torso[0])); //DRAW THE MODEL
740  DrawX( model_trimesh["waist"], modelTexture[0]);
741  glPopMatrix();
742 
743  glPushMatrix();
744  if (actHead == "on")
745  LDEsetM(dBodyGetPosition(neck[0]),dBodyGetRotation(torso[3])); //DRAW THE MODEL
746  else
747  LDEsetM(dGeomGetPosition(neck0_geom),dBodyGetRotation(torso[3]));
748 
749  DrawX( model_trimesh["torso"], modelTexture[0]);
750  glPopMatrix();
751  }
752 
753  }
755  glColor3d(1.0,1.0,1.0);
756  glPushMatrix();LDEsetM(dBodyGetPosition(torso[0]),dBodyGetRotation(torso[0]));
757  DrawBox(0.0470,fabs((jP_torso[0][2]-0.031/*torso pitch cyl rad*/)-(jP_leftLeg[5][2]-0.031/*leg cyl rad*/)),0.064,false,textured,2);glPopMatrix();
758 
759  glPushMatrix();LDEsetM(dBodyGetPosition(torso[1]),dBodyGetRotation(torso[1]));
760  //DrawBox(0.176,0.063,0.127,false,textured,2);glPopMatrix();
761  DrawCylinder(0.031,fabs(jP_leftLeg[3][1]-jP_rightLeg[3][1]),false,textured,2);glPopMatrix();
762  glColor3d(0.5,0.5,0.5);
763  glPushMatrix(); LDEsetM(dBodyGetPosition(torso[2]),dBodyGetRotation(torso[2]));
764  DrawCylinder(0.031,0.097,false,textured,2);glPopMatrix();
765 
766  glPushMatrix(); LDEsetM(dBodyGetPosition(torso[3]),dBodyGetRotation(torso[3]));
767  DrawCylinder(0.04,0.0274,false,textured,2);glPopMatrix();
768  glColor3d(1.0,1.0,1.0);
769 
770  if ((actSelfCol == "on") || (actCoversCol == "on")){ //the torso "boxes" are drawn - in this regime, we want to draw all objects that are relevant for the simulation
771  glPushMatrix();LDEsetM(dBodyGetPosition(torso[4]),dBodyGetRotation(torso[4]));
772  DrawBox(fabs(jP_leftArm[1][1]-jP_torso[2][1])-0.011-0.5*0.059 /*clavicule height + half shoulder height*/,fabs((jP_head[0][2]-0.015/*head pitch cyl rad*/)-(jP_torso[2][2]+0.031+0.0274/*torso roll cyl rad + torso yaw cyl height*/))/*0.118*/,0.109,false,textured,2);
773  glPopMatrix();
774 
775  glPushMatrix();LDEsetM(dBodyGetPosition(torso[5]),dBodyGetRotation(torso[5]));
776  DrawBox(fabs(jP_rightArm[1][1]-jP_torso[2][1])-0.011-0.5*0.059/*clavicule height + half shoulder height*/,fabs((jP_head[0][2]-0.015/*head pitch cyl rad*/)-(jP_torso[2][2]+0.031+0.0274/*torso roll cyl rad + torso yaw cyl height*/))/*0.118*/,0.109,false,textured,2);
777  glPopMatrix();
778 
779  }
780  else {
781  if (actTorsoCovers == "off") //if covers are off, the torso "boxes" are drawn; if they are on, they are not drawn as the cover visually fills up the space
782  {
783  glPushMatrix();LDEsetM(dBodyGetPosition(torso[4]),dBodyGetRotation(torso[4]));
784  DrawBox(fabs(jP_leftArm[1][1]-jP_torso[2][1])-0.011-0.5*0.059 /*clavicule height + half shoulder height*/,fabs((jP_head[0][2]-0.015/*head pitch cyl rad*/)-(jP_torso[2][2]+0.031+0.0274/*torso roll cyl rad + torso yaw cyl height*/))/*0.118*/,0.109,false,textured,2);
785  glPopMatrix();
786 
787  glPushMatrix();LDEsetM(dBodyGetPosition(torso[5]),dBodyGetRotation(torso[5]));
788  DrawBox(fabs(jP_rightArm[1][1]-jP_torso[2][1])-0.011-0.5*0.059/*clavicule height + half shoulder height*/,fabs((jP_head[0][2]-0.015/*head pitch cyl rad*/)-(jP_torso[2][2]+0.031+0.0274/*torso roll cyl rad + torso yaw cyl height*/))/*0.118*/,0.109,false,textured,2);
789  glPopMatrix();
790 
791  }
792  }
793  } // (actTorso == "on")
794 
795  if (actLArm == "off"){
796  glColor3d(1.0,1.0,1.0);
797  if (actLeftArmCovers == "on")
798  {
799  if (actCoversCol == "on"){ //in this case, the covers are placeable geoms initialized in initCovers() - we can retrieve their coordinates
800  glPushMatrix();
801  LDEsetM(dGeomGetPosition(model_ThreeD_obj["upperLeftArm"].geom),dGeomGetRotation(model_ThreeD_obj["upperLeftArm"].geom)); //DRAW THE MODEL
802  DrawX( model_trimesh["upperLeftArm"], modelTexture[0]);
803  glPopMatrix();
804 
805  glPushMatrix();
806  LDEsetM(dGeomGetPosition(model_ThreeD_obj["lowerLeftArm"].geom),dGeomGetRotation(model_ThreeD_obj["lowerLeftArm"].geom)); //DRAW THE MODEL
807  DrawX( model_trimesh["lowerLeftArm"], modelTexture[0]);
808  glPopMatrix();
809  } else { // covers are not placeable geoms but only "eye-candy" - coordinates need to be retrieved from other geoms
810  glPushMatrix();
811  LDEsetM(dGeomGetPosition(larm2_geom),dGeomGetRotation(larm2_geom)); //DRAW THE MODEL
812  glTranslatef(0.0, 0.0,0.5*fabs(jP_leftArm[4][2] - jP_leftArm[2][2]));
813  DrawX( model_trimesh["upperLeftArm"], modelTexture[0]);
814  glPopMatrix();
815 
816  //draw texture
817  glPushMatrix();
818  LDEsetM(dGeomGetPosition(larm3_geom),dGeomGetRotation(larm3_geom)); //DRAW THE MODEL
819  glTranslatef(0.0, 0.0,0.5*fabs(jP_leftArm[5][2] - jP_leftArm[3][2]));
820  DrawX( model_trimesh["lowerLeftArm"], modelTexture[0]);
821  glPopMatrix();
822  }
823  }
824  glColor3d(1.0,1.0,1.0);
825  glPushMatrix(); LDEsetM(dGeomGetPosition(larm0_geom),dGeomGetRotation(larm0_geom));
826  DrawCylinder(0.031,0.011,false,textured,2);glPopMatrix();
827  glColor3d(0.5,0.5,0.5);
828  glPushMatrix(); LDEsetM(dGeomGetPosition(larm1_geom),dGeomGetRotation(larm1_geom));
829  DrawCylinder(0.03,0.059,false,textured,2);glPopMatrix();
830  glColor3d(1.0,1.0,1.0);
831  glPushMatrix(); LDEsetM(dGeomGetPosition(larm2_geom),dGeomGetRotation(larm2_geom));
832  DrawCylinder(0.026 ,fabs(jP_leftArm[4][2]-jP_leftArm[2][2]),false,textured,2);glPopMatrix();
833 
834  glPushMatrix(); LDEsetM(dGeomGetPosition(larm3_geom),dGeomGetRotation(larm3_geom));
835  DrawCylinder(0.02 ,fabs(jP_leftArm[5][2]-jP_leftArm[3][2]),false,textured,2);glPopMatrix();
836 
837  }else{
838 
839  glColor3d(1.0,1.0,1.0);
840  if (actLeftArmCovers == "on")
841  {
842  if (actCoversCol == "on"){ //in this case, the covers are placeable geoms initialized in initCovers() - we can retrieve their coordinates
843  glPushMatrix();
844  LDEsetM(dGeomGetPosition(model_ThreeD_obj["upperLeftArm"].geom),dGeomGetRotation(model_ThreeD_obj["upperLeftArm"].geom)); //DRAW THE MODEL
845  DrawX( model_trimesh["upperLeftArm"], modelTexture[0]);
846  glPopMatrix();
847 
848  glPushMatrix();
849  LDEsetM(dGeomGetPosition(model_ThreeD_obj["lowerLeftArm"].geom),dGeomGetRotation(model_ThreeD_obj["lowerLeftArm"].geom)); //DRAW THE MODEL
850  DrawX( model_trimesh["lowerLeftArm"], modelTexture[0]);
851  glPopMatrix();
852  } else { // covers are not placeable geoms but only "eye-candy" - coordinates need to be retrieved from other geoms
853  glPushMatrix();
854  LDEsetM(dBodyGetPosition(body[6]),dBodyGetRotation(body[4])); //DRAW THE MODEL
855  DrawX( model_trimesh["upperLeftArm"], modelTexture[0]);
856  glPopMatrix();
857 
858  //draw texture
859  glPushMatrix();
860  LDEsetM(dBodyGetPosition(body[8]),dBodyGetRotation(body[8])); //DRAW THE MODEL
861  glTranslatef(0.0, 0.0,0.5*fabs(jP_leftArm[5][2] - jP_leftArm[3][2])); //for consinstency with initLeftArm
862  //glTranslatef(0.0, 0.0,0.5*fabs(jP_leftArm[5][2] - jP_leftArm[4][2]));
863  DrawX( model_trimesh["lowerLeftArm"], modelTexture[0]);
864  glPopMatrix();
865  }
866  }
867 
868  glColor3d(1.0,1.0,1.0);
869  glPushMatrix(); LDEsetM(dBodyGetPosition(body[0]),dBodyGetRotation(body[0]));
870  DrawCylinder(0.031,0.011,false,textured,2);glPopMatrix();
871  glColor3d(0.5,0.5,0.5);
872  glPushMatrix(); LDEsetM(dBodyGetPosition(body[2]),dBodyGetRotation(body[2]));
873  DrawCylinder(0.03,0.059,false,textured,2);glPopMatrix();
874  glColor3d(1.0,1.0,1.0);
875  glPushMatrix(); LDEsetM(dBodyGetPosition(body[4]),dBodyGetRotation(body[4]));
876  DrawCylinder(0.026 ,fabs(jP_leftArm[4][2]-jP_leftArm[2][2]),false,textured,2);
877  glPopMatrix();
878 
879  //glColor3d(1.0,1.0,0.0);
880  //glPushMatrix(); LDEsetM(dBodyGetPosition(body[6]),dBodyGetRotation(body[6]));
881  //DrawSphere(0.01,false,false,0);glPopMatrix();
882  glColor4d(1.0,1.0,1.0,0.5);
883  glPushMatrix(); LDEsetM(dBodyGetPosition(body[8]),dBodyGetRotation(body[8]));
884  DrawCylinder(0.02 ,fabs(jP_leftArm[5][2]-jP_leftArm[3][2]),false,textured,2);
885  glPopMatrix();
886 
887  }
888  if (actRArm == "off"){
889  glColor3d(1.0,1.0,1.0);
890  if (actRightArmCovers == "on")
891  {
892  if (actCoversCol == "on"){ //in this case, the covers are placeable geoms initialized in initCovers() - we can retrieve their coordinates
893  glPushMatrix();
894  LDEsetM(dGeomGetPosition(model_ThreeD_obj["upperRightArm"].geom),dGeomGetRotation(model_ThreeD_obj["upperRightArm"].geom)); //DRAW THE MODEL
895  DrawX( model_trimesh["upperRightArm"], modelTexture[0]);
896  glPopMatrix();
897 
898  glPushMatrix();
899  LDEsetM(dGeomGetPosition(model_ThreeD_obj["lowerRightArm"].geom),dGeomGetRotation(model_ThreeD_obj["lowerRightArm"].geom)); //DRAW THE MODEL
900  DrawX( model_trimesh["lowerRightArm"], modelTexture[0]);
901  glPopMatrix();
902  } else { // covers are not placeable geoms but only "eye-candy" - coordinates need to be retrieved from other geoms
903  glPushMatrix();
904  LDEsetM(dGeomGetPosition(rarm2_geom),dGeomGetRotation(rarm2_geom)); //DRAW THE MODEL
905  glTranslatef(0.0, 0.0,0.5*fabs(jP_rightArm[4][2] - jP_rightArm[2][2])); //Matej fix
906  DrawX( model_trimesh["upperRightArm"], modelTexture[0]);
907  glPopMatrix();
908 
909  //draw texture
910  glPushMatrix();
911  LDEsetM(dGeomGetPosition(rarm3_geom),dGeomGetRotation(rarm3_geom)); //DRAW THE MODEL
912  glTranslatef(0.0, 0.0,0.5*fabs(jP_rightArm[5][2] - jP_rightArm[3][2])); //Matej fix
913  DrawX( model_trimesh["lowerRightArm"], modelTexture[0]); //Matej fix
914  glPopMatrix();
915  }
916  }
917  glColor3d(1.0,1.0,1.0);
918  glPushMatrix(); LDEsetM(dGeomGetPosition(rarm0_geom),dGeomGetRotation(rarm0_geom));
919  DrawCylinder(0.031,0.011,false,textured,2);glPopMatrix();
920  glColor3d(0.5,0.5,0.5);
921  glPushMatrix(); LDEsetM(dGeomGetPosition(rarm1_geom),dGeomGetRotation(rarm1_geom));
922  DrawCylinder(0.03,0.059,false,textured,2);glPopMatrix();
923  glColor3d(1.0,1.0,1.0);
924  glPushMatrix(); LDEsetM(dGeomGetPosition(rarm2_geom),dGeomGetRotation(rarm2_geom));
925  DrawCylinder(0.026 ,fabs(jP_rightArm[4][2]-jP_rightArm[2][2]),false,textured,2);glPopMatrix();
926 
927  glPushMatrix(); LDEsetM(dGeomGetPosition(rarm3_geom),dGeomGetRotation(rarm3_geom));
928  DrawCylinder(0.02 ,fabs(jP_rightArm[5][2]-jP_rightArm[3][2]),false,textured,2);glPopMatrix();
929 
930  }else{
931 
932  if (actRightArmCovers == "on")
933  {
934  if (actCoversCol == "on"){ //in this case, the covers are placeable geoms initialized in initCovers() - we can retrieve their coordinates
935  glPushMatrix();
936  LDEsetM(dGeomGetPosition(model_ThreeD_obj["upperRightArm"].geom),dGeomGetRotation(model_ThreeD_obj["upperRightArm"].geom)); //DRAW THE MODEL
937  DrawX( model_trimesh["upperRightArm"], modelTexture[0]);
938  glPopMatrix();
939 
940  glPushMatrix();
941  LDEsetM(dGeomGetPosition(model_ThreeD_obj["lowerRightArm"].geom),dGeomGetRotation(model_ThreeD_obj["lowerRightArm"].geom)); //DRAW THE MODEL
942  DrawX( model_trimesh["lowerRightArm"], modelTexture[0]);
943  glPopMatrix();
944  } else { // covers are not placeable geoms but only "eye-candy" - coordinates need to be retrieved from other geoms
945  glPushMatrix();
946  LDEsetM(dBodyGetPosition(body[7]),dBodyGetRotation(body[5])); //DRAW THE MODEL
947  //glTranslatef(0.0, 0.0,0.5*fabs(jP_rightArm[4][2] - jP_rightArm[2][2]));
948  DrawX( model_trimesh["upperRightArm"], modelTexture[0]);
949  glPopMatrix();
950 
951  glPushMatrix();
952  LDEsetM(dBodyGetPosition(body[9]),dBodyGetRotation(body[9])); //DRAW THE MODEL
953  glTranslatef(0.0, 0.0,0.5*fabs(jP_rightArm[5][2] - jP_rightArm[3][2])); //Matej fix
954  DrawX( model_trimesh["lowerRightArm"], modelTexture[0]);
955  glPopMatrix();
956  }
957  }
958  glColor3d(1.0,1.0,1.0);
959  glPushMatrix(); LDEsetM(dBodyGetPosition(body[1]),dBodyGetRotation(body[1]));
960  DrawCylinder(0.031,0.011,false,textured,2);glPopMatrix();
961  glColor3d(0.5,0.5,0.5);
962  glPushMatrix(); LDEsetM(dBodyGetPosition(body[3]),dBodyGetRotation(body[3]));
963  DrawCylinder(0.03,0.059,false,textured,2);glPopMatrix();
964 
965  glColor3d(1.0,1.0,1.0);
966  glPushMatrix(); LDEsetM(dBodyGetPosition(body[5]),dBodyGetRotation(body[5]));
967  DrawCylinder(0.026 ,fabs(jP_rightArm[4][2]-jP_rightArm[2][2]),false,textured,2);glPopMatrix();
968 
969  //glPushMatrix(); LDEsetM(dBodyGetPosition(body[7]),dBodyGetRotation(body[7]));
970  //DrawSphere(0.01,false,false);glPopMatrix();
971 
972  glPushMatrix(); LDEsetM(dBodyGetPosition(body[9]),dBodyGetRotation(body[9]));
973  DrawCylinder(0.02 ,fabs(jP_rightArm[5][2]-jP_rightArm[3][2]),false,textured,2);glPopMatrix();
974 
975  }
976  if (actLHand == "off"){
977  glColor3d(1.0,1.0,1.0);
978 
979  if (actLeftArmCovers == "on"){
980  if (actCoversCol == "on"){ //in this case, the covers are placeable geoms initialized in initCovers() - we can retrieve their coordinates
981  glPushMatrix();
982  LDEsetM(dGeomGetPosition(model_ThreeD_obj["leftPalm"].geom),dGeomGetRotation(model_ThreeD_obj["leftPalm"].geom)); //DRAW THE MODEL
983  DrawX( model_trimesh["leftPalm"], modelTexture[0]);
984  glPopMatrix();
985  } else { // covers are not placeable geoms but only "eye-candy" - coordinates need to be retrieved from other geoms
986  glPushMatrix();
987  LDEsetM(dGeomGetPosition(l_hand0_geom),dGeomGetRotation(l_hand0_geom));
988  glTranslatef(0.0, 0.5*fabs(jP_leftArm[7][2] - jP_leftArm[6][2]), 0.0);
989  DrawX( model_trimesh["leftPalm"], modelTexture[0]);
990  glPopMatrix();
991  }
992  }
993 
994  if ((actSelfCol == "on") || (actCoversCol == "on")){ //the palm "box" is drawn - in this regime, we want to draw all objects that are relevant for the simulation
995  glPushMatrix();LDEsetM(dGeomGetPosition(l_hand0_geom),dGeomGetRotation(l_hand0_geom));
996  DrawBox(0.022,0.069,0.065,false,textured,2);glPopMatrix();
997  } else {
998  if (actLeftArmCovers == "off") //if covers are off, the palm "box" is drawn; if they are on, it is not drawn as the cover visually fills up the space
999  {
1000  glPushMatrix();LDEsetM(dGeomGetPosition(l_hand0_geom),dGeomGetRotation(l_hand0_geom));
1001  DrawBox(0.022,0.069,0.065,false,textured,2);glPopMatrix();
1002  }
1003  }
1004  glPushMatrix(); LDEsetM(dGeomGetPosition(l_hand1_geom),dGeomGetRotation(l_hand1_geom));
1005  DrawCylinder(0.0065,0.08,false,textured,2);glPopMatrix();
1006 
1007  glPushMatrix(); LDEsetM(dGeomGetPosition(l_hand2_geom),dGeomGetRotation(l_hand2_geom));
1008  DrawCylinder(0.0065,0.084,false,textured,2);glPopMatrix();
1009 
1010  glPushMatrix(); LDEsetM(dGeomGetPosition(l_hand3_geom),dGeomGetRotation(l_hand3_geom));
1011  DrawCylinder(0.0065,0.08,false,textured,2);glPopMatrix();
1012 
1013  glPushMatrix(); LDEsetM(dGeomGetPosition(l_hand4_geom),dGeomGetRotation(l_hand4_geom));
1014  DrawCylinder(0.0065,0.073,false,textured,2);glPopMatrix();
1015 
1016  glPushMatrix(); LDEsetM(dGeomGetPosition(l_hand5_geom),dGeomGetRotation(l_hand5_geom));
1017  DrawCylinder(0.0065,0.064,false,textured,2);glPopMatrix();
1018 
1019  }else{ //(actLHand == "on")
1020  if (actLeftArmCovers == "on"){
1021  if (actCoversCol == "on"){ //in this case, the covers are placeable geoms initialized in initCovers() - we can retrieve their coordinates
1022  glPushMatrix();
1023  LDEsetM(dGeomGetPosition(model_ThreeD_obj["leftPalm"].geom),dGeomGetRotation(model_ThreeD_obj["leftPalm"].geom)); //DRAW THE MODEL
1024  DrawX( model_trimesh["leftPalm"], modelTexture[0]);
1025  glPopMatrix();
1026  } else { // covers are not placeable geoms but only "eye-candy" - coordinates need to be retrieved from other geoms
1027  glPushMatrix();
1028  LDEsetM(dBodyGetPosition(body[10]),dBodyGetRotation(body[10]));
1029  glTranslatef(0.0, 0.5*fabs(jP_leftArm[7][2] - jP_leftArm[6][2]), 0.0);
1030  DrawX( model_trimesh["leftPalm"], modelTexture[0]);
1031  glPopMatrix();
1032  }
1033  }
1034 
1035  if ((actSelfCol == "on") || (actCoversCol == "on")){ //the palm "box" is drawn - in this regime, we want to draw all objects that are relevant for the simulation
1036  glPushMatrix();LDEsetM(dBodyGetPosition(body[10]),dBodyGetRotation(body[10]));
1037  DrawBox(0.022,0.069,0.065,false,textured,2);glPopMatrix();//Taken from ODE use Y, Z, Xs
1038  } else {
1039  if (actLeftArmCovers == "off") //if covers are off, the palm "box" is drawn; if they are on, it is not drawn as the cover visually fills up the space
1040  {
1041  glPushMatrix();LDEsetM(dBodyGetPosition(body[10]),dBodyGetRotation(body[10]));
1042  DrawBox(0.022,0.069,0.065,false,textured,2);glPopMatrix();//Taken from ODE use Y, Z, Xs
1043  }
1044  }
1045 
1046  //LEFT HAND + FINGERS
1047  glPushMatrix(); LDEsetM(dBodyGetPosition(body[12]),dBodyGetRotation(body[12]));
1048  DrawCylinder(0.0065,0.012,false,textured,2);glPopMatrix();
1049 
1050  glPushMatrix(); LDEsetM(dBodyGetPosition(body[13]),dBodyGetRotation(body[13]));
1051  DrawCylinder(0.0065,0.012,false,textured,2);glPopMatrix();
1052 
1053  glPushMatrix(); LDEsetM(dGeomGetPosition(lhandfings0_geom),dGeomGetRotation(lhandfings0_geom));
1054  DrawCylinder(0.0065,0.012,false,textured,2);glPopMatrix();
1055 
1056  glPushMatrix(); LDEsetM(dGeomGetPosition(lhandfings1_geom),dGeomGetRotation(lhandfings1_geom));
1057  DrawCylinder(0.0065,0.012,false,textured,2);glPopMatrix();
1058 
1059  glPushMatrix(); LDEsetM(dBodyGetPosition(body[16]),dBodyGetRotation(body[16]));
1060  DrawCylinder(0.0065,0.026,false,textured,2);glPopMatrix();
1061 
1062  glPushMatrix(); LDEsetM(dBodyGetPosition(body[17]),dBodyGetRotation(body[17]));
1063  DrawCylinder(0.0065,0.028,false,textured,2);glPopMatrix();
1064 
1065  glPushMatrix(); LDEsetM(dGeomGetPosition(lhandfings2_geom),dGeomGetRotation(lhandfings2_geom));
1066  DrawCylinder(0.0065,0.026,false,textured,2);glPopMatrix();
1067 
1068  glPushMatrix(); LDEsetM(dGeomGetPosition(lhandfings3_geom),dGeomGetRotation(lhandfings3_geom));
1069  DrawCylinder(0.0065,0.022,false,textured,2);glPopMatrix();
1070 
1071  glPushMatrix(); LDEsetM(dBodyGetPosition(body[20]),dBodyGetRotation(body[20]));
1072  DrawCylinder(0.0065,0.022,false,textured,2);glPopMatrix();
1073 
1074  glPushMatrix(); LDEsetM(dBodyGetPosition(body[21]),dBodyGetRotation(body[21]));
1075  DrawCylinder(0.0065,0.024,false,textured,2);glPopMatrix();
1076 
1077  glPushMatrix(); LDEsetM(dGeomGetPosition(lhandfings4_geom),dGeomGetRotation(lhandfings4_geom));
1078  DrawCylinder(0.0065,0.022,false,textured,2);glPopMatrix();
1079 
1080  glPushMatrix(); LDEsetM(dGeomGetPosition(lhandfings5_geom),dGeomGetRotation(lhandfings5_geom));
1081  DrawCylinder(0.0065,0.019,false,textured,2);glPopMatrix();
1082 
1083  glPushMatrix(); LDEsetM(dBodyGetPosition(body[24]),dBodyGetRotation(body[24]));
1084  DrawCylinder(0.0065,0.02,false,textured,2);glPopMatrix();
1085 
1086  glPushMatrix(); LDEsetM(dBodyGetPosition(body[25]),dBodyGetRotation(body[25]));
1087  DrawCylinder(0.0065,0.02,false,textured,2);glPopMatrix();
1088 
1089  glPushMatrix(); LDEsetM(dGeomGetPosition(lhandfings6_geom),dGeomGetRotation(lhandfings6_geom));
1090  DrawCylinder(0.0065,0.02,false,textured,2);glPopMatrix();
1091 
1092  glPushMatrix(); LDEsetM(dGeomGetPosition(lhandfings7_geom),dGeomGetRotation(lhandfings7_geom));
1093  DrawCylinder(0.0065,0.02,false,textured,2);glPopMatrix();
1094 
1095  glPushMatrix(); LDEsetM(dBodyGetPosition(body[28]),dBodyGetRotation(body[28]));
1096  DrawCylinder(0.0065,0.026,false,textured,2);glPopMatrix();
1097 
1098  glPushMatrix(); LDEsetM(dBodyGetPosition(body[29]),dBodyGetRotation(body[29]));
1099  DrawCylinder(0.0065,0.022,false,textured,2);glPopMatrix();
1100 
1101  glPushMatrix(); LDEsetM(dBodyGetPosition(body[30]),dBodyGetRotation(body[30]));
1102  DrawCylinder(0.0065,0.016,false,textured,2);glPopMatrix();
1103  }
1104  if (actRHand == "off"){
1105 
1106  glColor3d(1.0,1.0,1.0);
1107  if (actRightArmCovers == "on"){
1108  if (actCoversCol == "on"){ //in this case, the covers are placeable geoms initialized in initCovers() - we can retrieve their coordinates
1109  glPushMatrix();
1110  LDEsetM(dGeomGetPosition(model_ThreeD_obj["rightPalm"].geom),dGeomGetRotation(model_ThreeD_obj["rightPalm"].geom));
1111  DrawX( model_trimesh["rightPalm"], modelTexture[0]);
1112  glPopMatrix();
1113  } else { // covers are not placeable geoms but only "eye-candy" - coordinates need to be retrieved from other geoms
1114  glPushMatrix();
1115  LDEsetM(dGeomGetPosition(r_hand0_geom),dGeomGetRotation(r_hand0_geom)); //DRAW THE MODEL
1116  glTranslatef(0.0, 0.5*fabs(jP_rightArm[7][2] - jP_rightArm[6][2]), 0.0);
1117  DrawX( model_trimesh["rightPalm"], modelTexture[0]);
1118  glPopMatrix();
1119  }
1120  }
1121 
1122  if ((actSelfCol == "on") || (actCoversCol == "on")){ //the palm "box" is drawn - in this regime, we want to draw all objects that are relevant for the simulation
1123  glPushMatrix();LDEsetM(dGeomGetPosition(r_hand0_geom),dGeomGetRotation(r_hand0_geom));
1124  DrawBox(0.022,0.069,0.065,false,textured,2);glPopMatrix();
1125  } else {
1126  if (actRightArmCovers == "off") //if covers are off, the palm "box" is drawn; if they are on, it is not drawn as the cover visually fills up the space
1127  {
1128  glPushMatrix();LDEsetM(dGeomGetPosition(r_hand0_geom),dGeomGetRotation(r_hand0_geom));
1129  DrawBox(0.022,0.069,0.065,false,textured,2);glPopMatrix();
1130  }
1131  }
1132 
1133  glPushMatrix(); LDEsetM(dGeomGetPosition(r_hand1_geom),dGeomGetRotation(r_hand1_geom));
1134  DrawCylinder(0.0065,0.08,false,textured,2);glPopMatrix();
1135 
1136  glPushMatrix(); LDEsetM(dGeomGetPosition(r_hand2_geom),dGeomGetRotation(r_hand2_geom));
1137  DrawCylinder(0.0065,0.084,false,textured,2);glPopMatrix();
1138 
1139  glPushMatrix(); LDEsetM(dGeomGetPosition(r_hand3_geom),dGeomGetRotation(r_hand3_geom));
1140  DrawCylinder(0.0065,0.08,false,textured,2);glPopMatrix();
1141 
1142  glPushMatrix(); LDEsetM(dGeomGetPosition(r_hand4_geom),dGeomGetRotation(r_hand4_geom));
1143  DrawCylinder(0.0065,0.073,false,textured,2);glPopMatrix();
1144 
1145  glPushMatrix(); LDEsetM(dGeomGetPosition(r_hand5_geom),dGeomGetRotation(r_hand5_geom));
1146  DrawCylinder(0.0065,0.064,false,textured,2);glPopMatrix();
1147 
1148  }else{ //(actRHand == "on")
1149  if (actRightArmCovers == "on"){
1150  if (actCoversCol == "on"){ //in this case, the covers are placeable geoms initialized in initCovers() - we can retrieve their coordinates
1151  glPushMatrix();
1152  LDEsetM(dGeomGetPosition(model_ThreeD_obj["rightPalm"].geom),dGeomGetRotation(model_ThreeD_obj["rightPalm"].geom));
1153  DrawX( model_trimesh["rightPalm"], modelTexture[0]);
1154  glPopMatrix();
1155  } else { // covers are not placeable geoms but only "eye-candy" - coordinates need to be retrieved from other geoms
1156  glPushMatrix();
1157  LDEsetM(dBodyGetPosition(body[11]),dBodyGetRotation(body[11])); //DRAW THE MODEL
1158  glTranslatef(0.0, 0.5*fabs(jP_rightArm[7][2] - jP_rightArm[6][2]), 0.0);
1159  DrawX( model_trimesh["rightPalm"], modelTexture[0]);
1160  glPopMatrix();
1161  }
1162  }
1163 
1164  if ((actSelfCol == "on") || (actCoversCol == "on")){ //the palm "box" is drawn - in this regime, we want to draw all objects that are relevant for the simulation
1165  glPushMatrix();LDEsetM(dBodyGetPosition(body[11]),dBodyGetRotation(body[11]));
1166  DrawBox(0.022,0.069,0.065,false,textured,2);glPopMatrix();//Taken from ODE use Y, Z, Xs
1167  } else {
1168  if (actRightArmCovers == "off") //if covers are off, the palm "box" is drawn; if they are on, it is not drawn as the cover visually fills up the space
1169  {
1170  glPushMatrix();LDEsetM(dBodyGetPosition(body[11]),dBodyGetRotation(body[11]));
1171  DrawBox(0.022,0.069,0.065,false,textured,2);glPopMatrix();//Taken from ODE use Y, Z, Xs
1172  }
1173  }
1174 
1175  //RIGHT HAND FINGERS
1176  glPushMatrix(); LDEsetM(dBodyGetPosition(body[31]),dBodyGetRotation(body[31]));
1177  DrawCylinder(0.0065,0.012,false,textured,2);glPopMatrix();
1178 
1179  glPushMatrix(); LDEsetM(dBodyGetPosition(body[32]),dBodyGetRotation(body[32]));
1180  DrawCylinder(0.0065,0.012,false,textured,2);glPopMatrix();
1181 
1182  glPushMatrix(); LDEsetM(dGeomGetPosition(rhandfings0_geom),dGeomGetRotation(rhandfings0_geom));
1183  DrawCylinder(0.0065,0.012,false,textured,2);glPopMatrix();
1184 
1185  glPushMatrix(); LDEsetM(dGeomGetPosition(rhandfings1_geom),dGeomGetRotation(rhandfings1_geom));
1186  DrawCylinder(0.0065,0.012,false,textured,2);glPopMatrix();
1187 
1188  glPushMatrix(); LDEsetM(dBodyGetPosition(body[35]),dBodyGetRotation(body[35]));
1189  DrawCylinder(0.0065,0.026,false,textured,2);glPopMatrix();
1190 
1191  glPushMatrix(); LDEsetM(dBodyGetPosition(body[36]),dBodyGetRotation(body[36]));
1192  DrawCylinder(0.0065,0.028,false,textured,2);glPopMatrix();
1193 
1194  glPushMatrix(); LDEsetM(dGeomGetPosition(rhandfings2_geom),dGeomGetRotation(rhandfings2_geom));
1195  DrawCylinder(0.0065,0.026,false,textured,2);glPopMatrix();
1196 
1197  glPushMatrix(); LDEsetM(dGeomGetPosition(rhandfings3_geom),dGeomGetRotation(rhandfings3_geom));
1198  DrawCylinder(0.0065,0.022,false,textured,2);glPopMatrix();
1199 
1200  glPushMatrix(); LDEsetM(dBodyGetPosition(body[39]),dBodyGetRotation(body[39]));
1201  DrawCylinder(0.0065,0.022,false,textured,2);glPopMatrix();
1202 
1203  glPushMatrix(); LDEsetM(dBodyGetPosition(body[40]),dBodyGetRotation(body[40]));
1204  DrawCylinder(0.0065,0.024,false,textured,2);glPopMatrix();
1205 
1206  glPushMatrix(); LDEsetM(dGeomGetPosition(rhandfings4_geom),dGeomGetRotation(rhandfings4_geom));
1207  DrawCylinder(0.0065,0.022,false,textured,2);glPopMatrix();
1208 
1209  glPushMatrix(); LDEsetM(dGeomGetPosition(rhandfings5_geom),dGeomGetRotation(rhandfings5_geom));
1210  DrawCylinder(0.0065,0.019,false,textured,2);glPopMatrix();
1211 
1212  glPushMatrix(); LDEsetM(dBodyGetPosition(body[43]),dBodyGetRotation(body[43]));
1213  DrawCylinder(0.0065,0.02,false,textured,2);glPopMatrix();
1214 
1215  glPushMatrix(); LDEsetM(dBodyGetPosition(body[44]),dBodyGetRotation(body[44]));
1216  DrawCylinder(0.0065,0.02,false,textured,2);glPopMatrix();
1217 
1218  glPushMatrix(); LDEsetM(dGeomGetPosition(rhandfings6_geom),dGeomGetRotation(rhandfings6_geom));
1219  DrawCylinder(0.0065,0.02,false,textured,2);glPopMatrix();
1220 
1221  glPushMatrix(); LDEsetM(dGeomGetPosition(rhandfings7_geom),dGeomGetRotation(rhandfings7_geom));
1222  DrawCylinder(0.0065,0.02,false,textured,2);glPopMatrix();
1223 
1224  glPushMatrix(); LDEsetM(dBodyGetPosition(body[47]),dBodyGetRotation(body[47]));
1225  DrawCylinder(0.0065,0.026,false,textured,2);glPopMatrix();
1226 
1227  glPushMatrix(); LDEsetM(dBodyGetPosition(body[48]),dBodyGetRotation(body[48]));
1228  DrawCylinder(0.0065,0.022,false,textured,2);glPopMatrix();
1229 
1230  glPushMatrix(); LDEsetM(dBodyGetPosition(body[49]),dBodyGetRotation(body[49]));
1231  DrawCylinder(0.0065,0.016,false,textured,2);glPopMatrix();
1232  }
1233  if (actHead == "off"){
1234  glPushMatrix(); LDEsetM(dGeomGetPosition(neck0_geom),dGeomGetRotation(neck0_geom));
1235  DrawCylinder(0.015,0.077,false,textured,2);glPopMatrix();
1236 
1237  glPushMatrix(); LDEsetM(dGeomGetPosition(neck1_geom),dGeomGetRotation(neck1_geom));
1238  DrawCylinder(0.015,0.077,false,textured,2);glPopMatrix();
1239  }else{
1240  glPushMatrix(); LDEsetM(dBodyGetPosition(neck[0]),dBodyGetRotation(neck[0]));
1241  DrawCylinder(0.015,0.077,false,textured,2);glPopMatrix();
1242 
1243  glPushMatrix(); LDEsetM(dBodyGetPosition(neck[1]),dBodyGetRotation(neck[1]));
1244  DrawCylinder(0.015,0.077,false,textured,2);glPopMatrix();
1245  }
1246 
1247  glPushMatrix(); LDEsetM(dGeomGetPosition(head0_geom),dGeomGetRotation(head0_geom));
1248  DrawCylinder(0.015,0.06,false,textured,2);glPopMatrix();
1249 
1250  if (actHeadCover == "on"){
1251  glPushMatrix(); LDEsetM(dGeomGetPosition(eye1_geom),dGeomGetRotation(head1_geom));
1252  glTranslatef(0.0,0.0,-fabs(jP_head[3][0]-jP_head[2][0])*1.20);
1253  glScalef(0.95,0.95,1);
1254  iCubHeadModel->draw(false,8);
1255  glPopMatrix();
1256 
1257  if (!eyeLids){
1258  eyeLids = new EyeLids;
1259  eyeLids->setName( eyeLidsPortName );
1260  }
1261  glPushMatrix();LDEsetM(dGeomGetPosition(eye1_geom),dGeomGetRotation(head1_geom));
1262  eyeLids->checkPort();
1263  glRotatef(eyeLids->eyeLidsRotation,1,0,0);
1264  topEyeLidModel->draw(false,8);
1265  glRotatef(-2*eyeLids->eyeLidsRotation,1,0,0);
1266  bottomEyeLidModel->draw(false,8);
1267  glPopMatrix();
1268 
1269  }else{
1270  glPushMatrix(); LDEsetM(dGeomGetPosition(head1_geom),dGeomGetRotation(head1_geom));
1271  DrawBox(0.104, 0.002,0.052,false,textured,2);
1272  glPopMatrix();
1273 
1274  glColor3d(0.3,0.3,0.3);
1275  glPushMatrix(); LDEsetM(dGeomGetPosition(head2_geom),dGeomGetRotation(head2_geom));
1276  DrawBox(0.002, 0.093,0.052,false,false,2);glPopMatrix();
1277 
1278  glPushMatrix(); LDEsetM(dGeomGetPosition(head3_geom),dGeomGetRotation(head3_geom));
1279  DrawBox(0.002, 0.093,0.052,false,false,2);glPopMatrix();
1280  //glPushMatrix(); LDEsetM(dGeomGetPosition(head4_geom),dGeomGetRotation(head4_geom));
1281  //DrawBox( 0.104, 0.002 ,0.032,false,false,2);glPopMatrix();
1282 
1283  //glPushMatrix(); LDEsetM(dGeomGetPosition(head5_geom),dGeomGetRotation(head5_geom));
1284  //DrawBox( 0.011, 0.026,0.025,false,false,2);glPopMatrix();
1285  glColor3d(0.3,0.3,0.3);
1286  glPushMatrix(); LDEsetM(dGeomGetPosition(head6_geom),dGeomGetRotation(head6_geom));
1287  DrawBox( 0.011, 0.051,0.012,false,false,2);glPopMatrix();
1288 
1289  glPushMatrix(); LDEsetM(dGeomGetPosition(head7_geom),dGeomGetRotation(head7_geom));
1290  DrawBox( 0.02, 0.022, 0.012,false,false,2);glPopMatrix();
1291  }
1292  glPushMatrix(); LDEsetM(dGeomGetPosition(eye1_geom),dGeomGetRotation(eye1_geom));
1293  DrawCylinder(0.002,0.068,false,true,1);glPopMatrix();
1294 
1295  glPushMatrix(); LDEsetM(dGeomGetPosition(eye2_geom),dGeomGetRotation(eye2_geom));
1296  DrawCylinder(0.006,0.030,false,true,1);glPopMatrix();
1297 
1298  glPushMatrix(); LDEsetM(dGeomGetPosition(eye3_geom),dGeomGetRotation(eye3_geom));
1299  DrawCylinder(0.006,0.05,false,true,1);glPopMatrix();
1300 
1301  glPushMatrix(); LDEsetM(dGeomGetPosition(eye3_geom),dGeomGetRotation(eye3_geom));
1302  DrawCylinder(0.006,0.05,false,true,1);glPopMatrix();
1303 
1304  glPushMatrix(); LDEsetM(dGeomGetPosition(eye4_geom),dGeomGetRotation(eye4_geom));
1305  DrawCylinder(0.006,0.030,false,true,1);glPopMatrix();
1306 
1307  glPushMatrix(); LDEsetM(dGeomGetPosition(eye5_geom),dGeomGetRotation(eye5_geom));
1308  DrawCylinder(0.006,0.05,false,true,1);glPopMatrix();
1309 
1310  glPushMatrix(); LDEsetM(dGeomGetPosition(Leye1_geom),dGeomGetRotation(Leye1_geom));
1311  glColor3d(0,0,0);
1312  DrawCylinder(0.006,0.04,false,false,1);
1313  glColor3d(1,1,1);
1314  DrawSphere(0.0185,false,false,0);
1315  glPopMatrix();
1316 
1317  glPushMatrix(); LDEsetM(dGeomGetPosition(Reye1_geom),dGeomGetRotation(Reye1_geom));
1318  glColor3d(0,0,0);
1319  DrawCylinder(0.006,0.04,false,false,1);
1320  glColor3d(1,1,1);
1321  DrawSphere(0.0185,false,false,0);
1322  glPopMatrix();
1323 
1324  glColor3d(1,0.49,0.14);
1325  glPushMatrix();LDEsetM(dBodyGetPosition(inertialBody),dBodyGetRotation(inertialBody));
1326  DrawBox( 0.03, 0.02, 0.05,false,false,2);
1327  glPopMatrix();
1328 
1329 }
1330 bool ICubSim::loadJointPosition(const char *joints_path){
1331 
1332  ifstream fin(joints_path);
1333 
1334  if(!fin.is_open())
1335  {
1336  yError("Could not find joint.ini file!\n");
1337  return false;
1338  }
1339 
1340  stringstream sFin;
1341  sFin << fin.rdbuf();
1342 
1343  Bottle bJoints(sFin.str().c_str());
1344 
1345  /*---- legs ----*/
1346  Bottle &bLeftLeg=bJoints.findGroup("left_leg");
1347  Bottle &bRightLeg=bJoints.findGroup("right_leg");
1348 
1349 
1350  //the legs are inverse ordered
1351  for(int i=0; i<6; i++)
1352  {
1353  jP_leftLeg[5-i].resize(3);
1354  jP_rightLeg[5-i].resize(3);
1355 
1356  Bottle *leftJoint=bLeftLeg.get(i+1).asList();
1357  Bottle *rightJoint=bRightLeg.get(i+1).asList();
1358 
1359  for(int j=0; j<3; j++)
1360  {
1361  jP_leftLeg[5-i][j]=leftJoint->get(j).asDouble();
1362  jP_rightLeg[5-i][j]=rightJoint->get(j).asDouble();
1363  }
1364  }
1365 
1366  // Del Prete: read parameters from iKin (for arms and torso only)
1367  Matrix Hl(4,4), Hr(4,4); // rototranslation matrix
1368  Vector tl(3), tr(3), z(3);
1369 
1370  /*---- torso ----*/
1371  for(int i=0; i<3; i++)
1372  {
1373  jP_torso[i].resize(3);
1374  if(i==0)
1375  Hl = H_w2r * iKinLeftArm.getH0();
1376  else
1377  Hl = H_w2r * iKinLeftArm.getH(i-1);
1378  jP_torso[i][0] = Hl(2,3);
1379  jP_torso[i][1] = Hl(0,3);
1380  jP_torso[i][2] = Hl(1,3);
1381  jA_torso[i] = Hl.subcol(0,2,3);
1382  if(i>0)
1383  jA_torso[i] = -1.0*jA_torso[i];
1384  }
1385 
1386  /*---- arms ----*/
1387  for(int i=0; i<8; i++)
1388  {
1389  jP_leftArm[i].resize(3);
1390  Hl = H_w2r*iKinLeftArm.getH(i+2);
1391  jP_leftArm[i][0] = Hl(2,3);
1392  jP_leftArm[i][1] = Hl(0,3);
1393  jP_leftArm[i][2] = Hl(1,3);
1394  jA_leftArm[i] = Hl.subcol(0,2,3);
1395  if(i>1)
1396  jA_leftArm[i] = -1.0*jA_leftArm[i];
1397 
1398  jP_rightArm[i].resize(3);
1399  Hr = H_w2r*iKinRightArm.getH(i+2);
1400  jP_rightArm[i][0] = Hr(2,3);
1401  jP_rightArm[i][1] = Hr(0,3);
1402  jP_rightArm[i][2] = Hr(1,3);
1403  jA_rightArm[i] = Hr.subcol(0,2,3);
1404  if(i==1 || i==3)
1405  jA_rightArm[i] = -1.0*jA_rightArm[i];
1406  }
1407 
1408  /*---- head ----*/
1409  Bottle &bHead=bJoints.findGroup("head");
1410 
1411  for(int i=0; i<4; i++)
1412  {
1413  jP_head[i].resize(3);
1414 
1415  Bottle *headJoint=bHead.get(i+1).asList();
1416 
1417  for(int j=0; j<3; j++)
1418  jP_head[i][j]=headJoint->get(j).asDouble();
1419  }
1420 
1421  /*---- eyes ----*/
1422  Bottle &bLeftEye=bJoints.findGroup("left_eye");
1423  Bottle &bRightEye=bJoints.findGroup("right_eye");
1424 
1425  for(int i=0; i<2; i++)
1426  {
1427  jP_leftEye[i].resize(3);
1428  jP_rightEye[i].resize(3);
1429 
1430  Bottle *leftJoint=bLeftEye.get(i+1).asList();
1431  Bottle *rightJoint=bRightEye.get(i+1).asList();
1432 
1433  for(int j=0; j<3; j++)
1434  {
1435  jP_leftEye[i][j]=leftJoint->get(j).asDouble();
1436  jP_rightEye[i][j]=rightJoint->get(j).asDouble();
1437  }
1438  }
1439  /*---- inertial ----*/
1440  Bottle &bInertial=bJoints.findGroup("inertial");
1441  Bottle *inertialJP = bInertial.get(1).asList();
1442  jP_inertial.resize(3);
1443 
1444  for(int j=0; j<3; j++)
1445  {
1446  jP_inertial[j] = inertialJP->get(j).asDouble();
1447  }
1448  return true;
1449 }
1450 
1451 void ICubSim::setPosition(dReal agentX, dReal agentY, dReal agentZ ) {
1452  //Coordinates X Y Z using the 3D Cartesian coordinate system
1453  //dGeomSetPosition(geom_cube[0],0.0,0.05, 2); //reference on the z
1454 
1455  if (actScreen == "on")
1456  dGeomSetPosition(screenGeom,0.0,1.0,0.7);
1457 
1458  if (actLegs == "off"){
1459  //starting from the left foot
1460  dBodySetPosition (legs, jP_leftLeg[0][1], 0.0, jP_leftLeg[0][0]);
1461  }else{
1462  //left lower body part
1463  dBodySetPosition(leftLeg[0], jP_leftLeg[0][1], elev + 0.0021, jP_leftLeg[0][0]); //dBodySetPosition(leftLeg[0], 0.068, elev + 0.0021, 0.0); //FROM ODE Y, Z, X
1464  dBodySetPosition(leftLeg[1], jP_leftLeg[1][1], elev + jP_leftLeg[1][2], jP_leftLeg[1][0]);//dBodySetPosition(leftLeg[1], 0.068, elev + 0.031, -0.0235);
1465  dBodySetPosition(leftLeg[2], jP_leftLeg[1][1], elev + jP_leftLeg[1][2], jP_leftLeg[1][0]);//dBodySetPosition(leftLeg[2], 0.068, elev + 0.031, -0.034);
1466  dBodySetPosition(leftLeg[3], jP_leftLeg[2][1], elev + jP_leftLeg[2][2], jP_leftLeg[2][0]);//dBodySetPosition(leftLeg[3], 0.068, elev + 0.244, -0.034);
1467  dBodySetPosition(leftLeg[4], jP_leftLeg[4][1], elev + jP_leftLeg[4][2], jP_leftLeg[4][0]);//dBodySetPosition(leftLeg[4], 0.068, elev + 0.468, -0.034);
1468  dBodySetPosition(leftLeg[5],0.0295, elev + jP_leftLeg[4][2], jP_leftLeg[4][0]);//dBodySetPosition(leftLeg[5], 0.0295, elev + 0.468, -0.034);
1469  //right lower body part
1470  dBodySetPosition(rightLeg[0], jP_rightLeg[0][1], elev + 0.0021, jP_rightLeg[0][0]); //dBodySetPosition(rightLeg[0], -0.068, elev + 0.0021, 0.0);
1471  dBodySetPosition(rightLeg[1], jP_rightLeg[1][1], elev + jP_rightLeg[1][2], jP_rightLeg[1][0]);//dBodySetPosition(rightLeg[1], -0.068, elev + 0.031, -0.0235);
1472  dBodySetPosition(rightLeg[2], jP_rightLeg[1][1], elev + jP_rightLeg[1][2], jP_rightLeg[1][0]);//dBodySetPosition(rightLeg[2], -0.068, elev + 0.031, -0.034);
1473  dBodySetPosition(rightLeg[3], jP_rightLeg[2][1], elev + jP_rightLeg[2][2], jP_rightLeg[2][0]);//dBodySetPosition(rightLeg[3], -0.068, elev + 0.244, -0.034);
1474  dBodySetPosition(rightLeg[4], jP_rightLeg[4][1], elev + jP_rightLeg[4][2], jP_rightLeg[4][0]);//dBodySetPosition(rightLeg[4], -0.068, elev + 0.468, -0.034);
1475  dBodySetPosition(rightLeg[5],-0.0295, elev + jP_leftLeg[4][2], jP_leftLeg[4][0]);//dBodySetPosition(rightLeg[5], -0.0295, elev + 0.468, -0.034);
1476  }
1477  if (actTorso == "off"){
1478  dBodySetPosition (body_torso, jP_torso[0][1], elev + 0.5*fabs((jP_torso[0][2]-0.031/*torso pitch cyl rad*/)+(jP_leftLeg[5][2]-0.031/*leg cyl rad*/))/*0.4912*/, jP_torso[0][0]);
1479  dBodySetPosition (torso[4], 0.5*(jP_leftArm[1][1]+jP_torso[2][1]-0.011-0.5*0.059 /*clavicule height + half shoulder height*/)/*prev=0.038*/, elev +0.5*fabs((jP_head[0][2]-0.015/*head pitch cyl rad*/)+(jP_torso[2][2]+0.031+0.0274/*torso roll cyl rad + torso yaw cyl height*/))/*prev= 0.7414*/, jP_torso[2][0]);
1480  dBodySetPosition (torso[5], 0.5*(jP_rightArm[1][1]+jP_torso[2][1]+0.011+0.5*0.059/*clavicule height + half shoulder height*/)/*prev=-0.038*/, elev +0.5*fabs((jP_head[0][2]-0.015/*head pitch cyl rad*/)+(jP_torso[2][2]+0.031+0.0274/*torso roll cyl rad + torso yaw cyl height*/))/*prev= 0.7414*/, jP_torso[2][0]);
1481  }else{
1482  dBodySetPosition (torso[0], jP_torso[0][1], elev + 0.5*fabs((jP_torso[0][2]-0.031/*torso pitch cyl rad*/)+(jP_leftLeg[5][2]-0.031/*leg cyl rad*/))/*0.4912*/, jP_torso[0][0]);
1483  dBodySetPosition (torso[1], jP_torso[0][1], elev +jP_torso[0][2]/*0.557*/, jP_torso[0][0]);
1484  dBodySetPosition (torso[2], jP_torso[1][1], elev +jP_torso[1][2]/*0.557*/, jP_torso[1][0]);
1485  dBodySetPosition (torso[3], jP_torso[2][1], elev +jP_torso[2][2]+0.031+0.0274*0.5/*torso roll cyl rad + torso yaw cyl half height*/, jP_torso[2][0]); //0.0, elev +0.6687, -0.026);
1486  dBodySetPosition (torso[4], 0.5*(jP_leftArm[1][1]+jP_torso[2][1]-0.011-0.5*0.059 /*clavicule height + half shoulder height*/)/*prev=0.038*/, elev +0.5*fabs((jP_head[0][2]-0.015/*head pitch cyl rad*/)+(jP_torso[2][2]+0.031+0.0274/*torso roll cyl rad + torso yaw cyl height*/))/*prev= 0.7414*/, jP_torso[2][0]);
1487  dBodySetPosition (torso[5], 0.5*(jP_rightArm[1][1]+jP_torso[2][1]+0.011+0.5*0.059/*clavicule height + half shoulder height*/)/*prev=-0.038*/, elev +0.5*fabs((jP_head[0][2]-0.015/*head pitch cyl rad*/)+(jP_torso[2][2]+0.031+0.0274/*torso roll cyl rad + torso yaw cyl height*/))/*prev= 0.7414*/, jP_torso[2][0]);
1488  }
1489  if (actLArm == "off"){
1490  dBodySetPosition (larm, jP_leftArm[1][1]-0.5*(0.011+0.059) /*half clavicule height + half shoulder height*//*prev=0.0815*/, elev +jP_leftArm[1][2], jP_leftArm[1][0]);
1491  }else{
1492  //left arm
1493  dBodySetPosition (body[0], jP_leftArm[1][1]-0.5*(0.011+0.059) /*half clavicule height + half shoulder height*//*prev=0.0815*/, elev +jP_leftArm[1][2], jP_leftArm[1][0]);
1494  dBodySetPosition (body[2], jP_leftArm[2][1], elev + jP_leftArm[2][2], jP_leftArm[2][0]);
1495  dBodySetPosition (body[4], jP_leftArm[4][1], elev + 0.5*(jP_leftArm[4][2]+jP_leftArm[2][2]), jP_leftArm[4][0]);
1496  dBodySetPosition (body[6], jP_leftArm[3][1], elev + jP_leftArm[3][2], jP_leftArm[3][0]);
1497  dBodySetPosition (body[8], jP_leftArm[5][1], elev + 0.5*(jP_leftArm[5][2]+jP_leftArm[3][2]), jP_leftArm[5][0]);
1498  }
1499  if (actRArm == "off"){
1500  dBodySetPosition (rarm, jP_rightArm[1][1]+0.5*(0.011+0.059) /*half clavicule height + half shoulder height*//*prev=0.0815*/, elev +jP_rightArm[1][2], jP_rightArm[1][0]);
1501  }else{
1503  dBodySetPosition (body[1], jP_rightArm[1][1]+0.5*(0.011+0.059) /*half clavicule height + half shoulder height*//*prev=0.0815*/, elev +jP_rightArm[1][2], jP_rightArm[1][0]);
1504  dBodySetPosition (body[3], jP_rightArm[2][1], elev + jP_rightArm[2][2], jP_rightArm[2][0]);
1505  dBodySetPosition (body[5], jP_rightArm[4][1], elev + 0.5*(jP_rightArm[4][2]+jP_rightArm[2][2]), jP_rightArm[4][0]);
1506  dBodySetPosition (body[7], jP_rightArm[3][1], elev + jP_rightArm[3][2], jP_rightArm[3][0]);
1507  dBodySetPosition (body[9], jP_rightArm[5][1], elev + 0.5*(jP_rightArm[5][2]+jP_rightArm[3][2]), jP_rightArm[5][0]);
1508  }
1509  if (actLHand == "off"){
1510  dBodySetPosition(l_hand, jP_leftArm[7][1], elev + jP_leftArm[6][2]-0.5*fabs(jP_leftArm[7][2]-jP_leftArm[6][2]),jP_leftArm[7][0]);
1511  }else{
1512  //left hand fingers
1513  dBodySetPosition (body[10], jP_leftArm[7][1], elev + jP_leftArm[6][2]-0.5*fabs(jP_leftArm[7][2]-jP_leftArm[6][2]),jP_leftArm[7][0]);
1514  dBodySetPosition (body[12], jP_leftArm[7][1], elev +jP_leftArm[6][2]-0.5*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])-0.04, jP_leftArm[7][0]+0.025);
1515  dBodySetPosition (body[13], jP_leftArm[7][1], elev +jP_leftArm[6][2]-0.5*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])-0.04, jP_leftArm[7][0]+0.01);
1516  dBodySetPosition(lhandfingers0,jP_leftArm[7][1],elev +jP_leftArm[6][2]-0.5*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])-0.04, jP_leftArm[7][0]-0.016125);
1517  dBodySetPosition (body[16], jP_leftArm[7][1], elev +jP_leftArm[6][2]-0.5*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])-0.059, jP_leftArm[7][0]+0.025);
1518  dBodySetPosition (body[17], jP_leftArm[7][1], elev +jP_leftArm[6][2]-0.5*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])-0.060, jP_leftArm[7][0]+0.01);
1519  dBodySetPosition(lhandfingers1,jP_leftArm[7][1],elev +jP_leftArm[6][2]-0.5*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])-0.059, jP_leftArm[7][0]-0.016125);
1520  dBodySetPosition (body[20], jP_leftArm[7][1], elev +jP_leftArm[6][2]-0.5*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])-0.083, jP_leftArm[7][0]+0.025);
1521  dBodySetPosition (body[21], jP_leftArm[7][1], elev +jP_leftArm[6][2]-0.5*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])-0.086, jP_leftArm[7][0]+0.01);
1522  dBodySetPosition(lhandfingers2,jP_leftArm[7][1], elev +jP_leftArm[6][2]-0.5*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])-0.083, jP_leftArm[7][0]-0.016125);
1523  dBodySetPosition (body[24], jP_leftArm[7][1], elev +jP_leftArm[6][2]-0.5*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])-0.104, jP_leftArm[7][0]+0.025);
1524  dBodySetPosition (body[25], jP_leftArm[7][1], elev +jP_leftArm[6][2]-0.5*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])-0.108, jP_leftArm[7][0]+0.01);
1525  dBodySetPosition(lhandfingers3,jP_leftArm[7][1], elev +jP_leftArm[6][2]-0.5*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])-0.104, jP_leftArm[7][0]-0.016125);
1526  dBodySetPosition (body[28] , jP_leftArm[7][1], elev+ jP_leftArm[6][2]-0.045/*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])+0.016*/,jP_leftArm[7][0]+0.045); //left thumb1
1527  dBodySetPosition (body[29] , jP_leftArm[7][1], elev +jP_leftArm[6][2]-0.045/*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])+0.016*/,jP_leftArm[7][0]+0.069); //left thumb2
1528  dBodySetPosition (body[30] , jP_leftArm[7][1], elev +jP_leftArm[6][2]-0.045/*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])+0.016*/,jP_leftArm[7][0]+0.088); //left thumb3
1529  }
1530  if (actRHand == "off"){
1531  dBodySetPosition(r_hand, jP_rightArm[7][1], elev + jP_rightArm[6][2]-0.5*fabs(jP_rightArm[7][2]-jP_rightArm[6][2]),jP_rightArm[7][0]);
1532  }else{
1533  //right hand fingers
1534  dBodySetPosition (body[11], jP_rightArm[7][1], elev + jP_rightArm[6][2]-0.5*fabs(jP_rightArm[7][2]-jP_rightArm[6][2]),jP_rightArm[7][0]);
1535  dBodySetPosition (body[31], jP_rightArm[7][1], elev +jP_rightArm[6][2]-0.5*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])-0.04, jP_rightArm[7][0]+0.025);
1536  dBodySetPosition (body[32], jP_rightArm[7][1], elev +jP_rightArm[6][2]-0.5*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])-0.04, jP_rightArm[7][0]+0.01);
1537  dBodySetPosition(rhandfingers0,jP_rightArm[7][1],elev +jP_rightArm[6][2]-0.5*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])-0.04, jP_rightArm[7][0]-0.016125);
1538  dBodySetPosition (body[35], jP_rightArm[7][1], elev +jP_rightArm[6][2]-0.5*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])-0.059, jP_rightArm[7][0]+0.025);
1539  dBodySetPosition (body[36], jP_rightArm[7][1], elev+jP_rightArm[6][2]-0.5*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])-0.060, jP_rightArm[7][0]+0.01);
1540  dBodySetPosition(rhandfingers1,jP_rightArm[7][1],elev +jP_rightArm[6][2]-0.5*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])-0.059, jP_rightArm[7][0]-0.016125);
1541  dBodySetPosition (body[39], jP_rightArm[7][1], elev +jP_rightArm[6][2]-0.5*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])-0.083, jP_rightArm[7][0]+0.025);
1542  dBodySetPosition (body[40], jP_rightArm[7][1], elev +jP_rightArm[6][2]-0.5*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])-0.086, jP_rightArm[7][0]+0.01);
1543  dBodySetPosition(rhandfingers2,jP_rightArm[7][1], elev +jP_rightArm[6][2]-0.5*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])-0.083, jP_rightArm[7][0]-0.016125);
1544  dBodySetPosition (body[43], jP_rightArm[7][1], elev +jP_rightArm[6][2]-0.5*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])-0.104, jP_rightArm[7][0]+0.025);
1545  dBodySetPosition (body[44], jP_rightArm[7][1], elev +jP_rightArm[6][2]-0.5*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])-0.108, jP_rightArm[7][0]+0.01);
1546  dBodySetPosition(rhandfingers3,jP_rightArm[7][1], elev+ jP_rightArm[6][2]-0.5*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])-0.104, jP_rightArm[7][0]-0.016125);
1547  dBodySetPosition (body[47] , jP_rightArm[7][1], elev +jP_rightArm[6][2]-0.045/*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])+0.016*/,jP_rightArm[7][0]+0.045); //right thumb1
1548  dBodySetPosition (body[48] , jP_rightArm[7][1], elev+ jP_rightArm[6][2]-0.045/*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])+0.016*/,jP_rightArm[7][0]+0.069); //right thumb2
1549  dBodySetPosition (body[49] , jP_rightArm[7][1], elev +jP_rightArm[6][2]-0.045/*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])+0.016*/,jP_leftArm[7][0]+0.088); //right thumb3
1550  }
1551  if (actHead == "off"){
1552  dBodySetPosition(head, jP_head[0][1], elev +jP_head[2][2]+ 0.03, jP_head[0][0]);
1553  }else{
1554  dBodySetPosition (neck[0], jP_head[0][1], elev +jP_head[0][2], jP_head[0][0]);//dBodySetPosition (neck[0], -0.0, elev +0.815, -0.026);
1555  dBodySetPosition (neck[1], jP_head[1][1], elev +jP_head[1][2], jP_head[1][0]);//dBodySetPosition (neck[1], -0.0, elev +0.845, -0.026);
1556  dBodySetPosition (head, jP_head[2][1], elev +jP_head[2][2] + 0.03/*half neck yaw*/, jP_head[2][0]);//dBodySetPosition (head, -0.0, elev +0.89, -0.026);
1557  dBodySetPosition (eye, jP_head[3][1], elev +jP_head[3][2], jP_head[3][0]);//dBodySetPosition (eye, -0.0, elev +0.89, -0.026);
1558  dBodySetPosition (leye, jP_leftEye[1][1], elev + jP_leftEye[1][2], jP_leftEye[1][0]);//dBodySetPosition (leye, -0.0, elev +0.89, -0.026);
1559  dBodySetPosition (reye, jP_rightEye[1][1], elev + jP_rightEye[1][2], jP_rightEye[1][0]);//dBodySetPosition (reye, -0.0, elev +0.89, -0.026);
1560  }
1561  // eyelids position
1562  dBodySetPosition (topEyeLid, 0.0, elev + jP_head[3][2], 0.035);
1563  dBodySetPosition (bottomEyeLid, 0.0, elev + jP_head[3][2], 0.035);
1564 
1565  dBodySetPosition (inertialBody, jP_inertial[1], elev + jP_inertial[2], jP_inertial[0]);
1566 }
1567 
1568 #define FLAGIFY(flags,name) name = flags.name?"on":"off"
1569 
1570 void ICubSim::activateiCubParts(RobotConfig& config) {
1571 
1572  string parts = config.getFinder().findFile("parts");
1573 
1574  Property options;
1575  options.fromConfigFile(parts.c_str());
1576 
1577  RobotFlags& flags = config.getFlags();
1578 
1579  config.setFlags();
1580  FLAGIFY(flags,actScreen);
1581  FLAGIFY(flags,actElevation);
1582  FLAGIFY(flags,actLegs);
1583  FLAGIFY(flags,actTorso);
1584  FLAGIFY(flags,actLArm);
1585  FLAGIFY(flags,actRArm);
1586  FLAGIFY(flags,actLHand);
1587  FLAGIFY(flags,actRHand);
1588  FLAGIFY(flags,actHead);
1589  FLAGIFY(flags,actfixedHip);
1590  FLAGIFY(flags,actSelfCol);
1591  FLAGIFY(flags,actCoversCol);
1592  FLAGIFY(flags,actVision);
1593  FLAGIFY(flags,actPressure);
1594  FLAGIFY(flags,actSkinEmul);
1595  FLAGIFY(flags,actWorld);
1596  FLAGIFY(flags,actHeadCover);
1597  FLAGIFY(flags,actLegsCovers);
1598  FLAGIFY(flags,actLeftArmCovers);
1599  FLAGIFY(flags,actRightArmCovers);
1600  FLAGIFY(flags,actTorsoCovers);
1601  FLAGIFY(flags, actStartHomePos);
1602 
1603  if (actElevation == "off"){
1604  elev = 0;
1605  }else {
1606  elev = 0.2;
1607  }
1608  modelTextureIndex = 59;
1609 }
1610 
1611 void ICubSim::init( dWorldID world, dSpaceID space, dReal X, dReal Y, dReal Z, RobotConfig& config)
1612 {
1613  ResourceFinder& finder = config.getFinder();
1614  activateiCubParts(config);
1615 
1616  for (int x =0; x<100; x++)
1617  torqueData[x] = 0.0;
1618 
1619  init_iKin();
1620  //load joint positions from file and from iKin
1621  loadJointPosition(finder.findFile(finder.find("joints").asString().c_str()).c_str());
1622 
1623  // --- READ ODE CONFIGURATION PARAMETERS FROM CONFIG FILE ---
1624  OdeParams odeParameters = config.getOdeParameters();
1625 
1626  //init
1627  if (actSelfCol == "off"){
1628  iCub = dSimpleSpaceCreate(space);
1629  dSpaceSetCleanup(iCub,0);
1630  iCubHeadSpace = iCubTorsoSpace = iCubLeftArmSpace = iCubRightArmSpace = iCubLegsSpace = iCub; //there will be only iCub space within the "space" and hence no self-collisions
1631  } else {
1632  // We create subspace for individual body parts within the space - this will allow for their collisions
1633  // see part 9.7 of ODE manual - http://ode-wiki.org/wiki/index.php?title=Manual:_All
1634  iCub = dSimpleSpaceCreate(space);
1635  dSpaceSetCleanup(iCub,0);
1636  dSpaceSetSublevel(iCub, 1);
1637 
1638  iCubHeadSpace=dSimpleSpaceCreate(iCub);
1639  dSpaceSetCleanup(iCubHeadSpace,0);
1640  dSpaceSetSublevel(iCubHeadSpace, 2);
1641 
1642  iCubTorsoSpace=dSimpleSpaceCreate(iCub);
1643  dSpaceSetCleanup(iCubTorsoSpace,0);
1644  dSpaceSetSublevel(iCubTorsoSpace, 2);
1645 
1646  iCubLeftArmSpace=dSimpleSpaceCreate(iCub);
1647  dSpaceSetCleanup(iCubLeftArmSpace,0);
1648  dSpaceSetSublevel(iCubLeftArmSpace, 2);
1649 
1650  iCubRightArmSpace=dSimpleSpaceCreate(iCub);
1651  dSpaceSetCleanup(iCubRightArmSpace,0);
1652  dSpaceSetSublevel(iCubRightArmSpace, 2);
1653 
1654  iCubLegsSpace=dSimpleSpaceCreate(iCub);
1655  dSpaceSetCleanup(iCubLegsSpace,0);
1656  dSpaceSetSublevel(iCubLegsSpace, 2);
1657  }
1658  //For debugging purposes
1659  dSpaceNames[space]="space (top level)";
1660  dSpaceNames[iCub] = "iCub - top iCub space";
1661  dSpaceNames[iCubHeadSpace] = "iCubHeadSpace";
1662  dSpaceNames[iCubTorsoSpace] = "iCubTorsoSpace";
1663  dSpaceNames[iCubLeftArmSpace] = "iCubLeftArmSpace";
1664  dSpaceNames[iCubRightArmSpace] = "iCubRightArmSpace";
1665  dSpaceNames[iCubLegsSpace] = "iCubLegsSpace";
1666 
1667  // initCovers(finder); //This had to be moved lower down - in order to be able to attach the covers to bodies in the actSelfCol, the bodies need to be created first
1668 
1669  if (actScreen == "on")
1670  screenGeom = dCreateBox(space,1.0,1.0,0.01); //this will create a thin screen of width 1m height 1m and depth 0.01m
1671 
1672  if (actLegs == "off"){
1673  initLegsOff(world,iCubLegsSpace);
1674  }else{
1675  initLegsOn(world,iCubLegsSpace);
1676  }
1677  if (actTorso == "off"){
1678  initTorsoOff(world,iCubTorsoSpace);
1679  }else{
1680  initTorsoOn(world,iCubTorsoSpace);
1681  }
1682  if (actLArm == "off"){
1683  initLeftArmOff(world,iCubLeftArmSpace);
1684  }else{
1685  initLeftArmOn(world,iCubLeftArmSpace);
1686  }
1687  if (actRArm == "off"){
1688  initRightArmOff(world,iCubRightArmSpace);
1689  }else{
1690  initRightArmOn(world,iCubRightArmSpace);
1691  }
1692  if (actLHand == "off"){
1693  initLeftHandOff(world,iCubLeftArmSpace);
1694  }else{
1695  initLeftHandOn(world,iCubLeftArmSpace);
1696  }
1697 
1698  if (actRHand == "off"){
1699  initRightHandOff(world,iCubRightArmSpace);
1700  }else{
1701  initRightHandOn(world,iCubRightArmSpace);
1702  }
1703 
1704  initHead(world,iCubHeadSpace);
1705 
1706  initEyes(world,iCubHeadSpace);
1707 
1708  // inertial sensor box
1709  dMass m;
1710  inertialBody = dBodyCreate (world);dMassSetZero(&m);dMassSetBoxTotal(&m,0.0001,0.03,0.02,0.05);
1711  dBodySetMass(inertialBody,&m);
1712  inertialGeom = dCreateBox (iCubHeadSpace,0.03,0.02,0.05);dGeomSetBody (inertialGeom,inertialBody);
1713 
1714  setPosition( X, Y, Z);
1715 
1716  if (actElevation == "on")
1717  {
1718  elevJoint = dJointCreateFixed(world, 0);
1719  if (actTorso == "off"){
1720  dJointAttach (elevJoint,body_torso,0 );dJointSetFixed(elevJoint);
1721  }else{
1722  //dJointAttach (elevJoint,torso[0],0 );dJointSetFixed(elevJoint);
1723  }
1724  }
1725  //joints initialization
1726  for (int x=0; x<6; x++){
1727  LLegjoints[x] = dJointCreateHinge(world, 0);
1728  RLegjoints[x] = dJointCreateHinge(world, 0);
1729  }
1730  for (int x=0; x<5; x++){
1731  Torsojoints[x] = dJointCreateHinge(world,0);
1732  }
1733  for (int x=0; x<5; x++){
1734  LAjoints[x] = dJointCreateHinge(world,0);
1735  RAjoints[x] = dJointCreateHinge(world,0);
1736  }
1737  for(int x = 5; x < 6; x++) {
1738  LAjoints[x] = dJointCreateUniversal(world, 0);
1739  RAjoints[x] = dJointCreateUniversal(world, 0);
1740  }
1741  for(int x = 6; x < 22; x++) {
1742  LAjoints[x] = dJointCreateHinge(world, 0);
1743  RAjoints[x] = dJointCreateHinge(world, 0);
1744  }
1745  for(int x = 22; x < 23; x++) {
1746  LAjoints[x] = dJointCreateUniversal(world, 0);
1747  RAjoints[x] = dJointCreateUniversal(world, 0);
1748  }
1749  for(int x = 23; x < 25; x++) {
1750  LAjoints[x] = dJointCreateHinge(world, 0);
1751  RAjoints[x] = dJointCreateHinge(world, 0);
1752  }
1753  for(int x = 0; x <6; x++) {Hjoints[x] = dJointCreateHinge(world, 0);}
1754 
1755  initLegJoints();
1756  initTorsoJoints(odeParameters);
1757 
1758  initLeftArmJoints(odeParameters);
1759  initLeftHandJoints();
1760 
1761  initRightArmJoints(odeParameters);
1762  initRightHandJoints();
1763 
1764  initHeadJoints();
1765 
1766  //joint parameters
1767  for (int x=0; x<6; x++){
1768  //dJointSetHingeParam(LLegjoints[x], dParamVel, LLeg_speed[x]);// Desired motor velocity (this will be an angular or linear velocity).
1769  dJointSetHingeParam(LLegjoints[x], dParamFMax, odeParameters.motorMaxTorque); //The maximum force or torque that the motor will use to achieve//the desired velocity.
1770 
1771  dJointSetHingeParam(RLegjoints[x], dParamVel, RLeg_speed[x]);// Desired motor velocity (this will be an angular or linear velocity).
1772  dJointSetHingeParam(RLegjoints[x], dParamFMax,odeParameters.motorMaxTorque); //The maximum force or torque that the motor will use to achieve//the desired velocity.
1773  }
1774  for (int x=0; x<5; x++){
1775  dJointSetHingeParam(Torsojoints[x], dParamVel, Torso_speed[x]);
1776  dJointSetHingeParam(Torsojoints[x], dParamFMax,odeParameters.motorMaxTorque);
1777  }
1778  for (int x=0; x<5; x++){
1779  dJointSetHingeParam(LAjoints[x], dParamVel, la_speed[x]);
1780  dJointSetHingeParam(LAjoints[x], dParamFMax,odeParameters.motorMaxTorque);
1781  dJointSetHingeParam(RAjoints[x], dParamVel, ra_speed[x]);
1782  dJointSetHingeParam(RAjoints[x], dParamFMax,odeParameters.motorMaxTorque);
1783  }
1784  for (int x=5; x<6;x++){//for the hands
1785  dJointSetUniversalParam(LAjoints[x], dParamVel, la_speed[x]);
1786  dJointSetUniversalParam(LAjoints[x], dParamFMax,odeParameters.motorMaxTorque);
1787  dJointSetUniversalParam(LAjoints[x], dParamVel2, la_speed1[x]);
1788  dJointSetUniversalParam(LAjoints[x], dParamFMax2,odeParameters.motorMaxTorque);
1789 
1790  dJointSetUniversalParam(RAjoints[x], dParamVel, ra_speed[x]);
1791  dJointSetUniversalParam(RAjoints[x], dParamFMax,odeParameters.motorMaxTorque);
1792  dJointSetUniversalParam(RAjoints[x], dParamVel2, ra_speed1[x]);
1793  dJointSetUniversalParam(RAjoints[x], dParamFMax2,odeParameters.motorMaxTorque);
1794  }
1795  for (int x=6; x<25; x++){//22
1796  if (x!=9 && x!=13 && x!=17 && x!=21 && x!=22){
1797  dJointSetHingeParam(LAjoints[x], dParamVel, la_speed[x]);
1798  dJointSetHingeParam(LAjoints[x], dParamFMax,odeParameters.motorMaxTorque);
1799  dJointSetHingeParam(RAjoints[x], dParamVel, ra_speed[x]);
1800  dJointSetHingeParam(RAjoints[x], dParamFMax,odeParameters.motorMaxTorque);
1801  }
1802  }
1803  for (int x=22; x<23;x++){//for the hands
1804  dJointSetUniversalParam(LAjoints[x], dParamVel, la_speed[x]);
1805  dJointSetUniversalParam(LAjoints[x], dParamFMax, odeParameters.motorMaxTorque);
1806  dJointSetUniversalParam(LAjoints[x], dParamVel2, la_speed1[x]);
1807  dJointSetUniversalParam(LAjoints[x], dParamFMax2, odeParameters.motorMaxTorque);
1808 
1809  dJointSetUniversalParam(RAjoints[x], dParamVel, ra_speed[x]);
1810  dJointSetUniversalParam(RAjoints[x], dParamFMax, odeParameters.motorMaxTorque);
1811  dJointSetUniversalParam(RAjoints[x], dParamVel2, ra_speed1[x]);
1812  dJointSetUniversalParam(RAjoints[x], dParamFMax2, odeParameters.motorMaxTorque);
1813  }
1814 
1815  dJointSetHingeParam(Hjoints[0], dParamVel, h_speed[0]);
1816  dJointSetHingeParam(Hjoints[0], dParamFMax, odeParameters.motorMaxTorque);
1817  if (actHead == "on" ){
1818  /*-------------head parameters--------------*/
1819  for (int x=1; x<6; x++){//Joint parameters
1820  dJointSetHingeParam(Hjoints[x], dParamVel, h_speed[x]);
1821  dJointSetHingeParam(Hjoints[x], dParamFMax, odeParameters.motorMaxTorque);
1822  }
1823  }
1824 
1825  /* Create a fixed hip joint */
1826  if (actfixedHip == "on"){// && actElevation == "off") {
1827  fixedHipJoint = dJointCreateFixed(world, 0);
1828  if (actTorso == "off") {
1829  // attach to the lower torso
1830  dJointAttach (fixedHipJoint,body_torso,0);
1831  // move the torso up slightly (0.03 units)???
1832  //dBodySetPosition (body_torso, 0.0, 0.4912, -0.034);
1833  } else {
1834  // attach to the lower torso
1835  dJointAttach (fixedHipJoint,torso[0],0);
1836  // move the torso up slightly (0.03 units)???
1837  //dBodySetPosition (torso[0], 0.0, 0.4912, -0.034);
1838  }
1839  // this call fixes the joint to its current position in 3D space
1840  dJointSetFixed (fixedHipJoint);
1841  }
1842  inertialJoint = dJointCreateFixed(world, 0);
1843  dJointAttach (inertialJoint,inertialBody,head);
1844  dJointSetFixed (inertialJoint);
1845 
1846  initCovers(finder); //Moved this here - in order to be able to attach the covers to bodies, the bodies need to be created first; also to get the positions of
1847  // the cover geoms right, the bodies and geoms already have to be in right positions and also the joints are needed for some transforms (like in draw() )
1848 
1849  initSkinActivationBottles(); //these will be necessary for the skin emulation - output to ports - empty or full activations
1850 
1851 }
1852 
1853 void ICubSim::initLegsOff(dWorldID world, dSpaceID subspace)
1854 {
1855  dMass m, m2;
1856  dMatrix3 Rtx;
1857 
1858  legs = dBodyCreate (world);dMassSetZero(&m);dMassSetZero(&m2);
1859  l_leg0_geom = dCreateBox (subspace,0.054,0.004,0.13);dMassSetBoxTotal(&m2,0.08185,0.054,0.004,0.13);
1860  dGeomNames[l_leg0_geom] = "l_leg0_geom";
1861  dGeomSetBody (l_leg0_geom,legs);
1862  dGeomSetOffsetPosition(l_leg0_geom,-m2.c[0], 0.0021-m2.c[0], -m2.c[0]);
1863  dMassAdd (&m, &m2);
1864 
1865  l_leg1_geom = dCreateCylinder (subspace,0.027,0.095);dMassSetCylinderTotal(&m2,0.59285,3,0.027,0.095);
1866  dGeomNames[l_leg1_geom] = "l_leg1_geom";
1867  dGeomSetBody (l_leg1_geom,legs);
1868  dGeomSetOffsetPosition(l_leg1_geom,-m2.c[0], jP_leftLeg[1][2]-m2.c[0], -m2.c[0]);
1869  dMassAdd (&m, &m2);
1870 
1871  l_leg2_geom = dCreateCylinder (subspace,0.0245,0.063);dMassSetCylinderTotal(&m2,0.14801,3,0.0245,0.063);
1872  dGeomNames[l_leg2_geom] = "l_leg2_geom";
1873  dRFromAxisAndAngle(Rtx,0,1,0,M_PI * 0.5);
1874  dGeomSetBody (l_leg2_geom,legs);
1875  dGeomSetOffsetRotation(l_leg2_geom,Rtx);
1876  dGeomSetOffsetPosition(l_leg2_geom,-m2.c[0], jP_leftLeg[1][2]-m2.c[0], -m2.c[0]);
1877  dMassAdd (&m, &m2);
1878 
1879  l_leg3_geom = dCreateCylinder (subspace,0.0315,0.213);dMassSetCylinderTotal(&m2,0.95262,3,0.0315,0.213);
1880  dGeomNames[l_leg3_geom] = "l_leg3_geom";
1881  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
1882  dGeomSetBody (l_leg3_geom,legs);
1883  dGeomSetOffsetRotation(l_leg3_geom,Rtx);
1884  dGeomSetOffsetPosition(l_leg3_geom,-m2.c[0], 0.5*fabs(jP_leftLeg[2][2]+jP_leftLeg[1][2])-m2.c[0], -m2.c[0]);
1885  dMassAdd (&m, &m2);
1886 
1887  l_leg4_geom = dCreateCylinder (subspace,0.0315,0.077);dMassSetCylinderTotal(&m2,0.79206,3,0.0315,0.077);
1888  dGeomNames[l_leg4_geom] = "l_leg4_geom";
1889  dRFromAxisAndAngle(Rtx,0,1,0,M_PI * 0.5);
1890  dGeomSetBody (l_leg4_geom,legs);
1891  dGeomSetOffsetRotation(l_leg4_geom,Rtx);
1892  dGeomSetOffsetPosition(l_leg4_geom,-m2.c[0], jP_leftLeg[2][2]-m2.c[0], -m2.c[0]);
1893  dMassAdd (&m, &m2);
1894 
1895  l_leg5_geom = dCreateCylinder (subspace,0.034,0.224);dMassSetCylinderTotal(&m2,1.5304,3,0.034,0.224);
1896  dGeomNames[l_leg5_geom] = "l_leg5_geom";
1897  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
1898  dGeomSetBody (l_leg5_geom,legs);
1899  dGeomSetOffsetRotation(l_leg5_geom,Rtx);
1900  dGeomSetOffsetPosition(l_leg5_geom,-m2.c[0], 0.5*fabs(jP_leftLeg[3][2]+jP_leftLeg[2][2])-m2.c[0],-m2.c[0]);
1901 
1902  l_leg6_geom = dCreateCylinder (subspace,0.031,0.075);dMassSetCylinderTotal(&m2,1.5304,3,0.031,0.075);
1903  dGeomNames[l_leg6_geom] = "l_leg6_geom";
1904  dGeomSetBody (l_leg6_geom,legs);
1905  dGeomSetOffsetPosition(l_leg6_geom,-m2.c[0], jP_leftLeg[4][2]-m2.c[0], -m2.c[0]);
1906  dMassAdd (&m, &m2);
1907 
1908  l_leg7_geom = dCreateCylinder (subspace,0.038,0.013);dMassSetCylinderTotal(&m2,0.32708,3,0.038,0.013);
1909  dGeomNames[l_leg7_geom] = "l_leg7_geom";
1910  dRFromAxisAndAngle(Rtx,0,1,0,M_PI * 0.5);
1911  dGeomSetBody (l_leg7_geom,legs);
1912  dGeomSetOffsetRotation(l_leg7_geom,Rtx);
1913  dGeomSetOffsetPosition(l_leg7_geom,-fabs(jP_leftLeg[0][1]-0.0295)-m2.c[0], jP_leftLeg[4][2]-m2.c[0],-m2.c[0]);
1914  dMassAdd (&m, &m2);
1915 
1916  double r_foot_y=fabs(jP_leftLeg[0][1]-jP_rightLeg[0][1]);
1917 
1918  r_leg0_geom = dCreateBox (subspace,0.054,0.004,0.13);dMassSetBoxTotal(&m2,0.08185,0.054,0.004,0.13);
1919  dGeomNames[r_leg0_geom] = "r_leg0_geom";
1920  dGeomSetBody (r_leg0_geom,legs);
1921  dGeomSetOffsetPosition(r_leg0_geom,-r_foot_y-m2.c[0], 0.0021-m2.c[0], -m2.c[0]);
1922  dMassAdd (&m, &m2);
1923 
1924  r_leg1_geom = dCreateCylinder (subspace,0.027,0.095);dMassSetCylinderTotal(&m2,0.59285,3,0.027,0.095);
1925  dGeomNames[r_leg1_geom] = "r_leg1_geom";
1926  dGeomSetBody (r_leg1_geom,legs);
1927  dGeomSetOffsetPosition(r_leg1_geom,-r_foot_y-m2.c[0],jP_leftLeg[1][2]-m2.c[0], -m2.c[0]);
1928  dMassAdd (&m, &m2);
1929 
1930  r_leg2_geom = dCreateCylinder (subspace,0.0245,0.063);dMassSetCylinderTotal(&m2,0.14801,3,0.0245,0.063);
1931  dGeomNames[r_leg2_geom] = "r_leg2_geom";
1932  dRFromAxisAndAngle(Rtx,0,1,0,M_PI * 0.5);
1933  dGeomSetBody (r_leg2_geom,legs);
1934  dGeomSetOffsetRotation(r_leg2_geom,Rtx);
1935  dGeomSetOffsetPosition(r_leg2_geom,-r_foot_y-m2.c[0], jP_leftLeg[1][2]-m2.c[0], -m2.c[0]);
1936  dMassAdd (&m, &m2);
1937 
1938  r_leg3_geom = dCreateCylinder (subspace,0.0315,0.213);dMassSetCylinderTotal(&m2,0.95262,3,0.0315,0.213);
1939  dGeomNames[r_leg3_geom] = "r_leg3_geom";
1940  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
1941  dGeomSetBody (r_leg3_geom,legs);
1942  dGeomSetOffsetRotation(r_leg3_geom,Rtx);
1943  dGeomSetOffsetPosition(r_leg3_geom,-r_foot_y-m2.c[0], 0.5*fabs(jP_leftLeg[2][2]+jP_leftLeg[1][2])-m2.c[0], -m2.c[0]);
1944  dMassAdd (&m, &m2);
1945 
1946  r_leg4_geom = dCreateCylinder (subspace,0.0315,0.077);dMassSetCylinderTotal(&m2,0.79206,3,0.0315,0.077);
1947  dGeomNames[r_leg4_geom] = "r_leg4_geom";
1948  dRFromAxisAndAngle(Rtx,0,1,0,M_PI * 0.5);
1949  dGeomSetBody (r_leg4_geom,legs);
1950  dGeomSetOffsetRotation(r_leg4_geom,Rtx);
1951  dGeomSetOffsetPosition(r_leg4_geom,-r_foot_y-m2.c[0], jP_rightLeg[2][2]-m2.c[0], -m2.c[0]);
1952  dMassAdd (&m, &m2);
1953 
1954  r_leg5_geom = dCreateCylinder (subspace,0.034,0.224);dMassSetCylinderTotal(&m2,1.5304,3,0.034,0.224);
1955  dGeomNames[r_leg5_geom] = "r_leg5_geom";
1956  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
1957  dGeomSetBody (r_leg5_geom,legs);
1958  dGeomSetOffsetRotation(r_leg5_geom,Rtx);
1959  dGeomSetOffsetPosition(r_leg5_geom,-r_foot_y-m2.c[0],0.5*fabs(jP_leftLeg[3][2]+jP_leftLeg[2][2])-m2.c[0], -m2.c[0]);
1960 
1961  r_leg6_geom = dCreateCylinder (subspace,0.031,0.075);dMassSetCylinderTotal(&m2,1.5304,3,0.031,0.075);
1962  dGeomNames[r_leg6_geom] = "r_leg6_geom";
1963  dGeomSetBody (r_leg6_geom,legs);
1964  dGeomSetOffsetPosition(r_leg6_geom,-r_foot_y-m2.c[0], jP_rightLeg[4][2]-m2.c[0], -m2.c[0]);
1965  dMassAdd (&m, &m2);
1966 
1967  r_leg7_geom = dCreateCylinder (subspace,0.038,0.013);dMassSetCylinderTotal(&m2,0.32708,3,0.038,0.013);
1968  dGeomNames[r_leg7_geom] = "r_leg7_geom";
1969  dRFromAxisAndAngle(Rtx,0,1,0,M_PI * 0.5);
1970  dGeomSetBody (r_leg7_geom,legs);
1971  dGeomSetOffsetRotation(r_leg7_geom,Rtx);
1972  dGeomSetOffsetPosition(r_leg7_geom,fabs(jP_rightLeg[0][1]+0.0295)-r_foot_y-m2.c[0], jP_rightLeg[4][2]-m2.c[0], -m2.c[0]);
1973  dMassAdd (&m, &m2);
1974 
1975  dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
1976  dBodySetMass(legs,&m);
1977 }
1978 
1979 void ICubSim::initLegsOn(dWorldID world, dSpaceID subspace)
1980 {
1981  dMass m, m2;
1982  dMatrix3 Rtx;
1983  //rotation matrises
1984  dQuaternion q1;
1985  dQFromAxisAndAngle(q1,0,1,0,M_PI * 0.5);
1986 
1987  //left lower parts
1988  leftLeg[0] = dBodyCreate (world);dMassSetZero(&m);dMassSetBoxTotal(&m,0.08185,0.054,0.004,0.13); //Y, Z, X
1989  dBodySetMass(leftLeg[0],&m);
1990  leftLegGeom[0] = dCreateBox (subspace,0.054,0.004,0.13);dGeomSetBody (leftLegGeom[0],leftLeg[0]);
1991  dGeomNames[leftLegGeom[0]] = "leftLegGeom[0]";
1992 
1993  leftLeg[1] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.59285,3,0.027,0.095);
1994  dBodySetMass(leftLeg[1],&m);
1995  leftLegGeom[1] = dCreateCylinder (subspace,0.027,0.095);dGeomSetBody(leftLegGeom[1],leftLeg[1]);
1996  dGeomNames[leftLegGeom[1]] = "leftLegGeom[1]";
1997  //----------------------------------------------------------ankle encapsulated objects
1998  leftLeg[2] = dBodyCreate (world);dMassSetZero(&m);dMassSetZero(&m2);
1999  leftLeg_2_1 = dCreateCylinder (subspace,0.0245,0.063);dMassSetCylinderTotal(&m2,0.14801,3,0.0245,0.063);
2000  dGeomNames[leftLeg_2_1] = "leftLeg_2_1";
2001  dRFromAxisAndAngle(Rtx,0,1,0,M_PI * 0.5);
2002  dGeomSetBody (leftLeg_2_1,leftLeg[2]);
2003  dGeomSetOffsetRotation(leftLeg_2_1,Rtx);//dGeomSetPosition (leftLeg_1 , 0.0, 1.0, 0.0);
2004  dGeomSetOffsetPosition(leftLeg_2_1,-m2.c[0], -m2.c[0], -m2.c[0]);//dMassRotate(&m2,Rtx);
2005  dMassAdd (&m, &m2);
2006  //second object
2007  leftLeg_2_2 = dCreateCylinder (subspace,0.0315,fabs(jP_leftLeg[2][2]-jP_leftLeg[1][2]));dMassSetCylinderTotal(&m2,0.95262,3,0.0315,0.213);
2008  dGeomNames[leftLeg_2_2] = "leftLeg_2_2";
2009  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2010  dGeomSetBody (leftLeg_2_2,leftLeg[2]);
2011  dGeomSetOffsetRotation(leftLeg_2_2,Rtx);
2012  dGeomSetOffsetPosition(leftLeg_2_2,-m2.c[0], 0.5*fabs(jP_leftLeg[2][2]-jP_leftLeg[1][2])-m2.c[0], -m2.c[0]);//dGeomSetPosition (leftLeg_2 , 0.0, 1, 0.0);
2013  //add mass accumulated
2014  dMassAdd (&m, &m2);
2015  //translate
2016  dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
2017  //Set mass to the actual body
2018  dBodySetMass(leftLeg[2],&m);
2019  //----------------------------------
2020  leftLeg[3] = dBodyCreate (world);dMassSetZero(&m);dMassSetZero(&m2);
2021  leftLeg_3_1 = dCreateCylinder (subspace,0.0315,0.077);dMassSetCylinderTotal(&m2,0.79206,3,0.0315,0.077);
2022  dGeomNames[leftLeg_3_1] = "leftLeg_3_1";
2023  dRFromAxisAndAngle(Rtx,0,1,0,M_PI * 0.5);
2024  dGeomSetBody (leftLeg_3_1,leftLeg[3]);
2025  dGeomSetOffsetRotation(leftLeg_3_1,Rtx);//dGeomSetPosition (leftLeg_1 , 0.0, 1.0, 0.0);
2026  dGeomSetOffsetPosition(leftLeg_3_1,-m2.c[0], -m2.c[0], -m2.c[0]);//dMassRotate(&m2,Rtx);
2027  dMassAdd (&m, &m2);
2028  //second object
2029  leftLeg_3_2 = dCreateCylinder (subspace,0.034,fabs(jP_leftLeg[3][2]-jP_leftLeg[2][2]));dMassSetCylinderTotal(&m2,1.5304,3,0.034,0.224);
2030  dGeomNames[leftLeg_3_2] = "leftLeg_3_2";
2031  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2032  dGeomSetRotation (leftLeg_3_2,Rtx);
2033  dGeomSetBody (leftLeg_3_2,leftLeg[3]);
2034  dGeomSetOffsetRotation(leftLeg_3_2,Rtx);
2035  dGeomSetOffsetPosition(leftLeg_3_2,-m2.c[0], 0.5*fabs(jP_leftLeg[3][2]-jP_leftLeg[2][2])/*0.1065*/-m2.c[0], -m2.c[0]);//dGeomSetPosition (leftLeg_2 , 0.0, 1, 0.0);
2036  //add mass accumulated
2037  dMassAdd (&m, &m2);
2038  //translate
2039  dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
2040  //Set mass to the actual body
2041  dBodySetMass(leftLeg[3],&m);
2042  //---------------------------------------------------
2043  leftLeg[4] = dBodyCreate (world);dMassSetZero(&m);dMassSetZero(&m2);
2044  leftLeg_4_1 = dCreateSphere(subspace,0.017);dMassSetSphereTotal(&m,0.01,0.017);
2045  dGeomNames[leftLeg_4_1] = "leftLeg_4_1";
2046  dGeomSetBody (leftLeg_4_1,leftLeg[4]);
2047  dGeomSetOffsetPosition(leftLeg_4_1,-m2.c[0], -m2.c[0], -m2.c[0]);//dMassRotate(&m2,Rtx);
2048  dMassAdd (&m, &m2);
2049  //second object
2050  leftLeg_4_2 = dCreateCylinder (subspace,0.031,0.075);dMassSetCylinderTotal(&m2,1.5304,3,0.031,0.075);
2051  dGeomNames[leftLeg_4_2] = "leftLeg_4_2";
2052  dGeomSetBody (leftLeg_4_2,leftLeg[4]);
2053  dGeomSetOffsetPosition(leftLeg_4_2,-m2.c[0], -m2.c[0], -m2.c[0]);//dGeomSetPosition (leftLeg_2 , 0.0, 1, 0.0);
2054  //add mass accumulated
2055  dMassAdd (&m, &m2);
2056  //translate
2057  dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
2058  //Set mass to the actual body
2059  dBodySetMass(leftLeg[4],&m);
2060  //------------------------------------------------
2061  leftLeg[5] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.32708,3,0.038,0.013);
2062  dBodySetMass(leftLeg[5],&m);
2063  dBodySetQuaternion(leftLeg[5],q1);
2064  leftLegGeom[5] = dCreateCylinder (subspace,0.038,0.013);dGeomSetBody(leftLegGeom[5],leftLeg[5]);
2065  dGeomNames[leftLegGeom[5]] = "leftLegGeom[5]";
2066  //--------------------------------------------------------
2067  //RIGHT LEG
2068  //--------------------------------------------------------
2069  //right lower parts
2070  rightLeg[0] = dBodyCreate (world);dMassSetZero(&m);dMassSetBoxTotal(&m,0.08185,0.054,0.004,0.13); //Y, Z, X
2071  dBodySetMass(rightLeg[0],&m);
2072  rightLegGeom[0] = dCreateBox (subspace,0.054,0.004,0.13);dGeomSetBody (rightLegGeom[0],rightLeg[0]);
2073  dGeomNames[rightLegGeom[0]] = "rightLegGeom[0]";
2074 
2075  rightLeg[1] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.59285,3,0.027,0.095);
2076  dBodySetMass(rightLeg[1],&m);
2077  rightLegGeom[1] = dCreateCylinder (subspace,0.027,0.095);dGeomSetBody(rightLegGeom[1],rightLeg[1]);
2078  dGeomNames[rightLegGeom[1]] = "rightLegGeom[1]";
2079  //----------------------------------------------------------ankle encapsulated objects
2080  rightLeg[2] = dBodyCreate (world);dMassSetZero(&m);dMassSetZero(&m2);
2081  rightLeg_2_1 = dCreateCylinder (subspace,0.0245,0.063);dMassSetCylinderTotal(&m2,0.14801,3,0.0245,0.063);
2082  dGeomNames[rightLeg_2_1] = "rightLeg_2_1";
2083  dRFromAxisAndAngle(Rtx,0,1,0,M_PI * 0.5);
2084  dGeomSetBody (rightLeg_2_1,rightLeg[2]);
2085  dGeomSetOffsetRotation(rightLeg_2_1,Rtx);//dGeomSetPosition (rightLeg_1 , 0.0, 1.0, 0.0);
2086  dGeomSetOffsetPosition(rightLeg_2_1,-m2.c[0], -m2.c[0], -m2.c[0]);//dMassRotate(&m2,Rtx);
2087  dMassAdd (&m, &m2);
2088  //second object
2089  rightLeg_2_2 = dCreateCylinder (subspace,0.0315,fabs(jP_rightLeg[2][2]-jP_rightLeg[1][2]));dMassSetCylinderTotal(&m2,0.95262,3,0.0315,0.213);
2090  dGeomNames[rightLeg_2_2] = "rightLeg_2_2";
2091  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2092  dGeomSetRotation (rightLeg_2_2,Rtx);
2093  dGeomSetBody (rightLeg_2_2,rightLeg[2]);
2094  dGeomSetOffsetRotation(rightLeg_2_2,Rtx);
2095  dGeomSetOffsetPosition(rightLeg_2_2,-m2.c[0], 0.5*fabs(jP_leftLeg[2][2]-jP_leftLeg[1][2])-m2.c[0], -m2.c[0]);//dGeomSetPosition (rightLeg_2 , 0.0, 1, 0.0);
2096  //add mass accumulated
2097  dMassAdd (&m, &m2);
2098  //translate
2099  dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
2100  //Set mass to the actual body
2101  dBodySetMass(rightLeg[2],&m);
2102  //----------------------------------
2103  rightLeg[3] = dBodyCreate (world);dMassSetZero(&m);dMassSetZero(&m2);
2104  rightLeg_3_1 = dCreateCylinder (subspace,0.0315,0.077);dMassSetCylinderTotal(&m2,0.79206,3,0.0315,0.077);
2105  dGeomNames[rightLeg_3_1] = "rightLeg_3_1";
2106  dRFromAxisAndAngle(Rtx,0,1,0,M_PI * 0.5);
2107  dGeomSetBody (rightLeg_3_1,rightLeg[3]);
2108  dGeomSetOffsetRotation(rightLeg_3_1,Rtx);//dGeomSetPosition (rightLeg_1 , 0.0, 1.0, 0.0);
2109  dGeomSetOffsetPosition(rightLeg_3_1,-m2.c[0], -m2.c[0], -m2.c[0]);//dMassRotate(&m2,Rtx);
2110  dMassAdd (&m, &m2);
2111  //second object
2112  rightLeg_3_2 = dCreateCylinder (subspace,0.034,fabs(jP_rightLeg[3][2]-jP_rightLeg[2][2]));dMassSetCylinderTotal(&m2,1.5304,3,0.034,0.224);
2113  dGeomNames[rightLeg_3_2] = "rightLeg_3_2";
2114  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2115  dGeomSetRotation (rightLeg_3_2,Rtx);
2116  dGeomSetBody (rightLeg_3_2,rightLeg[3]);
2117  dGeomSetOffsetRotation(rightLeg_3_2,Rtx);
2118  dGeomSetOffsetPosition(rightLeg_3_2,-m2.c[0],0.5*fabs(jP_rightLeg[3][2]-jP_rightLeg[2][2])-m2.c[0], -m2.c[0]);//dGeomSetPosition (rightLeg_2 , 0.0, 1, 0.0);
2119  //add mass accumulated
2120  dMassAdd (&m, &m2);
2121  //translate
2122  dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
2123  //Set mass to the actual body
2124  dBodySetMass(rightLeg[3],&m);
2125  //---------------------------------------------------
2126  rightLeg[4] = dBodyCreate (world);dMassSetZero(&m);dMassSetZero(&m2);
2127  rightLeg_4_1 = dCreateSphere(subspace,0.017);dMassSetSphereTotal(&m,0.01,0.017);
2128  dGeomNames[rightLeg_4_1] = "rightLeg_4_1";
2129  dGeomSetBody (rightLeg_4_1,rightLeg[4]);
2130  dGeomSetOffsetPosition(rightLeg_4_1,-m2.c[0], -m2.c[0], -m2.c[0]);//dMassRotate(&m2,Rtx);
2131  dMassAdd (&m, &m2);
2132  //second object
2133  rightLeg_4_2 = dCreateCylinder (subspace,0.031,0.075);dMassSetCylinderTotal(&m2,1.5304,3,0.031,0.075);
2134  dGeomNames[rightLeg_4_2] = "rightLeg_4_2";
2135  dGeomSetBody (rightLeg_4_2,rightLeg[4]);
2136  dGeomSetOffsetPosition(rightLeg_4_2,-m2.c[0], -m2.c[0], -m2.c[0]);//dGeomSetPosition (rightLeg_2 , 0.0, 1, 0.0);
2137  //add mass accumulated
2138  dMassAdd (&m, &m2);
2139  //translate
2140  dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
2141  //Set mass to the actual body
2142  dBodySetMass(rightLeg[4],&m);
2143  //------------------------------------------------
2144  rightLeg[5] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.32708,3,0.038,0.013);
2145  dBodySetMass(rightLeg[5],&m);
2146  dBodySetQuaternion(rightLeg[5],q1);
2147  rightLegGeom[5] = dCreateCylinder (subspace,0.038,0.013);dGeomSetBody(rightLegGeom[5],rightLeg[5]);
2148  dGeomNames[rightLegGeom[5]] = "rightLegGeom[5]";
2149 }
2150 
2151 void ICubSim::initTorsoOff(dWorldID world, dSpaceID subspace)
2152 {
2153  dMass m, m2;
2154  dMatrix3 Rtx;
2155 
2156  body_torso = dBodyCreate (world);dMassSetZero(&m);dMassSetZero(&m2);
2157  torso0_geom = dCreateBox (subspace,0.0470,fabs((jP_torso[0][2]-0.031/*torso pitch cyl rad*/)-(jP_leftLeg[5][2]-0.031/*leg cyl rad*/)),0.064);dMassSetBoxTotal(&m2,0.20297,0.004,0.13,0.054);
2158  dGeomNames[torso0_geom] = "torso0_geom";
2159  dGeomSetBody (torso0_geom,body_torso);
2160  dGeomSetOffsetPosition(torso0_geom,-m2.c[0], -m2.c[0], -m2.c[0]);
2161  dMassAdd (&m, &m2);
2162 
2163  double z_offset=0.5*fabs((jP_torso[0][2]-0.031/*torso pitch cyl rad*/)+(jP_leftLeg[5][2]-0.031/*leg cyl rad*/));
2164 
2165  //torso1_geom = dCreateBox (subspace,0.176,0.063,0.127);dMassSetBoxTotal(&m2,3.623,0.176,0.063,0.127);
2166  torso1_geom = dCreateCylinder (subspace,0.031,fabs(jP_leftLeg[3][1]-jP_rightLeg[3][1]));dMassSetCylinderTotal(&m2,0.91179,3,0.031,fabs(jP_leftLeg[3][1]-jP_rightLeg[3][1]));
2167  dGeomNames[torso1_geom] = "torso1_geom";
2168  //dRFromAxisAndAngle(Rtx,0,1,0,M_PI * 0.5);
2169  dGeomSetBody (torso1_geom,body_torso);
2170  //dGeomSetOffsetRotation(torso3_geom,Rtx);
2171  dGeomSetOffsetPosition(torso1_geom,-m2.c[0], jP_torso[0][2]-z_offset-m2.c[0], -0.006-m2.c[0]);
2172  dMassAdd (&m, &m2);
2173 
2174  torso2_geom = dCreateCylinder (subspace,0.031,0.097);dMassSetCylinderTotal(&m2,0.91179,3,0.031,0.097);
2175  dGeomNames[torso2_geom] = "torso2_geom";
2176  //dRFromAxisAndAngle(Rtx,0,1,0,M_PI * 0.5);
2177  dGeomSetBody (torso2_geom,body_torso);
2178  //dGeomSetOffsetRotation(torso2_geom,Rtx);
2179  dGeomSetOffsetPosition(torso2_geom,jP_torso[1][1]-jP_torso[0][1]-m2.c[0], jP_torso[1][2]-z_offset-m2.c[0], -m2.c[0]);
2180  dMassAdd (&m, &m2);
2181 
2182  /*torso3_geom = dCreateCylinder (subspace,0.04,0.0274);dMassSetCylinderTotal(&m,0.45165,3,0.04,0.0274);
2183  dRFromAxisAndAngle(Rtx,0,1,0,M_PI * 0.5);
2184  dGeomSetBody (torso3_geom,body_torso);
2185  dGeomSetOffsetRotation(torso3_geom,Rtx);
2186  dGeomSetOffsetPosition(torso3_geom,-m2.c[0], 0.029-m2.c[0], -0.034-m2.c[0]);
2187  dMassAdd (&m, &m2);*/
2188 
2189  torso3_geom = dCreateCylinder (subspace,0.04,0.0274);dMassSetCylinderTotal(&m2,0.45165,3,0.04,0.0274);
2190  dGeomNames[torso3_geom] = "torso3_geom";
2191  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2192  dGeomSetBody (torso3_geom,body_torso);
2193  dGeomSetOffsetRotation(torso3_geom,Rtx);
2194  dGeomSetOffsetPosition(torso3_geom,jP_torso[2][1]-jP_torso[1][1]-m2.c[0], jP_torso[2][2]-z_offset-m2.c[0], jP_torso[1][0]-jP_torso[0][0]-m2.c[0]);
2195  dMassAdd (&m, &m2);
2196 
2197  dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
2198  dBodySetMass(body_torso,&m);
2199 
2200  torso[4] = dBodyCreate (world);dMassSetZero(&m);dMassSetBoxTotal(&m,1.8388,0.076,0.118,0.109);
2201  dBodySetMass(torso[4],&m);
2202  torsoGeom[4] = dCreateBox (subspace,fabs(jP_leftArm[1][1]-jP_torso[2][1])-0.011-0.5*0.059/*clavicule height + half shoulder height*/,0.118,0.109);dGeomSetBody (torsoGeom[4],torso[4]);
2203  dGeomNames[torsoGeom[4]] = "torsoGeom[4]";
2204 
2205  torso[5] = dBodyCreate (world);dMassSetZero(&m);dMassSetBoxTotal(&m,1.8388,0.076,0.118,0.109);
2206  dBodySetMass(torso[5],&m);
2207  torsoGeom[5] = dCreateBox (subspace,fabs(jP_leftArm[1][1]-jP_torso[2][1])-0.011-0.5*0.059/*clavicule height + half shoulder height*/,0.118,0.109);dGeomSetBody (torsoGeom[5],torso[5]);
2208  dGeomNames[torsoGeom[5]] = "torsoGeom[5]";
2209 
2210 }
2211 
2212 void ICubSim::initTorsoOn(dWorldID world, dSpaceID subspace)
2213 {
2214  dMass m;
2215  dQuaternion q, q1;
2216  dQFromAxisAndAngle(q,1,0,0, M_PI * 0.5);
2217  dQFromAxisAndAngle(q1,0,1,0,M_PI * 0.5);
2218 
2219  //TORSO CREATION
2220  torso[0] = dBodyCreate (world);dMassSetZero(&m);dMassSetBoxTotal(&m,0.20297,0.004,0.13,0.054); //Y, Z, X
2221  dBodySetMass(torso[0],&m);
2222  torsoGeom[0] = dCreateBox (subspace,0.0470,fabs((jP_torso[0][2]-0.031/*torso pitch cyl rad*/)-(jP_leftLeg[5][2]-0.031/*leg cyl rad*/)),0.064);dGeomSetBody (torsoGeom[0],torso[0]);
2223  dGeomNames[torsoGeom[0]] = "torsoGeom[0]";
2224 
2225  torso[1] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.91179,3,0.031,fabs(jP_leftLeg[3][1]-jP_rightLeg[3][1]));//dMassSetBoxTotal(&m,3.623,0.176,0.063,0.127); //Y, Z, X
2226  dBodySetMass(torso[1],&m);
2227  dBodySetQuaternion(torso[1],q1);
2228  torsoGeom[1] = dCreateCylinder (subspace,0.031,fabs(jP_leftLeg[3][1]-jP_rightLeg[3][1]));dGeomSetBody (torsoGeom[1],torso[1]);//dCreateBox (subspace,0.176,0.063,0.127);
2229  dGeomNames[torsoGeom[1]] = "torsoGeom[1]";
2230 
2231  torso[2] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.91179,3,0.031,0.097);
2232  dBodySetMass(torso[2],&m);
2233  torsoGeom[2] = dCreateCylinder (subspace,0.031,0.097);dGeomSetBody(torsoGeom[2],torso[2]);
2234  dGeomNames[torsoGeom[2]] = "torsoGeom[2]";
2235 
2236  torso[3] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.45165,3,0.04,0.0274);
2237  dBodySetMass(torso[3],&m);
2238  dBodySetQuaternion(torso[3],q);
2239  torsoGeom[3] = dCreateCylinder (subspace,0.04,0.0274);dGeomSetBody(torsoGeom[3],torso[3]);
2240  dGeomNames[torsoGeom[3]] = "torsoGeom[3]";
2241 
2242  torso[4] = dBodyCreate (world);dMassSetZero(&m);dMassSetBoxTotal(&m,1.8388,fabs(jP_leftArm[1][1]-jP_torso[2][1])-0.011-0.5*0.059/*clavicule height + half shoulder height*/,fabs((jP_head[0][2]-0.015/*head pitch cyl rad*/)-(jP_torso[2][2]+0.031+0.0274/*torso roll cyl rad + torso yaw cyl height*/)),0.109);
2243  dBodySetMass(torso[4],&m);
2244  torsoGeom[4] = dCreateBox (subspace,fabs(jP_leftArm[1][1]-jP_torso[2][1])-0.011-0.5*0.059,fabs((jP_head[0][2]-0.015)-(jP_torso[2][2]+0.031+0.0274)),0.109);
2245  dGeomSetBody (torsoGeom[4],torso[4]);
2246  dGeomNames[torsoGeom[4]] = "torsoGeom[4]";
2247 
2248  torso[5] = dBodyCreate (world);dMassSetZero(&m);dMassSetBoxTotal(&m,1.8388,fabs(jP_leftArm[1][1]-jP_torso[2][1])-0.011-0.5*0.059/*clavicule height + half shoulder height*/,fabs((jP_head[0][2]-0.015/*head pitch cyl rad*/)-(jP_torso[2][2]+0.031+0.0274/*torso roll cyl rad + torso yaw cyl height*/)),0.109);
2249  dBodySetMass(torso[5],&m);
2250  torsoGeom[5] = dCreateBox (subspace,fabs(jP_leftArm[1][1]-jP_torso[2][1])-0.011-0.5*0.059/*clavicule height + half shoulder height*/,fabs((jP_head[0][2]-0.015/*head pitch cyl rad*/)-(jP_torso[2][2]+0.031+0.0274/*torso roll cyl rad + torso yaw cyl height*/)),0.109);dGeomSetBody (torsoGeom[5],torso[5]);
2251  dGeomNames[torsoGeom[5]] = "torsoGeom[5]";
2252 
2253 }
2254 
2255 void ICubSim::initLeftArmOff(dWorldID world, dSpaceID subspace)
2256 {
2257  dMass m, m2;
2258  dMatrix3 Rtx;
2259 
2260  double offset[3];
2261  offset[0]=jP_leftArm[1][0];
2262  offset[1]=jP_leftArm[1][1]-0.5*(0.011+0.059) /*half clavicule height + half shoulder height*/;
2263  offset[2]=jP_leftArm[1][2];
2264 
2265  larm = dBodyCreate (world);dMassSetZero(&m);dMassSetZero(&m2);
2266  larm0_geom = dCreateCylinder (subspace,0.031,0.011);dMassSetCylinderTotal(&m2,0.48278,3,0.031,0.011);
2267  dGeomNames[larm0_geom] = "larm0_geom";
2268 
2269  dRFromAxisAndAngle(Rtx,0,1,0,M_PI * 0.5);
2270  dGeomSetBody (larm0_geom,larm);
2271  dGeomSetOffsetRotation(larm0_geom,Rtx);
2272  dGeomSetOffsetPosition(larm0_geom,-m2.c[0], -m2.c[0], -m2.c[0]);
2273  dMassAdd (&m, &m2);
2274 
2275  larm1_geom = dCreateCylinder (subspace,0.03,0.059);dMassSetCylinderTotal(&m2,0.20779,3,0.03,0.059);
2276  dGeomNames[larm1_geom] = "larm1_geom";
2277  dGeomSetBody (larm1_geom,larm);
2278  dGeomSetOffsetPosition(larm1_geom,jP_leftArm[2][1]-offset[1]-m2.c[0], jP_leftArm[2][2]-offset[2]-m2.c[0], -m2.c[0]);
2279  dMassAdd (&m, &m2);
2280 
2281  larm2_geom = dCreateCylinder (subspace, 0.026 ,fabs(jP_leftArm[4][2]-jP_leftArm[2][2]));dMassSetCylinderTotal(&m2,1.1584,3,0.026,fabs(jP_leftArm[4][2]-jP_leftArm[2][2]));
2282  dGeomNames[larm2_geom] = "larm2_geom";
2283  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2284  dGeomSetBody (larm2_geom,larm);
2285  dGeomSetOffsetRotation(larm2_geom,Rtx);
2286  dGeomSetOffsetPosition(larm2_geom,jP_leftArm[4][1]-offset[1]-m2.c[0], 0.5*(jP_leftArm[4][2]+jP_leftArm[2][2])-offset[2]-m2.c[0], -m2.c[0]);
2287  dMassAdd (&m, &m2);
2288 
2289  larm3_geom = dCreateCylinder (subspace, 0.02 ,fabs(jP_leftArm[5][2]-jP_leftArm[3][2]));dMassSetCylinderTotal(&m2,0.48774,3,0.02,fabs(jP_leftArm[5][2]-jP_leftArm[3][2]));
2290  dGeomNames[larm3_geom] = "larm3_geom";
2291  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2292  dGeomSetBody (larm3_geom,larm);
2293  dGeomSetOffsetRotation(larm3_geom,Rtx);
2294  dGeomSetOffsetPosition(larm3_geom,jP_leftArm[5][1]-offset[1]-m2.c[0], 0.5*(jP_leftArm[5][2]+jP_leftArm[3][2])-offset[2]-m2.c[0], -m2.c[0]);
2295  dMassAdd (&m, &m2);
2296 
2297  dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
2298  dBodySetMass(larm,&m);
2299 }
2300 
2301 void ICubSim::initLeftArmOn(dWorldID world, dSpaceID subspace)
2302 {
2303  dMass m, m2;
2304  //rotation matrises
2305  dQuaternion q, q1;
2306  dQFromAxisAndAngle(q,1,0,0, M_PI * 0.5);
2307  dQFromAxisAndAngle(q1,0,1,0,M_PI * 0.5);
2308 
2310  body[0] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.48278,3,0.031,0.011);
2311  dBodySetMass(body[0],&m);
2312  dBodySetQuaternion(body[0],q1);
2313  geom[0] = dCreateCylinder (subspace,0.031,0.011);dGeomSetBody(geom[0],body[0]);
2314  dGeomNames[geom[0]] = "geom[0]";
2315 
2316  body[2] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.20779,3,0.03,0.059);
2317  dBodySetMass(body[2],&m);
2318  geom[2] = dCreateCylinder (subspace,0.03,0.059);dGeomSetBody(geom[2],body[2]);
2319  dGeomNames[geom[2]] = "geom[2]";
2320 
2321  body[4] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,1.1584,3,0.026,fabs(jP_leftArm[4][2]-jP_leftArm[2][2]));
2322  dBodySetMass(body[4],&m);
2323  dBodySetQuaternion(body[4],q);
2324  geom[4] = dCreateCylinder(subspace, 0.026 ,fabs(jP_leftArm[4][2]-jP_leftArm[2][2]));dGeomSetBody(geom[4],body[4]);
2325  dGeomNames[geom[4]] = "geom[4]";
2326 
2327  body[6] = dBodyCreate (world);dMassSetZero(&m);dMassSetSphereTotal(&m,0.050798,0.01);
2328  dBodySetMass(body[6],&m);
2329  geom[6] = dCreateSphere(subspace,0.01) ;dGeomSetBody(geom[6],body[6]);
2330  dGeomNames[geom[6]] = "geom[6]";
2331  //left lower arm
2332  body[8] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.48774,3,0.02,fabs(jP_leftArm[5][2]-jP_leftArm[3][2]));
2333  dBodySetMass(body[8],&m);
2334  dBodySetQuaternion(body[8],q);
2335  geom[8] = dCreateCylinder(subspace, 0.02 ,fabs(jP_leftArm[5][2]-jP_leftArm[3][2]));dGeomSetBody(geom[8],body[8]);
2336  dGeomNames[geom[8]] = "geom[8]";
2337 }
2338 
2339 void ICubSim::initRightArmOff(dWorldID world, dSpaceID subspace)
2340 {
2341  dMass m, m2;
2342  dMatrix3 Rtx;
2343 
2344  double offset[3];
2345  offset[0]=jP_rightArm[1][0];
2346  offset[1]=jP_rightArm[1][1]+0.5*(0.011+0.059) /*half clavicule height + half shoulder height*/;
2347  offset[2]=jP_rightArm[1][2];
2348 
2349  rarm = dBodyCreate (world);dMassSetZero(&m);dMassSetZero(&m2);
2350  rarm0_geom = dCreateCylinder (subspace,0.031,0.011);dMassSetCylinderTotal(&m,0.48278,3,0.031,0.011);
2351  dGeomNames[rarm0_geom] = "rarm0_geom";
2352  dRFromAxisAndAngle(Rtx,0,1,0,M_PI * 0.5);
2353  dGeomSetBody (rarm0_geom,rarm);
2354  dGeomSetOffsetRotation(rarm0_geom,Rtx);
2355  dGeomSetOffsetPosition(rarm0_geom,-m2.c[0], -m2.c[0], -m2.c[0]);
2356  dMassAdd (&m, &m2);
2357 
2358  rarm1_geom = dCreateCylinder (subspace,0.03,0.059);dMassSetCylinderTotal(&m,0.20779,3,0.03,0.059);
2359  dGeomNames[rarm1_geom] = "rarm1_geom";
2360  dGeomSetBody (rarm1_geom,rarm);
2361  dGeomSetOffsetPosition(rarm1_geom,jP_rightArm[2][1]-offset[1]-m2.c[0], jP_rightArm[2][2]-offset[2]-m2.c[0], -m2.c[0]);
2362  dMassAdd (&m, &m2);
2363 
2364  rarm2_geom = dCreateCylinder (subspace, 0.026 ,fabs(jP_leftArm[4][2]-jP_leftArm[2][2]));dMassSetCylinderTotal(&m,1.1584,3,0.026,fabs(jP_rightArm[4][2]-jP_rightArm[2][2]));
2365  dGeomNames[rarm2_geom] = "rarm2_geom";
2366  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2367  dGeomSetBody (rarm2_geom,rarm);
2368  dGeomSetOffsetRotation(rarm2_geom,Rtx);
2369  dGeomSetOffsetPosition(rarm2_geom,jP_rightArm[4][1]-offset[1]-m2.c[0], 0.5*(jP_rightArm[4][2]+jP_rightArm[2][2])-offset[2]-m2.c[0], -m2.c[0]);
2370  dMassAdd (&m, &m2);
2371 
2372  rarm3_geom = dCreateCylinder (subspace, 0.02 ,fabs(jP_rightArm[5][2]-jP_rightArm[3][2]));dMassSetCylinderTotal(&m,0.48774,3,0.02,fabs(jP_rightArm[5][2]-jP_rightArm[3][2]));
2373  dGeomNames[rarm3_geom] = "rarm3_geom";
2374  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2375  dGeomSetBody (rarm3_geom,rarm);
2376  dGeomSetOffsetRotation(rarm3_geom,Rtx);
2377  dGeomSetOffsetPosition(rarm3_geom,jP_rightArm[5][1]-offset[1]-m2.c[0], 0.5*(jP_rightArm[5][2]+jP_rightArm[3][2])-offset[2]-m2.c[0], -m2.c[0]);
2378  dMassAdd (&m, &m2);
2379 
2380  dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
2381  dBodySetMass(rarm,&m);
2382 }
2383 
2384 void ICubSim::initRightArmOn(dWorldID world, dSpaceID subspace)
2385 {
2386  dMass m, m2;
2387  //rotation matrises
2388  dQuaternion q, q1;
2389  dQFromAxisAndAngle(q,1,0,0, M_PI * 0.5);
2390  dQFromAxisAndAngle(q1,0,1,0,M_PI * 0.5);
2391 
2393  body[1] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.48278,3,0.031,0.011);
2394  dBodySetMass(body[1],&m);
2395  dBodySetQuaternion(body[1],q1);
2396  geom[1] = dCreateCylinder (subspace,0.031,0.011);dGeomSetBody(geom[1],body[1]);
2397  dGeomNames[geom[1]] = "geom[1]";
2398 
2399  body[3] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.20779,3,0.03,0.059);
2400  dBodySetMass(body[3],&m);
2401  geom[3] = dCreateCylinder (subspace,0.03,0.059);dGeomSetBody(geom[3],body[3]);
2402  dGeomNames[geom[3]] = "geom[3]";
2403 
2404  body[5] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,1.1584,3,0.026,fabs(jP_rightArm[4][2]-jP_rightArm[2][2]));
2405  dBodySetMass(body[5],&m);
2406  dBodySetQuaternion(body[5],q);
2407  geom[5] = dCreateCylinder(subspace, 0.026 ,fabs(jP_rightArm[4][2]-jP_rightArm[2][2]));dGeomSetBody(geom[5],body[5]);
2408  dGeomNames[geom[5]] = "geom[5]";
2409 
2410  body[7] = dBodyCreate (world);dMassSetZero(&m);dMassSetSphereTotal(&m,0.050798,0.01);
2411  dBodySetMass(body[7],&m);
2412  geom[7] = dCreateSphere(subspace,0.01) ;dGeomSetBody(geom[7],body[7]);
2413  dGeomNames[geom[7]] = "geom[7]";
2414  //left lower arm
2415  body[9] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.48774,3,0.02,fabs(jP_rightArm[5][2]-jP_rightArm[3][2]));
2416  dBodySetMass(body[9],&m);
2417  dBodySetQuaternion(body[9],q);
2418  geom[9] = dCreateCylinder(subspace, 0.02 ,fabs(jP_rightArm[5][2]-jP_rightArm[3][2]));dGeomSetBody(geom[9],body[9]);
2419  dGeomNames[geom[9]] = "geom[9]";
2420 
2421 }
2422 
2423 void ICubSim::initLeftHandOff(dWorldID world, dSpaceID subspace)
2424 {
2425  dMass m, m2;
2426  dMatrix3 Rtx;
2427 
2428  //palm
2429  l_hand = dBodyCreate (world);dMassSetZero(&m);dMassSetZero(&m2);
2430  l_hand0_geom = dCreateBox (subspace,0.022,0.069,0.065);dMassSetBoxTotal(&m,0.19099,0.024,0.069,0.065);
2431  dGeomNames[l_hand0_geom] = "l_hand0_geom";
2432  dGeomSetBody (l_hand0_geom,l_hand);
2433  dGeomSetOffsetPosition(l_hand0_geom,-m2.c[0], -m2.c[0], -m2.c[0]);
2434  dMassAdd (&m, &m2);
2435 
2436  //index
2437  l_hand1_geom = dCreateCylinder (subspace,0.0065,0.08);dMassSetCylinderTotal(&m2,0.030947,3,0.0065,0.08);
2438  dGeomNames[l_hand1_geom] = "l_hand1_geom";
2439  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2440  dGeomSetBody(l_hand1_geom,l_hand);
2441  dGeomSetOffsetRotation(l_hand1_geom,Rtx);
2442  dGeomSetOffsetPosition(l_hand1_geom,-m2.c[0], -0.0745-m2.c[0], 0.02275-m2.c[0]);
2443  dMassAdd (&m, &m2);
2444 
2445  //middle
2446  l_hand2_geom = dCreateCylinder (subspace,0.0065,0.084);dMassSetCylinderTotal(&m2,0.030947,3,0.0065,0.084);
2447  dGeomNames[l_hand2_geom] = "l_hand2_geom";
2448  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2449  dGeomSetBody(l_hand2_geom,l_hand);
2450  dGeomSetOffsetRotation(l_hand2_geom,Rtx);
2451  dGeomSetOffsetPosition(l_hand2_geom,-m2.c[0], -0.0745-m2.c[0], 0.0065-m2.c[0]);
2452  dMassAdd (&m, &m2);
2453 
2454  //ring
2455  l_hand3_geom = dCreateCylinder (subspace,0.0065,0.08);dMassSetCylinderTotal(&m2,0.030947,3,0.0065,0.08);
2456  dGeomNames[l_hand3_geom] = "l_hand3_geom";
2457  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2458  dGeomSetBody(l_hand3_geom,l_hand);
2459  dGeomSetOffsetRotation(l_hand3_geom,Rtx);
2460  dGeomSetOffsetPosition(l_hand3_geom,-m2.c[0], -0.0745-m2.c[0], -0.00975-m2.c[0]);
2461  dMassAdd (&m, &m2);
2462 
2463  //little
2464  l_hand4_geom = dCreateCylinder (subspace,0.0065,0.073);dMassSetCylinderTotal(&m2,0.030947,3,0.0065,0.073);
2465  dGeomNames[l_hand4_geom] = "l_hand4_geom";
2466  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2467  dGeomSetBody(l_hand4_geom,l_hand);
2468  dGeomSetOffsetRotation(l_hand4_geom,Rtx);
2469  dGeomSetOffsetPosition(l_hand4_geom,-m2.c[0], -0.071-m2.c[0], -0.026-m2.c[0]);
2470  dMassAdd (&m, &m2);
2471 
2472  //thumb
2473  l_hand5_geom = dCreateCylinder (subspace,0.0065,0.064);dMassSetCylinderTotal(&m2,0.02341,3,0.0065,0.064);
2474  dGeomNames[l_hand5_geom] = "l_hand5_geom";
2475  dGeomSetBody(l_hand5_geom,l_hand);
2476  dGeomSetOffsetPosition(l_hand5_geom,-m2.c[0], 0.016-m2.c[0], 0.0645-m2.c[0]);
2477  dMassAdd (&m, &m2);
2478 
2479  dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
2480  dBodySetMass(l_hand,&m);
2481 }
2482 
2483 void ICubSim::initLeftHandOn(dWorldID world, dSpaceID subspace)
2484 {
2485  dMass m, m2;
2486  dMatrix3 Rtx;
2487  //rotation matrises
2488  dQuaternion q;
2489  dQFromAxisAndAngle(q,1,0,0, M_PI * 0.5);
2490 
2491  //Create all left fingers
2492  //this is the palm
2493  body[10] = dBodyCreate (world);dMassSetZero(&m);dMassSetBoxTotal(&m,0.19099,0.024,0.069,0.065);
2494  dBodySetMass(body[10],&m);
2495  geom[10] = dCreateBox (subspace,0.022,0.069,0.065);dGeomSetBody (geom[10],body[10]);
2496  dGeomNames[geom[10]] = "geom[10]";
2497 
2498  //most proximal part of index finger
2499  body[12] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.002,3,0.0065,0.012);
2500  dBodySetMass(body[12],&m);
2501  dBodySetQuaternion(body[12],q);
2502  geom[12] = dCreateCylinder (subspace,0.0065,0.012);dGeomSetBody(geom[12],body[12]);
2503  dGeomNames[geom[12]] = "geom[12]";
2504 
2505  //most proximal part of middle finger
2506  body[13] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.002,3,0.0065,0.012);
2507  dBodySetMass(body[13],&m);
2508  dBodySetQuaternion(body[13],q);
2509  geom[13] = dCreateCylinder (subspace,0.0065,0.012);dGeomSetBody(geom[13],body[13]);
2510  dGeomNames[geom[13]] = "geom[13]";
2511 
2512  //most proximal part of ring and little finger - they have one body because they move together
2513  lhandfingers0 = dBodyCreate (world);dMassSetZero(&m);dMassSetZero(&m2);//CreateCylinder(0.0065,0.012);
2514  //most proximal part ring finger?
2515  lhandfings0_geom = dCreateCylinder (subspace,0.0065,0.012);dMassSetCylinderTotal(&m2,0.002,3,0.0065,0.012);
2516  dGeomNames[lhandfings0_geom] = "lhandfings0_geom";
2517  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2518  dGeomSetBody (lhandfings0_geom,lhandfingers0);
2519  dGeomSetOffsetRotation(lhandfings0_geom,Rtx);//dGeomSetPosition (rightLeg_1 , 0.0, 1.0, 0.0);
2520  dGeomSetOffsetPosition(lhandfings0_geom,-m2.c[0], -m2.c[0], 0.008125-m2.c[0]);//dMassRotate(&m2,Rtx);
2521  dMassAdd (&m, &m2);
2522  //second object - most proximal part little finger?
2523  lhandfings1_geom = dCreateCylinder (subspace,0.0065,0.012);dMassSetCylinderTotal(&m2,0.002,3,0.0065,0.012);
2524  dGeomNames[lhandfings1_geom] = "lhandfings1_geom";
2525  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2526  dGeomSetRotation (lhandfings1_geom,Rtx);
2527  dGeomSetBody (lhandfings1_geom,lhandfingers0);
2528  dGeomSetOffsetRotation(lhandfings1_geom,Rtx);
2529  dGeomSetOffsetPosition(lhandfings1_geom,-m2.c[0], -m2.c[0], -0.008125-m2.c[0]);//dGeomSetPosition (rightLeg_2 , 0.0, 1, 0.0);
2530  //add mass accumulated
2531  dMassAdd (&m, &m2);
2532  //translate
2533  dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
2534  //Set mass to the actual body
2535  dBodySetMass(lhandfingers0,&m);
2536 
2537  //2nd most proximal bit of index
2538  body[16] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.002,3,0.0065,0.026);
2539  dBodySetMass(body[16],&m);
2540  dBodySetQuaternion(body[16],q);
2541  geom[16] = dCreateCylinder (subspace,0.0065,0.026);dGeomSetBody(geom[16],body[16]);
2542  dGeomNames[geom[16]] = "geom[16]";
2543 
2544  //2nd most proximal bit of middle
2545  body[17] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.002,3,0.0065,0.028);
2546  dBodySetMass(body[17],&m);
2547  dBodySetQuaternion(body[17],q);
2548  geom[17] = dCreateCylinder (subspace,0.0065,0.028);dGeomSetBody(geom[17],body[17]);
2549  dGeomNames[geom[17]] = "geom[17]";
2550 
2551  //2nd most proximal bits of ring and little
2552  lhandfingers1 = dBodyCreate (world);dMassSetZero(&m);dMassSetZero(&m2);//CreateCylinder(0.0065,0.012);
2553  lhandfings2_geom = dCreateCylinder (subspace,0.0065,0.026);dMassSetCylinderTotal(&m2,0.002,3,0.0065,0.026);
2554  dGeomNames[lhandfings2_geom] = "lhandfings2_geom";
2555  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2556  dGeomSetBody (lhandfings2_geom,lhandfingers1);
2557  dGeomSetOffsetRotation(lhandfings2_geom,Rtx);//dGeomSetPosition (rightLeg_1 , 0.0, 1.0, 0.0);
2558  dGeomSetOffsetPosition(lhandfings2_geom,-m2.c[0], -m2.c[0], 0.008125-m2.c[0]);//dMassRotate(&m2,Rtx);
2559  dMassAdd (&m, &m2);
2560  //second object
2561  lhandfings3_geom = dCreateCylinder (subspace,0.0065,0.022);dMassSetCylinderTotal(&m2,0.002,3,0.0065,0.022);
2562  dGeomNames[lhandfings3_geom] = "lhandfings3_geom";
2563  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2564  dGeomSetRotation (lhandfings3_geom,Rtx);
2565  dGeomSetBody (lhandfings3_geom,lhandfingers1);
2566  dGeomSetOffsetRotation(lhandfings3_geom,Rtx);
2567  dGeomSetOffsetPosition(lhandfings3_geom,-m2.c[0], 0.002-m2.c[0], -0.008125-m2.c[0]);//dGeomSetPosition (rightLeg_2 , 0.0, 1, 0.0);
2568  dMassAdd (&m, &m2);
2569  dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);dBodySetMass(lhandfingers1,&m);
2570 
2571  //3rd most proximal bit of index
2572  body[20] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.002,3,0.0065,0.022);dBodySetMass(body[20],&m);
2573  dBodySetQuaternion(body[20],q);geom[20] = dCreateCylinder(subspace, 0.0065,0.022);dGeomSetBody(geom[20],body[20]);
2574  dGeomNames[geom[20]] = "geom[20]";
2575  //3rd most proximal bit of middle
2576  body[21] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.002,3,0.0065,0.024);dBodySetMass(body[21],&m);
2577  dBodySetQuaternion(body[21],q);geom[21] = dCreateCylinder(subspace, 0.0065,0.024);dGeomSetBody(geom[21],body[21]);
2578  dGeomNames[geom[21]] = "geom[21]";
2579 
2580  //3rd most proximal bits of ring and little
2581  lhandfingers2 = dBodyCreate (world);dMassSetZero(&m);dMassSetZero(&m2);//CreateCylinder(0.0065,0.012);
2582  lhandfings4_geom = dCreateCylinder (subspace,0.0065,0.022);dMassSetCylinderTotal(&m2,0.002,3,0.0065,0.022);
2583  dGeomNames[lhandfings4_geom] = "lhandfings4_geom";
2584  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2585  dGeomSetBody (lhandfings4_geom,lhandfingers2);
2586  dGeomSetOffsetRotation(lhandfings4_geom,Rtx);//dGeomSetPosition (rightLeg_1 , 0.0, 1.0, 0.0);
2587  dGeomSetOffsetPosition(lhandfings4_geom,-m2.c[0], -m2.c[0], 0.008125-m2.c[0]);
2588  dMassAdd (&m, &m2);
2589  //second object
2590  lhandfings5_geom = dCreateCylinder (subspace,0.0065,0.019);dMassSetCylinderTotal(&m2,0.002,3,0.0065,0.019);
2591  dGeomNames[lhandfings5_geom] = "lhandfings5_geom";
2592  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2593  dGeomSetRotation (lhandfings5_geom,Rtx);
2594  dGeomSetBody (lhandfings5_geom,lhandfingers2);
2595  dGeomSetOffsetRotation(lhandfings5_geom,Rtx);
2596  dGeomSetOffsetPosition(lhandfings5_geom,-m2.c[0], 0.0055-m2.c[0], -0.008125-m2.c[0]);//dGeomSetPosition (rightLeg_2 , 0.0, 1, 0.0);
2597  dMassAdd (&m, &m2);
2598  dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);dBodySetMass(lhandfingers2,&m);
2599 
2600  //fingertip index
2601  body[24] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.002,3,0.0065,0.02);dBodySetMass(body[24],&m);
2602  dBodySetQuaternion(body[24],q);geom[24] = dCreateCylinder(subspace, 0.0065,0.02);dGeomSetBody(geom[24],body[24]);
2603  dGeomNames[geom[24]] = "geom[24]";
2604  //fingertip middle
2605  body[25] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.002,3,0.0065,0.02);dBodySetMass(body[25],&m);
2606  dBodySetQuaternion(body[25],q);geom[25] = dCreateCylinder(subspace, 0.0065,0.02);dGeomSetBody(geom[25],body[25]);
2607  dGeomNames[geom[25]] = "geom[25]";
2608 
2609  //fingertips ring and little
2610  lhandfingers3 = dBodyCreate (world);dMassSetZero(&m);dMassSetZero(&m2);//CreateCylinder(0.0065,0.012);
2611  lhandfings6_geom = dCreateCylinder (subspace,0.0065,0.02);dMassSetCylinderTotal(&m2,0.002,3,0.0065,0.02);
2612  dGeomNames[lhandfings6_geom] = "lhandfings6_geom";
2613  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2614  dGeomSetBody (lhandfings6_geom,lhandfingers3);
2615  dGeomSetOffsetRotation(lhandfings6_geom,Rtx);//dGeomSetPosition (rightLeg_1 , 0.0, 1.0, 0.0);
2616  dGeomSetOffsetPosition(lhandfings6_geom,-m2.c[0], -m2.c[0], 0.008125-m2.c[0]);
2617  dMassAdd (&m, &m2);
2618  //second object
2619  lhandfings7_geom = dCreateCylinder (subspace,0.0065,0.02);dMassSetCylinderTotal(&m2,0.002,3,0.0065,0.02);
2620  dGeomNames[lhandfings7_geom] = "lhandfings7_geom";
2621  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2622  dGeomSetRotation (lhandfings7_geom,Rtx);
2623  dGeomSetBody (lhandfings7_geom,lhandfingers3);
2624  dGeomSetOffsetRotation(lhandfings7_geom,Rtx);
2625  dGeomSetOffsetPosition(lhandfings7_geom,-m2.c[0], 0.007-m2.c[0], -0.008125-m2.c[0]);//dGeomSetPosition (rightLeg_2 , 0.0, 1, 0.0);
2626  dMassAdd (&m, &m2);
2627  dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);dBodySetMass(lhandfingers3,&m);
2628 
2629  //thumb most proximal
2630  body[28] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.002,3,0.0065,0.026);
2631  dBodySetMass(body[28],&m);//dBodySetQuaternion(body[28],q1);
2632  geom[28] = dCreateCylinder(subspace,0.0065,0.026);dGeomSetBody(geom[28],body[28]);
2633  dGeomNames[geom[28]] = "geom[28]";
2634 
2635  //thumb middle bit
2636  body[29] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.002,3,0.0065,0.022);
2637  dBodySetMass(body[29],&m);//dBodySetQuaternion(body[29],q1);
2638  geom[29] = dCreateCylinder(subspace,0.0065,0.022);dGeomSetBody(geom[29],body[29]);
2639  dGeomNames[geom[29]] = "geom[29]";
2640 
2641  //thumb fingertip
2642  body[30] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.002,3,0.0065,0.016);
2643  dBodySetMass(body[30],&m);//dBodySetQuaternion(body[30],q1);
2644  geom[30] = dCreateCylinder(subspace,0.0065,0.016);dGeomSetBody(geom[30],body[30]);
2645  dGeomNames[geom[30]] = "geom[30]";
2646 }
2647 
2648 void ICubSim::initRightHandOff(dWorldID world, dSpaceID subspace)
2649 {
2650  dMass m, m2;
2651  dMatrix3 Rtx;
2652 
2653  r_hand = dBodyCreate (world);dMassSetZero(&m);dMassSetZero(&m2);
2654  r_hand0_geom = dCreateBox (subspace,0.022,0.069,0.065);dMassSetBoxTotal(&m,0.19099,0.024,0.069,0.065);
2655  dGeomNames[r_hand0_geom] = "r_hand0_geom";
2656  dGeomSetBody (r_hand0_geom,r_hand);
2657  dGeomSetOffsetPosition(r_hand0_geom,-m2.c[0], -m2.c[0], -m2.c[0]);
2658  dMassAdd (&m, &m2);
2659 
2660  r_hand1_geom = dCreateCylinder (subspace,0.0065,0.08);dMassSetCylinderTotal(&m2,0.030947,3,0.0065,0.08);
2661  dGeomNames[r_hand1_geom] = "r_hand1_geom";
2662  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2663  dGeomSetBody(r_hand1_geom,r_hand);
2664  dGeomSetOffsetRotation(r_hand1_geom,Rtx);
2665  dGeomSetOffsetPosition(r_hand1_geom,-m2.c[0], -0.0745-m2.c[0], 0.02275-m2.c[0]);
2666  dMassAdd (&m, &m2);
2667 
2668  r_hand2_geom = dCreateCylinder (subspace,0.0065,0.084);dMassSetCylinderTotal(&m2,0.030947,3,0.0065,0.084);
2669  dGeomNames[r_hand2_geom] = "r_hand2_geom";
2670  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2671  dGeomSetBody(r_hand2_geom,r_hand);
2672  dGeomSetOffsetRotation(r_hand2_geom,Rtx);
2673  dGeomSetOffsetPosition(r_hand2_geom,-m2.c[0], -0.0745-m2.c[0], 0.0065-m2.c[0]);
2674  dMassAdd (&m, &m2);
2675 
2676  r_hand3_geom = dCreateCylinder (subspace,0.0065,0.08);dMassSetCylinderTotal(&m2,0.030947,3,0.0065,0.08);
2677  dGeomNames[r_hand3_geom] = "r_hand3_geom";
2678  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2679  dGeomSetBody(r_hand3_geom,r_hand);
2680  dGeomSetOffsetRotation(r_hand3_geom,Rtx);
2681  dGeomSetOffsetPosition(r_hand3_geom,-m2.c[0], -0.0745-m2.c[0], -0.00975-m2.c[0]);
2682  dMassAdd (&m, &m2);
2683 
2684  r_hand4_geom = dCreateCylinder (subspace,0.0065,0.073);dMassSetCylinderTotal(&m2,0.030947,3,0.0065,0.073);
2685  dGeomNames[r_hand4_geom] = "r_hand4_geom";
2686  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2687  dGeomSetBody(r_hand4_geom,r_hand);
2688  dGeomSetOffsetRotation(r_hand4_geom,Rtx);
2689  dGeomSetOffsetPosition(r_hand4_geom,-m2.c[0], -0.071-m2.c[0], -0.026-m2.c[0]);
2690  dMassAdd (&m, &m2);
2691 
2692  r_hand5_geom = dCreateCylinder (subspace,0.0065,0.064);dMassSetCylinderTotal(&m2,0.02341,3,0.0065,0.064);
2693  dGeomNames[r_hand5_geom] = "r_hand5_geom";
2694  dGeomSetBody(r_hand5_geom,r_hand);
2695  dGeomSetOffsetPosition(r_hand5_geom,-m2.c[0], 0.016-m2.c[0], 0.0645-m2.c[0]);
2696  dMassAdd (&m, &m2);
2697 
2698  dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
2699  dBodySetMass(r_hand,&m);
2700 }
2701 
2702 void ICubSim::initRightHandOn(dWorldID world, dSpaceID subspace)
2703 {
2704  dMass m, m2;
2705  dMatrix3 Rtx;
2706  //rotation matrises
2707  dQuaternion q;
2708  dQFromAxisAndAngle(q,1,0,0, M_PI * 0.5);
2709 
2710  //Create all right fingers
2711  //palm
2712  body[11] = dBodyCreate (world);dMassSetZero(&m);dMassSetBoxTotal(&m,0.19099,0.024,0.069,0.065);
2713  dBodySetMass(body[11],&m);
2714  geom[11] = dCreateBox (subspace,0.022,0.069,0.065);dGeomSetBody (geom[11],body[11]);
2715  dGeomNames[geom[11]] = "geom[11]";
2716 
2717  //most proximal part of index finger
2718  body[31] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.002,3,0.0065,0.012);
2719  dBodySetMass(body[31],&m);
2720  dBodySetQuaternion(body[31],q);
2721  geom[31] = dCreateCylinder (subspace,0.0065,0.012);dGeomSetBody(geom[31],body[31]);
2722  dGeomNames[geom[31]] = "geom[31]";
2723 
2724  //most proximal part of middle finger
2725  body[32] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.002,3,0.0065,0.012);
2726  dBodySetMass(body[32],&m);
2727  dBodySetQuaternion(body[32],q);
2728  geom[32] = dCreateCylinder (subspace,0.0065,0.012);dGeomSetBody(geom[32],body[32]);
2729  dGeomNames[geom[32]] = "geom[32]";
2730 
2731  //most proximal body for rign and little finger
2732  rhandfingers0 = dBodyCreate (world);dMassSetZero(&m);dMassSetZero(&m2);//CreateCylinder(0.0065,0.012);
2733  //probably most proximal bit of ring finger geom
2734  rhandfings0_geom = dCreateCylinder (subspace,0.0065,0.012);dMassSetCylinderTotal(&m2,0.002,3,0.0065,0.012);
2735  dGeomNames[rhandfings0_geom] = "rhandfings0_geom";
2736  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2737  dGeomSetBody (rhandfings0_geom,rhandfingers0);
2738  dGeomSetOffsetRotation(rhandfings0_geom,Rtx);//dGeomSetPosition (rightLeg_1 , 0.0, 1.0, 0.0);
2739  dGeomSetOffsetPosition(rhandfings0_geom,-m2.c[0], -m2.c[0], 0.008125-m2.c[0]);//dMassRotate(&m2,Rtx);
2740  dMassAdd (&m, &m2);
2741  //second object - probably most proximal bit of little finger geom
2742  rhandfings1_geom = dCreateCylinder (subspace,0.0065,0.012);dMassSetCylinderTotal(&m2,0.002,3,0.0065,0.012);
2743  dGeomNames[rhandfings1_geom] = "rhandfings1_geom";
2744  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2745  dGeomSetRotation (rhandfings1_geom,Rtx);
2746  dGeomSetBody (rhandfings1_geom,rhandfingers0);
2747  dGeomSetOffsetRotation(rhandfings1_geom,Rtx);
2748  dGeomSetOffsetPosition(rhandfings1_geom,-m2.c[0], -m2.c[0], -0.008125-m2.c[0]);//dGeomSetPosition (rightLeg_2 , 0.0, 1, 0.0);
2749  //add mass accumulated
2750  dMassAdd (&m, &m2);
2751  //translate
2752  dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
2753  //Set mass to the actual body
2754  dBodySetMass(rhandfingers0,&m);
2755 
2756  //2nd most proximal bit of index
2757  body[35] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.002,3,0.0065,0.026);
2758  dBodySetMass(body[35],&m);
2759  dBodySetQuaternion(body[35],q);
2760  geom[35] = dCreateCylinder (subspace,0.0065,0.026);dGeomSetBody(geom[35],body[35]);
2761  dGeomNames[geom[35]] = "geom[35]";
2762 
2763  //2nd most proximal bit of middle
2764  body[36] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.002,3,0.0065,0.028);
2765  dBodySetMass(body[36],&m);
2766  dBodySetQuaternion(body[36],q);
2767  geom[36] = dCreateCylinder (subspace,0.0065,0.028);dGeomSetBody(geom[36],body[36]);
2768  dGeomNames[geom[36]] = "geom[36]";
2769 
2770  //2nd most proximal bits of ring and little
2771  rhandfingers1 = dBodyCreate (world);dMassSetZero(&m);dMassSetZero(&m2);//CreateCylinder(0.0065,0.012);
2772  rhandfings2_geom = dCreateCylinder (subspace,0.0065,0.026);dMassSetCylinderTotal(&m2,0.002,3,0.0065,0.026);
2773  dGeomNames[rhandfings2_geom] = "rhandfings2_geom";
2774  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2775  dGeomSetBody (rhandfings2_geom,rhandfingers1);
2776  dGeomSetOffsetRotation(rhandfings2_geom,Rtx);//dGeomSetPosition (rightLeg_1 , 0.0, 1.0, 0.0);
2777  dGeomSetOffsetPosition(rhandfings2_geom,-m2.c[0], -m2.c[0], 0.008125-m2.c[0]);//dMassRotate(&m2,Rtx);
2778  dMassAdd (&m, &m2);
2779  //second object
2780  rhandfings3_geom = dCreateCylinder (subspace,0.0065,0.022);dMassSetCylinderTotal(&m2,0.002,3,0.0065,0.022);
2781  dGeomNames[rhandfings3_geom] = "rhandfings3_geom";
2782  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2783  dGeomSetRotation (rhandfings3_geom,Rtx);
2784  dGeomSetBody (rhandfings3_geom,rhandfingers1);
2785  dGeomSetOffsetRotation(rhandfings3_geom,Rtx);
2786  dGeomSetOffsetPosition(rhandfings3_geom,-m2.c[0], 0.002-m2.c[0], -0.008125-m2.c[0]);//dGeomSetPosition (rightLeg_2 , 0.0, 1, 0.0);
2787  dMassAdd (&m, &m2);
2788  dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);dBodySetMass(rhandfingers1,&m);
2789 
2790  //3rd most proximal bit of index
2791  body[39] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.002,3,0.0065,0.022);dBodySetMass(body[39],&m);
2792  dBodySetQuaternion(body[39],q);geom[39] = dCreateCylinder(subspace, 0.0065,0.022);dGeomSetBody(geom[39],body[39]);
2793  dGeomNames[geom[39]] = "geom[39]";
2794  //3rd most proximal bit of middle
2795  body[40] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.002,3,0.0065,0.024);dBodySetMass(body[40],&m);
2796  dBodySetQuaternion(body[40],q);geom[40] = dCreateCylinder(subspace, 0.0065,0.024);dGeomSetBody(geom[40],body[40]);
2797  dGeomNames[geom[40]] = "geom[40]";
2798 
2799  //3rd most proximal bits of ring and little
2800  rhandfingers2 = dBodyCreate (world);dMassSetZero(&m);dMassSetZero(&m2);//CreateCylinder(0.0065,0.012);
2801  rhandfings4_geom = dCreateCylinder (subspace,0.0065,0.022);dMassSetCylinderTotal(&m2,0.002,3,0.0065,0.022);
2802  dGeomNames[rhandfings4_geom] = "rhandfings4_geom";
2803  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2804  dGeomSetBody (rhandfings4_geom,rhandfingers2);
2805  dGeomSetOffsetRotation(rhandfings4_geom,Rtx);//dGeomSetPosition (rightLeg_1 , 0.0, 1.0, 0.0);
2806  dGeomSetOffsetPosition(rhandfings4_geom,-m2.c[0], -m2.c[0], 0.008125-m2.c[0]);
2807  dMassAdd (&m, &m2);
2808  //second object
2809  rhandfings5_geom = dCreateCylinder (subspace,0.0065,0.019);dMassSetCylinderTotal(&m2,0.002,3,0.0065,0.019);
2810  dGeomNames[rhandfings5_geom] = "rhandfings5_geom";
2811  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2812  dGeomSetRotation (rhandfings5_geom,Rtx);
2813  dGeomSetBody (rhandfings5_geom,rhandfingers2);
2814  dGeomSetOffsetRotation(rhandfings5_geom,Rtx);
2815  dGeomSetOffsetPosition(rhandfings5_geom,-m2.c[0], 0.0055-m2.c[0], -0.008125-m2.c[0]);//dGeomSetPosition (rightLeg_2 , 0.0, 1, 0.0);
2816  dMassAdd (&m, &m2);
2817  dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);dBodySetMass(rhandfingers2,&m);
2818 
2819  //fingertip index
2820  body[43] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.002,3,0.0065,0.02);dBodySetMass(body[43],&m);
2821  dBodySetQuaternion(body[43],q);geom[43] = dCreateCylinder(subspace, 0.0065,0.02);dGeomSetBody(geom[43],body[43]);
2822  dGeomNames[geom[43]] = "geom[43]";
2823  //fingertip middle
2824  body[44] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.002,3,0.0065,0.02);dBodySetMass(body[44],&m);
2825  dBodySetQuaternion(body[44],q);geom[44] = dCreateCylinder(subspace, 0.0065,0.02);dGeomSetBody(geom[44],body[44]);
2826  dGeomNames[geom[44]] = "geom[44]";
2827 
2828  //fingertips ring and little
2829  rhandfingers3 = dBodyCreate (world);dMassSetZero(&m);dMassSetZero(&m2);//CreateCylinder(0.0065,0.012);
2830  rhandfings6_geom = dCreateCylinder (subspace,0.0065,0.02);dMassSetCylinderTotal(&m2,0.002,3,0.0065,0.02);
2831  dGeomNames[rhandfings6_geom] = "rhandfings6_geom";
2832  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2833  dGeomSetBody (rhandfings6_geom,rhandfingers3);
2834  dGeomSetOffsetRotation(rhandfings6_geom,Rtx);//dGeomSetPosition (rightLeg_1 , 0.0, 1.0, 0.0);
2835  dGeomSetOffsetPosition(rhandfings6_geom,-m2.c[0], -m2.c[0], 0.008125-m2.c[0]);
2836  dMassAdd (&m, &m2);
2837  //second object
2838  rhandfings7_geom = dCreateCylinder (subspace,0.0065,0.02);dMassSetCylinderTotal(&m2,0.002,3,0.0065,0.02);
2839  dGeomNames[rhandfings7_geom] = "rhandfings7_geom";
2840  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2841  dGeomSetRotation (rhandfings7_geom,Rtx);
2842  dGeomSetBody (rhandfings7_geom,rhandfingers3);
2843  dGeomSetOffsetRotation(rhandfings7_geom,Rtx);
2844  dGeomSetOffsetPosition(rhandfings7_geom,-m2.c[0], 0.007-m2.c[0], -0.008125-m2.c[0]);//dGeomSetPosition (rightLeg_2 , 0.0, 1, 0.0);
2845  dMassAdd (&m, &m2);
2846  dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);dBodySetMass(rhandfingers3,&m);
2847 
2848  //most proximal bit of thumb
2849  body[47] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.002,3,0.0065,0.026);
2850  dBodySetMass(body[47],&m);//dBodySetQuaternion(body[28],q1);
2851  geom[47] = dCreateCylinder(subspace,0.0065,0.026);dGeomSetBody(geom[47],body[47]);
2852  dGeomNames[geom[47]] = "geom[47]";
2853 
2854  //2nd most proximal bit of thumb
2855  body[48] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.002,3,0.0065,0.022);
2856  dBodySetMass(body[48],&m);//dBodySetQuaternion(body[29],q1);
2857  geom[48] = dCreateCylinder(subspace,0.0065,0.022);dGeomSetBody(geom[48],body[48]);
2858  dGeomNames[geom[48]] = "geom[48]";
2859 
2860  //fingertip thumb
2861  body[49] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.002,3,0.0065,0.016);
2862  dBodySetMass(body[49],&m);//dBodySetQuaternion(body[30],q1);
2863  geom[49] = dCreateCylinder(subspace,0.0065,0.016);dGeomSetBody(geom[49],body[49]);
2864  dGeomNames[geom[49]] = "geom[49]";
2865 }
2866 
2867 void ICubSim::initHead(dWorldID world, dSpaceID subspace)
2868 {
2869  dMass m, m2;
2870  dMatrix3 Rtx;
2871  //rotation matrises
2872  dQuaternion q1;
2873  dQFromAxisAndAngle(q1,0,1,0,M_PI * 0.5);
2874 
2875  head = dBodyCreate (world);
2876  if (actHead == "off")
2877  {
2878  dMassSetZero(&m);dMassSetZero(&m2);
2879  neck0_geom = dCreateCylinder (subspace,0.015,0.077);dMassSetCylinderTotal(&m2,0.28252,3,0.015,0.077);
2880  dGeomNames[neck0_geom] = "neck0_geom";
2881  dRFromAxisAndAngle(Rtx,0,1,0,M_PI * 0.5);
2882  dGeomSetBody (neck0_geom,head);
2883  dGeomSetOffsetRotation(neck0_geom,Rtx);//dGeomSetPosition (rightLeg_1 , 0.0, 1.0, 0.0);
2884  dGeomSetOffsetPosition(neck0_geom, -fabs(jP_head[2][1]-jP_head[0][1])-m2.c[0], -fabs(jP_head[2][2]-jP_head[0][2])-0.03-m2.c[0], -fabs(jP_head[2][0]-jP_head[0][0])-m2.c[0]);
2885  dMassAdd (&m, &m2);
2886 
2887  neck1_geom = dCreateCylinder (subspace,0.015,0.077);dMassSetCylinderTotal(&m2,0.1,3,0.015,0.077);
2888  dGeomNames[neck1_geom] = "neck1_geom";
2889  dRFromAxisAndAngle(Rtx,0,0,1,M_PI * 0.5);
2890  dGeomSetBody (neck1_geom,head);
2891  dGeomSetOffsetRotation(neck1_geom,Rtx);//dGeomSetPosition (rightLeg_1 , 0.0, 1.0, 0.0);
2892  dGeomSetOffsetPosition(neck1_geom, -fabs(jP_head[2][1]-jP_head[1][1])-m2.c[0], -fabs(jP_head[2][2]-jP_head[1][2])-0.03-m2.c[0], -fabs(jP_head[2][0]-jP_head[1][0])-m2.c[0]);
2893  dMassAdd (&m, &m2);
2894  dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);dBodySetMass(head,&m);
2895 
2896  }else{
2897  neck[0] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.28252,3,0.015,0.077);
2898  dBodySetMass(neck[0],&m);dBodySetQuaternion(neck[0],q1);
2899  neckgeom[0] = dCreateCylinder(subspace, 0.015,0.077);dGeomSetBody(neckgeom[0],neck[0]);
2900  dGeomNames[neckgeom[0]] = "neckgeom[0]";
2901 
2902  neck[1] = dBodyCreate (world);dMassSetZero(&m);dMassSetCylinderTotal(&m,0.1,3,0.015,0.077);
2903  dBodySetMass(neck[1],&m);
2904  neckgeom[1] = dCreateCylinder(subspace, 0.015,0.077);dGeomSetBody(neckgeom[1],neck[1]);
2905  dGeomNames[neckgeom[1]] = "neckgeom[1]";
2906 
2907  eye = dBodyCreate (world);//left eye
2908  leye = dBodyCreate (world);//left eye
2909  reye = dBodyCreate (world);//right eye
2910  }
2911  topEyeLid = dBodyCreate (world);//top eye lid
2912  bottomEyeLid = dBodyCreate (world);//bottom eye lid
2913  dBodySetGravityMode(topEyeLid,0);
2914  dBodySetGravityMode(bottomEyeLid,0);
2915 
2916  dMassSetZero(&m);dMassSetZero(&m2);
2917  //Head encapsulated object to save computation on the joints....
2918 
2919  //head yaw cyl
2920  head0_geom = dCreateCylinder (subspace,0.015,0.06);dMassSetCylinderTotal (&m2,0.13913,3,0.015,0.06);
2921  dGeomNames[head0_geom] = "head0_geom";
2922  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
2923  dGeomSetBody (head0_geom,head);
2924  dGeomSetOffsetRotation(head0_geom,Rtx);//dGeomSetPosition (rightLeg_1 , 0.0, 1.0, 0.0);
2925  dGeomSetOffsetPosition(head0_geom,-m2.c[0], -m2.c[0], -m2.c[0]);
2926  dMassAdd (&m, &m2);
2927 
2928 
2929  //larger horizontal plate
2930  head1_geom = dCreateBox (subspace,0.104, 0.002,0.052);dMassSetBoxTotal (&m2,0.1562,0.104,0.002,0.052);
2931  dGeomNames[head1_geom] = "head1_geom";
2932  dGeomSetBody (head1_geom,head);
2933  dGeomSetOffsetPosition(head1_geom,-m2.c[0], /*-0.011-*/0.03/*half neck yaw*/-m2.c[0], -0.0125-m2.c[0]);
2934  dMassAdd (&m, &m2);
2935 
2936  //left vertical plate
2937  head2_geom = dCreateBox (subspace,0.002, 0.093,0.052);dMassSetBoxTotal (&m2,0.1562,0.002, 0.093,0.052);
2938  dGeomNames[head2_geom] = "head2_geom";
2939  dGeomSetBody (head2_geom,head);
2940  dGeomSetOffsetPosition(head2_geom ,0.052-m2.c[0], /*0.0355-*/0.03/*half neck yaw*/-m2.c[0], -0.0125-m2.c[0]);
2941  dMassAdd (&m, &m2);
2942 
2943  //right vertical plate
2944  head3_geom = dCreateBox (subspace,0.002, 0.093,0.052);dMassSetBoxTotal (&m2,0.1562,0.002, 0.093,0.032);
2945  dGeomNames[head3_geom] = "head3_geom";
2946  dGeomSetBody (head3_geom,head);
2947  dGeomSetOffsetPosition(head3_geom,-0.052-m2.c[0], /*0.0355-*/0.03/*half neck yaw*/-m2.c[0], -0.0125-m2.c[0]);
2948  dMassAdd (&m, &m2);
2949 
2950  //smaller horizontal plate (hidden)
2951  head4_geom = dCreateBox (subspace, 0.104, 0.002,0.032);dMassSetBoxTotal (&m2,0.01,0.104, 0.002,0.032);
2952  dGeomNames[head4_geom] = "head4_geom";
2953  dGeomSetBody (head4_geom,head);
2954  dGeomSetOffsetPosition(head4_geom,-m2.c[0], /*0.0355-*/0.03/*half neck yaw*/-m2.c[0], -0.0125-m2.c[0]);
2955  dMassAdd (&m, &m2);
2956 
2957  //nose initial box
2958  head5_geom = dCreateBox (subspace, 0.011, 0.026, 0.025);dMassSetBoxTotal (&m2,0.0278,0.011, 0.026,0.025);
2959  dGeomNames[head5_geom] = "head5_geom";
2960  dGeomSetBody (head5_geom,head);
2961  dGeomSetOffsetPosition(head5_geom,-m2.c[0], /*-0.01-*/m2.c[0], jP_head[3][0]-jP_head[2][0]-0.04-m2.c[0]);
2962  dMassAdd (&m, &m2);
2963 
2964  //nose diagonal box
2965  head6_geom = dCreateBox (subspace, 0.011, 0.051, 0.012);dMassSetBoxTotal (&m2,0.0278,0.011, 0.051,0.012);
2966  dGeomNames[head6_geom] = "head6_geom";
2967  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.2);
2968  dGeomSetBody (head6_geom,head);
2969  dGeomSetOffsetRotation(head6_geom,Rtx);
2970  dGeomSetOffsetPosition(head6_geom,-m2.c[0], /*0.001-*/0.03/*half neck yaw*/-m2.c[0], jP_head[3][0]-jP_head[2][0]-0.02-m2.c[0]);
2971  dMassAdd (&m, &m2);
2972 
2973  //nose end box
2974  head7_geom = dCreateBox (subspace, 0.02, 0.022, 0.012);dMassSetBox (&m2,0.0278,0.02, 0.022,0.012);
2975  dGeomNames[head7_geom] = "head7_geom";
2976  dGeomSetBody (head7_geom,head);
2977  dGeomSetOffsetPosition(head7_geom,-m2.c[0], /*0.028-*/jP_head[3][2]-jP_head[2][2]-0.03/*half neck yaw*/-m2.c[0], jP_head[3][0]-jP_head[2][0]-m2.c[0]);
2978  dMassAdd (&m, &m2);
2979 
2980  //dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);dBodySetMass(head,&m);
2981 }
2982 
2983 void ICubSim::initEyes(dWorldID world, dSpaceID subspace)
2984 {
2985  dBodyID select[3];
2986  double offset[3];
2987  if (actHead == "off")
2988  {
2989  select[0] = select[1] = select[2] = head;
2990  offset[0]=jP_head[3][0]-jP_head[2][0];
2991  offset[1]=jP_head[3][1]-jP_head[2][1];
2992  offset[2]=jP_head[3][2]-jP_head[2][2]-0.03;
2993  }
2994  else
2995  {
2996  select[0] = eye;
2997  select[1] = leye;
2998  select[2] = reye;
2999  offset[0]=offset[1]=offset[2]=0.0;
3000  }
3001 
3002  dMass m, m2;
3003  dMatrix3 Rtx;
3004 
3005  //eyes pitch cylinder
3006  dMassSetZero(&m);dMassSetZero(&m2);
3007  eye1_geom = dCreateCylinder (subspace,0.002,0.068);dMassSetCylinderTotal (&m2,0.0059678,3,0.002,0.068);
3008  dGeomNames[eye1_geom] = "eye1_geom";
3009  dRFromAxisAndAngle(Rtx,0,1,0,M_PI * 0.5);
3010  dGeomSetBody (eye1_geom,select[0]);
3011  dGeomSetOffsetRotation(eye1_geom,Rtx);
3012  dGeomSetOffsetPosition(eye1_geom,offset[1]-m2.c[0], /*0.037-*/+offset[2]-m2.c[0], +offset[0]-m2.c[0]);
3013  dMassAdd (&m, &m2);
3014 
3015  eye2_geom = dCreateCylinder (subspace,0.006,0.030);dMassSetCylinderTotal (&m2,0.11,3,0.006,0.030);
3016  dGeomNames[eye2_geom] = "eye2_geom";
3017  //dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
3018  dGeomSetBody (eye2_geom,select[0]);
3019  //dGeomSetOffsetRotation(eye2_geom,Rtx);
3020  dGeomSetOffsetPosition(eye2_geom,0.034+offset[1]-m2.c[0], /*0.037-*/+offset[2]-m2.c[0], -0.015+offset[0]-m2.c[0]);
3021  dMassAdd (&m, &m2);
3022 
3023  eye3_geom = dCreateCylinder (subspace,0.006,0.05);dMassSetCylinderTotal (&m2,0.0387,3,0.006,0.05);
3024  dGeomNames[eye3_geom] = "eye3_geom";
3025  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
3026  dGeomSetBody (eye3_geom,select[0]);
3027  dGeomSetOffsetRotation(eye3_geom,Rtx);
3028  dGeomSetOffsetPosition(eye3_geom,0.034+offset[1]-m2.c[0], /*0.037-*/+offset[2]-m2.c[0], -0.03+offset[0]-m2.c[0]);
3029  dMassAdd (&m, &m2);
3030 
3031  eye4_geom = dCreateCylinder (subspace,0.006,0.030);dMassSetCylinderTotal (&m2,0.0234,3,0.006,0.030);
3032  dGeomNames[eye4_geom] = "eye4_geom";
3033  //dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
3034  dGeomSetBody (eye4_geom,select[0]);
3035  //dGeomSetOffsetRotation(eye4_geom,Rtx);
3036  dGeomSetOffsetPosition(eye4_geom,-0.034+offset[1]-m2.c[0], /*0.037-*/+offset[2]-m2.c[0], -0.015+offset[0]-m2.c[0]);
3037  dMassAdd (&m, &m2);
3038 
3039  eye5_geom = dCreateCylinder (subspace,0.006,0.05);dMassSetCylinderTotal (&m2,0.0387,3,0.006,0.05);
3040  dGeomNames[eye5_geom] = "eye5_geom";
3041  dRFromAxisAndAngle(Rtx,1,0,0,M_PI * 0.5);
3042  dGeomSetBody (eye5_geom,select[0]);
3043  dGeomSetOffsetRotation(eye5_geom,Rtx);
3044  dGeomSetOffsetPosition(eye5_geom,-0.034+offset[1]-m2.c[0], /*0.037-*/+offset[2]-m2.c[0], -0.03+offset[0]-m2.c[0]);
3045  dMassAdd (&m, &m2);
3046  dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
3047 
3048  //eyeLids
3049  topEyeLid_geom = dCreateCylinder (subspace,0.01,0.01);
3050  dGeomNames[topEyeLid_geom] = "topEyeLid_geom";
3051  dGeomSetBody (topEyeLid_geom,topEyeLid);
3052 
3053  bottomEyeLid_geom = dCreateCylinder (subspace,0.01,0.01);
3054  dGeomNames[bottomEyeLid_geom] = "bottomEyeLid_geom";
3055  dGeomSetBody (bottomEyeLid_geom,bottomEyeLid);
3056 
3057  if (actHead == "on"){
3058  dBodySetMass(eye,&m);
3059  }else {
3060  dBodySetMass(head,&m);
3061  }
3062  dMassSetZero(&m);dMassSetZero(&m2);
3063 
3064  if(actHead=="off")
3065  {
3066  offset[0]=jP_leftEye[1][0]-jP_head[2][0];
3067  offset[1]=jP_leftEye[1][1]-jP_head[2][1];
3068  offset[2]=jP_leftEye[1][2]-jP_head[2][2]-0.03;
3069  }
3070  else
3071  {
3072  offset[0]=0.0;
3073  offset[1]=0.0;
3074  offset[2]=0.0;
3075  }
3076 
3077  Leye1_geom = dCreateCylinder (subspace,0.006,0.011);dMassSetCylinderTotal (&m2,0.0234,3,0.006,0.011);
3078  dGeomNames[Leye1_geom] = "Leye1_geom";
3079  dGeomSetBody (Leye1_geom,select[1]);
3080  dGeomSetOffsetPosition(Leye1_geom,offset[1]-m2.c[0], /*0.037*/+offset[2]-m2.c[0], +offset[0]-m2.c[0]);
3081  dMassAdd (&m, &m2);
3082 
3083  dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
3084  if (actHead == "on"){
3085  dBodySetMass(leye,&m);
3086  }else {
3087  dBodySetMass(head,&m);
3088  }
3089  dMassSetZero(&m);dMassSetZero(&m2);
3090 
3091  if(actHead=="off")
3092  {
3093  offset[0]=jP_rightEye[1][0]-jP_head[2][0];
3094  offset[1]=jP_rightEye[1][1]-jP_head[2][1];
3095  offset[2]=jP_rightEye[1][2]-jP_head[2][2]-0.03;
3096  }
3097  else
3098  {
3099  offset[0]=0.0;
3100  offset[1]=0.0;
3101  offset[2]=0.0;
3102  }
3103 
3104 
3105  Reye1_geom = dCreateCylinder (subspace,0.006,0.011);dMassSetCylinderTotal (&m2,0.0234,3,0.006,0.011);
3106  dGeomNames[Reye1_geom] = "Reye1_geom";
3107  dGeomSetBody (Reye1_geom,select[2]);
3108  dGeomSetOffsetPosition(Reye1_geom,offset[1]-m2.c[0], /*0.037*/+offset[2]-m2.c[0], +offset[0]-m2.c[0]);
3109  dMassAdd (&m, &m2);
3110 
3111  dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
3112  if (actHead == "on"){
3113  dBodySetMass(reye,&m);
3114  }else {
3115  dBodySetMass(head,&m);
3116  }
3117 }
3118 
3119 void ICubSim::initCovers(ResourceFinder& finder)
3120 {
3121  if (actHeadCover == "on")
3122  {
3123  iCubHeadModel = new Model();
3124  topEyeLidModel = new Model();
3125  bottomEyeLidModel = new Model();
3126  iCubHeadModel->loadModelData(finder.findFile("data/model/iCub_Head.ms3d").c_str());
3127  topEyeLidModel->loadModelData(finder.findFile("data/model/topEyeLid.ms3d").c_str());
3128  bottomEyeLidModel->loadModelData(finder.findFile("data/model/bottomEyeLid.ms3d").c_str());
3129  }
3130 
3131  numCovers = 10;
3132 
3133  if (actLegsCovers == "on")
3134  {
3135  model["lowerLeftLeg"] = finder.findFile("lowerLegCover"); //used for both legs
3136  model["lowerRightLeg"] = finder.findFile("lowerLegCover");
3137  model["upperRightLeg"] = finder.findFile("upperRightLegCover");
3138  model["upperLeftLeg"] = finder.findFile("upperLeftLegCover");
3139  model["rightFoot"] = finder.findFile("rightFootCover");
3140  model["leftFoot"] = finder.findFile("leftFootCover");
3141  }
3142  if(actLeftArmCovers == "on" || actRightArmCovers == "on"){
3143  model["lowerLeftArm"] = finder.findFile("lowerArmCover");//used for both arms
3144  model["lowerRightArm"] = finder.findFile("lowerArmCover");//used for both arms
3145  }
3146  if (actLeftArmCovers == "on")
3147  {
3148  model["upperLeftArm"] = finder.findFile("leftUpperArmCover");
3149  model["leftPalm"] = finder.findFile("leftPalm");
3150  }
3151 
3152  if (actRightArmCovers == "on")
3153  {
3154  model["upperRightArm"] = finder.findFile("rightUpperArmCover");
3155  model["rightPalm"] = finder.findFile("rightPalm");
3156  }
3157 
3158  if (actTorsoCovers == "on")
3159  {
3160  model["torso"] = finder.findFile("torsoCover");
3161  model["waist"] = finder.findFile("waistCover");
3162  }
3163 
3164  textureName[0] = finder.findFile("lowerArmTexture");//texture used for all covers
3165 
3166  yInfo() << "Creating 3D Model of the icub.......\n";
3167  for (map<string,string>::iterator itr=model.begin(); itr != model.end(); itr++) //iterating through all the cover names
3168  {
3169  model_TriData[(*itr).first] = dGeomTriMeshDataCreate();
3170  model_trimesh[(*itr).first] = dLoadMeshFromX(model[(*itr).first].c_str());
3171  if (!model_trimesh[(*itr).first])
3172  {
3173  yInfo() << "Check spelling/location of file";
3174  }
3175  else
3176  {
3177  dGeomTriMeshDataBuildSingle(model_TriData[(*itr).first], model_trimesh[(*itr).first]->Vertices, 3 * sizeof(float), model_trimesh[(*itr).first]->VertexCount, model_trimesh[(*itr).first]->Indices, model_trimesh[(*itr).first]->IndexCount, 3 * sizeof(int));
3178  if (actSelfCol == "off"){ //all covers geoms will go to the top-level iCub space
3179  model_ThreeD_obj[(*itr).first].geom = dCreateTriMesh(iCub, model_TriData[(*itr).first], 0, 0, 0);
3180  }
3181  else { //if self-collisions are on, we need to create it in the right collision space - torso / left arm / right arm / legs
3182  //note: if actSelfCol is on and actCoversCol is off, there still won't be any collisions, but let's keep the implementation of these features modular
3183  if ( (*itr).first.compare("torso")==0 || (*itr).first.compare("waist")==0){
3184  model_ThreeD_obj[(*itr).first].geom = dCreateTriMesh(iCubTorsoSpace, model_TriData[(*itr).first], 0, 0, 0);
3185  }
3186  else if ( (*itr).first.compare("upperLeftArm")==0 || (*itr).first.compare("lowerLeftArm")==0 || (*itr).first.compare("leftPalm")==0){
3187  model_ThreeD_obj[(*itr).first].geom = dCreateTriMesh(iCubLeftArmSpace, model_TriData[(*itr).first], 0, 0, 0);
3188  }
3189  else if ((*itr).first.compare("upperRightArm")==0 || (*itr).first.compare("lowerRightArm")==0 || (*itr).first.compare("rightPalm")==0){
3190  model_ThreeD_obj[(*itr).first].geom = dCreateTriMesh(iCubRightArmSpace, model_TriData[(*itr).first], 0, 0, 0);
3191  }
3192  else if ((*itr).first.compare("upperLeftLeg")==0 || (*itr).first.compare("lowerLeftLeg")==0 || (*itr).first.compare("leftFoot")==0 || (*itr).first.compare("upperRightLeg")==0 || (*itr).first.compare("lowerRightLeg")==0 || (*itr).first.compare("rightFoot")==0){
3193  model_ThreeD_obj[(*itr).first].geom = dCreateTriMesh(iCubLegsSpace, model_TriData[(*itr).first], 0, 0, 0);
3194  }
3195  else
3196  yError("ICubSim::initCovers(): unknown trimesh: %s.\n",(*itr).first.c_str());
3197  }
3198  dGeomSetData(model_ThreeD_obj[(*itr).first].geom,model_TriData[(*itr).first]);
3199  //dGeomSetPosition(model_ThreeD_obj[(*itr).first].geom,0.0,0.5,0.5);
3200 
3201  if(actCoversCol=="on") { // the covers need to become placeable geoms with bodies associated to them
3202  //leaving the head out for the moment as this cover is somehow a special "model"
3203  if ( (*itr).first.compare("torso")==0){
3204  dGeomNames[model_ThreeD_obj[(*itr).first].geom] = "torso cover";
3205  if (actTorso == "off"){
3206  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,body_torso);// this is the body that unites all the torso joints and their masses
3207  // it may be better to attach it to the upper body, but that is split into right and left part - torso[4], torso[5], so that would be weird to attach it to 1
3208  if (actHead == "on"){
3209  dGeomSetOffsetPosition(model_ThreeD_obj[(*itr).first].geom,dBodyGetPosition(neck[0])[0]-dBodyGetPosition(body_torso)[0],dBodyGetPosition(neck[0])[1]-dBodyGetPosition(body_torso)[1],dBodyGetPosition(neck[0])[2]-dBodyGetPosition(body_torso)[2]);
3210  }
3211  else{ //actHead == "off"
3212  dGeomSetOffsetPosition(model_ThreeD_obj[(*itr).first].geom,dGeomGetPosition(neck0_geom)[0]-dBodyGetPosition(body_torso)[0],dGeomGetPosition(neck0_geom)[1]-dBodyGetPosition(body_torso)[1],dGeomGetPosition(neck0_geom)[2]-dBodyGetPosition(body_torso)[2]);
3213  }
3214  dGeomSetOffsetRotation(model_ThreeD_obj[(*itr).first].geom,dGeomGetOffsetRotation(torso3_geom)); //importantly, we do GetOffsetRotation, rather than GetRotation
3215  // setOffset is expecting a dReal dMatrix3[4*3] as an argument, getOffset gives pointer to the matrix, so it should work fine like that
3216  }
3217  else { //(actTorso == "on")
3218  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,torso[3]);// let's pick the last, upmost torso joint
3219  if (actHead == "on"){
3220  dGeomSetOffsetWorldPosition (model_ThreeD_obj[(*itr).first].geom, dBodyGetPosition(neck[0])[0], dBodyGetPosition(neck[0])[1],dBodyGetPosition(neck[0])[2]);
3221  //Sets the geom's positional offset to move it to the new world coordinates. After this call, the geom will be at the world position passed in, and the offset will be the difference from the current body position
3222  }
3223  else{ //actHead == "off"
3224  dGeomSetOffsetPosition(model_ThreeD_obj[(*itr).first].geom,dGeomGetPosition(neck0_geom)[0]-dBodyGetPosition(torso[3])[0],dGeomGetPosition(neck0_geom)[1]-dBodyGetPosition(torso[3])[1],dGeomGetPosition(neck0_geom)[2]-dBodyGetPosition(torso[3])[2]);
3225  }
3226  // rotation is already fine - same as torso[3]
3227  }
3228 
3229  }
3230  else if ((*itr).first.compare("waist")==0){
3231  dGeomNames[model_ThreeD_obj[(*itr).first].geom] = "waist cover";
3232  if (actTorso == "off"){
3233  //in draw(), there is: //LDEsetM(dGeomGetPosition(torso1_geom),dGeomGetRotation(torso1_geom));
3234  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,body_torso);// this is the body that unites all the torso joints and their masses
3235  dGeomSetOffsetPosition(model_ThreeD_obj[(*itr).first].geom,dGeomGetOffsetPosition(torso1_geom)[0],dGeomGetOffsetPosition(torso1_geom)[1],dGeomGetOffsetPosition(torso1_geom)[2]);
3236  dGeomSetOffsetRotation(model_ThreeD_obj[(*itr).first].geom,dGeomGetOffsetRotation(torso1_geom));
3237  }
3238  else{ //(actTorso == "on")
3239  //in draw(), there is: // LDEsetM(dBodyGetPosition(torso[1]),dBodyGetRotation(torso[0])); //DRAW THE MODEL
3240  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,torso[0]);
3241  dGeomSetOffsetPosition(model_ThreeD_obj[(*itr).first].geom,dBodyGetPosition(torso[1])[0]-dBodyGetPosition(torso[0])[0],dBodyGetPosition(torso[1])[1]-dBodyGetPosition(torso[0])[1],dBodyGetPosition(torso[1])[2]-dBodyGetPosition(torso[0])[2]);
3242  }
3243  }
3244  else if ( (*itr).first.compare("upperLeftArm")==0 || (*itr).first.compare("lowerLeftArm")==0 || (*itr).first.compare("leftPalm")==0){
3245  if((*itr).first.compare("upperLeftArm")==0){
3246  dGeomNames[model_ThreeD_obj[(*itr).first].geom] = "upper left arm cover";
3247  if (actLArm == "off"){
3248  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,larm);
3249  //guessing from the draw() function: LDEsetM(dGeomGetPosition(larm2_geom),dGeomGetRotation(larm2_geom));
3250  //glTranslatef(0.0, 0.0,0.5*fabs(jP_leftArm[4][2] - jP_leftArm[2][2])); //after fix
3251  dGeomSetOffsetWorldPosition(model_ThreeD_obj[(*itr).first].geom,dGeomGetPosition(larm2_geom)[0],dGeomGetPosition(larm2_geom)[1]-0.5*fabs(jP_leftArm[4][2] - jP_leftArm[2][2]),dGeomGetPosition(larm2_geom)[2]);//+0.5*fabs(jP_leftArm[4][2] - jP_leftArm[2][2]));
3252  dGeomSetOffsetWorldRotation(model_ThreeD_obj[(*itr).first].geom,dGeomGetRotation(larm2_geom));
3253  }
3254  else{ //(actLArm == "on")
3255  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,body[4]);
3256  dGeomSetOffsetWorldPosition(model_ThreeD_obj[(*itr).first].geom,dBodyGetPosition(body[6])[0],dBodyGetPosition(body[6])[1],dBodyGetPosition(body[6])[2]);
3257  //rotation is already fine - same as body[4]
3258  }
3259  }
3260  else if((*itr).first.compare("lowerLeftArm")==0){
3261  dGeomNames[model_ThreeD_obj[(*itr).first].geom] = "lower left arm cover";
3262  if (actLArm == "off"){
3263  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,larm);
3264  dGeomSetOffsetWorldPosition(model_ThreeD_obj[(*itr).first].geom,dGeomGetPosition(larm3_geom)[0],dGeomGetPosition(larm3_geom)[1]-0.5*fabs(jP_leftArm[5][2] - jP_leftArm[3][2]),dGeomGetPosition(larm3_geom)[2]);
3265  dGeomSetOffsetWorldRotation(model_ThreeD_obj[(*itr).first].geom,dGeomGetRotation(larm3_geom));
3266  }
3267  else{ //(actLArm == "on")
3268  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,body[8]);
3269  dGeomSetOffsetPosition(model_ThreeD_obj[(*itr).first].geom,0,0,0.5*fabs(jP_leftArm[5][2] - jP_leftArm[3][2]));
3270  //rotation is already fine - same as body[8]
3271  }
3272  }
3273  else if((*itr).first.compare("leftPalm")==0){
3274  dGeomNames[model_ThreeD_obj[(*itr).first].geom] = "left palm cover";
3275  if (actLHand == "off"){
3276  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,l_hand);
3277  dGeomSetOffsetWorldPosition(model_ThreeD_obj[(*itr).first].geom,dGeomGetPosition(l_hand0_geom)[0]-0.01,dGeomGetPosition(l_hand0_geom)[1]+0.5*fabs(jP_leftArm[7][2] - jP_leftArm[6][2]),dGeomGetPosition(l_hand0_geom)[2]);
3278  }
3279  else { //(actLHand == "on")
3280  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,body[10]);
3281  dGeomSetOffsetPosition(model_ThreeD_obj[(*itr).first].geom,-0.01,0.5*fabs(jP_leftArm[7][2] - jP_leftArm[6][2]),0.0);
3282  // the -0.01 on the x is to shift the mesh slightly into the palm, such that we get the touch there - otherwise it seems to be hidden by the geom
3283  //note the coordinate axes where the shift occurs may be somewhat arbitrary - depending on the orientation of the mesh in the source file
3284  }
3285  } //left palm
3286  } //left arm
3287  else if ((*itr).first.compare("upperRightArm")==0 || (*itr).first.compare("lowerRightArm")==0 || (*itr).first.compare("rightPalm")==0){
3288  if((*itr).first.compare("upperRightArm")==0){
3289  dGeomNames[model_ThreeD_obj[(*itr).first].geom] = "upper right arm cover";
3290  if (actRArm == "off"){
3291  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,rarm);
3292  dGeomSetOffsetWorldPosition(model_ThreeD_obj[(*itr).first].geom,dGeomGetPosition(rarm2_geom)[0],dGeomGetPosition(rarm2_geom)[1]-0.5*fabs(jP_rightArm[4][2] - jP_rightArm[2][2]),dGeomGetOffsetPosition(rarm2_geom)[2]);
3293  dGeomSetOffsetWorldRotation(model_ThreeD_obj[(*itr).first].geom,dGeomGetRotation(rarm2_geom));
3294  }
3295  else{ //(actRArm == "on")
3296  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,body[5]);
3297  dGeomSetOffsetWorldPosition(model_ThreeD_obj[(*itr).first].geom,dBodyGetPosition(body[7])[0],dBodyGetPosition(body[7])[1],dBodyGetPosition(body[7])[2]);
3298  //rotation is already fine - same as body[5]
3299  }
3300  }
3301  else if((*itr).first.compare("lowerRightArm")==0){
3302  dGeomNames[model_ThreeD_obj[(*itr).first].geom] = "lower right arm cover";
3303  if (actRArm == "off"){
3304  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,rarm);
3305  dGeomSetOffsetWorldPosition(model_ThreeD_obj[(*itr).first].geom,dGeomGetPosition(rarm3_geom)[0],dGeomGetPosition(rarm3_geom)[1]-0.5*fabs(jP_rightArm[5][2] - jP_rightArm[3][2]),dGeomGetPosition(rarm3_geom)[2]);
3306  dGeomSetOffsetWorldRotation(model_ThreeD_obj[(*itr).first].geom,dGeomGetRotation(rarm3_geom));
3307  }
3308  else{ //(actRArm == "on")
3309  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,body[9]);
3310  dGeomSetOffsetPosition(model_ThreeD_obj[(*itr).first].geom,0,0,0.5*fabs(jP_rightArm[5][2] - jP_rightArm[3][2]));
3311  //rotation is already fine - same as body[9]
3312  }
3313  }
3314  else if((*itr).first.compare("rightPalm")==0){
3315  dGeomNames[model_ThreeD_obj[(*itr).first].geom] = "right palm cover";
3316  if (actRHand == "off"){
3317  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,r_hand);
3318  dGeomSetOffsetWorldPosition(model_ThreeD_obj[(*itr).first].geom,dGeomGetPosition(r_hand0_geom)[0]+0.01,dGeomGetPosition(r_hand0_geom)[1]+0.5*fabs(jP_rightArm[7][2] - jP_rightArm[6][2]),dGeomGetPosition(r_hand0_geom)[2]);
3319 
3320  }
3321  else{ //(actRHand == "on")
3322  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,body[11]);
3323  dGeomSetOffsetPosition(model_ThreeD_obj[(*itr).first].geom,0.01,0.5*fabs(jP_rightArm[7][2] - jP_rightArm[6][2]),0.0);
3324  // the 0.01 on the x is to shift the mesh slightly into the palm, such that we get the touch there - otherwise it seems to be hidden by the geom
3325  //note the coordinate axes where the shift occurs are somewhat arbitrary - depending on the orientation of the mesh in the source file
3326  }
3327  }
3328  } //right arm
3329  else if ((*itr).first.compare("upperLeftLeg")==0 || (*itr).first.compare("lowerLeftLeg")==0 || (*itr).first.compare("leftFoot")==0 || (*itr).first.compare("upperRightLeg")==0 || (*itr).first.compare("lowerRightLeg")==0 || (*itr).first.compare("rightFoot")==0){
3330  if ((*itr).first.compare("upperLeftLeg")==0){
3331  dGeomNames[model_ThreeD_obj[(*itr).first].geom] = "upper left leg cover";
3332  if (actLegs == "off"){
3333  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,legs);
3334  dGeomSetOffsetPosition(model_ThreeD_obj[(*itr).first].geom,dGeomGetOffsetPosition(l_leg6_geom)[0],dGeomGetOffsetPosition(l_leg6_geom)[1],dGeomGetOffsetPosition(l_leg6_geom)[2]);
3335  dGeomSetOffsetRotation(model_ThreeD_obj[(*itr).first].geom,dGeomGetOffsetRotation(l_leg5_geom));
3336  }
3337  else{ // actLegs == "on"
3338  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,leftLeg[3]);
3339  dGeomSetOffsetWorldPosition(model_ThreeD_obj[(*itr).first].geom,dGeomGetPosition(leftLeg_4_2)[0],dGeomGetPosition(leftLeg_4_2)[1],dGeomGetPosition(leftLeg_4_2)[2]);
3340  dGeomSetOffsetWorldRotation(model_ThreeD_obj[(*itr).first].geom,dGeomGetRotation(leftLeg_3_2));
3341  }
3342  }
3343  else if ((*itr).first.compare("lowerLeftLeg")==0){
3344  dGeomNames[model_ThreeD_obj[(*itr).first].geom] = "lower left leg cover";
3345  if (actLegs == "off"){
3346  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,legs);
3347  dGeomSetOffsetPosition(model_ThreeD_obj[(*itr).first].geom,dGeomGetOffsetPosition(l_leg3_geom)[0],dGeomGetOffsetPosition(l_leg3_geom)[1],dGeomGetOffsetPosition(l_leg3_geom)[2]);
3348  dGeomSetOffsetRotation(model_ThreeD_obj[(*itr).first].geom,dGeomGetOffsetRotation(l_leg3_geom));
3349  }
3350  else{ // actLegs == "on"
3351  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,leftLeg[2]);
3352  dGeomSetOffsetPosition(model_ThreeD_obj[(*itr).first].geom,dGeomGetOffsetPosition(leftLeg_2_2)[0],dGeomGetOffsetPosition(leftLeg_2_2)[1],dGeomGetOffsetPosition(leftLeg_2_2)[2]);
3353  dGeomSetOffsetRotation(model_ThreeD_obj[(*itr).first].geom,dGeomGetOffsetRotation(leftLeg_2_2));
3354  }
3355  }
3356  else if ((*itr).first.compare("leftFoot")==0){
3357  dGeomNames[model_ThreeD_obj[(*itr).first].geom] = "left foot cover";
3358  if (actLegs == "off"){
3359  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,legs);
3360  dGeomSetOffsetPosition(model_ThreeD_obj[(*itr).first].geom,dGeomGetOffsetPosition(l_leg1_geom)[0],dGeomGetOffsetPosition(l_leg1_geom)[1],dGeomGetOffsetPosition(l_leg1_geom)[2]);
3361  }
3362  else{ // actLegs == "on"
3363  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,leftLeg[0]);
3364  dGeomSetOffsetWorldPosition(model_ThreeD_obj[(*itr).first].geom,dBodyGetPosition(leftLeg[1])[0],dBodyGetPosition(leftLeg[1])[1],dBodyGetPosition(leftLeg[1])[2]);
3365  }
3366  }
3367  else if ((*itr).first.compare("upperRightLeg")==0){
3368  dGeomNames[model_ThreeD_obj[(*itr).first].geom] = "upper right leg cover";
3369  if (actLegs == "off"){
3370  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,legs);
3371  dGeomSetOffsetPosition(model_ThreeD_obj[(*itr).first].geom,dGeomGetOffsetPosition(r_leg6_geom)[0],dGeomGetOffsetPosition(r_leg6_geom)[1],dGeomGetOffsetPosition(r_leg6_geom)[2]);
3372  dGeomSetOffsetRotation(model_ThreeD_obj[(*itr).first].geom,dGeomGetOffsetRotation(r_leg5_geom));
3373  }
3374  else{ // actLegs == "on"
3375  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,rightLeg[3]);
3376  dGeomSetOffsetWorldPosition(model_ThreeD_obj[(*itr).first].geom,dGeomGetPosition(rightLeg_4_2)[0],dGeomGetPosition(rightLeg_4_2)[1],dGeomGetPosition(rightLeg_4_2)[2]);
3377  dGeomSetOffsetWorldRotation(model_ThreeD_obj[(*itr).first].geom,dGeomGetRotation(rightLeg_3_2));
3378  }
3379  }
3380  else if ((*itr).first.compare("lowerRightLeg")==0){
3381  dGeomNames[model_ThreeD_obj[(*itr).first].geom] = "lower right leg cover";
3382  if (actLegs == "off"){
3383  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,legs);
3384  dGeomSetOffsetPosition(model_ThreeD_obj[(*itr).first].geom,dGeomGetOffsetPosition(r_leg3_geom)[0],dGeomGetOffsetPosition(r_leg3_geom)[1],dGeomGetOffsetPosition(r_leg3_geom)[2]);
3385  dGeomSetOffsetRotation(model_ThreeD_obj[(*itr).first].geom,dGeomGetOffsetRotation(r_leg3_geom));
3386  }
3387  else{ // actLegs == "on"
3388  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,rightLeg[2]);
3389  dGeomSetOffsetPosition(model_ThreeD_obj[(*itr).first].geom,dGeomGetOffsetPosition(rightLeg_2_2)[0],dGeomGetOffsetPosition(rightLeg_2_2)[1],dGeomGetOffsetPosition(rightLeg_2_2)[2]);
3390  dGeomSetOffsetRotation(model_ThreeD_obj[(*itr).first].geom,dGeomGetOffsetRotation(rightLeg_2_2));
3391  }
3392  }
3393  else if ((*itr).first.compare("rightFoot")==0){
3394  dGeomNames[model_ThreeD_obj[(*itr).first].geom] = "right foot cover";
3395  if (actLegs == "off"){
3396  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,legs);
3397  dGeomSetOffsetPosition(model_ThreeD_obj[(*itr).first].geom,dGeomGetOffsetPosition(r_leg1_geom)[0],dGeomGetOffsetPosition(r_leg1_geom)[1],dGeomGetOffsetPosition(r_leg1_geom)[2]);
3398  // no offset rotation of r_leg1_geom w.r.t. legs dBodyID
3399  }
3400  else{ // actLegs == "on"
3401  dGeomSetBody(model_ThreeD_obj[(*itr).first].geom,rightLeg[0]);
3402  dGeomSetOffsetWorldPosition(model_ThreeD_obj[(*itr).first].geom,dBodyGetPosition(rightLeg[1])[0],dBodyGetPosition(rightLeg[1])[1],dBodyGetPosition(rightLeg[1])[2]);
3403  }
3404  }
3405  } //leg
3406  else{
3407  yError("ICubSim::initCovers(): unknown trimesh: %s.\n",(*itr).first.c_str());
3408  }
3409  } //if(actCoversCol=="on") - otherwise, no need to do anything, the covers will be just drawn as eye candy in draw()
3410  } //else - trimesh exists
3411  } //for all covers
3412  modelTextureIndex++;
3413  modelTexture[0] = modelTextureIndex;
3414  //the reference object
3415  //body_cube[0] = dBodyCreate(world); dMassSetZero(&m); dMassSetBoxTotal (&m,0.1,0.1,0.005,0.1);dBodySetMass(body_cube[0],&m);
3416  //geom_cube[0] = dCreateBox(space,0.1,0.005,0.1); //dGeomSetBody(geom_cube[0],body_cube[0]);
3417 }
3418 
3419 void ICubSim::initLegJoints()
3420 {
3421  if (actLegs == "off" && actTorso == "on")
3422  {
3423  dJointAttach (LLegjoints[0], legs, torso[0]);
3424  dJointSetHingeAnchor (LLegjoints[0], jP_leftLeg[5][1], elev + jP_leftLeg[5][2], jP_leftLeg[5][0]);//dJointSetHingeAnchor (LLegjoints[0], 0.0255, elev +0.468, -0.034);
3425  dJointSetHingeAxis (LLegjoints[0], 1.0, 0.0, 0.0);
3426  dJointSetHingeParam(LLegjoints[0],dParamLoStop, -2.7925);dJointSetHingeParam(LLegjoints[0],dParamHiStop, 2.7925);
3427 
3428  dJointAttach (RLegjoints[0], legs, torso[0]);
3429  dJointSetHingeAnchor (RLegjoints[0], jP_rightLeg[5][1], elev + jP_rightLeg[5][2], jP_rightLeg[5][0]);//dJointSetHingeAnchor (RLegjoints[0], -0.0255, elev +0.468, -0.034);
3430  dJointSetHingeAxis (RLegjoints[0], 1.0, 0.0, 0.0);
3431  dJointSetHingeParam(RLegjoints[0],dParamLoStop, -0.0);dJointSetHingeParam(RLegjoints[0],dParamHiStop, 0.0);
3432  }
3433  else if (actLegs == "off" && actTorso == "off")
3434  {
3435  dJointAttach (Torsojoints[0], legs, body_torso);
3436  dJointSetHingeAnchor (Torsojoints[0], jP_rightLeg[5][1], elev + jP_rightLeg[5][2], 0.0);//dJointSetHingeAnchor (Torsojoints[0], -0.0255, elev +0.468, 0.0);
3437  dJointSetHingeAxis (Torsojoints[0], 0.0, 0.0, 1.0);
3438  dJointSetHingeParam(Torsojoints[0],dParamLoStop, -2.7925);dJointSetHingeParam(LLegjoints[0],dParamHiStop, 2.7925);
3439  }
3440  else
3441  {
3442  dJointAttach (LLegjoints[0], leftLeg[0], leftLeg[1]);
3443  dJointSetHingeAnchor (LLegjoints[0], jP_leftLeg[0][1], elev + jP_leftLeg[0][2], jP_leftLeg[0][0]);//dJointSetHingeAnchor (LLegjoints[0], 0.068, elev +0.031, -0.0235);
3444  dJointSetHingeAxis (LLegjoints[0], 0.0, 0.0, 1.0);
3445  dJointSetHingeParam(LLegjoints[0],dParamLoStop, -2.7925);dJointSetHingeParam(LLegjoints[0],dParamHiStop, 2.7925);
3446 
3447  dJointAttach (LLegjoints[1], leftLeg[1], leftLeg[2]);
3448  dJointSetHingeAnchor (LLegjoints[1], jP_leftLeg[1][1], elev + jP_leftLeg[1][2], jP_leftLeg[1][0]);//dJointSetHingeAnchor (LLegjoints[1], 0.068,elev +0.031, -0.034);
3449  dJointSetHingeAxis (LLegjoints[1], 1.0, 0.0, 0.0);
3450  dJointSetHingeParam(LLegjoints[1],dParamLoStop, -2.7925);dJointSetHingeParam(LLegjoints[1],dParamHiStop, 2.7925);
3451 
3452  dJointAttach (LLegjoints[2], leftLeg[2], leftLeg[3]);
3453  dJointSetHingeAnchor (LLegjoints[2], jP_leftLeg[2][1], elev + jP_leftLeg[2][2], jP_leftLeg[2][0]);//dJointSetHingeAnchor (LLegjoints[2], 0.068, elev +0.244, -0.034);
3454  dJointSetHingeAxis (LLegjoints[2], 1.0, 0.0, 0.0);
3455  dJointSetHingeParam(LLegjoints[2],dParamLoStop, -2.7925);dJointSetHingeParam(LLegjoints[2],dParamHiStop, 2.7925);
3456 
3457  dJointAttach (LLegjoints[3], leftLeg[3], leftLeg[4]);
3458  dJointSetHingeAnchor (LLegjoints[3], jP_leftLeg[3][1], elev + jP_leftLeg[3][2], jP_leftLeg[3][0]);//dJointSetHingeAnchor (LLegjoints[3], 0.068, elev +0.468, -0.034);
3459  dJointSetHingeAxis (LLegjoints[3], 0.0, 1.0, 0.0);
3460  dJointSetHingeParam(LLegjoints[3],dParamLoStop, -2.7925);dJointSetHingeParam(LLegjoints[3],dParamHiStop, 2.7925);
3461 
3462  dJointAttach (LLegjoints[4], leftLeg[4], leftLeg[5]);
3463  dJointSetHingeAnchor (LLegjoints[4], jP_leftLeg[4][1], elev + jP_leftLeg[4][2], jP_leftLeg[4][0]);//dJointSetHingeAnchor (LLegjoints[4], 0.068, elev +0.468, -0.034);
3464  dJointSetHingeAxis (LLegjoints[4], 0.0, 0.0, 1.0);
3465  dJointSetHingeParam(LLegjoints[4],dParamLoStop, -2.7925);dJointSetHingeParam(LLegjoints[4],dParamHiStop, 2.7925);
3466 
3467  if (actTorso == "off")
3468  {
3469  dJointAttach (LLegjoints[5], leftLeg[5], body_torso);
3470  dJointSetHingeAnchor (LLegjoints[5], jP_leftLeg[5][1], elev + jP_leftLeg[5][2], jP_leftLeg[5][0]);//dJointSetHingeAnchor (LLegjoints[5], 0.0255, elev +0.468, -0.034);
3471  dJointSetHingeAxis (LLegjoints[5], 1.0, 0.0, 0.0);
3472  dJointSetHingeParam(LLegjoints[5],dParamLoStop, -2.7925);dJointSetHingeParam(LLegjoints[5],dParamHiStop, 2.7925);
3473  }
3474  else
3475  {
3476  dJointAttach (LLegjoints[5], leftLeg[5], torso[0]);
3477  dJointSetHingeAnchor (LLegjoints[5], jP_leftLeg[5][1], elev + jP_leftLeg[5][2], jP_leftLeg[5][0]);//dJointSetHingeAnchor (LLegjoints[5], 0.0255, elev +0.468, -0.034);
3478  dJointSetHingeAxis (LLegjoints[5], 1.0, 0.0, 0.0);
3479  dJointSetHingeParam(LLegjoints[5],dParamLoStop, -2.7925);dJointSetHingeParam(LLegjoints[5],dParamHiStop, 2.7925);
3480  }
3481  //RIGHT LEG JOINTS
3482  dJointAttach (RLegjoints[0], rightLeg[0], rightLeg[1]);
3483  dJointSetHingeAnchor (RLegjoints[0], jP_rightLeg[0][1], elev + jP_rightLeg[0][2], jP_rightLeg[0][0]);//dJointSetHingeAnchor (RLegjoints[0], -0.068, elev +0.031, -0.0235);
3484  dJointSetHingeAxis (RLegjoints[0], 0.0, 0.0, 1.0);
3485  dJointSetHingeParam(RLegjoints[0],dParamLoStop, -2.7925);dJointSetHingeParam(RLegjoints[0],dParamHiStop, 2.7925);
3486 
3487  dJointAttach (RLegjoints[1], rightLeg[1], rightLeg[2]);
3488  dJointSetHingeAnchor (RLegjoints[1], jP_rightLeg[1][1], elev + jP_rightLeg[1][2], jP_rightLeg[1][0]);//dJointSetHingeAnchor (RLegjoints[1], -0.068,elev +0.031, -0.034);
3489  dJointSetHingeAxis (RLegjoints[1], 1.0, 0.0, 0.0);
3490  dJointSetHingeParam(RLegjoints[1],dParamLoStop, -2.7925);dJointSetHingeParam(RLegjoints[1],dParamHiStop, 2.7925);
3491 
3492  dJointAttach (RLegjoints[2], rightLeg[2], rightLeg[3]);
3493  dJointSetHingeAnchor (RLegjoints[2], jP_rightLeg[2][1], elev + jP_rightLeg[2][2], jP_rightLeg[2][0]);//dJointSetHingeAnchor (RLegjoints[2], -0.068, elev +0.244, -0.034);
3494  dJointSetHingeAxis (RLegjoints[2], 1.0, 0.0, 0.0);
3495  dJointSetHingeParam(RLegjoints[2],dParamLoStop, -2.7925);dJointSetHingeParam(RLegjoints[2],dParamHiStop, 2.7925);
3496 
3497  dJointAttach (RLegjoints[3], rightLeg[3], rightLeg[4]);
3498  dJointSetHingeAnchor (RLegjoints[3], jP_rightLeg[3][1], elev + jP_rightLeg[3][2], jP_rightLeg[3][0]);//dJointSetHingeAnchor (RLegjoints[3], -0.068, elev +0.468, -0.034);
3499  dJointSetHingeAxis (RLegjoints[3], 0.0, 1.0, 0.0);
3500  dJointSetHingeParam(RLegjoints[3],dParamLoStop, -2.7925);dJointSetHingeParam(RLegjoints[3],dParamHiStop, 2.7925);
3501 
3502  dJointAttach (RLegjoints[4], rightLeg[4], rightLeg[5]);
3503  dJointSetHingeAnchor (RLegjoints[4], jP_rightLeg[4][1], elev + jP_rightLeg[4][2], jP_rightLeg[4][0]);//dJointSetHingeAnchor (RLegjoints[4], -0.068, elev +0.468, -0.034);
3504  dJointSetHingeAxis (RLegjoints[4], 0.0, 0.0, 1.0);
3505  dJointSetHingeParam(RLegjoints[4],dParamLoStop, -2.7925);dJointSetHingeParam(RLegjoints[4],dParamHiStop, 2.7925);
3506 
3507  if (actTorso == "off")
3508  {
3509  dJointAttach (RLegjoints[5], rightLeg[5], body_torso);
3510  dJointSetHingeAnchor (RLegjoints[5], jP_rightLeg[5][1], elev + jP_rightLeg[5][2], jP_rightLeg[5][0]);//dJointSetHingeAnchor (RLegjoints[5], -0.0255, elev +0.468, -0.034);
3511  dJointSetHingeAxis (RLegjoints[5], 1.0, 0.0, 0.0);
3512  dJointSetHingeParam(RLegjoints[5],dParamLoStop, -2.7925);dJointSetHingeParam(RLegjoints[5],dParamHiStop, 2.7925);
3513  }
3514  else
3515  {
3516  dJointAttach (RLegjoints[5], rightLeg[5], torso[0]);
3517  dJointSetHingeAnchor (RLegjoints[5], jP_rightLeg[5][1], elev + jP_rightLeg[5][2], jP_rightLeg[5][0]);//dJointSetHingeAnchor (RLegjoints[5], -0.0255, elev +0.468, -0.034);
3518  dJointSetHingeAxis (RLegjoints[5], 1.0, 0.0, 0.0);
3519  dJointSetHingeParam(RLegjoints[5],dParamLoStop, -2.7925);dJointSetHingeParam(RLegjoints[5],dParamHiStop, 2.7925);
3520  }
3521  }
3522 }
3523 
3524 void ICubSim::initTorsoJoints(OdeParams &p)
3525 {
3526  if (actTorso == "off")
3527  {
3528  dJointAttach (Torsojoints[3], body_torso, torso[4]);
3529  dJointSetHingeAnchor (Torsojoints[3], jP_torso[2][1], elev + jP_torso[2][2], jP_torso[2][0]);
3530  dJointSetHingeAxis (Torsojoints[3], 0.0, 1.0, 0.0);
3531  dJointSetHingeParam(Torsojoints[3],dParamLoStop, -2.7925);dJointSetHingeParam(Torsojoints[3],dParamHiStop, 2.7925);
3532 
3533  dJointAttach (Torsojoints[4], body_torso, torso[5]);
3534  dJointSetHingeAnchor (Torsojoints[4], jP_torso[2][1], elev + jP_torso[2][2], jP_torso[2][0]);
3535  dJointSetHingeAxis (Torsojoints[4], 0.0, 1.0, 0.0);
3536  dJointSetHingeParam(Torsojoints[4],dParamLoStop, -2.7925);dJointSetHingeParam(Torsojoints[4],dParamHiStop, 2.7925);
3537  return;
3538  }
3539 
3540  dJointAttach (Torsojoints[0], torso[0], torso[1]);
3541  dJointAttach (Torsojoints[1], torso[1], torso[2]);
3542  dJointAttach (Torsojoints[2], torso[2], torso[3]);
3543  dJointAttach (Torsojoints[3], torso[3], torso[4]);
3544  dJointAttach (Torsojoints[4], torso[3], torso[5]);
3545 
3546  int i=0;
3547  for(int j=0; j<5; j++)
3548  {
3549  i = j>2 ? 2 : j;
3550  dJointSetHingeAnchor(Torsojoints[i], jP_torso[i][1], elev+jP_torso[i][2], jP_torso[i][0]);
3551  dJointSetHingeAxis(Torsojoints[i], jA_torso[i][0], jA_torso[i][1], jA_torso[i][2]);
3552  }
3553 
3554  /* Torso joints are in reserved order with respect to robotMotorGui
3555  Max 50 30 70
3556  Min -50 -30 -10
3557  */
3558  double safetyMargin = 0.1 * CTRL_DEG2RAD; // margin added to the joint limits
3559  // this joint angle is the opposite of the real iCub joint angle so I had to invert the lower and upper bounds
3560  dJointSetHingeParam(Torsojoints[0],dParamLoStop, -70*CTRL_DEG2RAD-safetyMargin);
3561  dJointSetHingeParam(Torsojoints[0],dParamHiStop, 10*CTRL_DEG2RAD+safetyMargin);
3562  dJointSetHingeParam(Torsojoints[1],dParamLoStop, -30*CTRL_DEG2RAD-safetyMargin);
3563  dJointSetHingeParam(Torsojoints[2],dParamLoStop, -50*CTRL_DEG2RAD-safetyMargin);
3564  dJointSetHingeParam(Torsojoints[2],dParamHiStop, 50*CTRL_DEG2RAD+safetyMargin);
3565  dJointSetHingeParam(Torsojoints[3],dParamLoStop, -safetyMargin);
3566  dJointSetHingeParam(Torsojoints[3],dParamHiStop, safetyMargin);
3567  dJointSetHingeParam(Torsojoints[4],dParamLoStop, -safetyMargin);
3568  dJointSetHingeParam(Torsojoints[4],dParamHiStop, safetyMargin);
3569 
3570  for(int i=0;i<5;i++)
3571  {
3572  dJointSetHingeParam(Torsojoints[i], dParamFudgeFactor, p.fudgeFactor);
3573  dJointSetHingeParam(Torsojoints[i], dParamStopCFM, p.stopCFM);
3574  dJointSetHingeParam(Torsojoints[i], dParamStopERP, p.stopERP);
3575  dJointSetHingeParam(Torsojoints[i], dParamCFM, p.jointCFM);
3576  dJointSetHingeParam(Torsojoints[i], dParamBounce, p.jointStopBouncyness);
3577  }
3578 }
3579 
3580 void ICubSim::initLeftArmJoints(OdeParams &p)
3581 {
3582  if (actLArm == "off")
3583  {
3584  dJointAttach (LAjoints[0], torso[4], larm);
3585  dJointSetHingeAnchor (LAjoints[0], jP_leftArm[0][1], elev + jP_leftArm[0][2], jP_leftArm[0][0]);
3586  dJointSetHingeAxis (LAjoints[0], jA_leftArm[0][0], jA_leftArm[0][1], jA_leftArm[0][2]);
3587  //the angle has to be less than PI (180?) in order to be effective.....230? can not be reached
3588  dJointSetHingeParam(LAjoints[0],dParamLoStop, -0.0);
3589  dJointSetHingeParam(LAjoints[0],dParamHiStop, 0.0);
3590  return;
3591  }
3592 
3593  dJointAttach (LAjoints[0], torso[4], body[0]);//joint left clavicule and left shoulder1
3594  dJointAttach (LAjoints[1], body[0], body[2]); //joint left shoulder1 and left shoulder2
3595  dJointAttach (LAjoints[2], body[2], body[4]); //joint left shoulder1 and left upper arm
3596  dJointAttach (LAjoints[3], body[4], body[6]); //joint left upper arm and left elbow mechanism
3597  dJointAttach (LAjoints[4], body[6], body[8]); //joint left elbow mechanism and left lower arm
3598 
3599  for(int i=0; i<5; i++)
3600  {
3601  dJointSetHingeAnchor(LAjoints[i], jP_leftArm[i][1], elev+jP_leftArm[i][2], jP_leftArm[i][0]);
3602  dJointSetHingeAxis(LAjoints[i], jA_leftArm[i][0], jA_leftArm[i][1], jA_leftArm[i][2]);
3603  }
3604 
3605  double safetyMargin = 0.1 * CTRL_DEG2RAD; // margin added to the joint limits
3606  dJointSetHingeParam(LAjoints[0],dParamLoStop, -10.0*CTRL_DEG2RAD - safetyMargin);
3607  dJointSetHingeParam(LAjoints[0],dParamHiStop, 95.0*CTRL_DEG2RAD + safetyMargin);
3608  dJointSetHingeParam(LAjoints[1],dParamLoStop, -160.8*CTRL_DEG2RAD-safetyMargin);
3609  dJointSetHingeParam(LAjoints[1],dParamHiStop, 1.0+safetyMargin);
3610  dJointSetHingeParam(LAjoints[2],dParamLoStop, -52.0*CTRL_DEG2RAD-safetyMargin);
3611  dJointSetHingeParam(LAjoints[2],dParamHiStop, 80.0*CTRL_DEG2RAD+safetyMargin);
3612  dJointSetHingeParam(LAjoints[3],dParamLoStop, -1.0*CTRL_DEG2RAD-safetyMargin);
3613  dJointSetHingeParam(LAjoints[3],dParamHiStop, 106.0*CTRL_DEG2RAD+safetyMargin);
3614  dJointSetHingeParam(LAjoints[4],dParamLoStop, -90.0*CTRL_DEG2RAD-safetyMargin);
3615  dJointSetHingeParam(LAjoints[4],dParamHiStop, 90.5*CTRL_DEG2RAD+safetyMargin);
3616 
3617  for(int i=0;i<5;i++)
3618  {
3619  dJointSetHingeParam(LAjoints[i], dParamFudgeFactor, p.fudgeFactor);
3620  dJointSetHingeParam(LAjoints[i], dParamStopCFM, p.stopCFM);
3621  dJointSetHingeParam(LAjoints[i], dParamStopERP, p.stopERP);
3622  dJointSetHingeParam(LAjoints[i], dParamCFM, p.jointCFM);
3623  dJointSetHingeParam(LAjoints[i], dParamBounce, p.jointStopBouncyness);
3624  }
3625 }
3626 
3627 void ICubSim::initLeftHandJoints()
3628 {
3629  if (actLArm == "off" && actLHand == "off")
3630  {
3631  dJointAttach (LAjoints[5],larm,l_hand); //joint Universal left lower arm and left hand
3632  dJointSetUniversalAnchor (LAjoints[5], jP_leftArm[5][1], elev + jP_leftArm[5][2], jP_leftArm[5][0]);//dJointSetUniversalAnchor (LAjoints[5], 0.117, elev +0.4735, -0.026);
3633  dJointSetUniversalAxis1 (LAjoints[5],0, 0, 1);dJointSetUniversalAxis2 (LAjoints[5], 1,0,0);
3634  dJointSetUniversalParam(LAjoints[5], dParamLoStop, -2.7925);dJointSetUniversalParam(LAjoints[5], dParamHiStop, 2.7925);
3635  dJointSetUniversalParam(LAjoints[5], dParamLoStop2, -2.7925);dJointSetUniversalParam(LAjoints[5], dParamHiStop2, 2.7925);
3636  return;
3637  }
3638 
3639  if(actLArm == "on" && actLHand == "off")
3640  {
3641  dJointAttach (LAjoints[5],body[8],l_hand); //joint Universal left lower arm and left hand
3642  dJointSetUniversalAnchor (LAjoints[5], jP_leftArm[5][1], elev + jP_leftArm[5][2], jP_leftArm[5][0]);//dJointSetUniversalAnchor (LAjoints[5], 0.117, elev +0.4735, -0.026);
3643  dJointSetUniversalAxis1 (LAjoints[5],0,0, 1);dJointSetUniversalAxis2 (LAjoints[5], 1,0,0);
3644  dJointSetUniversalParam(LAjoints[5], dParamLoStop, -2.7925);dJointSetUniversalParam(LAjoints[5], dParamHiStop, 2.7925);
3645  dJointSetUniversalParam(LAjoints[5], dParamLoStop2, -2.7925);dJointSetUniversalParam(LAjoints[5], dParamHiStop2, 2.7925);
3646  return;
3647  }
3648 
3649  dBodyID temp;
3650  if (actLArm == "off")
3651  temp = larm;
3652  else
3653  temp = body[8];
3654  //LEFT HAND AND FINGER JOINTS
3655  dJointAttach (LAjoints[5],temp,body[10]); //joint Universal left lower arm and left hand
3656  dJointSetUniversalAnchor (LAjoints[5], jP_leftArm[5][1], elev + jP_leftArm[5][2], jP_leftArm[5][0]);//0.117, elev +0.4735, -0.026);//dJointSetUniversalAnchor (LAjoints[5], 0.117, elev +0.4735, -0.026);
3657  dJointSetUniversalAxis1 (LAjoints[5],0,0, 1);dJointSetUniversalAxis2 (LAjoints[5], 1,0,0);
3658  dJointSetUniversalParam(LAjoints[5], dParamLoStop, -2.7925);dJointSetUniversalParam(LAjoints[5], dParamHiStop, 2.7925);
3659  dJointSetUniversalParam(LAjoints[5], dParamLoStop2, -2.7925);dJointSetUniversalParam(LAjoints[5], dParamHiStop2, 2.7925);
3660 
3661  dJointAttach (LAjoints[6], body[10],body[12]); //joint hand index finger1
3662  dJointSetHingeAnchor (LAjoints[6], jP_leftArm[7][1], elev +jP_leftArm[6][2]-0.5*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])-0.034, jP_leftArm[7][0]+0.025);//dJointSetHingeAnchor (LAjoints[6], 0.117, elev +0.4055, -0.00325);
3663  dJointSetHingeAxis (LAjoints[6], 1.0, 0.0, 0.0);
3664  dJointSetHingeParam(LAjoints[6],dParamLoStop, -2.7925);dJointSetHingeParam(LAjoints[6],dParamHiStop, 2.7925);
3665 
3666  dJointAttach (LAjoints[7], body[10], body[13]); //joint hand middle finger1
3667  dJointSetHingeAnchor (LAjoints[7], jP_leftArm[7][1], elev +jP_leftArm[6][2]-0.5*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])-0.034, jP_leftArm[7][0]+0.01);//dJointSetHingeAnchor (LAjoints[7], 0.117, elev +0.4055, -0.0195);
3668  dJointSetHingeAxis (LAjoints[7], 1.0, 0.0, 0.0);
3669  dJointSetHingeParam(LAjoints[7],dParamLoStop, -2.7925);dJointSetHingeParam(LAjoints[7],dParamHiStop, 2.7925);
3670 
3671  dJointAttach (LAjoints[8], body[10],lhandfingers0); //joint hand ring finger1 lhandfingers0
3672  dJointSetHingeAnchor (LAjoints[8], jP_leftArm[7][1],elev +jP_leftArm[6][2]-0.5*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])-0.034, jP_leftArm[7][0]-0.016125);//dJointSetHingeAnchor (LAjoints[8], 0.117, elev +0.4055, -0.043875);
3673  dJointSetHingeAxis (LAjoints[8], 1.0, 0.0, 0.0);
3674  dJointSetHingeParam(LAjoints[8],dParamLoStop, -2.7925);dJointSetHingeParam(LAjoints[8],dParamHiStop, 2.7925);
3675 
3676  dJointAttach (LAjoints[10], body[12],body[16]); //index finger1 index finger2
3677  dJointSetHingeAnchor (LAjoints[10], jP_leftArm[7][1], elev +jP_leftArm[6][2]-0.5*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])-0.046, jP_leftArm[7][0]+0.025);//dJointSetHingeAnchor (LAjoints[10], 0.117,elev + 0.393, -0.00325);
3678  dJointSetHingeAxis (LAjoints[10], 0.0, 0.0, 1.0);
3679  dJointSetHingeParam(LAjoints[10],dParamLoStop, -2.7925);dJointSetHingeParam(LAjoints[10],dParamHiStop, 2.7925);
3680 
3681  dJointAttach (LAjoints[11], body[13], body[17]); //middle finger1 middle finger2
3682  dJointSetHingeAnchor (LAjoints[11], jP_leftArm[7][1], elev +jP_leftArm[6][2]-0.5*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])-0.046, jP_leftArm[7][0]+0.01);
3683  dJointSetHingeAxis (LAjoints[11], 0.0, 0.0, 1.0);
3684  dJointSetHingeParam(LAjoints[11],dParamLoStop, -2.7925);dJointSetHingeParam(LAjoints[11],dParamHiStop, 2.7925);
3685 
3686  dJointAttach (LAjoints[12], lhandfingers0,lhandfingers1); //ring finger1 ring finger2
3687  dJointSetHingeAnchor (LAjoints[12], jP_leftArm[7][1],elev +jP_leftArm[6][2]-0.5*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])-0.046, jP_leftArm[7][0]-0.016125);
3688  dJointSetHingeAxis (LAjoints[12], 0.0, 0.0, 1.0);
3689  dJointSetHingeParam(LAjoints[12],dParamLoStop, -2.7925);dJointSetHingeParam(LAjoints[12],dParamHiStop, 2.7925);
3690 
3691  dJointAttach (LAjoints[14], body[16],body[20]); //index finger2 index finger3
3692  dJointSetHingeAnchor (LAjoints[14], jP_leftArm[7][1], elev +jP_leftArm[6][2]-0.5*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])-0.072, jP_leftArm[7][0]+0.025);
3693  dJointSetHingeAxis (LAjoints[14], 0.0, 0.0, 1.0);
3694  dJointSetHingeParam(LAjoints[14],dParamLoStop, -2.7925);dJointSetHingeParam(LAjoints[14],dParamHiStop, 2.7925);
3695 
3696  dJointAttach (LAjoints[15], body[17], body[21]); //middle finger2 middle finger3
3697  dJointSetHingeAnchor (LAjoints[15], jP_leftArm[7][1], elev +jP_leftArm[6][2]-0.5*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])-0.074, jP_leftArm[7][0]+0.01);
3698  dJointSetHingeAxis (LAjoints[15], 0.0, 0.0, 1.0);
3699  dJointSetHingeParam(LAjoints[15],dParamLoStop, -2.7925);dJointSetHingeParam(LAjoints[15],dParamHiStop, 2.7925);
3700 
3701  dJointAttach (LAjoints[16], lhandfingers1,lhandfingers2); //ring finger2 ring finger3
3702  dJointSetHingeAnchor (LAjoints[16], jP_leftArm[7][1],elev +jP_leftArm[6][2]-0.5*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])-0.070, jP_leftArm[7][0]-0.016125);
3703  dJointSetHingeAxis (LAjoints[16], 0.0, 0.0, 1.0);
3704  dJointSetHingeParam(LAjoints[16],dParamLoStop, -2.7925);dJointSetHingeParam(LAjoints[16],dParamHiStop, 2.7925);
3705 
3706  dJointAttach (LAjoints[18], body[20],body[24]); //index finger3 index finger4
3707  dJointSetHingeAnchor (LAjoints[18], jP_leftArm[7][1], elev +jP_leftArm[6][2]-0.5*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])-0.094, jP_leftArm[7][0]+0.025);
3708  dJointSetHingeAxis (LAjoints[18], 0.0, 0.0, 1.0);
3709  dJointSetHingeParam(LAjoints[18],dParamLoStop, -2.7925);dJointSetHingeParam(LAjoints[18],dParamHiStop, 2.7925);
3710 
3711  dJointAttach (LAjoints[19], body[21],body[25]); //middle finger3 middle finger4
3712  dJointSetHingeAnchor (LAjoints[19], jP_leftArm[7][1], elev +jP_leftArm[6][2]-0.5*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])-0.098, jP_leftArm[7][0]+0.01);
3713  dJointSetHingeAxis (LAjoints[19], 0.0, 0.0, 1.0);
3714  dJointSetHingeParam(LAjoints[19],dParamLoStop, -2.7925);dJointSetHingeParam(LAjoints[19],dParamHiStop, 2.7925);
3715 
3716  dJointAttach (LAjoints[20], lhandfingers2,lhandfingers3); //ring finger3 ring finger4
3717  dJointSetHingeAnchor (LAjoints[20], jP_leftArm[7][1],elev +jP_leftArm[6][2]-0.5*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])-0.0905, jP_leftArm[7][0]-0.016125);
3718  dJointSetHingeAxis (LAjoints[20], 0.0, 0.0, 1.0);
3719  dJointSetHingeParam(LAjoints[20],dParamLoStop, -2.7925);dJointSetHingeParam(LAjoints[20],dParamHiStop, 2.7925);
3720 
3721  //thumb
3722  /*dJointAttach (LAjoints[22],body[10],body[28]);//left hand thumb1
3723  dJointSetHingeAnchor(LAjoints[22], 0.117,elev + 0.455,0.006);
3724  dJointSetHingeAxis(LAjoints[22],0,1,-0.5);
3725  dJointSetHingeParam(L
3726  Ajoints[22], dParamLoStop, (dReal)-2.7925);dJointSetHingeParam(LAjoints[22], dParamHiStop, (dReal) 2.7925);
3727  */
3728  dJointAttach (LAjoints[22],body[10],body[28]); //joint Universal left lower arm and left hand
3729  dJointSetUniversalAnchor (LAjoints[22], jP_leftArm[7][1], elev+ jP_leftArm[6][2]-0.045/*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])+0.016*/,jP_leftArm[7][0]+0.045 - 0.013);
3730  dJointSetUniversalAxis1 (LAjoints[22],0,1.5,-0.5);dJointSetUniversalAxis2 (LAjoints[22], 1,0,0);
3731  dJointSetUniversalParam(LAjoints[22], dParamLoStop, -2.7925);dJointSetUniversalParam(LAjoints[22], dParamHiStop, 2.7925);
3732  dJointSetUniversalParam(LAjoints[22], dParamLoStop2, -2.7925);dJointSetUniversalParam(LAjoints[22], dParamHiStop2, 2.7925);
3733 
3734  dJointAttach (LAjoints[23],body[28],body[29]);//left thumb1 and thumb2
3735  dJointSetHingeAnchor(LAjoints[23], jP_leftArm[7][1], elev +jP_leftArm[6][2]-0.045/*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])+0.016*/,jP_leftArm[7][0]+0.069 - 0.011);
3736  dJointSetHingeAxis(LAjoints[23],0.0,0.5,0);
3737  dJointSetHingeParam(LAjoints[23], dParamLoStop, (dReal)-2.7925);dJointSetHingeParam(LAjoints[23], dParamHiStop, (dReal) 2.7925);
3738 
3739  dJointAttach (LAjoints[24],body[29],body[30]);//left thumb2 and thumb3
3740  dJointSetHingeAnchor(LAjoints[24],jP_leftArm[7][1], elev +jP_leftArm[6][2]-0.045/*fabs(jP_leftArm[7][2]-jP_leftArm[6][2])+0.016*/,jP_leftArm[7][0]+0.088 - 0.008);
3741  dJointSetHingeAxis(LAjoints[24],0.0,0.5,0);
3742  dJointSetHingeParam(LAjoints[24], dParamLoStop, (dReal)-2.7925);dJointSetHingeParam(LAjoints[24], dParamHiStop, (dReal) 2.7925);
3743 }
3744 
3745 void ICubSim::initRightArmJoints(OdeParams &p)
3746 {
3747  if (actRArm == "off")
3748  {
3749  dJointAttach (RAjoints[0], torso[5], rarm);
3750  dJointSetHingeAnchor (RAjoints[0], jP_rightArm[0][1], elev + jP_rightArm[0][2], jP_rightArm[0][0]);
3751  dJointSetHingeAxis (RAjoints[0], jA_rightArm[0][0], jA_rightArm[0][1], jA_rightArm[0][2]);
3752  //the angle has to be less than PI (180?) in order to be effective.....230? can not be reached
3753  dJointSetHingeParam(RAjoints[0],dParamLoStop, -0.0);
3754  dJointSetHingeParam(RAjoints[0],dParamHiStop, 0.0);
3755  return;
3756  }
3757 
3758  dJointAttach (RAjoints[0], torso[5], body[1]);//joint right clavicule and right shoulder1
3759  dJointAttach (RAjoints[1], body[1], body[3]);//joint right shoulder1 and left shoulder2
3760  dJointAttach (RAjoints[2], body[3], body[5]); //joint right shoulder1 and right upper arm
3761  dJointAttach (RAjoints[3], body[5], body[7]); //joint right upper arm and right elbow mechanism
3762  dJointAttach (RAjoints[4], body[7], body[9]); //joint right elbow mechanism and right lower arm
3763 
3764  for(int i=0; i<5; i++)
3765  {
3766  dJointSetHingeAnchor(RAjoints[i], jP_rightArm[i][1], elev+jP_rightArm[i][2], jP_rightArm[i][0]);
3767  dJointSetHingeAxis(RAjoints[i], jA_rightArm[i][0], jA_rightArm[i][1], jA_rightArm[i][2]);
3768  }
3769 
3770  //Max 10 160.8 80 106 90 0 40 60 90 90 90 90 90 90 90 115
3771  //Min -95 0 -37 15.5 -90 -90 -20 0 0 0 0 0 0 0 0 0
3772  double safetyMargin = 0.1 * CTRL_DEG2RAD; // margin added to the joint limits
3773  dJointSetHingeParam(RAjoints[0],dParamLoStop, -10.0*CTRL_DEG2RAD-safetyMargin);
3774  dJointSetHingeParam(RAjoints[0],dParamHiStop, 95.0*CTRL_DEG2RAD+safetyMargin);
3775  dJointSetHingeParam(RAjoints[1],dParamLoStop, -1.0*CTRL_DEG2RAD-safetyMargin); // the lower limit should be 0 but it gives problem
3776  dJointSetHingeParam(RAjoints[1],dParamHiStop, 160.8*CTRL_DEG2RAD+safetyMargin);//180? cannot be fully reached have to make with 179.4?
3777  dJointSetHingeParam(RAjoints[2],dParamLoStop, -80.0*CTRL_DEG2RAD-safetyMargin);
3778  dJointSetHingeParam(RAjoints[2],dParamHiStop, 52.0*CTRL_DEG2RAD+safetyMargin);
3779  dJointSetHingeParam(RAjoints[3],dParamLoStop, -1.0*CTRL_DEG2RAD-safetyMargin); // lower limit should be 15? but it gives problem
3780  dJointSetHingeParam(RAjoints[3],dParamHiStop, 106.0*CTRL_DEG2RAD+safetyMargin);
3781  dJointSetHingeParam(RAjoints[4],dParamLoStop, -90.5*CTRL_DEG2RAD-safetyMargin);
3782  dJointSetHingeParam(RAjoints[4],dParamHiStop, 90.0*CTRL_DEG2RAD+safetyMargin);
3783 
3784  for(int i=0;i<5;i++)
3785  {
3786  dJointSetHingeParam(RAjoints[i], dParamFudgeFactor, p.fudgeFactor);
3787  dJointSetHingeParam(RAjoints[i], dParamStopCFM, p.stopCFM);
3788  dJointSetHingeParam(RAjoints[i], dParamStopERP, p.stopERP);
3789  dJointSetHingeParam(RAjoints[i], dParamCFM, p.jointCFM);
3790  dJointSetHingeParam(RAjoints[i], dParamBounce, p.jointStopBouncyness);
3791  }
3792 }
3793 
3794 void ICubSim::initRightHandJoints()
3795 {
3796  if (actRArm == "off" && actRHand == "off")
3797  {
3798  dJointAttach (RAjoints[5],rarm,r_hand); //joint Universal left lower arm and left hand
3799  dJointSetUniversalAnchor (RAjoints[5], jP_rightArm[5][1], elev + jP_rightArm[5][2], jP_rightArm[5][0]);//dJointSetUniversalAnchor (RAjoints[5], -0.117,elev + 0.4735, -0.026);
3800  dJointSetUniversalAxis1 (RAjoints[5],0,0, 1);dJointSetUniversalAxis2 (RAjoints[5], 1,0,0);
3801  dJointSetUniversalParam(RAjoints[5], dParamLoStop, -2.7925);dJointSetUniversalParam(RAjoints[5], dParamHiStop, 2.7925);
3802  dJointSetUniversalParam(RAjoints[5], dParamLoStop2, -2.7925);dJointSetUniversalParam(RAjoints[5], dParamHiStop2, 2.7925);
3803  return;
3804  }
3805  if(actRArm == "on" && actRHand =="off")
3806  {
3807  dJointAttach (RAjoints[5],body[9],r_hand); //joint Universal left lower arm and left hand
3808  dJointSetUniversalAnchor (RAjoints[5], jP_rightArm[5][1], elev + jP_rightArm[5][2], jP_rightArm[5][0]);//dJointSetUniversalAnchor (RAjoints[5], -0.117,elev + 0.4735, -0.026);
3809  dJointSetUniversalAxis1 (RAjoints[5],0,0, 1);dJointSetUniversalAxis2 (RAjoints[5], 1,0,0);
3810  dJointSetUniversalParam(RAjoints[5], dParamLoStop, -2.7925);dJointSetUniversalParam(RAjoints[5], dParamHiStop, 2.7925);
3811  dJointSetUniversalParam(RAjoints[5], dParamLoStop2, -2.7925);dJointSetUniversalParam(RAjoints[5], dParamHiStop2, 2.7925);
3812  return;
3813  }
3814 
3815  dBodyID temp1;
3816  if (actRArm =="off"){
3817  temp1 = rarm;
3818  }else{
3819  temp1 = body[9];
3820  }
3821  //CREATE ALL RIGHT HAND + FINGER JOINTS
3822  dJointAttach (RAjoints[5],temp1,body[11]); //joint Universal left lower arm and left hand
3823  dJointSetUniversalAnchor (RAjoints[5], jP_rightArm[5][1], elev + jP_rightArm[5][2], jP_rightArm[5][0]);//-0.117, elev +0.4735, -0.026);
3824  dJointSetUniversalAxis1 (RAjoints[5],0,0, 1);dJointSetUniversalAxis2 (RAjoints[5], 1,0,0);
3825  dJointSetUniversalParam(RAjoints[5], dParamLoStop, -2.7925);dJointSetUniversalParam(RAjoints[5], dParamHiStop, 2.7925);
3826  dJointSetUniversalParam(RAjoints[5], dParamLoStop2, -2.7925);dJointSetUniversalParam(RAjoints[5], dParamHiStop2, 2.7925);
3827 
3828  dJointAttach (RAjoints[6], body[11],body[31]); //joint hand index finger1
3829  dJointSetHingeAnchor (RAjoints[6], jP_rightArm[7][1], elev +jP_rightArm[6][2]-0.5*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])-0.034, jP_rightArm[7][0]+0.025);
3830  dJointSetHingeAxis (RAjoints[6], 1.0, 0.0, 0.0);
3831  dJointSetHingeParam(RAjoints[6],dParamLoStop, -2.7925);dJointSetHingeParam(RAjoints[6],dParamHiStop, 2.7925);
3832 
3833  dJointAttach (RAjoints[7], body[11], body[32]); //joint hand middle finger1
3834  dJointSetHingeAnchor (RAjoints[7], jP_rightArm[7][1], elev +jP_rightArm[6][2]-0.5*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])-0.034, jP_rightArm[7][0]+0.01);
3835  dJointSetHingeAxis (RAjoints[7], 1.0, 0.0, 0.0);
3836  dJointSetHingeParam(RAjoints[7],dParamLoStop, -2.7925);dJointSetHingeParam(RAjoints[7],dParamHiStop, 2.7925);
3837 
3838  dJointAttach (RAjoints[8], body[11],rhandfingers0); //joint hand ring finger1 lhandfingers0
3839  dJointSetHingeAnchor (RAjoints[8], jP_rightArm[7][1],elev +jP_rightArm[6][2]-0.5*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])-0.034, jP_rightArm[7][0]-0.016125);
3840  dJointSetHingeAxis (RAjoints[8], 1.0, 0.0, 0.0);
3841  dJointSetHingeParam(RAjoints[8],dParamLoStop, -2.7925);dJointSetHingeParam(RAjoints[8],dParamHiStop, 2.7925);
3842 
3843  dJointAttach (RAjoints[10], body[31],body[35]); //index finger1 index finger2
3844  dJointSetHingeAnchor (RAjoints[10], jP_rightArm[7][1], elev +jP_rightArm[6][2]-0.5*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])-0.046, jP_rightArm[7][0]+0.025);
3845  dJointSetHingeAxis (RAjoints[10], 0.0, 0.0, 1.0);
3846  dJointSetHingeParam(RAjoints[10],dParamLoStop, -2.7925);dJointSetHingeParam(RAjoints[10],dParamHiStop, 2.7925);
3847 
3848  dJointAttach (RAjoints[11], body[32], body[36]); //middle finger1 middle finger2
3849  dJointSetHingeAnchor (RAjoints[11], jP_rightArm[7][1], elev +jP_rightArm[6][2]-0.5*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])-0.046, jP_rightArm[7][0]+0.01);
3850  dJointSetHingeAxis (RAjoints[11], 0.0, 0.0, 1.0);
3851  dJointSetHingeParam(RAjoints[11],dParamLoStop, -2.7925);dJointSetHingeParam(RAjoints[11],dParamHiStop, 2.7925);
3852 
3853  dJointAttach (RAjoints[12], rhandfingers0,rhandfingers1); //ring finger1 ring finger2
3854  dJointSetHingeAnchor (RAjoints[12], jP_rightArm[7][1],elev +jP_rightArm[6][2]-0.5*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])-0.046, jP_rightArm[7][0]-0.016125);
3855  dJointSetHingeAxis (RAjoints[12], 0.0, 0.0, 1.0);
3856  dJointSetHingeParam(RAjoints[12],dParamLoStop, -2.7925);dJointSetHingeParam(RAjoints[12],dParamHiStop, 2.7925);
3857 
3858  dJointAttach (RAjoints[14], body[35],body[39]); //index finger2 index finger3
3859  dJointSetHingeAnchor (RAjoints[14], jP_rightArm[7][1], elev +jP_rightArm[6][2]-0.5*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])-0.072, jP_rightArm[7][0]+0.025);
3860  dJointSetHingeAxis (RAjoints[14], 0.0, 0.0, 1.0);
3861  dJointSetHingeParam(RAjoints[14],dParamLoStop, -2.7925);dJointSetHingeParam(RAjoints[14],dParamHiStop, 2.7925);
3862 
3863  dJointAttach (RAjoints[15], body[36], body[40]); //middle finger2 middle finger3
3864  dJointSetHingeAnchor (RAjoints[15], jP_rightArm[7][1], elev +jP_rightArm[6][2]-0.5*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])-0.074, jP_rightArm[7][0]+0.01);
3865  dJointSetHingeAxis (RAjoints[15], 0.0, 0.0, 1.0);
3866  dJointSetHingeParam(RAjoints[15],dParamLoStop, -2.7925);dJointSetHingeParam(RAjoints[15],dParamHiStop, 2.7925);
3867 
3868  dJointAttach (RAjoints[16], rhandfingers1,rhandfingers2); //ring finger2 ring finger3
3869  dJointSetHingeAnchor (RAjoints[16], jP_rightArm[7][1],elev +jP_rightArm[6][2]-0.5*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])-0.070, jP_rightArm[7][0]-0.016125);
3870  dJointSetHingeAxis (RAjoints[16], 0.0, 0.0, 1.0);
3871  dJointSetHingeParam(RAjoints[16],dParamLoStop, -2.7925);dJointSetHingeParam(RAjoints[16],dParamHiStop, 2.7925);
3872 
3873  dJointAttach (RAjoints[18], body[39],body[43]); //index finger3 index finger4
3874  dJointSetHingeAnchor (RAjoints[18], jP_rightArm[7][1], elev +jP_rightArm[6][2]-0.5*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])-0.094, jP_rightArm[7][0]+0.025);
3875  dJointSetHingeAxis (RAjoints[18], 0.0, 0.0, 1.0);
3876  dJointSetHingeParam(RAjoints[18],dParamLoStop, -2.7925);dJointSetHingeParam(RAjoints[18],dParamHiStop, 2.7925);
3877 
3878  dJointAttach (RAjoints[19], body[40],body[44]); //middle finger3 middle finger4
3879  dJointSetHingeAnchor (RAjoints[19], jP_rightArm[7][1], elev +jP_rightArm[6][2]-0.5*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])-0.098, jP_rightArm[7][0]+0.01);
3880  dJointSetHingeAxis (RAjoints[19], 0.0, 0.0, 1.0);
3881  dJointSetHingeParam(RAjoints[19],dParamLoStop, -2.7925);dJointSetHingeParam(RAjoints[19],dParamHiStop, 2.7925);
3882 
3883  dJointAttach (RAjoints[20], rhandfingers2,rhandfingers3); //ring finger3 ring finger4
3884  dJointSetHingeAnchor (RAjoints[20], jP_rightArm[7][1],elev +jP_rightArm[6][2]-0.5*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])-0.0905, jP_rightArm[7][0]-0.016125);
3885  dJointSetHingeAxis (RAjoints[20], 0.0, 0.0, 1.0);
3886  dJointSetHingeParam(RAjoints[20],dParamLoStop, -2.7925);dJointSetHingeParam(RAjoints[20],dParamHiStop, 2.7925);
3887 
3888  //thumb
3889  /* dJointAttach (RAjoints[22],body[11],body[47]);//left hand thumb1
3890  dJointSetHingeAnchor(RAjoints[22], -0.117,elev + 0.455,0.006);
3891  dJointSetHingeAxis(RAjoints[22],0,1,-0.5);
3892  dJointSetHingeParam(RAjoints[22], dParamLoStop, (dReal)-2.7925);dJointSetHingeParam(RAjoints[22], dParamHiStop, (dReal) 2.7925);
3893  */
3894 
3895  dJointAttach (RAjoints[22],body[11],body[47]); //palm to first thumb
3896  dJointSetUniversalAnchor (RAjoints[22], jP_rightArm[7][1],elev + jP_rightArm[6][2]-0.045/*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])+0.016*/, jP_rightArm[7][0]+0.045 -0.013);
3897  dJointSetUniversalAxis1 (RAjoints[22],0,1.5,-0.5);dJointSetUniversalAxis2 (RAjoints[22], 1,0,0);
3898  dJointSetUniversalParam(RAjoints[22], dParamLoStop, -2.7925);dJointSetUniversalParam(RAjoints[22], dParamHiStop, 2.7925);
3899  dJointSetUniversalParam(RAjoints[22], dParamLoStop2, -2.7925);dJointSetUniversalParam(RAjoints[22], dParamHiStop2, 2.7925);
3900 
3901  dJointAttach (RAjoints[23],body[47],body[48]);//left thumb1 and thumb2
3902  dJointSetHingeAnchor(RAjoints[23], jP_rightArm[7][1], elev + jP_rightArm[6][2]-0.045/*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])+0.016*/, jP_rightArm[7][0]+0.069 -0.011);
3903  dJointSetHingeAxis(RAjoints[23],0,0.5,0);
3904  dJointSetHingeParam(RAjoints[23], dParamLoStop, (dReal)-2.7925);dJointSetHingeParam(RAjoints[23], dParamHiStop, (dReal) 2.7925);
3905 
3906  dJointAttach (RAjoints[24],body[48],body[49]);//left thumb2 and thumb3
3907  dJointSetHingeAnchor(RAjoints[24], jP_rightArm[7][1], elev +jP_rightArm[6][2]-0.045/*fabs(jP_rightArm[7][2]-jP_rightArm[6][2])+0.016*/, jP_leftArm[7][0]+0.088 -0.008);
3908  dJointSetHingeAxis(RAjoints[24],0,0.5,0);
3909  dJointSetHingeParam(RAjoints[24], dParamLoStop, (dReal)-2.7925);dJointSetHingeParam(RAjoints[24], dParamHiStop, (dReal) 2.7925);
3910 }
3911 
3912 void ICubSim::initHeadJoints()
3913 {
3914  //HEAD JOINTS
3915  if (actTorso == "off" && actHead == "off"){
3916  dJointAttach (Hjoints[0], body_torso, head);
3917  dJointSetHingeAnchor(Hjoints[0], jP_head[0][1], elev + jP_head[0][2], jP_head[0][0]);//dJointSetHingeAnchor(Hjoints[0], -0.0,elev + 0.815, -0.026);
3918  dJointSetHingeAxis(Hjoints[0], 1, 0, 0);
3919  dJointSetHingeParam(Hjoints[0], dParamLoStop, -0.0); dJointSetHingeParam(Hjoints[0], dParamHiStop, 0.0);
3920  }else if(actTorso == "off" && actHead == "on"){
3921  dJointAttach (Hjoints[0],body_torso,neck[0]);//base + neck1
3922  dJointSetHingeAnchor(Hjoints[0], jP_head[0][1], elev + jP_head[0][2], jP_head[0][0]);//dJointSetHingeAnchor(Hjoints[0], -0.0, elev +0.815, -0.026);
3923  dJointSetHingeAxis(Hjoints[0], 1, 0, 0);
3924  dJointSetHingeParam(Hjoints[0], dParamLoStop, -2.7925); dJointSetHingeParam(Hjoints[0], dParamHiStop, 2.7925);
3925  }else if (actTorso == "on" && actHead == "off"){
3926  dJointAttach (Hjoints[0], torso[3], head);
3927  dJointSetHingeAnchor(Hjoints[0], jP_head[0][1], elev + jP_head[0][2], jP_head[0][0]);//dJointSetHingeAnchor(Hjoints[0], -0.0,elev + 0.815, -0.026);
3928  dJointSetHingeAxis(Hjoints[0], 1, 0, 0);
3929  dJointSetHingeParam(Hjoints[0], dParamLoStop, -0.0); dJointSetHingeParam(Hjoints[0], dParamHiStop, 0.0);
3930  }else{
3931  dJointAttach (Hjoints[0],torso[3],neck[0]);//base + neck1
3932  dJointSetHingeAnchor(Hjoints[0], jP_head[0][1], elev + jP_head[0][2], jP_head[0][0]);//dJointSetHingeAnchor(Hjoints[0], -0.0,elev + 0.815, -0.026);
3933  dJointSetHingeAxis(Hjoints[0], 1, 0, 0);
3934  dJointSetHingeParam(Hjoints[0], dParamLoStop, -2.7925); dJointSetHingeParam(Hjoints[0], dParamHiStop, 2.7925);
3935  }
3936  if (actHead == "on"){
3937  dJointAttach (Hjoints[1],neck[0],neck[1]);//neck1 + neck2
3938  dJointSetHingeAnchor(Hjoints[1], jP_head[1][1], elev + jP_head[1][2], jP_head[1][0]);//dJointSetHingeAnchor(Hjoints[1], -0.0,elev + 0.845, -0.026);
3939  dJointSetHingeAxis(Hjoints[1], 0, 0, 1);
3940  dJointSetHingeParam(Hjoints[1], dParamLoStop, -2.7925); dJointSetHingeParam(Hjoints[1], dParamHiStop, 2.7925);
3941 
3942  dJointAttach (Hjoints[2],neck[1],head);//neck2 and head
3943  dJointSetHingeAnchor(Hjoints[2], jP_head[2][1], elev + jP_head[2][2], jP_head[2][0]);//dJointSetHingeAnchor(Hjoints[2], 0.00,elev + 0.86,-0.026);
3944  dJointSetHingeAxis(Hjoints[2], 0,1,0);
3945  dJointSetHingeParam(Hjoints[2], dParamLoStop, -2.7925);dJointSetHingeParam(Hjoints[2], dParamHiStop, 2.7925);
3946 
3947  dJointAttach (Hjoints[3],head, eye);
3948  dJointSetHingeAnchor(Hjoints[3], jP_head[3][1], elev + jP_head[3][2], jP_head[3][0]);//dJointSetHingeAnchor(Hjoints[3], 0.0 ,elev +0.89, 0.038);
3949  dJointSetHingeAxis(Hjoints[3], 1, 0, 0);
3950  dJointSetHingeParam(Hjoints[3], dParamLoStop, -2.7925); dJointSetHingeParam(Hjoints[3], dParamHiStop, 2.7925);
3951 
3952  dJointAttach (Hjoints[4], eye, leye);
3953  dJointSetHingeAnchor(Hjoints[4], jP_leftEye[1][1], elev + jP_leftEye[1][2], jP_leftEye[1][0]);//dJointSetHingeAnchor(Hjoints[4], 0.034 ,elev + 0.89, 0.038);
3954  dJointSetHingeAxis(Hjoints[4], 0, 1, 0);
3955  dJointSetHingeParam(Hjoints[4], dParamLoStop, -2.7925); dJointSetHingeParam(Hjoints[4], dParamHiStop, 2.7925);
3956 
3957  dJointAttach (Hjoints[5], eye, reye);
3958  dJointSetHingeAnchor(Hjoints[5], jP_rightEye[1][1], elev + jP_rightEye[1][2], jP_rightEye[1][0]);//dJointSetHingeAnchor(Hjoints[5], -0.034 ,elev +0.89, 0.038);
3959  dJointSetHingeAxis(Hjoints[5], 0, 1, 0);
3960  dJointSetHingeParam(Hjoints[5], dParamLoStop, -2.7925); dJointSetHingeParam(Hjoints[5], dParamHiStop, 2.7925);
3961 
3962  /*dJointAttach (Hjoints[6], topEyeLid, head);
3963  dJointSetHingeAnchor(Hjoints[6], 0.0, elev + 0.928, 0.035);
3964  dJointSetHingeAxis(Hjoints[6], 0, 1, 0);
3965  dJointSetHingeParam(Hjoints[6], dParamLoStop,-2.7925); dJointSetHingeParam(Hjoints[6], dParamHiStop,2.7925);
3966 
3967  dJointAttach (Hjoints[7], bottomEyeLid, head);
3968  dJointSetHingeAnchor(Hjoints[7], 0.0, elev + 0.928, 0.035);
3969  dJointSetHingeAxis(Hjoints[7], 0, 1, 0);
3970  dJointSetHingeParam(Hjoints[7], dParamLoStop,-2.7925); dJointSetHingeParam(Hjoints[7], dParamHiStop,2.7925);*/
3971 
3972  }
3973 }
3974 
3975 void ICubSim::init_iKin()
3976 {
3977  // Matej Hoffmann - moved these init things from loadJointPosition() to here and made the iKin variables class variables -
3978  //besides joint position initialization (loadJointPosition()), they will be used repeatedly in the self-collision mode in ODE_process
3979 
3980  iKinLeftArm = iCubArm("left");
3981  iKinRightArm = iCubArm("right");
3982  iKinLeftArm.releaseLink(0); iKinLeftArm.releaseLink(1); iKinLeftArm.releaseLink(2);
3983  iKinRightArm.releaseLink(0); iKinRightArm.releaseLink(1); iKinRightArm.releaseLink(2);
3984  iKinLeftArm.setAllConstraints(false); // disable joint limits
3985  iKinRightArm.setAllConstraints(false);
3986  iKinLeftArm.setAng(zeros(10)); // set all ang to zero
3987  iKinRightArm.setAng(zeros(10));
3988 
3989  iKinInertialSensor = iCubInertialSensor(); // by default, it creates the "v1" - which matches the DH params used in skeleton.ini of the iCubGui
3990  iKinInertialSensor.setAllConstraints(false);
3991  iKinInertialSensor.setAng(zeros(6));
3992 
3993  // rototranslation from robot root to simulation world reference frame
3994  H_r2w.resize(4,4); H_r2w.zero();
3995  H_w2r.resize(4,4); H_w2r.zero();
3996  H_r2w(0,2) = -1.0; H_r2w(0,3) = -0.026;
3997  H_r2w(1,0) = -1.0;
3998  H_r2w(2,1) = 1.0; H_r2w(2,3) = -0.5976; //-elev;
3999  H_r2w(3,3) = 1.0;
4000  H_w2r = SE3inv(H_r2w);
4001 
4002  //TODO fingers, legs, head
4003 }
4004 
4005 void ICubSim::initSkinActivationBottles()
4006 {
4007  const int COUNT_HAND = 192;
4008  double hand_empty[COUNT_HAND];
4009  const int COUNT_FOREARM = 384;
4010  double forearm_empty[COUNT_FOREARM];
4011  double forearm_full[COUNT_FOREARM];
4012  const int COUNT_UPPER_ARM = 768;
4013  double upper_arm_empty[COUNT_UPPER_ARM];
4014  double upper_arm_full[COUNT_UPPER_ARM];
4015  const int COUNT_TORSO = 768;
4016  double torso_empty[COUNT_TORSO] = {0.0}; //this should initialize the whole array to 0
4017  double torso_full[COUNT_TORSO] = {255.0}; //this should initialize the whole array to 255
4018  int i = 0;
4019 
4020  for(i=0;i<COUNT_HAND;i++){
4021  hand_empty[i] = 0.0;
4022  }
4023  emptySkinActivationHand.clear(); //clear the bottle
4024  for(i=0;i<COUNT_HAND;i++){
4025  emptySkinActivationHand.addDouble(hand_empty[i]);
4026  }
4027 
4028  //init forearm
4029  for(i=0;i<COUNT_FOREARM;i++){
4030  forearm_empty[i] = 0.0;
4031  if ( ((i % 6) == 0) || ((i % 10) == 0)){ //the 7th and 11th taxel of every triangular modules are 0
4032  forearm_full[i]=0.0;
4033  }
4034  else{
4035  forearm_full[i]=255.0;
4036  }
4037  }
4038  //the second patch is incomplete - zero the missing modules
4039  for(i=192;i<204;i++){
4040  forearm_full[i]=0.0;
4041  }
4042  for(i=216;i<252;i++){
4043  forearm_full[i]=0.0;
4044  }
4045  for(i=264;i<288;i++){
4046  forearm_full[i]=0.0;
4047  }
4048  for(i=324;i<336;i++){
4049  forearm_full[i]=0.0;
4050  }
4051  for(i=360;i<384;i++){
4052  forearm_full[i]=0.0;
4053  }
4054  emptySkinActivationForearm.clear();
4055  fullSkinActivationForearm.clear(); //clear the bottle
4056  for(i=0;i<COUNT_FOREARM;i++){
4057  emptySkinActivationForearm.addDouble(forearm_empty[i]);
4058  fullSkinActivationForearm.addDouble(forearm_full[i]);
4059  }
4060  //yDebugg("fullSkinActivationForearm: %s \n",fullSkinActivationForearm.toString().c_str());
4061 
4062  //for the upper arm and torso, we keep the full all set to 255 for now - I haven't tested which taxels are 0s yet
4063  //init arm
4064  for(i=0;i<COUNT_UPPER_ARM;i++){
4065  upper_arm_empty[i] = 0.0;
4066  upper_arm_full[i]=255.0;
4067  }
4068  fullSkinActivationUpperArm.clear(); //clear the bottle
4069  for(i=0;i<COUNT_UPPER_ARM;i++){
4070  emptySkinActivationUpperArm.addDouble(upper_arm_empty[i]);
4071  fullSkinActivationUpperArm.addDouble(upper_arm_full[i]);
4072  }
4073 
4074  for(i=0;i<COUNT_TORSO;i++){
4075  torso_empty[i] = 0.0;
4076  torso_full[i]=255.0;
4077  }
4078  fullSkinActivationTorso.clear(); //clear the bottle
4079  for(i=0;i<COUNT_TORSO;i++){
4080  emptySkinActivationTorso.addDouble(torso_empty[i]);
4081  fullSkinActivationTorso.addDouble(torso_full[i]);
4082  }
4083 }
4084 
4086 
4087  //destroy all geoms
4088  if (actLegs == "off"){
4089  dGeomDestroy(l_leg0_geom); dGeomDestroy(l_leg1_geom); dGeomDestroy(l_leg2_geom); dGeomDestroy(l_leg3_geom);
4090  dGeomDestroy(l_leg4_geom); dGeomDestroy(l_leg5_geom); dGeomDestroy(l_leg6_geom); dGeomDestroy(l_leg7_geom);
4091  dGeomDestroy(r_leg0_geom); dGeomDestroy(r_leg1_geom); dGeomDestroy(r_leg2_geom); dGeomDestroy(r_leg3_geom);
4092  dGeomDestroy(r_leg4_geom); dGeomDestroy(r_leg5_geom); dGeomDestroy(r_leg6_geom); dGeomDestroy(r_leg7_geom);
4093  }else{
4094  dGeomDestroy(leftLegGeom[0]);dGeomDestroy(leftLegGeom[1]);dGeomDestroy(leftLeg_2_1);dGeomDestroy(leftLeg_2_2);
4095  dGeomDestroy(leftLeg_3_1);dGeomDestroy(leftLeg_3_2);dGeomDestroy(leftLeg_4_1);dGeomDestroy(leftLeg_4_2);
4096  dGeomDestroy(leftLegGeom[5]);
4097  dGeomDestroy(rightLegGeom[0]);dGeomDestroy(rightLegGeom[1]);dGeomDestroy(rightLeg_2_1);dGeomDestroy(rightLeg_2_2);
4098  dGeomDestroy(rightLeg_3_1);dGeomDestroy(rightLeg_3_2);dGeomDestroy(rightLeg_4_1);dGeomDestroy(rightLeg_4_2);
4099  dGeomDestroy(rightLegGeom[5]);
4100  }
4101  if (actTorso == "off"){
4102  dGeomDestroy(torso0_geom); dGeomDestroy(torso1_geom); dGeomDestroy(torso2_geom); dGeomDestroy(torso3_geom);
4103  dGeomDestroy(torsoGeom[4]); dGeomDestroy(torsoGeom[5]);
4104  }else{
4105  for (int i=0; i<6; i++){
4106  dGeomDestroy(torsoGeom[i]);
4107  }
4108  }
4109 
4110  if (actLArm == "off"){
4111  dGeomDestroy(larm0_geom); dGeomDestroy(larm1_geom); dGeomDestroy(larm2_geom); dGeomDestroy(larm3_geom);
4112  }else{
4113  dGeomDestroy(geom[0]); dGeomDestroy(geom[2]); dGeomDestroy(geom[4]); dGeomDestroy(geom[6]); dGeomDestroy(geom[8]);
4114  }
4115  if (actRArm == "off"){
4116  dGeomDestroy(rarm0_geom); dGeomDestroy(rarm1_geom); dGeomDestroy(rarm2_geom); dGeomDestroy(rarm3_geom);
4117  }else{
4118  dGeomDestroy(geom[1]); dGeomDestroy(geom[3]); dGeomDestroy(geom[5]); dGeomDestroy(geom[7]); dGeomDestroy(geom[9]);
4119  }
4120  if (actLHand == "off"){
4121  dGeomDestroy(l_hand0_geom); dGeomDestroy(l_hand1_geom); dGeomDestroy(l_hand2_geom); dGeomDestroy(l_hand3_geom); dGeomDestroy(l_hand4_geom); dGeomDestroy(l_hand5_geom);
4122  }else{
4123  dGeomDestroy(geom[10]);dGeomDestroy(geom[12]);dGeomDestroy(geom[13]);
4124  dGeomDestroy(lhandfings0_geom); dGeomDestroy(lhandfings1_geom);
4125  dGeomDestroy(geom[16]);dGeomDestroy(geom[17]);
4126  dGeomDestroy(lhandfings2_geom);dGeomDestroy(lhandfings3_geom);
4127  dGeomDestroy(geom[20]);dGeomDestroy(geom[21]);
4128  dGeomDestroy(lhandfings4_geom);dGeomDestroy(lhandfings5_geom);
4129  dGeomDestroy(geom[24]);dGeomDestroy(geom[25]);
4130  dGeomDestroy(lhandfings6_geom);dGeomDestroy(lhandfings7_geom);
4131  dGeomDestroy(geom[28]); dGeomDestroy(geom[29]); dGeomDestroy(geom[30]);
4132  }
4133 
4134  if (actRHand == "off"){
4135  dGeomDestroy(r_hand0_geom); dGeomDestroy(r_hand1_geom); dGeomDestroy(r_hand2_geom); dGeomDestroy(r_hand3_geom); dGeomDestroy(r_hand4_geom); dGeomDestroy(r_hand5_geom);
4136  }else{
4137  dGeomDestroy(geom[11]);dGeomDestroy(geom[31]);dGeomDestroy(geom[32]);
4138  dGeomDestroy(rhandfings0_geom);dGeomDestroy(rhandfings1_geom);
4139  dGeomDestroy(geom[35]);dGeomDestroy(geom[36]);
4140  dGeomDestroy(rhandfings2_geom);dGeomDestroy(rhandfings3_geom);
4141  dGeomDestroy(geom[39]);dGeomDestroy(geom[40]);
4142  dGeomDestroy(rhandfings4_geom);dGeomDestroy(rhandfings5_geom);
4143  dGeomDestroy(geom[43]);dGeomDestroy(geom[44]);
4144  dGeomDestroy(rhandfings6_geom);dGeomDestroy(rhandfings7_geom);
4145  dGeomDestroy(geom[47]); dGeomDestroy(geom[48]); dGeomDestroy(geom[49]);
4146  }
4147 
4148  if (actHead == "off"){
4149  dGeomDestroy(neck0_geom); dGeomDestroy(neck1_geom);
4150  }else{
4151  dGeomDestroy(neckgeom[0]); dGeomDestroy(neckgeom[1]);
4152  }
4153  dGeomDestroy(head0_geom); dGeomDestroy(head1_geom); dGeomDestroy(head2_geom); dGeomDestroy(head3_geom);
4154  dGeomDestroy(head4_geom); dGeomDestroy(head5_geom); dGeomDestroy(head6_geom); dGeomDestroy(head7_geom);
4155 
4156  dGeomDestroy(eye1_geom); dGeomDestroy(eye2_geom); dGeomDestroy(eye3_geom); dGeomDestroy(eye4_geom);
4157  dGeomDestroy(eye5_geom); dGeomDestroy(topEyeLid_geom); dGeomDestroy(bottomEyeLid_geom); dGeomDestroy(Leye1_geom); dGeomDestroy(Reye1_geom);
4158  if (iCubHeadModel!=0)
4159  delete iCubHeadModel;
4160 
4161  if (topEyeLidModel!=0)
4162  delete topEyeLidModel;
4163 
4164  if (bottomEyeLidModel!=0)
4165  delete bottomEyeLidModel;
4166 
4167  if (eyeLids!=0)
4168  delete eyeLids;
4169 
4170  if (model_ThreeD_obj.size()){ //destroying the cover geoms
4171  for (map<string,string>::iterator itr=model.begin(); itr != model.end(); itr++)
4172  {
4173  dGeomDestroy(model_ThreeD_obj[(*itr).first].geom);
4174  }
4175  }
4176 
4177  if (actSelfCol == "off"){
4178  dSpaceDestroy (iCub);
4179  } else{
4180  dSpaceDestroy(iCubHeadSpace); dSpaceDestroy(iCubTorsoSpace);
4181  dSpaceDestroy(iCubLeftArmSpace); dSpaceDestroy(iCubRightArmSpace);
4182  dSpaceDestroy(iCubLegsSpace); dSpaceDestroy(iCub);
4183  }
4184  dGeomNames.clear();
4185  dSpaceNames.clear();
4186 }
4187 
4188 ICubSim::ICubSim(dWorldID world, dSpaceID space, dReal X, dReal Y, dReal Z,
4189  RobotConfig& config)
4190 : ICubData() {
4191  resetSpeeds();
4192  init(world, space, X, Y, Z, config);
4193  reinitialized = true;
4194  eyeLids = 0;
4195 }
4196 
4197 //Auxiliary function to print info
4198 void ICubSim::printPositionOfGeom(dGeomID geomID)
4199 {
4200  const dReal * pos = dGeomGetPosition(geomID);
4201  yDebug("%f %f %f \n",pos[0],pos[1],pos[2]);
4202 }
4203 
4204 void ICubSim::printPositionOfBody(dBodyID bodyID)
4205 {
4206  const dReal * pos = dBodyGetPosition(bodyID);
4207  yDebug("%f %f %f \n",pos[0],pos[1],pos[2]);
4208 }
4209 
4210 
4211 
4212 
4213 void ICubSim::getSkinAndBodyPartFromSpaceAndGeomID(const dSpaceID geomSpaceID, const dGeomID geomID, SkinPart& skinPart, BodyPart& bodyPart, HandPart & handPart, bool& skinCoverFlag, bool& fingertipFlag)
4214 {
4215  OdeInit& odeinit = OdeInit::get();
4216 
4217  if (geomSpaceID == odeinit._iCub->iCubTorsoSpace){
4218  skinPart = SKIN_FRONT_TORSO;
4219  bodyPart = TORSO;
4220  if(actTorsoCovers == "on"){
4221  if(geomID == model_ThreeD_obj["torso"].geom){
4222  skinCoverFlag = true;
4223  }
4224  }
4225  //no neeed to check further which geom it was - skinPart and bodyPart is the same;
4226  //:KLUDGE for the "waist" cover, we don't signal the cover flag, because the waist does not have skin
4227  return;
4228  }
4229  else if (geomSpaceID == odeinit._iCub->iCubLeftArmSpace){
4230  if (odeinit._iCub->actLHand == "off"){
4231  if ((geomID == l_hand0_geom) || (geomID == l_hand1_geom) || (geomID == l_hand2_geom) || (geomID == l_hand3_geom) || (geomID == l_hand4_geom)
4232  || (geomID == l_hand5_geom))
4233  {
4234  skinPart = SKIN_LEFT_HAND;
4235  bodyPart = LEFT_ARM;
4236  //when the hand part is "off", there is no means to distinguish whether the fingertip was touched, so we will not emulate indiv. taxels and hence we don't need more details
4237  return;
4238  }
4239  }
4240  else{ // (odeinit._iCub->actLHand == "on")
4241  if((geomID == geom[10]) || (geomID == geom[12]) || (geomID == geom[13]) || (geomID == geom[16]) || (geomID == geom[17])
4242  || (geomID == geom[20]) || (geomID == geom[21]) || (geomID == geom[24]) || (geomID == geom[25])
4243  || (geomID == geom[28]) || (geomID == geom[29]) || (geomID == geom[30])
4244  || (geomID == lhandfings0_geom) || (geomID == lhandfings1_geom) || (geomID == lhandfings2_geom) || (geomID == lhandfings3_geom)
4245  || (geomID == lhandfings4_geom) || (geomID == lhandfings5_geom) || (geomID == lhandfings6_geom) || (geomID == lhandfings7_geom))
4246  {
4247  skinPart = SKIN_LEFT_HAND;
4248  bodyPart = LEFT_ARM;
4249  if(geomID==geom[30]){
4250  handPart = THUMB;
4251  fingertipFlag = true;
4252  }
4253  else if(geomID==geom[24]){
4254  handPart = INDEX;
4255  fingertipFlag = true;
4256  }
4257  else if(geomID==geom[25]){
4258  handPart = MIDDLE;
4259  fingertipFlag = true;
4260  }
4261  else if(geomID==lhandfings6_geom){
4262  handPart = RING;
4263  fingertipFlag = true;
4264  }
4265  else if(geomID==lhandfings7_geom){
4266  handPart = LITTLE;
4267  fingertipFlag = true;
4268  };
4269  // N.B. we could set the handPart like this for every geom on the hand, but for the non-fingertip, we will not need it - no skin emulation
4270 
4271  return;
4272  }
4273  }
4274 
4275  if(odeinit._iCub->actLArm == "off"){
4276  if( (geomID == larm0_geom) || (geomID == larm1_geom) || (geomID == larm2_geom)){
4277  skinPart = SKIN_LEFT_UPPER_ARM;
4278  bodyPart = LEFT_ARM;
4279  return;
4280  }
4281  else if(geomID == larm3_geom){
4282  skinPart = SKIN_LEFT_FOREARM;
4283  bodyPart = LEFT_ARM;
4284  return;
4285  }
4286  }
4287  else{ // odeinit._iCub->actLArm == "on"
4288  if ( (geomID == geom[0]) || (geomID == geom[2]) || (geomID == geom[4]) || (geomID == geom[6])){
4289  skinPart = SKIN_LEFT_UPPER_ARM;
4290  bodyPart = LEFT_ARM;
4291  return;
4292  }
4293  else if (geomID == geom[8]){
4294  skinPart = SKIN_LEFT_FOREARM;
4295  bodyPart = LEFT_ARM;
4296  return;
4297  }
4298  }
4299  //covers
4300  if(actLeftArmCovers == "on"){
4301  if(geomID == model_ThreeD_obj["leftPalm"].geom){
4302  skinPart = SKIN_LEFT_HAND;
4303  bodyPart = LEFT_ARM;
4304  skinCoverFlag = true;
4305  return;
4306  }
4307  else if(geomID == model_ThreeD_obj["lowerLeftArm"].geom){
4308  skinPart = SKIN_LEFT_FOREARM;
4309  bodyPart = LEFT_ARM;
4310  skinCoverFlag = true;
4311  return;
4312  }
4313  else if(geomID == model_ThreeD_obj["upperLeftArm"].geom){
4314  skinPart = SKIN_LEFT_UPPER_ARM;
4315  bodyPart = LEFT_ARM;
4316  skinCoverFlag = true;
4317  return;
4318  }
4319  }
4320 
4321  yError("ICubSim::getSkinAndBodyPartFromSpaceAndGeomID: Unknown skin part and body part on collision in left arm space.\n");
4322  skinPart = SKIN_PART_UNKNOWN;
4323  bodyPart = BODY_PART_UNKNOWN;
4324  return;
4325  }
4326  else if (geomSpaceID == odeinit._iCub->iCubRightArmSpace){
4327  if (odeinit._iCub->actRHand == "off"){
4328  if ((geomID == r_hand0_geom) || (geomID == r_hand1_geom) || (geomID == r_hand2_geom) || (geomID == r_hand3_geom) || (geomID == r_hand4_geom)
4329  || (geomID == r_hand5_geom)){
4330  skinPart = SKIN_RIGHT_HAND;
4331  bodyPart = RIGHT_ARM;
4332  return;
4333  }
4334  }
4335  else{ // (odeinit._iCub->actRHand == "on")
4336  if((geomID == geom[11]) || (geomID == geom[31]) || (geomID == geom[32]) || (geomID == geom[35]) || (geomID == geom[36])
4337  || (geomID == geom[39]) || (geomID == geom[40]) || (geomID == geom[43]) || (geomID == geom[44])
4338  || (geomID == geom[47]) || (geomID == geom[48]) || (geomID == geom[49])
4339  || (geomID == rhandfings0_geom) || (geomID == rhandfings1_geom) || (geomID == rhandfings2_geom) || (geomID == rhandfings3_geom)
4340  || (geomID == rhandfings4_geom) || (geomID == rhandfings5_geom) || (geomID == rhandfings6_geom) || (geomID == rhandfings7_geom))
4341  {
4342  skinPart = SKIN_RIGHT_HAND;
4343  bodyPart = RIGHT_ARM;
4344 
4345  if(geomID==geom[49]){
4346  handPart = THUMB;
4347  fingertipFlag = true;
4348  }
4349  else if(geomID==geom[43]){
4350  handPart = INDEX;
4351  fingertipFlag = true;
4352  }
4353  else if(geomID==geom[44]){
4354  handPart = MIDDLE;
4355  fingertipFlag = true;
4356  }
4357  else if(geomID==rhandfings6_geom){
4358  handPart = RING;
4359  fingertipFlag = true;
4360  }
4361  else if(geomID==rhandfings7_geom){
4362  handPart = LITTLE;
4363  fingertipFlag = true;
4364  };
4365  // N.B. we could set the handPart like this for every geom on the hand, but for the non-fingertip, we will not need it - no skin emulation
4366  return;
4367  }
4368  }
4369 
4370  if(odeinit._iCub->actRArm == "off"){
4371  if( (geomID == rarm0_geom) || (geomID == rarm1_geom) || (geomID == rarm2_geom)){
4372  skinPart = SKIN_RIGHT_UPPER_ARM;
4373  bodyPart = RIGHT_ARM;
4374  return;
4375  }
4376  else if(geomID == rarm3_geom){
4377  skinPart = SKIN_RIGHT_FOREARM;
4378  bodyPart = RIGHT_ARM;
4379  return;
4380  }
4381  }
4382  else{ // odeinit._iCub->actRArm == "on"
4383  if ( (geomID == geom[1]) || (geomID == geom[3]) || (geomID == geom[5]) || (geomID == geom[7])){
4384  skinPart = SKIN_RIGHT_UPPER_ARM;
4385  bodyPart = RIGHT_ARM;
4386  return;
4387  }
4388  else if (geomID == geom[9]){
4389  skinPart = SKIN_RIGHT_FOREARM;
4390  bodyPart = RIGHT_ARM;
4391  return;
4392  }
4393  }
4394 
4395  //covers
4396  if(actRightArmCovers == "on"){
4397  if(geomID == model_ThreeD_obj["rightPalm"].geom){
4398  skinPart = SKIN_RIGHT_HAND;
4399  bodyPart = RIGHT_ARM;
4400  skinCoverFlag = true;
4401  return;
4402  }
4403  else if(geomID == model_ThreeD_obj["lowerRightArm"].geom){
4404  skinPart = SKIN_RIGHT_FOREARM;
4405  bodyPart = RIGHT_ARM;
4406  skinCoverFlag = true;
4407  return;
4408  }
4409  else if(geomID == model_ThreeD_obj["upperRightArm"].geom){
4410  skinPart = SKIN_RIGHT_UPPER_ARM;
4411  bodyPart = RIGHT_ARM;
4412  skinCoverFlag = true;
4413  return;
4414  }
4415  }
4416 
4417  yError("ICubSim::getSkinAndBodyPartFromSpaceAndGeomID: Unknown skin part and body part on collision in right arm space.\n");
4418  skinPart = SKIN_PART_UNKNOWN;
4419  bodyPart = BODY_PART_UNKNOWN;
4420  return;
4421  }
4422  else if (geomSpaceID == odeinit._iCub->iCubHeadSpace){
4423  skinPart = SKIN_PART_UNKNOWN; //need to extend the skin parts
4424  bodyPart = HEAD;
4425  return;
4426  }
4427  else if (geomSpaceID == odeinit._iCub->iCubLegsSpace){
4428  skinPart = SKIN_PART_UNKNOWN; //need to extend the skin parts
4429  bodyPart = BODY_PART_UNKNOWN; // through checking the geoms, we could find out if it is left or right leg
4430  return;
4431  }
4432  else {
4433  yError("ICubSim::getSkinAndBodyPartFromSpaceAndGeomID:unknown iCub space.");
4434  skinPart = SKIN_PART_UNKNOWN;
4435  bodyPart = BODY_PART_UNKNOWN;
4436  return;
4437 
4438  }
4439 }
4440 
string actLArm
Definition: iCub.h:80
#define M_PI
Definition: XSensMTx.cpp:24
map< string, iCubCovers > model_ThreeD_obj
Definition: iCub.h:253
double checkTouchSensor_continuousValued(int bodyToCheck)
Definition: iCub.cpp:106
dxTriMeshX * dLoadMeshFromX(const char *FileName)
Definition: xloader.cpp:39
dGeomID rhandfings4_geom
Definition: iCub.h:141
The iCub header file.
void getSkinAndBodyPartFromSpaceAndGeomID(const dSpaceID geomSpaceID, const dGeomID geomID, SkinPart &skinPart, BodyPart &bodyPart, HandPart &handPart, bool &skinCoverFlag, bool &fingertipFlag)
Definition: iCub.cpp:4213
dGeomID lhandfings3_geom
Definition: iCub.h:126
dGeomID larm0_geom
Definition: iCub.h:155
dSpaceID iCubRightArmSpace
Definition: iCub.h:89
ICubSim * _iCub
Definition: OdeInit.h:67
static void printPositionOfGeom(dGeomID geomID)
Definition: iCub.cpp:4198
void DrawSphere(float radius, bool wireframe, bool texture, int whichtexture)
Definition: rendering.cpp:329
const dReal * pos
Definition: iCub_Sim.cpp:62
Model * iCubHeadModel
Definition: iCub.cpp:61
dSpaceID iCubLeftArmSpace
Definition: iCub.h:89
void reloadTextures()
Definition: MS3D.cpp:277
dGeomID lhandfings1_geom
Definition: iCub.h:123
~ICubSim()
Definition: iCub.cpp:4085
zeros(2, 2) eye(2
dGeomID r_hand1_geom
Definition: iCub.h:162
dGeomID r_hand3_geom
Definition: iCub.h:162
dGeomID lhandfings7_geom
Definition: iCub.h:132
void DrawCylinder(float radius, float length, bool wireframe, bool texture, int whichtexture)
Definition: rendering.cpp:353
double fudgeFactor
Definition: RobotConfig.h:40
dGeomID larm1_geom
Definition: iCub.h:155
dGeomID l_hand3_geom
Definition: iCub.h:161
dGeomID rhandfings3_geom
Definition: iCub.h:138
bool reinitialized
Definition: iCub.h:84
virtual yarp::os::ResourceFinder & getFinder()=0
void setName(std::string module)
dSpaceID iCubHeadSpace
Definition: iCub.h:89
void DrawBox(float width, float height, float length, bool wireframe, bool texture, int whichtexture)
Definition: rendering.cpp:226
Definition: iCub.h:65
dGeomID l_hand2_geom
Definition: iCub.h:161
dGeomID larm2_geom
Definition: iCub.h:155
EyeLids * eyeLids
Definition: iCub.h:78
iCubSimulationControl ** _controls
Definition: OdeInit.h:74
virtual RobotFlags & getFlags()=0
static void printPositionOfBody(dBodyID bodyID)
Definition: iCub.cpp:4204
dGeomID r_hand0_geom
Definition: iCub.h:162
dGeomID rarm3_geom
Definition: iCub.h:158
dGeomID geom[50]
Definition: iCub.h:118
dSpaceID iCubLegsSpace
Definition: iCub.h:89
static OdeInit & get()
Definition: OdeInit.cpp:189
dGeomID rhandfings1_geom
Definition: iCub.h:135
dGeomID r_hand4_geom
Definition: iCub.h:162
void LDEsetM(const dReal *pos, const dReal *R)
Definition: rendering.cpp:400
dGeomID lhandfings6_geom
Definition: iCub.h:132
void setFlags()
Definition: RobotConfig.cpp:37
double stopCFM
Definition: RobotConfig.h:41
ODE state information.
Definition: OdeInit.h:55
string actRHand
Definition: iCub.h:80
dGeomID l_hand4_geom
Definition: iCub.h:161
dGeomID rhandfings5_geom
Definition: iCub.h:141
This file deals with the loading of 3D objects into the simulator.
dGeomID rhandfings7_geom
Definition: iCub.h:144
double stopERP
Definition: RobotConfig.h:45
A class for defining the Inertia Sensor Kinematics of the iCub.
Definition: iKinFwd.h:1363
dGeomID rarm1_geom
Definition: iCub.h:158
Model * bottomEyeLidModel
Definition: iCub.cpp:63
Definition: MS3D.h:155
virtual OdeParams getOdeParameters()=0
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
dGeomID lhandfings0_geom
Definition: iCub.h:123
Header for the 3D x loader.
double getTouchSensorValue(dJointFeedback *fb)
Definition: iCub.cpp:75
dGeomID l_hand1_geom
Definition: iCub.h:161
double jointStopBouncyness
Definition: RobotConfig.h:52
string actRightArmCovers
Definition: iCub.h:80
dGeomID larm3_geom
Definition: iCub.h:155
dGeomID rarm0_geom
Definition: iCub.h:158
dGeomID l_hand5_geom
Definition: iCub.h:161
void resetSpeeds()
Definition: iCub.cpp:164
dGeomID rhandfings0_geom
Definition: iCub.h:135
dGeomID r_hand5_geom
Definition: iCub.h:162
double jointCFM
Definition: RobotConfig.h:42
virtual bool getControlModesRaw(int *modes) override
dGeomID rhandfings6_geom
Definition: iCub.h:144
ICubData()
Definition: iCub.cpp:66
degrees offset
Definition: sine.m:4
void draw(bool wireframed, int TextureNumber)
Definition: MS3D.cpp:213
This file contains the definition of unique IDs for the body parts and the skin parts of the robot...
#define TOUCH_SENSOR_MAX_FORCE
Definition: iCub.cpp:71
string actLHand
Definition: iCub.h:80
dGeomID l_hand0_geom
Definition: iCub.h:161
dGeomID rhandfings2_geom
Definition: iCub.h:138
Model * topEyeLidModel
Definition: iCub.cpp:62
Class that encloses everything relate to a skinPart.
Definition: skinPart.h:145
void DrawX(dTriMeshX trim, int whichtexture)
Definition: rendering.cpp:461
string actRArm
Definition: iCub.h:80
dGeomID r_hand2_geom
Definition: iCub.h:162
A class for defining the iCub Arm.
Definition: iKinFwd.h:1081
dGeomID lhandfings4_geom
Definition: iCub.h:129
void draw()
Definition: iCub.cpp:316
string actLeftArmCovers
Definition: iCub.h:80
string actTorsoCovers
Definition: iCub.h:80
#define FLAGIFY(flags, name)
Definition: iCub.cpp:1568
bool loadModelData(const char *filename)
Definition: MS3D.cpp:86
dGeomID lhandfings5_geom
Definition: iCub.h:129
bool checkTouchSensor(int bodyToCheck)
Definition: iCub.cpp:140
#define CTRL_DEG2RAD
Definition: iCub.cpp:55
dSpaceID iCubTorsoSpace
Definition: iCub.h:89
dGeomID rarm2_geom
Definition: iCub.h:158
dGeomID lhandfings2_geom
Definition: iCub.h:126
ICubSim(dWorldID world, dSpaceID space, dReal X, dReal Y, dReal Z, RobotConfig &config)
Definition: iCub.cpp:4188