iCub-main
tuning.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 __TUNING_H__
23 #define __TUNING_H__
24 
25 #include <mutex>
26 #include <condition_variable>
27 
28 #include <yarp/os/all.h>
29 #include <yarp/dev/all.h>
30 #include <yarp/sig/all.h>
31 #include <iCub/ctrl/math.h>
32 #include <iCub/ctrl/pids.h>
33 #include <iCub/ctrl/kalman.h>
34 #include <iCub/ctrl/filters.h>
35 #include <iCub/ctrl/minJerkCtrl.h>
37 
38 
39 namespace iCub
40 {
41 
42 namespace ctrl
43 {
44 
61 {
62 protected:
63  yarp::sig::Matrix A;
64  yarp::sig::Matrix F;
65  yarp::sig::Vector B;
66  yarp::sig::Matrix C;
67  yarp::sig::Matrix Ct;
68  yarp::sig::Matrix Q;
69  yarp::sig::Matrix P;
70  yarp::sig::Vector x;
71  yarp::sig::Vector _x;
72  double uOld;
73  double Ts;
74  double R;
75 
76 public:
81 
95  bool init(const double Ts, const double Q, const double R,
96  const double P0, const yarp::sig::Vector &x0);
97 
106  bool init(const double P0, const yarp::sig::Vector &x0);
107 
118  yarp::sig::Vector estimate(const double u, const double y);
119 
126  yarp::sig::Vector get_x() const { return _x; }
127 
133  yarp::sig::Matrix get_P() const { return P; }
134 
140  yarp::sig::Vector get_parameters() const { return _x.subVector(2,3); }
141 };
142 
143 
155 class OnlineStictionEstimator : public yarp::os::PeriodicThread
156 {
157 protected:
158  yarp::dev::IControlMode *imod;
159  yarp::dev::IControlLimits *ilim;
160  yarp::dev::IEncoders *ienc;
161  yarp::dev::IPidControl *ipid;
162  yarp::dev::IPWMControl *ipwm;
163  yarp::dev::ICurrentControl *icur;
164 
165  std::mutex mtx;
166  std::mutex mtx_doneEvent;
167  std::condition_variable cv_doneEvent;
168  yarp::sig::Vector gamma;
169  yarp::sig::Vector stiction;
170  yarp::os::Property info;
171  yarp::sig::Vector done;
172 
178 
179  int joint;
180  double dpos_dV;
181  double t0,T;
182  double x_min,x_max;
183  double x_pos,x_vel,x_acc;
184  double Kp,Ki,Kd;
185  double vel_thres,e_thres;
186  double tg,xd_pos;
188  bool adapt,adaptOld;
190 
191  enum
192  {
194  falling
195  } state;
196 
197  void applyStictionLimit();
198  bool threadInit();
199  void run();
200  void threadRelease();
201 
202  // prevent user from calling them directly
203  bool start();
204  void stop();
205 
206 public:
211 
256  virtual bool configure(yarp::dev::PolyDriver &driver, const yarp::os::Property &options);
257 
264  virtual bool reconfigure(const yarp::os::Property &options);
265 
271  virtual bool isConfigured() const { return configured; }
272 
278  virtual bool startEstimation() { return PeriodicThread::start(); }
279 
285  virtual bool isDone();
286 
293  virtual bool waitUntilDone();
294 
298  virtual void stopEstimation() { PeriodicThread::stop(); }
299 
308  virtual bool getResults(yarp::sig::Vector &results);
309 
322  virtual bool getInfo(yarp::os::Property &info);
323 
328 };
329 
330 
347 class OnlineCompensatorDesign : public yarp::os::PeriodicThread
348 {
349 protected:
353 
354  yarp::dev::IControlMode *imod;
355  yarp::dev::IControlLimits *ilim;
356  yarp::dev::IEncoders *ienc;
357  yarp::dev::IPositionControl *ipos;
358  yarp::dev::IPositionDirect *idir;
359  yarp::dev::IPidControl *ipid;
360  yarp::dev::IPWMControl *ipwm;
361  yarp::dev::ICurrentControl *icur;
362  yarp::dev::Pid *pidCur;
363  yarp::dev::Pid pidOld;
364  yarp::dev::Pid pidNew;
365 
366  std::mutex mtx;
367  std::mutex mtx_doneEvent;
368  std::condition_variable cv_doneEvent;
369  yarp::os::BufferedPort<yarp::sig::Vector> port;
370 
371  yarp::sig::Vector x0;
372  yarp::sig::Vector meanParams;
373  int meanCnt;
374  double P0;
375 
376  int joint;
377  double t0,t1;
378  double x_min,x_max,x_tg;
379  double max_time,max_pwm,dpos_dV;
391  bool pwm_pos;
393 
394  enum
395  {
399  controller_validation
400  } mode;
401 
402  void commandJoint(double &enc, double &u);
403  bool threadInit();
404  void run();
405  void threadRelease();
406 
407  // prevent user from calling them directly
408  bool start();
409  void stop();
410 
411 public:
416 
463  virtual bool configure(yarp::dev::PolyDriver &driver, const yarp::os::Property &options);
464 
470  virtual bool isConfigured() const { return configured; }
471 
515  virtual bool tuneController(const yarp::os::Property &options, yarp::os::Property &results);
516 
534  virtual bool startPlantEstimation(const yarp::os::Property &options);
535 
564  virtual bool startPlantValidation(const yarp::os::Property &options);
565 
583  virtual bool startStictionEstimation(const yarp::os::Property &options);
584 
629  virtual bool startControllerValidation(const yarp::os::Property &options);
630 
636  virtual bool isDone();
637 
643  virtual bool waitUntilDone();
644 
648  virtual void stopOperation() { PeriodicThread::stop(); }
649 
669  virtual bool getResults(yarp::os::Property &results);
670 
674  virtual ~OnlineCompensatorDesign();
675 };
676 
677 }
678 
679 }
680 
681 #endif
682 
683 
684 
yarp::dev::IControlLimits * ilim
Definition: tuning.h:355
virtual bool isConfigured() const
Check the configuration status.
Definition: tuning.h:271
virtual ~OnlineStictionEstimator()
Destructor.
Definition: tuning.h:327
yarp::dev::ICurrentControl * icur
Definition: tuning.h:163
General structure of parallel (non-interactive) PID.
Definition: pids.h:210
yarp::dev::IControlMode * imod
Definition: tuning.h:354
yarp::dev::IPWMControl * ipwm
Definition: tuning.h:162
yarp::dev::IControlLimits * ilim
Definition: tuning.h:159
static int stop
Definition: iCub_Sim.cpp:41
yarp::sig::Vector meanParams
Definition: tuning.h:372
OnlineDCMotorEstimator plant
Definition: tuning.h:350
yarp::dev::IControlMode * imod
Definition: tuning.h:158
OnlineDCMotorEstimator()
Default constructor.
Definition: tuning.cpp:30
virtual void stopOperation()
Stop any ongoing operation.
Definition: tuning.h:648
yarp::dev::ICurrentControl * icur
Definition: tuning.h:361
bool init(const double Ts, const double Q, const double R, const double P0, const yarp::sig::Vector &x0)
Initialize the estimation.
virtual void stopEstimation()
Stop the estimation procedure.
Definition: tuning.h:298
yarp::dev::IEncoders * ienc
Definition: tuning.h:356
yarp::sig::Vector stiction
Definition: tuning.h:169
virtual bool isConfigured() const
Check the configuration status.
Definition: tuning.h:470
Classic Kalman estimator.
Definition: kalman.h:41
A class for defining a saturated integrator based on Tustin formula: .
Definition: pids.h:47
yarp::sig::Matrix get_P() const
Return the estimated error covariance.
Definition: tuning.h:133
OnlineStictionEstimator stiction
Definition: tuning.h:351
yarp::dev::IPidControl * ipid
Definition: tuning.h:359
yarp::sig::Vector get_x() const
Return the estimated state.
Definition: tuning.h:126
yarp::dev::IPositionControl * ipos
Definition: tuning.h:357
std::condition_variable cv_doneEvent
Definition: tuning.h:167
Adaptive window linear fitting to estimate the first derivative.
yarp::dev::IPWMControl * ipwm
Definition: tuning.h:360
yarp::os::BufferedPort< yarp::sig::Vector > port
Definition: tuning.h:369
Online Compensator Design.
Definition: tuning.h:347
yarp::dev::IPidControl * ipid
Definition: tuning.h:161
yarp::sig::Vector estimate(const double u, const double y)
Estimate the state vector given the current input and the current measurement.
Definition: tuning.cpp:84
Generator of approximately minimum jerk trajectories.
Definition: minJerkCtrl.h:351
Adaptive window quadratic fitting to estimate the second derivative.
This file contains the definition of unique IDs for the body parts and the skin parts of the robot...
virtual bool startEstimation()
Start off the estimation procedure.
Definition: tuning.h:278
yarp::sig::Vector get_parameters() const
Return the system parameters.
Definition: tuning.h:140
Online DC Motor Parameters Estimator.
Definition: tuning.h:60
yarp::dev::IEncoders * ienc
Definition: tuning.h:160
GLenum mode
Definition: rendering.cpp:48
Online Stiction Estimator.
Definition: tuning.h:155
yarp::dev::IPositionDirect * idir
Definition: tuning.h:358
std::condition_variable cv_doneEvent
Definition: tuning.h:368