iCub-main
OdeInit.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 
21 #include "OdeInit.h"
22 #include <yarp/os/LogStream.h>
23 
24 OdeInit *OdeInit::_odeinit = NULL;
25 
26 OdeInit::OdeInit(RobotConfig *config) : robot_config(config)
27 {
28  //create the world parameters
29  world = dWorldCreate();
30  space = dHashSpaceCreate (0);
31  contactgroup = dJointGroupCreate (0);
32  //verbose = false;
33  verbosity = config->getVerbosity();
34 
35  dWorldSetGravity (world,0,-9.8,0);
36  dWorldSetERP(world, config->getWorldERP()); // error reduction parameter: in [0.1,0.8], the higher, the more springy constraints are
37  dWorldSetCFM(world, config->getWorldCFM()); // constraint force mixing: in [1e-9,1], the higher, the softer constraints are
38 
39  // Maximum correcting velocity the contacts are allowed to generate. Default value is infinity.
40  // Reducing it can help prevent "popping" of deeply embedded objects
41  dWorldSetContactMaxCorrectingVel(world, config->getMaxContactCorrectingVel());
42 
43  // Contacts are allowed to sink into the surface layer up to the given depth before coming to rest.
44  // The default value is zero. Increasing this to some small value (e.g. 0.001) can help prevent jittering
45  // problems due to contacts being repeatedly made and broken.
46  dWorldSetContactSurfaceLayer(world, config->getContactSurfaceLayer());
47 
48  //Contact joints can simulate friction at the contact by applying special forces in the two friction directions that are perpendicular to the normal.
49  //This value sets the friction coefficient, mju, for both directions. High means that the colliding surfaces will not slide along one another.
50  //0.0 means that they will slip with no resistance
51  contactFrictionCoefficient = config->getContactFrictionCoefficient(); //unlike the other ODE params fron .ini file that are used to intiialize the properties of the simulation (dWorldSet...),
52  //This parameter is employed on the run as contact joints are created (in OdeSdlSimulation::nearCallback() )
53 
54  ground = dCreatePlane (space,0, 1, 0, 0);
55  //feedback = new dJointFeedback;
56  //feedback1 = new dJointFeedback;
57  //feedback_mat = new dJointFeedback;
58  _iCub = new ICubSim(world, space, 0,0,0, *robot_config);
59  _wrld = new worldSim(world, space, 0,0,0, *robot_config);
60  _controls = new iCubSimulationControl*[MAX_PART];
61 
62 
63 // // info on spaces created - relevant in actSelfCol mode
64 // yDebug("ICubSim::init(): overview of the spaces created: \n");
65 // std::string s("space");
66 // printInfoOnSpace(space,s);
67 // s="iCub";
68 // printInfoOnSpace(_iCub->iCub,s);
69 // s="iCubHeadSpace";
70 // printInfoOnSpace(_iCub->iCubHeadSpace,s);
71 // s="iCubTorsoSpace";
72 // printInfoOnSpace(_iCub->iCubTorsoSpace,s);
73 // s="iCubLeftArmSpace";
74 // printInfoOnSpace(_iCub->iCubLeftArmSpace,s);
75 // s="iCubRightArmSpace";
76 // printInfoOnSpace(_iCub->iCubRightArmSpace,s);
77 // s="iCubLegsSpace";
78 // printInfoOnSpace(_iCub->iCubLegsSpace,s);
79 
80  // initialize at NULL
81  for (int i=0; i<MAX_PART; i++)
82  {
83  _controls[i] = NULL;
84  }
85 
86  _wrld->OBJNUM = 0;
87  _wrld->waitOBJ = 0;
88  _wrld->S_OBJNUM = 0;
89 
90  _wrld->SPHNUM = 0;
91  _wrld->waitSPH = 0;
92  _wrld->S_SPHNUM = 0;
93 
94  _wrld->cylOBJNUM = 0;
95  _wrld->waitOBJ1 = 0;
96  _wrld->S_cylOBJNUM = 0;
97 
98  _wrld->waitMOD = 0;
99  _wrld->MODEL_NUM = 0;
100 
101  _wrld->s_waitMOD = 0;
102  _wrld->s_MODEL_NUM = 0;
103 }
104 
106 {
107  delete _wrld;
108  delete _iCub;
109  delete[] _controls;
110 
111  dGeomDestroy(ground);
112  dJointGroupDestroy(contactgroup);
113  dSpaceDestroy(space);
114  dWorldDestroy(world);
115 }
116 
118 {
119  if (_odeinit==NULL)
120  {
121  _odeinit=new OdeInit(config);
122  }
123  return *_odeinit;
124 }
126 {
127  if (_controls != NULL)
128  {
129  _controls[part] = control;
130  }
131 }
132 
134  if (imu)
135  {
136  _imu = imu;
137  }
138 }
139 
141  _imu = nullptr;
142 }
143 
145 {
146  double refs[16] = {0,0,0,0,0,0,0,0,0,0,0,10*M_PI/180,10*M_PI/180,10*M_PI/180,10*M_PI/180,10*M_PI/180};
147  int position_vocabs[16] = {VOCAB_CM_POSITION,VOCAB_CM_POSITION,VOCAB_CM_POSITION,VOCAB_CM_POSITION,
148  VOCAB_CM_POSITION,VOCAB_CM_POSITION,VOCAB_CM_POSITION,VOCAB_CM_POSITION,
149  VOCAB_CM_POSITION,VOCAB_CM_POSITION,VOCAB_CM_POSITION,VOCAB_CM_POSITION,
150  VOCAB_CM_POSITION,VOCAB_CM_POSITION,VOCAB_CM_POSITION,VOCAB_CM_POSITION};
151  if (_wrld->actWorld == "on")
152  {
153  refs[0] = -0*M_PI/180;
154  refs[1] = 80*M_PI/180;
155  refs[3] = 50*M_PI/180;
156  refs[7] = 59*M_PI/180;
157  refs[8] = 20*M_PI/180;
158  refs[9] = 20*M_PI/180;
159  refs[10] = 20*M_PI/180;
160  }
161  else
162  {
163  refs[0] = -25*M_PI/180;
164  refs[1] = 20*M_PI/180;
165  refs[3] = 50*M_PI/180;
166  refs[7] = 59*M_PI/180;
167  refs[8] = 20*M_PI/180;
168  refs[9] = 20*M_PI/180;
169  refs[10] = 20*M_PI/180;
170  }
171 
172  if (_iCub->actLArm == "on" || _iCub->actLHand == "on"){
173  _controls[1]->setControlModesRaw(position_vocabs);
174  _controls[1]->positionMoveRaw(refs);
175  }
176  if (_iCub->actRArm == "on" || _iCub->actRHand == "on"){
177  _controls[2]->setControlModesRaw(position_vocabs);
178  _controls[2]->positionMoveRaw(refs);
179  }
180 }
181 
183  if (_controls != NULL)
184  {
185  _controls[part] = NULL;
186  }
187 }
188 
190 {
191  return *_odeinit;
192 }
193 
195 {
196  if (_odeinit!=NULL)
197  {
198  delete _odeinit;
199  _odeinit = NULL;
200  }
201 }
202 
203 //Auxiliary function to print class of geom - according to section 9.5 of ODE manual
204 void OdeInit::printGeomClassAndNr(int geom_class, int geom_nr)
205 {
206  switch(geom_class){
207  case 0:
208  yDebug(" Geom nr. %d is a sphere.\n",geom_nr);
209  break;
210  case 1:
211  yDebug(" Geom nr. %d is a box.\n",geom_nr);
212  break;
213  case 2:
214  yDebug(" Geom nr. %d is a capsule\n.",geom_nr);
215  break;
216  case 3:
217  yDebug(" Geom nr. %d is a cylinder\n.",geom_nr);
218  break;
219  case 4:
220  yDebug(" Geom nr. %d is a plane\n.",geom_nr);
221  break;
222  case 8:
223  yDebug(" Geom nr. %d is a triangle mesh\n.",geom_nr);
224  break;
225  case 10:
226  case 11:
227  yDebug(" Geom nr. %d is a simple space\n.",geom_nr);
228  break;
229  case 12:
230  yDebug(" Geom nr. %d is a hash space\n.",geom_nr);
231  break;
232  default:
233  yDebug(" Geom nr. %d has an unknown type (nr. %d).\n",geom_nr,geom_class);
234  break;
235  }
236 
237 }
238 
239 //Auxiliary function to print info on a space
240 void OdeInit::printInfoOnSpace(dSpaceID my_space,const std::string & my_space_name)
241 {
242  int num_geoms = 0;
243  dGeomID geom_temp;
244  int geom_class_temp = 0;
245  dReal aabb[6];
246  dBodyID body_temp;
247 
248  num_geoms = dSpaceGetNumGeoms(my_space);
249  yDebug("\nSpace: %s: ID: %p. sublevel: %d, nr. geoms: %d. \n",my_space_name.c_str(),my_space,dSpaceGetSublevel(my_space),num_geoms);
250  for (int i=0;i<=(num_geoms-1);i++){
251  geom_temp = dSpaceGetGeom (my_space, i);
252  geom_class_temp = dGeomGetClass(geom_temp);
253  printGeomClassAndNr(geom_class_temp,i);
254  if (!dGeomIsSpace(geom_temp)){
255  if (dGeomGetBody(geom_temp)!=NULL){ // i.e. a placeable geom
256  yDebug(" ID: %p, Coordinates:",geom_temp);
257  ICubSim::printPositionOfGeom(geom_temp);
258  dGeomGetAABB(geom_temp,aabb);
259  yDebug(" Bounding box coordinates: %f-%f,%f-%f,%f-%f\n:",aabb[0],aabb[1],aabb[2],aabb[3],aabb[4],aabb[5]);
260  if (geom_class_temp==8){ // trimesh
261  body_temp = dGeomGetBody(geom_temp);
262  yDebug(" Coordinates of associated body are:");
263  ICubSim::printPositionOfBody(body_temp);
264  }
265  } else {
266  dGeomGetAABB(geom_temp,aabb);
267  yDebug(" ID: %p; bounding box coordinates: %f-%f,%f-%f,%f-%f\n:",geom_temp,aabb[0],aabb[1],aabb[2],aabb[3],aabb[4],aabb[5]);
268  }
269  }
270  }
271 }
272 
273 
#define MAX_PART
int verbosity
Definition: main.cpp:20
#define M_PI
Definition: XSensMTx.cpp:24
static RobotConfig * robot_config
Definition: iCub_Sim.cpp:84
virtual double getContactSurfaceLayer()=0
Definition: iCub.h:75
~OdeInit()
Definition: OdeInit.cpp:105
static void printPositionOfGeom(dGeomID geomID)
Definition: iCub.cpp:4198
void removeSimulationIMU()
Definition: OdeInit.cpp:140
static void printPositionOfBody(dBodyID bodyID)
Definition: iCub.cpp:4204
static void destroy()
Definition: OdeInit.cpp:194
void sendHomePos()
Definition: OdeInit.cpp:144
static OdeInit & get()
Definition: OdeInit.cpp:189
virtual double getWorldCFM()=0
This file is responsible for the initialisation of the world parameters that are controlled by ODE...
ODE state information.
Definition: OdeInit.h:55
virtual double getMaxContactCorrectingVel()=0
virtual double getContactFrictionCoefficient()=0
static OdeInit & init(RobotConfig *config)
Definition: OdeInit.cpp:117
static void printInfoOnSpace(dSpaceID my_space, const std::string &my_space_name)
Definition: OdeInit.cpp:240
virtual int getVerbosity()=0
virtual double getWorldERP()=0
void setSimulationIMU(iCubSimulationIMU *imu)
Definition: OdeInit.cpp:133
void setSimulationControl(iCubSimulationControl *control, int part)
Definition: OdeInit.cpp:125
void removeSimulationControl(int part)
Definition: OdeInit.cpp:182
static void printGeomClassAndNr(int geom_class, int geom_nr)
Definition: OdeInit.cpp:204