commit 14831938f3c79ad335cf791fa802f988bc8aa5b9
Author: sschulz <sschulz@techfak.uni-bielefeld.de>
Date:   Thu Feb 25 17:28:36 2016 +0100

    added emotion ani

diff --git a/examples/yarp_icub/include/humotion_yarp_icub/icub_jointinterface.h b/examples/yarp_icub/include/humotion_yarp_icub/icub_jointinterface.h
index 674c589..a68ac22 100644
--- a/examples/yarp_icub/include/humotion_yarp_icub/icub_jointinterface.h
+++ b/examples/yarp_icub/include/humotion_yarp_icub/icub_jointinterface.h
@@ -94,6 +94,7 @@ class iCubJointInterface : public humotion::server::JointInterface{
     void execute_motion();
 
  private:
+    bool running_in_simulation_;
     yarp::dev::PolyDriver yarp_polydriver_;
 
     std::vector<double> pv_mix_last_error_;
@@ -102,6 +103,7 @@ class iCubJointInterface : public humotion::server::JointInterface{
 
     // yarp views
     yarp::dev::IVelocityControl *yarp_ivel_;
+    yarp::dev::IPositionControl *yarp_ipos_;
     yarp::dev::IControlLimits *yarp_ilimits_;
     yarp::dev::IAmplifierControl *yarp_amp_;
     yarp::dev::IPidControl *yarp_pid_;
diff --git a/examples/yarp_icub/src/icub_jointinterface.cpp b/examples/yarp_icub/src/icub_jointinterface.cpp
index 81fd19a..4a12586 100644
--- a/examples/yarp_icub/src/icub_jointinterface.cpp
+++ b/examples/yarp_icub/src/icub_jointinterface.cpp
@@ -39,6 +39,15 @@ using std::string;
 iCubJointInterface::iCubJointInterface(string _scope) : humotion::server::JointInterface() {
     scope = _scope;
 
+    // running in sim?
+    if (scope == "/icubSim") {
+        cout << "> icub simulation detected\n";
+        running_in_simulation_ = true;
+    } else {
+        running_in_simulation_ = false;
+    }
+
+
     // add mappings from icub ids to humotion ids
     init_id_map();
 
@@ -58,7 +67,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_);
@@ -79,22 +88,38 @@ iCubJointInterface::iCubJointInterface(string _scope) : humotion::server::JointI
 
 //! destructor
 iCubJointInterface::~iCubJointInterface() {
-    // stop velocity controller on exit
+    // stop controller on exit
     yarp_ivel_->stop();
+    yarp_ipos_->stop();
 }
 
 //! init the controller that allows to write target angles or velocities
 void iCubJointInterface::init_controller() {
     int number_of_joints;
 
-    // use position controller, first fetch no of axes:
-    yarp_ivel_->getAxes(&number_of_joints);
+    if (running_in_simulation_) {
+        // running in simulation, use position control mode as velocity seems unsupported right now
+        yarp_ipos_->getAxes(&number_of_joints);
+
+        // prepare to set ref accel
+        // yarp_commands_.resize(number_of_joints);
+        // yarp_commands_ = 200000.0;
+        // yarp_ipos_->setRefAccelerations(yarp_commands_.data());
 
-    // set ref acceleration to a value for all axes:
-    yarp_commands_.resize(number_of_joints);
-    yarp_commands_ = 1e6;
-    yarp_ivel_->setRefAccelerations(yarp_commands_.data());
-    yarp_ivel_->setVelocityMode();
+        // enable position control
+        yarp_ipos_->setPositionMode();
+    } else {
+        // use velocity 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_ = 1e6;
+        yarp_ivel_->setRefAccelerations(yarp_commands_.data());
+
+        // finally enable velocity control mode
+        yarp_ivel_->setVelocityMode();
+    }
 }
 
 //! initialise icub joint id to humotion joint id mappings
@@ -302,14 +327,29 @@ void iCubJointInterface::set_target_in_velocitymode(int icub_id) {
          << position_error << " "
          << " PID" << icub_id << "\n";*/
 
+
+    if (icub_id > ICUB_ID_EYES_VERGENCE) {
+        cerr << "> ERROR: set_target_positionmode(id=" << icub_id << ", ...) not supported\n";
+        return;
+    }
+
+
     // execute velocity move
     bool success;
     int count = 0;
     do {
-        success = yarp_ivel_->velocityMove(icub_id, pv_mix_velocity);
+        if (running_in_simulation_) {
+            // running in sim, use position control
+            success = yarp_ipos_->positionMove(icub_id, target_angle_[icub_id]);
+        } else {
+            // use smooth velocity control on real robot
+            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";
+            cerr << "ERROR: failed to send motion command! gave up after 10 trials\n";
             yarp_ivel_->stop();
+            yarp_ipos_->stop();
             exit(EXIT_FAILURE);
         }
     } while (!success);
@@ -393,12 +433,15 @@ void iCubJointInterface::set_joint_enable_state(int humotion_id, bool enable) {
     */
 
     if ((icub_jointid != -1) && (icub_jointid <= ICUB_ID_EYES_VERGENCE)) {
-        if (enable) {
-            yarp_amp_->enableAmp(icub_jointid);
-            yarp_pid_->enablePid(icub_jointid);
-        } else {
-            yarp_pid_->disablePid(icub_jointid);
-            yarp_amp_->disableAmp(icub_jointid);
+        if (!running_in_simulation_) {
+            // only set amp status on real robot. simulation crashes during this call ... :-X
+            if (enable) {
+                yarp_amp_->enableAmp(icub_jointid);
+                yarp_pid_->enablePid(icub_jointid);
+            } else {
+                yarp_pid_->disablePid(icub_jointid);
+                yarp_amp_->disableAmp(icub_jointid);
+            }
         }
     }
 }
