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 
122  virtual ~minJerkVelCtrlForIdealPlant();
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 
276  minJerkBaseGen& operator=(const minJerkBaseGen &z);
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 
387  minJerkTrajGen& operator=(const minJerkTrajGen &z);
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 
442  minJerkRefGen& operator=(const minJerkRefGen &z);
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 
const yarp::sig::Vector & getPos() const
Get the current position.
Definition: minJerkCtrl.h:298
IIR and FIR.
Definition: filters.h:76
virtual void reset(const yarp::sig::Vector &u0)=0
Resets the controller to a given value.
yarp::sig::Vector vel
Definition: minJerkCtrl.h:235
yarp::sig::Vector acc
Definition: minJerkCtrl.h:236
const yarp::sig::Vector & getVel() const
Get the current velocity.
Definition: minJerkCtrl.h:303
const dReal * pos
Definition: iCub_Sim.cpp:67
Abstract class for minimum-jerk controllers with velocity commands.
Definition: minJerkCtrl.h:46
yarp::sig::Vector lastRef
Definition: minJerkCtrl.h:237
double getTs() const
Get the sample time in seconds.
Definition: minJerkCtrl.h:320
const yarp::sig::Vector & getAcc() const
Get the current acceleration.
Definition: minJerkCtrl.h:308
_3f_vect_t acc
Definition: dataTypes.h:124
Generator of position, velocity and acceleration references that are approximately minimum jerk...
Definition: minJerkCtrl.h:406
virtual ~minJerkVelCtrl()
Destructor.
Definition: minJerkCtrl.h:70
yarp::sig::Vector pos
Definition: minJerkCtrl.h:234
double getT() const
Get the trajectory reference time in seconds (90% of steady-state value in t=_T, transient extinguish...
Definition: minJerkCtrl.h:315
Generator of approximately minimum jerk trajectories.
Definition: minJerkCtrl.h:351
Implements a minimum-jerk controller with velocity commands in the assumption that the plant can be m...
Definition: minJerkCtrl.h:81
Implements a minimum-jerk controller with velocity commands assuming a non ideal plant represented wi...
Definition: minJerkCtrl.h:135
This file contains the definition of unique IDs for the body parts and the skin parts of the robot...
virtual yarp::sig::Vector computeCmd(const double _T, const yarp::sig::Vector &e)=0
Computes the velocity command.
Base class for minimum jerk generators.
Definition: minJerkCtrl.h:227
virtual void computeCoeffs()=0
std::deque< ctrl::Filter * > F
Definition: minJerkCtrl.h:146