Experiments - 1.2.3

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 {
34  // This anonymous namespace contains helper functions to extract from the ConfigurationParameters
35  // object information needed by the PhyXxxx robot constructors
36 
37  World* extractWorld(ConfigurationParameters& params)
38  {
39  SimpleResourcesUser* r = params.getResourcesUserForResource("world");
40 
41  if (r == NULL) {
42  ConfigurationHelper::throwUserMissingResourceError("world", "We need a world to create the iCub robot!");
43  }
44 
45  return r->getResource<World>("world");
46  }
47 
48  QString extractRobotName(ConfigurationParameters& params, QString prefix, QString defaultName)
49  {
50  return ConfigurationHelper::getString(params, prefix + "name", defaultName);
51  }
52 
53  wMatrix extractRobotTranformation(ConfigurationParameters& params, QString prefix)
54  {
55  QString value = ConfigurationHelper::getString(params, prefix + "transformation", QString());
56 
57  if (value.isEmpty()) {
58  return wMatrix::identity();
59  }
60 
61  // The values on the same row are separated by spaces, rows are separated by semicolons
62  QStringList rows = value.split(";");
63  if (rows.size() != 4) {
64  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");
65  }
66 
67  wMatrix mtr;
68  for (int i = 0; i < rows.size(); i++) {
69  QStringList elements = rows[i].split(QRegExp("\\s+"));
70  if (elements.size() != 4) {
71  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");
72  }
73  for (int j = 0; j < elements.size(); j++) {
74  bool ok;
75  mtr[i][j] = elements[j].toDouble(&ok);
76  if (!ok) {
77  ConfigurationHelper::throwUserConfigError(prefix + "transformation", value, "The transformation matrix elements must be numbers");
78  }
79  }
80  }
81 
82  return mtr;
83  }
84 
85  bool extractCreateServerControlBoard(ConfigurationParameters& params, QString prefix)
86  {
87  return ConfigurationHelper::getBool(params, prefix + "createServerControlBoards", false);
88  }
89 }
90 
91 #ifdef FARSA_USE_YARP_AND_ICUB
92 iCubRobot::iCubRobot(ConfigurationParameters& params, QString prefix) :
93  Robot(params, prefix),
94  PhyiCub(extractWorld(params), extractRobotName(params, prefix, "iCub"), extractRobotTranformation(params, prefix), extractCreateServerControlBoard(params, prefix))
95 {
96  // Reading all configuration parameters (they are quite a lot...)
97  doKinematicSimulation(ConfigurationHelper::getBool(params, prefix + "kinematiciCub", false), ConfigurationHelper::getBool(params, prefix + "kinematiciCubWithDynamicLeftHand", false), ConfigurationHelper::getBool(params, prefix + "kinematiciCubWithDynamicRightHand", false));
98 
99  enableLeftKinematicHand(ConfigurationHelper::getBool(params, prefix + "kinematiciCubKinematicLeftHand", false));
100  enableRightKinematicHand(ConfigurationHelper::getBool(params, prefix + "kinematiciCubKinematicRightHand", false));
101 
102  enableLeftLeg(ConfigurationHelper::getBool(params, prefix + "enableLeftLeg", true));
103  enableRightLeg(ConfigurationHelper::getBool(params, prefix + "enableRightLeg", true));
104  enableTorso(ConfigurationHelper::getBool(params, prefix + "enableTorso", true));
105  enableCameras(false /*ConfigurationHelper::getBool(params, prefix + "enableCameras", true)*/); // Cameras don't work with multithreaded applications, disabling them
106  enableHead(ConfigurationHelper::getBool(params, prefix + "enableHead", true));
107  enableLeftArm(ConfigurationHelper::getBool(params, prefix + "enableLeftArm", true));
108  enableRightArm(ConfigurationHelper::getBool(params, prefix + "enableRightArm", true));
109 
110  blockTorso0(ConfigurationHelper::getBool(params, prefix + "blockTorso0", true));
111 
112  // Setting initial posture
113  QMap<int, real> posture;
114 
115  if (ConfigurationHelper::hasParameter(params, prefix, "init_torso_yaw")) {
116  posture[PhyiCub::torso_yaw] = ConfigurationHelper::getDouble(params, prefix + "init_torso_yaw");
117  }
118  if (ConfigurationHelper::hasParameter(params, prefix, "init_torso_roll")) {
119  posture[PhyiCub::torso_roll] = ConfigurationHelper::getDouble(params, prefix + "init_torso_roll");
120  }
121  if (ConfigurationHelper::hasParameter(params, prefix, "init_torso_pitch")) {
122  posture[PhyiCub::torso_pitch] = ConfigurationHelper::getDouble(params, prefix + "init_torso_pitch");
123  }
124  if (ConfigurationHelper::hasParameter(params, prefix, "init_left_shoulder_pitch")) {
125  posture[PhyiCub::left_shoulder_pitch] = ConfigurationHelper::getDouble(params, prefix + "init_left_shoulder_pitch");
126  }
127  if (ConfigurationHelper::hasParameter(params, prefix, "init_left_shoulder_roll")) {
128  posture[PhyiCub::left_shoulder_roll] = ConfigurationHelper::getDouble(params, prefix + "init_left_shoulder_roll");
129  }
130  if (ConfigurationHelper::hasParameter(params, prefix, "init_left_shoulder_yaw")) {
131  posture[PhyiCub::left_shoulder_yaw] = ConfigurationHelper::getDouble(params, prefix + "init_left_shoulder_yaw");
132  }
133  if (ConfigurationHelper::hasParameter(params, prefix, "init_left_elbow")) {
134  posture[PhyiCub::left_elbow] = ConfigurationHelper::getDouble(params, prefix + "init_left_elbow");
135  }
136  if (ConfigurationHelper::hasParameter(params, prefix, "init_left_wrist_prosup")) {
137  posture[PhyiCub::left_wrist_prosup] = ConfigurationHelper::getDouble(params, prefix + "init_left_wrist_prosup");
138  }
139  if (ConfigurationHelper::hasParameter(params, prefix, "init_left_wrist_pitch")) {
140  posture[PhyiCub::left_wrist_pitch] = ConfigurationHelper::getDouble(params, prefix + "init_left_wrist_pitch");
141  }
142  if (ConfigurationHelper::hasParameter(params, prefix, "init_left_wrist_yaw")) {
143  posture[PhyiCub::left_wrist_yaw] = ConfigurationHelper::getDouble(params, prefix + "init_left_wrist_yaw");
144  }
145  if (ConfigurationHelper::hasParameter(params, prefix, "init_left_hand_finger")) {
146  posture[PhyiCub::left_hand_finger] = ConfigurationHelper::getDouble(params, prefix + "init_left_hand_finger");
147  }
148  if (ConfigurationHelper::hasParameter(params, prefix, "init_left_thumb_oppose")) {
149  posture[PhyiCub::left_thumb_oppose] = ConfigurationHelper::getDouble(params, prefix + "init_left_thumb_oppose");
150  }
151  if (ConfigurationHelper::hasParameter(params, prefix, "init_left_thumb_proximal")) {
152  posture[PhyiCub::left_thumb_proximal] = ConfigurationHelper::getDouble(params, prefix + "init_left_thumb_proximal");
153  }
154  if (ConfigurationHelper::hasParameter(params, prefix, "init_left_thumb_distal")) {
155  posture[PhyiCub::left_thumb_distal] = ConfigurationHelper::getDouble(params, prefix + "init_left_thumb_distal");
156  }
157  if (ConfigurationHelper::hasParameter(params, prefix, "init_left_index_proximal")) {
158  posture[PhyiCub::left_index_proximal] = ConfigurationHelper::getDouble(params, prefix + "init_left_index_proximal");
159  }
160  if (ConfigurationHelper::hasParameter(params, prefix, "init_left_index_distal")) {
161  posture[PhyiCub::left_index_distal] = ConfigurationHelper::getDouble(params, prefix + "init_left_index_distal");
162  }
163  if (ConfigurationHelper::hasParameter(params, prefix, "init_left_middle_proximal")) {
164  posture[PhyiCub::left_middle_proximal] = ConfigurationHelper::getDouble(params, prefix + "init_left_middle_proximal");
165  }
166  if (ConfigurationHelper::hasParameter(params, prefix, "init_left_middle_distal")) {
167  posture[PhyiCub::left_middle_distal] = ConfigurationHelper::getDouble(params, prefix + "init_left_middle_distal");
168  }
169  if (ConfigurationHelper::hasParameter(params, prefix, "init_left_pinky")) {
170  posture[PhyiCub::left_pinky] = ConfigurationHelper::getDouble(params, prefix + "init_left_pinky");
171  }
172  if (ConfigurationHelper::hasParameter(params, prefix, "init_right_shoulder_pitch")) {
173  posture[PhyiCub::right_shoulder_pitch] = ConfigurationHelper::getDouble(params, prefix + "init_right_shoulder_pitch");
174  }
175  if (ConfigurationHelper::hasParameter(params, prefix, "init_right_shoulder_roll")) {
176  posture[PhyiCub::right_shoulder_roll] = ConfigurationHelper::getDouble(params, prefix + "init_right_shoulder_roll");
177  }
178  if (ConfigurationHelper::hasParameter(params, prefix, "init_right_shoulder_yaw")) {
179  posture[PhyiCub::right_shoulder_yaw] = ConfigurationHelper::getDouble(params, prefix + "init_right_shoulder_yaw");
180  }
181  if (ConfigurationHelper::hasParameter(params, prefix, "init_right_elbow")) {
182  posture[PhyiCub::right_elbow] = ConfigurationHelper::getDouble(params, prefix + "init_right_elbow");
183  }
184  if (ConfigurationHelper::hasParameter(params, prefix, "init_right_wrist_prosup")) {
185  posture[PhyiCub::right_wrist_prosup] = ConfigurationHelper::getDouble(params, prefix + "init_right_wrist_prosup");
186  }
187  if (ConfigurationHelper::hasParameter(params, prefix, "init_right_wrist_pitch")) {
188  posture[PhyiCub::right_wrist_pitch] = ConfigurationHelper::getDouble(params, prefix + "init_right_wrist_pitch");
189  }
190  if (ConfigurationHelper::hasParameter(params, prefix, "init_right_wrist_yaw")) {
191  posture[PhyiCub::right_wrist_yaw] = ConfigurationHelper::getDouble(params, prefix + "init_right_wrist_yaw");
192  }
193  if (ConfigurationHelper::hasParameter(params, prefix, "init_right_hand_finger")) {
194  posture[PhyiCub::right_hand_finger] = ConfigurationHelper::getDouble(params, prefix + "init_right_hand_finger");
195  }
196  if (ConfigurationHelper::hasParameter(params, prefix, "init_right_thumb_oppose")) {
197  posture[PhyiCub::right_thumb_oppose] = ConfigurationHelper::getDouble(params, prefix + "init_right_thumb_oppose");
198  }
199  if (ConfigurationHelper::hasParameter(params, prefix, "init_right_thumb_proximal")) {
200  posture[PhyiCub::right_thumb_proximal] = ConfigurationHelper::getDouble(params, prefix + "init_right_thumb_proximal");
201  }
202  if (ConfigurationHelper::hasParameter(params, prefix, "init_right_thumb_distal")) {
203  posture[PhyiCub::right_thumb_distal] = ConfigurationHelper::getDouble(params, prefix + "init_right_thumb_distal");
204  }
205  if (ConfigurationHelper::hasParameter(params, prefix, "init_right_index_proximal")) {
206  posture[PhyiCub::right_index_proximal] = ConfigurationHelper::getDouble(params, prefix + "init_right_index_proximal");
207  }
208  if (ConfigurationHelper::hasParameter(params, prefix, "init_right_index_distal")) {
209  posture[PhyiCub::right_index_distal] = ConfigurationHelper::getDouble(params, prefix + "init_right_index_distal");
210  }
211  if (ConfigurationHelper::hasParameter(params, prefix, "init_right_middle_proximal")) {
212  posture[PhyiCub::right_middle_proximal] = ConfigurationHelper::getDouble(params, prefix + "init_right_middle_proximal");
213  }
214  if (ConfigurationHelper::hasParameter(params, prefix, "init_right_middle_distal")) {
215  posture[PhyiCub::right_middle_distal] = ConfigurationHelper::getDouble(params, prefix + "init_right_middle_distal");
216  }
217  if (ConfigurationHelper::hasParameter(params, prefix, "init_right_pinky")) {
218  posture[PhyiCub::right_pinky] = ConfigurationHelper::getDouble(params, prefix + "init_right_pinky");
219  }
220  if (ConfigurationHelper::hasParameter(params, prefix, "init_neck_pitch")) {
221  posture[PhyiCub::neck_pitch] = ConfigurationHelper::getDouble(params, prefix + "init_neck_pitch");
222  }
223  if (ConfigurationHelper::hasParameter(params, prefix, "init_neck_roll")) {
224  posture[PhyiCub::neck_roll] = ConfigurationHelper::getDouble(params, prefix + "init_neck_roll");
225  }
226  if (ConfigurationHelper::hasParameter(params, prefix, "init_neck_yaw")) {
227  posture[PhyiCub::neck_yaw] = ConfigurationHelper::getDouble(params, prefix + "init_neck_yaw");
228  }
229  if (ConfigurationHelper::hasParameter(params, prefix, "init_eyes_tilt")) {
230  posture[PhyiCub::eyes_tilt] = ConfigurationHelper::getDouble(params, prefix + "init_eyes_tilt");
231  }
232  if (ConfigurationHelper::hasParameter(params, prefix, "init_eyes_version")) {
233  posture[PhyiCub::eyes_version] = ConfigurationHelper::getDouble(params, prefix + "init_eyes_version");
234  }
235  if (ConfigurationHelper::hasParameter(params, prefix, "init_eyes_vergence")) {
236  posture[PhyiCub::eyes_vergence] = ConfigurationHelper::getDouble(params, prefix + "init_eyes_vergence");
237  }
238  if (ConfigurationHelper::hasParameter(params, prefix, "init_left_hip_pitch")) {
239  posture[PhyiCub::left_hip_pitch] = ConfigurationHelper::getDouble(params, prefix + "init_left_hip_pitch");
240  }
241  if (ConfigurationHelper::hasParameter(params, prefix, "init_left_hip_roll")) {
242  posture[PhyiCub::left_hip_roll] = ConfigurationHelper::getDouble(params, prefix + "init_left_hip_roll");
243  }
244  if (ConfigurationHelper::hasParameter(params, prefix, "init_left_hip_yaw")) {
245  posture[PhyiCub::left_hip_yaw] = ConfigurationHelper::getDouble(params, prefix + "init_left_hip_yaw");
246  }
247  if (ConfigurationHelper::hasParameter(params, prefix, "init_left_knee")) {
248  posture[PhyiCub::left_knee] = ConfigurationHelper::getDouble(params, prefix + "init_left_knee");
249  }
250  if (ConfigurationHelper::hasParameter(params, prefix, "init_left_ankle_pitch")) {
251  posture[PhyiCub::left_ankle_pitch] = ConfigurationHelper::getDouble(params, prefix + "init_left_ankle_pitch");
252  }
253  if (ConfigurationHelper::hasParameter(params, prefix, "init_left_ankle_roll")) {
254  posture[PhyiCub::left_ankle_roll] = ConfigurationHelper::getDouble(params, prefix + "init_left_ankle_roll");
255  }
256  if (ConfigurationHelper::hasParameter(params, prefix, "init_right_hip_pitch")) {
257  posture[PhyiCub::right_hip_pitch] = ConfigurationHelper::getDouble(params, prefix + "init_right_hip_pitch");
258  }
259  if (ConfigurationHelper::hasParameter(params, prefix, "init_right_hip_roll")) {
260  posture[PhyiCub::right_hip_roll] = ConfigurationHelper::getDouble(params, prefix + "init_right_hip_roll");
261  }
262  if (ConfigurationHelper::hasParameter(params, prefix, "init_right_hip_yaw")) {
263  posture[PhyiCub::right_hip_yaw] = ConfigurationHelper::getDouble(params, prefix + "init_right_hip_yaw");
264  }
265  if (ConfigurationHelper::hasParameter(params, prefix, "init_right_knee")) {
266  posture[PhyiCub::right_knee] = ConfigurationHelper::getDouble(params, prefix + "init_right_knee");
267  }
268  if (ConfigurationHelper::hasParameter(params, prefix, "init_right_ankle_pitch")) {
269  posture[PhyiCub::right_ankle_pitch] = ConfigurationHelper::getDouble(params, prefix + "init_right_ankle_pitch");
270  }
271  if (ConfigurationHelper::hasParameter(params, prefix, "init_right_ankle_roll")) {
272  posture[PhyiCub::right_ankle_roll] = ConfigurationHelper::getDouble(params, prefix + "init_right_ankle_roll");
273  }
274 
275  if (posture.size() != 0) {
276  configurePosture(posture);
277  }
278 }
279 
280 void iCubRobot::save(ConfigurationParameters& params, QString prefix)
281 {
282  Logger::error("NOT IMPLEMENTED (iCubRobot::save)");
283  abort();
284 }
285 
286 void iCubRobot::describe(QString type)
287 {
288  Robot::describe(type);
289 
290  Descriptor d = addTypeDescription(type, "The simulated iCub robot in a physical world", "This type models the iCub robot in a physical world. Note that to create a robot we need a resource named \"world\" with a valid World instance");
291 
292  d.describeString("name").def("iCub").help("The name of the iCub robot", "The name of the iCub robot");
293 
294  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");
295 
296  d.describeBool("createServerControlBoards").def(false).help("Whether to create server control boards or not", "The server control boards are neede to control the robot through YARP. You can avoid creating them if you just plan to control the robot programmatically");
297 
298  d.describeBool("kinematiciCub").def(false).help("If true only the kinematic of the iCub robot will simulated", "When this parameter is true, the iCub is not physically simulated and will not collide with any object in the world. This modality, however, is a lot faster than normal simulation");
299 
300  d.describeBool("kinematiciCubWithDynamicLeftHand").def(false).help("If true the left hand will be dynamic even in kinematic modality", "If the iCub is kinematic and this parameter is set to true, the left hand will be dynamic. Collision of the left hand with other world objects will be simulated. Note however that this doesn't work very well");
301 
302  d.describeBool("kinematiciCubWithDynamicRightHand").def(false).help("If true the right hand will be dynamic even in kinematic modality", "If the iCub is kinematic and this parameter is set to true, the right hand will be dynamic. Collision of the right hand with other world objects will be simulated. Note however that this doesn't work very well");
303 
304  d.describeBool("kinematiciCubKinematicLeftHand").def(true).help("If true the kinematic of the left hand is simulated in kinematic mode", "When set to false the left hand kinematic is not computed. This can speed up a bit simulations that only need the arm kinematic. Note that there is no \"official\" hand kinematic for the iCub, so the results can be slightly inaccurate");
305 
306  d.describeBool("kinematiciCubKinematicRightHand").def(true).help("If true the kinematic of the right hand is simulated in kinematic mode", "When set to false the right hand kinematic is not computed. This can speed up a bit simulations that only need the arm kinematic. Note that there is no \"official\" hand kinematic for the iCub, so the results can be slightly inaccurate");
307 
308  d.describeBool("enableLeftLeg").def(true).help("If true enables simulation of the left leg", "When set to false the left leg is not simulated, making the simulation of the remaining robot parts a bit faster. To increase simulation speed try to enable only the needed parts of the robot");
309 
310  d.describeBool("enableRightLeg").def(true).help("If true enables simulation of the right leg", "When set to false the right leg is not simulated, making the simulation of the remaining robot parts a bit faster. To increase simulation speed try to enable only the needed parts of the robot");
311 
312  d.describeBool("enableTorso").def(true).help("If true enables simulation of the torso", "When set to false the torso is not simulated, making the simulation of the remaining robot parts a bit faster. To increase simulation speed try to enable only the needed parts of the robot");
313 
314  d.describeBool("enableHead").def(true).help("If true enables simulation of the head and neck", "When set to false the head and neck are not simulated, making the simulation of the remaining robot parts a bit faster. To increase simulation speed try to enable only the needed parts of the robot");
315 
316  // An enableCameras parameter is not present because the current multithread implentation of total99 is incompatible with camera simulation
317 
318  d.describeBool("enableLeftArm").def(true).help("If true enables simulation of the left arm", "When set to false the left arm is not simulated, making the simulation of the remaining robot parts a bit faster. To increase simulation speed try to enable only the needed parts of the robot");
319 
320  d.describeBool("enableRightArm").def(true).help("If true enables simulation of the right arm", "When set to false the right arm is not simulated, making the simulation of the remaining robot parts a bit faster. To increase simulation speed try to enable only the needed parts of the robot");
321 
322  d.describeBool("blockTorso0").def(true).help("If true the first element of the robot torso is blocked", "When set to true the first element of the robot torso is a static object. This way the robot cannot move or fall, just as if it was connected to a wall or floor with a fixed link");
323 
324  d.describeReal("init_torso_yaw").def(0.0).help("The initial angle for the torso_yaw joint", "This is the initial angle for the torso_yaw joint. Please note that we do not enforce joint limits, so be careful what you set.");
325 
326  d.describeReal("init_torso_roll").def(0.0).help("The initial angle for the torso_roll joint", "This is the initial angle for the torso_roll joint. Please note that we do not enforce joint limits, so be careful what you set.");
327 
328  d.describeReal("init_torso_pitch").def(0.0).help("The initial angle for the torso_pitch joint", "This is the initial angle for the torso_pitch joint. Please note that we do not enforce joint limits, so be careful what you set.");
329 
330  d.describeReal("init_left_shoulder_pitch").def(0.0).help("The initial angle for the left_shoulder_pitch joint", "This is the initial angle for the left_shoulder_pitch joint. Please note that we do not enforce joint limits, so be careful what you set.");
331 
332  d.describeReal("init_left_shoulder_roll").def(0.0).help("The initial angle for the left_shoulder_roll joint", "This is the initial angle for the left_shoulder_roll joint. Please note that we do not enforce joint limits, so be careful what you set.");
333 
334  d.describeReal("init_left_shoulder_yaw").def(0.0).help("The initial angle for the left_shoulder_yaw joint", "This is the initial angle for the left_shoulder_yaw joint. Please note that we do not enforce joint limits, so be careful what you set.");
335 
336  d.describeReal("init_left_elbow").def(0.0).help("The initial angle for the left_elbow joint", "This is the initial angle for the left_elbow joint. Please note that we do not enforce joint limits, so be careful what you set.");
337 
338  d.describeReal("init_left_wrist_prosup").def(0.0).help("The initial angle for the left_wrist_prosup joint", "This is the initial angle for the left_wrist_prosup joint. Please note that we do not enforce joint limits, so be careful what you set.");
339 
340  d.describeReal("init_left_wrist_pitch").def(0.0).help("The initial angle for the left_wrist_pitch joint", "This is the initial angle for the left_wrist_pitch joint. Please note that we do not enforce joint limits, so be careful what you set.");
341 
342  d.describeReal("init_left_wrist_yaw").def(0.0).help("The initial angle for the left_wrist_yaw joint", "This is the initial angle for the left_wrist_yaw joint. Please note that we do not enforce joint limits, so be careful what you set.");
343 
344  d.describeReal("init_left_hand_finger").def(0.0).help("The initial angle for the left_hand_finger joint", "This is the initial angle for the left_hand_finger joint. Please note that we do not enforce joint limits, so be careful what you set.");
345 
346  d.describeReal("init_left_thumb_oppose").def(0.0).help("The initial angle for the left_thumb_oppose joint", "This is the initial angle for the left_thumb_oppose joint. Please note that we do not enforce joint limits, so be careful what you set.");
347 
348  d.describeReal("init_left_thumb_proximal").def(0.0).help("The initial angle for the left_thumb_proximal joint", "This is the initial angle for the left_thumb_proximal joint. Please note that we do not enforce joint limits, so be careful what you set.");
349 
350  d.describeReal("init_left_thumb_distal").def(0.0).help("The initial angle for the left_thumb_distal joint", "This is the initial angle for the left_thumb_distal joint. Please note that we do not enforce joint limits, so be careful what you set.");
351 
352  d.describeReal("init_left_index_proximal").def(0.0).help("The initial angle for the left_index_proximal joint", "This is the initial angle for the left_index_proximal joint. Please note that we do not enforce joint limits, so be careful what you set.");
353 
354  d.describeReal("init_left_index_distal").def(0.0).help("The initial angle for the left_index_distal joint", "This is the initial angle for the left_index_distal joint. Please note that we do not enforce joint limits, so be careful what you set.");
355 
356  d.describeReal("init_left_middle_proximal").def(0.0).help("The initial angle for the left_middle_proximal joint", "This is the initial angle for the left_middle_proximal joint. Please note that we do not enforce joint limits, so be careful what you set.");
357 
358  d.describeReal("init_left_middle_distal").def(0.0).help("The initial angle for the left_middle_distal joint", "This is the initial angle for the left_middle_distal joint. Please note that we do not enforce joint limits, so be careful what you set.");
359 
360  d.describeReal("init_left_pinky").def(0.0).help("The initial angle for the left_pinky joint", "This is the initial angle for the left_pinky joint. Please note that we do not enforce joint limits, so be careful what you set.");
361 
362  d.describeReal("init_right_shoulder_pitch").def(0.0).help("The initial angle for the right_shoulder_pitch joint", "This is the initial angle for the right_shoulder_pitch joint. Please note that we do not enforce joint limits, so be careful what you set.");
363 
364  d.describeReal("init_right_shoulder_roll").def(0.0).help("The initial angle for the right_shoulder_roll joint", "This is the initial angle for the right_shoulder_roll joint. Please note that we do not enforce joint limits, so be careful what you set.");
365 
366  d.describeReal("init_right_shoulder_yaw").def(0.0).help("The initial angle for the right_shoulder_yaw joint", "This is the initial angle for the right_shoulder_yaw joint. Please note that we do not enforce joint limits, so be careful what you set.");
367 
368  d.describeReal("init_right_elbow").def(0.0).help("The initial angle for the right_elbow joint", "This is the initial angle for the right_elbow joint. Please note that we do not enforce joint limits, so be careful what you set.");
369 
370  d.describeReal("init_right_wrist_prosup").def(0.0).help("The initial angle for the right_wrist_prosup joint", "This is the initial angle for the right_wrist_prosup joint. Please note that we do not enforce joint limits, so be careful what you set.");
371 
372  d.describeReal("init_right_wrist_pitch").def(0.0).help("The initial angle for the right_wrist_pitch joint", "This is the initial angle for the right_wrist_pitch joint. Please note that we do not enforce joint limits, so be careful what you set.");
373 
374  d.describeReal("init_right_wrist_yaw").def(0.0).help("The initial angle for the right_wrist_yaw joint", "This is the initial angle for the right_wrist_yaw joint. Please note that we do not enforce joint limits, so be careful what you set.");
375 
376  d.describeReal("init_right_hand_finger").def(0.0).help("The initial angle for the right_hand_finger joint", "This is the initial angle for the right_hand_finger joint. Please note that we do not enforce joint limits, so be careful what you set.");
377 
378  d.describeReal("init_right_thumb_oppose").def(0.0).help("The initial angle for the right_thumb_oppose joint", "This is the initial angle for the right_thumb_oppose joint. Please note that we do not enforce joint limits, so be careful what you set.");
379 
380  d.describeReal("init_right_thumb_proximal").def(0.0).help("The initial angle for the right_thumb_proximal joint", "This is the initial angle for the right_thumb_proximal joint. Please note that we do not enforce joint limits, so be careful what you set.");
381 
382  d.describeReal("init_right_thumb_distal").def(0.0).help("The initial angle for the right_thumb_distal joint", "This is the initial angle for the right_thumb_distal joint. Please note that we do not enforce joint limits, so be careful what you set.");
383 
384  d.describeReal("init_right_index_proximal").def(0.0).help("The initial angle for the right_index_proximal joint", "This is the initial angle for the right_index_proximal joint. Please note that we do not enforce joint limits, so be careful what you set.");
385 
386  d.describeReal("init_right_index_distal").def(0.0).help("The initial angle for the right_index_distal joint", "This is the initial angle for the right_index_distal joint. Please note that we do not enforce joint limits, so be careful what you set.");
387 
388  d.describeReal("init_right_middle_proximal").def(0.0).help("The initial angle for the right_middle_proximal joint", "This is the initial angle for the right_middle_proximal joint. Please note that we do not enforce joint limits, so be careful what you set.");
389 
390  d.describeReal("init_right_middle_distal").def(0.0).help("The initial angle for the right_middle_distal joint", "This is the initial angle for the right_middle_distal joint. Please note that we do not enforce joint limits, so be careful what you set.");
391 
392  d.describeReal("init_right_pinky").def(0.0).help("The initial angle for the right_pinky joint", "This is the initial angle for the right_pinky joint. Please note that we do not enforce joint limits, so be careful what you set.");
393 
394  d.describeReal("init_neck_pitch").def(0.0).help("The initial angle for the neck_pitch joint", "This is the initial angle for the neck_pitch joint. Please note that we do not enforce joint limits, so be careful what you set.");
395 
396  d.describeReal("init_neck_roll").def(0.0).help("The initial angle for the neck_roll joint", "This is the initial angle for the neck_roll joint. Please note that we do not enforce joint limits, so be careful what you set.");
397 
398  d.describeReal("init_neck_yaw").def(0.0).help("The initial angle for the neck_yaw joint", "This is the initial angle for the neck_yaw joint. Please note that we do not enforce joint limits, so be careful what you set.");
399 
400  d.describeReal("init_eyes_tilt").def(0.0).help("The initial angle for the eyes_tilt joint", "This is the initial angle for the eyes_tilt joint. Please note that we do not enforce joint limits, so be careful what you set.");
401 
402  d.describeReal("init_eyes_version").def(0.0).help("The initial angle for the eyes_version joint", "This is the initial angle for the eyes_version joint. Please note that we do not enforce joint limits, so be careful what you set.");
403 
404  d.describeReal("init_eyes_vergence").def(0.0).help("The initial angle for the eyes_vergence joint", "This is the initial angle for the eyes_vergence joint. Please note that we do not enforce joint limits, so be careful what you set.");
405 
406  d.describeReal("init_left_hip_pitch").def(0.0).help("The initial angle for the left_hip_pitch joint", "This is the initial angle for the left_hip_pitch joint. Please note that we do not enforce joint limits, so be careful what you set.");
407 
408  d.describeReal("init_left_hip_roll").def(0.0).help("The initial angle for the left_hip_roll joint", "This is the initial angle for the left_hip_roll joint. Please note that we do not enforce joint limits, so be careful what you set.");
409 
410  d.describeReal("init_left_hip_yaw").def(0.0).help("The initial angle for the left_hip_yaw joint", "This is the initial angle for the left_hip_yaw joint. Please note that we do not enforce joint limits, so be careful what you set.");
411 
412  d.describeReal("init_left_knee").def(0.0).help("The initial angle for the left_knee joint", "This is the initial angle for the left_knee joint. Please note that we do not enforce joint limits, so be careful what you set.");
413 
414  d.describeReal("init_left_ankle_pitch").def(0.0).help("The initial angle for the left_ankle_pitch joint", "This is the initial angle for the left_ankle_pitch joint. Please note that we do not enforce joint limits, so be careful what you set.");
415 
416  d.describeReal("init_left_ankle_roll").def(0.0).help("The initial angle for the left_ankle_roll joint", "This is the initial angle for the left_ankle_roll joint. Please note that we do not enforce joint limits, so be careful what you set.");
417 
418  d.describeReal("init_right_hip_pitch").def(0.0).help("The initial angle for the right_hip_pitch joint", "This is the initial angle for the right_hip_pitch joint. Please note that we do not enforce joint limits, so be careful what you set.");
419 
420  d.describeReal("init_right_hip_roll").def(0.0).help("The initial angle for the right_hip_roll joint", "This is the initial angle for the right_hip_roll joint. Please note that we do not enforce joint limits, so be careful what you set.");
421 
422  d.describeReal("init_right_hip_yaw").def(0.0).help("The initial angle for the right_hip_yaw joint", "This is the initial angle for the right_hip_yaw joint. Please note that we do not enforce joint limits, so be careful what you set.");
423 
424  d.describeReal("init_right_knee").def(0.0).help("The initial angle for the right_knee joint", "This is the initial angle for the right_knee joint. Please note that we do not enforce joint limits, so be careful what you set.");
425 
426  d.describeReal("init_right_ankle_pitch").def(0.0).help("The initial angle for the right_ankle_pitch joint", "This is the initial angle for the right_ankle_pitch joint. Please note that we do not enforce joint limits, so be careful what you set.");
427 
428  d.describeReal("init_right_ankle_roll").def(0.0).help("The initial angle for the right_ankle_roll joint", "This is the initial angle for the right_ankle_roll joint. Please note that we do not enforce joint limits, so be careful what you set.");
429 }
430 
431 iCubRobot::~iCubRobot()
432 {
433  // Nothing to do here
434 }
435 
436 #endif // FARSA_USE_YARP_AND_ICUB
437 
438 RobotOnPlane::RobotOnPlane(ConfigurationParameters& params, QString prefix) :
439  Robot(params, prefix),
440  m_color(ConfigurationHelper::getString(params, prefix + "color", "+FFFFFF").replace("+", "#")) // We have to do this because # is for comments in .ini files
441 {
442  if (!m_color.isValid()) {
443  ConfigurationHelper::throwUserConfigError(prefix + "color", params.getValue(prefix + "onColor"), "The value of the \"color\" parameter is not a valid color");
444  }
445 }
446 
447 void RobotOnPlane::save(ConfigurationParameters& params, QString prefix)
448 {
449  Logger::error("NOT IMPLEMENTED (RobotOnPlane::save)");
450  abort();
451 }
452 
453 void RobotOnPlane::describe(QString type)
454 {
455  Robot::describe(type);
456 
457  Descriptor d = addTypeDescription(type, "The base class for robots that lie on a plane", "The base class for robots that lie on a plane");
458  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\"");
459 }
460 
461 RobotOnPlane::~RobotOnPlane()
462 {
463  // Nothing to do here
464 }
465 
466 void RobotOnPlane::setPosition(const Box2DWrapper* plane, const wVector& pos)
467 {
468  setPosition(plane, pos.x, pos.y);
469 }
470 
471 MarXbot::MarXbot(ConfigurationParameters& params, QString prefix) :
472  RobotOnPlane(params, prefix),
473  PhyMarXbot(extractWorld(params), extractRobotName(params, prefix, "marXbot"), extractRobotTranformation(params, prefix))
474 {
475  doKinematicSimulation(ConfigurationHelper::getBool(params, prefix + "kinematicRobot", false));
476 
477  const bool enableWheels = ConfigurationHelper::getBool(params, prefix + "enableWheels", true);
478  const bool enableProximityIR = ConfigurationHelper::getBool(params, prefix + "enableProximityIR", false);
479  const bool enableGroundBottomIR = ConfigurationHelper::getBool(params, prefix + "enableGroundBottomIR", false);
480  const bool enableGroundAroundIR = ConfigurationHelper::getBool(params, prefix + "enableGroundAroundIR", false);
481  const bool enableAttachDev = ConfigurationHelper::getBool(params, prefix + "enableAttachmentDevice", false);
482  const bool drawProximityIR = ConfigurationHelper::getBool(params, prefix + "drawProximityIR", false);
483  const bool drawGroundBottomIR = ConfigurationHelper::getBool(params, prefix + "drawGroundBottomIR", false);
484  const bool drawGroundAroundIR = ConfigurationHelper::getBool(params, prefix + "drawGroundAroundIR", false);
485  const bool drawIRRays =ConfigurationHelper::getBool(params, prefix + "drawIRRays", false);
486  const bool drawIRRaysRange = ConfigurationHelper::getBool(params, prefix + "drawIRRaysRange", false);
487 
488  wheelsController()->setEnabled(enableWheels);
489  proximityIRSensorController()->setEnabled(enableProximityIR);
490  setProximityIRSensorsGraphicalProperties(drawProximityIR, drawIRRays, drawIRRaysRange);
491  groundBottomIRSensorController()->setEnabled(enableGroundBottomIR);
492  setGroundBottomIRSensorsGraphicalProperties(drawGroundBottomIR, drawIRRays, drawIRRaysRange);
493  groundAroundIRSensorController()->setEnabled(enableGroundAroundIR);
494  setGroundAroundIRSensorsGraphicalProperties(drawGroundAroundIR, drawIRRays, drawIRRaysRange);
495  enableAttachmentDevice(enableAttachDev);
496 
497  // Setting the color of the robot
498  setColor(robotColor());
499 }
500 
501 void MarXbot::save(ConfigurationParameters& params, QString prefix) {
502  Logger::error("NOT IMPLEMENTED (MarXBot::save)");
503  abort();
504 }
505 
506 void MarXbot::describe(QString type) {
507  RobotOnPlane::describe(type);
508 
509  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");
510 
511  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");
512 
513  d.describeString("name").def("marXbot").help("The name of the MarXBot robot", "The name of the MarXBot robot");
514 
515  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");
516 
517  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");
518 
519  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");
520 
521  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");
522 
523  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");
524 
525  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");
526 
527  d.describeBool("drawProximityIR").def(false).help("If true draws the proximity IR sensors", "When set to true draws the proximity infrared sensors");
528 
529  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");
530 
531  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)");
532 
533  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");
534 
535  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)");
536 }
537 
538 MarXbot::~MarXbot() {
539  // Nothing to do here
540 }
541 
542 void MarXbot::setPosition(const Box2DWrapper* plane, real x, real y)
543 {
544  // The frame of reference of the MarXbot lies on the plane, we can simply set its position here
545  PhyMarXbot::setPosition(positionOnPlane(plane, x, y));
546 }
547 
548 wVector MarXbot::position() const
549 {
550  return matrix().w_pos;
551 }
552 
553 void MarXbot::setOrientation(const Box2DWrapper* plane, real angle)
554 {
555  wMatrix mtr = matrix();
556 
557  // Using the helper function to compute the new matrix
558  orientationOnPlane(plane, angle, mtr);
559 
560  setMatrix(mtr);
561 }
562 
563 real MarXbot::orientation(const Box2DWrapper* plane) const
564 {
565  // The -PI_GRECO / 2.0 term is due to the fact that the robot head is towards -y
566  return angleBetweenXAxes(plane->phyObject()->matrix(), matrix()) - PI_GRECO / 2.0;
567 }
568 
569 real MarXbot::robotHeight() const
570 {
571  return PhyMarXbot::basez + PhyMarXbot::bodyh;
572 }
573 
574 real MarXbot::robotRadius() const
575 {
576  return PhyMarXbot::bodyr;
577 }
578 
579 bool MarXbot::isKinematic() const
580 {
581  return PhyMarXbot::isKinematic();
582 }
583 
584 Epuck::Epuck(ConfigurationParameters& params, QString prefix) :
585  RobotOnPlane(params, prefix),
586  PhyEpuck(extractWorld(params), extractRobotName(params, prefix, "epuck"), extractRobotTranformation(params, prefix))
587 {
588  doKinematicSimulation(ConfigurationHelper::getBool(params, prefix + "kinematicRobot", false));
589 
590  const bool enableWheels = ConfigurationHelper::getBool(params, prefix + "enableWheels", true);
591  const bool enableProximityIR = ConfigurationHelper::getBool(params, prefix + "enableProximityIR", false);
592  const bool enableGroundIR = ConfigurationHelper::getBool(params, prefix + "enableGroundIR", false);
593  const bool drawProximityIR = ConfigurationHelper::getBool(params, prefix + "drawProximityIR", false);
594  const bool drawGroundIR = ConfigurationHelper::getBool(params, prefix + "drawGroundIR", false);
595  const bool drawIRRays =ConfigurationHelper::getBool(params, prefix + "drawIRRays", false);
596  const bool drawIRRaysRange = ConfigurationHelper::getBool(params, prefix + "drawIRRaysRange", false);
597 
598  wheelsController()->setEnabled(enableWheels);
599  proximityIRSensorController()->setEnabled(enableProximityIR);
600  setProximityIRSensorsGraphicalProperties(drawProximityIR, drawIRRays, drawIRRaysRange);
601  groundIRSensorController()->setEnabled(enableGroundIR);
602  setGroundIRSensorsGraphicalProperties(drawGroundIR, drawIRRays, drawIRRaysRange);
603 
604  // Setting the color of the robot
605  setColor(robotColor());
606 }
607 
608 void Epuck::save(ConfigurationParameters& params, QString prefix) {
609  Logger::error("NOT IMPLEMENTED (Epuck::save)");
610  abort();
611 }
612 
613 void Epuck::describe(QString type) {
614  RobotOnPlane::describe(type);
615 
616  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");
617 
618  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");
619 
620  d.describeString("name").def("epuck").help("The name of the Epuck robot", "The name of the Epuck robot");
621 
622  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");
623 
624  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");
625 
626  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");
627 
628  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");
629 
630  d.describeBool("drawProximityIR").def(false).help("If true draws the proximity IR sensors", "When set to true draws the proximity IR sensors");
631 
632  d.describeBool("drawGroundIR").def(false).help("If true draws the ground IR sensors", "When set to true draws the ground IR sensors");
633 
634  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");
635 
636  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)");
637 
638 }
639 
640 Epuck::~Epuck() {
641  // Nothing to do here
642 }
643 
644 void Epuck::setPosition(const Box2DWrapper* plane, real x, real y)
645 {
646  // The frame of reference of the e-puck lies on the plane, we can simply set its position here
647  PhyEpuck::setPosition(positionOnPlane(plane, x, y));
648 }
649 
650 wVector Epuck::position() const
651 {
652  return matrix().w_pos;
653 }
654 
655 void Epuck::setOrientation(const Box2DWrapper* plane, real angle)
656 {
657  wMatrix mtr = matrix();
658 
659  // Using the helper function to compute the new matrix
660  orientationOnPlane(plane, angle, mtr);
661 
662  setMatrix(mtr);
663 }
664 
665 real Epuck::orientation(const Box2DWrapper* plane) const
666 {
667  // The -PI_GRECO / 2.0 term is due to the fact that the robot head is towards -y
668  return angleBetweenXAxes(plane->phyObject()->matrix(), matrix()) - PI_GRECO / 2.0;
669 }
670 
671 real Epuck::robotHeight() const
672 {
673  return PhyEpuck::batteryplacedistancefromground + PhyEpuck::batteryplacez + PhyEpuck::bodyh;
674 }
675 
676 real Epuck::robotRadius() const
677 {
678  return PhyEpuck::bodyr;
679 }
680 
681 bool Epuck::isKinematic() const
682 {
683  return PhyEpuck::isKinematic();
684 }
685 
686 Khepera::Khepera(ConfigurationParameters& params, QString prefix) :
687  RobotOnPlane(params, prefix),
688  PhyKhepera(extractWorld(params), extractRobotName(params, prefix, "khepera"), extractRobotTranformation(params, prefix))
689 {
690  doKinematicSimulation(ConfigurationHelper::getBool(params, prefix + "kinematicRobot", false));
691 
692  const bool enableWheels = ConfigurationHelper::getBool(params, prefix + "enableWheels", true);
693  const bool enableProximityIR = ConfigurationHelper::getBool(params, prefix + "enableProximityIR", false);
694  const bool drawProximityIR = ConfigurationHelper::getBool(params, prefix + "drawProximityIR", false);
695  const bool drawIRRays =ConfigurationHelper::getBool(params, prefix + "drawIRRays", false);
696  const bool drawIRRaysRange = ConfigurationHelper::getBool(params, prefix + "drawIRRaysRange", false);
697 
698  wheelsController()->setEnabled(enableWheels);
699  proximityIRSensorController()->setEnabled(enableProximityIR);
700  setProximityIRSensorsGraphicalProperties(drawProximityIR, drawIRRays, drawIRRaysRange);
701 
702  setDrawFrontMarker(true);
703 
704  // Setting the color of the robot
705  setColor(robotColor());
706 }
707 
708 void Khepera::save(ConfigurationParameters& params, QString prefix) {
709  Logger::error("NOT IMPLEMENTED (Khepera::save)");
710  abort();
711 }
712 
713 void Khepera::describe(QString type) {
714  RobotOnPlane::describe(type);
715 
716  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");
717 
718  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");
719 
720  d.describeString("name").def("khepera").help("The name of the Khepera robot", "The name of the Khepera robot");
721 
722  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");
723 
724  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");
725 
726  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");
727 
728  d.describeBool("drawProximityIR").def(false).help("If true draws the proximity IR sensors", "When set to true draws the proximity IR sensors");
729 
730  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");
731 
732  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)");
733 
734 }
735 
736 Khepera::~Khepera() {
737  // Nothing to do here
738 }
739 
740 void Khepera::setPosition(const Box2DWrapper* plane, real x, real y)
741 {
742  // The frame of reference of the e-puck lies on the plane, we can simply set its position here
743  PhyKhepera::setPosition(positionOnPlane(plane, x, y));
744 }
745 
746 wVector Khepera::position() const
747 {
748  return matrix().w_pos;
749 }
750 
751 void Khepera::setOrientation(const Box2DWrapper* plane, real angle)
752 {
753  wMatrix mtr = matrix();
754 
755  // Using the helper function to compute the new matrix
756  orientationOnPlane(plane, angle, mtr);
757 
758  setMatrix(mtr);
759 }
760 
761 real Khepera::orientation(const Box2DWrapper* plane) const
762 {
763  // The -PI_GRECO / 2.0 term is due to the fact that the robot head is towards -y
764  return angleBetweenXAxes(plane->phyObject()->matrix(), matrix()) - PI_GRECO / 2.0;
765 }
766 
767 real Khepera::robotHeight() const
768 {
769  return PhyKhepera::bodydistancefromground + PhyKhepera::bodyh;
770 }
771 
772 real Khepera::robotRadius() const
773 {
774  return PhyKhepera::bodyr;
775 }
776 
777 bool Khepera::isKinematic() const
778 {
779  return PhyKhepera::isKinematic();
780 }
781 
782 } //end namespace farsa