/ - Diff - humotion - Research for Cognitive Interaction

     file(GLOB DUMMY_HEADER_LIST RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} include/*.h)
     ADD_EXECUTABLE(${MAIN} src/main.cpp src/icub_jointinterface.cpp src/icub_data_receiver.cpp ${DUMMY_HEADER_LIST})
     ADD_EXECUTABLE(${MAIN} src/main.cpp src/icub_jointinterface.cpp src/icub_faceinterface.cpp src/icub_data_receiver.cpp ${DUMMY_HEADER_LIST})
     # we now add the YARP and iCub libraries to our project.
     TARGET_LINK_LIBRARIES(${MAIN} ${Boost_LIBRARIES} ${YARP_LIBRARIES} ${ICUB_LIBRARIES} ${humotion_LIBRARIES})

     #include "icub_data_receiver.h"
     class iCubDataReceiver;
     class iCubFaceInterface;
     class iCubJointInterface : public humotion::server::JointInterface{
     public:
-...
         double target_angle[ICUB_JOINT_ID_ENUM_SIZE];
         double target_angle_previous[ICUB_JOINT_ID_ENUM_SIZE];
         void set_eyelid_angle(double angle);
         void set_eyebrow_angle(int id);
         void set_mouth();
         iCubDataReceiver *icub_data_receiver;
         void init_joints();
         double lid_angle;
         int lid_opening_previous;
         int previous_mouth_state;
         std::string scope;
         yarp::dev::PolyDriver dd;
-...
         yarp::sig::Vector velocities;
         yarp::sig::Vector commands;
         yarp::os::BufferedPort<yarp::os::Bottle> emotion_port[4];
         yarp::dev::IVelocityControl *ivel;
         yarp::dev::IPositionControl *ipos;
         yarp::dev::IEncodersTimed *iencs;
-...
         int convert_enum_to_motorid(int e);
         int convert_motorid_to_enum(int id);
         iCubFaceInterface *face_interface;
         typedef boost::bimap<int, int > enum_id_bimap_t;
         typedef enum_id_bimap_t::value_type enum_id_bimap_entry_t;

             icub_jointinterface->fetch_speed(i, velocities[i], 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());
         //fixme: use real id

     #include "icub_jointinterface.h"
     #include "icub_faceinterface.h"
     #include <yarp/os/Property.h>
     using namespace yarp::dev;
     using namespace yarp::sig;
-...
     //! 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
-...
             ivel->setRefAccelerations(commands.data());
+        }
         //attach to facial expressions:
         string emotion_scope = scope + "/face/raw/in";
         printf("> opening connection to %s\n", emotion_scope.c_str());
         for(int i=0; i<4; i++){
             //strange, if we use one output port only the first command is executed?! flushing issues?
             string emotion_port_out = "/emotionwriter" + to_string(i);
             if (!emotion_port[i].open(emotion_port_out.c_str())){
                 printf("> ERROR: failed to open to %s\n",emotion_port_out.c_str());
                 exit(EXIT_FAILURE);
+            }
             if (!Network::connect(emotion_port_out.c_str(), emotion_scope.c_str())){
                 printf("> ERROR: failed to connect emotion ports\n");
                 exit(EXIT_FAILURE);
+            }
+        }
+    }
     //! destructor
-...
         return it->second;
+    }
     //! special command to set eyelid angle
     //! \param angle in degrees
     void iCubJointInterface::set_eyelid_angle(double 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°
             int opening = (25.0 + 0.8*angle);
             opening = min(48, max(24, opening));
             if (opening == lid_opening_previous){
                 //no update necessary
                 return;
+            }
             lid_angle = angle;
             lid_opening_previous = opening;
             char buf[20];
             sprintf(buf, "S%2d", opening);
             //printf("> SETTING EYELID '%s' (%f -> %d)\n",buf,angle,opening);
             Bottle &cmd = emotion_port[0].prepare();
             cmd.clear();
             cmd.addString(buf);
             emotion_port[0].writeStrict();
         }else{
             printf("> ERROR: no icub emotion output\n");
             exit(EXIT_FAILURE);
+        }
+    }
     //! special command to set the eyebrow angle
     //! \param id {0=left, 1=right)
     //! \param angle in degrees
     void iCubJointInterface::set_eyebrow_angle(int id){
         int port_id;
         if (id == ICUB_ID_EYES_LEFT_BROW){
             port_id = 1;
         }else{
             port_id = 2;
+        }
         if (emotion_port[port_id].getOutputCount()>0){
             double angle = target_angle[id];
             int icub_val = 0;
             //swap rotation direction:
             if (id==ICUB_ID_EYES_LEFT_BROW) angle = -angle;
             //convert to icub representation
             if (angle < -20){
                 icub_val = 1;
             }else if (angle<10){
                 icub_val = 2;
             }else if (angle<20){
                 icub_val = 4;
             }else{
                 icub_val = 8;
+            }
             //make sure to update only on new values:
             if (icub_val == target_angle_previous[id]){
                     //no updata necessary
                     return;
+            }
             //store actual value:
             target_angle_previous[id] = icub_val;
             string cmd_s;
             if (id==ICUB_ID_EYES_LEFT_BROW){
                 cmd_s = "L0" + to_string(icub_val);
             }else{
                 cmd_s = "R0" + to_string(icub_val);
+            }
             printf("> SETTING EYEBROW %d (%f -> %s)\n",id,angle,cmd_s.c_str());
             Bottle &cmd = emotion_port[port_id].prepare();
             cmd.clear();
             cmd.addString(cmd_s);
             emotion_port[port_id].writeStrict();
         }else{
             printf("> ERROR: no icub emotion output\n");
             exit(EXIT_FAILURE);
+        }
+    }
     void iCubJointInterface::run(){
         iCubDataReceiver *data_receiver = new iCubDataReceiver(10/*1000.0 / MAIN_LOOP_FREQUENCY*/, iencs, this);
         iCubDataReceiver *data_receiver = new iCubDataReceiver(1000.0 / MAIN_LOOP_FREQUENCY, iencs, this);
         data_receiver->start();
+    }
-...
     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
+        }
-...
     //! \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);
         printf("> set joint %d = %f\n",id,value);
         target_angle[id] = value;
         //ipos->positionMove(id, value);
+    }
     //! execute a move in position mode
-...
         //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) ){
             if (id == ICUB_ID_NECK_PAN) speed = -speed;
             //if (id == ICUB_ID_NECK_PAN) speed = -speed;
             ivel->velocityMove(id, speed);
             printf("> VEL now=%3.2f target=%3.2f --> dist=%3.2f speed=%3.2f\n",target_angle_previous[id],value,distance,speed);
+        }
-...
         //eyelids: unfortuantely the icub has only 1dof for eyelids, so we use only one dof here:
         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:
         set_eyebrow_angle(ICUB_ID_EYES_LEFT_BROW);
         set_eyebrow_angle(ICUB_ID_EYES_RIGHT_BROW);
         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
         set_mouth();
+    }
     void iCubJointInterface::set_mouth(){
         //convert from 6DOF mouth displacement to icub leds:
         int led_value = 0;
         //fetch center opening:
         double center_opening = target_angle[ICUB_ID_LIP_CENTER_LOWER] - target_angle[ICUB_ID_LIP_CENTER_UPPER];
         bool mouth_open = (center_opening>15.0)?true:false;
         //side of mouth high or low?
         double center_avg = (target_angle[ICUB_ID_LIP_CENTER_LOWER] + target_angle[ICUB_ID_LIP_CENTER_UPPER])/2.0;
         double left_avg   = (target_angle[ICUB_ID_LIP_LEFT_LOWER] + target_angle[ICUB_ID_LIP_LEFT_UPPER])/2.0;
         double right_avg  = (target_angle[ICUB_ID_LIP_RIGHT_LOWER] + target_angle[ICUB_ID_LIP_RIGHT_UPPER])/2.0;
         //happy, neutral or sad?
         double diff_l = center_avg - left_avg;
         double diff_r = center_avg - right_avg;
         double diff   = (diff_l+diff_r)/2.0;
         if (diff > 2.0){
             if (mouth_open){
                 led_value = 0x14;
             }else{
                 if (diff > 2.6){
                     led_value = 0x0A;
                 }else{
                     led_value = 0x0B;
+                }
+            }
         }else if (diff < -3.0){
             if (mouth_open){
                 led_value = 0x06;
             }else{
                 led_value = 0x18;
+            }
         }else if (diff < -2.0){
             if (mouth_open){
                 led_value = 0x04; //0x25;
             }else{
                 led_value = 0x08;
+            }
         }else{
             if (mouth_open){
                 led_value = 0x16;
             }else{
                 led_value = 0x08;
+            }
+        }
         if (led_value == previous_mouth_state){
             //no update necessary
             return;
+        }
         previous_mouth_state = led_value;
         //convert to string:
         char buf[10];
         sprintf(buf, "M%02X",led_value);
         /*printf("> sending mouth '%s'\n",buf);
         printf("> mouth angles: %3.2f %3.2f %3.2f\n",target_angle[ICUB_ID_LIP_LEFT_UPPER],target_angle[ICUB_ID_LIP_CENTER_UPPER],target_angle[ICUB_ID_LIP_RIGHT_UPPER]);
         printf("  mouth         %3.2f %3.2f %3.2f\n",target_angle[ICUB_ID_LIP_LEFT_LOWER],target_angle[ICUB_ID_LIP_CENTER_LOWER],target_angle[ICUB_ID_LIP_RIGHT_LOWER]);
         printf("  mouth  open=%3.2f diff=%3.2f\n", center_opening, diff);*/
         //add mouth:
         Bottle &cmd = emotion_port[3].prepare();
         cmd.clear();
         cmd.addString(buf);
         emotion_port[3].writeStrict();
         face_interface->set_mouth(target_angle);
         //store joint values which we do not handle on icub here:
-...
         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(){
-...
                 return;
             case(100):
                 JointInterface::store_incoming_position(ID_EYES_RIGHT_LID_UPPER, lid_angle, timestamp);
                 //JointInterface::store_incoming_position(ID_EYES_RIGHT_LID_UPPER, lid_angle, timestamp);
                 break;
             case(ICUB_ID_NECK_PAN):
                 //PAN is inverted!
                 JointInterface::store_incoming_position(ID_NECK_PAN, -value, timestamp);
                 JointInterface::store_incoming_position(ID_NECK_PAN, value, timestamp);
                 break;
             case(ICUB_ID_NECK_TILT):
-...
             case(ICUB_ID_NECK_PAN):
                 //PAN IS INVERTED
                 JointInterface::store_incoming_speed(ID_NECK_PAN, -value, timestamp);
                 JointInterface::store_incoming_speed(ID_NECK_PAN, value, timestamp);
                 break;
             case(ICUB_ID_NECK_TILT):
-...
         //eyelids:
         joint_min[ID_EYES_RIGHT_LID_UPPER] = -50; //24-30;
         joint_max[ID_EYES_RIGHT_LID_UPPER] = 50; //48-30;
         lid_angle = joint_max[ID_EYES_RIGHT_LID_UPPER];
         //lid_angle = joint_max[ID_EYES_RIGHT_LID_UPPER];
         //eyebrows:
         joint_min[ID_EYES_LEFT_BROW] = -50;

         static const float SACCADE_SPEED_THRESHOLD;
     private:
         void setup_eyemotion(int dof, float target, float now);
         void setup_eyemotion(int dof, float target, float current_position, float current_speed);
         TimestampedList tsl_gaze_target_pan;
         TimestampedList tsl_gaze_target_tilt;

include/humotion/server/motion_generator.h
46	46
47	47	protected:
48	48	float get_current_position(int joint_id);
	49	float get_current_speed(int joint_id);
49	50	float limit_target(int joint_id, float val);
50	51
51	52	JointInterface *joint_interface;

         ~NeckMotionGenerator();
         void calculate_targets();
         void publish_targets();
         void setup_neckmotion(int dof, float target, float now);
         void setup_neckmotion(int dof, float target, float current_position, float current_speed);
     private:
         float get_breath_offset();

         ~ReflexxesMotionGenerator();
     protected:
         void reflexxes_set_input(int dof, float target, float position, float max_speed, float max_accel);
         void reflexxes_set_input(int dof, float target, float current_position, float current_speed, float max_speed, float max_accel);
         void reflexxes_calculate_profile();
         //*****************************************

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

     //! \param dof id of joint
     //! \param target angle
     //! \param current angle
     void EyeMotionGenerator::setup_eyemotion(int dof, float target, float now){
     void EyeMotionGenerator::setup_eyemotion(int dof, float target, float current_position, float current_speed){
         //get distance to target:
         float distance_abs = fabs(target - now);
         float distance_abs = fabs(target - current_position);
         //get max speed: factor can be found in encyc britannica: "linear.... being 300° per second for 10° and 500° per second for 30°" (max=700)
         float max_speed = fmin(700.0, 10.0*distance_abs + 200.0);
         //max accel: use data from ""Speed and Accuracy of Saccadic Eye Movements: Characteristics of Impulse Variability in the Oculomotor System"
-...
         float max_accel = fmin(80000.0, 1526.53*distance_abs + 10245.4);
         //feed reflexxes api with data
         reflexxes_set_input(dof, target, now, max_speed, max_accel);
         reflexxes_set_input(dof, target, current_position, current_speed, max_speed, max_accel);
+    }
     //! calculate joint targets
-...
         float eye_pan_l_now = get_current_position(JointInterface::ID_EYES_LEFT_LR);
         float eye_pan_r_now = get_current_position(JointInterface::ID_EYES_RIGHT_LR);
         float eye_tilt_now  = get_current_position(JointInterface::ID_EYES_BOTH_UD);
         //fetch current velocities
         float eye_pan_l_speed = get_current_speed(JointInterface::ID_EYES_LEFT_LR);
         float eye_pan_r_speed = get_current_speed(JointInterface::ID_EYES_RIGHT_LR);
         float eye_tilt_speed  = get_current_speed(JointInterface::ID_EYES_BOTH_UD);
         //pass paramaters to reflexxes api:
         setup_eyemotion(0, eye_pan_l_target, eye_pan_l_now);
         setup_eyemotion(1, eye_pan_r_target, eye_pan_r_now);
         setup_eyemotion(2, eye_tilt_target, eye_tilt_now);
         cout << "EYE MOTION 2 " << eye_tilt_target << " now=" << eye_tilt_now << "\n";
         setup_eyemotion(0, eye_pan_l_target, eye_pan_l_now, eye_pan_l_speed);
         setup_eyemotion(1, eye_pan_r_target, eye_pan_r_now, eye_pan_r_speed);
         setup_eyemotion(2, eye_tilt_target, eye_tilt_now, eye_tilt_speed);
         // cout << "EYE MOTION 2 " << eye_tilt_target << " now=" << eye_tilt_now << "\n";
         //call reflexxes to handle profile calculation:
         reflexxes_calculate_profile();

     MotionGenerator::~MotionGenerator(){
+    }
     //! fetch thr latest (=current) position of a joint
     //! 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){

         float neck_pan_now  = get_current_position(JointInterface::ID_NECK_PAN);
         float neck_tilt_now = get_current_position(JointInterface::ID_NECK_TILT);
         float neck_roll_now = get_current_position(JointInterface::ID_NECK_ROLL);
         // fetch current velocities
         float neck_pan_speed  = get_current_speed(JointInterface::ID_NECK_PAN);
         float neck_tilt_speed = get_current_speed(JointInterface::ID_NECK_TILT);
         float neck_roll_speed = get_current_speed(JointInterface::ID_NECK_ROLL);
         //reached target?
         float goal_diff   = fabs(get_current_gaze().distance_pt_abs(requested_gaze_state));
-...
         neck_tilt_target += get_breath_offset();
         //pass parameters to reflexxes api:
         setup_neckmotion(0, neck_pan_target,  neck_pan_now);
         setup_neckmotion(1, neck_tilt_target, neck_tilt_now);
         setup_neckmotion(2, neck_roll_target, neck_roll_now);
         setup_neckmotion(0, neck_pan_target,  neck_pan_now, neck_pan_speed);
         setup_neckmotion(1, neck_tilt_target, neck_tilt_now, neck_tilt_speed);
         setup_neckmotion(2, neck_roll_target, neck_roll_now, neck_roll_speed);
         //call reflexxes to handle profile calculation:
         reflexxes_calculate_profile();
-...
         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 %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:
-...
     //! \param dof id of joint
     //! \param target angle
     //! \param current angle
     void NeckMotionGenerator::setup_neckmotion(int dof, float target, float now){
     void NeckMotionGenerator::setup_neckmotion(int dof, float target, float current_position, float current_speed){
         //get distance to target:
         float distance_abs = fabs(target - now);
         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)
-...
         ///printf("MAX SPEED %4.2f / max accel %4.2f\n",max_speed, max_accel);
         //feed reflexxes api with data
         reflexxes_set_input(dof, target, now, max_speed, max_accel);
         reflexxes_set_input(dof, target, current_position, current_speed, max_speed, max_accel);
+    }

     //! \param target angle
     //! \param max_speed max reachable speed during accel
     //! \param max_accel max allowable acceleration
     void ReflexxesMotionGenerator::reflexxes_set_input(int dof, float target, float position, float max_speed, float max_accel){
     void ReflexxesMotionGenerator::reflexxes_set_input(int dof, float target, float current_position, float current_speed, float max_speed, float max_accel){
         assert(dof < dof_count);
         //set up reflexxes:
-...
         reflexxes_position_input->MaxVelocityVector->VecData[dof]     = max_speed;
         reflexxes_position_input->MaxAccelerationVector->VecData[dof] = max_accel;
         reflexxes_position_input->TargetVelocityVector->VecData[dof]  = 0.0; //target speed is zero (really?)
         reflexxes_position_input->CurrentPositionVector->VecData[dof]  = position;
         // feed back current pos & velocity
         reflexxes_position_input->CurrentPositionVector->VecData[dof]  = current_position;
         reflexxes_position_input->CurrentVelocityVector->VecData[dof]  = current_speed;
         // safety: libreflexxes does not like zero accellerations...
         if (reflexxes_position_input->MaxAccelerationVector->VecData[dof] == 0.0){
                 reflexxes_position_input->MaxAccelerationVector->VecData[dof] = 0.0001;
+        }
+    }
     //! calculate motion profile
     void ReflexxesMotionGenerator::reflexxes_calculate_profile(){
-...
         //feed back values:
         for(int i=0; i<dof_count; i++){
             //reflexxes_position_input->CurrentPositionVector->VecData[i]     = reflexxes_position_output->NewPositionVector->VecData[i];
             reflexxes_position_input->CurrentVelocityVector->VecData[i]     = reflexxes_position_output->NewVelocityVector->VecData[i];
             reflexxes_position_input->CurrentAccelerationVector->VecData[i] = reflexxes_position_output->NewAccelerationVector->VecData[i];
             //reflexxes_position_input->CurrentVelocityVector->VecData[i]     = reflexxes_position_output->NewVelocityVector->VecData[i];
             //reflexxes_position_input->CurrentAccelerationVector->VecData[i] = reflexxes_position_output->NewAccelerationVector->VecData[i];
+        }
+    }

humotion

Revision 0d0f5ca1