/ - Diff - humotion - Research for Cognitive Interaction

Revision 35b3ca25

     class iCubDataReceiver : public yarp::os::RateThread{
     public:
         iCubDataReceiver(int period, yarp::dev::IEncodersTimed *_iencs, iCubJointInterface *_icub_jointinterface);
         iCubDataReceiver(int period, iCubJointInterface *icub_jointinterface);
         bool threadInit();
         void threadRelease();
         void run();
     private:
         double get_timestamp_ms();
         void store_incoming_position(int icub_id, double value, double timestamp);
         yarp::sig::Vector positions;
         yarp::sig::Vector velocities;
         yarp::sig::Vector commands;
         yarp::sig::Vector timestamps;
         float target_eye_pan_;
         float target_eye_vergence_;
         yarp::dev::IEncodersTimed *iencs;
         iCubJointInterface *icub_jointinterface;
         yarp::sig::Vector yarp_positions_;
         yarp::sig::Vector yarp_commands_;
         yarp::sig::Vector yarp_timestamps_;
         iCubJointInterface *icub_jointinterface_;
         yarp::dev::IEncodersTimed *yarp_iencs_;
     };

         ~iCubFaceInterface();
         void set_eyelid_angle(double angle);
         void set_eyebrow_angle(int id, double *target_angle);
         void set_mouth(double *target_angle);
         void set_eyelid_angle(float angle);
         void set_eyebrow_angle(int id, float *target_angle);
         void set_mouth(float *target_angle);
     private:
         double lid_angle;

         iCubJointInterface(std::string scope);
         ~iCubJointInterface();
         //void fetch_position(Device *dev, double timestamp);
         //void fetch_speed(Device *dev, double timestamp);
         void fetch_position(int id, double value, double timestamp);
         void fetch_speed(int id, double value, double timestamp);
         void store_incoming_position(int id, double value, double timestamp);
         void run();
         int convert_icub_jointid_to_humotion(int id);
         int convert_humotion_jointid_to_icub(int id);
         enum JOINT_ID_ENUM{
             ICUB_ID_NECK_TILT = 0,
             ICUB_ID_NECK_ROLL = 1,
-...
             ICUB_ID_EYES_BOTH_UD = 3,
             ICUB_ID_EYES_PAN = 4,
             ICUB_ID_EYES_VERGENCE = 5,
             //the following ids are used for remapping:
             //ICUB_ID_EYES_LEFT_LR,
             //ICUB_ID_EYES_RIGHT_LR,
             ICUB_ID_EYES_LEFT_LID_LOWER,
             ICUB_ID_EYES_LEFT_LID_UPPER,
             ICUB_ID_EYES_RIGHT_LID_LOWER,
-...
             ICUB_JOINT_ID_ENUM_SIZE
         };
         yarp::dev::PolyDriver *get_yarp_polydriver() { return &yarp_polydriver_; }
         static const int MAIN_LOOP_FREQUENCY = 50;
     protected:
         void disable_joint(int e);
         void publish_target_position(int e);
         void publish_target(int e, float position, float velocity);
         void enable_joint(int e);
         void execute_motion();
     private:
         yarp::dev::PolyDriver yarp_polydriver_;
         std::vector<double> pv_mix_last_error_;
         std::vector<double> pv_mix_pid_p_;
         std::vector<double> pv_mix_pid_d_;
         // yarp views
         yarp::dev::IVelocityControl *yarp_ivel_;
         yarp::dev::IPositionControl *yarp_ipos_;
         //yarp::dev::IEncodersTimed *yarp_iencs_;
         yarp::dev::IControlLimits *yarp_ilimits_;
         yarp::dev::IAmplifierControl *yarp_amp_;
         yarp::dev::IPidControl *yarp_pid_;
         yarp::dev::IControlMode *yarp_icontrol_;
         yarp::sig::Vector yarp_commands_;
         float target_angle_[ICUB_JOINT_ID_ENUM_SIZE];
         float target_velocity_[ICUB_JOINT_ID_ENUM_SIZE];
         void init_controller();
         void init_id_map();
         void init_pv_mix_pid();
         void insert_icupid_to_humotionid_mapping(int icubid, int humotionid);
         void store_icub_joint_target(int icub_id, float position, float velocity);
         //*******************************************
         void set_joint_enable_state(int e, bool enabled);
         double get_timestamp_ms();
         double target_angle[ICUB_JOINT_ID_ENUM_SIZE];
         double target_angle_previous[ICUB_JOINT_ID_ENUM_SIZE];
         //double target_angle[ICUB_JOINT_ID_ENUM_SIZE];
         //double target_angle_previous[ICUB_JOINT_ID_ENUM_SIZE];
         iCubDataReceiver *icub_data_receiver;
         void init_joints();
         std::string scope;
         yarp::dev::PolyDriver dd;
         yarp::sig::Vector positions;
         yarp::sig::Vector velocities;
         yarp::sig::Vector commands;
         yarp::dev::IVelocityControl *ivel;
         yarp::dev::IPositionControl *ipos;
         yarp::dev::IEncodersTimed *iencs;
         yarp::dev::IControlLimits *ilimits;
         yarp::dev::IAmplifierControl *amp;
         yarp::dev::IPidControl *pid;
         yarp::dev::IControlMode *icontrol;
         std::vector<double> last_position_error;
         std::vector<double> PID_P;
         std::vector<double> PID_D;
         void store_min_max(yarp::dev::IControlLimits *ilimits, int id, int e);
-...
         int convert_enum_to_motorid(int e);
         int convert_motorid_to_enum(int id);
         iCubFaceInterface *face_interface;
         iCubFaceInterface *face_interface_;
         typedef boost::bimap<int, int > enum_id_bimap_t;
         typedef enum_id_bimap_t::value_type enum_id_bimap_entry_t;

     #include "icub_data_receiver.h"
     #include <humotion/server/joint_interface.h>
     #include <yarp/os/Property.h>
     using namespace yarp::dev;
     using namespace yarp::sig;
     using namespace yarp::os;
     using namespace std;
     iCubDataReceiver::iCubDataReceiver(int period, IEncodersTimed *_iencs, iCubJointInterface *_icub_jointinterface):RateThread(period){
         iencs = _iencs;
         icub_jointinterface = _icub_jointinterface;
     //using namespace yarp::dev;
     //using namespace yarp::sig;
     //using namespace yarp::os;
     using std::cout;
     using std::string;
     using humotion::server::JointInterface;
     using yarp::dev::IEncodersTimed;
     using yarp::sig::Vector;
     iCubDataReceiver::iCubDataReceiver(int period, iCubJointInterface *icub_jointinterface)
         : yarp::os::RateThread(period) {
         // store pointer to icub jointinterface
         icub_jointinterface_ = icub_jointinterface;
         //fetch yarp iencs view:
         yarp::dev::PolyDriver *poly_driver = icub_jointinterface->get_yarp_polydriver();
         bool success = poly_driver->view(yarp_iencs_);
         if (!success) {
             cout << "ERROR: polydriver failed to init iencs view\n";
             exit(EXIT_FAILURE);
+        }
         // resize data storage vectors to match the number of axes:
         int joints;
         iencs->getAxes(&joints);
         positions.resize(joints);
         velocities.resize(joints);
         timestamps.resize(joints);
         yarp_iencs_->getAxes(&joints);
         yarp_positions_.resize(joints);
         yarp_timestamps_.resize(joints);
+    }
     bool iCubDataReceiver::threadInit(){ return true; }
     void iCubDataReceiver::threadRelease(){ }
     double iCubDataReceiver::get_timestamp_ms(){
         struct timespec spec;
         clock_gettime(CLOCK_REALTIME, &spec);
         return spec.tv_sec+spec.tv_nsec/1000000000.0;
     bool iCubDataReceiver::threadInit() {
         return true;
+    }
     void iCubDataReceiver::run(){
     void iCubDataReceiver::threadRelease() {
+    }
     void iCubDataReceiver::run() {
         //grab pos+vel data:
         iencs->getEncodersTimed(positions.data(), timestamps.data());
         iencs->getEncoderSpeeds(velocities.data());
         //double timestamp = get_timestamp_ms();
         yarp_iencs_->getEncodersTimed(yarp_positions_.data(), yarp_timestamps_.data());
         //iencs->getEncoderSpeeds(velocities.data());
         //publish data to humotion
         for(int i=0; i<positions.size(); i++){
             icub_jointinterface->fetch_position(i, positions[i], timestamps[i]);
             icub_jointinterface->fetch_speed(i, velocities[i], timestamps[i]);
             if (i==iCubJointInterface::ICUB_ID_NECK_PAN){
                 printf("\nMMM p=%f v=%f\n",positions[i],velocities[i]);
+            }
         for(int i=0; i<yarp_positions_.size(); i++){
             store_incoming_position(i, yarp_positions_[i], yarp_timestamps_[i]);
+        }
         //printf("\n%f %f %f TIME\n", get_timestamp_ms(), timestamps[2], positions[2]);
         //printf("TIMEDIFF %f\n", get_timestamp_ms() - timestamps[2]);
         //tell humotion to update lid angle (hack)
         icub_jointinterface->fetch_position(100, 0.0, get_timestamp_ms());
         //small hack to tell humotion to update the lid angle
         //fixme: use real id
         store_incoming_position(100, 0.0, yarp_timestamps_[0]);
+    }
     void iCubDataReceiver::store_incoming_position(int icub_id, double value, double timestamp) {
         // store joint position in humotion backend
         if ((icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) ||
                 (icub_id == iCubJointInterface::ICUB_ID_EYES_VERGENCE)) {
             // the icub handles eyes differently
             // instead of using seperate left/right pan the icub uses
             // a combined pan angle and vergence. therfore we have to convert this here:
             if (icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) {
                 target_eye_pan_ = value;
             } else {
                 target_eye_vergence_ = value;
+            }
             float left  = target_eye_pan_ + target_eye_vergence_/2.0;
             float right = target_eye_pan_ - target_eye_vergence_/2.0;
             icub_jointinterface_->store_incoming_position(JointInterface::ID_EYES_LEFT_LR,
                                                          left, timestamp);
             icub_jointinterface_->store_incoming_position(JointInterface::ID_EYES_RIGHT_LR,
                                                          right, timestamp);
         } else if (icub_id == 100) {
             //HACK
             //icub_jointinterface->store_incoming_position(ID_EYES_RIGHT_LID_UPPER,
             //                                             lid_angle, timestamp);
         } else {
             // known configured mapping between joint ids
             int humotion_id = icub_jointinterface_->convert_icub_jointid_to_humotion(icub_id);
             icub_jointinterface_->store_incoming_position(humotion_id, value, timestamp);
+        }
+    }

     //! special command to set eyelid angle
     //! \param angle in degrees
     void iCubFaceInterface::set_eyelid_angle(double angle){
     void iCubFaceInterface::set_eyelid_angle(float angle){
         if (emotion_port[0].getOutputCount()>0){
             //try to set the value based on the upper one
             //some guesses from the sim: S30 = 0° / S40 = 10°
-...
     //! special command to set the eyebrow angle
     //! \param id {0=left, 1=right)
     //! \param angle in degrees
     void iCubFaceInterface::set_eyebrow_angle(int id, double *target_angle){
     void iCubFaceInterface::set_eyebrow_angle(int id, float *target_angle){
         int port_id;
         if (id == iCubJointInterface::ICUB_ID_EYES_LEFT_BROW){
             port_id = 1;
-...
+        }
+    }
     void iCubFaceInterface::set_mouth(double *target_angle){
     void iCubFaceInterface::set_mouth(float *target_angle){
         //convert from 6DOF mouth displacement to icub leds:
         int led_value = 0;

     http://wiki.icub.org/wiki/Motor_control
     */
     //WARNING: DO NOT CHANGE THIS; VELOCITYMODE IS NOT YET IMPLEMENTED
     #define POSITION_CONTROL 0
     using std::cout;
     //! constructor
     iCubJointInterface::iCubJointInterface(string _scope) : humotion::server::JointInterface(){
         scope = _scope;
         face_interface = new iCubFaceInterface(scope);
         //add mapping from ids to enums:
         //this might look strange at the first sight but we need to have a generic
         //way to acces joints from libhumotion. therefore the lib uses its enum with ID_* enum ids
         //to access the joints. now we need to define a mapping to map those to our motor ids.
         //this is what we use the enum bimap for (convertion fro/to motorid is handled
         //by \sa convert_enum_to_motorid() and \sa convert_motorid_to_enum() lateron
         //MOUTH
         enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_LEFT_UPPER,   ID_LIP_LEFT_UPPER));
         enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_LEFT_LOWER,   ID_LIP_LEFT_LOWER));
         enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_CENTER_UPPER, ID_LIP_CENTER_UPPER));
         enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_CENTER_LOWER, ID_LIP_CENTER_LOWER));
         enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_RIGHT_UPPER,  ID_LIP_RIGHT_UPPER));
         enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_RIGHT_LOWER,  ID_LIP_RIGHT_LOWER));
         //NECK
         enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_NECK_PAN,    ID_NECK_PAN));
         enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_NECK_TILT,   ID_NECK_TILT));
         enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_NECK_ROLL,   ID_NECK_ROLL));
         //EYES
         enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_PAN,   ID_EYES_LEFT_LR));
         enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_VERGENCE,   ID_EYES_RIGHT_LR));
         enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_BOTH_UD,   ID_EYES_BOTH_UD));
         //EYELIDS
         enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_LID_LOWER, ID_EYES_LEFT_LID_LOWER));
         enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_LID_UPPER, ID_EYES_LEFT_LID_UPPER));
         enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_BROW, ID_EYES_LEFT_BROW));
         enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_LID_LOWER, ID_EYES_RIGHT_LID_LOWER));
         enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_LID_UPPER,ID_EYES_RIGHT_LID_UPPER));
         enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_BROW, ID_EYES_RIGHT_BROW));
         //init pd control variables
         enum_id_bimap_t::const_iterator it;
         last_position_error.resize(ICUB_JOINT_ID_ENUM_SIZE);
         PID_P.resize(ICUB_JOINT_ID_ENUM_SIZE);
         PID_D.resize(ICUB_JOINT_ID_ENUM_SIZE);
         for(it = enum_id_bimap.begin(); it != enum_id_bimap.end(); ++it) {
             int id = it->left;
             last_position_error[id] = 0.0;
             PID_P[id] = 4.5;
             PID_D[id] = 0.3;
+        }
         // add mappings from icub ids to humotion ids
         init_id_map();
         // initialise the pd controller for the velocity and position mixer
         init_pv_mix_pid();
         // intantiate the face interface
         face_interface_ = new iCubFaceInterface(scope);
         // intantiate the polydriver
         Property options;
         options.put("device", "remote_controlboard");
         options.put("local", "/local/head");
         options.put("remote", scope+"/head");
         dd.open(options);
         //fetch views:
         dd.view(iencs);
         dd.view(ipos);
         dd.view(ivel);
         dd.view(ilimits);
         dd.view(pid);
         dd.view(amp);
         if ( (!iencs) || (!ipos) || (!ilimits) || (!ivel) || (!amp) || (!pid)){
             printf("> ERROR: failed to open icub views\n");
         options.put("remote", scope + "/head");
         yarp_polydriver_.open(options);
         // fetch yarp views:
         bool success = true;
         //success &= yarp_polydriver_.view(yarp_iencs_);
         success &= yarp_polydriver_.view(yarp_ipos_);
         success &= yarp_polydriver_.view(yarp_ivel_);
         success &= yarp_polydriver_.view(yarp_ilimits_);
         success &= yarp_polydriver_.view(yarp_pid_);
         success &= yarp_polydriver_.view(yarp_amp_);
         if (!success) {
             cout << "ERROR: failed to fetch one or more yarp views... exiting\n";
             exit(EXIT_FAILURE);
+        }
         int joints;
         //tell humotion about min/max joint values:
         init_joints();
         iencs->getAxes(&joints);
         positions.resize(joints);
         velocities.resize(joints);
         commands.resize(joints);
         //initialise joint controller
         init_controller();
+    }
     //! destructor
     iCubJointInterface::~iCubJointInterface(){
+    }
     //! init the controller that allows to write target angles or velocities
     void iCubJointInterface::init_controller() {
         int number_of_joints;
         //set position mode:
         if (POSITION_CONTROL){
             commands=200000.0;
             ipos->setRefAccelerations(commands.data());
             ipos->setPositionMode();
             // use position controller, first fetch no of axes:
             yarp_ipos_->getAxes(&number_of_joints);
             // set ref acceleration to a value for all axes:
             yarp_commands_.resize(number_of_joints);
             yarp_commands_ = 200000.0;
             yarp_ipos_->setRefAccelerations(yarp_commands_.data());
             yarp_ipos_->setPositionMode();
         }else{
             ivel->setVelocityMode();
             commands=300.0;
             ivel->setRefAccelerations(commands.data());
             // use position controller, first fetch no of axes:
             yarp_ivel_->getAxes(&number_of_joints);
             // set ref acceleration to a value for all axes:
             yarp_commands_.resize(number_of_joints);
             yarp_commands_=300.0;
             yarp_ivel_->setRefAccelerations(yarp_commands_.data());
             yarp_ivel_->setVelocityMode();
+        }
+    }
     //! destructor
     iCubJointInterface::~iCubJointInterface(){
+    }
     //! initialise icub joint id to humotion joint id mappings
     void iCubJointInterface::init_id_map() {
             insert_icupid_to_humotionid_mapping(ICUB_ID_LIP_LEFT_UPPER,   ID_LIP_LEFT_UPPER);
             insert_icupid_to_humotionid_mapping(ICUB_ID_LIP_LEFT_LOWER,   ID_LIP_LEFT_LOWER);
             insert_icupid_to_humotionid_mapping(ICUB_ID_LIP_CENTER_UPPER, ID_LIP_CENTER_UPPER);
             insert_icupid_to_humotionid_mapping(ICUB_ID_LIP_CENTER_LOWER, ID_LIP_CENTER_LOWER);
             insert_icupid_to_humotionid_mapping(ICUB_ID_LIP_RIGHT_UPPER,  ID_LIP_RIGHT_UPPER);
             insert_icupid_to_humotionid_mapping(ICUB_ID_LIP_RIGHT_LOWER,  ID_LIP_RIGHT_LOWER);
             insert_icupid_to_humotionid_mapping(ICUB_ID_NECK_PAN,    ID_NECK_PAN);
             insert_icupid_to_humotionid_mapping(ICUB_ID_NECK_TILT,   ID_NECK_TILT);
             insert_icupid_to_humotionid_mapping(ICUB_ID_NECK_ROLL,   ID_NECK_ROLL);
             // FIXME: remove this hack tha repurposes LEFT/RIGHT eye pan from humotion as vergence/pan
             insert_icupid_to_humotionid_mapping(ICUB_ID_EYES_PAN,   ID_EYES_LEFT_LR);
             insert_icupid_to_humotionid_mapping(ICUB_ID_EYES_VERGENCE,   ID_EYES_RIGHT_LR);
             insert_icupid_to_humotionid_mapping(ICUB_ID_EYES_BOTH_UD,   ID_EYES_BOTH_UD);
             insert_icupid_to_humotionid_mapping(ICUB_ID_EYES_LEFT_LID_LOWER, ID_EYES_LEFT_LID_LOWER);
             insert_icupid_to_humotionid_mapping(ICUB_ID_EYES_LEFT_LID_UPPER, ID_EYES_LEFT_LID_UPPER);
             insert_icupid_to_humotionid_mapping(ICUB_ID_EYES_LEFT_BROW, ID_EYES_LEFT_BROW);
             insert_icupid_to_humotionid_mapping(ICUB_ID_EYES_RIGHT_LID_LOWER, ID_EYES_RIGHT_LID_LOWER);
             insert_icupid_to_humotionid_mapping(ICUB_ID_EYES_RIGHT_LID_UPPER,ID_EYES_RIGHT_LID_UPPER);
             insert_icupid_to_humotionid_mapping(ICUB_ID_EYES_RIGHT_BROW, ID_EYES_RIGHT_BROW);
+    }
     //! initialize the position and velocity mixer PD controller
     void iCubJointInterface::init_pv_mix_pid() {
         // init control variables and last error variable for the internal
         // position and velocity mixer PD controller:
         pv_mix_pid_p_.resize(ICUB_JOINT_ID_ENUM_SIZE);
         pv_mix_pid_d_.resize(ICUB_JOINT_ID_ENUM_SIZE);
         pv_mix_last_error_.resize(ICUB_JOINT_ID_ENUM_SIZE);
     //! conversion table for humotion motor ids to our ids:
     //! \param enum from JointInterface::JOINT_ID_ENUM
     //! \return int value of motor id
     int iCubJointInterface::convert_enum_to_motorid(int e){
         enum_id_bimap_t::right_const_iterator it = enum_id_bimap.right.find(e);
         if(it == enum_id_bimap.right.end()) {
             //key does not exists, we are not interested in that dataset, return -1
             return -1;
         enum_id_bimap_t::const_iterator it;
         for(it = enum_id_bimap.begin(); it != enum_id_bimap.end(); ++it) {
             int id = it->left;
             pv_mix_pid_p_[id] = 4.5;
             pv_mix_pid_d_[id] = 0.3;
             pv_mix_last_error_[id] = 0.0;
+        }
         return it->second;
+    }
+    }
     //! conversion table for our ids to humotion motor ids:
     //! \param  int value of motor id
     //! \return enum from JointInterface::JOINT_ID_ENUM
     int iCubJointInterface::convert_motorid_to_enum(int id){
         enum_id_bimap_t::left_const_iterator it = enum_id_bimap.left.find(id);
         if(it == enum_id_bimap.left.end()) {
             //key does not exists, we are not interested in that dataset, return -1
             return -1;
+        }
         return it->second;
     //! add mapping from icub joint ids to humotion ids
     //! this might look strange at the first sight but we need to have a generic
     //! way to acces joints from libhumotion. therefore the lib uses its enum with ID_* enum ids
     //! to access the joints. now we need to define a mapping to map those to the icub motor ids.
     void iCubJointInterface::insert_icupid_to_humotionid_mapping(int icubid, int humotionid) {
         enum_id_bimap.insert(enum_id_bimap_entry_t(icubid, humotionid));
+    }
     void iCubJointInterface::run(){
         iCubDataReceiver *data_receiver = new iCubDataReceiver(0.5 * 1000.0 / MAIN_LOOP_FREQUENCY, iencs, this);
         float loop_duration_ms = 1000.0 / MAIN_LOOP_FREQUENCY;
         iCubDataReceiver *data_receiver = new iCubDataReceiver(loop_duration_ms, this);
         data_receiver->start();
+    }
     //! set the target position of a joint
     //! stores the target position & velocity of a given joint
     //! \param enum id of joint
     //! \param float value
     void iCubJointInterface::publish_target_position(int e){
         //first: convert humotion enum to our enum:
         int id = convert_enum_to_motorid(e);
         if (id == -1){
             return; //we are not interested in that data, so we just return here
+        }
         if (id == ICUB_ID_NECK_PAN){
             //PAN seems to be swapped
             store_joint(ICUB_ID_NECK_PAN, -joint_target[e]);
         }else if ((id == ICUB_ID_EYES_PAN) || ( id == ICUB_ID_EYES_VERGENCE)){
             //icub handles eyes differently, we have to set pan angle + vergence
             float pan      = (joint_target[ID_EYES_LEFT_LR] + joint_target[ID_EYES_RIGHT_LR]) / 2;
             float vergence = (joint_target[ID_EYES_LEFT_LR]  - joint_target[ID_EYES_RIGHT_LR]);
             //printf("EYEDBG %3.2f %3.2f --_> pan %3.2f verg=%3.2f\n",joint_target[ID_EYES_LEFT_LR], joint_target[ID_EYES_RIGHT_LR],pan,vergence);
             store_joint(ICUB_ID_EYES_PAN, pan);
             store_joint(ICUB_ID_EYES_VERGENCE, vergence);
     void iCubJointInterface::publish_target(int humotion_id, float position, float velocity){
         // special handler for eye joints
         if ((humotion_id == JointInterface::ID_EYES_LEFT_LR) ||
             (humotion_id == JointInterface::ID_EYES_RIGHT_LR)){
             // the icub has a combined pan angle for both eyes, so seperate this:
             float target_position_left  = get_target_position(JointInterface::ID_EYES_LEFT_LR);
             float target_position_right = get_target_position(JointInterface::ID_EYES_RIGHT_LR);
             float target_velocity_left  = get_target_velocity(JointInterface::ID_EYES_LEFT_LR);
             float target_velocity_right = get_target_velocity(JointInterface::ID_EYES_RIGHT_LR);
             // calculate target angles
             float target_position_pan      = (target_position_left + target_position_right) / 2;
             float target_position_vergence = (target_position_left - target_position_right);
             // calculate target velocities
             // for now just use the same velocity for pan and vergence
             float target_velocity_pan = (target_velocity_left + target_velocity_right) / 2.0;
             float target_velocity_tilt = target_velocity_pan;
             store_icub_joint_target(ICUB_ID_EYES_PAN,
                                     target_position_pan, target_velocity_pan);
             store_icub_joint_target(ICUB_ID_EYES_VERGENCE,
                                     target_position_vergence, target_velocity_tilt);
         }else{
             store_joint(id, joint_target[e]);
             // convert to icub joint id
             int icub_id = convert_humotion_jointid_to_icub(humotion_id);
             // store target data
             store_icub_joint_target(icub_id, position, velocity);
+        }
+    }
     //! set the target position of a joint
     //! set the target data for a given icub joint
     //! \param id of joint
     //! \param float value of position
     void iCubJointInterface::store_joint(int id, float value){
         printf("> set joint %d = %f\n",id,value);
         target_angle[id] = value;
     void iCubJointInterface::store_icub_joint_target(int icub_id, float position, float velocity) {
         printf("> set icub joint %d -> p = %f, v = %f\n",icub_id,position,velocity);
         target_angle_[icub_id] = position;
         target_velocity_[icub_id] = velocity;
+    }
     //! execute a move in position mode
     //! \param id of joint
     //! \param angle
     void iCubJointInterface::set_target_in_positionmode(int id){
         double target = target_angle[id];
     void iCubJointInterface::set_target_in_positionmode(int id) {
         assert(POSITION_CONTROL);
         assert(id<=ICUB_ID_EYES_VERGENCE);
         if (id>ICUB_ID_EYES_VERGENCE){
             printf("> ERROR: set_target_positionmode(id=%d, %3.2f) not supported for this id\n",id,target);
             return;
+        }
         // fetch the current target position
         float target = target_angle_[id];
         // execute motion as position control cmd
         ipos->positionMove(id, target);
         yarp_ipos_->positionMove(id, target);
+    }
     //! execute a move in velocity mode
     //! \param id of joint
     //! \param angle
     void iCubJointInterface::set_target_in_velocitymode(int id){
         // set speed cacluated as in velocity + set position -> replicates smoothmotion from flobi?!
         //first: calculate necessary speed to reach the given target within the next clock tick:
         double distance = target_angle[id] - target_angle_previous[id];
         //make the motion smooth: we want to reach 85% of the target in the next iteration:
         distance = 0.85 * distance;
         //distance = -5.0 / 50.0;
         //calculate speed
         //double speed = distance * ((double)MAIN_LOOP_FREQUENCY);
     void iCubJointInterface::set_target_in_velocitymode(int icub_id) {
         assert(!POSITION_CONTROL);
         // fetch humotion id from icub joint id
         int humotion_id = convert_humotion_jointid_to_icub(icub_id);
         // fetch the target velocity
         float target_velocity = target_velocity_[icub_id];
         int e = convert_motorid_to_enum(id);
         double speed = joint_target_speed[e];
         double max = 150.0;
         if (speed > max) speed = max;
         if (speed < -max) speed = -max;
         //speed = -speed;
         // find out the latency between incoming data and now:
         float latency = get_ts_speed(e).get_last_timestamp().to_seconds() - humotion::Timestamp::now().to_seconds();
         printf("TS DIFF %fms\n",latency*1000.0);
         float vmax = 150.0;
         if (target_velocity > vmax)  target_velocity = vmax;
         if (target_velocity < -vmax) target_velocity = -vmax;
         //execute:
         //ivel->velocityMove(id, speed);
         if ((id == ICUB_ID_NECK_PAN)  || (id == ICUB_ID_EYES_BOTH_UD) || (id == ICUB_ID_NECK_TILT) || (id == ICUB_ID_EYES_BOTH_UD) ||  (id == ICUB_ID_NECK_TILT) ){
             //do a pd control for velocity moves that incorporates position errors:
             humotion::Timestamp data_ts = get_ts_position(e).get_last_timestamp();
             //TODO: add interpolation into future!
             //humotion::Timestamp data_ts = humotion::Timestamp::now(); and extend get_interpol value with get_future_value
             double position_error = target_angle[id] - get_ts_position(e).get_interpolated_value(data_ts);
             double error_d = (position_error - last_position_error[id]) / (framerate*1000.0);
             last_position_error[id] = position_error;
             //finally do a PD loop to get the target velocity
             double target_velocity = 0.1*(PID_P[id] * position_error + PID_D[id]*error_d) - speed;
             printf("%f %f %f %f %f %f PID%d\n",
                    get_ts_position(e).get_interpolated_value(data_ts),
                    target_angle[id],
                    get_ts_speed(e).get_interpolated_value(data_ts),
                    target_velocity,
                    speed,
                    position_error,
                    id
                    );
             //if (id == ICUB_ID_NECK_PAN) speed = -speed;
             ivel->velocityMove(id, target_velocity);
             printf("> VEL now=%3.2f target=%3.2f --> dist=%3.2f speed=%3.2f\n",target_angle_previous[id],target_angle[id],distance,speed);
         if ((icub_id != ICUB_ID_NECK_PAN)  &&
             (icub_id != ICUB_ID_EYES_BOTH_UD) &&
             (icub_id != ICUB_ID_NECK_TILT) &&
             (icub_id != ICUB_ID_EYES_BOTH_UD) &&
             (icub_id != ICUB_ID_NECK_TILT)) {
             // limit to some joints for debugging...
             return;
+        }
         target_angle_previous[id] = get_ts_position(convert_motorid_to_enum(id)).get_newest_value();
         // we now add a pd control loop for velocity moves in order to handle position errors
         // TODO: add position interpolation into future. this requires to enable the velocity
         //       broadcast in the torso and head ini file and fetch that velocity in the
         //       icub_data_receiver as well. TODO: check if the can bus has enough bandwidth for that...
         // first: fetch the timstamp of the last known position
         humotion::Timestamp data_ts = get_ts_position(humotion_id).get_last_timestamp();
         // calculate position error:
         float position_error = target_angle_[icub_id] - get_ts_position(humotion_id).get_interpolated_value(data_ts);
         // calculate d term
         float error_d = (position_error - pv_mix_last_error_[icub_id]) / (framerate*1000.0);
         pv_mix_last_error_[icub_id] = position_error;
         // finally do a PD loop to get the target velocity
         float pv_mix_velocity = pv_mix_pid_p_[icub_id] * position_error
                                 + pv_mix_pid_p_[icub_id]*error_d
                                 + target_velocity;
         printf("%f %f %f %f %f %f PID%d\n",
                get_ts_position(humotion_id).get_interpolated_value(data_ts),
                target_angle_[icub_id],
 .4, //NOT USED ANYMORE//get_ts_speed(humotion_id).get_interpolated_value(data_ts),
                pv_mix_velocity,
                target_velocity,
                position_error,
                icub_id
                );
         // execute velocity move
         yarp_ivel_->velocityMove(icub_id, pv_mix_velocity);
+    }
     //! actually execute the scheduled motion commands
-...
         //eyelids: unfortuantely the icub has only 1dof for eyelids, so we use only one dof here:
         face_interface->set_eyelid_angle(target_angle[ICUB_ID_EYES_RIGHT_LID_UPPER]);
         face_interface_->set_eyelid_angle(target_angle_[ICUB_ID_EYES_RIGHT_LID_UPPER]);
         //eyebrows are set using a special command as well:
         face_interface->set_eyebrow_angle(ICUB_ID_EYES_LEFT_BROW, target_angle);
         face_interface->set_eyebrow_angle(ICUB_ID_EYES_RIGHT_BROW, target_angle);
         face_interface_->set_eyebrow_angle(ICUB_ID_EYES_LEFT_BROW, target_angle_);
         face_interface_->set_eyebrow_angle(ICUB_ID_EYES_RIGHT_BROW, target_angle_);
         //mouth
         face_interface->set_mouth(target_angle);
         face_interface_->set_mouth(target_angle_);
         //store joint values which we do not handle on icub here:
         double timestamp = get_timestamp_ms();
         JointInterface::store_incoming_position(ID_LIP_LEFT_UPPER,   target_angle[ICUB_ID_LIP_LEFT_UPPER], timestamp);
         JointInterface::store_incoming_position(ID_LIP_LEFT_LOWER,   target_angle[ICUB_ID_LIP_LEFT_LOWER], timestamp);
         JointInterface::store_incoming_position(ID_LIP_CENTER_UPPER, target_angle[ICUB_ID_LIP_CENTER_UPPER], timestamp);
         JointInterface::store_incoming_position(ID_LIP_CENTER_LOWER, target_angle[ICUB_ID_LIP_CENTER_LOWER], timestamp);
         JointInterface::store_incoming_position(ID_LIP_RIGHT_UPPER,  target_angle[ICUB_ID_LIP_RIGHT_UPPER], timestamp);
         JointInterface::store_incoming_position(ID_LIP_RIGHT_LOWER,  target_angle[ICUB_ID_LIP_RIGHT_LOWER], timestamp);
         double timestamp = humotion::Timestamp::now().to_seconds();
         JointInterface::store_incoming_position(ID_LIP_LEFT_UPPER,   target_angle_[ICUB_ID_LIP_LEFT_UPPER], timestamp);
         JointInterface::store_incoming_position(ID_LIP_LEFT_LOWER,   target_angle_[ICUB_ID_LIP_LEFT_LOWER], timestamp);
         JointInterface::store_incoming_position(ID_LIP_CENTER_UPPER, target_angle_[ICUB_ID_LIP_CENTER_UPPER], timestamp);
         JointInterface::store_incoming_position(ID_LIP_CENTER_LOWER, target_angle_[ICUB_ID_LIP_CENTER_LOWER], timestamp);
         JointInterface::store_incoming_position(ID_LIP_RIGHT_UPPER,  target_angle_[ICUB_ID_LIP_RIGHT_UPPER], timestamp);
         JointInterface::store_incoming_position(ID_LIP_RIGHT_LOWER,  target_angle_[ICUB_ID_LIP_RIGHT_LOWER], timestamp);
+    }
     double iCubJointInterface::get_timestamp_ms(){
         struct timespec spec;
         clock_gettime(CLOCK_REALTIME, &spec);
         return spec.tv_sec*1000 + spec.tv_nsec / 1.0e6;
+    }
     //! set the current position of a joint
     //! \param id of joint
     //! \param float value of position
     //! \param double timestamp
     void iCubJointInterface::fetch_position(int id, double value, double timestamp){
     void iCubJointInterface::store_incoming_position(int id, double value, double timestamp){
         //store joint based on id:
         switch(id){
             default:
-...
+    }
     //! set the current speed of a joint
     //! \param enum id of joint
     //! \param float value of speed
     //! \param double timestamp
     void iCubJointInterface::fetch_speed(int id, double value, double timestamp){
         switch(id){
             default:
                 printf("> ERROR: unhandled joint id %d\n",id);
                 return;
             case(ICUB_ID_NECK_PAN):
                 //PAN IS INVERTED
                 JointInterface::store_incoming_speed(ID_NECK_PAN, value, timestamp);
                 break;
             case(ICUB_ID_NECK_TILT):
                 JointInterface::store_incoming_speed(ID_NECK_TILT, value, timestamp);
                 break;
             case(ICUB_ID_NECK_ROLL):
                 JointInterface::store_incoming_speed(ID_NECK_ROLL, value, timestamp);
                 break;
             case(ICUB_ID_EYES_BOTH_UD):
                 JointInterface::store_incoming_speed(ID_EYES_BOTH_UD, value, timestamp);
                 break;
             //icub handles eyes differently, we have to set pan angle + vergence
             case(ICUB_ID_EYES_PAN): {//pan
                 last_vel_eye_pan = value;
                 float left  = last_vel_eye_pan + last_vel_eye_vergence/2.0;
                 float right = last_vel_eye_pan - last_vel_eye_vergence/2.0;
                 //printf("> eye: velocity pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_vel_eye_pan, last_vel_eye_vergence, left, right);
                 JointInterface::store_incoming_speed(ID_EYES_LEFT_LR, left, timestamp);
                 JointInterface::store_incoming_speed(ID_EYES_RIGHT_LR, right, timestamp);
                 break;
+            }
             case(ICUB_ID_EYES_VERGENCE): { //vergence
                 last_vel_eye_pan = value;
                 float left  = last_vel_eye_pan + last_vel_eye_vergence/2.0;
                 float right = last_vel_eye_pan - last_vel_eye_vergence/2.0;
                 //printf("> eye: velocity pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_vel_eye_pan, last_vel_eye_vergence, left, right);
                 JointInterface::store_incoming_speed(ID_EYES_LEFT_LR, left, timestamp);
                 JointInterface::store_incoming_speed(ID_EYES_RIGHT_LR, right, timestamp);
                 break;
+            }
+        }
+    }
     void iCubJointInterface::set_joint_enable_state(int e, bool enable) {
         int icub_jointid = -1;
-...
         if (icub_jointid != -1) {
             if (enable) {
                 amp->enableAmp(icub_jointid);
                 pid->enablePid(icub_jointid);
                 yarp_amp_->enableAmp(icub_jointid);
                 yarp_pid_->enablePid(icub_jointid);
             } else {
                 pid->disablePid(icub_jointid);
                 amp->disableAmp(icub_jointid);
                 yarp_pid_->disablePid(icub_jointid);
                 yarp_amp_->disableAmp(icub_jointid);
+            }
+        }
+    }
-...
     //! initialise a joint (set up controller mode etc)
     //! \param joint enum
     void iCubJointInterface::init_joints(){
         store_min_max(ilimits, ICUB_ID_NECK_TILT, ID_NECK_TILT);
         store_min_max(ilimits, ICUB_ID_NECK_ROLL, ID_NECK_ROLL);
         store_min_max(ilimits, ICUB_ID_NECK_PAN, ID_NECK_PAN);
         store_min_max(ilimits, ICUB_ID_EYES_BOTH_UD, ID_EYES_BOTH_UD);
         store_min_max(yarp_ilimits_, ICUB_ID_NECK_TILT, ID_NECK_TILT);
         store_min_max(yarp_ilimits_, ICUB_ID_NECK_ROLL, ID_NECK_ROLL);
         store_min_max(yarp_ilimits_, ICUB_ID_NECK_PAN, ID_NECK_PAN);
         store_min_max(yarp_ilimits_, ICUB_ID_EYES_BOTH_UD, ID_EYES_BOTH_UD);
         //icub handles eyes differently, we have to set pan angle + vergence
         double pan_min, pan_max, vergence_min, vergence_max;
         ilimits->getLimits(ICUB_ID_EYES_PAN, &pan_min, &pan_max);
         ilimits->getLimits(ICUB_ID_EYES_VERGENCE, &vergence_min, &vergence_max);
         yarp_ilimits_->getLimits(ICUB_ID_EYES_PAN, &pan_min, &pan_max);
         yarp_ilimits_->getLimits(ICUB_ID_EYES_VERGENCE, &vergence_min, &vergence_max);
         //this is not 100% correct, should be fixed:
         joint_min[ID_EYES_LEFT_LR] = pan_min; // - vergence_max/2;
-...
         joint_max[ID_LIP_RIGHT_UPPER] = 50;
         joint_min[ID_LIP_RIGHT_LOWER] = 5;
         joint_max[ID_LIP_RIGHT_LOWER] = 50;
+    }
     //! conversion table for humotion joint id to icub joint id
     //! \param int value for humotion joint id from JointInterface::JOINT_ID_ENUM
     //! \return int value of icub joint id
     int iCubJointInterface::convert_humotion_jointid_to_icub(int humotion_id){
         enum_id_bimap_t::right_const_iterator it = enum_id_bimap.right.find(humotion_id);
         if(it == enum_id_bimap.right.end()) {
             //key does not exist
             cout << "ERROR: invalid humotion joint id given. can not convert this. exiting\n";
             exit(EXIT_FAILURE);
+        }
         return it->second;
+    }
     //! conversion table for icub joint id to humotion joint id
     //! \param  int value of icub joint id
     //! \return int value of humotion joint id from JointInterface::JOINT_ID_ENUM
     int iCubJointInterface::convert_icub_jointid_to_humotion(int icub_id){
         enum_id_bimap_t::left_const_iterator it = enum_id_bimap.left.find(icub_id);
         if(it == enum_id_bimap.left.end()) {
             //key does not exist
             cout << "ERROR: invalid icub joint id given. can not convert this. exiting\n";
             exit(EXIT_FAILURE);
+        }
         return it->second;
+    }

         JointInterface();
         ~JointInterface();
         void set_target_position(int joint_id, float position, float speed);
         void set_target(int joint_id, float position, float velocity);
         float get_target_position(int joint_id);
         virtual void publish_target_position(int joint_id) = 0;
         float get_target_velocity(int joint_id);
         void publish_target(int joint_id);
         virtual void publish_target(int joint_id, float position, float velocity) = 0;
         virtual void execute_motion() = 0;
         typedef std::map<int, TimestampedList> joint_tsl_map_t;
-...
         float joint_min[JOINT_ID_ENUM_SIZE];
         float joint_max[JOINT_ID_ENUM_SIZE];
         float joint_target[JOINT_ID_ENUM_SIZE];
         float joint_target_speed[JOINT_ID_ENUM_SIZE];
     private:
         float joint_target_position_[JOINT_ID_ENUM_SIZE];
         float joint_target_velocity_[JOINT_ID_ENUM_SIZE];
         boost::mutex joint_ts_position_map_access_mutex;
         boost::mutex joint_ts_speed_map_access_mutex;
         joint_tsl_map_t joint_ts_position_map;

         reflexxes_calculate_profile();
         //tell the joint about the new values:
         joint_interface->set_target_position(JointInterface::ID_EYES_LEFT_LR,
                                              reflexxes_position_output->NewPositionVector->VecData[0],
                                              reflexxes_position_output->NewVelocityVector->VecData[0]);
         joint_interface->set_target_position(JointInterface::ID_EYES_RIGHT_LR,
                                              reflexxes_position_output->NewPositionVector->VecData[1],
                                              reflexxes_position_output->NewVelocityVector->VecData[1]);
         joint_interface->set_target_position(JointInterface::ID_EYES_BOTH_UD,
                                              reflexxes_position_output->NewPositionVector->VecData[2],
                                              reflexxes_position_output->NewVelocityVector->VecData[2]);
         joint_interface->set_target(JointInterface::ID_EYES_LEFT_LR,
                                     reflexxes_position_output->NewPositionVector->VecData[0],
                                     reflexxes_position_output->NewVelocityVector->VecData[0]);
         joint_interface->set_target(JointInterface::ID_EYES_RIGHT_LR,
                                     reflexxes_position_output->NewPositionVector->VecData[1],
                                     reflexxes_position_output->NewVelocityVector->VecData[1]);
         joint_interface->set_target(JointInterface::ID_EYES_BOTH_UD,
                                     reflexxes_position_output->NewPositionVector->VecData[2],
                                     reflexxes_position_output->NewVelocityVector->VecData[2]);
         cout << reflexxes_position_output->NewPositionVector->VecData[2] << " " << reflexxes_position_output->NewVelocityVector->VecData[2] ;
+    }
-...
     void EyeMotionGenerator::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_EYES_LEFT_LR);
             joint_interface->publish_target_position(JointInterface::ID_EYES_RIGHT_LR);
             joint_interface->publish_target_position(JointInterface::ID_EYES_BOTH_UD);
             joint_interface->publish_target(JointInterface::ID_EYES_LEFT_LR);
             joint_interface->publish_target(JointInterface::ID_EYES_RIGHT_LR);
             joint_interface->publish_target(JointInterface::ID_EYES_BOTH_UD);
+        }
+    }

         float eyebrow_right_target = requested_gaze_state.eyebrow_right;
         //store targets:
         joint_interface->set_target_position(JointInterface::ID_EYES_LEFT_BROW, eyebrow_left_target, 0.0);
         joint_interface->set_target_position(JointInterface::ID_EYES_RIGHT_BROW, eyebrow_right_target, 0.0);
         joint_interface->set_target(JointInterface::ID_EYES_LEFT_BROW, eyebrow_left_target, 0.0);
         joint_interface->set_target(JointInterface::ID_EYES_RIGHT_BROW, eyebrow_right_target, 0.0);
+    }
     //! publish targets to motor boards:
     void EyebrowMotionGenerator::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_EYES_LEFT_BROW);
             joint_interface->publish_target_position(JointInterface::ID_EYES_RIGHT_BROW);
             joint_interface->publish_target(JointInterface::ID_EYES_LEFT_BROW);
             joint_interface->publish_target(JointInterface::ID_EYES_RIGHT_BROW);
+        }
+    }

         //limit target angles:
         eyelid_upper_left_target  = limit_target(JointInterface::ID_EYES_LEFT_LID_UPPER,  eyelid_upper_left_target);
         eyelid_lower_left_target  = limit_target(JointInterface::ID_EYES_LEFT_LID_LOWER,  eyelid_lower_left_target);
         eyelid_upper_right_target = limit_target(JointInterface::ID_EYES_RIGHT_LID_UPPER,  eyelid_upper_right_target);
         eyelid_lower_right_target = limit_target(JointInterface::ID_EYES_RIGHT_LID_LOWER,  eyelid_lower_right_target);
         eyelid_upper_right_target = limit_target(JointInterface::ID_EYES_RIGHT_LID_UPPER, eyelid_upper_right_target);
         eyelid_lower_right_target = limit_target(JointInterface::ID_EYES_RIGHT_LID_LOWER, eyelid_lower_right_target);
         //(temporarily) store the target
         joint_interface->set_target_position(JointInterface::ID_EYES_LEFT_LID_UPPER,  eyelid_upper_left_target, 0.0);
         joint_interface->set_target_position(JointInterface::ID_EYES_LEFT_LID_LOWER,  eyelid_lower_left_target, 0.0);
         joint_interface->set_target_position(JointInterface::ID_EYES_RIGHT_LID_UPPER,  eyelid_upper_right_target, 0.0);
         joint_interface->set_target_position(JointInterface::ID_EYES_RIGHT_LID_LOWER,  eyelid_lower_right_target, 0.0);
         joint_interface->set_target(JointInterface::ID_EYES_LEFT_LID_UPPER,  eyelid_upper_left_target, 0.0);
         joint_interface->set_target(JointInterface::ID_EYES_LEFT_LID_LOWER,  eyelid_lower_left_target, 0.0);
         joint_interface->set_target(JointInterface::ID_EYES_RIGHT_LID_UPPER, eyelid_upper_right_target, 0.0);
         joint_interface->set_target(JointInterface::ID_EYES_RIGHT_LID_LOWER, eyelid_lower_right_target, 0.0);
         //check for saccade
         check_for_saccade();
-...
             //use the upper value + 10 deg as close state:
             value = joint_interface->get_joint_min(JointInterface::ID_EYES_LEFT_LID_UPPER) + 10.0;
             //overwrite last target_
             joint_interface->set_target_position(joint_id, value, 0.0);
             joint_interface->set_target(joint_id, value, 0.0);
             break;
         case(JointInterface::ID_EYES_RIGHT_LID_UPPER):
-...
             //use the upper value + 10 deg as close state:
             value = joint_interface->get_joint_min(JointInterface::ID_EYES_RIGHT_LID_UPPER) + 10.0;
             //overwrite last target_
             joint_interface->set_target_position(joint_id, value, 0.0);
             joint_interface->set_target(joint_id, value, 0.0);
             break;
+        }
+    }
-...
     void EyelidMotionGenerator::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_EYES_LEFT_LID_UPPER);
             joint_interface->publish_target_position(JointInterface::ID_EYES_LEFT_LID_LOWER);
             joint_interface->publish_target_position(JointInterface::ID_EYES_RIGHT_LID_UPPER);
             joint_interface->publish_target_position(JointInterface::ID_EYES_RIGHT_LID_LOWER);
             joint_interface->publish_target(JointInterface::ID_EYES_LEFT_LID_UPPER);
             joint_interface->publish_target(JointInterface::ID_EYES_LEFT_LID_LOWER);
             joint_interface->publish_target(JointInterface::ID_EYES_RIGHT_LID_UPPER);
             joint_interface->publish_target(JointInterface::ID_EYES_RIGHT_LID_LOWER);
+        }
+    }

     //! set joint target position
     //! \param joint_id of joint
     //! \param float position
     void JointInterface::set_target_position(int joint_id, float position, float speed){
     void JointInterface::set_target(int joint_id, float position, float velocity){
         assert(joint_id < JOINT_ID_ENUM_SIZE);
         //update current value
         joint_target[joint_id] = position;
         joint_target_speed[joint_id] = speed;
         joint_target_position_[joint_id] = position;
         joint_target_velocity_[joint_id] = velocity;
+    }
     //! fetch target position
     //! \param joint_id of joint
     float JointInterface::get_target_position(int joint_id){
         assert(joint_id < JOINT_ID_ENUM_SIZE);
         return joint_target[joint_id];
         return joint_target_position_[joint_id];
+    }
     //! fetch target velocity
     //! \param joint_id of joint
     float JointInterface::get_target_velocity(int joint_id){
         assert(joint_id < JOINT_ID_ENUM_SIZE);
         return joint_target_velocity_[joint_id];
+    }
     //! incoming position data
-...
         gaze_enabled = false;
+    }
     //! fetch maximum allowable joint position
     //! fetch maximum allowed joint position
     //! \return max position
     float JointInterface::get_joint_max(int joint_id){
         assert((joint_id > 0) && (joint_id < JOINT_ID_ENUM_SIZE));
         return joint_max[joint_id];
+    }
     //! fetch minimum allowable joint position
     //! fetch minimum allowed joint position
     //! \return min position
     float JointInterface::get_joint_min(int joint_id){
         assert((joint_id > 0) && (joint_id < JOINT_ID_ENUM_SIZE));
-...
     bool JointInterface::get_joint_position_map_empty(){
         return (joint_ts_position_map.empty());
+    }
     //! call the virtual store function with given position and velocities
     void JointInterface::publish_target(int joint_id) {
         publish_target(joint_id, joint_target_position_[joint_id], joint_target_velocity_[joint_id]);
+    }

             unsafe_target_upper = unsafe_target_lower - opening;
+        }
         //tell the joint about the new values:
         joint_interface->set_target_position(upper_id, unsafe_target_upper, 0.0);
         joint_interface->set_target_position(lower_id, unsafe_target_lower, 0.0);
         //printf("> update_mouth_target(u=%d, l=%d) -> upper = %4.2f, lower = %4.2f\n",upper_id, lower_id,unsafe_target_upper,unsafe_target_lower);
         // tell the joint about the new values:
         joint_interface->set_target(upper_id, unsafe_target_upper, 0.0);
         joint_interface->set_target(lower_id, unsafe_target_lower, 0.0);
+    }
-...
     void MouthMotionGenerator::publish_targets(){
         //publish values if there is an active gaze input within the last timerange
         if (mouth_target_input_active()){
             joint_interface->publish_target_position(JointInterface::ID_LIP_LEFT_UPPER);
             joint_interface->publish_target_position(JointInterface::ID_LIP_LEFT_LOWER);
             joint_interface->publish_target_position(JointInterface::ID_LIP_CENTER_UPPER);
             joint_interface->publish_target_position(JointInterface::ID_LIP_CENTER_LOWER);
             joint_interface->publish_target_position(JointInterface::ID_LIP_RIGHT_UPPER);
             joint_interface->publish_target_position(JointInterface::ID_LIP_RIGHT_LOWER);
             joint_interface->publish_target(JointInterface::ID_LIP_LEFT_UPPER);
             joint_interface->publish_target(JointInterface::ID_LIP_LEFT_LOWER);
             joint_interface->publish_target(JointInterface::ID_LIP_CENTER_UPPER);
             joint_interface->publish_target(JointInterface::ID_LIP_CENTER_LOWER);
             joint_interface->publish_target(JointInterface::ID_LIP_RIGHT_UPPER);
             joint_interface->publish_target(JointInterface::ID_LIP_RIGHT_LOWER);
+        }
+    }

         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(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(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]);
         joint_interface->set_target(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,
-...
     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);
             joint_interface->publish_target(JointInterface::ID_NECK_PAN);
             joint_interface->publish_target(JointInterface::ID_NECK_TILT);
             joint_interface->publish_target(JointInterface::ID_NECK_ROLL);
+        }
+    }

Also available in: Unified diff