PreviewController.h
Go to the documentation of this file.
00001 /* -*- coding:utf-8-unix mode:c++ -*- */
00002 #ifndef PREVIEW_H_
00003 #define PREVIEW_H_
00004 #include <iostream>
00005 #include <queue>
00006 #include <deque>
00007 #include <hrpUtil/Eigen3d.h>
00008 #include "hrpsys/util/Hrpsys.h"
00009 
00010 namespace rats
00011 {
00012   static const double DEFAULT_GRAVITATIONAL_ACCELERATION = 9.80665; // [m/s^2]
00013 
00014   template <std::size_t dim>
00015   struct riccati_equation
00016   {
00017     Eigen::Matrix<double, dim, dim> A;
00018     Eigen::Matrix<double, dim, 1> b;
00019     Eigen::Matrix<double, 1, dim> c;
00020     Eigen::Matrix<double, dim, dim> P;
00021     Eigen::Matrix<double, 1, dim> K;
00022     Eigen::Matrix<double, dim, dim> A_minus_bKt; /* for buffer */
00023     double Q, R;
00024     double R_btPb_inv; /* for buffer */
00025 
00026     riccati_equation() : A(Eigen::Matrix<double, dim, dim>::Zero()), b(Eigen::Matrix<double, dim, 1>::Zero()), c(Eigen::Matrix<double, 1, dim>::Zero()),
00027                          P(Eigen::Matrix<double, dim, dim>::Zero()), K(Eigen::Matrix<double, 1, dim>::Zero()),
00028                          A_minus_bKt(Eigen::Matrix<double, dim, dim>::Zero()), Q(0), R(0), R_btPb_inv(0) {};
00029     riccati_equation(const Eigen::Matrix<double, dim, dim>& _A, const Eigen::Matrix<double, dim, 1>& _b,
00030                      const Eigen::Matrix<double, 1, dim>& _c, const double _Q, const double _R)
00031       : A(_A), b(_b), c(_c), P(Eigen::Matrix<double, dim, dim>::Zero()), K(Eigen::Matrix<double, 1, dim>::Zero()), A_minus_bKt(Eigen::Matrix<double, dim, dim>::Zero()), Q(_Q), R(_R), R_btPb_inv(0) {};
00032     virtual ~riccati_equation() {};
00033     bool solve() {
00034       Eigen::Matrix<double, dim, dim> prev_P;
00035       for (int i = 0; i < 10000; i++) {
00036         R_btPb_inv = (1.0 / (R + (b.transpose() * P * b)(0,0)));
00037         Eigen::Matrix<double, dim, dim> tmp_pa(P * A);
00038         K = R_btPb_inv * b.transpose() * tmp_pa;
00039         prev_P = A.transpose() * tmp_pa + c.transpose() * Q * c - A.transpose() * P * b * K;
00040         if ((abs((P - prev_P).array()) < 5.0e-10).all()) {
00041           A_minus_bKt = (A - b * K).transpose();
00042           return true;
00043         }
00044         P = prev_P;
00045       }
00046       return false;
00047     }
00048   };
00049 
00050   template <std::size_t dim>
00051   class preview_control_base
00052   {
00053   protected:
00054     riccati_equation<dim> riccati;
00055     Eigen::Matrix<double, 3, 3> tcA;
00056     Eigen::Matrix<double, 3, 1> tcb;
00057     Eigen::Matrix<double, 1, 3> tcc;
00058     Eigen::Matrix<double, 3, 2> x_k;
00059     Eigen::Matrix<double, 1, 2> u_k;
00060     hrp::dvector f;
00061     std::deque<Eigen::Matrix<double, 2, 1> > p;
00062     std::deque<double> pz;
00063     std::deque< std::vector<hrp::Vector3> > qdata;
00064     double zmp_z, cog_z;
00065     size_t delay, ending_count;
00066     virtual void calc_f() = 0;
00067     virtual void calc_u() = 0;
00068     virtual void calc_x_k() = 0;
00069     void init_riccati(const Eigen::Matrix<double, dim, dim>& A,
00070                       const Eigen::Matrix<double, dim, 1>& b,
00071                       const Eigen::Matrix<double, 1, dim>& c,
00072                       const double q = 1.0, const double r = 1.0e-6)
00073     {
00074       riccati = riccati_equation<dim>(A, b, c, q, r);
00075       riccati.solve();
00076       calc_f();
00077     };
00078     /* inhibit copy constructor and copy insertion not by implementing */
00079     preview_control_base (const preview_control_base& _p);
00080     preview_control_base &operator=(const preview_control_base &_p);
00081   public:
00082     /* dt = [s], zc = [mm], d = [s] */
00083     preview_control_base(const double dt, const double zc,
00084                          const hrp::Vector3& init_xk, const double _gravitational_acceleration, const double d = 1.6)
00085       : riccati(), x_k(Eigen::Matrix<double, 3, 2>::Zero()), u_k(Eigen::Matrix<double, 1, 2>::Zero()), p(), pz(), qdata(),
00086         zmp_z(0), cog_z(zc), delay(static_cast<size_t>(round(d / dt))), ending_count(1+delay)
00087     {
00088       tcA << 1, dt, 0.5 * dt * dt,
00089         0, 1,  dt,
00090         0, 0,  1;
00091       tcb << 1 / 6.0 * dt * dt * dt,
00092         0.5 * dt * dt,
00093         dt;
00094       tcc << 1.0, 0.0, -zc / _gravitational_acceleration;
00095       x_k(0,0) = init_xk(0);
00096       x_k(0,1) = init_xk(1);
00097     };
00098     virtual ~preview_control_base()
00099     {
00100       p.clear();
00101       pz.clear();
00102       qdata.clear();
00103     };
00104     virtual void update_x_k(const hrp::Vector3& pr, const std::vector<hrp::Vector3>& qdata);
00105     virtual void update_x_k()
00106     {
00107       hrp::Vector3 pr;
00108       pr(0) = p.back()(0);
00109       pr(1) = p.back()(1);
00110       pr(2) = pz.back();
00111       update_x_k(pr, qdata.back());
00112       ending_count--;
00113     };
00114     // void update_zc(double zc);
00115     size_t get_delay () { return delay; };
00116     double get_preview_f (const size_t idx) { return f(idx); };
00117     void get_refcog (double* ret)
00118     {
00119       ret[0] = x_k(0,0);
00120       ret[1] = x_k(0,1);
00121       ret[2] = cog_z;
00122     };
00123     void get_refcog_vel (double* ret)
00124     {
00125       ret[0] = x_k(1,0);
00126       ret[1] = x_k(1,1);
00127       ret[2] = 0;
00128     };
00129     void get_refcog_acc (double* ret)
00130     {
00131       ret[0] = x_k(2,0);
00132       ret[1] = x_k(2,1);
00133       ret[2] = 0;
00134     };
00135     void get_cart_zmp (double* ret)
00136     {
00137       Eigen::Matrix<double, 1, 2> _p(tcc * x_k);
00138       ret[0] = _p(0, 0);
00139       ret[1] = _p(0, 1);
00140       ret[2] = pz.front();
00141     };
00142     void get_current_refzmp (double* ret)
00143     {
00144       ret[0] = p.front()(0);
00145       ret[1] = p.front()(1);
00146       ret[2] = pz.front();
00147     };
00148     void get_current_qdata (std::vector<hrp::Vector3>& _qdata)
00149     {
00150         _qdata = qdata.front();
00151     };
00152     bool is_doing () { return p.size() >= 1 + delay; };
00153     bool is_end () { return ending_count <= 0 ; };
00154     void remove_preview_queue(const size_t remain_length)
00155     {
00156       size_t num = p.size() - remain_length;
00157       for (size_t i = 0; i < num; i++) {
00158         p.pop_back();
00159         pz.pop_back();
00160         qdata.pop_back();
00161       }
00162     };
00163     void remove_preview_queue() // Remove all queue
00164     {
00165         p.clear();
00166         pz.clear();
00167         qdata.clear();
00168     };
00169     void set_preview_queue(const hrp::Vector3& pr, const std::vector<hrp::Vector3>& q, const size_t idx)
00170     {
00171       Eigen::Matrix<double, 2, 1> tmpv;
00172       tmpv(0,0) = pr(0);
00173       tmpv(1,0) = pr(1);
00174       p[idx] = tmpv;
00175       pz[idx] = pr(2);
00176       qdata[idx] = q;
00177     };
00178     size_t get_preview_queue_size()
00179     {
00180       return p.size();
00181     };
00182     void print_all_queue ()
00183     {
00184       std::cerr << "(list ";
00185       for (size_t i = 0; i < p.size(); i++) {
00186         std::cerr << "#f(" << p[i](0) << " " << p[i](1) << ") ";
00187       }
00188       std::cerr << ")" << std::endl;
00189     }
00190   };
00191 
00192   class preview_control : public preview_control_base<3>
00193   {
00194   private:
00195     void calc_f();
00196     void calc_u();
00197     void calc_x_k();
00198   public:
00199     preview_control(const double dt, const double zc,
00200                     const hrp::Vector3& init_xk, const double _gravitational_acceleration = DEFAULT_GRAVITATIONAL_ACCELERATION, const double q = 1.0,
00201                     const double r = 1.0e-6, const double d = 1.6)
00202         : preview_control_base<3>(dt, zc, init_xk, _gravitational_acceleration, d)
00203     {
00204       init_riccati(tcA, tcb, tcc, q, r);
00205     };
00206     virtual ~preview_control() {};
00207   };
00208 
00209   class extended_preview_control : public preview_control_base<4>
00210   {
00211   private:
00212     Eigen::Matrix<double, 4, 2> x_k_e;
00213     void calc_f();
00214     void calc_u();
00215     void calc_x_k();
00216   public:
00217     extended_preview_control(const double dt, const double zc,
00218                              const hrp::Vector3& init_xk, const double _gravitational_acceleration = DEFAULT_GRAVITATIONAL_ACCELERATION, const double q = 1.0,
00219                              const double r = 1.0e-6, const double d = 1.6)
00220       : preview_control_base<4>(dt, zc, init_xk, _gravitational_acceleration, d), x_k_e(Eigen::Matrix<double, 4, 2>::Zero())
00221     {
00222       Eigen::Matrix<double, 4, 4> A;
00223       Eigen::Matrix<double, 4, 1> b;
00224       Eigen::Matrix<double, 1, 4> c;
00225       Eigen::Matrix<double, 1, 3> tmpca(tcc * tcA);
00226       Eigen::Matrix<double, 1, 1> tmpcb(tcc * tcb);
00227       A << 1.0, tmpca(0,0), tmpca(0,1), tmpca(0,2),
00228         0.0, tcA(0,0), tcA(0,1), tcA(0,2),
00229         0.0, tcA(1,0), tcA(1,1), tcA(1,2),
00230         0.0, tcA(2,0), tcA(2,1), tcA(2,2);
00231       b << tmpcb(0,0),
00232         tcb(0,0),
00233         tcb(1,0),
00234         tcb(2,0);
00235       c << 1,0,0,0;
00236       x_k_e(0,0) = init_xk(0);
00237       x_k_e(0,1) = init_xk(1);
00238       init_riccati(A, b, c, q, r);
00239     };
00240     virtual ~extended_preview_control() {};
00241   };
00242 
00243   template <class previw_T>
00244   class preview_dynamics_filter
00245   {
00246     previw_T preview_controller;
00247     bool finishedp;
00248   public:
00249     preview_dynamics_filter() {};
00250     preview_dynamics_filter(const double dt, const double zc, const hrp::Vector3& init_xk, const double _gravitational_acceleration = DEFAULT_GRAVITATIONAL_ACCELERATION, const double q = 1.0, const double r = 1.0e-6, const double d = 1.6)
00251         : preview_controller(dt, zc, init_xk, _gravitational_acceleration, q, r, d), finishedp(false) {};
00252     ~preview_dynamics_filter() {};
00253     bool update(hrp::Vector3& p_ret, hrp::Vector3& x_ret, std::vector<hrp::Vector3>& qdata_ret, const hrp::Vector3& pr, const std::vector<hrp::Vector3>& qdata, const bool updatep)
00254     {
00255       bool flg;
00256       if (updatep) {
00257         preview_controller.update_x_k(pr, qdata);
00258         flg = preview_controller.is_doing();
00259       } else {
00260         if ( !preview_controller.is_end() )
00261           preview_controller.update_x_k();
00262         flg = !preview_controller.is_end();
00263       }
00264 
00265       if (flg) {
00266         preview_controller.get_current_refzmp(p_ret.data());
00267         preview_controller.get_refcog(x_ret.data());
00268         preview_controller.get_current_qdata(qdata_ret);
00269       }
00270       return flg;
00271     };
00272     void remove_preview_queue(const size_t remain_length)
00273     {
00274       preview_controller.remove_preview_queue(remain_length);
00275     };
00276     void remove_preview_queue()
00277     {
00278       preview_controller.remove_preview_queue();
00279     };
00280     void set_preview_queue(const hrp::Vector3& pr, const std::vector<hrp::Vector3>& qdata, const size_t idx)
00281     {
00282       preview_controller.set_preview_queue(pr, qdata, idx);
00283     }
00284     size_t get_preview_queue_size()
00285     {
00286       return preview_controller.get_preview_queue_size();
00287     };
00288     void print_all_queue ()
00289     {
00290       preview_controller.print_all_queue();
00291     }
00292 
00293     void get_cart_zmp (double* ret) { preview_controller.get_cart_zmp(ret);}
00294     void get_refcog_vel (double* ret) { preview_controller.get_refcog_vel(ret);}
00295     void get_refcog_acc (double* ret) { preview_controller.get_refcog_acc(ret);}
00296     void get_current_refzmp (double* ret) { preview_controller.get_current_refzmp(ret);}
00297     //void get_current_qdata (double* ret) { preview_controller.get_current_qdata(ret);}
00298     size_t get_delay () { return preview_controller.get_delay(); };
00299     double get_preview_f (const size_t idx) { return preview_controller.get_preview_f(idx); };
00300   };
00301 }
00302 #endif /*PREVIEW_H_*/


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:55