iCub-main
minJerkCtrl.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 __MINJERKCTRL_H__
23 #define __MINJERKCTRL_H__
24 
25 #include <string>
26 #include <deque>
27 
28 #include <yarp/os/Property.h>
29 #include <yarp/sig/Vector.h>
30 #include <yarp/sig/Matrix.h>
31 #include <iCub/ctrl/filters.h>
32 
33 
34 namespace iCub
35 {
36 
37 namespace ctrl
38 {
39 
47 {
48 protected:
49  virtual void computeCoeffs() = 0;
50 
51 public:
59  virtual yarp::sig::Vector computeCmd(const double _T, const yarp::sig::Vector &e) = 0;
60 
65  virtual void reset(const yarp::sig::Vector &u0) = 0;
66 
70  virtual ~minJerkVelCtrl() { }
71 };
72 
73 
82 {
83 private:
84  // Default constructor: not implemented.
86 
87 protected:
89 
90  double Ts;
91  double T;
92  int dim;
93 
94  virtual void computeCoeffs();
95 
96 public:
102  minJerkVelCtrlForIdealPlant(const double _Ts, const int _dim);
103 
111  virtual yarp::sig::Vector computeCmd(const double _T, const yarp::sig::Vector &e);
112 
117  virtual void reset(const yarp::sig::Vector &u0);
118 
123 };
124 
125 
136 {
137 private:
138  // Default constructor: not implemented.
140 
141 protected:
142  yarp::sig::Vector Kp;
143  yarp::sig::Vector Tz;
144  yarp::sig::Vector Tw;
145  yarp::sig::Vector Zeta;
146  std::deque<ctrl::Filter*> F;
147 
148  double Ts;
149  double T;
150  int dim;
151 
152  virtual void computeCoeffs();
153 
154 public:
160  minJerkVelCtrlForNonIdealPlant(const double _Ts, const int _dim);
161 
169  virtual yarp::sig::Vector computeCmd(const double _T, const yarp::sig::Vector &e);
170 
175  virtual void reset(const yarp::sig::Vector &u0);
176 
194  virtual void setPlantParameters(const yarp::os::Property &parameters,
195  const std::string &entryTag="dimension",
196  const yarp::os::Bottle &ordering=yarp::os::Bottle());
197 
212  virtual void getPlantParameters(yarp::os::Property &parameters,
213  const std::string &entryTag="dimension");
214 
219 };
220 
221 
228 {
229 protected:
230  Filter* posFilter; // filter used to compute the position
231  Filter* velFilter; // filter used to compute the velocity
232  Filter* accFilter; // filter used to compute the acceleration
233 
234  yarp::sig::Vector pos; // current position
235  yarp::sig::Vector vel; // current velocity
236  yarp::sig::Vector acc; // current acceleration
237  yarp::sig::Vector lastRef; // last reference position
238 
239  double Ts; // sample time in seconds
240  double T; // trajectory reference time in seconds
241  unsigned int dim; // dimension of the controlled variable
242 
243  virtual void computeCoeffs()=0; // compute the filter coefficients
244 
245 public:
253  minJerkBaseGen(const unsigned int _dim, const double _Ts, const double _T);
254 
262  minJerkBaseGen(const yarp::sig::Vector &y0, const double _Ts, const double _T);
263 
270 
277 
281  virtual ~minJerkBaseGen();
282 
287  virtual void init(const yarp::sig::Vector &y0);
288 
293  virtual void computeNextValues(const yarp::sig::Vector &yd)=0;
294 
298  const yarp::sig::Vector& getPos() const { return pos; }
299 
303  const yarp::sig::Vector& getVel() const { return vel; }
304 
308  const yarp::sig::Vector& getAcc() const { return acc; }
309 
315  double getT() const { return T; }
316 
320  double getTs() const { return Ts; }
321 
328  bool setT(const double _T);
329 
335  bool setTs(const double _Ts);
336 };
337 
338 
352 {
353 protected:
354  virtual void computeCoeffs();
355 
356 public:
364  minJerkTrajGen(const unsigned int _dim, const double _Ts, const double _T);
365 
373  minJerkTrajGen(const yarp::sig::Vector &y0, const double _Ts, const double _T);
374 
381 
388 
393  void computeNextValues(const yarp::sig::Vector &yd);
394 };
395 
396 
407 {
408 protected:
409  void computeCoeffs();
410 
411 public:
419  minJerkRefGen(const unsigned int _dim, const double _Ts, const double _T);
420 
428  minJerkRefGen(const yarp::sig::Vector &y0, const double _Ts, const double _T);
429 
435  minJerkRefGen(const minJerkRefGen &z);
436 
443 
448  void computeNextValues(const yarp::sig::Vector &y);
449 
455  virtual void computeNextValues(const yarp::sig::Vector &y,
456  const yarp::sig::Vector &yd);
457 };
458 
459 }
460 
461 }
462 
463 #endif
464 
465 
466 
iCub::ctrl::minJerkBaseGen::computeCoeffs
virtual void computeCoeffs()=0
iCub::ctrl::minJerkBaseGen::getTs
double getTs() const
Get the sample time in seconds.
Definition: minJerkCtrl.h:320
iCub::ctrl::minJerkTrajGen::computeCoeffs
virtual void computeCoeffs()
Definition: minJerkCtrl.cpp:407
iCub
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.
Definition: emotionInterface.h:17
iCub::ctrl::minJerkBaseGen::getPos
const yarp::sig::Vector & getPos() const
Get the current position.
Definition: minJerkCtrl.h:298
iCub::ctrl::minJerkBaseGen::setTs
bool setTs(const double _Ts)
Set the sample time.
Definition: minJerkCtrl.cpp:363
iCub::ctrl::minJerkVelCtrlForNonIdealPlant
Definition: minJerkCtrl.h:135
iCub::ctrl::minJerkBaseGen::pos
yarp::sig::Vector pos
Definition: minJerkCtrl.h:234
iCub::ctrl::minJerkBaseGen::operator=
minJerkBaseGen & operator=(const minJerkBaseGen &z)
Assignment operator.
Definition: minJerkCtrl.cpp:315
iCub::ctrl::minJerkVelCtrlForIdealPlant::dim
int dim
Definition: minJerkCtrl.h:92
iCub::ctrl::minJerkBaseGen::vel
yarp::sig::Vector vel
Definition: minJerkCtrl.h:235
iCub::ctrl::minJerkVelCtrlForNonIdealPlant::getPlantParameters
virtual void getPlantParameters(yarp::os::Property &parameters, const std::string &entryTag="dimension")
Allows user to retrieve plant parameters.
Definition: minJerkCtrl.cpp:241
iCub::ctrl::minJerkVelCtrlForNonIdealPlant::computeCmd
virtual yarp::sig::Vector computeCmd(const double _T, const yarp::sig::Vector &e)
Computes the velocity command.
Definition: minJerkCtrl.cpp:169
e
e
Definition: compute_ekf_fast.m:13
iCub::ctrl::minJerkBaseGen::~minJerkBaseGen
virtual ~minJerkBaseGen()
Destructor.
Definition: minJerkCtrl.cpp:306
iCub::ctrl::minJerkVelCtrlForNonIdealPlant::Kp
yarp::sig::Vector Kp
Definition: minJerkCtrl.h:142
iCub::ctrl::minJerkVelCtrlForNonIdealPlant::Ts
double Ts
Definition: minJerkCtrl.h:148
z
z
Definition: show_eyes_axes.m:22
iCub::ctrl::minJerkBaseGen::dim
unsigned int dim
Definition: minJerkCtrl.h:241
iCub::ctrl::minJerkBaseGen::getT
double getT() const
Get the trajectory reference time in seconds (90% of steady-state value in t=_T, transient extinguish...
Definition: minJerkCtrl.h:315
iCub::ctrl::minJerkRefGen::operator=
minJerkRefGen & operator=(const minJerkRefGen &z)
Assignment operator.
Definition: minJerkCtrl.cpp:487
iCub::ctrl::minJerkTrajGen
Definition: minJerkCtrl.h:351
iCub::ctrl::minJerkVelCtrlForIdealPlant::computeCoeffs
virtual void computeCoeffs()
Definition: minJerkCtrl.cpp:34
iCub::ctrl::minJerkVelCtrlForIdealPlant::computeCmd
virtual yarp::sig::Vector computeCmd(const double _T, const yarp::sig::Vector &e)
Computes the velocity command.
Definition: minJerkCtrl.cpp:72
iCub::ctrl::minJerkVelCtrlForNonIdealPlant::Tz
yarp::sig::Vector Tz
Definition: minJerkCtrl.h:143
filters.h
iCub::ctrl::minJerkRefGen::computeCoeffs
void computeCoeffs()
Definition: minJerkCtrl.cpp:496
iCub::ctrl::minJerkBaseGen::minJerkBaseGen
minJerkBaseGen(const unsigned int _dim, const double _Ts, const double _T)
Constructor.
Definition: minJerkCtrl.cpp:273
iCub::ctrl::minJerkTrajGen::operator=
minJerkTrajGen & operator=(const minJerkTrajGen &z)
Assignment operator.
Definition: minJerkCtrl.cpp:398
iCub::ctrl::minJerkVelCtrl::computeCmd
virtual yarp::sig::Vector computeCmd(const double _T, const yarp::sig::Vector &e)=0
Computes the velocity command.
iCub::ctrl::minJerkBaseGen::posFilter
Filter * posFilter
Definition: minJerkCtrl.h:230
iCub::ctrl::minJerkBaseGen::computeNextValues
virtual void computeNextValues(const yarp::sig::Vector &yd)=0
Compute the next position, velocity and acceleration.
iCub::ctrl::minJerkBaseGen::setT
bool setT(const double _T)
Set the trajectory reference time (90% of steady-state value in t=_T, transient extinguished for t>=1...
Definition: minJerkCtrl.cpp:352
iCub::ctrl::Filter
Definition: filters.h:76
iCub::ctrl::minJerkBaseGen::lastRef
yarp::sig::Vector lastRef
Definition: minJerkCtrl.h:237
iCub::ctrl::minJerkRefGen
Definition: minJerkCtrl.h:406
iCub::ctrl::minJerkVelCtrlForNonIdealPlant::dim
int dim
Definition: minJerkCtrl.h:150
iCub::ctrl::minJerkBaseGen::acc
yarp::sig::Vector acc
Definition: minJerkCtrl.h:236
y
y
Definition: show_eyes_axes.m:21
iCub::ctrl::minJerkBaseGen
Definition: minJerkCtrl.h:227
iCub::ctrl::minJerkVelCtrlForIdealPlant::T
double T
Definition: minJerkCtrl.h:91
iCub::ctrl::minJerkTrajGen::minJerkTrajGen
minJerkTrajGen(const unsigned int _dim, const double _Ts, const double _T)
Constructor.
Definition: minJerkCtrl.cpp:374
iCub::ctrl::minJerkBaseGen::init
virtual void init(const yarp::sig::Vector &y0)
Initialize the trajectory.
Definition: minJerkCtrl.cpp:335
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
iCub::ctrl::minJerkVelCtrl::reset
virtual void reset(const yarp::sig::Vector &u0)=0
Resets the controller to a given value.
iCub::ctrl::minJerkVelCtrlForIdealPlant::~minJerkVelCtrlForIdealPlant
virtual ~minJerkVelCtrlForIdealPlant()
Destructor.
Definition: minJerkCtrl.cpp:92
iCub::ctrl::minJerkVelCtrlForIdealPlant::F
ctrl::Filter * F
Definition: minJerkCtrl.h:88
iCub::ctrl::minJerkVelCtrlForNonIdealPlant::~minJerkVelCtrlForNonIdealPlant
virtual ~minJerkVelCtrlForNonIdealPlant()
Destructor.
Definition: minJerkCtrl.cpp:263
iCub::ctrl::minJerkRefGen::minJerkRefGen
minJerkRefGen(const unsigned int _dim, const double _Ts, const double _T)
Constructor.
Definition: minJerkCtrl.cpp:463
iCub::ctrl::minJerkVelCtrlForNonIdealPlant::reset
virtual void reset(const yarp::sig::Vector &u0)
Resets the controller to a given value.
Definition: minJerkCtrl.cpp:190
iCub::ctrl::minJerkVelCtrlForNonIdealPlant::Zeta
yarp::sig::Vector Zeta
Definition: minJerkCtrl.h:145
iCub::ctrl::minJerkVelCtrlForIdealPlant::reset
virtual void reset(const yarp::sig::Vector &u0)
Resets the controller to a given value.
Definition: minJerkCtrl.cpp:85
iCub::ctrl::minJerkBaseGen::accFilter
Filter * accFilter
Definition: minJerkCtrl.h:232
iCub::ctrl::minJerkVelCtrlForNonIdealPlant::setPlantParameters
virtual void setPlantParameters(const yarp::os::Property &parameters, const std::string &entryTag="dimension", const yarp::os::Bottle &ordering=yarp::os::Bottle())
Allows user to assign values to plant parameters.
Definition: minJerkCtrl.cpp:202
iCub::ctrl::minJerkVelCtrlForIdealPlant::Ts
double Ts
Definition: minJerkCtrl.h:90
iCub::ctrl::minJerkVelCtrl::computeCoeffs
virtual void computeCoeffs()=0
iCub::ctrl::minJerkVelCtrlForIdealPlant
Definition: minJerkCtrl.h:81
iCub::ctrl::minJerkBaseGen::getAcc
const yarp::sig::Vector & getAcc() const
Get the current acceleration.
Definition: minJerkCtrl.h:308
iCub::ctrl::minJerkBaseGen::getVel
const yarp::sig::Vector & getVel() const
Get the current velocity.
Definition: minJerkCtrl.h:303
iCub::ctrl::minJerkTrajGen::computeNextValues
void computeNextValues(const yarp::sig::Vector &yd)
Compute the next position, velocity and acceleration.
Definition: minJerkCtrl.cpp:447
iCub::ctrl::minJerkVelCtrlForNonIdealPlant::T
double T
Definition: minJerkCtrl.h:149
iCub::ctrl::minJerkVelCtrl::~minJerkVelCtrl
virtual ~minJerkVelCtrl()
Destructor.
Definition: minJerkCtrl.h:70
iCub::ctrl::minJerkVelCtrlForNonIdealPlant::Tw
yarp::sig::Vector Tw
Definition: minJerkCtrl.h:144
iCub::ctrl::minJerkVelCtrl
Definition: minJerkCtrl.h:46
iCub::ctrl::minJerkVelCtrlForNonIdealPlant::F
std::deque< ctrl::Filter * > F
Definition: minJerkCtrl.h:146
iCub::ctrl::minJerkBaseGen::T
double T
Definition: minJerkCtrl.h:240
iCub::ctrl::minJerkRefGen::computeNextValues
void computeNextValues(const yarp::sig::Vector &y)
Computes the position, velocity and acceleration references.
Definition: minJerkCtrl.cpp:542
iCub::ctrl::minJerkVelCtrlForNonIdealPlant::computeCoeffs
virtual void computeCoeffs()
Definition: minJerkCtrl.cpp:115
iCub::ctrl::minJerkBaseGen::Ts
double Ts
Definition: minJerkCtrl.h:239
iCub::ctrl::minJerkBaseGen::velFilter
Filter * velFilter
Definition: minJerkCtrl.h:231