dg_tools
reactive_lqr_controller.hpp
Go to the documentation of this file.
1 
12 #ifndef __SOT_reactive_lqr_controller_HH__
13 #define __SOT_reactive_lqr_controller_HH__
14 
15 /* Math */
16 
17 #include <math.h>
18 
19 /* Matrix */
20 #include <dynamic-graph/linear-algebra.h>
21 namespace dg = dynamicgraph;
22 
23 /* SOT */
24 #include <dynamic-graph/signal-time-dependent.h>
25 #include <dynamic-graph/signal-ptr.h>
26 #include <dynamic-graph/entity.h>
27 
28 namespace dynamicgraph{
29  namespace sot {
30  /* -----------------------------------------------------------------------*/
31  /* ---------- CLASS ------------------------------------------------------*/
32  /* -----------------------------------------------------------------------*/
33 
34  class ReactiveLQRController
35  : public Entity
36  {
37  public: /*-------------- Constructor ---------------------------------*/
38 
39  ReactiveLQRController( const std::string &name);
40 
41  void init (const double& step){};
42 
43  public: /*----------------- Constant ----------------------------------*/
44 
45  static const double TIME_STEP_DEFAULT;
46 
47  public: /* ----- ENTITY INHERITANCE -----*/
48 
49  static const std::string CLASS_NAME;
50  virtual const std::string& getClassName( void ) const {return CLASS_NAME;}
51 
52  public: /* ---- SIGNALS ----------------------------------------------*/
53 
54  SignalPtr<dg::Vector, int> com_posSIN; // is a 3d vector
55  SignalPtr<dg::Vector, int> com_velSIN; // is a 3d vector
56  SignalPtr<dg::Vector, int> com_oriSIN; //base orientation 4d vector quaternion
57  SignalPtr<dg::Vector, int> com_ang_velSIN;
58  SignalPtr<dg::Vector, int> end_eff_pos_12dSIN;
59  SignalPtr<dg::Vector, int> des_fffSIN;
60  SignalPtr<dg::Vector, int> cnt_valueSIN;
61  SignalPtr<dg::Vector, int> cnt_sensor_valueSIN;
62  SignalPtr<dg::Vector, int> state_vectorSIN;
63  SignalPtr<dg::Vector, int> massSIN;
64  SignalPtr<dg::Matrix, int> inertiaSIN;
65 
66  SignalPtr<dg::Vector, int> horizonSIN;
67  SignalPtr<dg::Matrix, int> qSIN;
68  SignalPtr<dg::Matrix, int> rSIN;
69 
70 
71  SignalTimeDependent<dg::Matrix, int> lqr_gainsSOUT;
72 
73 
74  protected:
75 
76  double TimeStep;
77  double& setsize(int dimension);
78 
79  Eigen::VectorXd compute_dyn_(Eigen::Vector3d com_pos, Eigen::Vector3d com_vel, Eigen::Quaternion<double> ori, Eigen::Vector3d com_ang_vel,
80  Eigen::VectorXd end_eff_pos_12d, Eigen::VectorXd des_fff, Eigen::Vector4d cnt_value, double mass,
81  Eigen::MatrixXd inertia, Eigen::MatrixXd& A_t, Eigen::MatrixXd& B_t);
82  void compute_lin_dyn_(Eigen::MatrixXd lin_A_t, Eigen::MatrixXd lin_B_t);
83  void compute_num_lin_dyn_(
84  Eigen::Vector3d com_pos_t, Eigen::VectorXd com_vel, Eigen::Vector3d com_ang_vel_t,
85  Eigen::Quaternion<double> ori_t, Eigen::VectorXd end_eff_pos_12d_t,
86  Eigen::Vector4d cnt_value_t,
87  Eigen::Vector3d com_pos_t1, Eigen::Vector3d com_vel_t1, Eigen::Vector3d com_ang_vel_t1,
88  Eigen::Quaternion<double> ori_t1,
89  Eigen::VectorXd end_eff_pos_12d_t1, Eigen::Vector4d cnt_value_t1,
90  Eigen::VectorXd des_fff, double mass, Eigen::MatrixXd inertia,
91  Eigen::MatrixXd& lin_A_t, Eigen::MatrixXd& lin_B_t);
92 
93 
94  Eigen::MatrixXd compute_r_cross_mat_(Eigen::VectorXd end_eff_pos, Eigen::Vector3d com_pos );
95 
96  void compute_lqr_gains_(Eigen::MatrixXd Q, Eigen::MatrixXd R, Eigen::MatrixXd lin_A,
97  Eigen::MatrixXd lin_B, Eigen::MatrixXd P_prev, Eigen::MatrixXd &P, Eigen::MatrixXd &K);
98 
99  Eigen::MatrixXd omega;
100  // Eigen::MatrixXd com_acc;
101  Eigen::MatrixXd inertia_global_inv; // inertia of solo body in global co ordinate (RIR^T)
102  Eigen::MatrixXd mass_inv_identity; // 1/m * I
103  Eigen::MatrixXd R_cross_mat;
104  Eigen::MatrixXd R_cross_mat_fl;
105  Eigen::MatrixXd R_cross_mat_fr;
106  Eigen::MatrixXd R_cross_mat_hl;
107  Eigen::MatrixXd R_cross_mat_hr;
108 
109  // to compute the liear dynamics;
110  Eigen::MatrixXd A_t;
111  Eigen::MatrixXd B_t;
112  Eigen::MatrixXd A_t1; // t+1 step
113  Eigen::MatrixXd B_t1; // t+1 step
114  Eigen::VectorXd x;
115  Eigen::VectorXd unit_vec;
116  Eigen::Vector3d com_pos_pd; // Partial derivative
117  Eigen::Vector3d com_vel_pd;
118  Eigen::Vector3d com_ang_vel_pd;
119  Eigen::Quaternion<double> com_ori_pd;
120 
121 
122  // dynamic grpah signal
123  dynamicgraph::Matrix& return_lqr_gains_(dynamicgraph::Matrix & lqr_gains, int t);
124  Eigen::MatrixXd P_prev;
125  Eigen::MatrixXd K_prev;
126 
127  Eigen::MatrixXd P;
128  Eigen::MatrixXd K;
129 
130  // Backward pass of LQR
131  //current timestep
132 
133  Eigen::VectorXd com_pos_t;
134  Eigen::VectorXd com_vel_t;
135  Eigen::VectorXd com_ang_vel_t;
136  Eigen::VectorXd ori_tmp;
137  Eigen::Quaternion<double> com_ori_t;
138  Eigen::VectorXd des_fff_t;
139  Eigen::VectorXd cnt_value_t;
140  Eigen::VectorXd end_eff_pos_12d_t;
141  Eigen::VectorXd xd_t;
142 
143 
144  // next timestep
145  Eigen::VectorXd com_pos_t1;
146  Eigen::VectorXd com_vel_t1;
147  Eigen::VectorXd com_ang_vel_t1;
148  Eigen::Quaternion<double> com_ori_t1;
149  Eigen::VectorXd des_fff_t1;
150  Eigen::VectorXd cnt_value_t1;
151  Eigen::VectorXd end_eff_pos_12d_t1;
152  Eigen::VectorXd xd_t1;
153 
154  Eigen::MatrixXd lin_A_ht;
155  Eigen::MatrixXd lin_B_ht;
156 
157 
158 
159 
160  };
161  }
162 }
163 
164 #endif
dynamicgraph::sot::ReactiveLQRController
Definition: reactive_lqr_controller.hpp:34
dynamicgraph
Definition: ComImpedanceController.hpp:31