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/controller.h"

#include "server/eye_motion_generator.h"

#include "server/eyelid_motion_generator.h"

#include "server/eyebrow_motion_generator.h"

#include "server/neck_motion_generator.h"

#include "server/mouth_motion_generator.h"

#include "timestamp.h"

using namespace std;

using namespace humotion;

using namespace humotion::server;

//! constructor

Controller::Controller(JointInterface *j) : activated(false) {

    joint_interface = j;

}

//! destructor

Controller::~Controller(){

}

//! initialise motion generators

void Controller::init_motion_generators(){

    //NOTE: the order of these generators is important!

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

    //1) eye motion generation:

    add_motion_generator(new EyeMotionGenerator(joint_interface));

    //2) eyelid motion generator

    add_motion_generator(new EyelidMotionGenerator(joint_interface));

    //3) neck motion generator

    add_motion_generator(new NeckMotionGenerator(joint_interface));

    //4) mouth motion generator

    add_motion_generator(new MouthMotionGenerator(joint_interface));

    //5) eyebrow motion generator

    add_motion_generator(new EyebrowMotionGenerator(joint_interface));

}

//! add a single motion genrator

void Controller::add_motion_generator(MotionGenerator *m){

    motion_generator_vector.push_back(m);

}

//! calculate target angles for all motion generators:

void Controller::calculate_targets(){

    for(motion_generator_vector_t::iterator it = motion_generator_vector.begin(); it<motion_generator_vector.end(); it++){

        (*it)->calculate_targets();

    }

}

//! publish all target angles to the devices:

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

void Controller::publish_targets(){

    for(motion_generator_vector_t::iterator it = motion_generator_vector.begin(); it<motion_generator_vector.end(); it++){

        (*it)->publish_targets();

    }

}

GazeState Controller::relative_gaze_to_absolute_gaze(GazeState relative){

    double pan, tilt, roll;

    GazeState absolute_gaze = relative;

    //incoming gaze state wants to set a relative gaze angle

    //in order to calc the new absolute gaze, we need to go back

    //in time and find out where the head was pointing at that specific time:

    Timestamp relative_target_timestamp = relative.timestamp;

    //check if this timestamp allows a valid conversion:

    Timestamp history_begin = joint_interface->get_ts_position(JointInterface::ID_NECK_PAN).get_first_timestamp();

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

    //printf("> incoming: %f, history is %f to %f\n",relative_target_timestamp.to_seconds(), history_begin.to_seconds(), history_end.to_seconds());

    //our history keeps the last n=100 elements in a timestamped list

    if ((relative_target_timestamp < history_begin) || (history_begin.is_null())){

        //when the incoming data is older than that it makes no sense

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

        if (last_known_absolute_timestamp != relative_target_timestamp){

            printf("> WARNING: restored/guessed absolute target for unknown timestamp %f [this should not happen]\n", relative_target_timestamp.to_seconds());

            last_known_absolute_target_pan = 0.0;

            last_known_absolute_target_tilt = 0.0;

            last_known_absolute_target_roll = 0.0;

        }

    }else{

        //all fine, we can reconstruct the absolute target:

        //fetch head / camera pose during that timestamp:

        double neck_pan  = joint_interface->get_ts_position(JointInterface::ID_NECK_PAN).get_interpolated_value(relative_target_timestamp);

        double eye_l_pan = joint_interface->get_ts_position(JointInterface::ID_EYES_LEFT_LR).get_interpolated_value(relative_target_timestamp);

        double eye_r_pan = joint_interface->get_ts_position(JointInterface::ID_EYES_RIGHT_LR).get_interpolated_value(relative_target_timestamp);

        last_known_absolute_target_pan       = neck_pan + (eye_l_pan + eye_r_pan)/2.0;

        //

        double neck_tilt = joint_interface->get_ts_position(JointInterface::ID_NECK_TILT).get_interpolated_value(relative_target_timestamp);

        double eye_tilt  = joint_interface->get_ts_position(JointInterface::ID_EYES_BOTH_UD).get_interpolated_value(relative_target_timestamp);

        last_known_absolute_target_tilt      = neck_tilt + eye_tilt;

        //

        last_known_absolute_target_roll      = joint_interface->get_ts_position(JointInterface::ID_NECK_ROLL).get_interpolated_value(relative_target_timestamp);

        //safe this timestamp as known:

        last_known_absolute_timestamp = relative_target_timestamp;

    }

    pan  = last_known_absolute_target_pan;

    tilt = last_known_absolute_target_tilt;

    roll = last_known_absolute_target_roll;

    //substract offsets:

    pan  -= relative.pan_offset;

    tilt -= relative.tilt_offset;

    roll -= relative.roll_offset;

    //build up absolute target:

    absolute_gaze.gaze_type  = GazeState::GAZETYPE_ABSOLUTE;

    absolute_gaze.pan   = pan + relative.pan;

    absolute_gaze.tilt  = tilt + relative.tilt;

    absolute_gaze.roll  = roll + relative.roll;

    printf("pan  now = %4.1f, rel=%4.1f ===> %4.2f\n",pan,relative.pan,absolute_gaze.pan);

    printf("tilt now = %4.1f, rel=%4.1f ===> %4.2f\n",tilt,relative.tilt,absolute_gaze.tilt);

    //FIXME: use ros TF for that calculation...

    //see http://wiki.ros.org/tf/Tutorials/Time%20travel%20with%20tf%20%28C%2B%2B%29

    //ros::Time past = now - ros::Duration(5.0);

    //listener.waitForTransform("/turtle2", now,J "/turtle1", past, "/world", ros::Duration(1.0));

    //listener.lookupTransform("/turtle2", now, "/turtle1", past, "/world", transform);

    //absolute_gaze.dump();

    return absolute_gaze;

}

//! activate controller

void Controller::set_activated(void){

    activated = true;

}

//! update gaze target:

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

void Controller::set_gaze_target(GazeState new_gaze_target){

    if (!activated){

        //not yet initialized, ignore incoming targets

        return;

    }

    GazeState target_gaze;

    //new_gaze_target.dump();

    //relative or absolute gaze update?

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

        //relative gaze target -> calculate target angles

        target_gaze = relative_gaze_to_absolute_gaze(new_gaze_target);

    }else{

        //already absolute gaze, set this

        target_gaze = new_gaze_target;

    }

    for(motion_generator_vector_t::iterator it = motion_generator_vector.begin(); it<motion_generator_vector.end(); 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){

    if (!activated){

        //not yet initialized, ignore incoming targets

        return;

    }

    for(motion_generator_vector_t::iterator it = motion_generator_vector.begin(); it<motion_generator_vector.end(); it++){

        (*it)->set_mouth_target(s);

    }

}