17#include <yarp/math/Math.h>
18#include <yarp/math/SVD.h>
22using namespace yarp::os;
24using namespace yarp::sig;
25using namespace yarp::math;
34 init(0.01,1.0,1.0,1e5,x0);
40 const double R,
const double P0,
48 C.resize(1,4);
C(0,0)=1.0;
56 x=
_x=x0.subVector(0,3);
73 x=
_x=x0.subVector(0,3);
92 double _exp_1=1.0-_exp;
94 double _tmp_1=(
Ts*
x3-_exp_1)/_x3_2;
113 P=
F*
P*
F.transposed()+
Q;
116 Matrix tmpMat=
C*
P*
Ct;
117 Matrix K=
P*
Ct/(tmpMat(0,0)+
R);
136 PeriodicThread(1.0), velEst(32,4.0), accEst(32,4.0),
137 trajGen(1,1.0,1.0), intErr(1.0,Vector(2,0.0)),
done(2)
152 if (driver.isValid() && options.check(
"joint"))
165 joint=options.find(
"joint").asInt32();
166 setPeriod(options.check(
"Ts",Value(0.01)).asFloat64());
168 T=options.check(
"T",Value(2.0)).asFloat64();
169 Kp=options.check(
"Kp",Value(10.0)).asFloat64();
170 Ki=options.check(
"Ki",Value(250.0)).asFloat64();
171 Kd=options.check(
"Kd",Value(15.0)).asFloat64();
172 vel_thres=fabs(options.check(
"vel_thres",Value(5.0)).asFloat64());
173 e_thres=fabs(options.check(
"e_thres",Value(1.0)).asFloat64());
176 if (Bottle *pB=options.find(
"gamma").asList())
178 size_t len=std::min(
gamma.length(),(
size_t)pB->size());
179 for (
size_t i=0; i<len; i++)
180 gamma[i]=pB->get(i).asFloat64();
184 if (Bottle *pB=options.find(
"stiction").asList())
186 size_t len=std::min(
stiction.length(),(
size_t)pB->size());
187 for (
size_t i=0; i<len; i++)
203 if (options.check(
"joint"))
204 joint=options.find(
"joint").asInt32();
206 if (options.check(
"Ts"))
207 setPeriod(options.find(
"Ts").asFloat64());
209 if (options.check(
"T"))
210 T=options.find(
"T").asFloat64();
212 if (options.check(
"Kp"))
213 Kp=options.find(
"Kp").asFloat64();
215 if (options.check(
"Ki"))
216 Ki=options.find(
"Ki").asFloat64();
218 if (options.check(
"Kd"))
219 Kd=options.find(
"Kd").asFloat64();
221 if (options.check(
"vel_thres"))
222 vel_thres=options.find(
"vel_thres").asFloat64();
224 if (options.check(
"e_thres"))
225 e_thres=options.find(
"e_thres").asFloat64();
227 if (options.check(
"gamma"))
229 if (Bottle *pB=options.find(
"gamma").asList())
231 size_t len=std::min(
gamma.length(),(
size_t)pB->size());
232 for (
size_t i=0; i<len; i++)
233 gamma[i]=pB->get(i).asFloat64();
237 if (options.check(
"stiction"))
239 if (Bottle *pB=options.find(
"stiction").asList())
241 size_t len=std::min(
stiction.length(),(
size_t)pB->size());
242 for (
size_t i=0; i<len; i++)
272 imod->setControlMode(
joint,VOCAB_CM_PWM);
287 Vector _Kp(1,
Kp), _Ki(1,
Ki), _Kd(1,
Kd);
288 Vector _Wp(1,1.0), _Wi(1,1.0), _Wd(1,1.0);
289 Vector _N(1,10.0), _Tt(1,1.0);
292 ipid->getPid(VOCAB_PIDTYPE_POSITION,
joint,&pidInfo);
293 dpos_dV=(pidInfo.kp>=0.0?-1.0:1.0);
296 _satLim(0,0)=-pidInfo.max_int;
297 _satLim(0,1)=pidInfo.max_int;
301 pid=
new parallelPID(getPeriod(),_Kp,_Ki,_Kd,_Wp,_Wi,_Wd,_N,_Tt,_satLim);
325 double t=Time::now()-
t0;
340 double u=fw+pid_out[0];
342 Vector adaptGate(
stiction.length(),0.0);
353 Vector e_mean=cumErr/t;
354 if (yarp::math::norm(e_mean)>
e_thres)
370 info.unput(
"voltage");
info.put(
"voltage",u);
385 imod->setControlMode(
joint,VOCAB_CM_POSITION);
398 lock_guard<mutex> lck(
mtx);
423 lock_guard<mutex> lck(
mtx);
435 lock_guard<mutex> lck(
mtx);
443 predictor(Matrix(2,2),Matrix(2,1),Matrix(1,2),
444 Matrix(2,2),Matrix(1,1))
461 if (!driver.isValid())
478 Bottle &optGeneral=options.findGroup(
"general");
479 if (optGeneral.isNull())
482 if (!optGeneral.check(
"joint"))
485 joint=optGeneral.find(
"joint").asInt32();
487 if (optGeneral.check(
"port"))
489 string name=optGeneral.find(
"port").asString();
493 if (!
port.open(name))
498 Bottle &optPlant=options.findGroup(
"plant_estimation");
499 if (optPlant.isNull())
503 ipid->getPid(VOCAB_PIDTYPE_POSITION,
joint,&pidInfo);
504 dpos_dV=(pidInfo.kp>=0.0?-1.0:1.0);
512 x0[2]=optPlant.check(
"tau",Value(1.0)).asFloat64();
513 x0[3]=optPlant.check(
"K",Value(1.0)).asFloat64();
515 double Ts=optPlant.check(
"Ts",Value(0.01)).asFloat64();
516 double Q=optPlant.check(
"Q",Value(1.0)).asFloat64();
517 double R=optPlant.check(
"R",Value(1.0)).asFloat64();
518 P0=optPlant.check(
"P0",Value(1e5)).asFloat64();
519 max_pwm=optPlant.check(
"max_pwm",Value(800)).asFloat64();
527 Bottle &optStiction=options.findGroup(
"stiction_estimation");
528 if (!optStiction.isNull())
530 Property propStiction(optStiction.toString().c_str());
533 propStiction.unput(
"joint");
534 propStiction.put(
"joint",
joint);
555 imod->setControlMode(
joint,VOCAB_CM_PWM);
570 imod->setControlMode(
joint,VOCAB_CM_PWM);
594 imod->setControlMode(
joint,VOCAB_CM_POSITION_DIRECT);
599 imod->setControlMode(
joint,VOCAB_CM_POSITION);
600 ipos->setRefAcceleration(
joint,std::numeric_limits<double>::max());
619 double t=Time::now();
626 if ((enc>
x_max) || timeoutExpired)
633 else if ((enc<
x_min) || timeoutExpired)
648 double t=Time::now();
653 lock_guard<mutex> lck(
mtx);
668 if (
port.getOutputCount()>0)
670 Vector &info=
port.prepare();
702 if (
port.getOutputCount()>0)
704 Vector &info=
port.prepare();
711 info=cat(info,Vector(4,0.0));
725 if (
port.getOutputCount()>0)
727 Property stiction_info;
730 Vector &info=
port.prepare();
734 info[1]=stiction_info.find(
"voltage").asFloat64();
735 info[2]=stiction_info.find(
"position").asFloat64();
736 info[3]=stiction_info.find(
"reference").asFloat64();
740 info=cat(info,results);
741 info=cat(info,Vector(3,0.0));
781 if (
port.getOutputCount()>0)
783 Vector &info=
port.prepare();
789 ipid->getPidReference(VOCAB_PIDTYPE_POSITION,
joint,&info[3]);
791 info=cat(info,Vector(4,0.0));
813 imod->setControlMode(
joint,VOCAB_CM_POSITION);
841 if (!options.check(
"tau") || !options.check(
"K") ||
842 !options.check(
"type") || !options.check(
"f_c"))
845 double tau=options.find(
"tau").asFloat64();
846 double K=options.find(
"K").asFloat64();
847 string type=options.check(
"type",Value(
"PI")).asString();
848 double omega_c=2.0*
M_PI*options.find(
"f_c").asFloat64();
854 Kp=(omega_c/K)*sqrt(1.0+omega_c*omega_c*tau*tau);
860 double T_dr=1.0/omega_c;
861 if (options.check(
"T_dr"))
862 T_dr=options.find(
"T_dr").asFloat64();
864 double tau_dr=T_dr/3.0;
865 double omega_dr=1.0/tau_dr;
868 omega_dr=std::min(omega_dr,1.0/(3.0*tau));
870 Kp=(omega_c/K)*sqrt(1.0+omega_c*omega_c*tau*tau);
871 Ki=omega_dr*(Kp-(omega_dr*(1.0-omega_dr*tau))/K);
874 Ki=std::min(Ki,sqrt(10.0)*omega_c*Kp);
877 Ki=std::min(Ki,Kp/tau);
883 results.put(
"Kp",Kp);
884 results.put(
"Ki",Ki);
896 max_time=options.check(
"max_time",Value(0.0)).asFloat64();
897 switch_timeout=options.check(
"switch_timeout",Value(0.0)).asFloat64();
900 return PeriodicThread::start();
907 if (!
configured || !options.check(
"tau") || !options.check(
"K"))
910 max_time=options.check(
"max_time",Value(0.0)).asFloat64();
911 switch_timeout=options.check(
"switch_timeout",Value(0.0)).asFloat64();
914 double tau=options.find(
"tau").asFloat64();
915 double K=options.find(
"K").asFloat64();
916 double Ts=getPeriod();
920 double Q=options.check(
"Q",Value(1.0)).asFloat64();
921 double R=options.check(
"R",Value(1.0)).asFloat64();
922 double P0=options.check(
"P0",Value(this->P0)).asFloat64();
925 double _exp=
exp(-Ts*a);
926 double _exp_1=1.0-_exp;
933 B(0,0)=b*(a*Ts-_exp_1)/(a*a);
948 return PeriodicThread::start();
958 Property opt=options;
959 max_time=opt.check(
"max_time",Value(0.0)).asFloat64();
966 return PeriodicThread::start();
976 max_time=options.check(
"max_time",Value(0.0)).asFloat64();
982 double Kp=options.find(
"Kp").asFloat64();
983 double Ki=options.check(
"Ki",Value(0.0)).asFloat64();
984 double Kd=options.check(
"Kd",Value(0.0)).asFloat64();
985 Kp=(Kp*
pidOld.kp>0.0)?Kp:-Kp;
986 Ki=(Ki*
pidOld.ki>0.0)?Ki:-Ki;
987 Kd=(Kd*
pidOld.kd>0.0)?Kd:-Kd;
991 pidNew.setStictionValues(0.0,0.0);
993 if (options.check(
"scale"))
994 pidNew.setScale(options.find(
"scale").asInt32());
1003 if (options.check(
"stiction"))
1005 if (Bottle *pB=options.find(
"stiction").asList())
1020 return PeriodicThread::start();
1030 return !isRunning();
1055 lock_guard<mutex> lck(
mtx);
1062 results.put(
"tau",params[0]);
1063 results.put(
"K",params[1]);
1073 results.put(
"position",response[0]);
1074 results.put(
"velocity",response[1]);
1089 Value val; val.fromString(str.str().c_str());
1090 results.put(
"stiction",val);
1100 ipid->getPidReference(VOCAB_PIDTYPE_POSITION,
joint,&info[2]);
1102 results.put(
"voltage",info[0]);
1103 results.put(
"position",info[1]);
1104 results.put(
"reference",info[2]);
Basic element for adaptive polynomial fitting.
yarp::sig::Vector estimate()
Execute the algorithm upon the elements list, with the max deviation threshold given by D.
const yarp::sig::Vector & integrate(const yarp::sig::Vector &x)
Executes one-step integration of input vector.
void reset(const yarp::sig::Vector &y0)
Resets the internal state and sets the output vector to the given value.
void setTs(const double _Ts)
Sets the sample time.
bool set_Q(const yarp::sig::Matrix &_Q)
Returns the process noise covariance matrix.
bool set_R(const yarp::sig::Matrix &_R)
Returns the measurement noise covariance matrix.
bool init(const yarp::sig::Vector &_x0, const yarp::sig::Matrix &_P0)
Set initial state and error covariance.
const yarp::sig::Vector & get_x() const
Returns the estimated state.
bool set_A(const yarp::sig::Matrix &_A)
Returns the state transition matrix.
bool set_B(const yarp::sig::Matrix &_B)
Returns the input matrix.
const yarp::sig::Vector & correct(const yarp::sig::Vector &z)
Corrects the current estimation of the state vector given the current measurement.
bool set_H(const yarp::sig::Matrix &_H)
Returns the measurement transition matrix.
const yarp::sig::Vector & predict(const yarp::sig::Vector &u)
Predicts the next state vector given the current input.
const yarp::sig::Matrix & get_P() const
Returns the estimated state covariance.
virtual bool isDone()
Check the status of the current ongoing operation.
OnlineCompensatorDesign()
Default constructor.
virtual bool tuneController(const yarp::os::Property &options, yarp::os::Property &results)
Tune the controller once given the plant characteristics.
void commandJoint(double &enc, double &u)
yarp::dev::IPidControl * ipid
yarp::dev::IPWMControl * ipwm
virtual bool startStictionEstimation(const yarp::os::Property &options)
Start off the stiction estimation procedure.
yarp::dev::IEncoders * ienc
OnlineStictionEstimator stiction
std::condition_variable cv_doneEvent
yarp::dev::IPositionControl * ipos
bool controller_validation_ref_square
yarp::dev::IControlMode * imod
yarp::dev::ICurrentControl * icur
virtual bool startPlantEstimation(const yarp::os::Property &options)
Start off the plant estimation procedure.
enum iCub::ctrl::OnlineCompensatorDesign::@2 mode
double controller_validation_stiction_down
virtual bool startPlantValidation(const yarp::os::Property &options)
Start off the plant validation procedure.
virtual bool getResults(yarp::os::Property &results)
Retrieve the results of the current ongoing operation.
virtual bool waitUntilDone()
Wait until the current ongoing operation is accomplished.
yarp::dev::IPositionDirect * idir
double controller_validation_ref_sustain_time
yarp::dev::IControlLimits * ilim
int controller_validation_cycles_to_switch
OnlineDCMotorEstimator plant
virtual bool configure(yarp::dev::PolyDriver &driver, const yarp::os::Property &options)
Configure the design.
double controller_validation_ref_period
int controller_validation_num_cycles
yarp::os::BufferedPort< yarp::sig::Vector > port
bool controller_validation_stiction_yarp
double controller_validation_stiction_up
virtual ~OnlineCompensatorDesign()
Destructor.
virtual bool startControllerValidation(const yarp::os::Property &options)
Start off the controller validation procedure.
yarp::sig::Vector meanParams
yarp::sig::Vector estimate(const double u, const double y)
Estimate the state vector given the current input and the current measurement.
yarp::sig::Vector get_parameters() const
Return the system parameters.
bool init(const double Ts, const double Q, const double R, const double P0, const yarp::sig::Vector &x0)
Initialize the estimation.
OnlineDCMotorEstimator()
Default constructor.
yarp::sig::Vector get_x() const
Return the estimated state.
OnlineStictionEstimator()
Default constructor.
yarp::dev::IPidControl * ipid
yarp::sig::Vector stiction
virtual void stopEstimation()
Stop the estimation procedure.
virtual bool waitUntilDone()
Wait until the condition |e_mean|<e_thres is met.
virtual bool configure(yarp::dev::PolyDriver &driver, const yarp::os::Property &options)
Configure the estimation.
virtual bool getResults(yarp::sig::Vector &results)
Retrieve the estimation.
enum iCub::ctrl::OnlineStictionEstimator::@1 state
std::condition_variable cv_doneEvent
void applyStictionLimit()
virtual bool isDone()
Check the current estimation status.
yarp::dev::IControlMode * imod
yarp::dev::ICurrentControl * icur
yarp::dev::IControlLimits * ilim
virtual bool getInfo(yarp::os::Property &info)
Retrieve useful information about the estimation experiment.
yarp::dev::IPWMControl * ipwm
virtual bool startEstimation()
Start off the estimation procedure.
yarp::dev::IEncoders * ienc
virtual bool reconfigure(const yarp::os::Property &options)
Reconfigure the estimation after first initialization.
double getT() const
Get the trajectory reference time in seconds (90% of steady-state value in t=_T, transient extinguish...
bool setT(const double _T)
Set the trajectory reference time (90% of steady-state value in t=_T, transient extinguished for t>=1...
const yarp::sig::Vector & getPos() const
Get the current position.
bool setTs(const double _Ts)
Set the sample time.
virtual void init(const yarp::sig::Vector &y0)
Initialize the trajectory.
void computeNextValues(const yarp::sig::Vector &yd)
Compute the next position, velocity and acceleration.
General structure of parallel (non-interactive) PID.
virtual void reset(const yarp::sig::Vector &u0)
Resets the internal state of integral and derivative part.
virtual const yarp::sig::Vector & compute(const yarp::sig::Vector &ref, const yarp::sig::Vector &fb)
Computes the PID output.