/src/server/neck_motion_generator.cpp - Diff - humotion - Research for Cognitive Interaction

Revision 0c8d22a5 src/server/neck_motion_generator.cpp

     * Excellence Initiative.
     */
     #include "server/neck_motion_generator.h"
     #include "server/gaze_motion_generator.h"
     #include "server/server.h"
     #include <cmath>
     using namespace std;
     using namespace humotion;
     using namespace humotion::server;
     #include "humotion/server/gaze_motion_generator.h"
     #include "humotion/server/neck_motion_generator.h"
     #include "humotion/server/server.h"
     const float NeckMotionGenerator::CONST_GUITTON87_A = 4.39/2.0; // / 2.0;
     const float NeckMotionGenerator::CONST_GUITTON87_B = 106.0/2.0; // /2.0;
     using humotion::server::NeckMotionGenerator;
     //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
     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
     //! constructor
     NeckMotionGenerator::NeckMotionGenerator(JointInterface *j) : GazeMotionGenerator(j, 3, 1.0/Server::MOTION_UPDATERATE){
     NeckMotionGenerator::NeckMotionGenerator(JointInterface *j) :
         GazeMotionGenerator(j, 3, 1.0/Server::MOTION_UPDATERATE) {
         breath_time = 0.0;
+    }
     //! destructor
     NeckMotionGenerator::~NeckMotionGenerator(){
     NeckMotionGenerator::~NeckMotionGenerator() {
+    }
     //! get a breath offset angle
     //! @return float of breath offset value
     float NeckMotionGenerator::get_breath_offset(){
         //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 NeckMotionGenerator::get_breath_offset() {
         // 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;
         float breath_time_normalized = (breath_time * 3)/CONST_BREATH_PERIOD; //0...1 -> move up, 1..2 -> return, 2..3 -> still
         // 0...1 -> move up, 1..2 -> return, 2..3 -> still
         float breath_time_normalized = (breath_time * 3)/CONST_BREATH_PERIOD;
         if (breath_time_normalized <= 0.5){
             //accelerated motion:
         if (breath_time_normalized <= 0.5) {
             // accelerated motion
             breath_offset = CONST_BREATH_AMPLITUDE * (2.0 * pow(breath_time_normalized, 2));
         }else if (breath_time_normalized <= 1.0){
             //deaccelerate:
         } else if (breath_time_normalized <= 1.0) {
             // deaccelerate
             breath_offset = CONST_BREATH_AMPLITUDE * (1.0 - 2.0 * pow(1.0 - breath_time_normalized, 2));
         }else if (breath_time_normalized <= 1.5){
             //accelerate again:
         } else if (breath_time_normalized <= 1.5) {
             // accelerate again
             breath_offset = CONST_BREATH_AMPLITUDE * (1.0 - (2.0 * pow(breath_time_normalized-1, 2)));
         }else if (breath_time_normalized <= 2.0){
         } else if (breath_time_normalized <= 2.0) {
             breath_offset = CONST_BREATH_AMPLITUDE * (2.0 * pow(2.0 - breath_time_normalized, 2));
         }else if (breath_time_normalized <= 3.0){
             //pause for some time
         } else if (breath_time_normalized <= 3.0) {
             // pause for some time
             breath_offset = 0;
+        }
         //fetch next time
         // fetch next time
         breath_time += 1000.0/Server::MOTION_UPDATERATE;
         if (breath_time >= CONST_BREATH_PERIOD){
         if (breath_time >= CONST_BREATH_PERIOD) {
             breath_time -= CONST_BREATH_PERIOD;
+        }
-...
     //! calculate joint targets
     void NeckMotionGenerator::calculate_targets(){
         //fetch current dataset:
     void NeckMotionGenerator::calculate_targets() {
         // fetch current dataset
         float neck_pan_now, neck_tilt_now, neck_roll_now;
         float neck_pan_speed, neck_tilt_speed, neck_roll_speed;
-...
                                                                 &neck_roll_now,
                                                                 &neck_roll_speed);
         //reached target?
         // reached target?
         float goal_diff   = fabs(get_current_gaze().distance_pt_abs(requested_gaze_state));
         float target_diff = fabs(requested_gaze_state.distance_pt_abs(previous_neck_target));
         //printf("GOAL DIFF = %f TARGET DIFF = %f\n",goal_diff,target_diff);
         //get_current_gaze().dump();
         //requested_gaze_state.dump();
         // printf("GOAL DIFF = %f TARGET DIFF = %f\n",goal_diff,target_diff);
         // get_current_gaze().dump();
         // requested_gaze_state.dump();
         //check if new target
         //close to goal?
         if ( (neck_saccade_active) && (goal_diff < 1.0)){
         // check if new target
         // close to goal?
         if ( (neck_saccade_active) && (goal_diff < 1.0) ) {
             neck_saccade_reached_goal = true;
+        }
         if (neck_saccade_active){
         if (neck_saccade_active) {
             previous_neck_target = requested_gaze_state;
+        }
         //if we get a new target now, we can stop the neck saccade
         if (target_diff > .1){
             if (neck_saccade_reached_goal){
         // if we get a new target now, we can stop the neck saccade
         if (target_diff > .1) {
             if (neck_saccade_reached_goal) {
                 // joint_interface->neck_saccade_done();
                 neck_saccade_active = false;
                 neck_saccade_reached_goal = false;
+            }
+        }
         if (neck_saccade_requested){
         if (neck_saccade_requested) {
             neck_saccade_active = true;
+        }
         //check if this is a small or big saccade:
         if (neck_saccade_active || neck_saccade_omr){
             //full saccade with neck motion -> update neck target
         // check if this is a small or big saccade
         if (neck_saccade_active || neck_saccade_omr) {
             // full saccade with neck motion -> update neck target
             requested_neck_state = requested_gaze_state;
+        }
         //get targets: this is the sum of stored neck target and up-to-date offset:
         // get targets: this is the sum of stored neck target and up-to-date offset:
         float neck_pan_target  = requested_neck_state.pan  + requested_gaze_state.pan_offset;
         float neck_tilt_target = requested_neck_state.tilt + requested_gaze_state.tilt_offset;
         //roll is always equal to requested gaze (not neck) state
         // roll is always equal to requested gaze (not neck) state
         float neck_roll_target = requested_gaze_state.roll + requested_gaze_state.roll_offset;
         //add breath wave to tilt:
         // add breath wave to tilt
         neck_tilt_target += get_breath_offset();
         //pass parameters to reflexxes api:
         // pass parameters to reflexxes api
         setup_neckmotion(0, neck_pan_target,  neck_pan_now,  neck_pan_speed,  neck_pan_ts);
         setup_neckmotion(1, neck_tilt_target, neck_tilt_now, neck_tilt_speed, neck_tilt_ts);
         setup_neckmotion(2, neck_roll_target, neck_roll_now, neck_roll_speed, neck_roll_ts);
         //call reflexxes to handle profile calculation:
         // call reflexxes to handle profile calculation
         reflexxes_calculate_profile();
         //tell the joint if about the new values:
         // tell the joint if about the new values
         joint_interface->set_target(JointInterface::ID_NECK_PAN,
                                     reflexxes_position_output->NewPositionVector->VecData[0],
                                     reflexxes_position_output->NewVelocityVector->VecData[0]);
-...
+    }
     //! publish targets to motor boards:
     void NeckMotionGenerator::publish_targets(){
         //publish values if there is an active gaze input within the last timerange
         if (gaze_target_input_active()){
     void NeckMotionGenerator::publish_targets() {
         // publish values if there is an active gaze input within the last timerange
         if (gaze_target_input_active()) {
             joint_interface->publish_target(JointInterface::ID_NECK_PAN);
             joint_interface->publish_target(JointInterface::ID_NECK_TILT);
             joint_interface->publish_target(JointInterface::ID_NECK_ROLL);
-...
     //! \param target angle
     //! \param current angle
     void NeckMotionGenerator::setup_neckmotion(int dof, float target, float current_position,
                                                float current_speed, humotion::Timestamp timestamp){
         //get distance to target:
                                                float current_velocity, humotion::Timestamp timestamp) {
         // get distance to target
         float distance_abs = fabs(target - current_position);
         //get max speed: according to [guitton87] there is a relation between distance_abs and v_max_head:
         //v_max = 4.39 * d_total + 106.0 (in degrees)
         // get max speed: according to [guitton87] there is a 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);
         //max accel: 	assuming linear acceleration we have
         /* v ^
         // max accel: assuming linear acceleration we have:
         /* v ^  _
         *   |  / \
         *   | /   \
         *   |/_____\___> t
-...
         // d_total = 1/4 * a * 4 * vmax^2 / a^2 = v_max^2 / a
         // d_total = a * 2 * d_total / (v_max^2)
         // and therefore
         //  a = v_max^2 / d_total
         // a = v_max^2 / d_total
         float max_accel = 0.0;
         if (distance_abs > 0.0){
         if (distance_abs > 0.0) {
             max_accel = pow(max_speed, 2) / distance_abs;
+        }
         //smoother motion
         max_accel = max_accel * 0.7; //1.0; //0.7;
         // smoother motion
         max_accel = max_accel * 0.7;
         //limit maximum acceleration to reduce noise FIXME!
         if (max_accel>1000){
         // 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);
         // 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_speed, timestamp, max_speed, max_accel);
         // feed reflexxes api with data
         reflexxes_set_input(dof, target, current_position, current_velocity,
                             timestamp, max_speed, max_accel);
+    }

Also available in: Unified diff