/examples/yarp_icub/src/icub_jointinterface.cpp - Diff - humotion - Research for Cognitive Interaction

Revision 0d0f5ca1 examples/yarp_icub/src/icub_jointinterface.cpp

     #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;

Also available in: Unified diff