commit 372eec673f44e570ade38cc6a2da5c95c18dcc87
Author: Simon Schulz <sschulz@cit-ec.uni-bielefeld.de>
Date:   Thu Feb 11 12:52:48 2016 +0100

    removed pos control

diff --git a/examples/yarp_icub/include/icub_jointinterface.h b/examples/yarp_icub/include/icub_jointinterface.h
index 8eabcdf..5193602 100644
--- a/examples/yarp_icub/include/icub_jointinterface.h
+++ b/examples/yarp_icub/include/icub_jointinterface.h
@@ -73,7 +73,7 @@ private:
 
     // yarp views
     yarp::dev::IVelocityControl *yarp_ivel_;
-    yarp::dev::IPositionControl *yarp_ipos_;
+    //yarp::dev::IPositionControl *yarp_ipos_;
 
     //yarp::dev::IEncodersTimed *yarp_iencs_;
     yarp::dev::IControlLimits *yarp_ilimits_;
@@ -110,15 +110,14 @@ private:
     //yarp::sig::Vector velocities;
 
 
-    void store_min_max(yarp::dev::IControlLimits *ilimits, int id, int e);
+    void store_min_max(yarp::dev::IControlLimits *ilimits, int icub_id);
 
    //float last_pos_eye_vergence;
     //float last_pos_eye_pan;
    // float last_vel_eye_vergence;
     //float last_vel_eye_pan;
 
-    void store_joint(int id, float value);
-    void set_target_in_positionmode(int id);
+    //void store_joint(int id, float value);
     void set_target_in_velocitymode(int id);
 
 
diff --git a/examples/yarp_icub/src/icub_jointinterface.cpp b/examples/yarp_icub/src/icub_jointinterface.cpp
index d8e7443..c1b82f9 100644
--- a/examples/yarp_icub/src/icub_jointinterface.cpp
+++ b/examples/yarp_icub/src/icub_jointinterface.cpp
@@ -2,26 +2,13 @@
 #include "icub_faceinterface.h"
 
 #include <yarp/os/Property.h>
-using namespace yarp::dev;
-using namespace yarp::sig;
-using namespace yarp::os;
-using namespace std;
-/*running:
-/media/local_data/sschulz/iros15/icub-nightly/share/iCub/contexts/simConfig:> iCub_SIM
-/media/local_data/sschulz/iros15/icub-nightly/share/iCub/contexts/simFaceExpressions:> ../../../../bin/simFaceExpressions
-yarp connect /face/eyelids /icubSim/face/eyelids
-yarp connect /face/image/out /icubSim/texture/face
-
-TEST: yarp write /writer /icubSim/face/raw/in
-
-http://wiki.icub.org/wiki/Motor_control
-*/
-
-#define POSITION_CONTROL 0
+
 using std::cout;
+using std::cerr;
+using std::string;
 
 //! constructor
-iCubJointInterface::iCubJointInterface(string _scope) : humotion::server::JointInterface(){
+iCubJointInterface::iCubJointInterface(string _scope) : humotion::server::JointInterface() {
     scope = _scope;
 
     // add mappings from icub ids to humotion ids
@@ -34,7 +21,7 @@ iCubJointInterface::iCubJointInterface(string _scope) : humotion::server::JointI
     face_interface_ = new iCubFaceInterface(scope);
 
     // intantiate the polydriver
-    Property options;
+    yarp::os::Property options;
     options.put("device", "remote_controlboard");
     options.put("local", "/local/head");
     options.put("remote", scope + "/head");
@@ -43,7 +30,7 @@ iCubJointInterface::iCubJointInterface(string _scope) : humotion::server::JointI
     // fetch yarp views:
     bool success = true;
     //success &= yarp_polydriver_.view(yarp_iencs_);
-    success &= yarp_polydriver_.view(yarp_ipos_);
+    //success &= yarp_polydriver_.view(yarp_ipos_);
     success &= yarp_polydriver_.view(yarp_ivel_);
     success &= yarp_polydriver_.view(yarp_ilimits_);
     success &= yarp_polydriver_.view(yarp_pid_);
@@ -70,28 +57,14 @@ iCubJointInterface::~iCubJointInterface(){
 void iCubJointInterface::init_controller() {
     int number_of_joints;
 
-    //set position mode:
-    if (POSITION_CONTROL){
-        // 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{
-        // 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();
-    }
-
+    // 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();
 }
 
 //! initialise icub joint id to humotion joint id mappings
@@ -205,26 +178,10 @@ void iCubJointInterface::store_icub_joint_target(int icub_id, float position, fl
     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) {
-    assert(POSITION_CONTROL);
-    assert(id<=ICUB_ID_EYES_VERGENCE);
-
-    // fetch the current target position
-    float target = target_angle_[id];
-
-    // execute motion as position control cmd
-    yarp_ipos_->positionMove(id, target);
-}
-
 //! execute a move in velocity mode
 //! \param id of joint
 //! \param angle
 void iCubJointInterface::set_target_in_velocitymode(int icub_id) {
-    assert(!POSITION_CONTROL);
-
     // fetch humotion id from icub joint id
     int humotion_id = convert_icub_jointid_to_humotion(icub_id);
 
@@ -283,20 +240,10 @@ void iCubJointInterface::set_target_in_velocitymode(int icub_id) {
 //! 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);
-        }
-    }else{
-        //velocity control
-        for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){
-            set_target_in_velocitymode(i);
-        }
+    // set up neck and eye motion commands in velocitymode
+    for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){
+        set_target_in_velocitymode(i);
     }
-    //printf("> TARGET PAN = %3.2f\n",target_angle[ICUB_ID_NECK_PAN]);
-
 
     // eyelids: unfortuantely the icub has only 1dof for eyelids
     // therefore we can only use an opening value
@@ -327,9 +274,9 @@ void iCubJointInterface::execute_motion(){
 }
 
 
-void iCubJointInterface::set_joint_enable_state(int e, bool enable) {
-    int icub_jointid = -1;
-
+void iCubJointInterface::set_joint_enable_state(int humotion_id, bool enable) {
+    int icub_jointid = convert_humotion_jointid_to_icub(humotion_id);
+/*
     switch(e){
         default:
             break;
@@ -360,8 +307,9 @@ void iCubJointInterface::set_joint_enable_state(int e, bool enable) {
         icub_jointid = ICUB_ID_EYES_VERGENCE;
         break;
     }
+    */
 
-    if (icub_jointid != -1) {
+    if ((icub_jointid != -1) && (icub_jointid <= ICUB_ID_EYES_VERGENCE)) {
         if (enable) {
             yarp_amp_->enableAmp(icub_jointid);
             yarp_pid_->enablePid(icub_jointid);
@@ -385,20 +333,21 @@ void iCubJointInterface::disable_joint(int e){
     set_joint_enable_state(e, false);
 }
 
-void iCubJointInterface::store_min_max(IControlLimits *ilimits, int id, int e){
+void iCubJointInterface::store_min_max(yarp::dev::IControlLimits *ilimits, int icub_id){
     double min, max;
-    ilimits->getLimits(id, &min, &max);
-    joint_min[e] = min;
-    joint_max[e] = max;
+    int humotion_id = convert_icub_jointid_to_humotion(icub_id);
+
+    ilimits->getLimits(icub_id, &min, &max);
+    joint_min[humotion_id] = min;
+    joint_max[humotion_id] = max;
 }
 
 //! initialise a joint (set up controller mode etc)
-//! \param joint enum
 void iCubJointInterface::init_joints(){
-    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);
+    store_min_max(yarp_ilimits_, ICUB_ID_NECK_TILT);
+    store_min_max(yarp_ilimits_, ICUB_ID_NECK_ROLL);
+    store_min_max(yarp_ilimits_, ICUB_ID_NECK_PAN);
+    store_min_max(yarp_ilimits_, ICUB_ID_EYES_BOTH_UD);
 
     //icub handles eyes differently, we have to set pan angle + vergence
     double pan_min, pan_max, vergence_min, vergence_max;
@@ -444,7 +393,7 @@ 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 (" << humotion_id << ") given. can not convert this. exiting\n";
+        cerr << "ERROR: invalid humotion joint id (" << humotion_id << ") given. can not convert this. exiting\n";
         exit(EXIT_FAILURE);
     }
     return it->second;
