iCub-main
world.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 "world.h"
29 #include <cstdio>
30 #include <yarp/os/LogStream.h>
31 #include <iostream>
32 #include "OdeInit.h"
33 
34 using namespace std;
35 using namespace yarp::os;
36 
37 static float xyz[3], hpr[3];
38 static dReal ballVel[3], ballDamp[3];
39 
41 }
42 
44 }
45 
47 {
48  ballVel[0] = dBodyGetLinearVel (ballBody)[0];
49  ballVel[1] = dBodyGetLinearVel (ballBody)[1];
50  ballVel[2] = dBodyGetLinearVel (ballBody)[2];
51 
52  ballDamp[0] = ballVel[0] * -0.8;
53  ballDamp[1] = ballVel[1] * -0.8;
54  ballDamp[2] = ballVel[2] * -0.8;
55 
56  j = dBodyGetJoint(ballBody, 0);
57 
58  if(j){
59  dBodyAddForce(ballBody,ballDamp[0],ballDamp[1],ballDamp[2]);
60  }
61 }
63 
64  if (actWorld == "on"){
66  glColor3d(0.6,0.6,0.0);
67  glPushMatrix();LDEsetM(dGeomGetPosition(tableGeom[0]),dGeomGetRotation(tableGeom[0]));
68  DrawBox(0.03,0.5,0.03,false,textured,2);glPopMatrix();
69 
70  glPushMatrix();LDEsetM(dGeomGetPosition(tableGeom[1]),dGeomGetRotation(tableGeom[1]));
71  DrawBox(0.03,0.5,0.03,false,textured,2);glPopMatrix();
72 
73  glPushMatrix();LDEsetM(dGeomGetPosition(tableGeom[2]),dGeomGetRotation(tableGeom[2]));
74  DrawBox(0.03,0.5,0.03,false,textured,2);glPopMatrix();
75 
76  glPushMatrix();LDEsetM(dGeomGetPosition(tableGeom[3]),dGeomGetRotation(tableGeom[3]));
77  DrawBox(0.03,0.5,0.03,false,textured,2);glPopMatrix();
78 
79  glPushMatrix();LDEsetM(dGeomGetPosition(tableGeom[4]),dGeomGetRotation(tableGeom[4]));
80  DrawBox(0.7,0.03,0.4,false,textured,2);glPopMatrix();
81 
82  /*glColor3d(0.8,0.0,0.0);
83  glPushMatrix();LDEsetM(dGeomGetPosition(box_part[0]),dGeomGetRotation(box_part[0]));
84  DrawBox(0.01,0.01,0.1,false,textured,2);glPopMatrix();
85 
86  glPushMatrix();LDEsetM(dGeomGetPosition(box_part[1]),dGeomGetRotation(box_part[1]));
87  DrawBox(0.01,0.01,0.1,false,textured,2);glPopMatrix();
88 
89  glPushMatrix();LDEsetM(dGeomGetPosition(box_part[2]),dGeomGetRotation(box_part[2]));
90  DrawBox(0.1,0.01,0.01,false,textured,2);glPopMatrix();
91 
92  glPushMatrix();LDEsetM(dGeomGetPosition(box_part[3]),dGeomGetRotation(box_part[3]));
93  DrawBox(0.1,0.01,0.01,false,textured,2);glPopMatrix();
94 
95  glPushMatrix();LDEsetM(dGeomGetPosition(box_part[4]),dGeomGetRotation(box_part[4]));
96  DrawBox(0.01,0.1,0.01,false,textured,2);glPopMatrix();
97 
98  glPushMatrix();LDEsetM(dGeomGetPosition(box_part[5]),dGeomGetRotation(box_part[5]));
99  DrawBox(0.01,0.1,0.01,false,textured,2);glPopMatrix();
100 
101  glPushMatrix();LDEsetM(dGeomGetPosition(box_part[6]),dGeomGetRotation(box_part[6]));
102  DrawBox(0.01,0.1,0.01,false,textured,2);glPopMatrix();
103 
104  glPushMatrix();LDEsetM(dGeomGetPosition(box_part[7]),dGeomGetRotation(box_part[7]));
105  DrawBox(0.01,0.1,0.01,false,textured,2);glPopMatrix();
106 
107  glPushMatrix();LDEsetM(dGeomGetPosition(box_part[8]),dGeomGetRotation(box_part[8]));
108  DrawBox(0.01,0.01,0.1,false,textured,2);glPopMatrix();
109 
110  glPushMatrix();LDEsetM(dGeomGetPosition(box_part[9]),dGeomGetRotation(box_part[9]));
111  DrawBox(0.01,0.01,0.1,false,textured,2);glPopMatrix();
112 
113  glPushMatrix();LDEsetM(dGeomGetPosition(box_part[10]),dGeomGetRotation(box_part[10]));
114  DrawBox(0.1,0.01,0.01,false,textured,2);glPopMatrix();
115 
116  glPushMatrix();LDEsetM(dGeomGetPosition(box_part[11]),dGeomGetRotation(box_part[11]));
117  DrawBox(0.1,0.01,0.01,false,textured,2);glPopMatrix();
118  */
119  glColor3d(0.0,0.0,0.8);
120  glPushMatrix(); LDEsetM(dBodyGetPosition(ballBody),dBodyGetRotation(ballBody));
121  DrawSphere(0.04,false,textured,2);glPopMatrix();
122 
123  }
124 
125  for (int i=0; i<OBJNUM; i++) {
126  glColor3d(color[i][0],color[i][1],color[i][2]);
127  glPushMatrix();LDEsetM(dBodyGetPosition(obj[i].boxbody),dBodyGetRotation(obj[i].boxbody));
128  DrawBox(obj[i].size[0],obj[i].size[1],obj[i].size[2],false,textured,2);glPopMatrix();
129  }
130  for (int i=0; i<S_OBJNUM; i++) {
131  glColor3d(s_color[i][0],s_color[i][1],s_color[i][2]);
132  glPushMatrix();LDEsetM(dGeomGetPosition(s_obj[i].geom[0]),dGeomGetRotation(s_obj[i].geom[0]));
133  DrawBox(s_obj[i].size[0],s_obj[i].size[1],s_obj[i].size[2],false,textured,2);glPopMatrix();
134  }
135 
136  for (int i=0; i<cylOBJNUM; i++) {
137  glColor3d(color1[i][0],color1[i][1],color1[i][2]);
138  glPushMatrix();LDEsetM(dBodyGetPosition(cyl_obj[i].cylbody),dBodyGetRotation(cyl_obj[i].cylbody));
139  DrawCylinder(cyl_obj[i].radius,cyl_obj[i].length,false,textured,2);glPopMatrix();
140  }
141 
142  for (int i=0; i<S_cylOBJNUM; i++) {
143  glColor3d(s_color1[i][0],s_color1[i][1],s_color1[i][2]);
144  glPushMatrix();LDEsetM(dGeomGetPosition(s_cyl_obj[i].cylgeom[0]),dGeomGetRotation(s_cyl_obj[i].cylgeom[0]));
145  DrawCylinder(s_cyl_obj[i].radius,s_cyl_obj[i].length,false,textured,2);glPopMatrix();
146  }
147 
148  for (int i=0; i<MODEL_NUM; i++){
149  glColor3d(1.0,1.0,1.0);
150  glPushMatrix();LDEsetM(dGeomGetPosition(ThreeD_obj[i].geom),dGeomGetRotation(ThreeD_obj[i].geom)); //DRAW THE MODEL
151  DrawX( trimesh[i], modelTexture[i]);
152  glPopMatrix();
153  }
154  for (int i=0; i<s_MODEL_NUM; i++){
155  glColor3d(1.0,1.0,1.0);
156  glPushMatrix();LDEsetM(dGeomGetPosition(s_ThreeD_obj[i].geom),dGeomGetRotation(s_ThreeD_obj[i].geom)); //DRAW THE MODEL
157  DrawX( s_trimesh[i], s_modelTexture[i]);
158  glPopMatrix();
159  }
160  for (int i=0; i<SPHNUM; i++) {
161  glColor3d(color2[i][0],color2[i][1],color2[i][2]);
162  glPushMatrix();LDEsetM(dBodyGetPosition(sph[i].sphbody),dBodyGetRotation(sph[i].sphbody));
163  DrawSphere(sph[i].radius,false,textured,2);glPopMatrix();
164  }
165  for (int i=0; i<S_SPHNUM; i++) {
166  glColor3d(s_color2[i][0],s_color2[i][1],s_color2[i][2]);
167  glPushMatrix();LDEsetM(dGeomGetPosition(s_sph[i].sphgeom[0]),dGeomGetRotation(s_sph[i].sphgeom[0]));
168  DrawSphere(s_sph[i].radius,false,textured,2);glPopMatrix();
169  }
170 }
171 
172 void worldSim::loadTexture(string texture, int numTexture){
173 
174  // yInfo() << " NUMBER TEXTURE " << numTexture;
175  string tmptext = (char *) model_DIR.c_str();
176  texture = tmptext + "/"+ texture;
177  setupTexture( (char* ) texture.c_str(), numTexture );
178 }
179 
180 void worldSim::setPosition(dReal agent1X, dReal agent1Y, dReal agent1Z ) {
181  //dBodySetPosition(tempBody,-0.0,0.2, -100);
182  //dGeomSetPosition(tempGeom[1],-0.4,0.2, -100);
183  if (actWorld == "on"){
184  dGeomSetPosition(tableGeom[0],0.3,0.25,0.3);
185  dGeomSetPosition(tableGeom[1],-0.3,0.25,0.3);
186  dGeomSetPosition(tableGeom[2],0.3,0.25,0.6);
187  dGeomSetPosition(tableGeom[3],-0.3,0.25,0.6);
188  dGeomSetPosition(tableGeom[4],0.0,0.5,0.45);
189 
190  //dBodySetPosition(Box,0.05,0.6, 0.55);
191 
192  dBodySetPosition(ballBody,-0.15,0.65, 0.35);
193  }
194 }
195 
197  ResourceFinder& finder = config.getFinder();
198 
199  Property options;
200  //start left arm device driver
201  string parts = finder.findFile("parts");
202  options.fromConfigFile(parts.c_str());
203 
204  actWorld = options.findGroup("RENDER").check("objects",Value(1),"What did the user select?").asString();
205 }
206 
207 void worldSim::init( dWorldID world, dSpaceID space, dReal X, dReal Y, dReal Z,
208  RobotConfig& config) {
209 
210  model_DIR = config.getFinder().findPath("model_path_default");
211 
212  activateWorld(config);
213  /*------------iCub Space creation-------------*/
214  /*
215  * objects in the same space do not collide...see collision function in ICub_sim
216  */
217  boxObj = dSimpleSpaceCreate(space);
218  dSpaceSetCleanup(boxObj,0);
219 
220  //tempBody = dBodyCreate (world);
221  //tempGeom[0] = dCreateBox (space,0.1,0.1,0.1);
222  //dGeomSetBody(tempGeom[0],tempBody);
223  //tempGeom[1] = dCreateBox (space,0.1,0.1,0.1);
224 
225  if (actWorld == "on"){
226  dMass m, m2;
227 
228  float ms = 1;
229  /*---------------Body creation-----------*/
230  tableGeom[0] = dCreateBox (space,0.03,0.5,0.03);
231  tableGeom[1] = dCreateBox (space,0.03,0.5,0.03);
232  tableGeom[2] = dCreateBox (space,0.03,0.5,0.03);
233  tableGeom[3] = dCreateBox (space,0.03,0.5,0.03);
234  tableGeom[4] = dCreateBox (space,0.7,0.03,0.4);
235 
236  dMassSetZero(&m);
237  dMassSetZero(&m2);
238 
239  /*Box = dBodyCreate (world);dMassSetZero(&m);dMassSetZero(&m2);
240  box_part[0] = dCreateBox (boxObj,0.01,0.01,0.1); dMassSetBoxTotal(&m2,0.01, 0.1, 0.01,0.01);
241  dGeomSetBody (box_part[0],Box);
242  dGeomSetOffsetPosition(box_part[0],-0.05-m2.c[0], -m2.c[0], -m2.c[0]);
243  dMassAdd (&m, &m2);
244 
245  box_part[1] = dCreateBox (boxObj,0.01,0.01,0.1); dMassSetBoxTotal(&m2,0.01, 0.01,0.01,0.1);
246  dGeomSetBody (box_part[1],Box);
247  dGeomSetOffsetPosition(box_part[1],0.05-m2.c[0], -m2.c[0], -m2.c[0]);
248  dMassAdd (&m, &m2);
249 
250  box_part[2] = dCreateBox (boxObj,0.1,0.01,0.01); dMassSetBoxTotal(&m2,0.01, 0.1,0.01,0.01);
251  dGeomSetBody (box_part[2],Box);
252  dGeomSetOffsetPosition(box_part[2],-m2.c[0], -m2.c[0], 0.045-m2.c[0]);
253  dMassAdd (&m, &m2);
254 
255  box_part[3] = dCreateBox (boxObj,0.1,0.01,0.01); dMassSetBoxTotal(&m2,0.01, 0.1,0.01,0.01);
256  dGeomSetBody (box_part[3],Box);
257  dGeomSetOffsetPosition(box_part[3],-m2.c[0], -m2.c[0], -0.045-m2.c[0]);
258  dMassAdd (&m, &m2);
259 
260  box_part[4] = dCreateBox (boxObj, 0.01,0.1,0.01); dMassSetBoxTotal(&m2,0.01, 0.01,0.1,0.01);
261  dGeomSetBody (box_part[4],Box);
262  dGeomSetOffsetPosition(box_part[4],-0.05-m2.c[0], 0.045-m2.c[0], -0.045-m2.c[0]);
263  dMassAdd (&m, &m2);
264 
265  box_part[5] = dCreateBox (boxObj, 0.01,0.1,0.01); dMassSetBoxTotal(&m2,0.01, 0.01,0.1,0.01);
266  dGeomSetBody (box_part[5],Box);
267  dGeomSetOffsetPosition(box_part[5],-0.05-m2.c[0], 0.045-m2.c[0], 0.045-m2.c[0]);
268  dMassAdd (&m, &m2);
269 
270  box_part[6] = dCreateBox (boxObj, 0.01,0.1,0.01); dMassSetBoxTotal(&m2,0.01, 0.01,0.1, 0.01);
271  dGeomSetBody (box_part[6],Box);
272  dGeomSetOffsetPosition(box_part[6],0.05-m2.c[0], 0.045-m2.c[0], -0.045-m2.c[0]);
273  dMassAdd (&m, &m2);
274 
275  box_part[7] = dCreateBox (boxObj, 0.01,0.1,0.01); dMassSetBoxTotal(&m2,0.01, 0.01,0.1, 0.01);
276  dGeomSetBody (box_part[7],Box);
277  dGeomSetOffsetPosition(box_part[7],0.05-m2.c[0], 0.045-m2.c[0], 0.045-m2.c[0]);
278  dMassAdd (&m, &m2);
279 
280  box_part[8] = dCreateBox (boxObj,0.01,0.01,0.1); dMassSetBoxTotal(&m2,0.015, 0.1, 0.01,0.01);
281  dGeomSetBody (box_part[8],Box);
282  dGeomSetOffsetPosition(box_part[8],-0.05-m2.c[0], 0.1-m2.c[0], -m2.c[0]);
283  dMassAdd (&m, &m2);
284 
285  box_part[9] = dCreateBox (boxObj,0.01,0.01,0.1); dMassSetBoxTotal(&m2,0.01, 0.01,0.01,0.1);
286  dGeomSetBody (box_part[9],Box);
287  dGeomSetOffsetPosition(box_part[9],0.05-m2.c[0], 0.1-m2.c[0], -m2.c[0]);
288  dMassAdd (&m, &m2);
289 
290  box_part[10] = dCreateBox (boxObj,0.1,0.01,0.01); dMassSetBoxTotal(&m2,0.01, 0.1,0.01,0.01);
291  dGeomSetBody (box_part[10],Box);
292  dGeomSetOffsetPosition(box_part[10],-m2.c[0], 0.1-m2.c[0], 0.045-m2.c[0]);
293  dMassAdd (&m, &m2);
294 
295  box_part[11] = dCreateBox (boxObj,0.1,0.01,0.01); dMassSetBoxTotal(&m2,0.01, 0.1,0.01,0.01);
296  dGeomSetBody (box_part[11],Box);
297  dGeomSetOffsetPosition(box_part[11],-m2.c[0], 0.1-m2.c[0], -0.045-m2.c[0]);
298  dMassAdd (&m, &m2);
299 
300  dBodySetMass(Box,&m);
301  */
302  /*-----------------Add independent objects to the space-------------*/
303 
304  /*---------------Independed encapsulated object position -------------*/
305 
306  dMassSetZero(&m);
307  ballBody = dBodyCreate(world);
308  dMassSetSphereTotal(&m,0.5, 0.04);
309  ballGeom = dCreateSphere (space, 0.04);
310  dGeomSetBody(ballGeom,ballBody);
311  dBodySetMass(ballBody, &m);
312  }
313  setPosition( X, Y, Z );
314 }
315 
317 
318  if (actWorld == "on"){
319  for (int i=0; i<5; i++){
320  dGeomDestroy(tableGeom[i]);
321  }
322  //for (int i=0; i<12; i++){
323  // dGeomDestroy(box_part[i]);
324  //}
325  dGeomDestroy(ballGeom);
326  }
327  dSpaceDestroy (boxObj);
328 }
329 
330 worldSim::worldSim(dWorldID world, dSpaceID space, dReal X, dReal Y, dReal Z,
331  RobotConfig& config)
332  : box_dynamic(obj,MAXNUM,OBJNUM,color),
333  box_static(s_obj,MAXNUM,S_OBJNUM,s_color),
334  cylinder_dynamic(cyl_obj,MAXNUM,cylOBJNUM,color1),
335  cylinder_static(s_cyl_obj,MAXNUM,S_cylOBJNUM,s_color1),
336  model_dynamic(ThreeD_obj,MAXNUM,MODEL_NUM,NULL),
337  model_static(s_ThreeD_obj,MAXNUM,s_MODEL_NUM,NULL),
338  sphere_dynamic(sph,MAXNUM,SPHNUM,color2),
339  sphere_static(s_sph,MAXNUM,S_SPHNUM,s_color2)
340 {
341  init(world, space, X, Y, Z, config);
342 }
343 
344 
345 // BOX
346 bool worldSim::MyObject::create(const WorldOp& op, WorldResult& result, int idx) {
347  if (!op.size.isValid()) {
348  result.setFail("size not set");
349  return false;
350  }
351  size[0] = op.size.get(0);
352  size[1] = op.size.get(1);
353  size[2] = op.size.get(2);
354  OdeInit& odeinit = OdeInit::get();
355  if (op.dynamic.get()) {
356  dMass m;
357  dMassSetZero(&m);
358  boxbody = dBodyCreate (odeinit.world);
359  dMassSetBoxTotal (&m,DENSITY,size[0],size[1],size[2]);
360  dBodySetMass (boxbody,&m);
361  }
362  if (op.collide.get())
363  geom[0] = dCreateBox (odeinit.space,size[0],size[1],size[2]);
364  else
365  geom[0] = dCreateBox (odeinit._iCub->iCub,size[0],size[1],size[2]);
366 
367  if (op.dynamic.get()) {
368  dGeomSetBody (geom[0],boxbody);
369  }
370  return true;
371 }
372 
373 // CYLINDER
374 bool worldSim::MyObject1::create(const WorldOp& op, WorldResult& result, int idx) {
375  if (!op.radius.isValid()) {
376  result.setFail("radius not set");
377  return false;
378  }
379  if (!op.length.isValid()) {
380  result.setFail("length not set");
381  return false;
382  }
383  radius = op.radius.get();
384  length = op.length.get();
385  OdeInit& odeinit = OdeInit::get();
386  if (op.dynamic.get()) {
387  dMass m;
388  dMassSetZero(&m);
389  cylbody = dBodyCreate (odeinit.world);
390  dMassSetCylinderTotal (&m,DENSITY,3,radius,length);
391  dBodySetMass (cylbody,&m);
392  }
393  if (op.collide.get())
394  cylgeom[0] = dCreateCylinder(odeinit.space,radius,length);
395  else
396  cylgeom[0] = dCreateCylinder(odeinit._iCub->iCub,radius,length);
397 
398  if (op.dynamic.get()) {
399  dGeomSetBody (cylgeom[0],cylbody);
400  }
401  return true;
402 }
403 
404 // 3D model
405 bool worldSim::MyObject2::create(const WorldOp& op, WorldResult& result, int idx) {
406  yDebug("Making a model\n");
407  if (!op.modelName.isValid()) {
408  result.setFail("model name not set");
409  return false;
410  }
411  if (!op.modelTexture.isValid()) {
412  result.setFail("model texture not set");
413  return false;
414  }
415 
416  string model = op.modelName.get();
417  OdeInit& odeinit = OdeInit::get();
418  odeinit._wrld->texture = op.modelTexture.get().c_str();
419 
420  bool dynamic = op.dynamic.get();
421  dTriMeshDataID *tridata = dynamic?odeinit._wrld->TriData:odeinit._wrld->s_TriData;
422  dTriMeshX *trimesh = dynamic?odeinit._wrld->trimesh:odeinit._wrld->s_trimesh;
423  int *modelTexture = dynamic?odeinit._wrld->modelTexture:odeinit._wrld->s_modelTexture;
424 
425  tridata[idx] = dGeomTriMeshDataCreate();
426  string tmp = (char *) odeinit._wrld->model_DIR.c_str();
427  model = string(tmp.c_str()) + "/" + model.c_str();
428  trimesh[idx] = dLoadMeshFromX(model.c_str());
429  if (!trimesh[idx]){
430  result.setFail("Check spelling and location of model file");
431  return false;
432  }
433 
434  dGeomTriMeshDataBuildSingle(tridata[idx],
435  trimesh[idx]->Vertices,
436  3 * sizeof(float),
437  trimesh[idx]->VertexCount,
438  trimesh[idx]->Indices,
439  trimesh[idx]->IndexCount,
440  3 * sizeof(int));
441  if (dynamic) {
442  body = dBodyCreate (odeinit.world);
443  }
444  if (op.collide.get())
445  geom = dCreateTriMesh(odeinit.space, tridata[idx], 0, 0, 0);
446  else
447  geom = dCreateTriMesh(odeinit._iCub->iCub, tridata[idx], 0, 0, 0);
448 
449  //geom = dCreateTriMesh(odeinit.space, tridata[idx], 0, 0, 0);
450  dGeomSetData(geom,tridata[idx]);
451  if (dynamic) {
452  dMass m;
453  dMassSetZero(&m);
454  dMassSetTrimesh(&m, 0.1, geom);
455  dGeomSetBody(geom, body);
456  dMassTranslate(&m, -m.c[0], -m.c[1], -m.c[2]);
457  dBodySetMass(body, &m);
458  }
459 
460  // this all seems incredibly dodgy
461  modelTexture[idx] = idx + 1 + (dynamic?19:49);
462  odeinit.mtxTexture.lock();
463  odeinit._wrld->static_model = !dynamic;
464  odeinit._wrld->WAITLOADING = true;
465  odeinit.mtxTexture.unlock();
466 
467  return true;
468 }
469 
470 
471 // SPHERE
472 bool worldSim::MyObject3::create(const WorldOp& op, WorldResult& result, int idx) {
473  if (!op.radius.isValid()) {
474  result.setFail("radius not set");
475  return false;
476  }
477  radius = op.radius.get();
478  OdeInit& odeinit = OdeInit::get();
479  if (op.dynamic.get()) {
480  dMass m;
481  dMassSetZero(&m);
482  sphbody = dBodyCreate(odeinit.world);
483  dMassSetSphereTotal(&m,DENSITY, radius);
484  dBodySetMass(sphbody, &m);
485  }
486  if (op.collide.get())
487  sphgeom[0] = dCreateSphere (odeinit.space, radius);
488  else
489  sphgeom[0] = dCreateSphere (odeinit._iCub->iCub, radius);
490  if (op.dynamic.get()) {
491  dGeomSetBody(sphgeom[0], sphbody);
492  }
493  return true;
494 }
495 
WorldOpScalar length
Definition: WorldOp.h:125
dxTriMeshX * dLoadMeshFromX(const char *FileName)
Definition: xloader.cpp:39
double get() const
Definition: WorldOp.h:94
virtual bool create(const WorldOp &op, WorldResult &result, int idx)
Definition: world.cpp:472
dTriMeshX s_trimesh[100]
Definition: world.h:194
static uint32_t idx[BOARD_NUM]
bool WAITLOADING
Definition: world.h:223
bool static_model
Definition: world.h:224
ICubSim * _iCub
Definition: OdeInit.h:67
void DrawSphere(float radius, bool wireframe, bool texture, int whichtexture)
Definition: rendering.cpp:329
worldSimData()
Definition: world.cpp:40
dWorldID world
Definition: OdeInit.h:58
void DrawCylinder(float radius, float length, bool wireframe, bool texture, int whichtexture)
Definition: rendering.cpp:353
void ballDamping()
Definition: world.cpp:46
void syncAngles()
Definition: world.cpp:43
double get(int offset) const
Definition: WorldOp.h:110
#define DENSITY
STL namespace.
bool get() const
Definition: WorldOp.h:72
worldSim(dWorldID world, dSpaceID space, dReal X, dReal Y, dReal Z, RobotConfig &config)
Definition: world.cpp:330
int modelTexture[100]
Definition: world.h:261
virtual yarp::os::ResourceFinder & getFinder()=0
dTriMeshDataID TriData[100]
Definition: world.h:190
~worldSim()
Definition: world.cpp:316
static dReal ballVel[3]
Definition: world.cpp:38
void DrawBox(float width, float height, float length, bool wireframe, bool texture, int whichtexture)
Definition: rendering.cpp:226
dSpaceID space
Definition: OdeInit.h:59
Header for the world creation.
static float hpr[3]
Definition: world.cpp:37
WorldOpFlag dynamic
Definition: WorldOp.h:127
WorldOpName modelName
Definition: WorldOp.h:130
static OdeInit & get()
Definition: OdeInit.cpp:189
static float xyz[3]
Definition: world.cpp:37
void LDEsetM(const dReal *pos, const dReal *R)
Definition: rendering.cpp:400
std::mutex mtxTexture
Definition: OdeInit.h:66
WorldOpTriplet size
Definition: WorldOp.h:121
This file is responsible for the initialisation of the world parameters that are controlled by ODE...
ODE state information.
Definition: OdeInit.h:55
int s_modelTexture[100]
Definition: world.h:262
WorldOpScalar radius
Definition: WorldOp.h:124
void draw()
Definition: world.cpp:62
virtual bool create(const WorldOp &op, WorldResult &result, int idx)
Definition: world.cpp:374
std::string texture
Definition: world.h:295
worldSim * _wrld
Definition: OdeInit.h:68
bool isValid() const
Definition: WorldOp.h:46
void setupTexture(char *filename, int whichtexture)
Definition: rendering.cpp:504
static dReal ballDamp[3]
Definition: world.cpp:38
dSpaceID iCub
Definition: iCub.h:88
void activateWorld(RobotConfig &config)
Definition: world.cpp:196
void setPosition(dReal agent1X, dReal agent1Z, dReal agent1Y)
Definition: world.cpp:180
std::string model_DIR
Definition: world.h:296
void DrawX(dTriMeshX trim, int whichtexture)
Definition: rendering.cpp:461
void setFail(const char *msg)
Definition: WorldOp.h:158
virtual bool create(const WorldOp &op, WorldResult &result, int idx)
Definition: world.cpp:405
WorldOpFlag collide
Definition: WorldOp.h:132
virtual bool create(const WorldOp &op, WorldResult &result, int idx)
Definition: world.cpp:346
WorldOpName modelTexture
Definition: WorldOp.h:131
#define MAXNUM
Definition: world.h:153
std::string get() const
Definition: WorldOp.h:61
void loadTexture(std::string texture, int numTexture)
Definition: world.cpp:172
dTriMeshX trimesh[100]
Definition: world.h:191
void init(dWorldID world, dSpaceID space, dReal X, dReal Y, dReal Z, RobotConfig &config)
Definition: world.cpp:207
dTriMeshDataID s_TriData[100]
Definition: world.h:193