iCub-main
OdeWorldManager.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: Paul Fitzpatrick, Vadim Tikhanoff
6 * email: paulfitz@alum.mit.edu, 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 "OdeWorldManager.h"
22 
23 #include "OdeInit.h"
24 #include "iCub_Sim.h"
25 #include <yarp/os/LogStream.h>
26 #include <map>
27 #include <mutex>
28 
29 #define DENSITY (1.0) // density of all objects
30 
31 using namespace yarp::os;
32 using namespace std;
33 
34 class OdeLink {
35  std::mutex ODE_access;
36 public:
37  const WorldOp& op;
39  int setBody;
40  dBodyID bid;
41  dGeomID gid;
44 
45  OdeLink(const WorldOp& op, WorldResult& result) : op(op), result(result) {
46  setBody = -1;
47  object = NULL;
48  store = NULL;
49  }
50 
51  bool checkObject(bool forCreate = false);
52 
53  void doGet();
54  void doSet();
55  void doMake();
56  void doGrab();
57  void doRotate();
58  void doDelete();
59  void doColor();
60  void doNumber();
61  void apply();
62 };
63 
64 bool OdeLink::checkObject(bool justNeedKind) {
65  //op.show();
66  OdeInit& odeinit = OdeInit::get();
67 
68  bid = NULL;
69  gid = NULL;
70  store = NULL;
71  object = NULL;
72 
73  std::string kind = op.kind.get();
74 
75  if (kind=="cube") {
76  bid = odeinit._wrld->Box;
77  } else if (kind=="ball") {
78  bid = odeinit._wrld->ballBody;
79  } else if (kind=="screen") {
80  gid = odeinit._iCub->screenGeom;
81  return true;
82  } else if (kind=="hand") {
83  if (op.rightHanded.get()) {
84  if (
85  odeinit._iCub->actRHand=="on") {
86  bid = odeinit._iCub->body[11];
87  yDebug("Full left hand\n");
88  } else {
89  bid = odeinit._iCub->r_hand;
90  yDebug("slim left hand\n");
91  }
92  } else {
93  if (
94  odeinit._iCub->actLHand=="on") {
95  bid = odeinit._iCub->body[10];
96  yDebug("Full left hand\n");
97  } else {
98  bid = odeinit._iCub->l_hand;
99  yDebug("slim left hand\n");
100  }
101  }
102  }
103 
104  if (bid!=NULL) {
105  return true;
106  }
107 
108  if (!justNeedKind) {
109  if ((!bid) && !op.index.isValid()) {
110  result.setFail("object without index is not known");
111  return false;
112  }
113  }
114 
115  if (!op.dynamic.isValid()) {
116  result.setFail("do not know if object is dynamic or static");
117  return false;
118  }
119 
120  bool dynamic = op.dynamic.get();
121 
122  if (kind=="box") {
123  store = dynamic?(&odeinit._wrld->box_dynamic):(&odeinit._wrld->box_static);
124  } else if (kind=="cyl") {
125  store = dynamic?(&odeinit._wrld->cylinder_dynamic):(&odeinit._wrld->cylinder_static);
126  } else if (kind=="model") {
127  store = dynamic?(&odeinit._wrld->model_dynamic):(&odeinit._wrld->model_static);
128  } else if (kind=="sph") {
129  store = dynamic?(&odeinit._wrld->sphere_dynamic):(&odeinit._wrld->sphere_static);
130  }
131 
132  if (store==NULL) {
133  result.setFail("unknown object");
134  return false;
135  }
136 
137  if (!justNeedKind) {
138  int index = op.index.get()-1;
139  if (!store->inRange(index)) {
140  result.setFail("out of range");
141  return false;
142  }
143 
144  WorldObject& obj = store->get(index);
145  if (op.dynamic.get()) {
146  bid = obj.getBody();
147  } else {
148  gid = obj.getGeometry();
149  }
150  object = &obj;
151  }
152 
153  return true;
154 }
155 
156 
158  OdeInit& odeinit = OdeInit::get();
159 
160  if (op.parameter.get()) {
161  if (op.kind.get()=="mdir") {
162  result.path = WorldOpName(odeinit._wrld->model_DIR.c_str());
163  result.setOk();
164  return;
165  } else {
166  result.setFail("parameter not recognized");
167  return;
168  }
169  }
170 
171  if (!checkObject()) return;
172  if (bid!=NULL) {
173  lock_guard<mutex> lck(odeinit.mtx);
174  const dReal *coords = dBodyGetPosition(bid);
175  result.location = WorldOpTriplet(coords[0],coords[1],coords[2]);
176  result.setOk();
177  return;
178  }
179  if (gid!=NULL) {
180  lock_guard<mutex> lck(odeinit.mtx);
181  const dReal *coords = dGeomGetPosition(gid);
182  result.location = WorldOpTriplet(coords[0],coords[1],coords[2]);
183  result.setOk();
184  return;
185  }
186  result.setFail("no object found");
187 }
188 
190  OdeInit& odeinit = OdeInit::get();
191 
192  if (op.parameter.get()) {
193  if (op.kind.get()=="mdir") {
194  odeinit._wrld->model_DIR = op.modelName.get().c_str();
195  result.setOk();
196  return;
197  } else {
198  result.setFail("parameter not recognized");
199  return;
200  }
201  }
202 
203  if (!checkObject()) return;
204 
205  if (!op.location.isValid()) {
206  result.setFail("no location set");
207  return;
208  }
209  if (bid!=NULL) {
210  lock_guard<mutex> lck(odeinit.mtx);
211  dBodySetPosition(bid,
212  op.location.get(0),
213  op.location.get(1),
214  op.location.get(2));
215  dBodySetLinearVel(bid,0.0,0.0,0.0);
216  dBodySetAngularVel(bid,0.0,0.0,0.0);
217  result.setOk();
218  return;
219  }
220  if (gid!=NULL) {
221  lock_guard<mutex> lck(odeinit.mtx);
222  dGeomSetPosition(gid,
223  op.location.get(0),
224  op.location.get(1),
225  op.location.get(2));
226  result.setOk();
227  return;
228  }
229  result.setFail("no object found");
230 }
231 
233  if (!checkObject(true)) return;
234  if (store==NULL) {
235  result.setFail("cannot create that kind of object");
236  return;
237  }
238  OdeInit& odeinit = OdeInit::get();
239  lock_guard<mutex> lck(odeinit.mtx);
240  if (store->create(op,result)) {
241  result.setOk();
242  }
243 }
244 
246  OdeInit& odeinit = OdeInit::get();
247 
248  if (!checkObject()) return;
249 
250  if (!op.rightHanded.isValid()) {
251  result.setFail("hand not set");
252  return;
253  }
254  bool right = op.rightHanded.get();
255  bool left = !right;
256 
257  if (left && odeinit._iCub->actLHand!="off") {
258  result.setFail("left hand must be disabled, cannot use grab with fingers");
259  return;
260  }
261  if (right && odeinit._iCub->actRHand!="off") {
262  result.setFail("right hand must be disabled, cannot use grab with fingers");
263  return;
264  }
265 
266  if (!op.active.isValid()) {
267  result.setFail("activity flag not set");
268  return;
269  }
270  bool active = op.active.get();
271 
272  if (bid==NULL) {
273  result.setFail("grab requires a dynamic object");
274  return;
275  }
276 
277  lock_guard<mutex> lck(odeinit.mtx);
278  if (active) {
279  if (bid!=NULL) {
280  if (left) {
281  odeinit._iCub->grab = dJointCreateFixed(odeinit.world,0);
282  dJointAttach (odeinit._iCub->grab,odeinit._iCub->l_hand,bid);
283  dJointSetFixed(odeinit._iCub->grab);
284  }
285  if (right) {
286  odeinit._iCub->grab1 = dJointCreateFixed(odeinit.world,0);
287  dJointAttach (odeinit._iCub->grab1,odeinit._iCub->r_hand,bid);
288  dJointSetFixed(odeinit._iCub->grab1);
289  }
290  }
291  } else {
292  if (left) {
293  dJointDestroy(odeinit._iCub->grab);
294  }
295  if (right) {
296  dJointDestroy(odeinit._iCub->grab1);
297  }
298  }
299  result.setOk();
300 }
301 
302 
304  OdeInit& odeinit = OdeInit::get();
305 
306  if (!checkObject()) return;
307  if (object==NULL) {
308  result.setFail("no geometry found");
309  return;
310  }
311 
312  // We want to get the object rotation
313  if (!op.rotation.isValid()) {
314  if (bid!=NULL) {
315  lock_guard<mutex> lck(odeinit.mtx);
316  const dReal *R = dBodyGetRotation(bid);
317  result.rotation = WorldOpTriplet(atan2(R[9], R[10])*180/M_PI, asin(-R[8])*180/M_PI, atan2(R[4], R[0])*180/M_PI);
318  result.setOk();
319  return;
320  }
321  if (gid!=NULL) {
322  lock_guard<mutex> lck(odeinit.mtx);
323  const dReal *R = dGeomGetRotation(gid);
324  result.rotation = WorldOpTriplet(atan2(R[9], R[10])*180/M_PI, asin(-R[8])*180/M_PI, atan2(R[4], R[0])*180/M_PI);
325  result.setOk();
326  return;
327  }
328  result.setFail("no object found");
329  }
330  if (object==NULL) {
331  result.setFail("no geometry found");
332  return;
333  } else {
334  // We want to set the object rotation
335 
336  dMatrix3 Rtx,Rty,Rtz, Rtmp1,Rtmp2;
337 
338  double rotx = (op.rotation.get(0) * M_PI) / 180;
339  double roty = (op.rotation.get(1) * M_PI) / 180;
340  double rotz = (op.rotation.get(2) * M_PI) / 180;
341 
342  dRFromAxisAndAngle(Rtx,1,0,0,rotx);
343  dRFromAxisAndAngle(Rty,0,1,0,roty);
344  dRFromAxisAndAngle(Rtz,0,0,1,rotz);
345 
346  dMultiply0 (Rtmp1,Rty,Rtz,3,3,3);
347  dMultiply0 (Rtmp2,Rtx,Rtmp1,3,3,3);
348  lock_guard<mutex> lck(odeinit.mtx);
349  dGeomSetRotation(object->getGeometry(),Rtmp2);
350  result.setOk();
351  }
352 }
353 
355  OdeInit& odeinit = OdeInit::get();
356  if (!checkObject()) return;
357  if (op.color.isValid()) {
358  if (store->colors!=NULL) {
359  store->colors[op.index.index-1][0] = op.color.get(0);
360  store->colors[op.index.index-1][1] = op.color.get(1);
361  store->colors[op.index.index-1][2] = op.color.get(2);
362 
363  }
364  } else {
365  if (store->colors!=NULL) {
366  result.color =
367  WorldOpTriplet(store->colors[op.index.index-1][0],
368  store->colors[op.index.index-1][1],
369  store->colors[op.index.index-1][2]);
370  }
371  }
372  if (object==NULL) {
373  result.setFail("no geometry found");
374  return;
375  }
376  result.setOk();
377 }
378 
380  OdeInit& odeinit = OdeInit::get();
381 
382  if (op.kind.get() == "all") {
383  lock_guard<mutex> lck(odeinit.mtx);
384  odeinit._wrld->box_dynamic.clear();
385  odeinit._wrld->box_static.clear();
386  odeinit._wrld->cylinder_dynamic.clear();
387  odeinit._wrld->cylinder_static.clear();
388  odeinit._wrld->model_dynamic.clear();
389  odeinit._wrld->model_static.clear();
390  odeinit._wrld->sphere_dynamic.clear();
391  odeinit._wrld->sphere_static.clear();
392  result.setOk();
393  return;
394  }
395  if (!checkObject()) return;
396  if (store!=NULL) {
397  // "store" + "object" can be used to access object to be destroyed
398  }
399  result.setFail("delete operation implemented only for target: all");
400 }
401 
403  if (!checkObject(true)) return;
404  if (store==NULL) {
405  result.setFail("cannot access that kind of object");
406  return;
407  }
408  OdeInit& odeinit = OdeInit::get();
409  lock_guard<mutex> lck(odeinit.mtx);
410  int ct = store->length();
411  result.count = WorldOpIndex(ct);
412  result.setOk();
413 }
414 
416  yDebug("ODE world\n");
417  //op.show();
418  std::lock_guard<std::mutex> lck(ODE_access);
419  switch (op.cmd) {
420  case WORLD_OP_GET:
421  doGet();
422  break;
423  case WORLD_OP_SET:
424  doSet();
425  break;
426  case WORLD_OP_MK:
427  doMake();
428  break;
429  case WORLD_OP_GRAB:
430  doGrab();
431  break;
432  case WORLD_OP_ROT:
433  doRotate();
434  break;
435  case WORLD_OP_DEL:
436  doDelete();
437  break;
438  case WORLD_OP_COL:
439  doColor();
440  break;
441  case WORLD_OP_NUM:
442  doNumber();
443  break;
444  default:
445  result.setFail("unrecognized command");
446  break;
447  }
448  //result.show();
449 }
450 
451 void OdeWorldManager::apply(const WorldOp& op, WorldResult& result) {
452  OdeLink link(op,result);
453  link.apply();
454 }
WorldObject
Definition: world.h:46
WORLD_OP_GRAB
@ WORLD_OP_GRAB
Definition: WorldOp.h:33
OdeWorldManager::apply
virtual void apply(const WorldOp &op, WorldResult &result)
Definition: OdeWorldManager.cpp:451
OdeInit::world
dWorldID world
Definition: OdeInit.h:58
WorldOpIndex
Definition: WorldOp.h:75
worldSim::box_static
WorldObjectListOf< MyObject > box_static
Definition: world.h:240
doRotate
bool doRotate(ManagerState &state)
Definition: WorldManager.cpp:347
ICubSim::body
dBodyID body[50]
Definition: iCub.h:117
WorldObjectList
Definition: world.h:56
doSet
bool doSet(ManagerState &state)
Definition: WorldManager.cpp:285
iCub_Sim.h
This class controls the simulation speed using dWorldstep for "exact" calculations,...
WORLD_OP_COL
@ WORLD_OP_COL
Definition: WorldOp.h:36
ICubSim::grab1
dJointID grab1
Definition: iCub.h:206
ICubSim::screenGeom
dGeomID screenGeom
Definition: iCub.h:83
worldSim::cylinder_dynamic
WorldObjectListOf< MyObject1 > cylinder_dynamic
Definition: world.h:259
WORLD_OP_DEL
@ WORLD_OP_DEL
Definition: WorldOp.h:35
ICubSim::grab
dJointID grab
Definition: iCub.h:205
WorldObject::getBody
virtual dBodyID getBody() const =0
doGet
bool doGet(ManagerState &state)
Definition: WorldManager.cpp:265
OdeInit.h
This file is responsible for the initialisation of the world parameters that are controlled by ODE....
strain::regulation::apply
bool apply(cDownloader *down, const FullRegulation &reg)
WORLD_OP_GET
@ WORLD_OP_GET
Definition: WorldOp.h:30
worldSim::model_static
WorldObjectListOf< MyObject2 > model_static
Definition: world.h:276
OdeInit::get
static OdeInit & get()
Definition: OdeInit.cpp:189
doDelete
bool doDelete(ManagerState &state)
Definition: WorldManager.cpp:358
worldSim::sphere_static
WorldObjectListOf< MyObject3 > sphere_static
Definition: world.h:292
worldSim::ballBody
dBodyID ballBody
Definition: world.h:209
worldSim::box_dynamic
WorldObjectListOf< MyObject > box_dynamic
Definition: world.h:241
OdeWorldManager.h
WorldOpName
Definition: WorldOp.h:51
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
worldSim::sphere_dynamic
WorldObjectListOf< MyObject3 > sphere_dynamic
Definition: world.h:293
worldSim::cylinder_static
WorldObjectListOf< MyObject1 > cylinder_static
Definition: world.h:258
doNumber
bool doNumber(ManagerState &state)
Definition: WorldManager.cpp:366
WORLD_OP_MK
@ WORLD_OP_MK
Definition: WorldOp.h:32
doMake
bool doMake(ManagerState &state)
Definition: WorldManager.cpp:303
WORLD_OP_ROT
@ WORLD_OP_ROT
Definition: WorldOp.h:34
WORLD_OP_SET
@ WORLD_OP_SET
Definition: WorldOp.h:31
doColor
bool doColor(ManagerState &state)
Definition: WorldManager.cpp:273
WorldOp
Definition: WorldOp.h:114
WORLD_OP_NUM
@ WORLD_OP_NUM
Definition: WorldOp.h:37
WorldObject::getGeometry
virtual dGeomID getGeometry() const =0
doGrab
bool doGrab(ManagerState &state)
Definition: WorldManager.cpp:337
WorldOpTriplet
Definition: WorldOp.h:97
OdeInit::_wrld
worldSim * _wrld
Definition: OdeInit.h:68
WorldResult
Definition: WorldOp.h:138
ICubSim::actRHand
string actRHand
Definition: iCub.h:80
worldSim::model_dynamic
WorldObjectListOf< MyObject2 > model_dynamic
Definition: world.h:277
ICubSim::l_hand
dBodyID l_hand
Definition: iCub.h:163
OdeInit::_iCub
ICubSim * _iCub
Definition: OdeInit.h:67
worldSim::Box
dBodyID Box
Definition: world.h:215
ICubSim::r_hand
dBodyID r_hand
Definition: iCub.h:163
M_PI
#define M_PI
Definition: XSensMTx.cpp:24
ICubSim::actLHand
string actLHand
Definition: iCub.h:80
OdeInit::mtx
std::mutex mtx
Definition: OdeInit.h:65
OdeInit
ODE state information.
Definition: OdeInit.h:55
worldSim::model_DIR
std::string model_DIR
Definition: world.h:296