PreviewController.cpp
Go to the documentation of this file.
00001 /* -*- coding:utf-8-unix; mode:c++; -*- */
00002 #include "PreviewController.h"
00003 
00004 using namespace hrp;
00005 using namespace rats;
00006 
00007 template <std::size_t dim>
00008 void preview_control_base<dim>::update_x_k(const hrp::Vector3& pr, const std::vector<hrp::Vector3>& _qdata)
00009 {
00010   zmp_z = pr(2);
00011   Eigen::Matrix<double, 2, 1> tmpv;
00012   tmpv(0,0) = pr(0);
00013   tmpv(1,0) = pr(1);
00014   p.push_back(tmpv);
00015   pz.push_back(pr(2));
00016   qdata.push_back(_qdata);
00017   if ( p.size() > 1 + delay ) {
00018     p.pop_front();
00019     pz.pop_front();
00020     qdata.pop_front();
00021   }
00022   if ( is_doing() ) calc_x_k();
00023 }
00024 
00025 // template <std::size_t dim>
00026 // void preview_control_base<dim>::update_zc(double zc)
00027 // {
00028 //   riccati.c(0, 2) = - zc / gravitational_acceleration; 
00029 //   riccati.solve();
00030 // }
00031 
00032 void preview_control::calc_f()
00033 {
00034   f.resize(delay+1);
00035   f(0)=0;
00036   Eigen::Matrix<double, 1, 1> fa;
00037   hrp::Matrix33 gsi(hrp::Matrix33::Identity());
00038   for (size_t i = 0; i < delay; i++) {
00039     fa = riccati.R_btPb_inv * riccati.b.transpose() * (gsi * riccati.Q * riccati.c.transpose());
00040     gsi = riccati.A_minus_bKt * gsi;
00041     f(i+1) = fa(0,0);
00042   }
00043 }
00044 
00045 void preview_control::calc_u()
00046 {
00047   Eigen::Matrix<double, 1, 2> gfp(Eigen::Matrix<double, 1, 2>::Zero());
00048   for (size_t i = 0; i < 1 + delay; i++)
00049     gfp += f(i) * p[i];
00050   u_k = -riccati.K * x_k + gfp;
00051 };
00052 
00053 void preview_control::calc_x_k()
00054 {
00055   calc_u();
00056   x_k = riccati.A * x_k + riccati.b * u_k;  
00057 }
00058 
00059 void extended_preview_control::calc_f()
00060 {
00061   f.resize(delay + 1);
00062   f(0)=0;
00063   Eigen::Matrix<double, 1, 1> fa;
00064   Eigen::Matrix<double, 4, 4> gsi(Eigen::Matrix<double, 4, 4>::Identity());
00065   Eigen::Matrix<double, 4, 1> qt(riccati.Q * riccati.c.transpose());
00066   for (size_t i = 0; i < delay; i++) {
00067     if ( i == delay - 1 ) qt = riccati.P * qt;
00068     fa = riccati.R_btPb_inv * riccati.b.transpose() * (gsi * qt);
00069     gsi = riccati.A_minus_bKt * gsi;
00070     f(i+1) = fa(0,0);
00071   }
00072 }
00073 
00074 void extended_preview_control::calc_u()
00075 {
00076   Eigen::Matrix<double, 1, 2> gfp(Eigen::Matrix<double, 1, 2>::Zero());
00077   for (size_t i = 0; i < 1 + delay; i++)
00078     gfp += f(i) * p[i];
00079   u_k = -riccati.K * x_k_e + gfp;
00080 };
00081 
00082 void extended_preview_control::calc_x_k()
00083 {
00084   calc_u();
00085   x_k_e = riccati.A * x_k_e + riccati.b * u_k;
00086   for (size_t i = 0; i < 3; i++)
00087     for (size_t j = 0; j < 2; j++)
00088       x_k(i,j) += x_k_e(i+1,j);
00089 }
00090 


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