WorldSim - 1.2.0

Utilities Configuration GA
NNFW WorldSim Total 99% Experiments
motorcontrollers.cpp
1 /********************************************************************************
2  * WorldSim -- library for robot simulations *
3  * Copyright (C) 2008-2011 Gianluca Massera <emmegian@yahoo.it> *
4  * *
5  * This program is free software; you can redistribute it and/or modify *
6  * it under the terms of the GNU General Public License as published by *
7  * the Free Software Foundation; either version 2 of the License, or *
8  * (at your option) any later version. *
9  * *
10  * This program is distributed in the hope that it will be useful, *
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
13  * GNU General Public License for more details. *
14  * *
15  * You should have received a copy of the GNU General Public License *
16  * along with this program; if not, write to the Free Software *
17  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA *
18  ********************************************************************************/
19 
20 #include "worldsimconfig.h"
21 #include "motorcontrollers.h"
22 #include "phyjoint.h"
23 #include "world.h"
24 
25 #ifdef FARSA_USE_YARP_AND_ICUB
26 
27 #ifdef __GNUC__
28 #pragma GCC diagnostic ignored "-Wunused-parameter"
29 #endif
30  #include <yarp/os/Searchable.h>
31 #ifdef __GNUC__
32 #pragma GCC diagnostic warning "-Wunused-parameter"
33 #endif
34 
35 using namespace yarp::dev;
36 using namespace yarp::os;
37 #endif
38 
39 namespace farsa {
40 
41 wPID::wPID() {
42  p_gain = 0;
43  i_gain = 0;
44  d_gain = 0;
45  acc_ff = 0;
46  fri_ff = 0;
47  vel_ff = 0;
48  bias = 0;
49  accel_r = 0;
50  setpt = 0;
51  min = 0;
52  max = 0;
53  slew = 0;
54  this_target = 0;
55  next_target = 0;
56  integral = 0;
57  last_error = 0;
58  last_output = 0;
59 }
60 
61 double wPID::pidloop( double PV ) {
62  double this_error;
63  double this_output;
64  double accel;
65  double deriv;
66  double friction;
67  /* the desired PV for this iteration is the value */
68  /* calculated as next_target during the last loop */
69  this_target = next_target;
70  /* test for acceleration, compute new target PV for */
71  /* the next pass through the loop */
72  if ( accel_r > 0 && this_target != setpt) {
73  if ( this_target < setpt ) {
74  next_target += accel_r;
75  if ( next_target > setpt ) {
76  next_target = setpt;
77  }
78  } else { /* this_target > setpoint */
79  next_target -= accel_r;
80  if ( next_target < setpt ) {
81  next_target = setpt;
82  }
83  }
84  } else {
85  next_target = setpt;
86  }
87  /* acceleration is the difference between the PV */
88  /* target on this pass and the next pass through the */
89  /* loop */
90  accel = next_target - this_target;
91  /* the error for the current pass is the difference */
92  /* between the current target and the current PV */
93  this_error = this_target - PV;
94  /* the derivative is the difference between the error */
95  /* for the current pass and the previous pass */
96  deriv = this_error - last_error;
97  /* a very simple determination of whether there is */
98  /* special friction to be overcome on the next pass, */
99  /* if the current PV is 0 and the target for the next */
100  /* pass is not 0, stiction could be a problem */
101  friction = ( PV == 0.0 && next_target != 0.0 );
102  /* the new error is added to the integral */
103  integral += this_target - PV;
104  /* now that all of the variable terms have been */
105  /* computed they can be multiplied by the appropriate */
106  /* coefficients and the resulting products summed */
107  this_output = p_gain * this_error
108  + i_gain * integral
109  + d_gain * deriv
110  + acc_ff * accel
111  + vel_ff * next_target
112  + fri_ff * friction
113  + bias;
114  last_error = this_error;
115  /* check for slew rate limiting on the output change */
116  if ( 0 != slew ) {
117  if ( this_output - last_output > slew ) {
118  this_output = last_output + slew;
119  } else if ( last_output - this_output > slew ) {
120  this_output = last_output - slew;
121  }
122  }
123  /* now check the output value for absolute limits */
124  if ( this_output < min ) {
125  this_output = min;
126  } else if ( this_output > max ) {
127  this_output = max;
128  }
129  /* store the new output value to be used as the old */
130  /* output value on the next loop pass */
131  return last_output = this_output;
132 }
133 
134 void wPID::reset() {
135  // Resetting status variables
136  this_target = 0;
137  next_target = 0;
138  integral = 0;
139  last_error = 0;
140  last_output = 0;
141 }
142 
143 MotorController::MotorController( World* world ) {
144  enabled = true;
145  worldv = world;
146 }
147 
148 MotorController::~MotorController() {
149 }
150 
151 
152 MultiMotorController::MultiMotorController( QVector<PhyDOF*> dofs, World* world ) :
153  MotorController(world),
154 #ifdef FARSA_USE_YARP_AND_ICUB
155  IPositionControl(),
156  IVelocityControl(),
157 #endif
158  motorsv(dofs)
159 {
160  //--- common control variable initialization
161  controlModes.fill( 1, motorsv.size() );
162  refsacc.fill( 1.0, motorsv.size() );
163  //--- IPositionControl variable initialization
164  refs.fill( 0.0, motorsv.size() );
165  refsvel.fill( 1.0, motorsv.size() );
166  xfx0.fill( 0.0, motorsv.size() );
167  x0.fill( 0.0, motorsv.size() );
168  dx0.fill( 0.0, motorsv.size() );
169  t.fill( 0.0, motorsv.size() );
170  T.fill( 0.0, motorsv.size() );
171  stops.fill( true, motorsv.size() );
172  lastErrors.fill( 0.0, motorsv.size() );
173  lastIs.fill( 0.0, motorsv.size() );
174  lastTorques.fill( 0.0, motorsv.size() );
175  // create PIDs with default values
176  pos_pids.resize( motorsv.size() );
177  for( int i=0; i<motorsv.size(); i++ ) {
178  pos_pids[i] = new wPID();
179  pos_pids[i]->p_gain = 0.8;
180  pos_pids[i]->i_gain = 0.05;
181  pos_pids[i]->d_gain = 0.0;
182  pos_pids[i]->accel_r = 0;
183  pos_pids[i]->min = -PI_GRECO/2.0;
184  pos_pids[i]->max = +PI_GRECO/2.0;
185  }
186  //--- IVelocityControl variable initialization
187  desiredVel.fill( 0.0, motorsv.size() );
188  // create PIDs with default values
189  vel_pids.resize( motorsv.size() );
190  for( int i=0; i<motorsv.size(); i++ ) {
191  vel_pids[i] = new wPID();
192  vel_pids[i]->p_gain = 0.8;
193  vel_pids[i]->i_gain = 0.1;
194  vel_pids[i]->d_gain = 0.0;
195  //vel_pids[i]->accel_r = 3;
196  //vel_pids[i]->acc_ff = 0.3;
197  vel_pids[i]->min = -toRad( 120 ); // rads / second
198  vel_pids[i]->max = +toRad( 120 ); // rads / second
199  }
200  //--- IControlLimits variable initialization
201  loLimits.resize( motorsv.size() );
202  hiLimits.resize( motorsv.size() );
203  for( int i=0; i<motorsv.size(); i++ ) {
204  real fmin, fmax;
205  motorsv[i]->limits( fmin, fmax );
206  loLimits[i] = fmin;
207  hiLimits[i] = fmax;
208  }
209 }
210 
211 MultiMotorController::~MultiMotorController() {
212  for( int i=0; i<motorsv.size(); i++ ) {
213  delete (vel_pids[i]);
214  delete (pos_pids[i]);
215  }
216 }
217 
218 void MultiMotorController::update() {
219  real accum, /*wishangle,*/ wishvel;
220  for( int i=0; i<motorsv.size(); i++ ) {
221  switch( controlModes[i] ) {
222  case 1: /* Position Controller */
223  if ( ! stops[i] ) {
224  real tFracT = t[i] / T[i];
225  real tFracT3= tFracT * tFracT * tFracT;
226  real tFracT4= tFracT * tFracT3;
227  real tFracT5= tFracT * tFracT4;
228 
229  //--- code from p5f code from iCub/src/controller4Dc/Code/trajectory.c
230  //float xfx0 = _xf[jj] - _x0[jj]; --- already computed when positionMove has been called
231  //float dx0 = _dx0[jj]; --- computed when positionMove has been called
232  accum = dx0[i] * tFracT;
233  accum = accum + tFracT3 * (10.0*xfx0[i] - 6.0*dx0[i]);
234  accum = accum - tFracT4 * (15.0*xfx0[i] - 8.0*dx0[i]);
235  accum = accum + tFracT5 * (6.0*xfx0[i] - 3.0*dx0[i]);
236  //--- in p5f code are missing x0 because it's added inside step_trajectory
237  accum += x0[i];
238  } else {
239  accum = refs[i];
240  }
241  //--- accum is the angle at which the joint must be at current time
242  pos_pids[i]->setpt = accum;
243 // wishangle = pos_pids[i]->pidloop( fromRawPosition(i) );
244  motorsv[i]->setDesiredPosition( toRawPosition(accum, i) /*wishangle*/ );
245  //--- advance t
246  t[i] += world()->timeStep();
247  //--- stop if necessary
248  stops[i] = ( t[i] / T[i] > 1.0 );
249  break;
250  case 2: /* Velocity Controller */
251  //--- the velocity at which the joint must be at current time
252  vel_pids[i]->setpt = desiredVel[i];
253  wishvel = vel_pids[i]->pidloop( motorsv[i]->velocity() );
254  motorsv[i]->setDesiredVelocity( wishvel );
255 // motorsv[i]->setDesiredVelocity(desiredVel[i]); // This disables the PID
256  break;
257  default:
258  break;
259  }
260  }
261  return;
262 }
263 
264 #ifdef FARSA_USE_YARP_AND_ICUB
265 
266 bool MultiMotorController::open( yarp::os::Searchable & ) {
267  return true;
268 }
269 
270 bool MultiMotorController::close( ) {
271  return true;
272 }
273 
274 bool MultiMotorController::configure( yarp::os::Searchable & ) {
275  return true;
276 }
277 
278 #endif
279 
280 bool MultiMotorController::getAxes( int *ax ) {
281  *ax = motorsv.size();
282  return true;
283 }
284 
285 bool MultiMotorController::setPositionMode() {
286  //controlMode = 1;
287  return true;
288 }
289 
290 bool MultiMotorController::positionMove(int j, double ref) {
291  if ( j < 0 && j >= motorsv.size() ) {
292  return false;
293  }
294  if ( motorsv[j]->translate() ) {
295  refs[j] = ref;
296  } else {
297  refs[j] = toRad( ref );
298  }
299  if ( motorsv[j]->isLimited() ) {
300  //--- check the limits
301  //--- stay too near to the limits may create oscillation of the joint
302  //--- about 0.5% of the range of the joint
303  real offset = (fabs(hiLimits[j])-fabs(loLimits[j]))*0.005;
304  refs[j] = ramp( loLimits[j]+offset, hiLimits[j]-offset, refs[j] );
305  }
306  //--- calculate xfx0, dx0, T
307  x0[j] = fromRawPosition(j);
308  xfx0[j] = refs[j] - x0[j];
309  dx0[j] = motorsv[j]->velocity();
310  T[j] = fabs( xfx0[j] ) / refsvel[j];
311  //--- reset t to zero
312  t[j] = 0.0;
313  // if T[j] is less that 10 ms, then it simply stay there
314  stops[j] = ( T[j] < 0.010 );
315  controlModes[j] = 1;
316  return true;
317 }
318 
319 bool MultiMotorController::positionMove(const double *newrefs) {
320  for( int i=0; i<refs.size(); i++ ) {
321  if ( motorsv[i]->translate() ) {
322  refs[i] = newrefs[i];
323  } else {
324  refs[i] = toRad( newrefs[i] );
325  }
326  if ( motorsv[i]->isLimited() ) {
327  //--- check the limits
328  //--- stay too near to the limits may create oscillation of the joint
329  //--- about 0.5% of the range of the joint
330  real offset = (fabs(hiLimits[i])-fabs(loLimits[i]))*0.005;
331  refs[i] = ramp( loLimits[i]+offset, hiLimits[i]-offset, refs[i] );
332  }
333  //--- calculate xfx0, dx0, T
334  x0[i] = fromRawPosition( i );
335  xfx0[i] = refs[i] - x0[i];
336  dx0[i] = motorsv[i]->velocity();
337  T[i] = fabs( xfx0[i] ) / refsvel[i];
338  //--- reset t to zero
339  t[i] = 0.0;
340  // if T[j] is less that 10 ms, then it simply stay there
341  stops[i] = ( T[i] < 0.010 );
342  controlModes[i] = 1;
343  }
344  return true;
345 }
346 
347 bool MultiMotorController::relativeMove(int j, double delta) {
348  if ( j < 0 && j >= motorsv.size() ) {
349  return false;
350  }
351  if ( motorsv[j]->translate() ) {
352  refs[j] += delta;
353  } else {
354  refs[j] += toRad( delta );
355  }
356  if ( motorsv[j]->isLimited() ) {
357  //--- check the limits
358  //--- stay too near to the limits may create oscillation of the joint
359  //--- about 0.5% of the range of the joint
360  real offset = (fabs(hiLimits[j])-fabs(loLimits[j]))*0.005;
361  refs[j] = ramp( loLimits[j]+offset, hiLimits[j]-offset, refs[j] );
362  }
363  //--- calculate xfx0, dx0, T
364  x0[j] = fromRawPosition(j);
365  xfx0[j] = refs[j] - x0[j];
366  dx0[j] = motorsv[j]->velocity();
367  T[j] = fabs( xfx0[j] ) / refsvel[j];
368  //--- reset t to zero
369  t[j] = 0.0;
370  stops[j] = false;
371  controlModes[j] = 1;
372  return true;
373 }
374 
375 bool MultiMotorController::relativeMove(const double *deltas) {
376  for( int i=0; i<refs.size(); i++ ) {
377  if ( motorsv[i]->translate() ) {
378  refs[i] += deltas[i];
379  } else {
380  refs[i] += toRad( deltas[i] );
381  }
382  if ( motorsv[i]->isLimited() ) {
383  //--- check the limits
384  //--- stay too near to the limits may create oscillation of the joint
385  //--- about 0.5% of the range of the joint
386  real offset = (fabs(hiLimits[i])-fabs(loLimits[i]))*0.005;
387  refs[i] = ramp( loLimits[i]+offset, hiLimits[i]-offset, refs[i] );
388  }
389  //--- calculate xfx0, dx0, T
390  x0[i] = fromRawPosition( i );
391  xfx0[i] = refs[i] - x0[i];
392  dx0[i] = motorsv[i]->velocity();
393  T[i] = fabs( xfx0[i] ) / refsvel[i];
394  //--- reset t to zero
395  t[i] = 0.0;
396  stops[i] = false;
397  controlModes[i] = 1;
398  }
399  return true;
400 }
401 
402 bool MultiMotorController::checkMotionDone(int j, bool *flag) {
403  if ( j < 0 && j >= motorsv.size() ) {
404  return false;
405  }
406  //--- I should check the actual position or just if stops[j] is true ??
407  *flag = ( fabs(fromRawPosition(j) - refs[j]) < 0.01745329 );
408  //*flag = stops[j];
409  return true;
410 }
411 
412 bool MultiMotorController::checkMotionDone(bool *flag) {
413  bool ends = true;
414  for( int i=0; i<refs.size(); i++ ) {
415  //--- I should check the actual position or just if stops[j] is true ??
416  ends &= ( fabs(fromRawPosition(i) - refs[i]) < 0.01745329 );
417  //ends &= stops[i];
418  if ( !ends ) break;
419  }
420  *flag = ends;
421  return true;
422 }
423 
424 bool MultiMotorController::setRefSpeed(int j, double sp) {
425  if ( j < 0 && j >= motorsv.size() ) {
426  return false;
427  }
428  if ( motorsv[j]->translate() ) {
429  refsvel[j] = sp;
430  } else {
431  refsvel[j] = toRad(sp);
432  }
433  motorsv[j]->setMaxVelocity( refsvel[j] );
434  return true;
435 }
436 
437 bool MultiMotorController::setRefSpeeds(const double *spds) {
438  for( int i=0; i<refs.size(); i++ ) {
439  if ( motorsv[i]->translate() ) {
440  refsvel[i] = spds[i];
441  } else {
442  refsvel[i] = toRad(spds[i]);
443  }
444  motorsv[i]->setMaxVelocity( refsvel[i] );
445  }
446  return true;
447 }
448 
449 bool MultiMotorController::setRefAcceleration(int j, double acc) {
450  if ( j < 0 && j >= motorsv.size() ) {
451  return false;
452  }
453  if ( motorsv[j]->translate() ) {
454  refsacc[j] = acc;
455  } else {
456  refsacc[j] = toRad(acc);
457  }
458  return true;
459 }
460 
461 bool MultiMotorController::setRefAccelerations(const double *accs) {
462  for( int i=0; i<refs.size(); i++ ) {
463  if ( motorsv[i]->translate() ) {
464  refsacc[i] = accs[i];
465  } else {
466  refsacc[i] = toRad(accs[i]);
467  }
468  }
469  return true;
470 }
471 
472 bool MultiMotorController::getRefSpeed(int j, double *ref) {
473  if ( j < 0 && j >= motorsv.size() ) {
474  return false;
475  }
476  if ( motorsv[j]->translate() ) {
477  *ref = refsvel[j];
478  } else {
479  *ref = toDegree(refsvel[j]);
480  }
481  return true;
482 }
483 
484 bool MultiMotorController::getRefSpeeds(double *spds) {
485  for( int i=0; i<refsvel.size(); i++ ) {
486  if ( motorsv[i]->translate() ) {
487  spds[i] = refsvel[i];
488  } else {
489  spds[i] = toDegree(refsvel[i]);
490  }
491  }
492  return true;
493 }
494 
495 bool MultiMotorController::getRefAcceleration(int j, double *acc) {
496  if ( j < 0 && j >= motorsv.size() ) {
497  return false;
498  }
499  if ( motorsv[j]->translate() ) {
500  *acc = refsacc[j];
501  } else {
502  *acc = toDegree(refsacc[j]);
503  }
504  return true;
505 }
506 
507 bool MultiMotorController::getRefAccelerations(double *accs) {
508  for( int i=0; i<refsvel.size(); i++ ) {
509  if ( motorsv[i]->translate() ) {
510  accs[i] = refsacc[i];
511  } else {
512  accs[i] = toDegree(refsacc[i]);
513  }
514  }
515  return true;
516 }
517 
518 bool MultiMotorController::stop(int j) {
519  if ( j < 0 && j >= motorsv.size() ) {
520  return false;
521  }
522  refs[j] = fromRawPosition(j);
523  stops[j] = true;
524  desiredVel[j] = 0;
525  motorsv[j]->setVelocity(0.0);
526  controlModes[j] = 1;
527  vel_pids[j]->reset();
528  return true;
529 }
530 
531 bool MultiMotorController::stop() {
532  for( int i=0; i<motorsv.size(); i++ ) {
533  refs[i] = fromRawPosition(i);
534  stops[i] = true;
535  desiredVel[i] = 0;
536  motorsv[i]->setVelocity(0.0);
537  controlModes[i] = 1;
538  vel_pids[i]->reset();
539  }
540  return true;
541 }
542 
543 bool MultiMotorController::setVelocityMode() {
544  //controlMode = 2;
545  return true;
546 }
547 
548 bool MultiMotorController::velocityMove(int j, double sp) {
549  if ( j < 0 && j >= motorsv.size() ) {
550  return false;
551  }
552  controlModes[j] = 2;
553  desiredVel[j] = toRad(sp);
554  return true;
555 }
556 
557 bool MultiMotorController::velocityMove(const double *sp) {
558  for( int i=0; i<desiredVel.size(); i++ ) {
559  desiredVel[i] = toRad(sp[i]);
560  controlModes[i] = 2;
561  }
562  return true;
563 }
564 
565 bool MultiMotorController::setPositionMode( int j ) {
566  if ( j < 0 && j >= motorsv.size() ) {
567  return false;
568  }
569  //--- set the position to actual position and stop the joint
570  refs[j] = fromRawPosition(j);
571  stops[j] = true;
572  controlModes[j] = 1;
573  return true;
574 }
575 
576 bool MultiMotorController::setVelocityMode(int j ) {
577  if ( j < 0 && j >= motorsv.size() ) {
578  return false;
579  }
580  //--- set the desired velocity to zero and stop the joint
581  desiredVel[j] = 0;
582  stops[j] = true;
583  controlModes[j] = 2;
584  return true;
585 }
586 
587 bool MultiMotorController::setTorqueMode( int j ) {
588  if ( j < 0 && j >= motorsv.size() ) {
589  return false;
590  }
591  //--- torque mode not implemented yet
592  return false;
593 }
594 
595 bool MultiMotorController::getControlMode( int j, int* mode ) {
596  if ( j < 0 && j >= motorsv.size() ) {
597  return false;
598  }
599  *mode = controlModes[j];
600  return true;
601 }
602 
603 bool MultiMotorController::getControlModes( int* modes ) {
604  for( int i=0; i<motorsv.size(); i++ ) {
605  modes[i] = controlModes[i];
606  }
607  return true;
608 }
609 
610 bool MultiMotorController::setOpenLoopMode( int ) {
611  return false;
612 }
613 
614 bool MultiMotorController::resetEncoder( int ) {
615  return true;
616 }
617 
618 bool MultiMotorController::resetEncoders() {
619  return true;
620 }
621 
622 bool MultiMotorController::setEncoder( int, double ) {
623  return true;
624 }
625 
626 bool MultiMotorController::setEncoders( const double* ) {
627  return true;
628 }
629 
630 bool MultiMotorController::getEncoder( int j, double *v ) {
631  if ( j < 0 && j >= motorsv.size() ) {
632  return false;
633  }
634  if ( motorsv[j]->translate() ) {
635  (*v) = fromRawPosition(j);
636  } else {
637  (*v) = toDegree( fromRawPosition(j) );
638  }
639  return true;
640 }
641 
642 bool MultiMotorController::getEncoders( double *encs ) {
643  for( int i=0; i<motorsv.size(); i++ ) {
644  if ( motorsv[i]->translate() ) {
645  encs[i] = fromRawPosition(i);
646  } else {
647  encs[i] = toDegree( fromRawPosition(i) );
648  }
649  }
650  return true;
651 }
652 
653 bool MultiMotorController::getEncoderSpeed( int j, double *sp ) {
654  if ( j < 0 && j >= motorsv.size() ) {
655  return false;
656  }
657  if ( motorsv[j]->translate() ) {
658  (*sp) = motorsv[j]->velocity();
659  } else {
660  (*sp) = toDegree(motorsv[j]->velocity());
661  }
662  return true;
663 }
664 
665 bool MultiMotorController::getEncoderSpeeds( double *spds ) {
666  for( int i=0; i<motorsv.size(); i++ ) {
667  if ( motorsv[i]->translate() ) {
668  spds[i] = motorsv[i]->velocity();
669  } else {
670  spds[i] = toDegree(motorsv[i]->velocity());
671  }
672  }
673  return true;
674 }
675 
676 bool MultiMotorController::getEncoderAcceleration( int, double * ) {
677  return false;
678 }
679 
680 bool MultiMotorController::getEncoderAccelerations( double * ) {
681  return false;
682 }
683 
684 bool MultiMotorController::setLimits( int axis, double min, double max ) {
685  if ( axis < 0 && axis >= motorsv.size() ) {
686  return false;
687  }
688  if ( motorsv[axis]->translate() ) {
689  loLimits[axis] = min;
690  hiLimits[axis] = max;
691  } else {
692  loLimits[axis] = toRad( min );
693  hiLimits[axis] = toRad( max );
694  }
695  return true;
696 }
697 
698 bool MultiMotorController::getLimits( int axis, double *min, double *max ) {
699  if ( axis < 0 && axis >= motorsv.size() ) {
700  return false;
701  }
702  if ( motorsv[axis]->translate() ) {
703  *min = loLimits[axis];
704  *max = hiLimits[axis];
705  } else {
706  *min = toDegree( loLimits[axis] );
707  *max = toDegree( hiLimits[axis] );
708  }
709  return true;
710 }
711 
712 bool MultiMotorController::setLimitsRaw( int axis, double min, double max ) {
713  if ( axis < 0 && axis >= motorsv.size() ) {
714  return false;
715  }
716  if ( motorsv[axis]->translate() ) {
717  motorsv[axis]->setLimits( min, max );
718  } else {
719  motorsv[axis]->setLimits( toRad(min), toRad(max) );
720  }
721  return true;
722 }
723 
724 bool MultiMotorController::getLimitsRaw( int axis, double *min, double *max ) {
725  if ( axis < 0 && axis >= motorsv.size() ) {
726  return false;
727  }
728  real fmin, fmax;
729  motorsv[axis]->limits( fmin, fmax );
730  if ( motorsv[axis]->translate() ) {
731  *min = fmin;
732  *max = fmax;
733  } else {
734  *min = toDegree( fmin );
735  *max = toDegree( fmax );
736  }
737  return true;
738 }
739 
740 bool MultiMotorController::setEnableLimitsRaw( int axis, bool enable ) {
741  if ( axis < 0 && axis >= motorsv.size() ) {
742  return false;
743  }
744  if ( enable ) {
745  motorsv[axis]->enableLimits();
746  } else {
747  motorsv[axis]->disableLimits();
748  }
749  return true;
750 }
751 
752 real MultiMotorController::fromRawPosition( int i ) {
753  real rmin, rmax;
754  motorsv[i]->limits( rmin, rmax );
755  return linearMap( motorsv[i]->position(), rmin, rmax, loLimits[i], hiLimits[i] );
756 }
757 
758 real MultiMotorController::toRawPosition( real norawpos, int i ) {
759  real rmin, rmax;
760  motorsv[i]->limits( rmin, rmax );
761  return linearMap( norawpos, loLimits[i], hiLimits[i], rmin, rmax );
762 }
763 
764 void wheeledRobotsComputeKinematicMovement(wMatrix &mtr, real leftWheelVelocity, real rightWheelVelocity, real wheelr, real axletrack, real timestep)
765 {
766  const wMatrix origMtr = mtr;
767 
768  // To compute the actual movement of the robot I assume the movement is on the plane on which
769  // the two wheels lie. This means that I assume that both wheels are in contact with a plane
770  // at every time. Then I compute the instant centre of rotation and move the robot rotating
771  // it around the axis passing through the instant center of rotation and perpendicular to the
772  // plane on which the wheels lie (i.e. around the local XY plane)
773 
774  // First of all computing the linear speed of the two wheels
775  const real leftSpeed = leftWheelVelocity * wheelr;
776  const real rightSpeed = rightWheelVelocity * wheelr;
777 
778  // If the speed of the two wheels is very similar, simply moving the robot forward (doing the
779  // computations would probably lead to invalid matrices)
780  if (fabs(leftSpeed - rightSpeed) < 0.00001f) {
781  // The front of the robot is towards -y
782  mtr.w_pos += -mtr.y_ax.scale(rightSpeed * timestep);
783  } else {
784  // The first thing to do is to compute the instant centre of rotation. We do the
785  // calculation in the robot local frame of reference. We proceed as follows (with
786  // uppercase letters we indicate vectors and points):
787  //
788  // +------------------------->
789  // | X axis
790  // | ^
791  // | |\
792  // | | \A
793  // | | \
794  // | L| ^
795  // | | |\
796  // | | | \
797  // | | R| \
798  // | | | \
799  // | O+--->----+C
800  // | D
801  // |
802  // |
803  // v Y axis (the robot moves forward towards -Y)
804  //
805  // In the picture L and R are the velocity vectors of the two wheels, D is the vector
806  // going from the center of the left wheel to the center of the right wheel, A is the
807  // vector going from the tip of L to the tip of R and C is the instant centre of
808  // rotation. D is fixed an parallel to the X axis, while L and R are always parallel
809  // to the Y axis. Also, for the moment, we take the frame of reference origin at O.
810  // The construction shown in the picture allows to find the instant center of rotation
811  // for any L and R except when they are equal. In this case C is "at infinite" and the
812  // robot moves forward of backward without rotation. All the vectors lie on the local
813  // XY plane. To find C we proceed in the following way. First of all we note that
814  // A = R - L. If a and b are two real numbers, then we can say that C is at the
815  // instersection of L + aA with bD, that is we need a and b such that:
816  // L + aA = bD.
817  // If we take the cross product on both sides we get:
818  // (L + aA) x D = bD x D => (L + aA) x D = 0
819  // This means that we need to find a such that L + aA and D are parallel. As D is parallel
820  // to the X axis, its Y component is 0, so we can impose that L + aA has a 0 Y component,
821  // too, that is:
822  // (Ly) + a(Ay) = 0 => a = -(Ly) / (Ay)
823  // Once we have a, we can easily compute C:
824  // C = L + aA
825  // In the following code we use the same names we have used in the description above. Also
826  // note that the actual origin of the frame of reference is not in O: it is in the middle
827  // between the left and right wheel (we need to take this into account when computing the
828  // point around which the robot rotates)
829  const wVector L(0.0, -leftSpeed, 0.0);
830  const wVector A(axletrack, -(rightSpeed - leftSpeed), 0.0);
831  const real a = -(L.y / A.y);
832  const wVector C = L + A.scale(a);
833 
834  // Now we have to compute the angular velocity of the robot. This is simply equal to v/r where
835  // v is the linear velocity at a point that is distant r from C. We use the velocity of one of
836  // the wheel: which weels depends on its distance from C. We take the wheel which is furthest from
837  // C to avoid having r = 0 (which would lead to an invalid value of the angular velocity). Distances
838  // are signed (they are taken along the X axis)
839  const real distLeftWheel = -C.x;
840  const real distRightWheel = -(C.x - A.x); // A.x is equal to D.x
841  const real angularVelocity = (fabs(distLeftWheel) > fabs(distRightWheel)) ? (-leftSpeed / distLeftWheel) : (-rightSpeed / distRightWheel);
842 
843  // The angular displacement is simply the angular velocity multiplied by the timeStep. Here we
844  // also compute the center of rotation in world coordinates (we also need to compute it in the frame
845  // of reference centered between the wheels, not in O) and the axis of rotation (that is simply the
846  // z axis of the robot in world coordinates)
847  const real angularDisplacement = angularVelocity * timestep;
848  const wVector centerOfRotation = origMtr.transformVector(C - wVector(axletrack / 2.0, 0.0, 0.0));
849  const wVector axisOfRotation = origMtr.z_ax;
850 
851  // Rotating the robot transformation matrix to obtain the new position
852  mtr = mtr.rotateAround(axisOfRotation, centerOfRotation, angularDisplacement);
853  }
854 }
855 
856 WheelMotorController::WheelMotorController( QVector<PhyDOF*> wheels, World* world ) :
857  MotorController(world),
858  motorsv(wheels) {
859  desiredVel.fill( 0.0, motorsv.size() );
860  minVel.fill( -1.0, motorsv.size() );
861  maxVel.fill( 1.0, motorsv.size() );
862 }
863 
864 WheelMotorController::~WheelMotorController() {
865  /* nothing to do */
866 }
867 
868 void WheelMotorController::update() {
869  for( int i=0; i<motorsv.size(); i++ ) {
870  motorsv[i]->setDesiredVelocity( desiredVel[i] );
871  }
872 }
873 
874 void WheelMotorController::setSpeeds( QVector<double> speeds ) {
875  for( int i=0; i<qMin(speeds.size(), desiredVel.size()); i++ ) {
876  if ( speeds[i] < minVel[i] ) {
877  desiredVel[i] = minVel[i];
878  } else if ( speeds[i] > maxVel[i] ) {
879  desiredVel[i] = maxVel[i];
880  } else {
881  desiredVel[i] = speeds[i];
882  }
883  }
884 }
885 
886 void WheelMotorController::setSpeeds( double sp1, double sp2 ) {
887  if ( sp1 < minVel[0] ) {
888  desiredVel[0] = minVel[0];
889  } else if ( sp1 > maxVel[0] ) {
890  desiredVel[0] = maxVel[0];
891  } else {
892  desiredVel[0] = sp1;
893  }
894 
895  if ( sp2 < minVel[1] ) {
896  desiredVel[1] = minVel[1];
897  } else if ( sp2 > maxVel[1] ) {
898  desiredVel[1] = maxVel[1];
899  } else {
900  desiredVel[1] = sp2;
901  }
902 }
903 
904 void WheelMotorController::getSpeeds( QVector<double>& speeds ) {
905  speeds.clear();
906  for( int i=0; i<motorsv.size(); i++ ) {
907  speeds.append(motorsv[i]->velocity());
908  }
909 }
910 
911 void WheelMotorController::getSpeeds( double& sp1, double& sp2 ) {
912  sp1 = motorsv[0]->velocity();
913  sp2 = motorsv[1]->velocity();
914 }
915 
916 void WheelMotorController::setSpeedLimits( QVector<double> minSpeeds, QVector<double> maxSpeeds ) {
917  minVel = minSpeeds;
918  maxVel = maxSpeeds;
919 }
920 
921 void WheelMotorController::setSpeedLimits( double minSpeed1, double minSpeed2, double maxSpeed1, double maxSpeed2 ) {
922  minVel[0] = minSpeed1;
923  minVel[1] = minSpeed2;
924  maxVel[0] = maxSpeed1;
925  maxVel[1] = maxSpeed2;
926 }
927 
928 void WheelMotorController::getSpeedLimits( QVector<double>& minSpeeds, QVector<double>& maxSpeeds ) {
929  minSpeeds = minVel;
930  maxSpeeds = maxVel;
931 }
932 
933 void WheelMotorController::getSpeedLimits( double& minSpeed1, double& minSpeed2, double& maxSpeed1, double& maxSpeed2 ) {
934  minSpeed1 = minVel[0];
935  minSpeed2 = minVel[1];
936  maxSpeed1 = maxVel[0];
937  maxSpeed2 = maxVel[1];
938 }
939 
940 } // end namespace farsa