GazeStabilization.hh

Go to the documentation of this file.

#pragma once
 
#include "symbolic_routines/mbs_sensor2.h"
#include "GazeStabInputOutput.hh"
#include "GazeStabOptions.hh"
 
#include <Eigen/Dense>
 
 
class GazeStabilization
{
public:
    GazeStabilization(GazeStabOptions* options);
    ~GazeStabilization();
 
    GazeStabInput*   input;    // controller input
    GazeStabOutput*  output;   // controller output
    GazeStabOptions* options;  // options of the gaze stabilization control
 
    void init();
    void control_loop();
    void finish();
 
    double get_qdvirt(int i)
    {
        return qd_gaze_des(i + n_UB_joints);
    }
    double get_optFl()
    {
        return fabs((19.13 / q_virt[0]) * get_qdvirt(0)) + fabs(1.2054 * get_qdvirt(1)) + fabs(2.1431 * get_qdvirt(2));
    }
 
    double get_pos_err(int i)
    {
        return pos_err(i);
    }
    double get_or_err()
    {
        return R_xFP[2][0] / R_xFP[0][0];
    }
 
    int get_n_real_joints(void)
    {
        return n_LB_joints + n_UB_joints;
    }
    int get_n_gaze_joints(void)
    {
        return n_UB_joints + n_virt_joints;
    }
    int get_n_all_joints(void)
    {
        return n_LB_joints + n_UB_joints + n_virt_joints;
    }
 
    std::vector<float> getOptFlowPred()
    {
        return optFlow_pred;
    }
 
    std::vector<float> getIMUPred()
    {
        return gyroscopeRotation_pred;
    }
 
    double getMeanOptFlPred()
    {
        return mean_optFl_pred;
    }
 
private:
 
    /////////////////////////////////
    // predictor part (TODO move in a specific class)
    void predict_IMU();
    void predict_optFlow_xy();
    void predict_optFlow_index();
 
    //std::vector<float> orientationQuaternion;
    std::vector<float> gyroscopeRotation_pred; // head IMU velocity in head attached frame (x,y,z) [rad/s]
    std::vector<float> optFlow_pred; // optical flow prediction [x, y] in [deg/s]
    double mean_optFl_pred; // mean distance of the optical flow [deg/s]
 
    /////////////////////////////////
 
    Eigen::VectorXd qd_gaze_des; // Upper Body and virtual joints velocity references
    Eigen::VectorXd q_UB_des; // Upper Body and virtual joints position references (control via nullspace projection)
 
    double q_virt[3]; // virtual joints configuration [rad and m]
 
    double last_t_ctrl;
 
    Eigen::VectorXd vel_xFP;
    Eigen::Vector3d pos_xFP;
    Eigen::Vector3d pos_err;
    double R_xFP[3][3];
    Eigen::VectorXd desired_vel_xFP;
 
    std::vector<double> mbs_data_virt_q;
    std::vector<double> mbs_data_virt_qd;
    std::vector<double> mbs_data_virt_qdd;
    MbsSensor sens_virt, sens_eye, sens_head_imu;
 
    Eigen::MatrixXd Jac_FK;
    Eigen::MatrixXd Jac_IK;
    Eigen::MatrixXd inv_Jac;
    Eigen::MatrixXd null_space;
 
    int n_LB_joints;  // number of lower body joints used in the feedforward model
    int n_UB_joints;  // number of upper body joints used in the feedforward model
    int n_virt_joints;// number of virt joints used in the feedforward model
 
};