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;
186  double tg,xd_pos;
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;
391  bool pwm_pos;
393 
394  enum
395  {
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 
Adaptive window linear fitting to estimate the first derivative.
Adaptive window quadratic fitting to estimate the second derivative.
A class for defining a saturated integrator based on Tustin formula: .
Definition: pids.h:48
Classic Kalman estimator.
Definition: kalman.h:42
Online Compensator Design.
Definition: tuning.h:348
virtual bool isDone()
Check the status of the current ongoing operation.
Definition: tuning.cpp:1025
OnlineCompensatorDesign()
Default constructor.
Definition: tuning.cpp:442
virtual bool tuneController(const yarp::os::Property &options, yarp::os::Property &results)
Tune the controller once given the plant characteristics.
Definition: tuning.cpp:838
void commandJoint(double &enc, double &u)
Definition: tuning.cpp:617
yarp::dev::IPidControl * ipid
Definition: tuning.h:359
yarp::dev::IPWMControl * ipwm
Definition: tuning.h:360
virtual bool startStictionEstimation(const yarp::os::Property &options)
Start off the stiction estimation procedure.
Definition: tuning.cpp:953
yarp::dev::IEncoders * ienc
Definition: tuning.h:356
OnlineStictionEstimator stiction
Definition: tuning.h:351
std::condition_variable cv_doneEvent
Definition: tuning.h:368
yarp::dev::IPositionControl * ipos
Definition: tuning.h:357
virtual void stopOperation()
Stop any ongoing operation.
Definition: tuning.h:648
yarp::dev::IControlMode * imod
Definition: tuning.h:354
yarp::dev::ICurrentControl * icur
Definition: tuning.h:361
virtual bool startPlantEstimation(const yarp::os::Property &options)
Start off the plant estimation procedure.
Definition: tuning.cpp:891
enum iCub::ctrl::OnlineCompensatorDesign::@2 mode
virtual bool startPlantValidation(const yarp::os::Property &options)
Start off the plant validation procedure.
Definition: tuning.cpp:905
virtual bool getResults(yarp::os::Property &results)
Retrieve the results of the current ongoing operation.
Definition: tuning.cpp:1048
virtual bool waitUntilDone()
Wait until the current ongoing operation is accomplished.
Definition: tuning.cpp:1035
yarp::dev::IPositionDirect * idir
Definition: tuning.h:358
yarp::dev::IControlLimits * ilim
Definition: tuning.h:355
virtual bool isConfigured() const
Check the configuration status.
Definition: tuning.h:470
OnlineDCMotorEstimator plant
Definition: tuning.h:350
virtual bool configure(yarp::dev::PolyDriver &driver, const yarp::os::Property &options)
Configure the design.
Definition: tuning.cpp:459
yarp::os::BufferedPort< yarp::sig::Vector > port
Definition: tuning.h:369
virtual ~OnlineCompensatorDesign()
Destructor.
Definition: tuning.cpp:1115
virtual bool startControllerValidation(const yarp::os::Property &options)
Start off the controller validation procedure.
Definition: tuning.cpp:971
yarp::sig::Vector meanParams
Definition: tuning.h:372
Online DC Motor Parameters Estimator.
Definition: tuning.h:61
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
bool init(const double P0, const yarp::sig::Vector &x0)
Initialize the internal state.
yarp::sig::Vector get_parameters() const
Return the system parameters.
Definition: tuning.h:140
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.
Definition: tuning.cpp:30
yarp::sig::Matrix get_P() const
Return the estimated error covariance.
Definition: tuning.h:133
yarp::sig::Vector get_x() const
Return the estimated state.
Definition: tuning.h:126
Online Stiction Estimator.
Definition: tuning.h:156
OnlineStictionEstimator()
Default constructor.
Definition: tuning.cpp:135
virtual ~OnlineStictionEstimator()
Destructor.
Definition: tuning.h:327
yarp::dev::IPidControl * ipid
Definition: tuning.h:161
yarp::sig::Vector stiction
Definition: tuning.h:169
virtual void stopEstimation()
Stop the estimation procedure.
Definition: tuning.h:298
virtual bool waitUntilDone()
Wait until the condition |e_mean|<e_thres is met.
Definition: tuning.cpp:405
virtual bool configure(yarp::dev::PolyDriver &driver, const yarp::os::Property &options)
Configure the estimation.
Definition: tuning.cpp:150
virtual bool getResults(yarp::sig::Vector &results)
Retrieve the estimation.
Definition: tuning.cpp:418
enum iCub::ctrl::OnlineStictionEstimator::@1 state
std::condition_variable cv_doneEvent
Definition: tuning.h:167
virtual bool isDone()
Check the current estimation status.
Definition: tuning.cpp:393
yarp::dev::IControlMode * imod
Definition: tuning.h:158
yarp::dev::ICurrentControl * icur
Definition: tuning.h:163
yarp::dev::IControlLimits * ilim
Definition: tuning.h:159
virtual bool getInfo(yarp::os::Property &info)
Retrieve useful information about the estimation experiment.
Definition: tuning.cpp:430
yarp::dev::IPWMControl * ipwm
Definition: tuning.h:162
virtual bool startEstimation()
Start off the estimation procedure.
Definition: tuning.h:278
yarp::dev::IEncoders * ienc
Definition: tuning.h:160
virtual bool reconfigure(const yarp::os::Property &options)
Reconfigure the estimation after first initialization.
Definition: tuning.cpp:199
virtual bool isConfigured() const
Check the configuration status.
Definition: tuning.h:271
Generator of approximately minimum jerk trajectories.
Definition: minJerkCtrl.h:352
General structure of parallel (non-interactive) PID.
Definition: pids.h:211
static int stop
Definition: iCub_Sim.cpp:41
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.