humotion

/*

* 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 "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;

const float NeckMotionGenerator::CONST_GUITTON87_A = 4.39/2.0; // / 2.0;

const float NeckMotionGenerator::CONST_GUITTON87_B = 106.0/2.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){

    breath_time = 0.0;

}

//! destructor

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 breath_offset = 0.0;

    float breath_time_normalized = (breath_time * 3)/CONST_BREATH_PERIOD; //0...1 -> move up, 1..2 -> return, 2..3 -> still

    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:

        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:

        breath_offset = CONST_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));

    }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;

    }

    return breath_offset;

}

//! calculate joint targets

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;

    humotion::Timestamp neck_pan_ts = get_timestamped_state(JointInterface::ID_NECK_PAN,

                                                            &neck_pan_now,

                                                            &neck_pan_speed);

    humotion::Timestamp neck_tilt_ts = get_timestamped_state(JointInterface::ID_NECK_TILT,

                                                            &neck_tilt_now,

                                                            &neck_tilt_speed);

    humotion::Timestamp neck_roll_ts = get_timestamped_state(JointInterface::ID_NECK_ROLL,

                                                            &neck_roll_now,

                                                            &neck_roll_speed);

    //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();

    //check if new target

    //close to goal?

    if ( (neck_saccade_active) && (goal_diff < 1.0)){

        neck_saccade_reached_goal = true;

    }

    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){

            // joint_interface->neck_saccade_done();

            neck_saccade_active = false;

            neck_saccade_reached_goal = false;

        }

    }

    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

        requested_neck_state = requested_gaze_state;

    }

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

    float neck_roll_target = requested_gaze_state.roll + requested_gaze_state.roll_offset;

    //add breath wave to tilt:

    neck_tilt_target += get_breath_offset();

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

    reflexxes_calculate_profile();

    //tell the joint if about the new values:

    joint_interface->set_target_position(JointInterface::ID_NECK_PAN,

                                         reflexxes_position_output->NewPositionVector->VecData[0],

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

    joint_interface->set_target_position(JointInterface::ID_NECK_TILT,

                                         reflexxes_position_output->NewPositionVector->VecData[1],

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

    joint_interface->set_target_position(JointInterface::ID_NECK_ROLL,

                                         reflexxes_position_output->NewPositionVector->VecData[2],

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

    printf("\n%f %f %f %f %f DBG\n",

            neck_pan_now, neck_pan_target,

            reflexxes_position_output->NewPositionVector->VecData[0],

            joint_interface->get_ts_speed(JointInterface::ID_NECK_PAN).get_newest_value(),

            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()){

        joint_interface->publish_target_position(JointInterface::ID_NECK_PAN);

        joint_interface->publish_target_position(JointInterface::ID_NECK_TILT);

        joint_interface->publish_target_position(JointInterface::ID_NECK_ROLL);

    }

}

//! set up neck motion profile

//! this will use speed and acceleration calc formulas from literature:

//! \param dof id of joint

//! \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 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)

    float max_speed = (CONST_GUITTON87_A * distance_abs + CONST_GUITTON87_B);

    //max accel:         assuming linear acceleration we have

    /* v ^

    *   |  / \

    *   | /   \

    *   |/_____\___> t

    */

    // d_total = 2 * 1/2 * a * (t_total/2)^2 = 1/4 * a * t_total^2

    // as we use linear accel we have

    // v_max = a * t_total/2  --> t_total = 2*v_max / a

    // combine both

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

    float max_accel = 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;

    //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_speed, timestamp, max_speed, max_accel);

}