humotion

################################################################

# check for ROS support:

IF (NOT catkin_FOUND)

  MESSAGE(FATAL_ERROR "Error: could not find ROS middleware!")

ENDIF (NOT catkin_FOUND)

        humotion

)

#

###################################

#link_directories (${Boost_LIBRARY_DIRS} ${REFLEXXES_LIBRARY_DIRS} ${catkin_LIBRARY_DIRS})

## Declare a cpp library

    src/mouth_state.cpp

    src/gaze_state.cpp

## Add cmake target dependencies of the executable/library

## as an example, message headers may need to be generated before nodes

## Specify libraries to link a library or executable target against

    ${Boost_LIBRARIES}

    ${catkin_LIBRARIES}

    ${REFLEXXES_LIBRARY}

)

#############

## Install ##

# )

## Mark executables and/or libraries for installation

    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

  ${CMAKE_CURRENT_SOURCE_DIR}/src

  ${CMAKE_CURRENT_SOURCE_DIR}/src

  ${CMAKE_CURRENT_BINARY_DIR}/src)

#workaround for qtcreator ide integration. do not remove!

file(GLOB_RECURSE NODE_DUMMY_TARGETS RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} *.h *.cfg *.yaml *.xml *.launch)

 private:

};

}  // namespace client

    virtual void tick() = 0;

 protected:

};

}  // namespace client

    void tick();

 private:

};

}  // namespace client

    void set_activated(void);

 private:

    void add_motion_generator(MotionGenerator *m);

    GazeState relative_gaze_to_absolute_gaze(GazeState relative);

    typedef std::vector<MotionGenerator *> motion_generator_vector_t;

};

}  // namespace server

    void setup_eyemotion(int dof, float target, float current_position,

                         float current_velocity, Timestamp timestamp);

};

}  // namespace server

 private:

    void start_external_eyeblinks(int duration_left, int duration_right);

    void process_saccadic_eyeblinks();

    void process_periodic_eyeblinks();

    void close_eyelid(int joint_id);

    static const float SACCADE_SPEED_THRESHOLD;

    static const float EYEBLINK_DURATION_MS;

    static const float EYEBLINK_EYERY_MS_MIN;

    static const float EYEBLINK_EYERY_MS_MAX;

    static const float EYEBLINK_BLOCKING_TIME;

};

}  // namespace server

    unsigned int get_and_clear_incoming_position_count();

        ZERO = 0,

        ID_LIP_LEFT_UPPER,

        ID_LIP_LEFT_LOWER,

    float joint_target_position_[JOINT_ID_ENUM_SIZE];

    float joint_target_velocity_[JOINT_ID_ENUM_SIZE];

};

}  // namespace server

    virtual void tick() = 0;

 protected:

};

}  // namespace server

#define INCLUDE_HUMOTION_SERVER_MIDDLEWARE_ROS_H_

#include <boost/shared_ptr.hpp>

#include <ros/ros.h>

#include <string>

#include "humotion/gaze.h"

#include "humotion/mouth.h"

#include "humotion/server/middleware.h"

    bool ok();

    void tick();

 private:

    void incoming_mouth_target(const humotion::mouth::ConstPtr& msg);

    void incoming_gaze_target(const humotion::gaze::ConstPtr& msg);

};

}  // namespace server

    float get_current_speed(int joint_id);

    humotion::Timestamp get_timestamped_state(int joint_id, float *position, float *velocity);

    float limit_target(int joint_id, float val);

    // gaze

    // mouth

};

}  // namespace server

 private:

    float get_breath_offset();

    static const float CONST_GUITTON87_A;

    static const float CONST_GUITTON87_B;

    static const float CONST_BREATH_PERIOD;

    static const float CONST_BREATH_AMPLITUDE;

};

 private:

    void start_motion_generation_thread();

    void motion_generation_thread();

};

}  // namespace server

    Timestamp get_first_timestamp();

 private:

    float interpolate(TimestampedFloat a, TimestampedFloat b, Timestamp timestamp);

};

}  // namespace humotion

    // start middleware

    if (mw == "ROS") {

    } else {

        printf("> ERROR: invalid mw '%s' given. RSB support was droppd. please use ROS\n\n",

               mw.c_str());

//! check if connection is ok

//! \return true if conn is alive

bool Client::ok() {

}

//! do a single middleware tick:

void Client::tick() {

}

//! set mouth position

//! \param MouthState m to set

//! \param send data to server (optional, use manual call to send_*() to trigger update on server)

void Client::update_mouth_target(MouthState m, bool send) {

}

//! set gaze direction

//! \param GazeState m to set

//! \param send data to server (optional, use manual call to send_*() to trigger update on server)

void Client::update_gaze_target(GazeState s, bool send) {

}

//! send all targets to server

void Client::send_all() {

}

//! constructor

//! open a new Middleware instance.

Middleware::Middleware(std::string scope) {

}

//! destructor

//! \param MouthState m to set

//! \param send data to server (optional, use manual call to send_*() to trigger update on server)

void Middleware::update_mouth_target(MouthState m, bool send) {

    if (send) {

        send_mouth_target();

    }

//! \param GazeState m to set

//! \param send data to server (optional, use manual call to send_*() to trigger update on server)

void Middleware::update_gaze_target(GazeState s, bool send) {

    if (send) {

        send_gaze_target();

    }

MiddlewareROS::MiddlewareROS(std::string scope) : Middleware(scope) {

    // start ros core

    if (!ros::isInitialized()) {

        node_name.erase(std::remove(node_name.begin(), node_name.end(), '/'), node_name.end());

        ros::M_string no_remapping;

        ros::init(no_remapping, node_name);

    } else {

        // another ros thread already takes care of spinning

    }

    // create node handle

    ros::NodeHandle n;

    // set up publishers

                                                          "/humotion/mouth/target", 100);

                                                         "/humotion/gaze/target", 100);

}

//! do a single tick

void MiddlewareROS::tick() {

        ros::spinOnce();

    }

}

    // set timestamp

    msg.header.stamp = ros::Time::now();

    // add position to send queue

    // allow ros to handle data

    tick();

    // set timestamp

    msg.header.stamp = ros::Time::now();

        msg.gaze_type = humotion::gaze::GAZETYPE_ABSOLUTE;

    } else {

        msg.gaze_type = humotion::gaze::GAZETYPE_RELATIVE;

    }

    // add position to send queue

    // allow ros to handle data

    tick();

using humotion::server::Controller;

//! constructor

}

//! destructor

    //       (i.e. the neck generator must be added after the eye generator!)

    // eye motion generation:

    // eyelid motion generator

    // neck motion generator

    // mouth motion generator

    // eyebrow motion generator

}

//! add a single motion genrator

void Controller::add_motion_generator(MotionGenerator *m) {

}

//! calculate target angles for all motion generators:

void Controller::calculate_targets() {

    Controller::motion_generator_vector_t::iterator it;

        (*it)->calculate_targets();

    }

}

//! NOTE: this is done in an extra loop to have a low delay between consequent sets:

void Controller::publish_targets() {

    Controller::motion_generator_vector_t::iterator it;

        (*it)->publish_targets();

    }

}

    // check if this timestamp allows a valid conversion:

    Timestamp history_begin =

    // Timestamp history_end   =

    //       joint_interface->get_ts_position(JointInterface::ID_NECK_PAN).get_last_timestamp();

        // to do any guesswork and try to calculate a valid absolute target

        // therefore we will use the last known targets (see below)

        // in case we did not see this timestamp before, show a warning:

            printf("> WARNING: restored/guessed absolute target for unknown timestamp %f "

                   "[this should not happen]\n", relative_target_timestamp.to_seconds());

        }

    } else {

        // all fine, we can reconstruct the absolute target

        // fetch head / camera pose during that timestamp

                JointInterface::ID_NECK_PAN).get_interpolated_value(relative_target_timestamp);

                JointInterface::ID_EYES_LEFT_LR).get_interpolated_value(relative_target_timestamp);

                JointInterface::ID_EYES_RIGHT_LR).get_interpolated_value(relative_target_timestamp);

        //

                JointInterface::ID_NECK_TILT).get_interpolated_value(relative_target_timestamp);

                JointInterface::ID_EYES_BOTH_UD).get_interpolated_value(relative_target_timestamp);

        //

                JointInterface::ID_NECK_ROLL).get_interpolated_value(relative_target_timestamp);

        // safe this timestamp as known:

    }

    // substract offsets

    pan  -= relative.pan_offset;

//! activate controller

void Controller::set_activated(void) {

}

//! update gaze target:

//! \param GazeState with target values for the overall gaze

void Controller::set_gaze_target(humotion::GazeState new_gaze_target) {

        // not yet initialized, ignore incoming targets

        return;

    }

    }

    Controller::motion_generator_vector_t::iterator it;

        (*it)->set_gaze_target(target_gaze);

    }

}

//! update mouth state:

//! \param MouthState with target values for the mouth joints

void Controller::set_mouth_target(MouthState s) {

        // not yet initialized, ignore incoming targets

        return;

    }

    Controller::motion_generator_vector_t::iterator it;

        (*it)->set_mouth_target(s);

    }

}

//! calculate joint targets

void EyeMotionGenerator::calculate_targets() {

    // use the target values for a faster response

    // calculate target angles for the eyes

    // right eye is dominant -> direct output

            - (neck_pan_now);

    // left eye is non dominant -> filtered output: TODO: activate low pass filtered output

    // FIXME: USE LOWPASS FILTER HERE --> output_angle[angle_names::EYE_PAN_L]

    //        + 0.1 * (eye_pan_l_target - output_angle[angle_names::EYE_PAN_L]) etc;

            - (neck_pan_now);

    // tilt

    if (0) {

        eye_pan_r_target = 0.0;

    reflexxes_calculate_profile();

    // tell the joint about the new values

                                reflexxes_position_output->NewPositionVector->VecData[0],

                                reflexxes_position_output->NewVelocityVector->VecData[0]);

                                reflexxes_position_output->NewPositionVector->VecData[1],

                                reflexxes_position_output->NewVelocityVector->VecData[1]);

                                reflexxes_position_output->NewPositionVector->VecData[2],

                                reflexxes_position_output->NewVelocityVector->VecData[2]);

}

void EyeMotionGenerator::publish_targets() {

    // publish values if there is an active gaze input within the last timerange

    if (gaze_target_input_active()) {

    }

}

//! calculate joint targets

void EyebrowMotionGenerator::calculate_targets() {

    // printf("> humotion: calculating eyebrow targets\n");

    // store targets

}

//! publish targets to motor boards:

void EyebrowMotionGenerator::publish_targets() {

    // publish values if there is an active gaze input within the last timerange

    if (gaze_target_input_active()) {

    }

}

//! constructor

EyelidMotionGenerator::EyelidMotionGenerator(JointInterface *j) : EyeMotionGenerator(j) {

}

//! destructor

    float eye_tilt_now  = get_current_position(JointInterface::ID_EYES_BOTH_UD);

    // calculate left eyelid targets

    // calculate right eyelid targets

    // limit target angles

    eyelid_upper_left_target  = limit_target(JointInterface::ID_EYES_LEFT_LID_UPPER,

                                             eyelid_lower_right_target);

    // (temporarily) store the target

                                eyelid_upper_left_target, 0.0);

                                eyelid_lower_left_target, 0.0);

                                eyelid_upper_right_target, 0.0);

                                eyelid_lower_right_target, 0.0);

    // check for saccade

    check_for_saccade();

    // is there a blink request?

    // do we have a saccade & do we want to exec an eyeblink?

    process_saccadic_eyeblinks();

    // manual eyeblinks will ALWAYs get executed as we use

    // a negative block timeout (=timeout has already passed)

    if ((duration_left != 0) || (duration_right != 0)) {

    }

        // nothing to do

    } else if (duration_left < 0) {

        // infinite sleep -> close

    } else {

        // timeout sleep

        start_eyeblink(LEFT, duration_left);

    }

    if (duration_right == 0) {

        // nothing to do

    } else if (duration_right < 0) {

        // infinite sleep -> close

    } else {

        // timeout sleep

        start_eyeblink(RIGHT, duration_right);

    }

}

//! process saccadic blink requests

//! -> when we do a saccade, the chances to blink are much higher

void EyelidMotionGenerator::process_saccadic_eyeblinks() {

        // every n-th's saccade requests an eyeblink

        // here: use 0.3 even though humans use bigger prob value (but that looks stupid on robot)

        if ((std::rand()%3) == 0) {

            start_eyeblink(LEFT);

            start_eyeblink(RIGHT);

        }

    }

}

//! process periodic blink requests

//! -> we want to have an eyeblink every n...m seconds

void EyelidMotionGenerator::process_periodic_eyeblinks() {

        // calculate next timeout for a new periodic eyeblink

        int milliseconds_to_next_blink = EYEBLINK_EYERY_MS_MIN

                + (std::rand() % static_cast<int>(EYEBLINK_EYERY_MS_MAX-EYEBLINK_EYERY_MS_MIN));

                + boost::posix_time::milliseconds(milliseconds_to_next_blink);

    }

        // printf("> periodic eyeblink:\n");

        start_eyeblink(LEFT);

        start_eyeblink(RIGHT);

    // take care of re-opening eye timeout

    for (int i=LEFT; i <= RIGHT; i++) {

            }

        }

    }

    // take care of blocking time

                + boost::posix_time::milliseconds(EYEBLINK_BLOCKING_TIME);

    }

}

//! this will override (=close) the eyelids for all possible eyeblink requests

void EyelidMotionGenerator::override_lids_for_eyeblink() {

    // close the requested eyelids

        close_eyelid(JointInterface::ID_EYES_LEFT_LID_UPPER);

        close_eyelid(JointInterface::ID_EYES_LEFT_LID_LOWER);

    }

        close_eyelid(JointInterface::ID_EYES_RIGHT_LID_UPPER);

        close_eyelid(JointInterface::ID_EYES_RIGHT_LID_LOWER);

    }

    case(JointInterface::ID_EYES_LEFT_LID_UPPER):

    case(JointInterface::ID_EYES_LEFT_LID_LOWER):

        // use the upper value + 10 deg as close state:

        // overwrite last target_

        break;

    case(JointInterface::ID_EYES_RIGHT_LID_UPPER):

    case(JointInterface::ID_EYES_RIGHT_LID_LOWER):

        // use the upper value + 10 deg as close state:

        // overwrite last target_

        break;

    }

}

//! \param int id of side (LEFT or RIGHT)

void EyelidMotionGenerator::start_eyeblink(int side, int duration) {

    // cout << "BLOCKED UNTIL " << eyeblink_blocked_timeout << "\n";

        // return if we are still in the block time

        return;

    }

    // request for n ms eyeblink:

}

//! check if there is an ongoing saccade:

void EyelidMotionGenerator::check_for_saccade() {

    if (get_eye_saccade_active()) {

        // this is a saccade, check if not already in saccade:

            // this is a new saccade! restart blink timer

        }

    } else {

    }

}

void EyelidMotionGenerator::publish_targets() {

    // publish values if there is an active gaze input within the last timerange

    if (gaze_target_input_active()) {

    }

}

//! update gaze target:

//! \param GazeState with target values for the overall gaze

void GazeMotionGenerator::set_gaze_target(GazeState new_gaze_target) {

        printf("> ERROR: gaze targets should be converted to absolute before calling this\n");

        exit(EXIT_FAILURE);

    }

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

    // check requested speed

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

    if (dist > NECK_SACCADE_THRESHOLD) {

    // check for eye getting close to ocolomotor range

    // 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 *

    float left_max  = OMR_LIMIT_TRIGGERS_NECK_SACCADE *

    float right_min = OMR_LIMIT_TRIGGERS_NECK_SACCADE *

    float right_max = OMR_LIMIT_TRIGGERS_NECK_SACCADE *

    float ud_min    = OMR_LIMIT_TRIGGERS_NECK_SACCADE *

    float ud_max    = OMR_LIMIT_TRIGGERS_NECK_SACCADE *

    if (

            (eye_target_l < left_min) || (eye_target_l > left_max) ||

bool GazeMotionGenerator::get_eye_saccade_active() {

    bool saccade_active;

                JointInterface::ID_EYES_LEFT_LR).get_newest_value();

                JointInterface::ID_EYES_RIGHT_LR).get_newest_value();

                JointInterface::ID_EYES_BOTH_UD).get_newest_value();

    float speed_total_l = sqrt(speed_left*speed_left + speed_tilt*speed_tilt);

//! get overall gaze

humotion::GazeState GazeMotionGenerator::get_current_gaze() {

    // shortcut, makes the following easier to read

    gaze.pan = j->get_ts_position(JointInterface::ID_NECK_PAN).get_newest_value() +

            (j->get_ts_position(JointInterface::ID_EYES_LEFT_LR).get_newest_value()

//! constructor

JointInterface::JointInterface() {

    framerate = 50.0;

}

//! destructor

    // lock the tsd_list for this access. by doing this we assure

    // that no other thread accessing this element can diturb the

    // following atomic instructions:

    // printf("> humotion: incoming joint position for joint id 0x%02X "

    // "= %4.2f (ts=%.2f)\n",joint_id,position,timestamp.to_seconds());

}

//! return incoming position data count & clear this counter

//! this can be used as a keep alive status check

//! \return number of incoming joint positions since the last call

unsigned int JointInterface::get_and_clear_incoming_position_count() {

    return i;

}

    // lock the tsd_list for this access. by doing this we assure

    // that no other thread accessing this element can disturb the

    // following atomic instructions:

    // printf("> humotion: incoming joint velocity for joint id 0x%02X = %4.2f "

    // "(ts=%.2f)\n",joint_id,velocity,timestamp.to_seconds());

}

//! return the timestamped float for the given joints position

    // lock the tsd_list for this access. by doing this we assure

    // that no other thread accessing this element can disturb the

    // following atomic instructions

    // search map

        printf("> humotion: no ts_position for joint id 0x%02X found\n", joint_id);

        return TimestampedList();

    }

    // lock the tsd_list for this access. by doing this we assure

    // that no other thread accessing this element can diturb the

    // following atomic instructions

    // search map

        printf("> humotion: no ts_speed for joint id 0x%02X found\n", joint_id);

        return humotion::TimestampedList();

    }

//! enable all mouth joints

void JointInterface::enable_mouth_joints() {

    // already enabled? skip this

        return;

    }

    enable_joint(ID_LIP_CENTER_LOWER);

    enable_joint(ID_LIP_RIGHT_UPPER);

    enable_joint(ID_LIP_RIGHT_LOWER);

}

//! disable all mouth joints

void JointInterface::disable_mouth_joints() {

    // already disabled? skip this

        return;

    }

    disable_joint(ID_LIP_CENTER_LOWER);

    disable_joint(ID_LIP_RIGHT_UPPER);

    disable_joint(ID_LIP_RIGHT_LOWER);

}

//! enable all gaze joints

void JointInterface::enable_gaze_joints() {

    // already enabled? skip this

        return;

    }

    enable_joint(ID_NECK_TILT);

    enable_joint(ID_NECK_ROLL);

}

//! disable all gaze joints

void JointInterface::disable_gaze_joints() {

    // already disabled? skip this

        return;

    }

    disable_joint(ID_NECK_TILT);

    disable_joint(ID_NECK_ROLL);

}

//! fetch maximum allowed joint position

//! check if joint position map is empty

//! \return true if empty

bool JointInterface::get_joint_position_map_empty() {

}

//! call the virtual store function with given position and velocities

//! constructor

//! open a new Middleware instance.

Middleware::Middleware(std::string scope, Controller *c) {

}

//! destructor

    // start ros core?

    if (ros::isInitialized()) {

        // oh another process is doing ros comm, fine, we do not need to call it

    } else {

        // we have to take care of ros

        printf("> no active ros middleware, calling ros::init() "

        printf("> registering on ROS as node %s\n", node_name.c_str());

        ros::M_string no_remapping;

        ros::init(no_remapping, node_name);

    }

    // create node handle

    ros::NodeHandle n;

    // set up subscribers

                                          &MiddlewareROS::incoming_mouth_target ,

                                          this, ros::TransportHints().unreliable());

                                          &MiddlewareROS::incoming_gaze_target,

                                          this, ros::TransportHints().unreliable());

}

//! destructor

//! do a single tick

void MiddlewareROS::tick() {

        ros::spinOnce();

    }

}

    mouth_state.opening_center = msg->opening.center;

    mouth_state.opening_right  = msg->opening.right;

}

    gaze_state.timestamp.set(msg->gaze_timestamp.sec, msg->gaze_timestamp.nsec);

}