14#include <yarp/os/Time.h>
15#include <yarp/math/Math.h>
19using namespace yarp::os;
20using namespace yarp::sig;
21using namespace yarp::math;
26minJerkVelCtrlForIdealPlant::minJerkVelCtrlForIdealPlant(
const double _Ts,
const int _dim) :
27 Ts(_Ts), dim(_dim),
T(1.0),
F(NULL)
38 double twoOnTs=2.0/
Ts;
42 double a=-150.765868956161/T3;
43 double b=-84.9812819469538/T2;
44 double c=-15.9669610709384/
T;
50 double c1=twoOnTs*(twoOnTs-c)-b;
58 den[1]=-2.0*(twoOnTs*twoOnTs+b)/c1;
59 den[2]=(twoOnTs*(twoOnTs+c)-b)/c1;
63 Vector e0(
dim); e0=0.0;
99minJerkVelCtrlForNonIdealPlant::minJerkVelCtrlForNonIdealPlant(
const double _Ts,
const int _dim) :
100 Ts(_Ts), dim(_dim),
T(1.0)
107 for (
int i=0; i<
dim; i++)
125 double a=-150.765868956161/T3;
126 double b=-84.9812819469538/T2;
127 double c=-15.9669610709384/
T;
131 double _num_0=3.0*Ts3;
132 double _den_0=4.0*
Ts;
133 double _den_1=2.0*Ts2;
134 double _den_2=_den_1*c;
136 double _den_4=3.0*_den_3;
138 for (
int i=0; i<
dim; i++)
141 double _num_1=4.0*
Zeta[i]*Ts2*
Tw[i];
142 double _num_2=4.0*
Ts*
Tw[i]*
Tw[i];
143 num[0]=a * (Ts3 + _num_1 + _num_2);
144 num[1]=a * (_num_0 + _num_1 - _num_2);
145 num[2]=a * (_num_0 - _num_1 - _num_2);
146 num[3]=a * (Ts3 - _num_1 + _num_2);
148 double _den_5=_den_1*
Tz[i]*b;
149 double _den_6=_den_0*
Tz[i]*c;
150 double _den_7=8.0*
Tz[i];
151 double _den_8=3.0*_den_7;
152 den[0]=
Kp[i] * (_den_3 - _den_7 - _den_0 + _den_2 + _den_5 + _den_6);
153 den[1]=
Kp[i] * (_den_0 + _den_8 + _den_4 + _den_2 + _den_5 - _den_6);
154 den[2]=
Kp[i] * (_den_0 - _den_8 + _den_4 - _den_2 - _den_5 - _den_6);
155 den[3]=
Kp[i] * (_den_7 - _den_0 + _den_3 - _den_2 - _den_5 + _den_6);
159 Vector e0(1); e0=0.0;
163 F[i]->adjustCoeffs(num,den);
177 Vector
y(
dim), _e(1);
178 for (
int i=0; i<
dim; i++)
181 Vector _y=
F[i]->filt(_e);
193 for (
int i=0; i<
dim; i++)
203 const string &entryTag,
204 const Bottle &ordering)
212 int len=ordering.size()==0?
dim:(int)ordering.size();
213 for (
int i=0; i<len; i++)
216 entry<<entryTag<<
"_"<<(ordering.size()==0?i:ordering.get(i).asInt32());
217 if (parameters.check(entry.str()))
219 if (Bottle *options=parameters.find(entry.str()).asList())
221 if (options->check(
"Kp"))
222 Kp[i]=options->find(
"Kp").asFloat64();
224 if (options->check(
"Tz"))
225 Tz[i]=options->find(
"Tz").asFloat64();
227 if (options->check(
"Tw"))
228 Tw[i]=options->find(
"Tw").asFloat64();
230 if (options->check(
"Zeta"))
231 Zeta[i]=options->find(
"Zeta").asFloat64();
242 const string &entryTag)
245 for (
int i=0; i<
dim; i++)
247 entry<<
"("<<entryTag<<
"_"<<i<<
" (";
250 prop.put(
"Kp",
Kp[i]);
251 prop.put(
"Tz",
Tz[i]);
252 prop.put(
"Tw",
Tw[i]);
253 prop.put(
"Zeta",
Zeta[i]);
255 entry<<prop.toString()<<
")) ";
258 parameters.fromString(entry.str());
265 for (
size_t i=0; i<
F.size(); i++)
274 :dim(_dim), Ts(_Ts),
T(_T)
283 :dim((unsigned int)y0.size()), Ts(_Ts),
T(_T)
411 double a = -150.765868956161/(
T*
T*
T);
412 double b = -84.9812819469538/(
T*
T);
413 double c = -15.9669610709384/
T;
417 double n = 2.0*b*
Ts*
Ts;
419 Vector num = cat(
p, 3.0*
p, 3.0*
p,
p);
420 Vector den = cat(m+
n+
p-8.0, -m+
n+3.0*
p+24.0, -m-
n+3.0*
p-24.0, m-
n+
p+8.0);
428 num = cat(
p,
p, -
p, -
p);
437 num = cat(
p, -
p, -
p,
p);
500 double a = -150.765868956161/(
T*
T*
T);
501 double b = -84.9812819469538/(
T*
T);
502 double c = -15.9669610709384/
T;
506 double n = 2.0*b*
Ts*
Ts;
508 Vector num = cat(
p, 3.0*
p, 3.0*
p,
p);
509 Vector den = cat(m+
n+
p-8.0, -m+
n+3.0*
p+24.0, -m-
n+3.0*
p-24.0, m-
n+
p+8.0);
516 double twoOnTs=2.0/
Ts;
517 double c1=twoOnTs*(twoOnTs-c)-b;
519 num = cat(c2, 2.0*c2, c2);
520 den = cat(1.0, -2.0*(twoOnTs*twoOnTs+b)/c1, (twoOnTs*(twoOnTs+c)-b)/c1);
531 num = cat(-
p, 0.0,
p);
532 den = cat(4.0-m-
n, -8.0+m-2.0*
n, 4.0-
n);
virtual const yarp::sig::Vector & filt(const yarp::sig::Vector &u)
Performs filtering on the actual input.
bool adjustCoeffs(const yarp::sig::Vector &num, const yarp::sig::Vector &den)
Modifies the values of existing filter coefficients without varying their lengths.
virtual void init(const yarp::sig::Vector &y0)
Internal state reset.
Base class for minimum jerk generators.
virtual void computeCoeffs()=0
minJerkBaseGen(const unsigned int _dim, const double _Ts, const double _T)
Constructor.
virtual ~minJerkBaseGen()
Destructor.
minJerkBaseGen & operator=(const minJerkBaseGen &z)
Assignment operator.
bool setT(const double _T)
Set the trajectory reference time (90% of steady-state value in t=_T, transient extinguished for t>=1...
yarp::sig::Vector lastRef
bool setTs(const double _Ts)
Set the sample time.
virtual void init(const yarp::sig::Vector &y0)
Initialize the trajectory.
Generator of position, velocity and acceleration references that are approximately minimum jerk.
void computeNextValues(const yarp::sig::Vector &y)
Computes the position, velocity and acceleration references.
minJerkRefGen(const unsigned int _dim, const double _Ts, const double _T)
Constructor.
minJerkRefGen & operator=(const minJerkRefGen &z)
Assignment operator.
Generator of approximately minimum jerk trajectories.
minJerkTrajGen & operator=(const minJerkTrajGen &z)
Assignment operator.
virtual void computeCoeffs()
void computeNextValues(const yarp::sig::Vector &yd)
Compute the next position, velocity and acceleration.
minJerkTrajGen(const unsigned int _dim, const double _Ts, const double _T)
Constructor.
virtual void reset(const yarp::sig::Vector &u0)
Resets the controller to a given value.
virtual ~minJerkVelCtrlForIdealPlant()
Destructor.
virtual void computeCoeffs()
virtual yarp::sig::Vector computeCmd(const double _T, const yarp::sig::Vector &e)
Computes the velocity command.
virtual void computeCoeffs()
virtual yarp::sig::Vector computeCmd(const double _T, const yarp::sig::Vector &e)
Computes the velocity command.
virtual ~minJerkVelCtrlForNonIdealPlant()
Destructor.
virtual void reset(const yarp::sig::Vector &u0)
Resets the controller to a given value.
std::deque< ctrl::Filter * > F
virtual void setPlantParameters(const yarp::os::Property ¶meters, const std::string &entryTag="dimension", const yarp::os::Bottle &ordering=yarp::os::Bottle())
Allows user to assign values to plant parameters.
virtual void getPlantParameters(yarp::os::Property ¶meters, const std::string &entryTag="dimension")
Allows user to retrieve plant parameters.