/ - Diff - humotion - Research for Cognitive Interaction

Revision 372eec67

         // 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_;
-...
         //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);

     #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
-...
         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");
-...
         // 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_);
-...
     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
-...
         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);
-...
     //! 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
-...
+    }
     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;
-...
             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);
-...
         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;
-...
         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;

Also available in: Unified diff