/ - Diff - humotion - Research for Cognitive Interaction

     ## Declare a cpp library
     add_library(${PROJECT_NAME}
         src/mouth_state.cpp
         src/gaze_state.cpp
         src/client/client.cpp
         src/client/middleware.cpp
         src/client/middleware_ros.cpp
         src/server/server.cpp
         src/server/config.cpp
         src/server/middleware.cpp
         src/server/middleware_ros.cpp
         src/server/controller.cpp
         src/server/joint_interface.cpp
         src/server/motion_generator.cpp
-...
         src/server/eyelid_motion_generator.cpp
         src/server/eyebrow_motion_generator.cpp
         src/server/neck_motion_generator.cpp
         src/mouth_state.cpp
         src/gaze_state.cpp
         src/timestamp.cpp
         src/timestamped_list.cpp
+    )

     gen = ParameterGenerator()
     group = gen.add_group("My Group")
     group.add("my_group_param", int_t, 0, "An int within My Group", 0)
     general_group = gen.add_group("general")
     general_group.add("eye_saccade_velocity_threshold", double_t, 0, "eye velocity threshold for saccade detection (in deg/s)", 15.0, 1.0, 30.0)
     general_group.add("neck_saccade_threshold", double_t, 0, "magnitude of gaze change that triggers neck saccade (in deg)", 15.0, 1.0, 30.0)
     general_group.add("neck_saccade_omr_trigger", double_t, 0, "a deflection exceeding <VAL> * OMR will trigger a correction neck saccade", 0.95, 0.1, 1.0)
     neck_group = gen.add_group("neck")
     neck_group.add("neck_max_acceleration", int_t, 0, "maximum neck acceleration limit (in deg/s^2)", 1000, 100, 4000)
     neck_group.add("neck_max_velocity", int_t, 0, "maximum neck velocity limit (in deg/s)", 1000, 100, 800)
     neck_group.add("neck_velocity_scale", double_t, 0, "scaling factor for neck accelerations (1.0 = full human velocities)", 0.7, 0.1, 1.0)
     #float max_speed = (CONST_GUITTON87_A * distance_abs + CONST_GUITTON87_B);
     neck_group.add("neck_velocity_lin_eq_const_a", double_t, 0, "constant A (scale) for linear velocity equation (guitton 4.39)", 4.39 / 2.0, 1.0, 10.0)
     neck_group.add("neck_velocity_lin_eq_const_b", double_t, 0, "constant B (offset) for linear velocity equation (guitton 4.39)", 106.0 / 2.0, 1, 200)
     #others
     neck_group.add("neck_breath_period", int_t, 0, "breath period (inhale+pause+exhale) in ms)", 3*1500, 3000, 5000)
     neck_group.add("neck_breath_amplitude", double_t, 0, "amplitude of head tilt deflection during breath (given in deg)", 1.0, 0.0, 5.0)
     eye_group = gen.add_group("eye")
     eye_group.add("eye_max_acceleration", int_t, 0, "maximum eye acceleration limit (in deg/s^2)", 80000, 1000, 80000)
     eye_group.add("eye_max_velocity", int_t, 0, "maximum eye velocity limit (in deg/s)", 700, 100, 700)
     eye_group.add("eye_velocity_scale", double_t, 0, "scaling factor for eye accelerations (1.0 = full human velocities)", 1.0, 0.1, 1.0)
     eyeblink_group = gen.add_group("eyeblink")
     eyeblink_group.add("eyeblink_duration", int_t, 0, "eyeblink duration (in ms)", 150, 50, 500)
     eyeblink_group.add("eyeblink_every_min", int_t, 0, "eyeblink every n ms, lower bound (in ms)",  2000, 1000, 10000)
     eyeblink_group.add("eyeblink_every_max", int_t, 0, "eyeblink every n ms, upper bound (in ms)", 10000, 1000, 20000)
     eyeblink_group.add("eyeblink_blocking_time", int_t, 0, "eyeblink blocked timeout (in ms)", 1000, 100, 5000)
     #eyelid:
     # 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;
+    #
     # static const float MOUTH_MIN_OPENING;
+    #
     #neck
     #    static const float CONST_GUITTON87_A;
     #    static const float CONST_GUITTON87_B;
+    #
     #    static const float CONST_BREATH_PERIOD;
     #    static const float CONST_BREATH_AMPLITUDE;
+    #
     #misc for neck and eyes:
     #max accel
     #v_scale
     #timing_mode_enum = gen.enum([
     #    gen.const("free_running", int_t, 0, "camera acquires images at a maximum possible framerate"),

     /*
     * This file is part of humotion
+    *
     * Copyright(c) sschulz <AT> techfak.uni-bielefeld.de
     * http://opensource.cit-ec.de/projects/humotion
+    *
     * This file may be licensed under the terms of the
     * GNU Lesser General Public License Version 3 (the ``LGPL''),
     * or (at your option) any later version.
+    *
     * Software distributed under the License is distributed
     * on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either
     * express or implied. See the LGPL for the specific language
     * governing rights and limitations.
+    *
     * You should have received a copy of the LGPL along with this
     * program. If not, go to http://www.gnu.org/licenses/lgpl.html
     * or write to the Free Software Foundation, Inc.,
     * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+    *
     * The development of this software was supported by the
     * Excellence Cluster EXC 277 Cognitive Interaction Technology.
     * The Excellence Cluster EXC 277 is a grant of the Deutsche
     * Forschungsgemeinschaft (DFG) in the context of the German
     * Excellence Initiative.
     */
     #ifndef INCLUDE_HUMOTION_SERVER_CONFIG_H_
     #define INCLUDE_HUMOTION_SERVER_CONFIG_H_
     namespace humotion {
     namespace server {
     class Config {
      public:
         Config();
         ~Config();
         void init_defaults();
         // ********************************************************
         // NOTE: See config.cpp for explanations and default values
         // ********************************************************
         // saccade detection thresholds
         float threshold_velocity_eye_saccade;
         float threshold_angle_neck_saccade;
         float threshold_angle_omr_limit;
         // neck motion generation configuration
         float scale_velocity_neck;
         float scale_acceleration_neck;
         float limit_velocity_neck;
         float limit_acceleration_neck;
         // eye motion generation configuration
         float scale_velocity_eye;
         float scale_acceleration_eye;
         float limit_velocity_eye;
         float limit_acceleration_eye;
         // parameters fo the breathing pattern
         float breath_period;
         float breath_amplitude;
         // parameters for eye blinking
         float eyeblink_duration;
         float eyeblink_periodic_distribution_lower;
         float eyeblink_periodic_distribution_upper;
         float eyeblink_probability_after_saccade;
         float eyeblink_blocked_time;
     };
     }  // namespace server
     }  // namespace humotion
     #endif  // INCLUDE_HUMOTION_SERVER_CONFIG_H_

     #include "humotion/gaze_state.h"
     #include "humotion/mouth_state.h"
     #include "humotion/server/config.h"
     #include "humotion/server/joint_interface.h"
     #include "humotion/server/motion_generator.h"
     #include "humotion/timestamp.h"
-...
         motion_generator_vector_t motion_generator_vector_;
         JointInterface *joint_interface_;
         Config *config_;
         bool activated_;

     class EyeMotionGenerator : public GazeMotionGenerator {
      public:
         explicit EyeMotionGenerator(JointInterface *j);
         EyeMotionGenerator(JointInterface *j, Config *cfg);
         ~EyeMotionGenerator();
         void calculate_targets();
         void publish_targets();
         static const float SACCADE_SPEED_THRESHOLD;
         // static const float SACCADE_SPEED_THRESHOLD;
      private:
         void setup_eyemotion(int dof, float target, float current_position,
                              float current_velocity, Timestamp timestamp);

     class EyebrowMotionGenerator : public MotionGenerator {
      public:
         explicit EyebrowMotionGenerator(JointInterface *j);
         explicit EyebrowMotionGenerator(JointInterface *j, Config *cfg);
         ~EyebrowMotionGenerator();
         void calculate_targets();
         void publish_targets();

     class EyelidMotionGenerator : public EyeMotionGenerator {
      public:
         explicit EyelidMotionGenerator(JointInterface *j);
         EyelidMotionGenerator(JointInterface *j, Config *cfg);
         ~EyelidMotionGenerator();
         void calculate_targets();
-...
         void handle_eyeblink_timeout();
         void override_lids_for_eyeblink();
         void check_for_saccade();
         void start_eyeblink(int side, int duration = EYEBLINK_DURATION_MS);
         void start_eyeblink(int side, int duration);
         void close_eyelid(int joint_id);
-...
         bool eyeblink_active_[SIDE_ID_SIZE];
         bool eyelid_closed_[SIDE_ID_SIZE];
         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;
         boost::system_time periodic_blink_start_time_;
         boost::system_time eyeblink_timeout_[SIDE_ID_SIZE];
         boost::system_time eyeblink_blocked_timeout_;

     class GazeMotionGenerator : public ReflexxesMotionGenerator {
      public:
         GazeMotionGenerator(JointInterface *j, int dof, float t);
         GazeMotionGenerator(JointInterface *j, Config *cfg, int dof, float t);
         ~GazeMotionGenerator();
         void set_gaze_target(GazeState s);
         static const float SACCADE_SPEED_THRESHOLD;
         static const float SACCADE_LATENCY;
      protected:
         GazeState get_current_gaze();
         bool get_eye_saccade_active();
         bool neck_saccade_requested;
         bool neck_saccade_omr;
      private:
         // constants
         static const float NECK_SACCADE_THRESHOLD;
         static const float EYE_SACCADE_SPEED_THRESHOLD;
         static const float OMR_LIMIT_TRIGGERS_NECK_SACCADE;
     };
     }  // namespace server

     #include <string>
     #include "humotion/server/config.h"
     #include "humotion/server/joint_interface.h"
     namespace humotion {
-...
     class MotionGenerator {
      public:
         explicit MotionGenerator(JointInterface *j);
         MotionGenerator(JointInterface *j, Config *cfg);
         ~MotionGenerator();
         virtual void calculate_targets() = 0;
-...
         virtual void set_mouth_target(MouthState s);
      protected:
         Config *config;
         float get_current_position(int joint_id);
         float get_current_speed(int joint_id);
         humotion::Timestamp get_timestamped_state(int joint_id, float *position, float *velocity);

     class MouthMotionGenerator : public MotionGenerator {
      public:
         explicit MouthMotionGenerator(JointInterface *j);
         MouthMotionGenerator(JointInterface *j, Config *cfg);
         ~MouthMotionGenerator();
         void calculate_targets();
         void publish_targets();

     class NeckMotionGenerator : public GazeMotionGenerator {
      public:
         explicit NeckMotionGenerator(JointInterface *j);
         NeckMotionGenerator(JointInterface *j, Config *cfg);
         ~NeckMotionGenerator();
         void calculate_targets();
         void publish_targets();
-...
         bool neck_saccade_reached_goal_;
         bool neck_saccade_active_;
         float breath_time_;
         static const float CONST_GUITTON87_A;
         static const float CONST_GUITTON87_B;
         static const float CONST_BREATH_PERIOD;
         static const float CONST_BREATH_AMPLITUDE;
     };
     }  // namespace server

     class ReflexxesMotionGenerator : public MotionGenerator{
      public:
         ReflexxesMotionGenerator(JointInterface *j, int dof, float t);
         ReflexxesMotionGenerator(JointInterface *j, Config *cfg, int dof, float t);
         ~ReflexxesMotionGenerator();
      protected:

     /*
     * This file is part of humotion
+    *
     * Copyright(c) sschulz <AT> techfak.uni-bielefeld.de
     * http://opensource.cit-ec.de/projects/humotion
+    *
     * This file may be licensed under the terms of the
     * GNU Lesser General Public License Version 3 (the ``LGPL''),
     * or (at your option) any later version.
+    *
     * Software distributed under the License is distributed
     * on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either
     * express or implied. See the LGPL for the specific language
     * governing rights and limitations.
+    *
     * You should have received a copy of the LGPL along with this
     * program. If not, go to http://www.gnu.org/licenses/lgpl.html
     * or write to the Free Software Foundation, Inc.,
     * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+    *
     * The development of this software was supported by the
     * Excellence Cluster EXC 277 Cognitive Interaction Technology.
     * The Excellence Cluster EXC 277 is a grant of the Deutsche
     * Forschungsgemeinschaft (DFG) in the context of the German
     * Excellence Initiative.
     */
     #include "humotion/server/config.h"
     using humotion::server::Config;
     Config::Config() {
         init_defaults();
+    }
     Config::~Config() {
+    }
     void Config::init_defaults() {
         // saccade detection thresholds:
         // 1) velocity threshold, values above this value trigger an eye saccade
         //    value is given in deg/s
         threshold_velocity_eye_saccade = 15.0;
         // 2) angular threshold, a target change higher than this value will trigger a neck saccade
         //    value is given in deg
         threshold_angle_neck_saccade = 15.0;
         // 3) eyes reaching ocolumotor limits will trigger correction saccade
         //    value given in percent (NOTE: 1.0 = 100%)
         threshold_angle_omr_limit = 0.95;
         // neck motion generation configuration
         // humotion calculates neck velocities based on the linear equation Hmax from
         // [Guitton87] "Gaze control in humans: eye-head coordination during orienting movements ..."
         // In order to allow better adaption to the robot capabilities humotion allows to
         // scale the calculated velocity, value is given in percent (NOTE: 1.0 = 100% human velocity)
         scale_velocity_neck = 0.5;
         // scale acceleration
         scale_acceleration_neck = 0.7;
         // additionally humotion allows to limit the maximum velocity, value is given in deg/s
         limit_velocity_neck = 700.0;
         // limit the maximum acceleration, value is given in deg/s^2
         limit_acceleration_neck = 1000.0;
         // eye motion generation configuration
         // scale the calculated velocity, value is given in percent (NOTE: 1.0 = 100% human velocity)
         scale_velocity_eye = 1.0;
         // scale acceleration
         scale_acceleration_eye = 1.0;
         // additionally humotion allows to limit the maximum velocity, value is given in deg/s
         limit_velocity_eye = 700.0;
         // limit the maximum acceleration, value is given in deg/s^2
         limit_acceleration_eye = 80000;
         // parameters fo the breathing pattern
         // healthy adult human: 12-15 breaths/min (see e.g. "Ganong's review of medical physiology")
         // total breathe: 60/12-15 = 3-5s
         // inhale 1.5-2s
         // exhale 1.5-2s
         // pause      2s
         // overall period given in seconds
         breath_period = 1.5 + 1.5 + 1.5;  // inhale, pause & exhale
         // amplitude given in degrees
         breath_amplitude = 1.0;
         // parameters for eye blinking
         // duration for one eyeblink, value is given in seconds
         eyeblink_duration = 0.15;
         // occurance of periodic eyeblinks, uniformly distributed over the given range
         // typical values for a human are one blink every 2...10s
         // values are given in seconds
         eyeblink_periodic_distribution_lower = 2.0;
         eyeblink_periodic_distribution_upper = 10.0;
         // probability that one eye saccade causes an eyeblink, value given in percent
         // for humans this is up to 95%, this gets quite annoying on the robot so the default is lower
         eyeblink_probability_after_saccade = 0.33;
         // blocking time where further eyeblinks are suppressed, value given in seconds
         eyeblink_blocked_time = 1000.0;
+    }

     // using namespace humotion::server;
     using humotion::server::Controller;
     using humotion::server::Config;
     //! constructor
     Controller::Controller(JointInterface *j) {
         activated_ = false;
         joint_interface_ = j;
         config_ = new Config();
+    }
     //! destructor
-...
         //       (i.e. the neck generator must be added after the eye generator!)
         // eye motion generation:
         add_motion_generator(new EyeMotionGenerator(joint_interface_));
         add_motion_generator(new EyeMotionGenerator(joint_interface_, config_));
         // eyelid motion generator
         add_motion_generator(new EyelidMotionGenerator(joint_interface_));
         add_motion_generator(new EyelidMotionGenerator(joint_interface_, config_));
         // neck motion generator
         add_motion_generator(new NeckMotionGenerator(joint_interface_));
         add_motion_generator(new NeckMotionGenerator(joint_interface_, config_));
         // mouth motion generator
         add_motion_generator(new MouthMotionGenerator(joint_interface_));
         add_motion_generator(new MouthMotionGenerator(joint_interface_, config_));
         // eyebrow motion generator
         add_motion_generator(new EyebrowMotionGenerator(joint_interface_));
         add_motion_generator(new EyebrowMotionGenerator(joint_interface_, config_));
+    }
     //! add a single motion genrator

     using humotion::server::EyeMotionGenerator;
     // saccade detection threshold in deg/s
     const float EyeMotionGenerator::SACCADE_SPEED_THRESHOLD = 15.0;
     //! constructor
     EyeMotionGenerator::EyeMotionGenerator(JointInterface *j) :
         GazeMotionGenerator(j, 3, 1.0/Server::MOTION_UPDATERATE) {
     EyeMotionGenerator::EyeMotionGenerator(JointInterface *j, Config *cfg) :
         GazeMotionGenerator(j, cfg, 3, 1.0/Server::MOTION_UPDATERATE) {
+    }
-...
                                              humotion::Timestamp timestamp) {
         // get distance to target:
         float distance_abs = fabs(target - current_position);
         // get max speed: factor can be found in encyc britannica:
         // "linear.... being 300° per second for 10° and 500° per second for 30°" (max=700)
         float max_speed = fmin(700.0, 10.0*distance_abs + 200.0);
         float max_velocity = fmin(700.0, 10.0*distance_abs + 200.0);
         // scale and limit max speed
         max_velocity = max_velocity * config->scale_velocity_eye;
         max_velocity = fmin(max_velocity, config->limit_velocity_eye);
         // max accel: use data from:
         // "Speed and Accuracy of Saccadic Eye Movements:
         //      Characteristics of Impulse Variability in the Oculomotor System"
         // http://www-personal.umich.edu/~kornblum/files/journal_exp_psych_HPP_15-3.pdf [table 2]
         float max_accel = fmin(80000.0, 1526.53*distance_abs + 10245.4);
         // scale and limit max acceleration
         max_accel = max_accel * config->scale_acceleration_eye;
         max_accel = fmin(max_accel, config->limit_acceleration_eye);
         // feed reflexxes api with data
         reflexxes_set_input(dof, target, current_position, current_velocity,
                             timestamp, max_speed, max_accel);
                             timestamp, max_velocity, max_accel);
+    }
     //! calculate joint targets

     using humotion::server::EyebrowMotionGenerator;
     //! constructor
     EyebrowMotionGenerator::EyebrowMotionGenerator(JointInterface *j) : MotionGenerator(j) {
     EyebrowMotionGenerator::EyebrowMotionGenerator(JointInterface *j, Config *cfg) :
         MotionGenerator(j, cfg) {
+    }

     // using namespace humotion::server;
     using humotion::server::EyelidMotionGenerator;
     // saccade detection threshold in deg/s
     const float EyelidMotionGenerator::SACCADE_SPEED_THRESHOLD = 15.0;  // deg/s
     // eyeblink duration:
     const float EyelidMotionGenerator::EYEBLINK_DURATION_MS    = 150.0;  // ms
     // periodic eyeblinks: every 2..10s:
     const float EyelidMotionGenerator::EYEBLINK_EYERY_MS_MIN =  2000.0;  // ms
     const float EyelidMotionGenerator::EYEBLINK_EYERY_MS_MAX = 10000.0;  // ms
     // how many seconds do we block further blinks after each eyeblink?
     const float EyelidMotionGenerator::EYEBLINK_BLOCKING_TIME = 1000.0;  // ms
     using humotion::server::Config;
     //! constructor
     EyelidMotionGenerator::EyelidMotionGenerator(JointInterface *j) : EyeMotionGenerator(j) {
     EyelidMotionGenerator::EyelidMotionGenerator(JointInterface *j, Config *cfg) :
         EyeMotionGenerator(j, cfg) {
         saccade_blink_active_ = false;
         saccade_blink_requested_ = false;
         eyelid_closed_[LEFT] = false;
-...
     void EyelidMotionGenerator::process_saccadic_eyeblinks() {
         if (saccade_blink_requested_) {
             // 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) {
                 // printf("> saccadic eyeblink:\n");
                 start_eyeblink(LEFT);
                 start_eyeblink(RIGHT);
             float frnd = static_cast <float> (std::rand()) / static_cast <float> (RAND_MAX);
             if (frnd <= config->eyeblink_probability_after_saccade) {
                 printf("> saccadic eyeblink:\n");
                 start_eyeblink(LEFT, config->eyeblink_duration * 1000.0);
                 start_eyeblink(RIGHT, config->eyeblink_duration * 1000.0);
+            }
             saccade_blink_requested_ = false;
+        }
-...
     //! -> we want to have an eyeblink every n...m seconds
     void EyelidMotionGenerator::process_periodic_eyeblinks() {
         if (eyeblink_active_[LEFT] || eyeblink_active_[RIGHT]) {
             float range = config->eyeblink_periodic_distribution_upper
                     - config->eyeblink_periodic_distribution_lower;
             // random number 0...1
             float frnd = static_cast <float> (std::rand()) / static_cast <float> (RAND_MAX);
             // 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));
             float seconds_to_next_blink =
                     config->eyeblink_periodic_distribution_lower  + frnd * range;
             periodic_blink_start_time_ = boost::get_system_time()
                     + boost::posix_time::milliseconds(milliseconds_to_next_blink);
                     + boost::posix_time::seconds(seconds_to_next_blink);
+        }
         if (boost::get_system_time() > periodic_blink_start_time_) {
             // printf("> periodic eyeblink:\n");
             start_eyeblink(LEFT);
             start_eyeblink(RIGHT);
             start_eyeblink(LEFT, config->eyeblink_duration * 1000.0);
             start_eyeblink(RIGHT, config->eyeblink_duration * 1000.0);
+        }
+    }
-...
         // take care of blocking time
         if (eyeblink_active_[LEFT] || eyeblink_active_[RIGHT]) {
             eyeblink_blocked_timeout_ = boost::get_system_time()
                     + boost::posix_time::milliseconds(EYEBLINK_BLOCKING_TIME);
                     + boost::posix_time::seconds(config->eyeblink_blocked_time);
+        }
+    }

     #include "humotion/server/server.h"
     using humotion::server::GazeMotionGenerator;
     // 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?
     // 0.95 = reaching 95% of OMR will trigger a correction
     const float GazeMotionGenerator::OMR_LIMIT_TRIGGERS_NECK_SACCADE = 0.95;
     using humotion::server::Config;
     //! constructor
     GazeMotionGenerator::GazeMotionGenerator(JointInterface *j, int dof, float t)
         : ReflexxesMotionGenerator(j, dof, t) {
     GazeMotionGenerator::GazeMotionGenerator(JointInterface *j, Config *cfg, int dof, float t)
         : ReflexxesMotionGenerator(j, cfg, dof, t) {
+    }
     //! destructor
-...
                 / (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) {
         if (dist > config->threshold_angle_neck_saccade) {
             // the next saccade has to use neck motion as well
             if (speed > EYE_SACCADE_SPEED_THRESHOLD) {
             if (speed > config->threshold_velocity_eye_saccade) {
                 neck_saccade_requested = true;
+            }
         } else {
-...
         float eye_target_ud = joint_interface_->get_target_position(JointInterface::ID_EYES_BOTH_UD);
         // min/max bounds
         float left_min  = OMR_LIMIT_TRIGGERS_NECK_SACCADE *
         float left_min  = config->threshold_angle_omr_limit *
                 joint_interface_->get_joint_min(JointInterface::ID_EYES_LEFT_LR);
         float left_max  = OMR_LIMIT_TRIGGERS_NECK_SACCADE *
         float left_max  = config->threshold_angle_omr_limit *
                 joint_interface_->get_joint_max(JointInterface::ID_EYES_LEFT_LR);
         float right_min = OMR_LIMIT_TRIGGERS_NECK_SACCADE *
         float right_min = config->threshold_angle_omr_limit *
                 joint_interface_->get_joint_min(JointInterface::ID_EYES_RIGHT_LR);
         float right_max = OMR_LIMIT_TRIGGERS_NECK_SACCADE *
         float right_max = config->threshold_angle_omr_limit *
                 joint_interface_->get_joint_max(JointInterface::ID_EYES_RIGHT_LR);
         float ud_min    = OMR_LIMIT_TRIGGERS_NECK_SACCADE *
         float ud_min    = config->threshold_angle_omr_limit *
                 joint_interface_->get_joint_min(JointInterface::ID_EYES_BOTH_UD);
         float ud_max    = OMR_LIMIT_TRIGGERS_NECK_SACCADE *
         float ud_max    = config->threshold_angle_omr_limit *
                 joint_interface_->get_joint_max(JointInterface::ID_EYES_BOTH_UD);
         if (
-...
         float speed_total_r = sqrt(speed_right*speed_right + speed_tilt*speed_tilt);
         // thresholding
         if ((speed_total_l > EYE_SACCADE_SPEED_THRESHOLD) ||
                 (speed_total_r > EYE_SACCADE_SPEED_THRESHOLD)) {
         if ((speed_total_l > config->threshold_velocity_eye_saccade) ||
                 (speed_total_r > config->threshold_velocity_eye_saccade)) {
             // this is a saccade
             saccade_active = true;
         } else {

     //! attach to dynamic reconfigure server
     void MiddlewareROS::attach_to_reconfiguration_server(ros::NodeHandle priv_nodehandle) {
         ROS_DEBUG("connecting to dynamic reconfiguration server");
         ros::NodeHandle reconf_node(priv_nodehandle, "configuration");
         ros::NodeHandle reconf_node(priv_nodehandle, "humotion/configuration");
         reconf_server_ = new dynamic_reconfigure::Server<humotion::humotionConfig>(reconf_node);
         reconf_server_->setCallback(boost::bind(

     #include "humotion/server/motion_generator.h"
     using humotion::server::MotionGenerator;
     using humotion::server::Config;
     //! constructor
     MotionGenerator::MotionGenerator(JointInterface *j) {
     MotionGenerator::MotionGenerator(JointInterface *j, Config *cfg) {
         config = cfg;
         joint_interface_ = j;
         last_mouth_target_update_ = boost::posix_time::ptime(boost::posix_time::min_date_time);
         last_gaze_target_update_  = boost::posix_time::ptime(boost::posix_time::min_date_time);

     #include "humotion/server/mouth_motion_generator.h"
     using humotion::server::MouthMotionGenerator;
     using humotion::server::Config;
     // minimum mouth opening
     const float MouthMotionGenerator::MOUTH_MIN_OPENING = 9.0;  // mm
     //! constructor
     MouthMotionGenerator::MouthMotionGenerator(JointInterface *j) : MotionGenerator(j) {
     MouthMotionGenerator::MouthMotionGenerator(JointInterface *j, Config *cfg) :
         MotionGenerator(j, cfg) {
+    }
     //! destructor

     #include "humotion/server/server.h"
     using humotion::server::NeckMotionGenerator;
     const float NeckMotionGenerator::CONST_GUITTON87_A = 4.39/2.0;
     const float NeckMotionGenerator::CONST_GUITTON87_B = 106.0/2.0;
     // healthy adult human: 12-15 breaths/min (see "Ganong's review of medical physiology")
     // total: 60/12-15 = 3-5s
     // inhale 1.5-2s
     // exhale 1.5-2s
     // pause      2s
     const float NeckMotionGenerator::CONST_BREATH_PERIOD = 1500.0+1500.0+1500.0;  // given in ms
     const float NeckMotionGenerator::CONST_BREATH_AMPLITUDE = 1.0;  // degrees
     using humotion::server::Config;
     //! constructor
     NeckMotionGenerator::NeckMotionGenerator(JointInterface *j) :
         GazeMotionGenerator(j, 3, 1.0/Server::MOTION_UPDATERATE) {
     NeckMotionGenerator::NeckMotionGenerator(JointInterface *j, Config *cfg) :
         GazeMotionGenerator(j, cfg, 3, 1.0/Server::MOTION_UPDATERATE) {
         breath_time_ = 0.0;
+    }
-...
         // we want to have a constant acceleration
         // -> triangular wave as speeds -> (x<0.5)? 2*x*x:  1- 2*(1-x)**2 = 4x - 2x**2 - 1
         float breath_offset = 0.0;
         // 0...1 -> move up, 1..2 -> return, 2..3 -> still
         float breath_time_normalized = (breath_time_ * 3)/CONST_BREATH_PERIOD;
         float breath_time_normalized = (breath_time_ * 3)/config->breath_period;
         if (breath_time_normalized <= 0.5) {
             // accelerated motion
             breath_offset = CONST_BREATH_AMPLITUDE * (2.0 * pow(breath_time_normalized, 2));
             breath_offset = config->breath_amplitude * (2.0 * pow(breath_time_normalized, 2));
         } else if (breath_time_normalized <= 1.0) {
             // deaccelerate
             breath_offset = CONST_BREATH_AMPLITUDE * (1.0 - 2.0 * pow(1.0 - breath_time_normalized, 2));
             breath_offset = config->breath_amplitude * (1.0 - 2.0
                                                         * pow(1.0 - breath_time_normalized, 2));
         } else if (breath_time_normalized <= 1.5) {
             // accelerate again
             breath_offset = CONST_BREATH_AMPLITUDE * (1.0 - (2.0 * pow(breath_time_normalized-1, 2)));
             breath_offset = config->breath_amplitude * (1.0 - (2.0 * pow(breath_time_normalized-1, 2)));
         } else if (breath_time_normalized <= 2.0) {
             breath_offset = CONST_BREATH_AMPLITUDE * (2.0 * pow(2.0 - breath_time_normalized, 2));
             breath_offset = config->breath_amplitude * (2.0 * pow(2.0 - breath_time_normalized, 2));
         } else if (breath_time_normalized <= 3.0) {
             // pause for some time
             breath_offset = 0;
-...
         // fetch next time
         breath_time_ += 1000.0/Server::MOTION_UPDATERATE;
         if (breath_time_ >= CONST_BREATH_PERIOD) {
             breath_time_ -= CONST_BREATH_PERIOD;
         if (breath_time_ >= config->breath_period) {
             breath_time_ -= config->breath_period;
+        }
         return breath_offset;
-...
         // get distance to target
         float distance_abs = fabs(target - current_position);
         // get max speed: according to [guitton87] there is a relation
         // get max speed: according to the equation Hmax from [guitton87] there is a linear relation
         // between distance_abs and v_max_head:
         // v_max = 4.39 * d_total + 106.0 (in degrees)
         float max_speed = (CONST_GUITTON87_A * distance_abs + CONST_GUITTON87_B);
         float max_velocity = 4.39 * distance_abs + 106.0;
         // scale and limit max speed:
         max_velocity = max_velocity * config->scale_velocity_neck;
         max_velocity = fmin(max_velocity, config->limit_velocity_neck);
         // max accel: assuming linear acceleration we have:
         /* v ^  _
-...
         // and therefore
         // a = v_max^2 / d_total
         float max_accel = 0.0;
         if (distance_abs > 0.0) {
             max_accel = pow(max_speed, 2) / distance_abs;
             max_accel = pow(max_velocity, 2) / distance_abs;
+        }
         // smoother motion
         max_accel = max_accel * 0.7;
         // scale and limit acceleration
         max_accel = max_accel * config->scale_acceleration_neck;
         max_accel = fmin(max_accel, config->limit_acceleration_neck);
         // limit maximum acceleration to reduce noise FIXME!
         if (max_accel > 1000) {
             max_accel = 1000;
+        }
         // printf("MAX SPEED %4.2f / max accel %4.2f\n",max_speed, max_accel);
         // feed reflexxes api with data
         reflexxes_set_input(dof, target, current_position, current_velocity,
                             timestamp, max_speed, max_accel);
                             timestamp, max_velocity, max_accel);
+    }

     #include "humotion/server/eye_motion_generator.h"
     using humotion::server::ReflexxesMotionGenerator;
     using humotion::server::Config;
     //! constructor
     ReflexxesMotionGenerator::ReflexxesMotionGenerator(JointInterface *j, int dof, float t) :
         MotionGenerator(j) {
     ReflexxesMotionGenerator::ReflexxesMotionGenerator(JointInterface *j, Config *c, int dof, float t) :
         MotionGenerator(j, c) {
         dof_count = dof;
         // create Reflexxes API for <dof> DOF actuator

humotion

Revision 6d13138a