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

Revision 35b3ca25 examples/yarp_icub/src/icub_jointinterface.cpp

     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;
+    }

Also available in: Unified diff