Experiments - 1.2.0

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 {
441  // No parameters here
442 }
443 
444 void RobotOnPlane::save(ConfigurationParameters& params, QString prefix)
445 {
446  Logger::error("NOT IMPLEMENTED (RobotOnPlane::save)");
447  abort();
448 }
449 
450 void RobotOnPlane::describe(QString type)
451 {
452  Robot::describe(type);
453 
454  Descriptor d = addTypeDescription(type, "The base class for robots that lie on a plane", "The base class for robots that lie on a plane");
455 }
456 
457 RobotOnPlane::~RobotOnPlane()
458 {
459  // Nothing to do here
460 }
461 
462 void RobotOnPlane::setPosition(const Box2DWrapper* plane, const wVector& pos)
463 {
464  setPosition(plane, pos.x, pos.y);
465 }
466 
467 wVector RobotOnPlane::positionOnPlane(const Box2DWrapper* plane, real x, real y)
468 {
469  wVector pos = plane->position();
470 
471  pos.x = x;
472  pos.y = y;
473  pos.z += plane->phyObject()->sideZ() / 2.0f;
474 
475  return pos;
476 }
477 
478 void RobotOnPlane::orientationOnPlane(const Box2DWrapper* plane, real angle, wMatrix& mtr)
479 {
480  wMatrix rotatedMtr = plane->phyObject()->matrix();
481 
482  // Now rotating the matrix around the Z axis
483  rotatedMtr = rotatedMtr.rotateAround(rotatedMtr.z_ax, rotatedMtr.w_pos, angle);
484 
485  // Setting the position of the rotated matrix to be the same as the original one
486  rotatedMtr.w_pos = mtr.w_pos;
487 
488  // Now overwriting the matrix
489  mtr = rotatedMtr;
490 }
491 
492 real RobotOnPlane::angleBetweenXAxes(const wMatrix& mtr1, const wMatrix& mtr2)
493 {
494  // Taking the two x axes. We can take the x axis of mtr1 as is, while we need to project the X axis
495  // of mtr2 onto the XY plane of mtr1. To do so we simply project the x axis of mtr2 on the z axis of
496  // mtr1 and subtract this vector from the original x axis of mtr2
497  const wVector& x1 = mtr1.x_ax;
498  const wVector x2 = mtr2.x_ax - mtr1.z_ax.scale(mtr2.x_ax % mtr1.z_ax);
499 
500  // Now normalizing both axes
501  const wVector normX1 = x1.scale(1.0 / x1.norm());
502  const wVector normX2 = x2.scale(1.0 / x2.norm());
503 
504  // To get the angle (unsigned), computing the acos of the dot product of the two vectors
505  const double unsignedAngle = acos(normX1 % normX2);
506 
507  // Now choosing the right sign. To do this we first compute the cross product of the two x axes and
508  // then we see if it has the same direction of the z axis of the first matrix or not
509  const double s = mtr1.z_ax % (normX1 * normX2);
510  return (s < 0.0) ? -unsignedAngle : unsignedAngle;
511 }
512 
513 MarXbot::MarXbot(ConfigurationParameters& params, QString prefix) :
514  RobotOnPlane(params, prefix),
515  PhyMarXbot(extractWorld(params), extractRobotName(params, prefix, "marXbot"), extractRobotTranformation(params, prefix))
516 {
517  doKinematicSimulation(ConfigurationHelper::getBool(params, prefix + "kinematicRobot", false));
518 
519  const bool enableWheels = ConfigurationHelper::getBool(params, prefix + "enableWheels", true);
520  const bool enableProximityIR = ConfigurationHelper::getBool(params, prefix + "enableProximityIR", false);
521  const bool enableGroundBottomIR = ConfigurationHelper::getBool(params, prefix + "enableGroundBottomIR", false);
522  const bool enableGroundAroundIR = ConfigurationHelper::getBool(params, prefix + "enableGroundAroundIR", false);
523  const bool drawProximityIR = ConfigurationHelper::getBool(params, prefix + "drawProximityIR", false);
524  const bool drawGroundBottomIR = ConfigurationHelper::getBool(params, prefix + "drawGroundBottomIR", false);
525  const bool drawGroundAroundIR = ConfigurationHelper::getBool(params, prefix + "drawGroundAroundIR", false);
526  const bool drawIRRays =ConfigurationHelper::getBool(params, prefix + "drawIRRays", false);
527  const bool drawIRRaysRange = ConfigurationHelper::getBool(params, prefix + "drawIRRaysRange", false);
528 
529  wheelsController()->setEnabled(enableWheels);
530  proximityIRSensorController()->setEnabled(enableProximityIR);
531  setProximityIRSensorsGraphicalProperties(drawProximityIR, drawIRRays, drawIRRaysRange);
532  groundBottomIRSensorController()->setEnabled(enableGroundBottomIR);
533  setGroundBottomIRSensorsGraphicalProperties(drawGroundBottomIR, drawIRRays, drawIRRaysRange);
534  groundAroundIRSensorController()->setEnabled(enableGroundAroundIR);
535  setGroundAroundIRSensorsGraphicalProperties(drawGroundAroundIR, drawIRRays, drawIRRaysRange);
536 }
537 
538 void MarXbot::save(ConfigurationParameters& params, QString prefix) {
539  Logger::error("NOT IMPLEMENTED (MarXBot::save)");
540  abort();
541 }
542 
543 void MarXbot::describe(QString type) {
544  RobotOnPlane::describe(type);
545 
546  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");
547 
548  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");
549 
550  d.describeString("name").def("marXbot").help("The name of the MarXBot robot", "The name of the MarXBot robot");
551 
552  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");
553 
554  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");
555 
556  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");
557 
558  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");
559 
560  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");
561 
562  d.describeBool("drawProximityIR").def(false).help("If true draws the proximity IR sensors", "When set to true draws the proximity infrared sensors");
563 
564  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");
565 
566  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)");
567 
568  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");
569 
570  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)");
571 }
572 
573 MarXbot::~MarXbot() {
574  // Nothing to do here
575 }
576 
577 void MarXbot::setPosition(const Box2DWrapper* plane, real x, real y)
578 {
579  // The frame of reference of the MarXbot lies on the plane, we can simply set its position here
580  PhyMarXbot::setPosition(positionOnPlane(plane, x, y));
581 }
582 
583 wVector MarXbot::position() const
584 {
585  return matrix().w_pos;
586 }
587 
588 void MarXbot::setOrientation(const Box2DWrapper* plane, real angle)
589 {
590  wMatrix mtr = matrix();
591 
592  // Using the helper function to compute the new matrix
593  orientationOnPlane(plane, angle, mtr);
594 
595  setMatrix(mtr);
596 }
597 
598 real MarXbot::orientation(const Box2DWrapper* plane) const
599 {
600  return angleBetweenXAxes(plane->phyObject()->matrix(), matrix());
601 }
602 
603 real MarXbot::robotHeight() const
604 {
605  return PhyMarXbot::basez + PhyMarXbot::bodyh;
606 }
607 
608 real MarXbot::robotRadius() const
609 {
610  return PhyMarXbot::bodyr;
611 }
612 
613 bool MarXbot::isKinematic() const
614 {
615  return PhyMarXbot::isKinematic();
616 }
617 
618 Epuck::Epuck(ConfigurationParameters& params, QString prefix) :
619  RobotOnPlane(params, prefix),
620  PhyEpuck(extractWorld(params), extractRobotName(params, prefix, "epuck"), extractRobotTranformation(params, prefix))
621 {
622  doKinematicSimulation(ConfigurationHelper::getBool(params, prefix + "kinematicRobot", false));
623 
624  const bool enableWheels = ConfigurationHelper::getBool(params, prefix + "enableWheels", true);
625  const bool enableProximityIR = ConfigurationHelper::getBool(params, prefix + "enableProximityIR", false);
626  const bool enableGroundIR = ConfigurationHelper::getBool(params, prefix + "enableGroundIR", false);
627  const bool drawProximityIR = ConfigurationHelper::getBool(params, prefix + "drawProximityIR", false);
628  const bool drawGroundIR = ConfigurationHelper::getBool(params, prefix + "drawGroundIR", false);
629  const bool drawIRRays =ConfigurationHelper::getBool(params, prefix + "drawIRRays", false);
630  const bool drawIRRaysRange = ConfigurationHelper::getBool(params, prefix + "drawIRRaysRange", false);
631 
632  wheelsController()->setEnabled(enableWheels);
633  proximityIRSensorController()->setEnabled(enableProximityIR);
634  setProximityIRSensorsGraphicalProperties(drawProximityIR, drawIRRays, drawIRRaysRange);
635  groundIRSensorController()->setEnabled(enableGroundIR);
636  setGroundIRSensorsGraphicalProperties(drawGroundIR, drawIRRays, drawIRRaysRange);
637 }
638 
639 void Epuck::save(ConfigurationParameters& params, QString prefix) {
640  Logger::error("NOT IMPLEMENTED (Epuck::save)");
641  abort();
642 }
643 
644 void Epuck::describe(QString type) {
645  RobotOnPlane::describe(type);
646 
647  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");
648 
649  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");
650 
651  d.describeString("name").def("epuck").help("The name of the Epuck robot", "The name of the Epuck robot");
652 
653  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");
654 
655  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");
656 
657  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");
658 
659  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");
660 
661  d.describeBool("drawProximityIR").def(false).help("If true draws the proximity IR sensors", "When set to true draws the proximity IR sensors");
662 
663  d.describeBool("drawGroundIR").def(false).help("If true draws the ground IR sensors", "When set to true draws the ground IR sensors");
664 
665  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");
666 
667  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)");
668 
669 }
670 
671 Epuck::~Epuck() {
672  // Nothing to do here
673 }
674 
675 void Epuck::setPosition(const Box2DWrapper* plane, real x, real y)
676 {
677  // The frame of reference of the e-puck lies on the plane, we can simply set its position here
678  PhyEpuck::setPosition(positionOnPlane(plane, x, y));
679 }
680 
681 wVector Epuck::position() const
682 {
683  return matrix().w_pos;
684 }
685 
686 void Epuck::setOrientation(const Box2DWrapper* plane, real angle)
687 {
688  wMatrix mtr = matrix();
689 
690  // Using the helper function to compute the new matrix
691  orientationOnPlane(plane, angle, mtr);
692 
693  setMatrix(mtr);
694 }
695 
696 real Epuck::orientation(const Box2DWrapper* plane) const
697 {
698  return angleBetweenXAxes(plane->phyObject()->matrix(), matrix());
699 }
700 
701 real Epuck::robotHeight() const
702 {
703  return PhyEpuck::batteryplacedistancefromground + PhyEpuck::batteryplacez + PhyEpuck::bodyh;
704 }
705 
706 real Epuck::robotRadius() const
707 {
708  return PhyEpuck::bodyr;
709 }
710 
711 bool Epuck::isKinematic() const
712 {
713  return PhyEpuck::isKinematic();
714 }
715 
716 Khepera::Khepera(ConfigurationParameters& params, QString prefix) :
717  RobotOnPlane(params, prefix),
718  PhyKhepera(extractWorld(params), extractRobotName(params, prefix, "khepera"), extractRobotTranformation(params, prefix))
719 {
720  doKinematicSimulation(ConfigurationHelper::getBool(params, prefix + "kinematicRobot", false));
721 
722  const bool enableWheels = ConfigurationHelper::getBool(params, prefix + "enableWheels", true);
723  const bool enableProximityIR = ConfigurationHelper::getBool(params, prefix + "enableProximityIR", false);
724  const bool drawProximityIR = ConfigurationHelper::getBool(params, prefix + "drawProximityIR", false);
725  const bool drawIRRays =ConfigurationHelper::getBool(params, prefix + "drawIRRays", false);
726  const bool drawIRRaysRange = ConfigurationHelper::getBool(params, prefix + "drawIRRaysRange", false);
727 
728  wheelsController()->setEnabled(enableWheels);
729  proximityIRSensorController()->setEnabled(enableProximityIR);
730  setProximityIRSensorsGraphicalProperties(drawProximityIR, drawIRRays, drawIRRaysRange);
731 }
732 
733 void Khepera::save(ConfigurationParameters& params, QString prefix) {
734  Logger::error("NOT IMPLEMENTED (Khepera::save)");
735  abort();
736 }
737 
738 void Khepera::describe(QString type) {
739  RobotOnPlane::describe(type);
740 
741  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");
742 
743  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");
744 
745  d.describeString("name").def("khepera").help("The name of the Khepera robot", "The name of the Khepera robot");
746 
747  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");
748 
749  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");
750 
751  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");
752 
753  d.describeBool("drawProximityIR").def(false).help("If true draws the proximity IR sensors", "When set to true draws the proximity IR sensors");
754 
755  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");
756 
757  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)");
758 
759 }
760 
761 Khepera::~Khepera() {
762  // Nothing to do here
763 }
764 
765 void Khepera::setPosition(const Box2DWrapper* plane, real x, real y)
766 {
767  // The frame of reference of the e-puck lies on the plane, we can simply set its position here
768  PhyKhepera::setPosition(positionOnPlane(plane, x, y));
769 }
770 
771 wVector Khepera::position() const
772 {
773  return matrix().w_pos;
774 }
775 
776 void Khepera::setOrientation(const Box2DWrapper* plane, real angle)
777 {
778  wMatrix mtr = matrix();
779 
780  // Using the helper function to compute the new matrix
781  orientationOnPlane(plane, angle, mtr);
782 
783  setMatrix(mtr);
784 }
785 
786 real Khepera::orientation(const Box2DWrapper* plane) const
787 {
788  return angleBetweenXAxes(plane->phyObject()->matrix(), matrix());
789 }
790 
791 real Khepera::robotHeight() const
792 {
793  return PhyKhepera::bodydistancefromground + PhyKhepera::bodyh;
794 }
795 
796 real Khepera::robotRadius() const
797 {
798  return PhyKhepera::bodyr;
799 }
800 
801 bool Khepera::isKinematic() const
802 {
803  return PhyKhepera::isKinematic();
804 }
805 
806 } //end namespace farsa