iCub-main
iCub.h
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 #ifndef ICUBSIMULATION_ICUB_INC
29 #define ICUBSIMULATION_ICUB_INC
30 
31 #include <iostream>
32 #include <map>
33 #include "SDL.h"
34 #include "SDL_opengl.h"
35 #include "rendering.h"
36 
37 #include <yarp/sig/Vector.h>
38 
39 #include <ode/ode.h>
40 #include <string>
41 #include "RobotConfig.h"
42 #include "OdeInit.h"
43 
44 #include "EyeLidsController.h"
45 
46 //these were added for the self-collision and skin emulation
48 #include <iCub/ctrl/math.h>
49 #include <yarp/math/Math.h>
50 #include <map>
51 #include <iCub/iKin/iKinFwd.h>
52 #include <yarp/sig/Matrix.h>
53 
54 #ifdef _MSC_VER
55 #pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
56 #endif
57 using namespace yarp::os;
58 using namespace yarp::sig;
59 using namespace std;
60 
61  using namespace yarp::math;
62  using namespace iCub::skinDynLib;
63  using namespace iCub::ctrl;
64 
65 class ICubData{
66 public:
67  //some Object constants....
68  #define MAX_CONTACTS 10// maximum number of contact points per body
69  #define NUM 10 // max number of objects
70 
71  string configPath;
72  ICubData();
73 };
74 
75 class ICubSim : public ICubData {
76 public:
77 
79  static const bool textured = true;
80  string actElevation, actStartHomePos, actLegs, actTorso, actLArm, actRArm, actLHand, actRHand, actHead, actfixedHip, actVision, actHeadCover, actWorld, actPressure, actScreen, actLegsCovers, actLeftArmCovers, actRightArmCovers, actTorsoCovers, actSelfCol, actCoversCol, actSkinEmul;
81  double elev;
82 
83  dGeomID screenGeom;
85  float eyeLidRot;
87 
88  dSpaceID iCub;
89  dSpaceID iCubHeadSpace, iCubTorsoSpace, iCubLeftArmSpace, iCubRightArmSpace, iCubLegsSpace; //these are needed for the iCub self-collisions;
90  //nevertheless, if actSelfCol == off, then they will all be set to iCub
91  std::map<dSpaceID, string> dSpaceNames; //needed for development and testing of self-collisions
92  std::map<dGeomID, string> dGeomNames;
93 
94  dGeomID geom_cube[1];
95 
96  dBodyID inertialBody;
97  dGeomID inertialGeom;
98  dBodyID iCubHead;
99  dGeomID iCubHeadGeom;
100  /*----Lower Body parts----*/
101  dBodyID leftLeg[20];
102  dGeomID leftLegGeom[20];
103  //for encapsulated objects
104  dGeomID leftLeg_2_1,leftLeg_2_2;
105  dGeomID leftLeg_3_1,leftLeg_3_2;
106  dGeomID leftLeg_4_1,leftLeg_4_2;
107 
108  dBodyID rightLeg[20];
109  dGeomID rightLegGeom[20];
110  dGeomID rightLeg_2_1,rightLeg_2_2;
111  dGeomID rightLeg_3_1,rightLeg_3_2;
112  dGeomID rightLeg_4_1,rightLeg_4_2;
113 
114  dBodyID torso[9];
115  dGeomID torsoGeom[9];
116 
117  dBodyID body[50];
118  dGeomID geom[50];
119 
120  dBodyID neck[2];
121  dGeomID neckgeom[2];
122 
123  dGeomID lhandfings0_geom,lhandfings1_geom;
124  dBodyID lhandfingers0;
125 
126  dGeomID lhandfings2_geom,lhandfings3_geom;
127  dBodyID lhandfingers1;
128 
129  dGeomID lhandfings4_geom,lhandfings5_geom;
130  dBodyID lhandfingers2;
131 
132  dGeomID lhandfings6_geom,lhandfings7_geom;
133  dBodyID lhandfingers3;
134 
135  dGeomID rhandfings0_geom,rhandfings1_geom;
136  dBodyID rhandfingers0;
137 
138  dGeomID rhandfings2_geom,rhandfings3_geom;
139  dBodyID rhandfingers1;
140 
141  dGeomID rhandfings4_geom,rhandfings5_geom;
142  dBodyID rhandfingers2;
143 
144  dGeomID rhandfings6_geom,rhandfings7_geom;
145  dBodyID rhandfingers3;
146 
147  //init. encapsulated object-the left+right leg
148  dGeomID l_leg0_geom,l_leg1_geom,l_leg2_geom,l_leg3_geom,l_leg4_geom,l_leg5_geom,l_leg6_geom,l_leg7_geom,l_leg8_geom;
149  dGeomID r_leg0_geom,r_leg1_geom,r_leg2_geom,r_leg3_geom,r_leg4_geom,r_leg5_geom,r_leg6_geom,r_leg7_geom,r_leg8_geom;
150  dBodyID legs;
151  //init. encapsulated object-the torso
152  dGeomID torso0_geom,torso1_geom,torso1b_geom,torso2_geom,torso3_geom,torso4_geom,torso5_geom;
153  dBodyID body_torso;
154  //init. encapsulated object-the left arm
155  dGeomID larm0_geom,larm1_geom,larm2_geom,larm3_geom,larm4_geom,larm5_geom;
156  dBodyID larm;
157  //init. encapsulated object-the right arm
158  dGeomID rarm0_geom,rarm1_geom,rarm2_geom,rarm3_geom,rarm4_geom,rarm5_geom;
159  dBodyID rarm;
160  //init. encapsulated object-the hand if user selects it
161  dGeomID l_hand0_geom, l_hand1_geom, l_hand2_geom, l_hand3_geom, l_hand4_geom, l_hand5_geom;
162  dGeomID r_hand0_geom, r_hand1_geom, r_hand2_geom, r_hand3_geom, r_hand4_geom, r_hand5_geom;
163  dBodyID l_hand, r_hand;
164 
165  //init. encapsulated object-the head
166  dGeomID head0_geom, head1_geom, head2_geom, head3_geom,head4_geom,head5_geom,head6_geom,head7_geom,head8_geom;
167  dGeomID neck0_geom, neck1_geom;
168  dBodyID head;
169 
170  //init encapsulated object_the EYE
171  dGeomID eye1, eye2,eye3,eye4,eye5;
172  dGeomID eye1_geom,eye2_geom,eye3_geom,eye4_geom,eye5_geom;
173  dBodyID eye;
174 
175  //init. encapsulated object - the left eye
176  dGeomID List1_L_E, List2_L_E;
177  dGeomID Leye1_geom, Leye2_geom;
178  dBodyID leye;
179 
180  //init. encapsulated object - the right eye
181  dGeomID List1_R_E,List2_R_E;
182  dGeomID Reye1_geom,Reye2_geom;
183  dBodyID reye;
184 
185  //init. eye lids
186  dGeomID topEyeLid_geom;
188  dBodyID topEyeLid;
189  dBodyID bottomEyeLid;
190 
191  /*----Lower Body joints----*/
192  dJointID elevJoint;
193 
194  dJointID LLegjoints [20];
195  dJointID RLegjoints [20];
196 
197  dJointID Torsojoints[8];
198  dJointID LAjoints [25];
199  dJointID RAjoints [25];
200 
201  dJointID Hjoints [6];
202  dJointID inertialJoint;
203 
204 
205  dJointID grab;
206  dJointID grab1;
207 
208  dJointID fixedHipJoint;
209 
210  /*---- joint speeds----*/
211  dReal LLeg_speed [20];
212  dReal RLeg_speed [20];
213  dReal Torso_speed[8];
214  dReal la_speed[25];
215  dReal la_speed1[25];
216  dReal ra_speed[25];
217  dReal ra_speed1[25];
218  dReal h_speed[25];
219 
220  /*---- joint torques----*/
221  dReal LLeg_torques[20];
222  dReal RLeg_torques[20];
223  dReal Torso_torques[8];
224  dReal la_torques[25];
225  dReal ra_torques[25];
226  dReal h_torques[25];
227 
228  map <string, string> model;
229  string textureName[100];
230  map <string, dTriMeshDataID> model_TriData;
231  map <string, dTriMeshX> model_trimesh;
232 
233  int modelTexture[100];
236 
237  double torqueData[100];
238 
239  class iCubCovers {
240  public:
241  dBodyID body;
242  dGeomID geom;
243  virtual dBodyID getBody()const {return body;}
244  virtual dGeomID getGeom()const {return geom;}
245 
246  void reloadTexture(string texture, const int &modelTexture)
247  {
248  //string tmptext = (char *) model_DIR.c_str();
249  //string texture = ("C:/DEV/iCub/app/simConfig/models/blueCovers.bmp");
250  setupTexture( (char* ) texture.c_str(), modelTexture);
251  }
252  };
253  map<string, iCubCovers> model_ThreeD_obj;
254 
255  /*------ Joints Positions------*/
256  Vector jP_torso[3];
257  Vector jP_leftArm[8];
258  Vector jP_rightArm[8];
259  Vector jP_head[4];
260  Vector jP_leftEye[2];
261  Vector jP_rightEye[2];
262  Vector jP_leftLeg[6];
263  Vector jP_rightLeg[6];
264  Vector jP_inertial;
265  /*------- Joint Axes ----------*/
266  Vector jA_torso[3];
267  Vector jA_leftArm[8];
268  Vector jA_rightArm[8];
269 
270  //We make these class variables, besides joint position initialization (loadJointPosition()), they will be used repeatedly in the self-collision mode in ODE_process
272  iCub::iKin::iCubInertialSensor iKinInertialSensor; //needed to get FoR 3 in the kinematics - the first neck joint - FoR for the skin of the torso
273 
274  // Preset bottles with empty or full activation of some skin parts that can be sent to a port
282 
283  // rototranslation form robot root to simulation world reference frame and vice versa
284  Matrix H_r2w, H_w2r;
285 
286  ICubSim(dWorldID world, dSpaceID space, dReal X, dReal Y, dReal Z,
287  RobotConfig& config);
288 
289  ~ICubSim();
290 
291  void resetSpeeds();
292 
297  void setJointControlAction();
298  //void syncAngles();
299 
300  bool checkTouchSensor(int bodyToCheck);
301  bool checkTouchSensor(dBodyID id);
302  double checkTouchSensor_continuousValued(int bodyToCheck);
303  double checkTouchSensor_continuousValued(dBodyID id);
304  void draw();
305 
306  void getSkinAndBodyPartFromSpaceAndGeomID(const dSpaceID geomSpaceID, const dGeomID geomID, SkinPart& skinPart,BodyPart& bodyPart, HandPart& handPart, bool& skinCoverFlag, bool& fingertipFlag);
307  static void printPositionOfGeom(dGeomID geomID);
308  static void printPositionOfBody(dBodyID bodyID);
309 
310  private:
311  //int inc;
312  bool loadJointPosition(const char *joints_path);
313  void setPosition(dReal agentX, dReal agentY, dReal agentZ );
314  void init( dWorldID world, dSpaceID space, dReal X, dReal Y, dReal Z,
315  RobotConfig& config);
316  void activateiCubParts(RobotConfig& config);
317 
318  void initLegsOff(dWorldID world, dSpaceID subspace);
319  void initLegsOn(dWorldID world, dSpaceID subspace);
320  void initTorsoOff(dWorldID world, dSpaceID subspace);
321  void initTorsoOn(dWorldID world, dSpaceID subspace);
322  void initLeftArmOff(dWorldID world, dSpaceID subspace);
323  void initLeftArmOn(dWorldID world, dSpaceID subspace);
324  void initRightArmOff(dWorldID world, dSpaceID subspace);
325  void initRightArmOn(dWorldID world, dSpaceID subspace);
326  void initLeftHandOff(dWorldID world, dSpaceID subspace);
327  void initLeftHandOn(dWorldID world, dSpaceID subspace);
328  void initRightHandOff(dWorldID world, dSpaceID subspace);
329  void initRightHandOn(dWorldID world, dSpaceID subspace);
330  void initHead(dWorldID world, dSpaceID subspace);
331  void initEyes(dWorldID world, dSpaceID subspace);
332  void initCovers(ResourceFinder& finder);
333 
334  void initLegJoints();
335  void initTorsoJoints(OdeParams &p);
336  void initLeftArmJoints(OdeParams &p);
337  void initRightArmJoints(OdeParams &p);
338  void initLeftHandJoints();
339  void initRightHandJoints();
340  void initHeadJoints();
341 
342  void init_iKin();
343  void initSkinActivationBottles();
344 };
345 
346 
347 #endif
dGeomID torso5_geom
Definition: iCub.h:152
dBodyID reye
Definition: iCub.h:183
map< string, dTriMeshDataID > model_TriData
Definition: iCub.h:230
iCub::iKin::iCubInertialSensor iKinInertialSensor
Definition: iCub.h:272
dGeomID eye5_geom
Definition: iCub.h:172
map< string, iCubCovers > model_ThreeD_obj
Definition: iCub.h:253
dGeomID bottomEyeLid_geom
Definition: iCub.h:187
dBodyID rarm
Definition: iCub.h:159
virtual dGeomID getGeom() const
Definition: iCub.h:244
dGeomID iCubHeadGeom
Definition: iCub.h:99
map< string, string > model
Definition: iCub.h:228
iCub::iKin::iCubArm iKinRightArm
Definition: iCub.h:271
dBodyID inertialBody
Definition: iCub.h:96
string eyeLidsPortName
Definition: iCub.h:86
dGeomID rightLeg_4_2
Definition: iCub.h:112
Definition: iCub.h:75
dGeomID lhandfings3_geom
Definition: iCub.h:126
dBodyID lhandfingers0
Definition: iCub.h:124
dBodyID iCubHead
Definition: iCub.h:98
dGeomID lhandfings1_geom
Definition: iCub.h:123
dGeomID l_leg8_geom
Definition: iCub.h:148
dBodyID r_hand
Definition: iCub.h:163
dJointID grab
Definition: iCub.h:205
dGeomID topEyeLid_geom
Definition: iCub.h:186
dGeomID lhandfings7_geom
Definition: iCub.h:132
string configPath
Definition: iCub.h:71
STL namespace.
Vector jP_inertial
Definition: iCub.h:264
dGeomID Reye2_geom
Definition: iCub.h:182
dGeomID Leye2_geom
Definition: iCub.h:177
dGeomID rhandfings3_geom
Definition: iCub.h:138
bool reinitialized
Definition: iCub.h:84
dBodyID body_torso
Definition: iCub.h:153
dGeomID leftLeg_2_2
Definition: iCub.h:104
dGeomID rarm5_geom
Definition: iCub.h:158
Definition: iCub.h:65
EyeLids * eyeLids
Definition: iCub.h:78
dJointID grab1
Definition: iCub.h:206
Bottle fullSkinActivationUpperArm
Definition: iCub.h:280
dBodyID topEyeLid
Definition: iCub.h:188
dGeomID larm5_geom
Definition: iCub.h:155
dJointID elevJoint
Definition: iCub.h:192
dGeomID rhandfings1_geom
Definition: iCub.h:135
dBodyID rhandfingers1
Definition: iCub.h:139
dBodyID head
Definition: iCub.h:168
dBodyID lhandfingers1
Definition: iCub.h:127
Bottle fullSkinActivationForearm
Definition: iCub.h:279
int modelTextureIndex
Definition: iCub.h:234
Bottle emptySkinActivationForearm
Definition: iCub.h:276
This file is responsible for the initialisation of the world parameters that are controlled by ODE...
dBodyID lhandfingers2
Definition: iCub.h:130
dGeomID rhandfings5_geom
Definition: iCub.h:141
dGeomID rightLeg_3_2
Definition: iCub.h:111
dGeomID rhandfings7_geom
Definition: iCub.h:144
Bottle emptySkinActivationHand
Definition: iCub.h:275
double elev
Definition: iCub.h:81
dGeomID leftLeg_3_2
Definition: iCub.h:105
Bottle emptySkinActivationUpperArm
Definition: iCub.h:277
Header file for the rendering.
dGeomID inertialGeom
Definition: iCub.h:97
A class for defining the Inertia Sensor Kinematics of the iCub.
Definition: iKinFwd.h:1363
dGeomID rightLeg_2_2
Definition: iCub.h:110
float eyeLidRot
Definition: iCub.h:85
void reloadTexture(string texture, const int &modelTexture)
Definition: iCub.h:246
dBodyID body
Definition: iCub.h:241
dGeomID l_hand5_geom
Definition: iCub.h:161
void setupTexture(char *filename, int whichtexture)
Definition: rendering.cpp:504
dBodyID legs
Definition: iCub.h:150
dGeomID eye5
Definition: iCub.h:171
Matrix H_w2r
Definition: iCub.h:284
dJointID inertialJoint
Definition: iCub.h:202
dGeomID r_hand5_geom
Definition: iCub.h:162
dSpaceID iCub
Definition: iCub.h:88
dGeomID screenGeom
Definition: iCub.h:83
map< string, dTriMeshX > model_trimesh
Definition: iCub.h:231
dBodyID lhandfingers3
Definition: iCub.h:133
dBodyID eye
Definition: iCub.h:173
dBodyID larm
Definition: iCub.h:156
virtual dBodyID getBody() const
Definition: iCub.h:243
Class that encloses everything relate to a skinPart.
Definition: skinPart.h:145
string actWorld
Definition: iCub.h:80
dGeomID head8_geom
Definition: iCub.h:166
A class for defining the iCub Arm.
Definition: iKinFwd.h:1081
dBodyID bottomEyeLid
Definition: iCub.h:189
std::map< dSpaceID, string > dSpaceNames
Definition: iCub.h:91
dJointID fixedHipJoint
Definition: iCub.h:208
dGeomID List2_L_E
Definition: iCub.h:176
dGeomID neck1_geom
Definition: iCub.h:167
Bottle fullSkinActivationTorso
Definition: iCub.h:281
std::map< dGeomID, string > dGeomNames
Definition: iCub.h:92
dGeomID lhandfings5_geom
Definition: iCub.h:129
dGeomID r_leg8_geom
Definition: iCub.h:149
dBodyID leye
Definition: iCub.h:178
dBodyID rhandfingers3
Definition: iCub.h:145
dGeomID leftLeg_4_2
Definition: iCub.h:106
Bottle emptySkinActivationTorso
Definition: iCub.h:278
dBodyID rhandfingers0
Definition: iCub.h:136
dGeomID geom
Definition: iCub.h:242
dSpaceID iCubTorsoSpace
Definition: iCub.h:89
dBodyID rhandfingers2
Definition: iCub.h:142
int numCovers
Definition: iCub.h:235
dGeomID List2_R_E
Definition: iCub.h:181