22 #include <yarp/os/LogStream.h>
24 OdeInit *OdeInit::_odeinit = NULL;
29 world = dWorldCreate();
30 space = dHashSpaceCreate (0);
31 contactgroup = dJointGroupCreate (0);
35 dWorldSetGravity (world,0,-9.8,0);
54 ground = dCreatePlane (space,0, 1, 0, 0);
96 _wrld->S_cylOBJNUM = 0;
101 _wrld->s_waitMOD = 0;
102 _wrld->s_MODEL_NUM = 0;
113 dSpaceDestroy(
space);
114 dWorldDestroy(
world);
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};
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;
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;
208 yDebug(
" Geom nr. %d is a sphere.\n",geom_nr);
211 yDebug(
" Geom nr. %d is a box.\n",geom_nr);
214 yDebug(
" Geom nr. %d is a capsule\n.",geom_nr);
217 yDebug(
" Geom nr. %d is a cylinder\n.",geom_nr);
220 yDebug(
" Geom nr. %d is a plane\n.",geom_nr);
223 yDebug(
" Geom nr. %d is a triangle mesh\n.",geom_nr);
227 yDebug(
" Geom nr. %d is a simple space\n.",geom_nr);
230 yDebug(
" Geom nr. %d is a hash space\n.",geom_nr);
233 yDebug(
" Geom nr. %d has an unknown type (nr. %d).\n",geom_nr,geom_class);
244 int geom_class_temp = 0;
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);
254 if (!dGeomIsSpace(geom_temp)){
255 if (dGeomGetBody(geom_temp)!=NULL){
256 yDebug(
" ID: %p, Coordinates:",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){
261 body_temp = dGeomGetBody(geom_temp);
262 yDebug(
" Coordinates of associated body are:");
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]);
This file is responsible for the initialisation of the world parameters that are controlled by ODE.
static void printPositionOfBody(dBodyID bodyID)
static void printPositionOfGeom(dGeomID geomID)
void removeSimulationControl(int part)
void setSimulationIMU(iCubSimulationIMU *imu)
void removeSimulationIMU()
static void printInfoOnSpace(dSpaceID my_space, const std::string &my_space_name)
static void printGeomClassAndNr(int geom_class, int geom_nr)
dJointGroupID contactgroup
static OdeInit & init(RobotConfig *config)
iCubSimulationControl ** _controls
void setSimulationControl(iCubSimulationControl *control, int part)
virtual double getMaxContactCorrectingVel()=0
virtual int getVerbosity()=0
virtual double getContactFrictionCoefficient()=0
virtual double getWorldCFM()=0
virtual double getWorldERP()=0
virtual double getContactSurfaceLayer()=0
virtual bool positionMoveRaw(int j, double ref) override
virtual bool setControlModesRaw(const int n_joint, const int *joints, int *modes) override
static RobotConfig * robot_config