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

Revision 18e9b892 examples/yarp_icub/src/icub_jointinterface.cpp

     //! destructor
     iCubJointInterface::~iCubJointInterface(){
         //stop velocity controller on exit
         yarp_ivel_->stop();
+    }
     //! init the controller that allows to write target angles or velocities
-...
         // set ref acceleration to a value for all axes:
         yarp_commands_.resize(number_of_joints);
         yarp_commands_=300.0;
         yarp_commands_ = 1e6;
         yarp_ivel_->setRefAccelerations(yarp_commands_.data());
         yarp_ivel_->setVelocityMode();
+    }
-...
         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_p_[id] = 15.5;
             pv_mix_pid_d_[id] = 0.3;
             pv_mix_last_error_[id] = 0.0;
+        }
-...
             // 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;
             float target_velocity_vergence = target_velocity_left - target_velocity_right;
             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);
                                     target_position_vergence, target_velocity_vergence);
         }else{
             // convert to icub joint id
             int icub_id = convert_humotion_jointid_to_icub(humotion_id);
-...
     void iCubJointInterface::store_icub_joint_target(int icub_id, float position, float velocity) {
         cout << "store_icub_joint_target(" << icub_id << ", " << position << ", ..)\n";
         if (icub_id == ICUB_ID_NECK_PAN) {
         if ((icub_id == ICUB_ID_NECK_PAN) || (icub_id == ICUB_ID_EYES_VERGENCE)){
             // icub uses an inverted neck pan specification
             position = -position;
             velocity = -velocity;
-...
         // fetch the target velocity
         float target_velocity = target_velocity_[icub_id];
         float vmax = 150.0;
         float vmax = 350.0;
         if (target_velocity > vmax)  target_velocity = vmax;
         if (target_velocity < -vmax) target_velocity = -vmax;
         //execute:
         //ivel->velocityMove(id, 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)) {
         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_PAN)
             && (icub_id != ICUB_ID_EYES_VERGENCE)
             //&& (icub_id != ICUB_ID_NECK_TILT)
                 ){
             // limit to some joints for debugging...
             return;
         }*/
+        }
         // 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
-...
         // first: fetch the timstamp of the last known position
         humotion::Timestamp data_ts = get_ts_position(humotion_id).get_last_timestamp();
         // fetch current position & velocity
         float current_position = get_ts_position(humotion_id).get_interpolated_value(data_ts);
         float current_velocity = get_ts_speed(humotion_id).get_interpolated_value(data_ts);
         // the icub has a combined eye pan actuator, therefore
         // we have to calculate pan angle and vergence based on the left and right target angles:
         if (icub_id == ICUB_ID_EYES_PAN) {
             // fetch timestamp
             data_ts = get_ts_position(ID_EYES_LEFT_LR).get_last_timestamp();
             // get the left and right positions
             float pos_left  = get_ts_position(ID_EYES_LEFT_LR).get_interpolated_value(data_ts);
             float pos_right = get_ts_position(ID_EYES_RIGHT_LR).get_interpolated_value(data_ts);
             current_position = (pos_left + pos_right) / 2.0;
             // same for velocities:
             float vel_left  = get_ts_speed(ID_EYES_LEFT_LR).get_interpolated_value(data_ts);
             float vel_right = get_ts_speed(ID_EYES_RIGHT_LR).get_interpolated_value(data_ts);
             current_velocity = (vel_left + vel_right) / 2.0;
         }else if (icub_id == ICUB_ID_EYES_VERGENCE) {
             // fetch timestamp
             data_ts = get_ts_position(ID_EYES_LEFT_LR).get_last_timestamp();
             // get the left and right positions
             float pos_left  = get_ts_position(ID_EYES_LEFT_LR).get_interpolated_value(data_ts);
             float pos_right = get_ts_position(ID_EYES_RIGHT_LR).get_interpolated_value(data_ts);
             current_position = pos_left - pos_right;
             // same for velocities:
             float vel_left  = get_ts_speed(ID_EYES_LEFT_LR).get_interpolated_value(data_ts);
             float vel_right = get_ts_speed(ID_EYES_RIGHT_LR).get_interpolated_value(data_ts);
             current_velocity = (vel_left - vel_right);
+        }
         // calculate position error:
         float position_error = target_angle_[icub_id] - get_ts_position(humotion_id).get_interpolated_value(data_ts);
         float position_error = target_angle_[icub_id] - current_position;
         // calculate d term
         float error_d = (position_error - pv_mix_last_error_[icub_id]) / (framerate*1000.0);
-...
                                 + pv_mix_pid_p_[icub_id]*error_d
                                 + target_velocity;
         //if (icub_id == ICUB_ID_EYES_VERGENCE) { target_velocity = 0.0; pv_mix_velocity = 0.0;}
         cout << "\n"
              << get_ts_position(humotion_id).get_interpolated_value(data_ts) << " "
              << current_position << " "
              << target_angle_[icub_id] << " "
              << 123.4  << " "
              << current_velocity << " "
              << pv_mix_velocity  << " "
              << target_velocity  << " "
              << position_error << " "
              << icub_id << " PID\n";
              << " PID" << icub_id << "\n";
         // execute velocity move
         yarp_ivel_->velocityMove(icub_id, pv_mix_velocity);
         bool success;
         int count = 0;
         do{
             success = yarp_ivel_->velocityMove(icub_id, pv_mix_velocity);
             if (count++ >= 10 ) {
                 cerr << "ERROR: failed to send velocity move command! gave up after 10 trials\n";
                 yarp_ivel_->stop();
                 exit(EXIT_FAILURE);
+            }
         } while(!success);
+    }
     //! actually execute the scheduled motion commands
-...
         for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){
             set_target_in_velocitymode(i);
+        }
     /*
         // eyelids: unfortuantely the icub has only 1dof for eyelids
         // therefore we can only use an opening value
         float opening_left  = target_angle_[ICUB_ID_EYES_LEFT_LID_UPPER]
-...
         //mouth
         face_interface_->set_mouth(target_angle_);
     */
         //store joint values which we do not handle on icub here:
         double timestamp = humotion::Timestamp::now().to_seconds();

Also available in: Unified diff