/ - Diff - humotion - Research for Cognitive Interaction

         void run();
     private:
         void store_incoming_position(int icub_id, double value, double timestamp);
         void store_incoming_velocity(int icub_id, double velocity, double timestamp);
         yarp::sig::Vector calculate_velocities(yarp::sig::Vector positions,
                                   yarp::sig::Vector timestamps);
         float target_eye_pan_;
         float target_eye_vergence_;
         yarp::sig::Vector yarp_positions_;
         yarp::sig::Vector yarp_commands_;
         yarp::sig::Vector yarp_timestamps_;
         float target_eye_pan_velocity_;
         float target_eye_vergence_velocity_;
         yarp::sig::Vector positions_;
         yarp::sig::Vector timestamps_;
         yarp::sig::Vector velocities_;
         yarp::sig::Vector commands_;
         yarp::sig::Vector previous_positions_;
         yarp::sig::Vector previous_timestamps_;
         iCubJointInterface *icub_jointinterface_;
         yarp::dev::IEncodersTimed *yarp_iencs_;
         yarp::dev::IEncodersTimed *iencs_;
     };

         iCubJointInterface(std::string scope);
         ~iCubJointInterface();
         void store_incoming_position(int humotion_id, double position, double timestamp);
         //void store_incoming_position(int humotion_id, double position, double timestamp);
         //void store_incoming_velocity(int humotion_id, double position, double timestamp);
         void run();
         int convert_icub_jointid_to_humotion(int id);

     #include "icub_data_receiver.h"
     #include <humotion/server/joint_interface.h>
     #include <yarp/os/Property.h>
     #include <boost/format.hpp>
     //using namespace yarp::dev;
     //using namespace yarp::sig;
     //using namespace yarp::os;
-...
     using yarp::dev::IEncodersTimed;
     using yarp::sig::Vector;
     #define ICUB_DATA_RECEIVER_USE_ENCODERSPEED 0
     iCubDataReceiver::iCubDataReceiver(int period, iCubJointInterface *icub_jointinterface)
         : yarp::os::RateThread(period) {
         // store pointer to icub jointinterface
         icub_jointinterface_ = icub_jointinterface;
         //fetch yarp iencs view:
         //fetch iencs view from yarp
         yarp::dev::PolyDriver *poly_driver = icub_jointinterface->get_yarp_polydriver();
         bool success = poly_driver->view(yarp_iencs_);
         bool success = poly_driver->view(iencs_);
         if (!success) {
             cout << "ERROR: polydriver failed to init iencs view\n";
             exit(EXIT_FAILURE);
+        }
         // resize data storage vectors to match the number of axes:
         // resize data storage vectors to match the number of axes
         int joints;
         yarp_iencs_->getAxes(&joints);
         yarp_positions_.resize(joints);
         yarp_timestamps_.resize(joints);
         iencs_->getAxes(&joints);
         positions_.resize(joints);
         velocities_.resize(joints);
         timestamps_.resize(joints);
+    }
     bool iCubDataReceiver::threadInit() {
-...
     void iCubDataReceiver::threadRelease() {
+    }
     yarp::sig::Vector iCubDataReceiver::calculate_velocities(yarp::sig::Vector positions,
                                                              yarp::sig::Vector timestamps) {
         yarp::sig::Vector velocities;
         velocities.resize(positions.size());
         cout << "\n";
         if (previous_positions_.size() == 0){
             // first run, no valid old position available, return zero velocities
             // by setting all all elements to zero
             velocities = 0.0;
         }else{
             // calculate speed based on positions:
             for(int i=0; i<positions.size(); i++){
                 float diff = positions[i] - previous_positions_[i];
                 float timediff = timestamps[i] - previous_timestamps_[i];
                 // calc speed:
                 velocities[i] = diff / timediff;
                 cout << " [" << i << "]=" <<  boost::format("%6.3f") % velocities[i];
+            }
+        }
         cout << " VEL\n";
         previous_positions_ = positions;
         previous_timestamps_ = timestamps;
         return velocities;
+    }
     void iCubDataReceiver::run() {
         float velocity;
         //grab pos+vel data:
         yarp_iencs_->getEncodersTimed(yarp_positions_.data(), yarp_timestamps_.data());
         //iencs->getEncoderSpeeds(velocities.data());
         iencs_->getEncodersTimed(positions_.data(), timestamps_.data());
         //publish data to humotion
         for(int i=0; i<yarp_positions_.size(); i++){
             store_incoming_position(i, yarp_positions_[i], yarp_timestamps_[i]);
         #if ICUB_DATA_RECEIVER_USE_ENCODERSPEED
         // fetch data from icub. NOTE: make sure to enable the vel broadcast in the ini file!
         iencs_->getEncoderSpeeds(velocities_.data());
         #else
         // manually calculate the speed based on old position:
         velocities_ = calculate_velocities(positions_, timestamps_);
         #endif
         // publish data to humotion
         for(int i=0; i<positions_.size(); i++){
             // store position values
             store_incoming_position(i, positions_[i], timestamps_[i]);
             // store velocity
             store_incoming_velocity(i, velocities_[i], timestamps_[i]);
+        }
         printf("\n");
         //small hack to tell humotion to update the lid angle
         //fixme: use real id
         store_incoming_position(100, 0.0, yarp_timestamps_[0]);
         store_incoming_position(100, 0.0, timestamps_[0]);
+    }
     void iCubDataReceiver::store_incoming_position(int icub_id, double value, double timestamp) {
         cout << "iCubDataReceiver::store_incoming_position(icubid=" << icub_id << ", " << value << "..)\n";
         cout << "iCubDataReceiver::store_incoming_position(icub=" << icub_id << ", " << value << ")\n";
         // store joint position in humotion backend
         if ((icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) ||
-...
             icub_jointinterface_->store_incoming_position(humotion_id, value, timestamp);
+        }
+    }
     void iCubDataReceiver::store_incoming_velocity(int icub_id, double velocity, double timestamp) {
         cout << "iCubDataReceiver::store_incoming_velocity(icub=" << icub_id << ", " << velocity << ")\n";
         // store joint position in humotion backend
         if ((icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) ||
                 (icub_id == iCubJointInterface::ICUB_ID_EYES_VERGENCE)) {
             // the icub handles eyes differently
             // instead of using seperate left/right pan the icub uses
             // a combined pan angle and vergence. therfore we have to convert this here:
             if (icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) {
                 target_eye_pan_velocity_ = velocity;
             } else {
                 target_eye_vergence_velocity_ = -velocity;
+            }
             float left  = target_eye_pan_velocity_ + target_eye_vergence_velocity_/2.0;
             float right = target_eye_pan_velocity_ - target_eye_vergence_velocity_/2.0;
             icub_jointinterface_->store_incoming_velocity(JointInterface::ID_EYES_LEFT_LR,
                                                          left, timestamp);
             icub_jointinterface_->store_incoming_velocity(JointInterface::ID_EYES_RIGHT_LR,
                                                          right, timestamp);
         } else if (icub_id == 100) {
             //HACK
             //icub_jointinterface->store_incoming_position(ID_EYES_RIGHT_LID_UPPER,
             //                                             lid_angle, timestamp);
         } else {
             if (icub_id == iCubJointInterface::ID_NECK_PAN) {
                 // icub uses an inverted neck pan specification
                 velocity = -velocity;
+            }
             // known configured mapping between joint ids
             int humotion_id = icub_jointinterface_->convert_icub_jointid_to_humotion(icub_id);
             icub_jointinterface_->store_incoming_velocity(humotion_id, velocity, timestamp);
+        }
+    }

+    }
     //! set the current position of a joint
     //! \param id of joint
     //! \param float value of position
     //! \param double timestamp
     void iCubJointInterface::store_incoming_position(int humotion_id, double position, double timestamp){
         cout << "iCubJointInterface::store_incoming_position(humotionid=" << humotion_id <<
                 ", " << position << ", ...)\n";
         JointInterface::store_incoming_position(humotion_id, position, 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(ICUB_ID_NECK_PAN):
                 //PAN is inverted!
                 JointInterface::store_incoming_position(ID_NECK_PAN, value, timestamp);
                 break;
             case(ICUB_ID_NECK_TILT):
                 JointInterface::store_incoming_position(ID_NECK_TILT, value, timestamp);
                 break;
             case(ICUB_ID_NECK_ROLL):
                 JointInterface::store_incoming_position(ID_NECK_ROLL, value, timestamp);
                 break;
             case(ICUB_ID_EYES_BOTH_UD):
                 JointInterface::store_incoming_position(ID_EYES_BOTH_UD, value, timestamp);
                 break;
             //icub handles eyes differently, we have to set pan angle + vergence
             case(ICUB_ID_EYES_PAN): {//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(ICUB_ID_EYES_VERGENCE): { //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;
+            }
+        }
     }*/
     void iCubJointInterface::set_joint_enable_state(int e, bool enable) {
         int icub_jointid = -1;

         bool get_joint_position_map_empty();
         void store_incoming_position(int joint_id, float position, Timestamp timestamp);
         void store_incoming_velocity(int joint_id, float velocity, Timestamp timestamp);
     protected:
         void store_incoming_position(int joint_id, float position, Timestamp timestamp);
         void store_incoming_speed(int joint_id, float speed, Timestamp timestamp);
         virtual void enable_joint(int id) = 0;
         virtual void disable_joint(int id) = 0;

         //following atomic instructions:
         mutex::scoped_lock scoped_lock(joint_ts_position_map_access_mutex);
         //printf("> humotion: incoming joint position for joint id 0x%02X = %4.2f (ts=%.2f)\n",joint_id,position,timestamp.to_seconds());
         printf("> humotion: incoming joint position for joint id 0x%02X = %4.2f (ts=%.2f)\n",joint_id,position,timestamp.to_seconds());
         joint_ts_position_map[joint_id].insert(timestamp, position);
         incoming_position_count++;
-...
     //! \param joint name
     //! \param speed
     //! \param timestamp when the position was measured
     void JointInterface::store_incoming_speed(int joint_id, float speed, Timestamp timestamp){
     void JointInterface::store_incoming_velocity(int joint_id, float velocity, Timestamp timestamp){
         //lock the tsd_list for this access. by doing this we assure
         //that no other thread accessing this element can diturb the
         //following atomic instructions:
         mutex::scoped_lock scoped_lock(joint_ts_speed_map_access_mutex);
         //printf("> humotion: incoming joint speed for joint id 0x%02X = %4.2f (ts=%.2f)\n",joint_id,speed,timestamp.to_seconds());
         joint_ts_speed_map[joint_id].insert(timestamp, speed);
         //printf("> humotion: incoming joint velocity for joint id 0x%02X = %4.2f (ts=%.2f)\n",joint_id,velocity,timestamp.to_seconds());
         joint_ts_speed_map[joint_id].insert(timestamp, velocity);
+    }
     //! return the timestamped float for the given joints position

humotion

Revision ea29304b