GazeStabilization.hh
Go to the documentation of this file.
1 #pragma once
2 
4 #include "GazeStabInputOutput.hh"
5 #include "GazeStabOptions.hh"
6 
7 #include <Eigen/Dense>
8 
9 
11 {
12 public:
15 
16  GazeStabInput* input; // controller input
17  GazeStabOutput* output; // controller output
18  GazeStabOptions* options; // options of the gaze stabilization control
19 
20  void init();
21  void control_loop();
22  void finish();
23 
24  double get_qdvirt(int i)
25  {
26  return qd_gaze_des(i + n_UB_joints);
27  }
28  double get_optFl()
29  {
30  return fabs((19.13 / q_virt[0]) * get_qdvirt(0)) + fabs(1.2054 * get_qdvirt(1)) + fabs(2.1431 * get_qdvirt(2));
31  }
32 
33  double get_pos_err(int i)
34  {
35  return pos_err(i);
36  }
37  double get_or_err()
38  {
39  return R_xFP[2][0] / R_xFP[0][0];
40  }
41 
43  {
44  return n_LB_joints + n_UB_joints;
45  }
47  {
48  return n_UB_joints + n_virt_joints;
49  }
50  int get_n_all_joints(void)
51  {
52  return n_LB_joints + n_UB_joints + n_virt_joints;
53  }
54 
55  std::vector<float> getOptFlowPred()
56  {
57  return optFlow_pred;
58  }
59 
60  std::vector<float> getIMUPred()
61  {
62  return gyroscopeRotation_pred;
63  }
64 
66  {
67  return mean_optFl_pred;
68  }
69 
70 private:
71 
72  /////////////////////////////////
73  // predictor part (TODO move in a specific class)
74  void predict_IMU();
75  void predict_optFlow_xy();
76  void predict_optFlow_index();
77 
78  //std::vector<float> orientationQuaternion;
79  std::vector<float> gyroscopeRotation_pred; // head IMU velocity in head attached frame (x,y,z) [rad/s]
80  std::vector<float> optFlow_pred; // optical flow prediction [x, y] in [deg/s]
81  double mean_optFl_pred; // mean distance of the optical flow [deg/s]
82 
83  /////////////////////////////////
84 
85  Eigen::VectorXd qd_gaze_des; // Upper Body and virtual joints velocity references
86  Eigen::VectorXd q_UB_des; // Upper Body and virtual joints position references (control via nullspace projection)
87 
88  double q_virt[3]; // virtual joints configuration [rad and m]
89 
90  double last_t_ctrl;
91 
92  Eigen::VectorXd vel_xFP;
93  Eigen::Vector3d pos_xFP;
94  Eigen::Vector3d pos_err;
95  double R_xFP[3][3];
96  Eigen::VectorXd desired_vel_xFP;
97 
98  std::vector<double> mbs_data_virt_q;
99  std::vector<double> mbs_data_virt_qd;
100  std::vector<double> mbs_data_virt_qdd;
101  MbsSensor sens_virt, sens_eye, sens_head_imu;
102 
103  Eigen::MatrixXd Jac_FK;
104  Eigen::MatrixXd Jac_IK;
105  Eigen::MatrixXd inv_Jac;
106  Eigen::MatrixXd null_space;
107 
108  int n_LB_joints; // number of lower body joints used in the feedforward model
109  int n_UB_joints; // number of upper body joints used in the feedforward model
110  int n_virt_joints;// number of virt joints used in the feedforward model
111 
112 };
113 
mbs_sensor2.h
GazeStabilization::get_or_err
double get_or_err()
Definition: GazeStabilization.hh:37
GazeStabOutput
Definition: GazeStabInputOutput.hh:26
GazeStabilization::get_n_gaze_joints
int get_n_gaze_joints(void)
Definition: GazeStabilization.hh:46
GazeStabilization::finish
void finish()
Definition: GazeStabilization.cc:275
GazeStabilization::output
GazeStabOutput * output
Definition: GazeStabilization.hh:17
GazeStabilization
Definition: GazeStabilization.hh:10
GazeStabilization::GazeStabilization
GazeStabilization(GazeStabOptions *options)
Definition: GazeStabilization.cc:15
GazeStabOptions
Definition: GazeStabOptions.hh:3
GazeStabInputOutput.hh
GazeStabilization::input
GazeStabInput * input
Definition: GazeStabilization.hh:16
GazeStabilization::get_qdvirt
double get_qdvirt(int i)
Definition: GazeStabilization.hh:24
GazeStabilization::getIMUPred
std::vector< float > getIMUPred()
Definition: GazeStabilization.hh:60
GazeStabilization::get_pos_err
double get_pos_err(int i)
Definition: GazeStabilization.hh:33
GazeStabInput
Definition: GazeStabInputOutput.hh:6
GazeStabilization::options
GazeStabOptions * options
Definition: GazeStabilization.hh:18
GazeStabilization::get_n_all_joints
int get_n_all_joints(void)
Definition: GazeStabilization.hh:50
GazeStabilization::~GazeStabilization
~GazeStabilization()
Definition: GazeStabilization.cc:52
GazeStabilization::init
void init()
Definition: GazeStabilization.cc:60
GazeStabilization::get_n_real_joints
int get_n_real_joints(void)
Definition: GazeStabilization.hh:42
GazeStabilization::get_optFl
double get_optFl()
Definition: GazeStabilization.hh:28
GazeStabilization::getMeanOptFlPred
double getMeanOptFlPred()
Definition: GazeStabilization.hh:65
GazeStabOptions.hh
MbsSensor
Definition: mbs_sensor2.h:17
GazeStabilization::getOptFlowPred
std::vector< float > getOptFlowPred()
Definition: GazeStabilization.hh:55
GazeStabilization::control_loop
void control_loop()
Definition: GazeStabilization.cc:110