humotion

#include "server/gaze_motion_generator.h"

#include "server/server.h"

using namespace std;

using namespace humotion;

using namespace humotion::server;

//saccade detection threshold in deg/s

///const float GazeMotionGenerator::SACCADE_SPEED_THRESHOLD = 15.0;

//some constants to decide if the requested move will trigger a neck or eye saccade

const float GazeMotionGenerator::NECK_SACCADE_THRESHOLD      = 15.0; //deg

const float GazeMotionGenerator::EYE_SACCADE_SPEED_THRESHOLD = 15.0; //deg/s

//close to ocolumotor-range?

const float GazeMotionGenerator::OMR_LIMIT_TRIGGERS_NECK_SACCADE = 0.95; //0.95 = reaching 95% of OMR will trigger a correction

    : ReflexxesMotionGenerator(j, dof, t){

GazeMotionGenerator::~GazeMotionGenerator(){

void GazeMotionGenerator::set_gaze_target(GazeState new_gaze_target){

    if (requested_gaze_state.gaze_type == GazeState::GAZETYPE_RELATIVE){

    //check magnitude of gaze change to detect eye-neck saccades:

    //check requested speed

    float speed = fabs(requested_gaze_state.distance_pt_abs(new_gaze_target)) / (new_gaze_target.timestamp.to_seconds()-requested_gaze_state.timestamp.to_seconds());

    //check magnitude and speed of gaze change to detect eye-neck saccades:

    if (dist > NECK_SACCADE_THRESHOLD){

        //the next saccade has to use neck motion as well

        if (speed > EYE_SACCADE_SPEED_THRESHOLD){

    }else{

    //check for eye getting close to ocolomotor range

    //float eye_pos_l = joint_interface->get_ts_position(JointInterface::ID_EYES_LEFT_LR).get_newest_value();

    //float eye_pos_r = joint_interface->get_ts_position(JointInterface::ID_EYES_RIGHT_LR).get_newest_value();

    //float eye_pos_ud = joint_interface->get_ts_position(JointInterface::ID_EYES_BOTH_UD).get_newest_value();

    //actually it makes more sense to trigger the neck saccade based on the target getting out of the OMR

    //min/max bounds:

    float left_min  = OMR_LIMIT_TRIGGERS_NECK_SACCADE * joint_interface->get_joint_min(JointInterface::ID_EYES_LEFT_LR);

    float left_max  = OMR_LIMIT_TRIGGERS_NECK_SACCADE * joint_interface->get_joint_max(JointInterface::ID_EYES_LEFT_LR);

    float right_min = OMR_LIMIT_TRIGGERS_NECK_SACCADE * joint_interface->get_joint_min(JointInterface::ID_EYES_RIGHT_LR);

    float right_max = OMR_LIMIT_TRIGGERS_NECK_SACCADE * joint_interface->get_joint_max(JointInterface::ID_EYES_RIGHT_LR);

    float ud_min    = OMR_LIMIT_TRIGGERS_NECK_SACCADE * joint_interface->get_joint_min(JointInterface::ID_EYES_BOTH_UD);

    float ud_max    = OMR_LIMIT_TRIGGERS_NECK_SACCADE * joint_interface->get_joint_max(JointInterface::ID_EYES_BOTH_UD);

            (eye_target_ud < ud_min) || (eye_target_ud > ud_max)){

        //the eyeball gets close to OMR, activate a neck compensation motion:

    }else{

    //use base class set method:

bool GazeMotionGenerator::get_eye_saccade_active(){

    float speed_left  = joint_interface->get_ts_speed(JointInterface::ID_EYES_LEFT_LR).get_newest_value();

    float speed_right = joint_interface->get_ts_speed(JointInterface::ID_EYES_RIGHT_LR).get_newest_value();

    float speed_tilt  = joint_interface->get_ts_speed(JointInterface::ID_EYES_BOTH_UD).get_newest_value();

    //thresholding

    if ((speed_total_l > EYE_SACCADE_SPEED_THRESHOLD) || (speed_total_r > EYE_SACCADE_SPEED_THRESHOLD)){

        //this is a saccade

    }else{

GazeState GazeMotionGenerator::get_current_gaze(){

    //shortcut, makes the following easier to read

            (j->get_ts_position(JointInterface::ID_EYES_LEFT_LR).get_newest_value() + j->get_ts_position(JointInterface::ID_EYES_RIGHT_LR).get_newest_value())/2.0;

    gaze.tilt = j->get_ts_position(JointInterface::ID_NECK_TILT).get_newest_value() + j->get_ts_position(JointInterface::ID_EYES_BOTH_UD).get_newest_value();

    gaze.vergence = j->get_ts_position(JointInterface::ID_EYES_LEFT_LR).get_newest_value() - j->get_ts_position(JointInterface::ID_EYES_RIGHT_LR).get_newest_value();