iCub-main
pids.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
3  * Copyright (C) 2006-2010 RobotCub Consortium
4  * All rights reserved.
5  *
6  * This software may be modified and distributed under the terms
7  * of the BSD-3-Clause license. See the accompanying LICENSE file for
8  * details.
9 */
10 
22 #ifndef __PIDS_H__
23 #define __PIDS_H__
24 
25 #include <deque>
26 
27 #include <yarp/os/Bottle.h>
28 #include <yarp/sig/Vector.h>
29 #include <yarp/sig/Matrix.h>
30 
31 #include <iCub/ctrl/math.h>
32 #include <iCub/ctrl/filters.h>
33 
34 
35 namespace iCub
36 {
37 
38 namespace ctrl
39 {
40 
48 {
49 private:
50  // Default constructor: not implemented.
51  Integrator();
52 
53 protected:
54  unsigned int dim;
55  yarp::sig::Vector y;
56  yarp::sig::Vector x_old;
57  yarp::sig::Matrix lim;
58  double Ts;
59  bool applySat;
60 
61  void allocate(const Integrator &I);
62  yarp::sig::Vector saturate(const yarp::sig::Vector &v);
63 
64 public:
75  Integrator(const double _Ts, const yarp::sig::Vector &y0, const yarp::sig::Matrix &_lim);
76 
83  Integrator(const double _Ts, const yarp::sig::Vector &y0);
84 
89  Integrator(const Integrator &I) { allocate(I); }
90 
96  Integrator &operator=(const Integrator &I) { allocate(I); return *this; }
97 
104  const yarp::sig::Vector& integrate(const yarp::sig::Vector &x);
105 
111  void setSaturation(bool _applySat);
112 
117  bool getSaturation() { return applySat; }
118 
123  void setTs(const double _Ts);
124 
129  void setLim(const yarp::sig::Matrix &_lim);
130 
135  double getTs() { return Ts; }
136 
141  const yarp::sig::Matrix& getLim() { return lim; }
142 
148  void reset(const yarp::sig::Vector &y0);
149 
154  const yarp::sig::Vector& get() const { return y; }
155 };
156 
157 
165 {
166 public:
176  static void addVectorToOption(yarp::os::Bottle &option, const char *key, const yarp::sig::Vector &val);
177 
190  static bool getVectorFromOption(const yarp::os::Bottle &options, const char *key, yarp::sig::Vector &val, int &size);
191 };
192 
193 
210 class parallelPID : public helperPID
211 {
212 private:
213  // Default constructor: not implemented.
214  parallelPID();
215 
216 protected:
217  yarp::sig::Vector Kp;
218  yarp::sig::Vector Ki;
219  yarp::sig::Vector Kd;
220 
221  yarp::sig::Vector Wp;
222  yarp::sig::Vector Wi;
223  yarp::sig::Vector Wd;
224 
225  yarp::sig::Vector N;
226  yarp::sig::Vector Tt;
227  yarp::sig::Matrix satLim;
228 
229  yarp::sig::Vector P;
230  yarp::sig::Vector I;
231  yarp::sig::Vector D;
232  yarp::sig::Vector u;
233  yarp::sig::Vector uSat;
234 
235  unsigned int dim;
236  double Ts;
237 
239  std::deque<Filter*> Der;
240 
241 public:
258  parallelPID(const double _Ts,
259  const yarp::sig::Vector &_Kp, const yarp::sig::Vector &_Ki, const yarp::sig::Vector &_Kd,
260  const yarp::sig::Vector &_Wp, const yarp::sig::Vector &_Wi, const yarp::sig::Vector &_Wd,
261  const yarp::sig::Vector &_N, const yarp::sig::Vector &_Tt, const yarp::sig::Matrix &_satLim);
262 
269  virtual const yarp::sig::Vector& compute(const yarp::sig::Vector &ref, const yarp::sig::Vector &fb);
270 
275  virtual void reset(const yarp::sig::Vector &u0);
276 
286  virtual void getOptions(yarp::os::Bottle &options);
287 
301  virtual void setOptions(const yarp::os::Bottle &options);
302 
306  ~parallelPID();
307 };
308 
309 
323 class seriesPID : public helperPID
324 {
325 private:
326  // Default constructor: not implemented.
327  seriesPID();
328 
329 protected:
330  yarp::sig::Vector Kp;
331  yarp::sig::Vector Ti;
332  yarp::sig::Vector Kd;
333 
334  yarp::sig::Vector N;
335  yarp::sig::Matrix satLim;
336 
337  yarp::sig::Vector e;
338  yarp::sig::Vector P;
339  yarp::sig::Vector I;
340  yarp::sig::Vector D;
341  yarp::sig::Vector u;
342  yarp::sig::Vector uSat;
343 
344  unsigned int dim;
345  double Ts;
346 
347  std::deque<Filter*> Int;
348  std::deque<Filter*> Der;
349 
350 public:
363  seriesPID(const double _Ts,
364  const yarp::sig::Vector &_Kp, const yarp::sig::Vector &_Ti, const yarp::sig::Vector &_Kd,
365  const yarp::sig::Vector &_N, const yarp::sig::Matrix &_satLim);
366 
373  virtual const yarp::sig::Vector& compute(const yarp::sig::Vector &ref, const yarp::sig::Vector &fb);
374 
378  virtual void reset();
379 
389  virtual void getOptions(yarp::os::Bottle &options);
390 
404  virtual void setOptions(const yarp::os::Bottle &options);
405 
409  ~seriesPID();
410 };
411 
412 }
413 
414 }
415 
416 #endif
417 
418 
419 
Integrator * Int
Definition: pids.h:238
yarp::sig::Vector u
Definition: pids.h:232
yarp::sig::Vector I
Definition: pids.h:339
yarp::sig::Vector Kd
Definition: pids.h:332
yarp::sig::Vector Ti
Definition: pids.h:331
yarp::sig::Matrix satLim
Definition: pids.h:227
General structure of parallel (non-interactive) PID.
Definition: pids.h:210
General structure of series (interactive) PID.
Definition: pids.h:323
yarp::sig::Vector Wd
Definition: pids.h:223
yarp::sig::Vector N
Definition: pids.h:334
yarp::sig::Vector D
Definition: pids.h:231
const yarp::sig::Vector & integrate(const yarp::sig::Vector &x)
Executes one-step integration of input vector.
Definition: pids.cpp:115
void setLim(const yarp::sig::Matrix &_lim)
Sets the output vector constraints matrix.
Definition: pids.cpp:104
yarp::sig::Vector Wp
Definition: pids.h:221
Integrator(const Integrator &I)
Creates a new Integrator from an already existing object.
Definition: pids.h:89
yarp::sig::Vector x_old
Definition: pids.h:56
void setTs(const double _Ts)
Sets the sample time.
Definition: pids.cpp:96
yarp::sig::Vector Kp
Definition: pids.h:217
yarp::sig::Vector y
Definition: pids.h:55
void reset(const yarp::sig::Vector &y0)
Resets the internal state and sets the output vector to the given value.
Definition: pids.cpp:128
bool getSaturation()
Returns the current saturation status.
Definition: pids.h:117
yarp::sig::Vector D
Definition: pids.h:340
static int v
Definition: iCub_Sim.cpp:47
void setSaturation(bool _applySat)
Sets the saturation status.
Definition: pids.cpp:87
yarp::sig::Vector saturate(const yarp::sig::Vector &v)
Definition: pids.cpp:67
Helper class providing useful methods to deal with pid options.
Definition: pids.h:164
unsigned int dim
Definition: pids.h:344
yarp::sig::Vector P
Definition: pids.h:229
yarp::sig::Vector I
Definition: pids.h:230
yarp::sig::Vector N
Definition: pids.h:225
yarp::sig::Vector uSat
Definition: pids.h:233
std::deque< Filter * > Int
Definition: pids.h:347
A class for defining a saturated integrator based on Tustin formula: .
Definition: pids.h:47
yarp::sig::Vector e
Definition: pids.h:337
yarp::sig::Vector Kd
Definition: pids.h:219
yarp::sig::Vector uSat
Definition: pids.h:342
yarp::sig::Matrix satLim
Definition: pids.h:335
yarp::sig::Vector Ki
Definition: pids.h:218
const yarp::sig::Matrix & getLim()
Returns the constraints matrix.
Definition: pids.h:141
unsigned int dim
Definition: pids.h:54
This file contains the definition of unique IDs for the body parts and the skin parts of the robot...
Integrator & operator=(const Integrator &I)
Copies a Integrator object into the current one.
Definition: pids.h:96
yarp::sig::Vector Wi
Definition: pids.h:222
yarp::sig::Matrix lim
Definition: pids.h:57
unsigned int dim
Definition: pids.h:235
std::deque< Filter * > Der
Definition: pids.h:239
yarp::sig::Vector Tt
Definition: pids.h:226
double getTs()
Returns the sample time.
Definition: pids.h:135
yarp::sig::Vector P
Definition: pids.h:338
void allocate(const Integrator &I)
Definition: pids.cpp:56
yarp::sig::Vector u
Definition: pids.h:341
yarp::sig::Vector Kp
Definition: pids.h:330
std::deque< Filter * > Der
Definition: pids.h:348