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

#include "boost/date_time/posix_time/posix_time.hpp"

using namespace std;

using namespace humotion;

using namespace humotion::server;

using namespace boost;

//! constructor

MotionGenerator::MotionGenerator(JointInterface *j){

    joint_interface = j;

    last_mouth_target_update = posix_time::ptime(posix_time::min_date_time);

    last_gaze_target_update  = posix_time::ptime(posix_time::min_date_time);

}

//! destructor

MotionGenerator::~MotionGenerator(){

}

//! fetch the latest position and velocity and return the timestamp of that dataset

//! \param joint id

//! \param pointer to position variable

//! \param pointer to veolocity variable

//! \return Timestamp of this dataset

humotion::Timestamp MotionGenerator::get_timestamped_state(int joint_id,

                                                           float *position, float *velocity) {

    humotion::Timestamp stamp = joint_interface->get_ts_position(joint_id).get_last_timestamp();

    *position = joint_interface->get_ts_position(joint_id).get_interpolated_value(stamp);

    *velocity = joint_interface->get_ts_speed(joint_id).get_interpolated_value(stamp);

    return stamp;

}

//! fetch the latest (=current) speed of a joint

//! \param joint_id

//! \return float value of joint speed

float MotionGenerator::get_current_speed(int joint_id){

    return joint_interface->get_ts_speed(joint_id).get_newest_value();

}

//! fetch the latest (=current) position of a joint

//! \param joint_id

//! \return float value of joint position

float MotionGenerator::get_current_position(int joint_id){

    /*Timestamp tsl = joint_interface->get_ts_position(joint_id).get_last_timestamp();

    Timestamp now = Timestamp::now();

    Timestamp diff=now-tsl;

    printf("TIME DIFF %fs %fns\n",diff.sec, diff.nsec);*/

    return joint_interface->get_ts_position(joint_id).get_newest_value();

}

//! update gaze target:

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

void MotionGenerator::set_gaze_target(GazeState new_gaze_target){

    //store value for next iteration

    requested_gaze_state        = new_gaze_target;

    //keep track if the gaze targets are comming in regulary

    last_gaze_target_update = get_system_time();

}

//! update mouth state:

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

void MotionGenerator::set_mouth_target(MouthState s){

    //store value

    requested_mouth_target = s;

    //keep track if the mouth targets are comming in regulary

    last_mouth_target_update = get_system_time();

}

//! was there incoming gaze data the last second?

//! \return true if there was data incoming in the last second, false otherwise

bool MotionGenerator::gaze_target_input_active(){

    if (last_gaze_target_update  + posix_time::milliseconds(1000) > get_system_time() ){

        //incoming data -> if gaze is disabled, enable it!

        joint_interface->enable_gaze_joints();

        return true;

    }else{

        //no incoming data, disable!

        joint_interface->disable_gaze_joints();

        return false;

    }

}

//! was there incoming mouth data the last second?

//! \return true if there was data incoming in the last second, false otherwise

bool MotionGenerator::mouth_target_input_active(){

    if (last_mouth_target_update + posix_time::milliseconds(1000) > get_system_time() ){

        //incoming data -> if mouth is disabled, enable it!

        joint_interface->enable_mouth_joints();

        return true;

    }else{

        //no incoming data, disable!

        joint_interface->disable_mouth_joints();

        return false;

    }

}

//! limit target to min/max bounds:

float MotionGenerator::limit_target(int joint_id, float val){

    assert(joint_id < JointInterface::JOINT_ID_ENUM_SIZE);

    //fetch min/max for joint:

    float min = joint_interface->get_joint_min(joint_id);

    float max = joint_interface->get_joint_max(joint_id);

    if (max < min){

        printf("> ERROR: how can min (%4.2f) be bigger than max (%4.2f)?? FIX THIS! EXITING NOW\n",min,max);

        exit(0);

    }

    val = fmin(val, max);

    val = fmax(val, min);

    return val;

}