WorldSim - 1.2.3

Utilities Configuration GA
NNFW WorldSim Total 99% Experiments
motorcontrollers.h
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 #ifndef MOTORCONTROLLERS_H
21 #define MOTORCONTROLLERS_H
22 
23 #include "worldsimconfig.h"
24 #include "phyjoint.h"
25 #include "ownable.h"
26 #include <QVector>
27 
28 // All this stuff is to get rid of annoying warnings...
29 #ifdef __GNUC__
30 #if __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ > 5)
31 #pragma GCC diagnostic push
32 #pragma GCC diagnostic ignored "-Wunused-parameter"
33 #else
34 #pragma GCC diagnostic ignored "-Wunused-parameter"
35 #endif
36 #endif
37 #ifdef FARSA_USE_YARP_AND_ICUB
38  #include <yarp/dev/ControlBoardInterfaces.h>
39  #include <yarp/dev/ControlBoardPid.h>
40 #endif
41 #ifdef __GNUC__
42 #if __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ > 5)
43 #pragma GCC diagnostic pop
44 #else
45 #pragma GCC diagnostic warning "-Wunused-parameter"
46 #endif
47 #endif
48 
49 namespace farsa {
50 
51 class World;
52 
59 class FARSA_WSIM_API wPID {
60 public:
62  wPID();
66  double pidloop( double currentvalue );
69  void reset();
71  double p_gain;
73  double i_gain;
75  double d_gain;
77  double acc_ff;
79  double fri_ff;
81  double vel_ff;
83  double bias;
85  double accel_r;
87  double setpt;
89  double min;
91  double max;
93  double slew;
94 private:
95  double this_target;
96  double next_target;
97  double integral;
98  double last_error;
99  double last_output;
100 };
101 
111 class FARSA_WSIM_API MotorController : public Ownable {
112 public:
116  MotorController( World* world );
118  virtual ~MotorController();
120  virtual void update() = 0;
122  bool isEnabled() {
123  return enabled;
124  };
126  void setEnabled( bool b ) {
127  enabled = b;
128  };
130  World* world() {
131  return worldv;
132  };
133 
134 private:
136  World* worldv;
138  bool enabled;
139 
141  MotorController(const MotorController& other);
142 
144  MotorController& operator=(const MotorController& other);
145 };
146 
179 class FARSA_WSIM_API MultiMotorController : public MotorController
180 #ifdef FARSA_USE_YARP_AND_ICUB
181 , public yarp::dev::DeviceDriver, public yarp::dev::IPositionControl, public yarp::dev::IVelocityControl, public yarp::dev::IControlMode, public yarp::dev::IEncoders, public yarp::dev::IControlLimits, public yarp::dev::IControlLimitsRaw
182 #endif
183 {
184 public:
188  MultiMotorController( QVector<PhyDOF*> motors, World* world );
190  virtual ~MultiMotorController();
192  QVector<PhyDOF*>& motors() {
193  return motorsv;
194  };
196  QVector<wPID*>& positionPIDs() {
197  return pos_pids;
198  };
200  bool setEnableLimitsRaw( int axis, bool b );
202  virtual void update();
204  QVector<wPID*>& velocityPIDs() {
205  return vel_pids;
206  };
207 
210 #ifdef FARSA_USE_YARP_AND_ICUB
211 
212  virtual bool close( );
214  virtual bool configure( yarp::os::Searchable &config );
216  virtual bool open( yarp::os::Searchable &config );
217 #endif
218 
219 
222  virtual bool getAxes(int *ax);
224  virtual bool getRefAcceleration(int j, double *acc);
226  virtual bool getRefAccelerations(double *accs);
228  virtual bool setRefAcceleration(int j, double acc);
230  virtual bool setRefAccelerations(const double *accs);
232  virtual bool stop(int j);
234  virtual bool stop();
236 
239  virtual bool checkMotionDone(int j, bool *flag);
241  virtual bool checkMotionDone(bool *flag);
243  virtual bool getRefSpeed(int j, double *ref);
245  virtual bool getRefSpeeds(double *spds);
247  virtual bool positionMove(int j, double ref);
249  virtual bool positionMove(const double *refs);
251  virtual bool relativeMove(int j, double delta);
253  virtual bool relativeMove(const double *deltas);
255  virtual bool setPositionMode();
257  virtual bool setRefSpeed(int j, double sp);
259  virtual bool setRefSpeeds(const double *spds);
261 
264  virtual bool setVelocityMode();
266  virtual bool velocityMove(int j, double sp);
268  virtual bool velocityMove(const double *sp);
270 
273  virtual bool getControlMode( int j, int* mode );
275  virtual bool getControlModes( int* modes );
277  virtual bool setImpedancePositionMode(int) { return false; }
279  virtual bool setImpedanceVelocityMode(int) { return false; }
281  virtual bool setOpenLoopMode(int j);
283  virtual bool setPositionMode( int j );
285  virtual bool setTorqueMode( int j );
287  virtual bool setVelocityMode(int j );
289 
292  virtual bool getEncoder( int j, double *v );
294  virtual bool getEncoders(double *encs);
296  virtual bool getEncoderAcceleration( int j, double *spds );
298  virtual bool getEncoderAccelerations( double *accs );
300  virtual bool getEncoderSpeed( int j, double *sp );
302  virtual bool getEncoderSpeeds( double *spds );
304  virtual bool resetEncoder( int j );
306  virtual bool resetEncoders();
308  virtual bool setEncoder( int j, double val );
310  virtual bool setEncoders( const double *vals );
312 
315  virtual bool getLimits( int axis, double *min, double *max );
317  virtual bool setLimits( int axis, double min, double max );
319 
322  virtual bool getLimitsRaw( int axis, double *min, double *max );
324  virtual bool setLimitsRaw( int axis, double min, double max );
326 private:
328  QVector<PhyDOF*> motorsv;
329  //--- Common Variable among Control Interfaces
334  QVector<int> controlModes;
336  QVector<real> refsacc;
337 
338  //--- IPositionControl Variables
340  QVector<real> refs;
342  QVector<real> refsvel;
344  QVector<real> xfx0, x0, dx0, t, T;
348  QVector<bool> stops;
350  QVector<wPID*> pos_pids;
352  QVector<real> lastErrors, lastIs, lastTorques;
354  real fromRawPosition( int i );
356  real toRawPosition( real noRawPos, int i );
357  //--- END IPositionControl Variables
358 
359  //--- IVelocityControl Variables
361  QVector<real> desiredVel;
363  QVector<wPID*> vel_pids;
364  //--- END IVelocityControl Variables
365 
366  //--- IControlLimits Variables
368  QVector<double> loLimits;
370  QVector<double> hiLimits;
371  //--- END IControlLimits Variables
372 };
373 
387 void wheeledRobotsComputeKinematicMovement(wMatrix &mtr, real leftWheelVelocity, real rightWheelVelocity, real wheelr, real axletrack, real timestep);
388 
391 class FARSA_WSIM_API WheelMotorController : public MotorController {
392 public:
396  WheelMotorController( QVector<PhyDOF*> wheels, World* world );
398  virtual ~WheelMotorController();
400  QVector<PhyDOF*>& wheels() {
401  return motorsv;
402  };
404  virtual void update();
408  void setSpeeds( QVector<double> speeds );
412  void setSpeeds( double sp1, double sp2 );
416  void getSpeeds( QVector<double>& speeds );
420  void getSpeeds( double& sp1, double& sp2 );
424  void setSpeedLimits( QVector<double> minSpeeds, QVector<double> maxSpeeds );
428  void setSpeedLimits( double minSpeed1, double minSpeed2, double maxSpeed1, double maxSpeed2 );
432  void getSpeedLimits( QVector<double>& minSpeeds, QVector<double>& maxSpeeds );
436  void getSpeedLimits( double& minSpeed1, double& minSpeed2, double& maxSpeed1, double& maxSpeed2 );
437 private:
439  QVector<PhyDOF*> motorsv;
441  QVector<double> desiredVel;
443  QVector<double> minVel;
445  QVector<double> maxVel;
446 };
447 
448 class PhyMarXbot;
449 class PhyFixed;
450 
492 class FARSA_WSIM_API MarXbotAttachmentDeviceMotorController : public MotorController
493 {
494 public:
499  enum Status {
500  Open,
501  HalfClosed,
502  Closed
503  };
504 
505 public:
512  MarXbotAttachmentDeviceMotorController(PhyMarXbot* robot);
513 
517  virtual ~MarXbotAttachmentDeviceMotorController();
518 
522  virtual void update();
523 
531  bool attachmentDeviceEnabled() const;
532 
539  void setMaxVelocity(double speed);
540 
548  double getMaxVelocity() const;
549 
556  void setDesiredPosition(double pos);
557 
563  double getDesiredPosition() const;
564 
570  double getPosition() const;
571 
578  void setDesiredVelocity(double vel);
579 
585  double getDesiredVelocity() const;
586 
592  double getVelocity() const;
593 
601  void setDesiredStatus(Status status);
602 
608  Status getDesiredStatus() const
609  {
610  return m_desiredStatus;
611  }
612 
618  Status getStatus() const
619  {
620  return m_status;
621  }
622 
632  bool attachmentPossible() const
633  {
634  return (tryToAttach() != NULL);
635  }
636 
642  bool attachedToRobot() const
643  {
644  return (m_attachedRobot != NULL);
645  }
646 
654  PhyMarXbot* attachedRobot()
655  {
656  return m_attachedRobot;
657  }
658 
666  const PhyMarXbot* attachedRobot() const
667  {
668  return m_attachedRobot;
669  }
670 
677  bool otherAttachedToUs() const
678  {
679  return !m_otherAttachedRobots.isEmpty();
680  }
681 
688  QVector<PhyMarXbot*> otherAttachedRobots()
689  {
690  return m_otherAttachedRobots;
691  }
692 
693 private:
703  PhyMarXbot* tryToAttach() const;
704 
709  void attachmentDeviceAboutToBeDestroyed();
710 
714  PhyMarXbot* const m_robot;
715 
719  Status m_status;
720 
724  Status m_desiredStatus;
725 
732  PhyJoint* m_joint;
733 
739  PhyMarXbot* m_attachedRobot;
740 
746  QVector<PhyMarXbot*> m_otherAttachedRobots;
747 
752  friend class PhyMarXbot;
753 };
754 
755 } // end namespace farsa
756 
757 #endif