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"

using namespace std;

using namespace humotion;

using namespace humotion::server;

//! constructor

Controller::Controller(JointInterface *j){

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

    }

}

//! update gaze target:

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

void Controller::set_gaze_target(GazeState new_gaze_target){

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

        (*it)->set_gaze_target(new_gaze_target);

    }

}

//! update mouth state:

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

void Controller::set_mouth_target(MouthState s){

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

        (*it)->set_mouth_target(s);

    }

}