/examples/meka/src/mekajointinterface.cpp - Diff - humotion - Research for Cognitive Interaction

Revision ddccf279 examples/meka/src/mekajointinterface.cpp

     //! NOTE: this should also prefill the min/max positions for this joint
     //! \param the enum id of a joint
     void MekaJointInterface::enable_joint(int e){
     #if 0
         //FIXME ADD THIS:
         // enable the amplifier and the pid controller on each joint
         /*for (i = 0; i < nj; i++) {
            amp->enableAmp(i);
            pid->enablePid(i);
         }*/
         //set up smooth motion controller
         //step1: set up framerate
         //dev->set_register_blocking(XSC3_REGISTER_PID_RAMP, framerate, true);
         //step2: set controllertype:
         //printf("> activating smooth motion control for joint id 0x%02X (%s)\n", motor_id, joint_name.c_str());
         //dev->set_register_blocking(XSC3_REGISTER_PID_CONTROLLER, XSC3_PROTOCOL_PID_CONTROLLERTYPE_SMOOTH_PLAYBACK, true);
         //step3: set pid controller:
         /*if ((e == ID_LIP_LEFT_UPPER) || (e == ID_LIP_LEFT_LOWER) || (e == ID_LIP_CENTER_UPPER) || (e == ID_LIP_CENTER_LOWER) || (e == ID_LIP_RIGHT_UPPER) || (e == ID_LIP_RIGHT_LOWER)){
             printf("> fixing PID i controller value for smooth motion (FIXME: restore old value!!)\n");
             dev->set_register_blocking(XSC3_REGISTER_CONST_I, 10, true);
         }*/
         //uint16_t result = dev->get_register_blocking_raw(XSC3_REGISTER_PID_CONTROLLER);
         //check if setting pid controllertype was successfull:
         /*if (result != XSC3_PROTOCOL_PID_CONTROLLERTYPE_SMOOTH_PLAYBACK){
             printf("> failed to set smooth motion controller for joint %s (res=0x%04X)\n",joint_name.c_str(),result);
             exit(1);
         }*/
         //fetch min/max:
        // init_joint(e);
         //ok fine, now enable motor:
         //printf("> enabling motor %s\n", joint_name.c_str());
         //dev->set_register_blocking(XSC3_REGISTER_STATUS, XSC3_PROTOCOL_STATUS_BITMASK_MOTOR_ENABLE, true);
     #endif
         //meka does not support this, joints are always enabled
+    }
     //! shutdown and disable a joint
     //! \param the enum id of a joint
     void MekaJointInterface::disable_joint(int e){
         /*
             //first: convert humotion enum to our enum:
             int motor_id = convert_enum_to_motorid(e);
             if (motor_id == -1){
                 return; //we are not interested in that data, so we just return here
+            }
             //fetch device:
             Device *dev = get_device(motor_id);
             printf("> FIXME: ADD DISABLE CODE\n");
             printf("> FIXME: ADD DISABLE CODE\n");
             printf("> FIXME: ADD DISABLE CODE\n");
             printf("> FIXME: ADD DISABLE CODE\n");
             printf("> FIXME: ADD DISABLE CODE\n");
             printf("> FIXME: ADD DISABLE CODE\n");
             printf("> FIXME: ADD DISABLE CODE\n");
             printf("> FIXME: ADD DISABLE CODE\n");
             printf("> FIXME: ADD DISABLE CODE\n");
             */
         //meka does not support this, joints are always enabled
+    }
     void MekaJointInterface::store_min_max(int id, float min, float max){
-...
         store_min_max(ID_LIP_RIGHT_LOWER, -1, 1);
+    }
     #if 0
     //! 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;
+        }
         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;
+    }
     //! 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].write();
         }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].write();
         }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);
         data_receiver->start();
+    }
     //! set the target position of a 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_LEFT_LR) || ( id == ICUB_ID_EYES_RIGHT_LR)){
             //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);
         }else{
             store_joint(id, joint_target[e]);
+        }
+    }
     //! set the target position of a 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;
         //ipos->positionMove(id, value);
+    }
     //! execute a move in position mode
     //! \param id of joint
     //! \param angle
     void iCubJointInterface::set_target_in_positionmode(int id, double value){
         if (id>ICUB_ID_EYES_VERGENCE){
             printf("> ERROR: set_target_positionmode(id=%d, %3.2f) not supported for this id\n",id,value);
             return;
+        }
     #if 0
         //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 = value - target_angle_previous[id];
         //make the motion smooth: we want to reach 85% of the target in the next iteration:
         //calculate speed for that:
         double speed = 0.85 * distance * ((double)MAIN_LOOP_FREQUENCY);
         //set up speed for controller:
         ipos->setRefSpeed(id, speed);
     #endif
         //execute motion
         ipos->positionMove(id, value);
+    }
     //! execute a move in velocity mode
     //! \param id of joint
     //! \param angle
     void iCubJointInterface::set_target_in_velocitymode(int id, double value){
         //first: calculate necessary speed to reach the given target within the next clock tick:
         double distance = value - target_angle_previous[id];
         //make the motion smooth: we want to reach 85% of the target in the next iteration:
         distance = 0.85 * distance;
         //calculate speed
         double speed = distance * ((double)MAIN_LOOP_FREQUENCY);
         //execute:
         ivel->velocityMove(id, speed);
         //if (id == ICUB_ID_NECK_PAN) printf("> VEL now=%3.2f target=%3.2f --> dist=%3.2f speed=%3.2f\n",target_angle_previous[id],value,distance,speed);
         target_angle_previous[id] = get_ts_position(convert_motorid_to_enum(id)).get_newest_value();
+    }
     //! actually execute the scheduled motion commands
     void iCubJointInterface::execute_motion(){
         // set up neck and eye motion commands:
         if (POSITION_CONTROL){
             //position control
             for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){
                 set_target_in_positionmode(i, target_angle[i]);
+            }
         }else{
             //velocity control
             for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){
                 set_target_in_velocitymode(i, target_angle[i]);
+            }
+        }
         //printf("> TARGET PAN = %3.2f\n",target_angle[ICUB_ID_NECK_PAN]);
         //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]);
         //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);
         //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].write();
         //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 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){
         //store joint based on id:
         switch(id){
             default:
                 printf("> ERROR: unhandled joint id %d\n",id);
                 return;
             case(100):
                 JointInterface::store_incoming_position(ID_EYES_RIGHT_LID_UPPER, lid_angle, timestamp);
                 break;
             case(2):
                 //PAN is inverted!
                 JointInterface::store_incoming_position(ID_NECK_PAN, -value, timestamp);
                 break;
             case(0):
                 JointInterface::store_incoming_position(ID_NECK_TILT, value, timestamp);
                 break;
             case(1):
                 JointInterface::store_incoming_position(ID_NECK_ROLL, value, timestamp);
                 break;
             case(3):
                 JointInterface::store_incoming_position(ID_EYES_BOTH_UD, value, timestamp);
                 break;
             //icub handles eyes differently, we have to set pan angle + vergence
             case(4): {//pan
                 last_pos_eye_pan = value;
                 float left  = last_pos_eye_pan + last_pos_eye_vergence/2.0;
                 float right = last_pos_eye_pan - last_pos_eye_vergence/2.0;
                 //printf("> eye: pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_pos_eye_pan, last_pos_eye_vergence, left, right);
                 JointInterface::store_incoming_position(ID_EYES_LEFT_LR, left, timestamp);
                 JointInterface::store_incoming_position(ID_EYES_RIGHT_LR, right, timestamp);
                 break;
+            }
             case(5): { //vergence
                 last_pos_eye_vergence = value;
                 float left  = last_pos_eye_pan + last_pos_eye_vergence/2.0;
                 float right = last_pos_eye_pan - last_pos_eye_vergence/2.0;
                 //printf("> eye: pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_pos_eye_pan, last_pos_eye_vergence, left, right);
                 JointInterface::store_incoming_position(ID_EYES_LEFT_LR, left, timestamp);
                 JointInterface::store_incoming_position(ID_EYES_RIGHT_LR, right, timestamp);
                 break;
+            }
+        }
+    }
     //! 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(2):
                 JointInterface::store_incoming_speed(ID_NECK_PAN, value, timestamp);
                 break;
             case(0):
                 JointInterface::store_incoming_speed(ID_NECK_TILT, value, timestamp);
                 break;
             case(1):
                 JointInterface::store_incoming_speed(ID_NECK_ROLL, value, timestamp);
                 break;
             case(3):
                 JointInterface::store_incoming_speed(ID_EYES_BOTH_UD, value, timestamp);
                 break;
             //icub handles eyes differently, we have to set pan angle + vergence
             case(4): {//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(5): { //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;
+            }
+        }
     /*
         JointInterface::store_incoming_speed(ID_LIP_LEFT_UPPER, 0.0, timestamp);
         JointInterface::store_incoming_speed(ID_LIP_LEFT_LOWER, 0.0, timestamp);
         JointInterface::store_incoming_speed(ID_LIP_CENTER_UPPER, 0.0, timestamp);
         JointInterface::store_incoming_speed(ID_LIP_CENTER_LOWER, 0.0, timestamp);
         JointInterface::store_incoming_speed(ID_LIP_RIGHT_UPPER, 0.0, timestamp);
         JointInterface::store_incoming_speed(ID_LIP_RIGHT_LOWER, 0.0, timestamp);
         JointInterface::store_incoming_speed(ID_EYES_LEFT_LID_LOWER, 0.0, timestamp);
         JointInterface::store_incoming_speed(ID_EYES_LEFT_LID_UPPER, 0.0, timestamp);
         JointInterface::store_incoming_speed(ID_EYES_LEFT_BROW, 0.0, timestamp);
         JointInterface::store_incoming_speed(ID_EYES_RIGHT_LID_LOWER, 0.0, timestamp);
         JointInterface::store_incoming_speed(ID_EYES_RIGHT_LID_UPPER,0.0, timestamp);
         JointInterface::store_incoming_speed(ID_EYES_RIGHT_BROW, 0.0, timestamp);*/
         /*
         //fetch enum id:
         int e = convert_motorid_to_enum(device->get_device_id());
         if (e == -1){
             return; //we are not interested in that data, so we just return here
+        }
         JointInterface::store_incoming_speed(e, device->get_register(XSC3_REGISTER_MOTORSPEED), timestamp);*/
+    }
     /*
     //! conversion table for humotion motor ids to our ids:
     //! \param enum from JointInterface::JOINT_ID_ENUM
     //! \return int value of motor id
     int HumotionJointInterface::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;
+        }
         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 HumotionJointInterface::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;
+    }
     */
     //! prepare and enable a joint
     //! NOTE: this should also prefill the min/max positions for this joint
     //! \param the enum id of a joint
     void iCubJointInterface::enable_joint(int e){
         //FIXME ADD THIS:
         // enable the amplifier and the pid controller on each joint
         /*for (i = 0; i < nj; i++) {
            amp->enableAmp(i);
            pid->enablePid(i);
         }*/
         //set up smooth motion controller
         //step1: set up framerate
         //dev->set_register_blocking(XSC3_REGISTER_PID_RAMP, framerate, true);
         //step2: set controllertype:
         //printf("> activating smooth motion control for joint id 0x%02X (%s)\n", motor_id, joint_name.c_str());
         //dev->set_register_blocking(XSC3_REGISTER_PID_CONTROLLER, XSC3_PROTOCOL_PID_CONTROLLERTYPE_SMOOTH_PLAYBACK, true);
         //step3: set pid controller:
         /*if ((e == ID_LIP_LEFT_UPPER) || (e == ID_LIP_LEFT_LOWER) || (e == ID_LIP_CENTER_UPPER) || (e == ID_LIP_CENTER_LOWER) || (e == ID_LIP_RIGHT_UPPER) || (e == ID_LIP_RIGHT_LOWER)){
             printf("> fixing PID i controller value for smooth motion (FIXME: restore old value!!)\n");
             dev->set_register_blocking(XSC3_REGISTER_CONST_I, 10, true);
         }*/
         //uint16_t result = dev->get_register_blocking_raw(XSC3_REGISTER_PID_CONTROLLER);
         //check if setting pid controllertype was successfull:
         /*if (result != XSC3_PROTOCOL_PID_CONTROLLERTYPE_SMOOTH_PLAYBACK){
             printf("> failed to set smooth motion controller for joint %s (res=0x%04X)\n",joint_name.c_str(),result);
             exit(1);
         }*/
         //fetch min/max:
        // init_joint(e);
         //ok fine, now enable motor:
         //printf("> enabling motor %s\n", joint_name.c_str());
         //dev->set_register_blocking(XSC3_REGISTER_STATUS, XSC3_PROTOCOL_STATUS_BITMASK_MOTOR_ENABLE, true);
+    }
     void iCubJointInterface::store_min_max(IControlLimits *ilimits, int id, int e){
         double min, max;
         ilimits->getLimits(id, &min, &max);
         joint_min[e] = min;
         joint_max[e] = max;
+    }
     //! initialise a joint (set up controller mode etc)
     //! \param joint enum
     void iCubJointInterface::init_joints(){
         store_min_max(ilimits, 0, ID_NECK_TILT);
         store_min_max(ilimits, 1, ID_NECK_ROLL);
         store_min_max(ilimits, 2, ID_NECK_PAN);
         store_min_max(ilimits, 3, 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(4, &pan_min, &pan_max);
         ilimits->getLimits(5, &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_EYES_LEFT_LR] = pan_max; // - vergence_max/2;
         joint_min[ID_EYES_RIGHT_LR] = joint_min[ID_EYES_LEFT_LR];
         joint_max[ID_EYES_RIGHT_LR] = joint_max[ID_EYES_LEFT_LR];
         //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];
         //eyebrows:
         joint_min[ID_EYES_LEFT_BROW] = -50;
         joint_max[ID_EYES_LEFT_BROW] = 50;
         joint_min[ID_EYES_RIGHT_BROW] = joint_min[ID_EYES_LEFT_BROW];
         joint_max[ID_EYES_RIGHT_BROW] = joint_max[ID_EYES_LEFT_BROW];
         //mouth:
         joint_min[ID_LIP_CENTER_UPPER] = 5;
         joint_max[ID_LIP_CENTER_UPPER] = 50;
         joint_min[ID_LIP_CENTER_LOWER] = 5;
         joint_max[ID_LIP_CENTER_LOWER] = 50;
         joint_min[ID_LIP_LEFT_UPPER] = 5;
         joint_max[ID_LIP_LEFT_UPPER] = 50;
         joint_min[ID_LIP_LEFT_LOWER] = 5;
         joint_max[ID_LIP_LEFT_LOWER] = 50;
         joint_min[ID_LIP_RIGHT_UPPER] = 5;
         joint_max[ID_LIP_RIGHT_UPPER] = 50;
         joint_min[ID_LIP_RIGHT_LOWER] = 5;
         joint_max[ID_LIP_RIGHT_LOWER] = 50;
+    }
     //! shutdown and disable a joint
     //! \param the enum id of a joint
     void iCubJointInterface::disable_joint(int e){
         /*
             //first: convert humotion enum to our enum:
             int motor_id = convert_enum_to_motorid(e);
             if (motor_id == -1){
                 return; //we are not interested in that data, so we just return here
+            }
             //fetch device:
             Device *dev = get_device(motor_id);
             printf("> FIXME: ADD DISABLE CODE\n");
             printf("> FIXME: ADD DISABLE CODE\n");
             printf("> FIXME: ADD DISABLE CODE\n");
             printf("> FIXME: ADD DISABLE CODE\n");
             printf("> FIXME: ADD DISABLE CODE\n");
             printf("> FIXME: ADD DISABLE CODE\n");
             printf("> FIXME: ADD DISABLE CODE\n");
             printf("> FIXME: ADD DISABLE CODE\n");
             printf("> FIXME: ADD DISABLE CODE\n");
             */
+    }
     #endif

Also available in: Unified diff