Experiments - 1.4.5

Utilities Configuration GA
NNFW WorldSim Total 99% Experiments
robots.cpp
1 /********************************************************************************
2  * FARSA Experiments Library *
3  * Copyright (C) 2007-2012 *
4  * Tomassino Ferrauto <tomassino.ferrauto@istc.cnr.it> *
5  * Stefano Nolfi <stefano.nolfi@istc.cnr.it> *
6  * Onofrio Gigliotta <onofrio.gigliotta@istc.cnr.it> *
7  * Gianluca Massera <emmegian@yahoo.it> *
8  * *
9  * This program is free software; you can redistribute it and/or modify *
10  * it under the terms of the GNU General Public License as published by *
11  * the Free Software Foundation; either version 2 of the License, or *
12  * (at your option) any later version. *
13  * *
14  * This program is distributed in the hope that it will be useful, *
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
17  * GNU General Public License for more details. *
18  * *
19  * You should have received a copy of the GNU General Public License *
20  * along with this program; if not, write to the Free Software *
21  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA *
22  ********************************************************************************/
23 
24 #include "robots.h"
25 #include "configurationhelper.h"
26 #include "logger.h"
27 #include "mathutils.h"
28 #include <QStringList>
29 #include <QRegExp>
30 
31 namespace farsa {
32 
33 namespace robotConfigurationUtilities {
34  World* extractWorld(ConfigurationParameters& params)
35  {
36  SimpleResourcesUser* r = params.getResourcesUserForResource("world");
37 
38  if (r == NULL) {
39  ConfigurationHelper::throwUserMissingResourceError("world", "We need a world to create the iCub robot!");
40  }
41 
42  return r->getResource<World>("world");
43  }
44 
45  QString extractRobotName(ConfigurationParameters& params, QString prefix, QString defaultName)
46  {
47  return ConfigurationHelper::getString(params, prefix + "name", defaultName);
48  }
49 
50  wMatrix extractRobotTranformation(ConfigurationParameters& params, QString prefix)
51  {
52  QString value = ConfigurationHelper::getString(params, prefix + "transformation", QString());
53 
54  if (value.isEmpty()) {
55  return wMatrix::identity();
56  }
57 
58  // The values on the same row are separated by spaces, rows are separated by semicolons
59  QStringList rows = value.split(";");
60  if (rows.size() != 4) {
61  ConfigurationHelper::throwUserConfigError(prefix + "transformation", value, "The transformation matrix must have 4 rows. The format is: a11 a12 a13 a14; a21 a22 a23 a24; a31 a32 a33 a34; a41 a42 a43 a44");
62  }
63 
64  wMatrix mtr;
65  for (int i = 0; i < rows.size(); i++) {
66  QStringList elements = rows[i].split(QRegExp("\\s+"));
67  if (elements.size() != 4) {
68  ConfigurationHelper::throwUserConfigError(prefix + "transformation", value, "The transformation matrix must have 4 columns. The format is: a11 a12 a13 a14; a21 a22 a23 a24; a31 a32 a33 a34; a41 a42 a43 a44");
69  }
70  for (int j = 0; j < elements.size(); j++) {
71  bool ok;
72  mtr[i][j] = elements[j].toDouble(&ok);
73  if (!ok) {
74  ConfigurationHelper::throwUserConfigError(prefix + "transformation", value, "The transformation matrix elements must be numbers");
75  }
76  }
77  }
78 
79  return mtr;
80  }
81 }
82 
83 using namespace robotConfigurationUtilities;
84 
85 RobotOnPlane::RobotOnPlane(ConfigurationParameters& params, QString prefix) :
86  Robot(params, prefix),
87  m_color(ConfigurationHelper::getString(params, prefix + "color", "+FFFFFF").replace("+", "#")) // We have to do this because # is for comments in .ini files
88 {
89  if (!m_color.isValid()) {
90  ConfigurationHelper::throwUserConfigError(prefix + "color", params.getValue(prefix + "color"), "The value of the \"color\" parameter is not a valid color");
91  }
92 }
93 
94 void RobotOnPlane::save(ConfigurationParameters& /*params*/, QString /*prefix*/)
95 {
96  Logger::error("NOT IMPLEMENTED (RobotOnPlane::save)");
97  abort();
98 }
99 
100 void RobotOnPlane::describe(QString type)
101 {
102  Robot::describe(type);
103 
104  Descriptor d = addTypeDescription(type, "The base class for robots that lie on a plane", "The base class for robots that lie on a plane");
105  d.describeString("color").def("+FFFFFF").help("The color of the robot.", "This is a string. Its format can be: +RGB, +RRGGBB, +RRRGGGBBB, +RRRRGGGGBBBB (each of R, G, and B is a single hex digit) or a name from the list of colors defined in the list of SVG color keyword names provided by the World Wide Web Consortium (see http://www.w3.org/TR/SVG/types.html#ColorKeywords). The default value is \"+FFFFFF\"");
106 }
107 
108 RobotOnPlane::~RobotOnPlane()
109 {
110  // Nothing to do here
111 }
112 
113 void RobotOnPlane::setPosition(const Box2DWrapper* plane, const wVector& pos)
114 {
115  setPosition(plane, pos.x, pos.y);
116 }
117 
118 MarXbot::MarXbot(ConfigurationParameters& params, QString prefix) :
119  RobotOnPlane(params, prefix),
120  PhyMarXbot(extractWorld(params), extractRobotName(params, prefix, "marXbot"), extractRobotTranformation(params, prefix))
121 {
122  doKinematicSimulation(ConfigurationHelper::getBool(params, prefix + "kinematicRobot", false));
123 
124  const bool enableWheels = ConfigurationHelper::getBool(params, prefix + "enableWheels", true);
125  const bool enableProximityIR = ConfigurationHelper::getBool(params, prefix + "enableProximityIR", false);
126  const bool enableGroundBottomIR = ConfigurationHelper::getBool(params, prefix + "enableGroundBottomIR", false);
127  const bool enableGroundAroundIR = ConfigurationHelper::getBool(params, prefix + "enableGroundAroundIR", false);
128  const bool enableAttachDev = ConfigurationHelper::getBool(params, prefix + "enableAttachmentDevice", false);
129  const bool drawProximityIR = ConfigurationHelper::getBool(params, prefix + "drawProximityIR", false);
130  const bool drawGroundBottomIR = ConfigurationHelper::getBool(params, prefix + "drawGroundBottomIR", false);
131  const bool drawGroundAroundIR = ConfigurationHelper::getBool(params, prefix + "drawGroundAroundIR", false);
132  const bool drawIRRays =ConfigurationHelper::getBool(params, prefix + "drawIRRays", false);
133  const bool drawIRRaysRange = ConfigurationHelper::getBool(params, prefix + "drawIRRaysRange", false);
134 
135  wheelsController()->setEnabled(enableWheels);
136  proximityIRSensorController()->setEnabled(enableProximityIR);
137  setProximityIRSensorsGraphicalProperties(drawProximityIR, drawIRRays, drawIRRaysRange);
138  groundBottomIRSensorController()->setEnabled(enableGroundBottomIR);
139  setGroundBottomIRSensorsGraphicalProperties(drawGroundBottomIR, drawIRRays, drawIRRaysRange);
140  groundAroundIRSensorController()->setEnabled(enableGroundAroundIR);
141  setGroundAroundIRSensorsGraphicalProperties(drawGroundAroundIR, drawIRRays, drawIRRaysRange);
142  enableAttachmentDevice(enableAttachDev);
143 
144  QString ledColorsString = ConfigurationHelper::getString(params, prefix + "ledColors", "").replace("+", "#");
145  if (!ledColorsString.isEmpty()) {
146  // Converting all colors
147  QList<QColor> ledColors;
148  QStringList c = ledColorsString.split(" ", QString::SkipEmptyParts);
149  if (c.size() != 12) {
150  ConfigurationHelper::throwUserConfigError(prefix + "ledColors", params.getValue(prefix + "ledColors"), "The ledColors parameter must be a list of exactly 12 elements");
151  }
152  for (int i = 0; i < c.size(); ++i) {
153  const QColor curColor(c[i]);
154  if (!curColor.isValid()) {
155  ConfigurationHelper::throwUserConfigError(prefix + "ledColors", params.getValue(prefix + "ledColors"), QString("The value of the %1th color is not a valid color").arg(i));
156  }
157  ledColors << curColor;
158  }
159  // Finally setting the led colors
160  setLedColors(ledColors);
161  }
162 
163  // Setting the color of the robot
164  setColor(configuredRobotColor());
165 }
166 
167 void MarXbot::save(ConfigurationParameters& /*params*/, QString /*prefix*/) {
168  Logger::error("NOT IMPLEMENTED (MarXBot::save)");
169  abort();
170 }
171 
172 void MarXbot::describe(QString type) {
173  RobotOnPlane::describe(type);
174 
175  Descriptor d = addTypeDescription(type, "The simulated wheeled MarXBot robot in a physical world", "This type models the wheeled MarXBot robot in a physical world. Note that to create a robot we need a resource named \"world\" with a valid World instance");
176 
177  d.describeBool("kinematicRobot").def(false).help("Whether only the kinematic behaviour of the robot has to be simulated or not", "If true only the kinematic properties of the robot are simulated (i.e. velocity); is false (the default) the whole dynamic of the robot is simulated");
178 
179  d.describeString("name").def("marXbot").help("The name of the MarXBot robot", "The name of the MarXBot robot");
180 
181  d.describeString("transformation").def("").help("The initial transformation matrix for the robot", "The transformation matrix must be a 4x4 matrix. The format is: a11 a12 a13 a14; a21 a22 a23 a24; a31 a32 a33 a34; a41 a42 a43 a44");
182 
183  d.describeBool("enableWheels").def(true).help("If true enables the wheel motors", "When set to false the wheel motors are not enabled, so the robot cannot move by itself");
184 
185  d.describeBool("enableProximityIR").def(false).help("If true enables the proximity IR sensors", "When set to false the proximity infrared sensors are not enabled, thus making simulation a bit faster. To increase simulation speed try to enable only the needed parts of the robot");
186 
187  d.describeBool("enableGroundBottomIR").def(false).help("If true enables the bottom ground IR sensors", "When set to false the ground infrared sensors below the battery pack are not enabled, thus making simulation a bit faster. To increase simulation speed try to enable only the needed parts of the robot");
188 
189  d.describeBool("enableGroundAroundIR").def(false).help("If true enables the around ground IR sensors", "When set to false the ground infrared sensors on the bottom part of the base (just above the wheels) are not enabled, thus making simulation a bit faster. To increase simulation speed try to enable only the needed parts of the robot");
190 
191  d.describeBool("enableAttachmentDevice").def(false).help("If true enables the attachement device of the MarXbot", "When set to true the attachement device is enabled. This device can be used by the robot to attach to another robot. The default is false");
192 
193  d.describeBool("drawProximityIR").def(false).help("If true draws the proximity IR sensors", "When set to true draws the proximity infrared sensors");
194 
195  d.describeBool("drawGroundBottomIR").def(false).help("If true draws the bottom ground IR sensors", "When set to true draws the ground infrared sensors below the battery pack");
196 
197  d.describeBool("drawGroundAroundIR").def(false).help("If true draws the around ground IR sensors", "When set to true draws the ground infrared sensors on the bottom part of the base (just above the wheels)");
198 
199  d.describeBool("drawIRRays").def(false).help("If true draws rays of enabled IR sensors", "When set to true rays of the enabled IR sensors (i.e. proximity, ground bottom and ground around infrared sensors) are drawn");
200 
201  d.describeBool("drawIRRaysRange").def(false).help("If true rays of enabled IR sensors are shown in their real range", "When drawIRRays is true, this parameter sets whether rays are drawn in their real range (the parameter is true) or instead only their direction is shown (the parameter is false)");
202 
203  d.describeString("ledColors").def("+FFFFFF +FFFFFF +FFFFFF +FFFFFF +FFFFFF +FFFFFF +FFFFFF +FFFFFF +FFFFFF +FFFFFF +FFFFFF +FFFFFF").help("The color of the leds of the robot.", "This is a list of colors separated by white spaces. Each color can be in one of the following formats: +RGB, +RRGGBB, +RRRGGGBBB, +RRRRGGGGBBBB (each of R, G, and B is a single hex digit) or a name from the list of colors defined in the list of SVG color keyword names provided by the World Wide Web Consortium (see http://www.w3.org/TR/SVG/types.html#ColorKeywords). The list must have exactly 12 elements. The default value is white for all leds");
204 }
205 
206 MarXbot::~MarXbot() {
207  // Nothing to do here
208 }
209 
210 void MarXbot::setPosition(const Box2DWrapper* plane, real x, real y)
211 {
212  // The frame of reference of the MarXbot lies on the plane, we can simply set its position here
213  PhyMarXbot::setPosition(positionOnPlane(plane, x, y));
214 }
215 
216 wVector MarXbot::position() const
217 {
218  return matrix().w_pos;
219 }
220 
221 void MarXbot::setOrientation(const Box2DWrapper* plane, real angle)
222 {
223  wMatrix mtr = matrix();
224 
225  // Using the helper function to compute the new matrix
226  orientationOnPlane(plane, angle, mtr);
227 
228  setMatrix(mtr);
229 }
230 
231 real MarXbot::orientation(const Box2DWrapper* plane) const
232 {
233  // The -PI_GRECO / 2.0 term is due to the fact that the robot head is towards -y
234  return angleBetweenXAxes(plane->phyObject()->matrix(), matrix()) - PI_GRECO / 2.0;
235 }
236 
237 real MarXbot::robotHeight() const
238 {
239  return PhyMarXbot::basez + PhyMarXbot::bodyh;
240 }
241 
242 real MarXbot::robotRadius() const
243 {
244  return PhyMarXbot::bodyr;
245 }
246 
247 bool MarXbot::isKinematic() const
248 {
249  return PhyMarXbot::isKinematic();
250 }
251 
252 QColor MarXbot::robotColor() const
253 {
254  return PhyMarXbot::color();
255 }
256 
257 Epuck::Epuck(ConfigurationParameters& params, QString prefix) :
258  RobotOnPlane(params, prefix),
259  PhyEpuck(extractWorld(params), extractRobotName(params, prefix, "epuck"), extractRobotTranformation(params, prefix))
260 {
261  doKinematicSimulation(ConfigurationHelper::getBool(params, prefix + "kinematicRobot", false));
262 
263  const bool enableWheels = ConfigurationHelper::getBool(params, prefix + "enableWheels", true);
264  const bool enableProximityIR = ConfigurationHelper::getBool(params, prefix + "enableProximityIR", false);
265  const bool enableGroundIR = ConfigurationHelper::getBool(params, prefix + "enableGroundIR", false);
266  const bool drawProximityIR = ConfigurationHelper::getBool(params, prefix + "drawProximityIR", false);
267  const bool drawGroundIR = ConfigurationHelper::getBool(params, prefix + "drawGroundIR", false);
268  const bool drawIRRays =ConfigurationHelper::getBool(params, prefix + "drawIRRays", false);
269  const bool drawIRRaysRange = ConfigurationHelper::getBool(params, prefix + "drawIRRaysRange", false);
270 
271  wheelsController()->setEnabled(enableWheels);
272  proximityIRSensorController()->setEnabled(enableProximityIR);
273  setProximityIRSensorsGraphicalProperties(drawProximityIR, drawIRRays, drawIRRaysRange);
274  groundIRSensorController()->setEnabled(enableGroundIR);
275  setGroundIRSensorsGraphicalProperties(drawGroundIR, drawIRRays, drawIRRaysRange);
276 
277  // Setting the color of the robot
278  setColor(configuredRobotColor());
279 }
280 
281 void Epuck::save(ConfigurationParameters& /*params*/, QString /*prefix*/) {
282  Logger::error("NOT IMPLEMENTED (Epuck::save)");
283  abort();
284 }
285 
286 void Epuck::describe(QString type) {
287  RobotOnPlane::describe(type);
288 
289  Descriptor d = addTypeDescription(type, "The simulated wheeled Epuck robot in a physical world", "This type models the wheeled Epuck robot in a physical world. Note that to create a robot we need a resource named \"world\" with a valid World instance");
290 
291  d.describeBool("kinematicRobot").def(false).help("Whether only the kinematic behaviour of the robot has to be simulated or not", "If true only the kinematic properties of the robot are simulated (i.e. velocity); is false (the default) the whole dynamic of the robot is simulated");
292 
293  d.describeString("name").def("epuck").help("The name of the Epuck robot", "The name of the Epuck robot");
294 
295  d.describeString("transformation").def("").help("The initial transformation matrix for the robot", "The transformation matrix must be a 4x4 matrix. The format is: a11 a12 a13 a14; a21 a22 a23 a24; a31 a32 a33 a34; a41 a42 a43 a44");
296 
297  d.describeBool("enableWheels").def(true).help("If true enables the wheel motors", "When set to false the wheel motors are not enabled, so the robot cannot move by itself");
298 
299  d.describeBool("enableProximityIR").def(false).help("If true enables the proximity IR sensors", "When set to false the proximity infrared sensors are not enabled, thus making simulation a bit faster. To increase simulation speed try to enable only the needed parts of the robot");
300 
301  d.describeBool("enableGroundIR").def(false).help("If true enables the ground IR sensors", "When set to false the ground infrared sensors are not enabled, thus making simulation a bit faster. To increase simulation speed try to enable only the needed parts of the robot");
302 
303  d.describeBool("drawProximityIR").def(false).help("If true draws the proximity IR sensors", "When set to true draws the proximity IR sensors");
304 
305  d.describeBool("drawGroundIR").def(false).help("If true draws the ground IR sensors", "When set to true draws the ground IR sensors");
306 
307  d.describeBool("drawIRRays").def(false).help("If true draws rays of enabled IR sensors", "When set to true rays of the enabled IR sensors (i.e. proximity and ground infrared sensors) are drawn");
308 
309  d.describeBool("drawIRRaysRange").def(false).help("If true rays of enabled IR sensors are shown in their real range", "When drawIRRays is true, this parameter sets whether rays are drawn in their real range (the parameter is true) or instead only their direction is shown (the parameter is false)");
310 
311 }
312 
313 Epuck::~Epuck() {
314  // Nothing to do here
315 }
316 
317 void Epuck::setPosition(const Box2DWrapper* plane, real x, real y)
318 {
319  // The frame of reference of the e-puck lies on the plane, we can simply set its position here
320  PhyEpuck::setPosition(positionOnPlane(plane, x, y));
321 }
322 
323 wVector Epuck::position() const
324 {
325  return matrix().w_pos;
326 }
327 
328 void Epuck::setOrientation(const Box2DWrapper* plane, real angle)
329 {
330  wMatrix mtr = matrix();
331 
332  // Using the helper function to compute the new matrix
333  orientationOnPlane(plane, angle, mtr);
334 
335  setMatrix(mtr);
336 }
337 
338 real Epuck::orientation(const Box2DWrapper* plane) const
339 {
340  // The -PI_GRECO / 2.0 term is due to the fact that the robot head is towards -y
341  return angleBetweenXAxes(plane->phyObject()->matrix(), matrix()) - PI_GRECO / 2.0;
342 }
343 
344 real Epuck::robotHeight() const
345 {
346  return PhyEpuck::batteryplacedistancefromground + PhyEpuck::batteryplacez + PhyEpuck::bodyh;
347 }
348 
349 real Epuck::robotRadius() const
350 {
351  return PhyEpuck::bodyr;
352 }
353 
354 bool Epuck::isKinematic() const
355 {
356  return PhyEpuck::isKinematic();
357 }
358 
359 QColor Epuck::robotColor() const
360 {
361  return PhyEpuck::color();
362 }
363 
364 Khepera::Khepera(ConfigurationParameters& params, QString prefix) :
365  RobotOnPlane(params, prefix),
366  PhyKhepera(extractWorld(params), extractRobotName(params, prefix, "khepera"), extractRobotTranformation(params, prefix))
367 {
368  doKinematicSimulation(ConfigurationHelper::getBool(params, prefix + "kinematicRobot", false));
369 
370  const bool enableWheels = ConfigurationHelper::getBool(params, prefix + "enableWheels", true);
371  const bool enableProximityIR = ConfigurationHelper::getBool(params, prefix + "enableProximityIR", false);
372  const bool drawProximityIR = ConfigurationHelper::getBool(params, prefix + "drawProximityIR", false);
373  const bool drawIRRays = ConfigurationHelper::getBool(params, prefix + "drawIRRays", false);
374  const bool drawIRRaysRange = ConfigurationHelper::getBool(params, prefix + "drawIRRaysRange", false);
375 
376  wheelsController()->setEnabled(enableWheels);
377  proximityIRSensorController()->setEnabled(enableProximityIR);
378  setProximityIRSensorsGraphicalProperties(drawProximityIR, drawIRRays, drawIRRaysRange);
379 
380  setDrawFrontMarker(true);
381 
382  // Setting the color of the robot
383  setColor(configuredRobotColor());
384 }
385 
386 void Khepera::save(ConfigurationParameters& /*params*/, QString /*prefix*/) {
387  Logger::error("NOT IMPLEMENTED (Khepera::save)");
388  abort();
389 }
390 
391 void Khepera::describe(QString type) {
392  RobotOnPlane::describe(type);
393 
394  Descriptor d = addTypeDescription(type, "The simulated wheeled Khepera II robot in a physical world", "This type models the wheeled Khepera II robot in a physical world. Note that to create a robot we need a resource named \"world\" with a valid World instance");
395 
396  d.describeBool("kinematicRobot").def(false).help("Whether only the kinematic behaviour of the robot has to be simulated or not", "If true only the kinematic properties of the robot are simulated (i.e. velocity); is false (the default) the whole dynamic of the robot is simulated");
397 
398  d.describeString("name").def("khepera").help("The name of the Khepera robot", "The name of the Khepera robot");
399 
400  d.describeString("transformation").def("").help("The initial transformation matrix for the robot", "The transformation matrix must be a 4x4 matrix. The format is: a11 a12 a13 a14; a21 a22 a23 a24; a31 a32 a33 a34; a41 a42 a43 a44");
401 
402  d.describeBool("enableWheels").def(true).help("If true enables the wheel motors", "When set to false the wheel motors are not enabled, so the robot cannot move by itself");
403 
404  d.describeBool("enableProximityIR").def(false).help("If true enables the proximity IR sensors", "When set to false the proximity infrared sensors are not enabled, thus making simulation a bit faster. To increase simulation speed try to enable only the needed parts of the robot");
405 
406  d.describeBool("drawProximityIR").def(false).help("If true draws the proximity IR sensors", "When set to true draws the proximity IR sensors");
407 
408  d.describeBool("drawIRRays").def(false).help("If true draws rays of enabled IR sensors", "When set to true rays of the enabled IR sensors (i.e. proximity and ground infrared sensors) are drawn");
409 
410  d.describeBool("drawIRRaysRange").def(false).help("If true rays of enabled IR sensors are shown in their real range", "When drawIRRays is true, this parameter sets whether rays are drawn in their real range (the parameter is true) or instead only their direction is shown (the parameter is false)");
411 
412 }
413 
414 Khepera::~Khepera() {
415  // Nothing to do here
416 }
417 
418 void Khepera::setPosition(const Box2DWrapper* plane, real x, real y)
419 {
420  // The frame of reference of the e-puck lies on the plane, we can simply set its position here
421  PhyKhepera::setPosition(positionOnPlane(plane, x, y));
422 }
423 
424 wVector Khepera::position() const
425 {
426  return matrix().w_pos;
427 }
428 
429 void Khepera::setOrientation(const Box2DWrapper* plane, real angle)
430 {
431  wMatrix mtr = matrix();
432 
433  // Using the helper function to compute the new matrix
434  orientationOnPlane(plane, angle, mtr);
435 
436  setMatrix(mtr);
437 }
438 
439 real Khepera::orientation(const Box2DWrapper* plane) const
440 {
441  // The -PI_GRECO / 2.0 term is due to the fact that the robot head is towards -y
442  return angleBetweenXAxes(plane->phyObject()->matrix(), matrix()) - PI_GRECO / 2.0;
443 }
444 
445 real Khepera::robotHeight() const
446 {
447  return PhyKhepera::bodydistancefromground + PhyKhepera::bodyh;
448 }
449 
450 real Khepera::robotRadius() const
451 {
452  return PhyKhepera::bodyr;
453 }
454 
455 bool Khepera::isKinematic() const
456 {
457  return PhyKhepera::isKinematic();
458 }
459 
460 QColor Khepera::robotColor() const
461 {
462  return PhyKhepera::color();
463 }
464 
465 } //end namespace farsa