GazeStabilization.cc

Go to the documentation of this file.

 
#include "GazeStabilization.hh"
 
#include <cstdio>
#include <iostream>
 
#include "symbolic_routines/symbolic_routines.h"
//#include "mbs_sensor.h"
#include "pinv.hh"
 
#include <Eigen/Dense>
 
#include <ArmarXCore/core/logging/Logging.h>
 
GazeStabilization::GazeStabilization(GazeStabOptions* options)
{
 
    this->options = options;
 
    if (options->model == 0) // armar 4 WO torso
    {
        n_LB_joints = 6;
        n_UB_joints = 7;
        n_virt_joints = 3;
    }
    else if (options->model == 1) // armar 4 W torso
    {
        n_LB_joints = 8;
        n_UB_joints = 7;
        n_virt_joints = 3;
    }
    else if (options->model == 2) // armar 3
    {
        n_LB_joints = 10;
        n_UB_joints = 5;
        n_virt_joints = 3;
    }
 
    input   = new GazeStabInput(n_LB_joints, n_UB_joints);
    output  = new GazeStabOutput(n_UB_joints);
 
 
    mbs_data_virt_q.reserve(get_n_all_joints() + 1);
    mbs_data_virt_qd.reserve(get_n_all_joints() + 1);
    mbs_data_virt_qdd.reserve(get_n_all_joints() + 1);
 
    optFlow_pred.resize(2);
    gyroscopeRotation_pred.resize(3);
}
 
 
GazeStabilization::~GazeStabilization()
{
    delete input;
    delete output;
    delete options;
}
 
 
void GazeStabilization::init()
{
 
    printf("hello from GazeSatb init\n");
 
    // create corresponding sensors
    allocate_sensor(&sens_virt, get_n_all_joints());
    init_sensor(&sens_virt, get_n_all_joints());
 
    allocate_sensor(&sens_eye, get_n_real_joints());
    init_sensor(&sens_eye, get_n_real_joints());
 
    allocate_sensor(&sens_head_imu, get_n_real_joints() - 2);
    init_sensor(&sens_head_imu, get_n_real_joints() - 2);
 
    qd_gaze_des = Eigen::VectorXd::Zero(get_n_gaze_joints());
    q_UB_des  = Eigen::VectorXd::Zero(n_UB_joints);
 
    if ((options->model == 0) || (options->model == 1)) // armar4
    {
        q_UB_des(2) = M_PI;  // = initial configuration
 
        q_virt[0] = -1.9415;
        q_virt[1] = 0.0;
        q_virt[2] = 0.0;
    }
    else // armar 3
    {
        q_virt[0] = 1.18;
        q_virt[1] = M_PI;
        q_virt[2] = -M_PI / 2.;
    }
 
    //input->pos_target[0] = input->qd_LB[1] + 1;  // 1m in front of the robot
 
    // TODO use newton raphson to find the initial q_virt
 
    vel_xFP = Eigen::VectorXd::Zero(6);
    pos_xFP = Eigen::Vector3d::Zero();
    desired_vel_xFP = Eigen::VectorXd::Zero(6);
 
    Jac_IK.resize(6, get_n_gaze_joints());
    Jac_FK.resize(6, get_n_all_joints());
    inv_Jac.resize(get_n_gaze_joints(), 6);
    null_space.resize(get_n_gaze_joints(), get_n_gaze_joints());
 
    last_t_ctrl = 0.;
 
}
 
void GazeStabilization::control_loop()
{
 
    /************
     * Get input
     * *********/
    double tsim = input->tsim;
 
    /************
     * Forward kinematics (FK)
     * *********/
 
    // copy lower body joints pos
    for (int i = 1; i <= n_LB_joints; i++)
    {
        mbs_data_virt_q[i]  = input->q_LB[i - 1];
        mbs_data_virt_qd[i] = input->qd_LB[i - 1];
    }
 
    // copy head joints pos
    for (int i = n_LB_joints + 1; i <= get_n_real_joints(); i++)
    {
        mbs_data_virt_q[i]  = input->q_UB[i - (n_LB_joints + 1)];
        mbs_data_virt_qd[i] = input->qd_UB[i - (n_LB_joints + 1)];
    }
 
    // copy virt joints pos
    for (int i = get_n_real_joints() + 1; i <= get_n_all_joints(); i++)
    {
        mbs_data_virt_q[i]  = q_virt[i - (get_n_real_joints() + 1)];
        mbs_data_virt_qd[i] = qd_gaze_des(i - (n_LB_joints + 1));
    }
 
    if (options->model == 0) // Armar4 WO torso
    {
        mbs_sensor_ArmarIV_GazeStab_virt(&sens_virt, &mbs_data_virt_q[0], &mbs_data_virt_qd[0], &mbs_data_virt_qdd[0], 1);
    }
    else if (options->model == 1) // Armar4 W torso
    {
        mbs_sensor_ArmarIV_W_Torso_virt(&sens_virt, &mbs_data_virt_q[0], &mbs_data_virt_qd[0], &mbs_data_virt_qdd[0], 1);
    }
    else if (options->model == 2) // Armar3
    {
        mbs_sensor_ArmarIII_simplified_virt(&sens_virt, &mbs_data_virt_q[0], &mbs_data_virt_qd[0], 3);  // virtual fixation point
    }
 
    predict_optFlow_xy();
    predict_optFlow_index();
    predict_IMU();
 
    for (int row = 0; row < 6; row++)
        for (int col = 0; col < get_n_all_joints(); col++)
        {
            Jac_FK(row, col) = sens_virt.J[row + 1][col + 1];
        }
 
    vel_xFP = Jac_FK.block(0, 0, 6, n_LB_joints) * input->qd_LB_des;
 
    for (int i = 0; i < 3; i++)
    {
        pos_xFP(i) = sens_virt.P[i + 1];
    }
 
    for (int row = 0; row < 3; row++)
        for (int col = 0; col < 3; col++)
        {
            R_xFP[row][col] = sens_virt.R[row + 1][col + 1];
        }
 
    pos_err = input->pos_target - pos_xFP;
 
    //    if (pos_err.norm() > 0.1)
    //    {
    //        ARMARX_IMPORTANT_S << "fixation point error large = " << pos_err(0) << " " << pos_err(1) << " " << pos_err(2) << " m";
    //        pos_err = 0.1 * pos_err / pos_err.norm();
    //    }
 
    double K1 = 0.;//5.;//100;
    double K2 = 0.;//2.;//40;
    double K3 = 0.;//5.;//100;  // null space
    Eigen::VectorXd drift_corr(6);
    drift_corr << K1* pos_err, 0, K2* get_or_err(), 0;
 
    desired_vel_xFP = -1 * vel_xFP + drift_corr;
 
    /************
    * Inverse kinematics (IK)
    * *********/
 
    Jac_IK = Jac_FK.block(0, n_LB_joints, 6, get_n_gaze_joints());
 
    if (options->weighted_inverse)
    {
        Eigen::VectorXd w(get_n_gaze_joints()); // weights
 
        for (int i = 0; i < get_n_gaze_joints(); i++)
        {
            w(i) = 1.;
        }
 
        w(get_n_gaze_joints() - 3) = options->lambda * 19.13 / fabs(q_virt[0]); // zoom
        w(get_n_gaze_joints() - 2) = options->lambda * 1.2054;
        w(get_n_gaze_joints() - 1) = options->lambda * 2.1431;
 
        //w(get_n_gaze_joints() - 4) = 8.;  // minimize eye motion
 
        inv_Jac = weightedPseudoInverse(Jac_IK, w);
    }
    else if (options->pseudo_inverse)
    {
        inv_Jac = pseudoInverse(Jac_IK);
    }
 
    // null space projection
    null_space = Eigen::MatrixXd::Identity(get_n_gaze_joints(), get_n_gaze_joints()) - inv_Jac * Jac_IK;
 
    Eigen::Map<Eigen::VectorXd> tmp(&input->q_UB[0], n_UB_joints);
    Eigen::VectorXd null_space_err(get_n_gaze_joints());
    null_space_err <<  K3*(q_UB_des - tmp), 0., 0., 0.;    // 0 correction for virt joints
 
    qd_gaze_des = inv_Jac * desired_vel_xFP + null_space * null_space_err;
 
    //std::cout << "hello : " << Jac_IK*inv_Jac << std::endl;
    //std::cout << "hello : " << Jac_FK.block(0,0,6,6)*qd_LB_des + Jac_IK*qd_gaze_des << std::endl;
 
    /************
    * Update q_virt
    * *********/
 
    double dt = tsim - last_t_ctrl;
    if (dt > 0.1)
    {
        ARMARX_IMPORTANT_S << "gaze stab control perido too large = " << dt << " s";
        dt = 0.;
    }
 
    for (int i = 0; i < n_virt_joints; i++)
    {
        q_virt[i] += dt * qd_gaze_des(i + n_UB_joints);
    }
 
    last_t_ctrl = tsim;
 
    /************
     * Write output
     * *********/
 
    for (int i = 0; i < n_UB_joints; i++)
    {
        output->qd_UB_des[i] = qd_gaze_des(i);
    }
 
    //ARMARX_LOG_S << pos_err;
    //ARMARX_IMPORTANT_S << "hello" << input->qd_LB_des[7];
    //    for (int i = 0; i < 10; i++)
    //    {
    //        ARMARX_IMPORTANT_S << "input lower body velocity " << i << " " << input->qd_LB_des[i];
    //    }
 
    //    for (int i = 0; i < 5; i++)
    //    {
    //        ARMARX_IMPORTANT_S << "output upper body velocity  " << i << " " << output->qd_UB_des[i];
    //    }
}
 
void GazeStabilization::finish()
{
 
    printf("hello from GazeSatb finish \n");
 
    // free sensors
    free_sensor(&sens_virt);
    free_sensor(&sens_eye);
    free_sensor(&sens_head_imu);
 
}
 
 
// predicotr part
void GazeStabilization::predict_IMU()
{
    mbs_sensor_ArmarIII_simplified_virt(&sens_head_imu, &mbs_data_virt_q[0], &mbs_data_virt_qd[0], 1);  //head IMU kin
 
    gyroscopeRotation_pred[0] = -1 * (sens_head_imu.R[1][1] * sens_head_imu.OM[1] + sens_head_imu.R[2][1] * sens_head_imu.OM[2] + sens_head_imu.R[3][1] * sens_head_imu.OM[3]);
    gyroscopeRotation_pred[2] = sens_head_imu.R[1][2] * sens_head_imu.OM[1] + sens_head_imu.R[2][2] * sens_head_imu.OM[2] + sens_head_imu.R[3][2] * sens_head_imu.OM[3];
    gyroscopeRotation_pred[1] = sens_head_imu.R[1][3] * sens_head_imu.OM[1] + sens_head_imu.R[2][3] * sens_head_imu.OM[2] + sens_head_imu.R[3][3] * sens_head_imu.OM[3];
}
 
void GazeStabilization::predict_optFlow_xy()
{
    // TODO (use head orientation to select x and y optical flow axis)
 
    //optFlow_pred[0] = atan2(sens_virt.V[2], q_virt[0]);
    optFlow_pred[1] = -1 * atan2(sens_virt.V[3], q_virt[0]);
}
 
void GazeStabilization::predict_optFlow_index()
{
    double v_zoom;  // zoom velocity [m/s]
    double v_hor;   // horizontal velocity [m/s]
    double v_ver;   // vertical velocity [m/s]
    double v_tilt;  // tilt rotational velocity [rad/s]
    double v_yaw;   // yaw rotational velocity [rad/s]
    double v_roll;  // roll rotational velocity [rad/s]
 
    const double f = 529.9 * (59.7 / 640.); // focal length converted in deg (from camera calibration file)
 
    mbs_sensor_ArmarIII_simplified_virt(&sens_eye, &mbs_data_virt_q[0], &mbs_data_virt_qd[0], 2);  //eye kin
 
    v_zoom = get_qdvirt(0);
    v_tilt = get_qdvirt(1);
    v_yaw  = sens_eye.R[1][3] * sens_virt.OM[1] + sens_eye.R[2][3] * sens_virt.OM[2] + sens_eye.R[3][3] * sens_virt.OM[3]; //get_qdvirt(2);
 
    v_hor = sens_eye.R[1][2] * sens_eye.V[1] + sens_eye.R[2][2] * sens_eye.V[2] + sens_eye.R[3][2] * sens_eye.V[3];  // vy in eye frame
    v_ver = sens_eye.R[1][3] * sens_eye.V[1] + sens_eye.R[2][3] * sens_eye.V[2] + sens_eye.R[3][3] * sens_eye.V[3];  // vz in eye frame;
    v_roll = sens_eye.R[1][1] * sens_eye.OM[1] + sens_eye.R[2][1] * sens_eye.OM[2] + sens_eye.R[3][1] * sens_eye.OM[3];
 
    //double integral with X in [-15:15] deg and Y in [-11:11] deg for:
    // computed with www.wolframalpha.com
    double surf = 30.*22.;
 
    // 1) zoom : sqrt(X^2 + Y^2)/ surf
    double int_zoom = 6612.53 / surf;
 
    // 2) tilt : sqrt((X*Y)^2 + Y^4) / (f * surf)
    double int_tilt = 40359.9 / (f * surf);
 
    // 3) yaw : sqrt((X*Y)^2 + X^4) / (f * surf)
    double int_yaw = 58943.5 / (f * surf);
 
    // 4) v_hor : constant f
    double int_hor = f ;
 
    // 5) v_ver : constant f
    double int_ver = f;
 
    // 6) roll : sqrt(X^2 + Y^2) / surf
    double int_roll = 6612.53 / surf;
 
 
    // mean distance of the optical flow [deg]
    mean_optFl_pred = 0.;
    mean_optFl_pred += int_zoom * fabs(v_zoom / q_virt[0]);
    mean_optFl_pred += int_tilt * fabs(v_tilt);
    mean_optFl_pred += int_yaw  * fabs(v_yaw);
    mean_optFl_pred += int_hor  * fabs(v_hor / q_virt[0]);
    mean_optFl_pred += int_ver  * fabs(v_ver / q_virt[0]);
    mean_optFl_pred += int_roll * fabs(v_roll);
 
    double f_rad = f * M_PI / 180.0;  // = 0.85
    optFlow_pred[0]  = (f_rad / q_virt[0]) * v_hor + f_rad * v_yaw;
    optFlow_pred[0]  = optFlow_pred[0] / 3.;
 
}