WorldSim - 1.1.0

Utilities Configuration GA
NNFW WorldSim Total 99% Experiments
phykhepera.cpp
1 /********************************************************************************
2  * WorldSim -- library for robot simulations *
3  * Copyright (C) 2012-2013 *
4  * Gianluca Massera <emmegian@yahoo.it> *
5  * Fabrizio Papi <erkito87@gmail.com> *
6  * *
7  * This program is free software; you can redistribute it and/or modify *
8  * it under the terms of the GNU General Public License as published by *
9  * the Free Software Foundation; either version 2 of the License, or *
10  * (at your option) any later version. *
11  * *
12  * This program is distributed in the hope that it will be useful, *
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
15  * GNU General Public License for more details. *
16  * *
17  * You should have received a copy of the GNU General Public License *
18  * along with this program; if not, write to the Free Software *
19  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA *
20  ********************************************************************************/
21 
22 #include "phykhepera.h"
23 #include "phybox.h"
24 #include "phycylinder.h"
25 #include "phyfixed.h"
26 #include "phyballandsocket.h"
27 #include "physphere.h"
28 #include "physuspension.h"
29 #include "phycompoundobject.h"
30 #include "phycone.h"
31 #include "mathutils.h"
32 #include <cmath>
33 
34 namespace farsa {
35 
36 #ifdef __GNUC__
37  #warning NON HO TUTTE LE MISURE DEL ROBOT, ALCUNI VALORI LI HO DECISI IO
38 #endif
39 // Lengths are in meters, weights in kilograms.
40 const real PhyKhepera::bodydistancefromground = 0.0015f;
41 const real PhyKhepera::bodyr = 0.035f;
42 const real PhyKhepera::bodyh = 0.030f;
43 const real PhyKhepera::bodym = 0.066f;
44 const real PhyKhepera::wheelr = 0.015f;
45 const real PhyKhepera::wheelh = 0.003f;
46 const real PhyKhepera::wheelm = 0.005f;
47 const real PhyKhepera::axletrack = 0.055f;
48 const real PhyKhepera::passivewheelr = 0.003f;
49 const real PhyKhepera::passivewheelm = 0.002f;
50 
51 // PhyKhepera::PhyKhepera(World* world, QString name, const wMatrix& transformation) :
52 // WObject(world, name, transformation, false),
53 // m_body(NULL),
54 // m_bodyTransformation(),
55 // m_bodyInvTransformation(),
56 // m_wheels(),
57 // m_wheelsTransformation(),
58 // m_wheelJoints(),
59 // m_wheelsCtrl(NULL),
60 // m_kinematicSimulation(false),
61 // m_leftWheelVelocity(0.0f),
62 // m_rightWheelVelocity(0.0f)
63 // {
64 // // --- reference frame
65 // // X
66 // // ^
67 // // | ------
68 // // | | |
69 // // | -------------------
70 // // | | battery |
71 // // | | |
72 // // | -------------------
73 // // | | |
74 // // | ------
75 // // |
76 // // +---------------------> Y
77 // // The front of the robot is towards -Y (positive speeds of the wheel make the robot move towards -Y)
78 //
79 // // Creating a material for the khepera
80 // world->materials().createMaterial("kheperaMaterial");
81 // world->materials().setProperties("kheperaMaterial", "default", 0.0f, 0.0f, 0.01f, 0.01f, true);
82 // world->materials().createMaterial("kheperaTire");
83 // world->materials().setProperties("kheperaTire", "default", 1.2f, 0.9f, 0.01f, 0.01f, true);
84 //
85 // // Now creating the body of the khepera. It is made up of a single cylinder. We put the
86 // // frame of reference of the body (which is equal to that of the whole robot) on the ground to have
87 // // move stability and to simplify moving the robot (no need to apply displacements to put it on a
88 // // plane)
89 // {
90 // QUIQUIQUI, RICORDARSI DELLE MATRICI DI DISPLACEMENT DEL BODY
91 // wMatrix tm = wMatrix::identity();
92 // // Creating the body
93 // m_body = new PhyCylinder(bodyr, bodyh, world, "body");
94 // tm = wMatrix::yaw(toRad(90.0f));
95 // tm.w_pos.z = batteryplacedistancefromground + (bodyh / 2.0f);
96 // obj->setMatrix( tm );
97 // // Finally we create the compound object actually modelling the body
98 // m_body = new PhyCompoundObject(bodyObjs, world, "base");
99 // m_body->setMass(batterym + wholebodym);
100 // m_body->setMaterial("epuckMaterial");
101 // m_body->setOwner(this, false);
102 // m_body->setMatrix(transformation);
103 // }
104 //
105 // // Creating all the wheels. We first create the two motorized wheels and then the two passive ones.
106 // // The position of the wheels and their indexes are as follows (remember that the front of the
107 // // robot is towards -Y):
108 // //
109 // // X
110 // // ^
111 // // | ------
112 // // | | 0 |
113 // // | -------------------
114 // // | | battery |
115 // // | | 3 2 |
116 // // | -------------------
117 // // | | 1 |
118 // // | ------
119 // // |
120 // // +---------------------> Y
121 //
122 // // Creating the first motorized wheel and its joint
123 // {
124 // // The matrix is relative to the base. First creating the wheel
125 // wMatrix tm = wMatrix::identity();
126 // tm.w_pos = wVector(axletrack / 2.0f, 0.0f, wheelr);
127 // PhyObject* wheel = new PhyCylinder(wheelr, wheelh, world, "motorWheel");
128 // wheel->setMass(wheelm);
129 // wheel->setColor(Qt::blue);
130 // wheel->setMaterial("epuckTire");
131 // wheel->setOwner(this, false);
132 // wheel->setMatrix(tm * m_body->matrix());
133 // m_wheels.append(wheel);
134 // m_wheelsTransformation.append(tm);
135 //
136 // // Now creating the joint
137 // PhyJoint* joint = new PhySuspension(m_body->matrix().x_ax, wheel->matrix().w_pos, m_body->matrix().z_ax, m_body, wheel);
138 // joint->dofs()[0]->disableLimits();
139 // joint->setOwner(this, false);
140 // joint->updateJointInfo();
141 // m_wheelJoints.append(joint);
142 // }
143 //
144 // // Creating the second motorized wheel and its joint
145 // {
146 // // The matrix is relative to the base. First creating the wheel
147 // wMatrix tm = wMatrix::identity();
148 // tm.w_pos = wVector(-axletrack / 2.0f, 0.0f, wheelr);
149 // PhyObject* wheel = new PhyCylinder(wheelr, wheelh, world, "motorWheel");
150 // wheel->setMass(wheelm);
151 // wheel->setColor(Qt::blue);
152 // wheel->setMaterial("epuckTire");
153 // wheel->setOwner(this, false);
154 // wheel->setMatrix(tm * m_body->matrix());
155 // m_wheels.append(wheel);
156 // m_wheelsTransformation.append(tm);
157 //
158 // // Now creating the joint
159 // PhyJoint* joint = new PhySuspension(m_body->matrix().x_ax, wheel->matrix().w_pos, m_body->matrix().z_ax, m_body, wheel);
160 // joint->dofs()[0]->disableLimits();
161 // joint->setOwner(this, false);
162 // joint->updateJointInfo();
163 // m_wheelJoints.append(joint);
164 // }
165 //
166 // // Creating the first passive wheel and its joint
167 // {
168 // // The matrix is relative to the base. First creating the wheel
169 // wMatrix tm = wMatrix::identity();
170 // tm.w_pos = wVector(0.0f, (batteryplacey / 2.0f) - passivewheelr, passivewheelr);
171 // PhyObject* wheel = new PhySphere(passivewheelr, world, "passiveWheel");
172 // wheel->setMass(passivewheelm);
173 // wheel->setColor(Qt::blue);
174 // wheel->setMaterial("epuckTire");
175 // wheel->setOwner(this, false);
176 // wheel->setMatrix(tm * m_body->matrix());
177 // m_wheels.append(wheel);
178 // m_wheelsTransformation.append(tm);
179 //
180 // // Now creating the joint
181 // PhyJoint* joint = new PhyBallAndSocket(wheel->matrix().w_pos, m_body, wheel);
182 // joint->setOwner(this, false);
183 // joint->updateJointInfo();
184 // m_wheelJoints.append(joint);
185 // }
186 //
187 // // Creating the second passive wheel and its joint
188 // {
189 // // The matrix is relative to the base. First creating the wheel
190 // wMatrix tm = wMatrix::identity();
191 // tm.w_pos = wVector(0.0f, -((batteryplacey / 2.0f) - passivewheelr), passivewheelr);
192 // PhyObject* wheel = new PhySphere(passivewheelr, world, "passiveWheel");
193 // wheel->setMass(passivewheelm);
194 // wheel->setColor(Qt::blue);
195 // wheel->setMaterial("epuckTire");
196 // wheel->setOwner(this, false);
197 // wheel->setMatrix(tm * m_body->matrix());
198 // m_wheels.append(wheel);
199 // m_wheelsTransformation.append(tm);
200 //
201 // // Now creating the joint
202 // PhyJoint* joint = new PhyBallAndSocket(wheel->matrix().w_pos, m_body, wheel);
203 // joint->setOwner(this, false);
204 // joint->updateJointInfo();
205 // m_wheelJoints.append(joint);
206 // }
207 //
208 // // Now we can create the motor controller
209 // QVector<PhyDOF*> motors;
210 // motors << m_wheelJoints[0]->dofs()[0] << m_wheelJoints[1]->dofs()[0];
211 // m_wheelsCtrl = new WheelMotorController(motors, world);
212 // m_wheelsCtrl->setOwner(this, false);
213 // // The min and max speed in rad/s have been derived for the e-puck wheel considering as maximum linear
214 // // displacement 14 cm/s (that for the give wheel radius is approximately 1.1 ratations per second)
215 // const real maxAngularSpeed = 1.1 * 2.0 * PI_GRECO;
216 // m_wheelsCtrl->setSpeedLimits(-maxAngularSpeed, -maxAngularSpeed, maxAngularSpeed, maxAngularSpeed);
217 //
218 // // Connecting wheels speed signals to be able to move the robot when in kinematic
219 // connect(m_wheelJoints[0]->dofs()[0], SIGNAL(changedDesiredVelocity(real)), this, SLOT(setRightWheelDesideredVelocity(real)));
220 // connect(m_wheelJoints[1]->dofs()[0], SIGNAL(changedDesiredVelocity(real)), this, SLOT(setLeftWheelDesideredVelocity(real)));
221 //
222 // // Creating the proximity IR sensors
223 // QVector<SingleIR> sensors;
224 //
225 // #ifdef __GNUC__
226 // #warning QUI DECIDERE PER I RANGE/APERTURE E LE POSIZIONI
227 // #endif
228 // // Adding the sensors. The epuck has 8 proximity infrared sensors
229 // for (unsigned int i = 0; i < 8; i++) {
230 // const double curAngle = double(i) * (360.0 / 8.0) + ((360.0 / 8.0) / 2.0);
231 // const double radius = bodyr;
232 //
233 // wMatrix mtr = wMatrix::yaw(deg2rad(curAngle + 90.0)) * wMatrix::pitch(deg2rad(90.0f));
234 // mtr.w_pos.x = radius * cos(deg2rad(curAngle));
235 // mtr.w_pos.y = radius * sin(deg2rad(curAngle));
236 // mtr.w_pos.z = batteryplacez + batteryplacedistancefromground;
237 //
238 // sensors.append(SingleIR(m_body, mtr, 0.01, 0.05, 10.0, 5));
239 // }
240 // m_proximityIR = new SimulatedIRProximitySensorController(world, sensors);
241 //
242 // // Now creating the three ground IR sensors
243 // sensors.clear();
244 // wMatrix mtr = wMatrix::yaw(deg2rad(180.0));
245 //
246 // // Adding the sensors. The e-puck has 3 ground infrared sensors in the front part
247 // mtr.w_pos = wVector(-batteryplacex / 2.0f, -batteryplacey / 2.0f, batteryplacedistancefromground);
248 // sensors.append(SingleIR(m_body, mtr, 0.0, mtr.w_pos.z + 0.005, 0.0, 1));
249 // mtr.w_pos = wVector(0.0f, -batteryplacey / 2.0f, batteryplacedistancefromground);
250 // sensors.append(SingleIR(m_body, mtr, 0.0, mtr.w_pos.z + 0.005, 0.0, 1));
251 // mtr.w_pos = wVector(batteryplacex / 2.0f, -batteryplacey / 2.0f, batteryplacedistancefromground);
252 // sensors.append(SingleIR(m_body, mtr, 0.0, mtr.w_pos.z + 0.005, 0.0, 1));
253 // m_groundIR = new SimulatedIRGroundSensorController(world, sensors);
254 //
255 // world->pushObject(this);
256 // }
257 
258 
259 
260 
261 
262 
263 
264 
265 
266 
267 
268 
269 
270 
271 
272 // PhyEpuck::~PhyEpuck()
273 // {
274 // foreach (PhyJoint* j, m_wheelJoints) {
275 // delete j;
276 // }
277 // foreach (PhyObject* w, m_wheels) {
278 // delete w;
279 // }
280 // delete m_wheelsCtrl;
281 // delete m_body;
282 // delete m_proximityIR;
283 // delete m_groundIR;
284 // }
285 //
286 // void PhyEpuck::preUpdate()
287 // {
288 // // Updating motors
289 // if (m_wheelsCtrl->isEnabled()) {
290 // m_wheelsCtrl->update();
291 // }
292 //
293 // if (m_kinematicSimulation) {
294 // // In kinematic mode, we have to manually move the robot depending on the wheel velocities
295 // wMatrix mtr = matrix();
296 //
297 // // To compute the actual movement of the robot I assume the movement is on the plane on which
298 // // the two wheels lie. This means that I assume that both wheels are in contact with a plane
299 // // at every time. Then I compute the instant centre of rotation and move the robot rotating
300 // // it around the axis passing through the instant center of rotation and perpendicular to the
301 // // plane on which the wheels lie (i.e. around the local XY plane)
302 //
303 // // First of all computing the linear speed of the two wheels
304 // const real leftSpeed = m_leftWheelVelocity * wheelr;
305 // const real rightSpeed = m_rightWheelVelocity * wheelr;
306 //
307 // // If the speed of the two wheels is very similar, simply moving the robot forward (doing the
308 // // computations would probably lead to invalid matrices)
309 // if (fabs(leftSpeed - rightSpeed) < 0.00001f) {
310 // // The front of the robot is towards -y
311 // mtr.w_pos += -mtr.y_ax.scale(rightSpeed * world()->timeStep());
312 // } else {
313 // // The first thing to do is to compute the instant centre of rotation. We do the
314 // // calculation in the robot local frame of reference. We proceed as follows (with
315 // // uppercase letters we indicate vectors and points):
316 // //
317 // // +------------------------->
318 // // | X axis
319 // // | ^
320 // // | |\
321 // // | | \A
322 // // | | \
323 // // | L| ^
324 // // | | |\
325 // // | | | \
326 // // | | R| \
327 // // | | | \
328 // // | O+--->----+C
329 // // | D
330 // // |
331 // // |
332 // // v Y axis (the robot moves forward towards -Y)
333 // //
334 // // In the picture L and R are the velocity vectors of the two wheels, D is the vector
335 // // going from the center of the left wheel to the center of the right wheel, A is the
336 // // vector going from the tip of L to the tip of R and C is the instant centre of
337 // // rotation. D is fixed an parallel to the X axis, while L and R are always parallel
338 // // to the Y axis. Also, for the moment, we take the frame of reference origin at O.
339 // // The construction shown in the picture allows to find the instant center of rotation
340 // // for any L and R except when they are equal. In this case C is "at infinite" and the
341 // // robot moves forward of backward without rotation. All the vectors lie on the local
342 // // XY plane. To find C we proceed in the following way. First of all we note that
343 // // A = R - L. If a and b are two real numbers, then we can say that C is at the
344 // // instersection of L + aA with bD, that is we need a and b such that:
345 // // L + aA = bD.
346 // // If we take the cross product on both sides we get:
347 // // (L + aA) x D = bD x D => (L + aA) x D = 0
348 // // This means that we need to find a such that L + aA and D are parallel. As D is parallel
349 // // to the X axis, its Y component is 0, so we can impose that L + aA has a 0 Y component,
350 // // too, that is:
351 // // (Ly) + a(Ay) = 0 => a = -(Ly) / (Ay)
352 // // Once we have a, we can easily compute C:
353 // // C = L + aA
354 // // In the following code we use the same names we have used in the description above. Also
355 // // note that the actual origin of the frame of reference is not in O: it is in the middle
356 // // between the left and right wheel (we need to take this into account when computing the
357 // // point around which the robot rotates)
358 // const wVector L(0.0, -leftSpeed, 0.0);
359 // const wVector A(axletrack, -(rightSpeed - leftSpeed), 0.0);
360 // const real a = -(L.y / A.y);
361 // const wVector C = L + A.scale(a);
362 //
363 // // Now we have to compute the angular velocity of the robot. This is simply equal to v/r where
364 // // v is the linear velocity at a point that is distant r from C. We use the velocity of one of
365 // // the wheel: which weels depends on its distance from C. We take the wheel which is furthest from
366 // // C to avoid having r = 0 (which would lead to an invalid value of the angular velocity). Distances
367 // // are signed (they are taken along the X axis)
368 // const real distLeftWheel = -C.x;
369 // const real distRightWheel = -(C.x - A.x); // A.x is equal to D.x
370 // const real angularVelocity = (fabs(distLeftWheel) > fabs(distRightWheel)) ? (-leftSpeed / distLeftWheel) : (-rightSpeed / distRightWheel);
371 //
372 // // The angular displacement is simply the angular velocity multiplied by the timeStep. Here we
373 // // also compute the center of rotation in world coordinates (we also need to compute it in the frame
374 // // of reference centered between the wheels, not in O) and the axis of rotation (that is simply the
375 // // z axis of the robot in world coordinates)
376 // const real angularDisplacement = angularVelocity * world()->timeStep();
377 // const wVector centerOfRotation = matrix().transformVector(C - wVector(axletrack / 2.0, 0.0, 0.0));
378 // const wVector axisOfRotation = matrix().z_ax;
379 //
380 // // Rotating the robot transformation matrix to obtain the new position
381 // mtr = mtr.rotateAround(axisOfRotation, centerOfRotation, angularDisplacement);
382 // }
383 //
384 // // This will also update the position of all pieces
385 // setMatrix(mtr);
386 // }
387 // }
388 //
389 // void PhyEpuck::postUpdate()
390 // {
391 // // Updating sensors
392 // if (m_proximityIR->isEnabled()) {
393 // m_proximityIR->update();
394 // }
395 // if (m_groundIR->isEnabled()) {
396 // m_groundIR->update();
397 // }
398 //
399 // // Updating the transformation matrix of the robot. It is coincident with the matrix of the body
400 // tm = m_body->matrix();
401 // }
402 //
403 // void PhyEpuck::setProximityIRSensorsGraphicalProperties(bool drawSensor, bool drawRay, bool drawRealRay)
404 // {
405 // m_proximityIR->setGraphicalProperties(drawSensor, drawRay, drawRealRay);
406 // }
407 //
408 // void PhyEpuck::setGroundIRSensorsGraphicalProperties(bool drawSensor, bool drawRay, bool drawRealRay)
409 // {
410 // m_groundIR->setGraphicalProperties(drawSensor, drawRay, drawRealRay);
411 // }
412 //
413 // void PhyEpuck::doKinematicSimulation(bool k)
414 // {
415 // if (m_kinematicSimulation == k) {
416 // return;
417 // }
418 //
419 // m_kinematicSimulation = k;
420 // if (m_kinematicSimulation) {
421 // // First disabling all joints...
422 // for (int i = 0; i < m_wheelJoints.size(); i++) {
423 // m_wheelJoints[i]->enable(false);
424 // }
425 //
426 // // ... then setting all objects to kinematic behaviour
427 // m_body->setKinematic(true);
428 // for (int i = 0; i < m_wheels.size(); i++) {
429 // m_wheels[i]->setKinematic(true);
430 // }
431 // } else {
432 // // First setting all objects to dynamic behaviour...
433 // m_body->setKinematic(false);
434 // for (int i = 0; i < m_wheels.size(); i++) {
435 // m_wheels[i]->setKinematic(false);
436 // }
437 //
438 // // ... then enabling all joints (if the corresponding part is enabled)
439 // for (int i = 0; i < m_wheelJoints.size(); i++) {
440 // m_wheelJoints[i]->enable(true);
441 // m_wheelJoints[i]->updateJointInfo();
442 // }
443 // }
444 // }
445 //
446 // void PhyEpuck::setLeftWheelDesideredVelocity(real velocity)
447 // {
448 // m_leftWheelVelocity = velocity;
449 // }
450 //
451 // void PhyEpuck::setRightWheelDesideredVelocity(real velocity)
452 // {
453 // m_rightWheelVelocity = velocity;
454 // }
455 //
456 // void PhyEpuck::changedMatrix()
457 // {
458 // wMatrix tm = matrix();
459 // m_body->setMatrix(tm);
460 // for (int i = 0; i < m_wheels.size(); i++) {
461 // m_wheels[i]->setMatrix(m_wheelsTransformation[i] * tm);
462 // }
463 // foreach (PhyJoint* j, m_wheelJoints) {
464 // j->updateJointInfo();
465 // }
466 // }
467 
468 } // end namespace farsa