SVHFingerManager.cpp
Go to the documentation of this file.
00001 // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
00002 
00003 // -- BEGIN LICENSE BLOCK ----------------------------------------------
00004 // This file is part of the SCHUNK SVH Driver suite.
00005 //
00006 // This program is free software licensed under the LGPL
00007 // (GNU LESSER GENERAL PUBLIC LICENSE Version 3).
00008 // You can find a copy of this license in LICENSE folder in the top
00009 // directory of the source code.
00010 //
00011 // © Copyright 2014 SCHUNK Mobile Greifsysteme GmbH, Lauffen/Neckar Germany
00012 // © Copyright 2014 FZI Forschungszentrum Informatik, Karlsruhe, Germany
00013 //
00014 // -- END LICENSE BLOCK ------------------------------------------------
00015 
00016 //----------------------------------------------------------------------
00032 //----------------------------------------------------------------------
00033 #include <driver_svh/Logging.h>
00034 #include <driver_svh/SVHFingerManager.h>
00035 
00036 #include <icl_core/TimeStamp.h>
00037 
00038 #include <boost/bind/bind.hpp>
00039 
00040 namespace driver_svh {
00041 
00042 SVHFingerManager::SVHFingerManager(const std::vector<bool> &disable_mask, const uint32_t &reset_timeout) :
00043   m_controller(new SVHController()),
00044   m_feedback_thread(),
00045   m_connected(false),
00046   m_connection_feedback_given(false),
00047   m_homing_timeout(10),
00048   m_ticks2rad(0),
00049   m_position_min(eSVH_DIMENSION, 0),
00050   m_position_max(eSVH_DIMENSION, 0),
00051   m_position_home(eSVH_DIMENSION, 0),
00052   m_is_homed(eSVH_DIMENSION, false),
00053   m_is_switched_off(eSVH_DIMENSION,false),
00054   m_movement_state(eST_DEACTIVATED),
00055   m_reset_speed_factor(0.2),
00056   m_reset_timeout(reset_timeout),
00057   m_current_settings(eSVH_DIMENSION),
00058   m_current_settings_given(eSVH_DIMENSION,false),
00059   m_position_settings(eSVH_DIMENSION),
00060   m_position_settings_given(eSVH_DIMENSION,false),
00061   m_home_settings(eSVH_DIMENSION),
00062   m_serial_device("/dev/ttyUSB0")
00063 {
00064 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_
00065   m_ws_broadcaster = boost::shared_ptr<icl_comm::websocket::WsBroadcaster>(new icl_comm::websocket::WsBroadcaster(icl_comm::websocket::WsBroadcaster::eRT_SVH));
00066   if (m_ws_broadcaster)
00067   {
00068     // Register a custom handler for received JSON Messages
00069     m_ws_broadcaster->registerHintCallback(boost::bind(&SVHFingerManager::receivedHintMessage,this,_1));
00070 
00071     m_ws_broadcaster->robot->setInputToRadFactor(1);
00072     m_ws_broadcaster->robot->setHint(eHT_NOT_CONNECTED);
00073     m_ws_broadcaster->sendHints(); // Hints are updated Manually
00074     m_ws_broadcaster->sendState(); // Initial send in case someone is waiting for it
00075   }
00076 #endif
00077 
00078 
00079   // load home position default parameters
00080   setDefaultHomeSettings();
00081 
00082   // set default reset order of all channels
00083   m_reset_order.resize(eSVH_DIMENSION);
00084   m_reset_order[0] = eSVH_INDEX_FINGER_PROXIMAL;
00085   m_reset_order[1] = eSVH_MIDDLE_FINGER_PROXIMAL;
00086   m_reset_order[2] = eSVH_THUMB_OPPOSITION;
00087   m_reset_order[3] = eSVH_THUMB_FLEXION;
00088   m_reset_order[4] = eSVH_FINGER_SPREAD;
00089   m_reset_order[5] = eSVH_MIDDLE_FINGER_DISTAL;
00090   m_reset_order[6] = eSVH_INDEX_FINGER_DISTAL;
00091   m_reset_order[7] = eSVH_RING_FINGER;
00092   m_reset_order[8] = eSVH_PINKY;
00093 
00094   for (size_t i = 0; i < eSVH_DIMENSION; ++i)
00095   {
00096     m_is_switched_off[i] = disable_mask[i];
00097     if (m_is_switched_off[i])
00098     {
00099       LOGGING_INFO_C(DriverSVH, SVHFingerManager, "Joint: " << m_controller->m_channel_description[i] << " was disabled as per user request. It will not do anything!" << endl);
00100 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_
00101       if (m_ws_broadcaster)
00102       {
00103         m_ws_broadcaster->robot->setHint(eHT_CHANNEL_SWITCHED_OF);
00104         m_ws_broadcaster->sendHints(); // Hints are updated Manually
00105       }
00106 #endif
00107     }
00108   }
00109 
00110 
00111 }
00112 
00113 SVHFingerManager::~SVHFingerManager()
00114 {
00115   if (m_connected)
00116   {
00117     disconnect();
00118   }
00119 
00120   if (m_controller != NULL)
00121   {
00122     delete m_controller;
00123     m_controller = NULL;
00124   }
00125 }
00126 
00127 bool SVHFingerManager::connect(const std::string &dev_name,const unsigned int &_retry_count)
00128 {
00129    LOGGING_TRACE_C(DriverSVH, SVHFingerManager, "Finger manager is trying to connect to the Hardware..." << endl);
00130 
00131 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_
00132    // Reset the connection specific hints and give it a go aggain.
00133     if (m_ws_broadcaster)
00134     {
00135       m_ws_broadcaster->robot->clearHint(eHT_NOT_CONNECTED);
00136       m_ws_broadcaster->robot->clearHint(eHT_DEVICE_NOT_FOUND);
00137       m_ws_broadcaster->robot->clearHint(eHT_CONNECTION_FAILED);
00138       m_ws_broadcaster->sendHints(); // Hints are updated Manually
00139     }
00140 #endif
00141 
00142   // Save device handle for next use
00143   m_serial_device = dev_name;
00144 
00145 
00146   if (m_connected)
00147   {
00148     disconnect();
00149   }
00150 
00151   if (m_controller != NULL)
00152   {
00153       if (m_controller->connect(dev_name))
00154       {
00155           unsigned int retry_count=_retry_count;
00156           do {
00157               // Reset the package counts (in case a previous attempt was made)
00158               m_controller->resetPackageCounts();
00159 
00160               // initialize feedback polling thread
00161               m_feedback_thread = new SVHFeedbackPollingThread(icl_core::TimeSpan::createFromMSec(100), this);
00162 
00163               // load default position settings before the fingers are resetted
00164               std::vector<SVHPositionSettings> position_settings = getDefaultPositionSettings(true);
00165 
00166               // load default current settings
00167               std::vector<SVHCurrentSettings> current_settings
00168                       = getDefaultCurrentSettings();
00169 
00170               m_controller->disableChannel(eSVH_ALL);
00171 
00172               // initialize all channels
00173               for (size_t i = 0; i < eSVH_DIMENSION; ++i)
00174               {
00175                   // request controller feedback to have a valid starting point
00176                   m_controller->requestControllerFeedback(static_cast<SVHChannel>(i));
00177 
00178                   // Actually set the new position settings
00179                   m_controller->setPositionSettings(static_cast<SVHChannel>(i), position_settings[i]);
00180 
00181                   // set current settings
00182                   m_controller->setCurrentSettings(static_cast<SVHChannel>(i), current_settings[i]);
00183               }
00184 
00185               // check for correct response from hardware controller
00186               icl_core::TimeStamp start_time = icl_core::TimeStamp::now();
00187               bool timeout = false;
00188               unsigned int received_count = 0;
00189               unsigned int send_count = 0;
00190               while (!timeout && !m_connected)
00191               {
00192                   send_count = m_controller->getSentPackageCount();
00193                   received_count = m_controller->getReceivedPackageCount();
00194                   if (send_count == received_count)
00195                   {
00196                       m_connected = true;
00197                       LOGGING_INFO_C(DriverSVH, SVHFingerManager, "Successfully established connection to SCHUNK five finger hand." << endl
00198                                      << "Send packages = " << send_count << ", received packages = " << received_count << endl);
00199 
00200                   }
00201                   LOGGING_TRACE_C(DriverSVH, SVHFingerManager, "Try to connect to SCHUNK five finger hand: Send packages = " << send_count << ", received packages = " << received_count << endl);
00202 
00203                   // check for timeout
00204                   if ((icl_core::TimeStamp::now() - start_time).tsSec() > m_reset_timeout)
00205                   {
00206                       timeout = true;
00207                       LOGGING_ERROR_C(DriverSVH, SVHFingerManager, "Connection timeout! Could not connect to SCHUNK five finger hand." << endl
00208                                       << "Send packages = " << send_count << ", received packages = " << received_count << endl);
00209 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_
00210                       if (m_ws_broadcaster)
00211                       {
00212                           m_ws_broadcaster->robot->setHint(eHT_CONNECTION_FAILED);
00213                           m_ws_broadcaster->sendHints(); //Hints are updated Manually
00214                       }
00215 #endif
00216 
00217                   }
00218                   icl_core::os::usleep(50000);
00219               }
00220 
00221               // Try Aggain, but ONLY if we at least got one package back, otherwise its futil
00222               if (!m_connected)
00223               {
00224                 if (received_count > 0 && retry_count >= 0)
00225                 {
00226                     retry_count--;
00227                     LOGGING_ERROR_C(DriverSVH, SVHFingerManager, "Connection Failed! Send packages = " << send_count << ", received packages = " << received_count << ". Retrying, count: " << retry_count << endl);
00228                 }
00229                 else
00230                 {
00231                     retry_count = 0;
00232                     LOGGING_ERROR_C(DriverSVH, SVHFingerManager, "Connection Failed! Send packages = " << send_count << ", received packages = " << received_count << ". Not Retrying anymore."<< endl);
00233                 }
00234               }
00235           // Keep trying to reconnect several times because the brainbox often makes problems
00236           } while (!m_connected && retry_count > 0);
00237 
00238 
00239            if (!m_connected && retry_count<= 0)
00240            {
00241              LOGGING_ERROR_C(DriverSVH, SVHFingerManager, "A Stable connection could NOT be made, however some packages where received. Please check the hardware!" << endl);
00242            }
00243 
00244 
00245           if (m_connected)
00246           {
00247 
00248 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_
00249 
00250               if (m_ws_broadcaster)
00251               {
00252                   // Intitial connection, any failures regarding the connection must be gone so we can safely clear them all
00253                   m_ws_broadcaster->robot->clearHint(eHT_CONNECTION_FAILED);
00254                   m_ws_broadcaster->robot->clearHint(eHT_NOT_CONNECTED);
00255                   m_ws_broadcaster->robot->clearHint(eHT_DEVICE_NOT_FOUND);
00256                   // Next up, resetting, so give a hint for that
00257                   m_ws_broadcaster->robot->setHint(eHT_NOT_RESETTED);
00258                   m_ws_broadcaster->sendHints(); // Needs to be called if not done by the feedback polling thread
00259               }
00260 #endif
00261 
00262               // Request firmware information once at the beginning
00263               getFirmwareInfo();
00264               // start feedback polling thread
00265               LOGGING_TRACE_C(DriverSVH, SVHFingerManager, "Finger manager is starting the fedback polling thread" << endl);
00266               if (m_feedback_thread != NULL)
00267               {
00268                   m_feedback_thread->start();
00269               }
00270           }
00271       }
00272       else
00273       {
00274 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_
00275           if (m_ws_broadcaster)
00276           {
00277               m_ws_broadcaster->robot->setHint(eHT_DEVICE_NOT_FOUND);
00278               m_ws_broadcaster->sendHints(); // Hints are updated Manually
00279           }
00280 #endif
00281           LOGGING_ERROR_C(DriverSVH, SVHFingerManager, "Connection FAILED! Device could NOT be opened" << endl);
00282       }
00283   }
00284 
00285   return m_connected;
00286 }
00287 
00288 void SVHFingerManager::disconnect()
00289 {
00290   LOGGING_TRACE_C(DriverSVH, SVHFingerManager, "Finger manager is trying to discoconnect to the Hardware..." << endl);
00291   m_connected = false;
00292   m_connection_feedback_given = false;
00293 
00294   // Disable Polling
00295   if (m_feedback_thread != NULL)
00296   {
00297     // wait until thread has stopped
00298     m_feedback_thread->stop();
00299     m_feedback_thread->join();
00300 
00301     delete m_feedback_thread;
00302     m_feedback_thread = NULL;
00303     LOGGING_TRACE_C(DriverSVH, SVHFingerManager, "Feedback thread terminated" << endl);
00304   }
00305 
00306   // Tell the Controller to terminate the rest
00307   if (m_controller != NULL)
00308   {
00309     m_controller->disconnect();
00310   }
00311 
00312 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_
00313     // Connection hint is always true when no connection is established :)
00314     if (m_ws_broadcaster)
00315     {
00316       m_ws_broadcaster->robot->clearAllHints();
00317       m_ws_broadcaster->robot->setHint(eHT_NOT_CONNECTED);
00318       m_ws_broadcaster->sendHints(); // Hints are Transmitted Manually
00319     }
00320 #endif
00321 
00322 }
00323 
00325 bool SVHFingerManager::resetChannel(const SVHChannel &channel)
00326 {
00327   if (m_connected)
00328   {
00329     // reset all channels
00330     if (channel == eSVH_ALL)
00331     {
00332 
00333       bool reset_all_success = true;
00334       for (size_t i = 0; i < eSVH_DIMENSION; ++i)
00335       {
00336         // try three times to reset each finger
00337         size_t max_reset_counter = 3;
00338         bool reset_success = false;
00339         while (!reset_success && max_reset_counter > 0)
00340         {
00341           SVHChannel channel = static_cast<SVHChannel>(m_reset_order[i]);
00342           reset_success = resetChannel(channel);
00343           max_reset_counter--;
00344         }
00345 
00346         LOGGING_DEBUG_C(DriverSVH, resetChannel, "Channel " << m_reset_order[i] << " reset success = " << reset_success << endl);
00347 
00348         // set all reset flag
00349         reset_all_success = reset_all_success && reset_success;
00350       }
00351 
00352 
00353 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_
00354         // In case we still told the user that this was an issue, it is clearly resolved now.
00355         if (reset_all_success && m_ws_broadcaster)
00356         {
00357           m_ws_broadcaster->robot->clearHint(eHT_RESET_FAILED);
00358           m_ws_broadcaster->robot->clearHint(eHT_NOT_RESETTED);
00359           m_ws_broadcaster->sendHints(); // Hints are Transmitted Manually
00360         }
00361 #endif
00362 
00363       return reset_all_success;
00364     }
00365     else if (channel > eSVH_ALL && eSVH_ALL < eSVH_DIMENSION)
00366     {
00367       // Tell the websockets
00368       MovementState last_movement_state = m_movement_state;
00369       setMovementState(eST_RESETTING);
00370 
00371 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_
00372       if (m_ws_broadcaster)
00373       {
00374         m_ws_broadcaster->robot->setJointEnabled(false,channel);
00375         m_ws_broadcaster->robot->setJointHomed(false,channel);
00376       }
00377 #endif // _IC_BUILDER_ICL_COMM_WEBSOCKET_
00378 
00379 
00380       LOGGING_DEBUG_C(DriverSVH, SVHFingerManager, "Start homing channel " << channel << endl);
00381 
00382       if (!m_is_switched_off[channel])
00383       {
00384         LOGGING_TRACE_C(DriverSVH, SVHFingerManager, "Setting reset position values for controller of channel " << channel << endl);
00385 
00386         m_controller->setPositionSettings(channel, getDefaultPositionSettings(true)[channel]);
00387 
00388         // reset homed flag
00389         m_is_homed[channel] = false;
00390 
00391         // read default home settings for channel
00392         SVHHomeSettings home = m_home_settings[channel];
00393 
00394         SVHPositionSettings pos_set;
00395         SVHCurrentSettings cur_set;
00396         m_controller->getPositionSettings(channel, pos_set);
00397         m_controller->getCurrentSettings(channel, cur_set);
00398 
00399         // find home position
00400         m_controller->disableChannel(eSVH_ALL);
00401         int32_t position = 0;
00402 
00403         if (home.direction > 0)
00404         {
00405           position = static_cast<int32_t>(pos_set.wmx);
00406         }
00407         else
00408         {
00409           position = static_cast<int32_t>(pos_set.wmn);
00410         }
00411 
00412         LOGGING_INFO_C(DriverSVH, SVHFingerManager, "Driving channel " << channel << " to hardstop. Detection thresholds: Current MIN: "<< home.resetCurrentFactor * cur_set.wmn << "mA MAX: "<< home.resetCurrentFactor * cur_set.wmx <<"mA" << endl);
00413 
00414         m_controller->setControllerTarget(channel, position);
00415         m_controller->enableChannel(channel);
00416 
00417         SVHControllerFeedback control_feedback_previous;
00418         SVHControllerFeedback control_feedback;
00419 
00420         // initialize timeout
00421         icl_core::TimeStamp start_time = icl_core::TimeStamp::now();
00422         icl_core::TimeStamp start_time_log = icl_core::TimeStamp::now();
00423         // Debug helper to just notify about fresh stales
00424         bool stale_notification_sent = false;
00425 
00426         for (size_t hit_count = 0; hit_count < 10; )
00427         {
00428           m_controller->setControllerTarget(channel, position);
00429           //m_controller->requestControllerFeedback(channel);
00430           m_controller->getControllerFeedback(channel, control_feedback);
00431 
00432           // Quite extensive Current output!
00433           if ((icl_core::TimeStamp::now() - start_time_log).milliSeconds() > 100)
00434           {
00435             LOGGING_INFO_C(DriverSVH, SVHFingerManager,"Resetting Channel "<< channel << ":" << m_controller->m_channel_description[channel] << " current: " << control_feedback.current << " mA" << endl);
00436             start_time_log = icl_core::TimeStamp::now();
00437           }
00438 
00439           if ((home.resetCurrentFactor * cur_set.wmn >= control_feedback.current) || (control_feedback.current >= home.resetCurrentFactor * cur_set.wmx))
00440           {
00441             hit_count++;
00442             LOGGING_TRACE_C(DriverSVH, SVHFingerManager,"Resetting Channel "<< channel << ":" << m_controller->m_channel_description[channel] << " Hit Count increased: " << hit_count << endl);
00443           }
00444           else if (hit_count > 0)
00445           {
00446             hit_count--;
00447             LOGGING_TRACE_C(DriverSVH, SVHFingerManager,"Resetting Channel "<< channel << ":" << m_controller->m_channel_description[channel] << " Hit Count Decreased: " << hit_count << endl);
00448           }
00449 
00450           // check for time out: Abort, if position does not change after homing timeout.
00451           if ((icl_core::TimeStamp::now() - start_time).tsSec() > m_homing_timeout)
00452           {
00453             m_controller->disableChannel(eSVH_ALL);
00454             LOGGING_ERROR_C(DriverSVH, SVHFingerManager, "Timeout: Aborted finding home position for channel " << channel << endl);
00455             // Timeout could mean serious hardware issues or just plain wrong settings
00456 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_
00457 
00458             if (m_ws_broadcaster)
00459             {
00460               m_ws_broadcaster->robot->setHint(eHT_RESET_FAILED);
00461               m_ws_broadcaster->sendHints(); // Hints are Transmitted Manually
00462             }
00463 #endif
00464             return false;
00465           }
00466 
00467           // reset time of position changes
00468           if (control_feedback.position != control_feedback_previous.position)
00469           {
00470             start_time = icl_core::TimeStamp::now();
00471             if (stale_notification_sent)
00472             {
00473              LOGGING_TRACE_C(DriverSVH, SVHFingerManager,"Resetting Channel "<< channel << ":" << m_controller->m_channel_description[channel] << " Stale resolved, continuing detection" << endl);
00474              stale_notification_sent = false;
00475             }
00476           }
00477           else
00478           {
00479             if (!stale_notification_sent)
00480             {
00481              LOGGING_TRACE_C(DriverSVH, SVHFingerManager,"Resetting Channel "<< channel << ":" << m_controller->m_channel_description[channel] << " Stale detected. Starting Timeout" << endl);
00482              stale_notification_sent = true;
00483             }
00484           }
00485 
00486           // save previous control feedback
00487           control_feedback_previous = control_feedback;
00488         }
00489 
00490         LOGGING_DEBUG_C(DriverSVH, SVHFingerManager, "Hit counter of " << channel << " reached." << endl);
00491 
00492         m_controller->disableChannel(eSVH_ALL);
00493 
00494         // set reference values
00495         m_position_min[channel] = static_cast<int32_t>(control_feedback.position + home.minimumOffset);
00496         m_position_max[channel] = static_cast<int32_t>(control_feedback.position + home.maximumOffset);
00497         m_position_home[channel] = static_cast<int32_t>(control_feedback.position + home.idlePosition);
00498         LOGGING_DEBUG_C(DriverSVH, SVHFingerManager, "Setting soft stops for Channel " << channel << " min pos = " << m_position_min[channel]
00499                         << " max pos = " << m_position_max[channel] << " home pos = " << m_position_home[channel] << endl);
00500 
00501         position = static_cast<int32_t>(control_feedback.position + home.idlePosition);
00502 
00503         // go to idle position
00504         m_controller->enableChannel(channel);
00505         while (true)
00506         {
00507           m_controller->setControllerTarget(channel, position);
00508           //m_controller->requestControllerFeedback(channel);
00509           m_controller->getControllerFeedback(channel, control_feedback);
00510 
00511           if (abs(position - control_feedback.position) < 1000)
00512           {
00513             break;
00514           }
00515         }
00516         m_controller->disableChannel(eSVH_ALL);
00517 
00518         LOGGING_TRACE_C(DriverSVH, SVHFingerManager, "Restoring default position values for controller of channel " << channel << endl);
00519         m_controller->setPositionSettings(channel, getDefaultPositionSettings(false)[channel]);
00520       }
00521       else
00522       {
00523 
00524          LOGGING_INFO_C(DriverSVH, SVHFingerManager, "Channel " << channel << "switched of by user, homing is set to finished" << endl);
00525       }
00526 
00527 
00528       m_is_homed[channel] = true;
00529 
00530       // Check if this reset has trigger the reset of all the Fingers
00531       bool reset_all_success = true;
00532       for (size_t i = 0; i < eSVH_DIMENSION; ++i)
00533       {
00534         reset_all_success == reset_all_success && m_is_homed[channel];
00535       }
00536 
00537       if (reset_all_success)
00538       {
00539 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_
00540         // In case we still told the user that this was an issue, it is clearly resolved now.
00541         if (m_ws_broadcaster)
00542         {
00543           m_ws_broadcaster->robot->clearHint(eHT_RESET_FAILED);
00544           m_ws_broadcaster->robot->clearHint(eHT_NOT_RESETTED);
00545           m_ws_broadcaster->sendHints(); // Hints are Transmitted Manually
00546         }
00547 #endif
00548         setMovementState(eST_RESETTED);
00549       }
00550       else
00551       {
00552         setMovementState(last_movement_state);
00553       }
00554 
00555 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_
00556       if (m_ws_broadcaster)
00557       {
00558         m_ws_broadcaster->robot->setJointHomed(true,channel);
00559       }
00560 #endif // _IC_BUILDER_ICL_COMM_WEBSOCKET_
00561 
00562       LOGGING_INFO_C(DriverSVH, SVHFingerManager, "Successfully homed channel " << channel << endl);
00563 
00564       return true;
00565     }
00566     else
00567     {
00568       LOGGING_ERROR_C(DriverSVH, SVHFingerManager, "Channel " << channel << " is out of bounds!" << endl);
00569       return false;
00570     }
00571   }
00572   else
00573   {
00574     LOGGING_ERROR_C(DriverSVH, SVHFingerManager, "Could not reset channel " << channel << ": No connection to SCHUNK five finger hand!" << endl);
00575     return false;
00576   }
00577 }
00578 
00579 // enables controller of channel
00580 bool SVHFingerManager::enableChannel(const SVHChannel &channel)
00581 {
00582   if (isConnected() && isHomed(channel))
00583   {
00584     if (channel == eSVH_ALL)
00585     {
00586       for (size_t i = 0; i < eSVH_DIMENSION; ++i)
00587       {
00588         // Just for safety, enable chanels in the same order as we have resetted them (otherwise developers might geht confused)
00589         SVHChannel real_channel = static_cast<SVHChannel>(m_reset_order[i]);
00590         if (!m_is_switched_off[real_channel])
00591         {
00592           // recursion to get the other updates corresponing with activation of a channel
00593           enableChannel(real_channel);
00594         }
00595       }
00596     }
00597     else if (channel > eSVH_ALL && eSVH_ALL < eSVH_DIMENSION)
00598     {
00599       // Note: This part is another one of thise places where the names can lead to confusion. I am sorry about that
00600       // Switched off is a logical term. The user has chosen NOT to use this channel because of hardware trouble.
00601       // To enable a smooth driver behaviour all replys regarding these channels will be answered in the most positive way
00602       // the caller could expect. Enabled refers to the actual enabled state of the hardware controller loops that drive the motors.
00603       // As the user has chosen not to use certain channels we explicitly do NOT enable these but tell a calling driver that we did
00604       if (!m_is_switched_off[channel])
00605       {
00606         m_controller->enableChannel(channel);
00607       }
00608 
00609 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_
00610       if (m_ws_broadcaster)
00611       {
00612         m_ws_broadcaster->robot->setJointEnabled(true,channel);
00613       }
00614 #endif // _IC_BUILDER_ICL_COMM_WEBSOCKET_
00615 
00616       setMovementState(eST_PARTIALLY_ENABLED);
00617       if (isEnabled(eSVH_ALL))
00618       {
00619         setMovementState(eST_ENABLED);
00620       }
00621     }
00622     return true;
00623   }
00624   return false;
00625 }
00626 
00627 void SVHFingerManager::disableChannel(const SVHChannel &channel)
00628 {
00629   if (channel == eSVH_ALL)
00630   {
00631     for (size_t i = 0; i < eSVH_DIMENSION; ++i)
00632     {
00633       disableChannel(static_cast<SVHChannel>(i));
00634     }
00635   }
00636   else
00637   {
00638     if (!m_is_switched_off[channel])
00639     {
00640       m_controller->disableChannel(channel);
00641     }
00642 
00643 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_
00644     if (m_ws_broadcaster)
00645     {
00646       m_ws_broadcaster->robot->setJointEnabled(false,channel);
00647     }
00648 #endif // _IC_BUILDER_ICL_COMM_WEBSOCKET_
00649 
00650     setMovementState(eST_PARTIALLY_ENABLED);
00651 
00652     bool all_disabled = true;
00653     for (size_t i = 0; i < eSVH_DIMENSION; ++i)
00654     {
00655       // Aggain only check channels that are not switched off. Switched off channels will always answer that they are enabled
00656       all_disabled = all_disabled && (m_is_switched_off[channel] ||!isEnabled(static_cast<SVHChannel>(i)));
00657     }
00658     if (all_disabled)
00659     {
00660       setMovementState(eST_DEACTIVATED);
00661     }
00662 
00663   }
00664 }
00665 
00666 bool SVHFingerManager::requestControllerFeedback(const SVHChannel &channel)
00667 {
00668   if (isConnected())
00669   {
00670     m_controller->requestControllerFeedback(channel);
00671     return true;
00672   }
00673 
00674   LOGGING_WARNING_C(DriverSVH, SVHFingerManager, "Feedback for channel " << channel << " could not be requested. FM is not connected to HW." << endl);
00675   return false;
00676 }
00677 
00678 // returns actual position value for given channel
00679 bool SVHFingerManager::getPosition(const SVHChannel &channel, double &position)
00680 {
00681   SVHControllerFeedback controller_feedback;
00682   if ((channel >=0 && channel < eSVH_DIMENSION) && isHomed(channel) && m_controller->getControllerFeedback(channel, controller_feedback))
00683   {
00684     // Switched off channels will always remain at zero position as the tics we get back migh be total gibberish
00685     if (m_is_switched_off[channel])
00686     {
00687       position = 0.0;
00688       return true;
00689     }
00690 
00691     //int32_t cleared_position_ticks = controller_feedback.position;
00692     position = convertTicks2Rad(channel,controller_feedback.position);
00693 
00694     // Safety overwrite: If controller drives to a negative position (should not happen but might in case the soft stops are placed badly)
00695     // we cannot get out because inputs smaller than 0 will be ignored
00696     if (position < 0)
00697     {
00698       position = 0.0;
00699     }
00700 
00701     // DISABLED as the output was realy spamming everything else :)
00702     //LOGGING_TRACE_C(DriverSVH, SVHFingerManager, "Channel " << channel << ": position_ticks = " << controller_feedback.position
00703     //                << " | cleared_position_ticks = " << cleared_position_ticks << " | position rad = " << position << endl);
00704     return true;
00705   }
00706   else
00707   {
00708     LOGGING_WARNING_C(DriverSVH, SVHFingerManager, "Could not get postion for channel " << channel << endl);
00709     return false;
00710   }
00711 }
00712 
00713 
00714 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_
00715 void SVHFingerManager::receivedHintMessage(const int &hint)
00716 {
00717   LOGGING_DEBUG_C(DriverSVH, SVHFingerManager, "Received a special command to clear error :" << hint << endl);
00718   switch (hint)
00719   {
00720   case eHT_DEVICE_NOT_FOUND:
00721     LOGGING_DEBUG_C(DriverSVH, SVHFingerManager, "Retrying connection with device handle: " << m_serial_device << endl);
00722     connect(m_serial_device);
00723     break;
00724   case eHT_CONNECTION_FAILED:
00725     LOGGING_DEBUG_C(DriverSVH, SVHFingerManager, "Retrying connection with device handle: " << m_serial_device << endl);
00726     connect(m_serial_device);
00727     break;
00728   case eHT_NOT_RESETTED:
00729     LOGGING_DEBUG_C(DriverSVH, SVHFingerManager, "Resetting ALL fingers " << endl);
00730     resetChannel(eSVH_ALL);
00731     break;
00732   case eHT_NOT_CONNECTED:
00733     LOGGING_DEBUG_C(DriverSVH, SVHFingerManager, "Retrying connection with device handle: " << m_serial_device << endl);
00734     connect(m_serial_device);
00735     break;
00736   case eHT_RESET_FAILED:
00737     LOGGING_DEBUG_C(DriverSVH, SVHFingerManager, "Resetting ALL fingers " << endl);
00738     resetChannel(eSVH_ALL);
00739     break;
00740   case eHT_CHANNEL_SWITCHED_OF:
00741     LOGGING_DEBUG_C(DriverSVH, SVHFingerManager, "No specific action associated with command" << hint << endl);
00742     break;
00743   case eHT_DANGEROUS_CURRENTS:
00744     LOGGING_DEBUG_C(DriverSVH, SVHFingerManager, "No specific action associated with command" << hint << endl);
00745     break;
00746   default:
00747     LOGGING_ERROR_C(DriverSVH, SVHFingerManager, "Special error clearing command " << hint << " could not be mapped. No action is taken please contact support if this happens." << endl);
00748     break;
00749   }
00750 }
00751 #endif // _IC_BUILDER_ICL_COMM_WEBSOCKET_
00752 
00753 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_
00754 void SVHFingerManager::updateWebSocket()
00755 {
00756   if (m_ws_broadcaster)
00757   {
00758     double position;
00759     //double current // will be implemented in future releases
00760     for (size_t i = 0; i < eSVH_DIMENSION; ++i)
00761     {
00762       // NOTE: Although the call to getPosition and current cann fail due to multiple reason, the only one we would encounter with these calls is a
00763       // non-homed finger. So it is quite safe to assume that the finger is NOT homed if these calls fail and we can do without multiple acces to the homed variable
00764 
00765       if (isHomed(static_cast<SVHChannel>(i)) && getPosition(static_cast<SVHChannel>(i),position)) // && (getCurrent(i,current))
00766       {
00767         m_ws_broadcaster->robot->setJointPosition(position,i);
00768         //m_ws_broadcaster>robot>setJointCurrent(current,i); // will be implemented in future releases
00769       }
00770       else
00771       {
00772         m_ws_broadcaster->robot->setJointHomed(false,i);
00773       }
00774 
00775       // One of the few places we actually need to call the sendstate as this function is periodically called by the feedback polling thread
00776       if (!m_ws_broadcaster->sendState())
00777       {
00778         //LOGGING_INFO_C(DriverSVH, SVHFingerManager, "Can't send ws_broadcaster state - reconnect pending..." << endl);
00779       }
00780     }
00781   }
00782 }
00783 #endif // _IC_BUILDER_ICL_COMM_WEBSOCKET_
00784 
00785 
00786 
00787 
00788 
00789 // returns actual current value for given channel
00790 bool SVHFingerManager::getCurrent(const SVHChannel &channel, double &current)
00791 {
00792   SVHControllerFeedback controller_feedback;
00793   if ((channel >=0 && channel < eSVH_DIMENSION) && isHomed(channel) && m_controller->getControllerFeedback(channel, controller_feedback))
00794   {
00795     current = controller_feedback.current;
00796     return true;
00797   }
00798   else
00799   {
00800     LOGGING_WARNING_C(DriverSVH, SVHFingerManager, "Could not get current for channel " << channel << endl);
00801     return false;
00802   }
00803 }
00804 
00805 // set all target positions at once
00806 bool SVHFingerManager::setAllTargetPositions(const std::vector<double>& positions)
00807 {
00808   if (isConnected())
00809   {
00810     // check size of position vector
00811     if (positions.size() == eSVH_DIMENSION)
00812     {
00813       // create target positions vector
00814       std::vector<int32_t> target_positions(eSVH_DIMENSION, 0);
00815 
00816       bool reject_command = false;
00817       for (size_t i = 0; i < eSVH_DIMENSION; ++i)
00818       {
00819         SVHChannel channel = static_cast<SVHChannel>(i);
00820 
00821         // enable all homed and disabled channels.. except its switched of
00822         if (!m_is_switched_off[channel] && isHomed(channel) && !isEnabled(channel))
00823         {
00824           enableChannel(channel);
00825         }
00826 
00827         // convert all channels to ticks
00828         target_positions[channel] = convertRad2Ticks(channel, positions[channel]);
00829 
00830         // check for out of bounds (except the switched off channels)
00831         if (!m_is_switched_off[channel] && !isInsideBounds(channel, target_positions[channel]))
00832         {
00833           reject_command = true;
00834 
00835         }
00836       }
00837 
00838       // send target position vector to controller and SCHUNK hand
00839       if (!reject_command)
00840       {
00841         m_controller->setControllerTargetAllChannels(target_positions);
00842         return true;
00843       }
00844       else
00845       {
00846         LOGGING_WARNING_C(DriverSVH, SVHFingerManager, "Could not set target position vector: At least one channel is out of bounds!" << endl);
00847         return false;
00848       }
00849 
00850     }
00851     else
00852     {
00853       LOGGING_WARNING_C(DriverSVH, SVHFingerManager, "Size of target position vector wrong: size = " << positions.size() << " expected size = " << (int)eSVH_DIMENSION << endl);
00854       return false;
00855     }
00856   }
00857   else
00858   {
00859     if (!m_connection_feedback_given)
00860     {
00861       LOGGING_ERROR_C(DriverSVH, SVHFingerManager, "Could not set target position vector: No connection to SCHUNK five finger hand!" << endl);
00862       m_connection_feedback_given = true;
00863     }
00864     return false;
00865   }
00866 }
00867 
00868 bool SVHFingerManager::setTargetPosition(const SVHChannel &channel, double position, double current)
00869 {
00870   if (isConnected())
00871   {
00872     if (channel >= 0 && channel < eSVH_DIMENSION)
00873     {
00874       if (m_is_switched_off[channel])
00875       {
00876         // Switched off channels  behave transparent so we return a true value while we ignore the input
00877         LOGGING_TRACE_C(DriverSVH, SVHFingerManager, "Target position for channel " << channel << " was ignored as it is switched off by the user"<< endl);
00878         return true;
00879       }
00880 
00881 
00882       if (isHomed(channel))
00883       {
00884         int32_t target_position = convertRad2Ticks(channel, position);
00885 
00886         //Disabled as the output will spam everything
00887         //LOGGING_DEBUG_C(DriverSVH, SVHFingerManager, "Target position for channel " << channel << " = " << target_position << endl);
00888 
00889         // check for bounds
00890         if (isInsideBounds(channel, target_position))
00891         {
00892           if (!isEnabled(channel))
00893           {
00894             enableChannel(channel);
00895           }
00896 
00897           m_controller->setControllerTarget(channel, target_position);
00898           return true;
00899         }
00900         else
00901         {
00902           LOGGING_ERROR_C(DriverSVH, SVHFingerManager, "Target position for channel " << channel << " out of bounds!" << endl);
00903           return false;
00904         }
00905       }
00906       else
00907       {
00908         LOGGING_ERROR_C(DriverSVH, SVHFingerManager, "Could not set target position for channel " << channel << ": Reset first!" << endl);
00909         return false;
00910       }
00911     }
00912     else
00913     {
00914       LOGGING_ERROR_C(DriverSVH, SVHFingerManager, "Could not set target position for channel " << channel << ": Illegal Channel" << endl);
00915       return false;
00916     }
00917   }
00918   else
00919   {
00920     // Give the Warning about no Connection exactly once! Otherwise this will immediately spam the log
00921     if (!m_connection_feedback_given)
00922     {
00923       LOGGING_ERROR_C(DriverSVH, SVHFingerManager, "Could not set target position for channel " << channel << ": No connection to SCHUNK five finger hand!" << endl);
00924       m_connection_feedback_given = true;
00925     }
00926     return false;
00927   }
00928 }
00929 
00930 // return enable flag
00931 bool SVHFingerManager::isEnabled(const SVHChannel &channel)
00932 {
00933   if (channel==eSVH_ALL)
00934   {
00935     bool all_enabled = true;
00936     for (size_t i = 0; i < eSVH_DIMENSION; ++i)
00937     {
00938       all_enabled = all_enabled && isEnabled(static_cast<SVHChannel>(i));
00939       // disabled for now, to noisy
00940 //      if (!isEnabled(static_cast<SVHChannel>(i)))
00941 //      {
00942 //        LOGGING_WARNING_C(DriverSVH, SVHFingerManager, "All finger enabled check failed: Channel: " << channel << " : " << SVHController::m_channel_description[i] << " is not enabled" << endl);
00943 //      }
00944     }
00945 
00946     return all_enabled;
00947   }
00948   else if (channel >=0 && channel < eSVH_DIMENSION)
00949   {
00950     // Switched off Channels will aways be reported as enabled to simulate everything is fine. Others need to ask the controller
00951     // if the channel is realy switched on
00952     // Note: i can see that based on the names this might lead to a little confusion... sorry about that but there are only limited number of
00953     // words for not active ;) enabled refers to the actual state of the position and current controllers. So enabled
00954     // means enabled on a hardware level. Switched off is a logical decission in this case. The user has specified this
00955     // particular channel not to be used (due to hardware issues) and therefore the driver (aka the finger manager) will act
00956     // AS IF the channel was enabled but is in fact switched off by the user. If you have a better variable name or a better
00957     // idea how to handle that you are welcome to change it. (GH 2014-05-26)
00958     return (m_is_switched_off[channel] || m_controller->isEnabled(channel));
00959   }
00960   else
00961   {
00962     LOGGING_ERROR_C(DriverSVH, SVHFingerManager, "isEnabled was requested for UNKNOWN Channel: " << channel << endl);
00963     return false;
00964   }
00965 }
00966 
00967 bool SVHFingerManager::isHomed(const SVHChannel &channel)
00968 {
00969   if (channel == eSVH_ALL)
00970   {
00971     bool all_homed = true;
00972     for (size_t i = 0; i < eSVH_DIMENSION; ++i)
00973     {
00974       all_homed = all_homed && isHomed(static_cast<SVHChannel>(i));
00975       if (!isHomed(static_cast<SVHChannel>(i)))
00976       {
00977         LOGGING_WARNING_C(DriverSVH, SVHFingerManager, "All finger homed check failed: Channel: " << i << " : " << SVHController::m_channel_description[i] << " is not homed" << endl);
00978       }
00979     }
00980 
00981     return all_homed;
00982   }
00983   else if (channel >=0 && channel < eSVH_DIMENSION)
00984   {
00985     // Channels that are switched off will always be reported as homed to simulate everything is fine. Others have to check
00986     return (m_is_switched_off[channel] || m_is_homed[channel]);
00987   }
00988   else //should not happen but better be save than sorry
00989   {
00990     LOGGING_ERROR_C(DriverSVH, SVHFingerManager, "isHomed was requested for UNKNOWN Channel: " << channel << endl);
00991     return false;
00992   }
00993 }
00994 
00995 void SVHFingerManager::setMovementState(const SVHFingerManager::MovementState &state)
00996 {
00997   m_movement_state = state;
00998 
00999 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_
01000   if (m_ws_broadcaster)
01001   {
01002     m_ws_broadcaster->robot->setMovementState(state);
01003   }
01004 #endif // _IC_BUILDER_ICL_COMM_WEBSOCKET_
01005 }
01006 
01007 bool SVHFingerManager::getCurrentSettings(const SVHChannel &channel, SVHCurrentSettings &current_settings)
01008 {
01009   if (channel >=0 && channel < eSVH_DIMENSION)
01010   {
01011     return m_controller->getCurrentSettings(channel, current_settings);
01012   }
01013   else
01014   {
01015     LOGGING_ERROR_C(DriverSVH, SVHFingerManager, "Could not get current settings for unknown/unsupported channel " << channel << endl);
01016     return false;
01017   }
01018 }
01019 
01020 bool SVHFingerManager::getPositionSettings(const SVHChannel &channel, SVHPositionSettings &position_settings)
01021 {
01022   if (channel >=0 && channel < eSVH_DIMENSION)
01023   {
01024     return m_controller->getPositionSettings(channel, position_settings);
01025   }
01026   else
01027   {
01028     LOGGING_ERROR_C(DriverSVH, SVHFingerManager, "Could not get position settings for unknown/unsupported channel " << channel << endl);
01029     return false;
01030   }
01031 }
01032 
01033 bool SVHFingerManager::currentSettingsAreSafe(const SVHChannel &channel,const SVHCurrentSettings &current_settings)
01034 {
01035   bool settingsAreSafe = true;
01036 
01037   // Yeah i would also love to use static arrays here or other fancier things but c++98 is no fun dealing with these things
01038   // so i just wrote down the "dumb" approach, its just to veryfy absolute values anyhow
01039   switch (channel)
01040   {
01041     case eSVH_THUMB_FLEXION:
01042       settingsAreSafe = (current_settings.wmn >= -1800.0) &&
01043                         (current_settings.wmx <=  1800.0);
01044     break;
01045   case eSVH_THUMB_OPPOSITION:
01046 
01047     settingsAreSafe = (current_settings.wmn >= -1800.0) &&
01048                       (current_settings.wmx <=  1800.0);
01049     break;
01050   case eSVH_INDEX_FINGER_DISTAL:
01051   case eSVH_MIDDLE_FINGER_DISTAL:
01052   case eSVH_RING_FINGER:
01053   case eSVH_PINKY:
01054 
01055     settingsAreSafe = (current_settings.wmn >= -650.0) &&
01056                       (current_settings.wmx <=  650.0);
01057     break;
01058   case eSVH_INDEX_FINGER_PROXIMAL:
01059   case eSVH_MIDDLE_FINGER_PROXIMAL:
01060 
01061     settingsAreSafe = (current_settings.wmn >= -1100.0) &&
01062                       (current_settings.wmx <=  1100.0);
01063     break;
01064   case eSVH_FINGER_SPREAD:
01065     settingsAreSafe = (current_settings.wmn >= -1000.0) &&
01066                       (current_settings.wmx <=  1000.0);
01067     break;
01068   default:
01069      // no valid channel was given anyway, this will be disregarded by the controller
01070      settingsAreSafe = true;
01071     break;
01072   }
01073 
01074   return settingsAreSafe;
01075 }
01076 
01077 // overwrite current parameters
01078 bool SVHFingerManager::setCurrentSettings(const SVHChannel &channel, const SVHCurrentSettings &current_settings)
01079 {
01080 
01081   if (channel >=0 && channel < eSVH_DIMENSION)
01082   {
01083     // For now we will only evaluate this and print out warnings, however if you care to do these things.. go right ahead...
01084     if (!currentSettingsAreSafe(channel,current_settings))
01085     {
01086        LOGGING_ERROR_C(DriverSVH, SVHFingerManager, "WARNING!!! Current Controller Params for channel " << channel << " are dangerous! THIS MIGHT DAMAGE YOUR HARDWARE!!!" << endl);
01087 #ifdef _IC_BUILDER_ICL_COMM_WEBSOCKET_
01088       if (m_ws_broadcaster)
01089       {
01090         m_ws_broadcaster->robot->setHint(eHT_DANGEROUS_CURRENTS);
01091         m_ws_broadcaster->sendHints(); // Hints are Transmitted Manually
01092       }
01093 #endif
01094     }
01095 
01096       // First of save the values
01097       m_current_settings[channel] = current_settings;
01098       m_current_settings_given[channel] = true;
01099 
01100       // In case the Hardware is connected, update the values
01101       if (isConnected())
01102       {
01103           m_controller->setCurrentSettings(channel, current_settings);
01104       }
01105       return true;
01106   }
01107   else
01108   {
01109     LOGGING_ERROR_C(DriverSVH, SVHFingerManager, "Could not set Current Controller Params for channel " << channel << ": No such channel" << endl);
01110     return false;
01111   }
01112 }
01113 
01114 // overwrite position parameters
01115 bool SVHFingerManager::setPositionSettings(const SVHChannel &channel, const SVHPositionSettings &position_settings)
01116 {
01117 
01118   if (channel >=0 && channel < eSVH_DIMENSION)
01119   {
01120     // First of save the values
01121     m_position_settings[channel] = position_settings;
01122     m_position_settings_given[channel] = true;
01123 
01124     // In case the Hardware is connected, update the values
01125     if (isConnected())
01126     {
01127       m_controller->setPositionSettings(channel, position_settings);
01128     }
01129 
01130     return true;
01131   }
01132   else
01133   {
01134     LOGGING_ERROR_C(DriverSVH, SVHFingerManager, "Could not set Position Controller Params for channel " << channel << ": No such channel" << endl);
01135     return false;
01136   }
01137 }
01138 
01139 //overwirte home settings
01140 bool SVHFingerManager::setHomeSettings(const SVHChannel &channel, const driver_svh::SVHHomeSettings &home_settings)
01141 {
01142   if (channel >=0 && channel < eSVH_DIMENSION)
01143   {
01144     // First of save the values
01145     m_home_settings[channel] = home_settings;
01146     LOGGING_TRACE_C(DriverSVH,SVHFingerManager, "Channel " << channel << " setting new homing settings : ");
01147     LOGGING_TRACE_C(DriverSVH,SVHFingerManager, "Direction " << home_settings.direction << " " << "Min offset " << home_settings.minimumOffset << " "
01148                                              << "Max offset "<< home_settings.maximumOffset << " " << "idle pos "  << home_settings.idlePosition  << " "
01149                                              << "Range Rad " << home_settings.rangeRad << " " << "Reset Curr Factor " << home_settings.resetCurrentFactor << " " << endl
01150                     );
01151 
01152     // Update the conversion factor for this finger:
01153     float range_ticks = m_home_settings[channel].maximumOffset - m_home_settings[channel].minimumOffset;
01154     m_ticks2rad[channel] = m_home_settings[channel].rangeRad / range_ticks * (-m_home_settings[channel].direction);
01155 
01156     return true;
01157   }
01158   else
01159   {
01160     LOGGING_ERROR_C(DriverSVH, SVHFingerManager, "Could not set homing settings for channel " << channel << ": No such channel" << endl);
01161     return false;
01162   }
01163 }
01164 
01165 void SVHFingerManager::setDefaultHomeSettings()
01166 {
01167   // homing parameters are important for software end stops
01168 
01169   // All values are based on the hardware description for maximum tics and maximum allowable range of movements
01170   // direction, minimum offset, maximum offset, idle position, range in rad, resetcurrent(factor)
01171   m_home_settings[eSVH_THUMB_FLEXION]          =  SVHHomeSettings(+1, -175.0e3f,  -5.0e3f, -15.0e3f, 0.97, 0.75);    // thumb flexion
01172   m_home_settings[eSVH_THUMB_OPPOSITION]       =  SVHHomeSettings(+1, -105.0e3f,  -5.0e3f, -15.0e3f, 0.99, 0.75); // thumb opposition
01173   m_home_settings[eSVH_INDEX_FINGER_DISTAL]    =  SVHHomeSettings(+1,  -47.0e3f,  -2.0e3f,  -8.0e3f, 1.33, 0.75);    // index finger distal joint
01174   m_home_settings[eSVH_INDEX_FINGER_PROXIMAL]  =  SVHHomeSettings(-1,    2.0e3f,  42.0e3f,   8.0e3f, 0.8, 0.75);  // index finger proximal joint
01175   m_home_settings[eSVH_MIDDLE_FINGER_DISTAL]   =  SVHHomeSettings(+1,  -47.0e3f,  -2.0e3f,  -8.0e3f, 1.33, 0.75);    // middle finger distal joint
01176   m_home_settings[eSVH_MIDDLE_FINGER_PROXIMAL] =  SVHHomeSettings(-1,    2.0e3f,  42.0e3f,   8.0e3f, 0.8, 0.75);  // middle finger proximal joint
01177   m_home_settings[eSVH_RING_FINGER]            =  SVHHomeSettings(+1,  -47.0e3f,  -2.0e3f,  -8.0e3f, 0.98, 0.75);    // ring finger
01178   m_home_settings[eSVH_PINKY]                  =  SVHHomeSettings(+1,  -47.0e3f,  -2.0e3f,  -8.0e3f, 0.98, 0.75);    // pinky
01179   m_home_settings[eSVH_FINGER_SPREAD]          =  SVHHomeSettings(+1,  -47.0e3f,  -2.0e3f,  -25.0e3f,0.58, 0.4);    // finger spread
01180 
01181   m_ticks2rad.resize(eSVH_DIMENSION, 0.0);
01182   for (size_t i = 0; i < eSVH_DIMENSION; ++i)
01183   {
01184     float range_ticks = m_home_settings[i].maximumOffset - m_home_settings[i].minimumOffset;
01185     m_ticks2rad[i] = m_home_settings[i].rangeRad / range_ticks * (-m_home_settings[i].direction);
01186   }
01187 
01188 }
01189 
01190 
01191 
01192 std::vector<SVHCurrentSettings> SVHFingerManager::getDefaultCurrentSettings()
01193 {
01194   // BEWARE! Only change these values if you know what you are doing !! Setting wrong values could damage the hardware!!!
01195 
01196   std::vector<SVHCurrentSettings> current_settings(eSVH_DIMENSION);
01197 
01198 
01199   //SVHCurrentSettings cur_set_thumb            = {-400.0f, 400.0f, 0.405f, 4e-6f, -400.0f, 400.0f, 0.850f, 85.0f, -500.0f, 500.0f};  // Backup values that are based on  MeCoVis suggestions
01200   //SVHCurrentSettings cur_set_thumb_opposition = {-400.0f, 400.0f, 0.405f, 4e-6f, -400.0f, 400.0f, 0.90f, 85.0f, -800.0f, 800.0f}; // Backup values that are based on  MeCoVis suggestions
01201   //SVHCurrentSettings cur_set_distal_joint     = {-176.0f, 176.0f, 0.405f, 4e-6f, -300.0f, 300.0f, 0.850f, 85.0f, -254.0f, 254.0f};  // Backup values that are based on  MeCoVis suggestions
01202   //SVHCurrentSettings cur_set_proximal_joint   = {-167.0f, 167.0f, 0.405f, 4e-6f, -300.0f, 300.0f, 0.850f, 85.0f, -254.0f, 254.0f};  // Backup values that are based on  MeCoVis suggestions
01203   //SVHCurrentSettings cur_set_finger_spread    = {-200.0f, 200.0f, 0.405f, 4e-6f, -300.0f, 300.0f, 0.850f, 85.0f, -254.0f, 254.0f};  // Backup values that are based on  MeCoVis suggestions
01204 
01205   // curr min, Curr max,ky(error output scaling),dt(time base),imn (integral windup min), imx (integral windup max), kp,ki,umn,umx (output limter)
01206   SVHCurrentSettings cur_set_thumb(-500.0f, 500.0f, 0.405f, 4e-6f, -500.0f, 500.0f, 0.6f, 10.0f, -255.0f, 255.0f); // Much Smoother values that produce nice motions and are actually reasonabl
01207   SVHCurrentSettings cur_set_thumb_opposition(-500.0f, 500.0f, 0.405f, 4e-6f, -500.0f, 500.0f, 0.6f, 10.0f, -255.0f, 255.0f); // Much Smoother values that produce nice motions and are actually reasonable
01208   SVHCurrentSettings cur_set_distal_joint(-300.0f, 300.0f, 0.405f, 4e-6f, -300.0f, 300.0f, 0.3f, 10.0f, -255.0f, 255.0f); // Much Smoother values that produce nice motions and are actually reasonable
01209   SVHCurrentSettings cur_set_proximal_joint(-350.0f, 350.0f, 0.405f, 4e-6f, -350.0f, 350.0f, 0.5f, 10.0f, -255.0f, 255.0f); // Much Smoother values that produce nice motions and are actually reasonable
01210   SVHCurrentSettings cur_set_finger_spread(-300.0f, 300.0f, 0.405f, 4e-6f, -300.0f, 300.0f, 0.70f, 60.0f, -255.0f, 255.0f); // Somewhat better values based on the MeCoVis software
01211 
01212 
01213   current_settings[eSVH_THUMB_FLEXION]          = m_current_settings_given[eSVH_THUMB_FLEXION]          ? m_current_settings[eSVH_THUMB_FLEXION]          :cur_set_thumb;              // thumb flexion
01214   current_settings[eSVH_THUMB_OPPOSITION]       = m_current_settings_given[eSVH_THUMB_OPPOSITION]       ? m_current_settings[eSVH_THUMB_OPPOSITION]       :cur_set_thumb_opposition;   // thumb opposition
01215   current_settings[eSVH_INDEX_FINGER_DISTAL]    = m_current_settings_given[eSVH_INDEX_FINGER_DISTAL]    ? m_current_settings[eSVH_INDEX_FINGER_DISTAL]    :cur_set_distal_joint;       // index finger distal joint
01216   current_settings[eSVH_INDEX_FINGER_PROXIMAL]  = m_current_settings_given[eSVH_INDEX_FINGER_PROXIMAL]  ? m_current_settings[eSVH_INDEX_FINGER_PROXIMAL]  :cur_set_proximal_joint;     // index finger proximal joint
01217   current_settings[eSVH_MIDDLE_FINGER_DISTAL]   = m_current_settings_given[eSVH_MIDDLE_FINGER_DISTAL]   ? m_current_settings[eSVH_MIDDLE_FINGER_DISTAL]   :cur_set_distal_joint;       // middle finger distal joint
01218   current_settings[eSVH_MIDDLE_FINGER_PROXIMAL] = m_current_settings_given[eSVH_MIDDLE_FINGER_PROXIMAL] ? m_current_settings[eSVH_MIDDLE_FINGER_PROXIMAL] :cur_set_proximal_joint;     // middle finger proximal joint
01219   current_settings[eSVH_RING_FINGER]            = m_current_settings_given[eSVH_RING_FINGER]            ? m_current_settings[eSVH_RING_FINGER]            :cur_set_distal_joint;       // ring finger
01220   current_settings[eSVH_PINKY]                  = m_current_settings_given[eSVH_PINKY]                  ? m_current_settings[eSVH_PINKY]                  :cur_set_distal_joint;       // pinky
01221   current_settings[eSVH_FINGER_SPREAD]          = m_current_settings_given[eSVH_FINGER_SPREAD]          ? m_current_settings[eSVH_FINGER_SPREAD]          :cur_set_finger_spread;      // finger spread
01222 
01223   return current_settings;
01224 }
01225 
01229 std::vector<SVHPositionSettings> SVHFingerManager::getDefaultPositionSettings(const bool& reset)
01230 {
01231   std::vector<SVHPositionSettings> position_settings(eSVH_DIMENSION);
01232 
01233   // Original conservative settings
01234 //  SVHPositionSettings pos_set_thumb = {-1.0e6f, 1.0e6f,  3.4e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.5f, 0.05f, 0.0f};
01235 //  SVHPositionSettings pos_set_finger = {-1.0e6f, 1.0e6f,  8.5e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.5f, 0.05f, 0.0f};
01236 //  SVHPositionSettings pos_set_spread = {-1.0e6f, 1.0e6f, 17.0e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.5f, 0.05f, 0.0f};
01237 
01238     // All Fingers 0.5rad/sec except the fingers (1.5)
01239 //  SVHPositionSettings pos_set_thumb_flexion =          {-1.0e6f, 1.0e6f,  26.288e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.5f, 0.05f, 0.0f};
01240 //  SVHPositionSettings pos_set_thumb_opposition =       {-1.0e6f, 1.0e6f,  15.151e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.5f, 0.05f, 0.0f};
01241 //  SVHPositionSettings pos_set_finger_index_distal =    {-1.0e6f, 1.0e6f,  16.917e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.5f, 0.05f, 0.0f};
01242 //  SVHPositionSettings pos_set_finger_index_proximal =  {-1.0e6f, 1.0e6f,  25.0e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.5f, 0.05f, 0.0f};
01243 //  SVHPositionSettings pos_set_finger_middle_distal =   {-1.0e6f, 1.0e6f,  16.917e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.5f, 0.05f, 0.0f};
01244 //  SVHPositionSettings pos_set_finger_middle_proximal = {-1.0e6f, 1.0e6f,  25.0e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.5f, 0.05f, 0.0f};
01245 //  SVHPositionSettings pos_set_finger_ring =            {-1.0e6f, 1.0e6f,  22.959e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.5f, 0.05f, 0.0f};
01246 //  SVHPositionSettings pos_set_finger_pinky =           {-1.0e6f, 1.0e6f,  22.959e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.5f, 0.05f, 0.0f};
01247 //  SVHPositionSettings pos_set_spread =                 {-1.0e6f, 1.0e6f, 21.551e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.5f, 0.05f, 0.0f};
01248 
01249   // All Fingers with a speed that will close the complete range of the finger in 0.5 Seconds (except the thumb that will take 4)
01250 //    SVHPositionSettings pos_set_thumb_flexion =          {-1.0e6f, 1.0e6f,  42.5e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.5f, 0.05f, 0.0f};
01251 //    SVHPositionSettings pos_set_thumb_opposition =       {-1.0e6f, 1.0e6f,  25.0e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.5f, 0.05f, 0.0f};
01252 //    SVHPositionSettings pos_set_finger_index_distal =    {-1.0e6f, 1.0e6f,  90.0e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.5f, 0.05f, 0.0f};
01253 //    SVHPositionSettings pos_set_finger_index_proximal =  {-1.0e6f, 1.0e6f,  80.0e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.5f, 0.05f, 0.0f};
01254 //    SVHPositionSettings pos_set_finger_middle_distal =   {-1.0e6f, 1.0e6f,  90.0e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.5f, 0.05f, 0.0f};
01255 //    SVHPositionSettings pos_set_finger_middle_proximal = {-1.0e6f, 1.0e6f,  80.0e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.5f, 0.05f, 0.0f};
01256 //    SVHPositionSettings pos_set_finger_ring =            {-1.0e6f, 1.0e6f,  90.0e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.5f, 0.05f, 0.0f};
01257 //    SVHPositionSettings pos_set_finger_pinky =           {-1.0e6f, 1.0e6f,  90.0e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.5f, 0.05f, 0.0f};
01258 //    SVHPositionSettings pos_set_spread =                 {-1.0e6f, 1.0e6f,  50.0e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.5f, 0.05f, 0.0f};
01259 
01260   // All Fingers with a speed that will close the complete range of the finger in 1 Seconds    (except the thumb that wikll take 4)
01261   SVHPositionSettings pos_set_thumb_flexion            (-1.0e6f, 1.0e6f,  65.0e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.5f, 0.05f, 0.0f);
01262   SVHPositionSettings pos_set_thumb_opposition         (-1.0e6f, 1.0e6f,  50.0e3f, 1.00f, 1e-3f, -4000.0f, 4000.0f, 0.05f, 0.1f, 0.0f);
01263   SVHPositionSettings pos_set_finger_index_distal      (-1.0e6f, 1.0e6f,  45.0e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.5f, 0.05f, 0.0f);
01264   SVHPositionSettings pos_set_finger_index_proximal    (-1.0e6f, 1.0e6f,  40.0e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.3f, 0.05f, 0.0f);
01265   SVHPositionSettings pos_set_finger_middle_distal     (-1.0e6f, 1.0e6f,  45.0e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.5f, 0.05f, 0.0f);
01266   SVHPositionSettings pos_set_finger_middle_proximal   (-1.0e6f, 1.0e6f,  40.0e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.3f, 0.05f, 0.0f);
01267   SVHPositionSettings pos_set_finger_ring              (-1.0e6f, 1.0e6f,  45.0e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.5f, 0.05f, 0.0f);
01268   SVHPositionSettings pos_set_finger_pinky             (-1.0e6f, 1.0e6f,  45.0e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.5f, 0.05f, 0.0f);
01269   SVHPositionSettings pos_set_spread                   (-1.0e6f, 1.0e6f,  25.0e3f, 1.00f, 1e-3f, -500.0f, 500.0f, 0.5f, 0.05f, 0.0f);
01270 
01271   //Return either the default values or the ones given from outside
01272   position_settings[eSVH_THUMB_FLEXION]           = m_position_settings_given[eSVH_THUMB_FLEXION] ? m_position_settings[eSVH_THUMB_FLEXION] : pos_set_thumb_flexion;   // thumb flexion
01273   position_settings[eSVH_THUMB_OPPOSITION]        = m_position_settings_given[eSVH_THUMB_OPPOSITION] ? m_position_settings[eSVH_THUMB_OPPOSITION] :pos_set_thumb_opposition;   // thumb opposition
01274   position_settings[eSVH_INDEX_FINGER_DISTAL]     = m_position_settings_given[eSVH_INDEX_FINGER_DISTAL] ? m_position_settings[eSVH_INDEX_FINGER_DISTAL] :pos_set_finger_index_distal;  // index finger distal joint
01275   position_settings[eSVH_INDEX_FINGER_PROXIMAL]   = m_position_settings_given[eSVH_INDEX_FINGER_PROXIMAL] ? m_position_settings[eSVH_INDEX_FINGER_PROXIMAL] :pos_set_finger_index_proximal;  // index finger proximal joint
01276   position_settings[eSVH_MIDDLE_FINGER_DISTAL]    = m_position_settings_given[eSVH_MIDDLE_FINGER_DISTAL] ? m_position_settings[eSVH_MIDDLE_FINGER_DISTAL] :pos_set_finger_middle_distal;  // middle finger distal joint
01277   position_settings[eSVH_MIDDLE_FINGER_PROXIMAL]  = m_position_settings_given[eSVH_MIDDLE_FINGER_PROXIMAL] ? m_position_settings[eSVH_MIDDLE_FINGER_PROXIMAL] :pos_set_finger_middle_proximal;  // middle finger proximal joint
01278   position_settings[eSVH_RING_FINGER]             = m_position_settings_given[eSVH_RING_FINGER] ? m_position_settings[eSVH_RING_FINGER] :pos_set_finger_ring;  // ring finger
01279   position_settings[eSVH_PINKY]                   = m_position_settings_given[eSVH_PINKY] ? m_position_settings[eSVH_PINKY] :pos_set_finger_pinky;  // pinky
01280   position_settings[eSVH_FINGER_SPREAD]           = m_position_settings_given[eSVH_FINGER_SPREAD]  ? m_position_settings[eSVH_FINGER_SPREAD] :pos_set_spread;  // finger spread
01281 
01282   // Modify the reset speed in case these position settings are meant to be used during the reset
01283   if (reset)
01284   {
01285     for (size_t i = 0; i < eSVH_DIMENSION; ++i)
01286     {
01287       position_settings[i].dwmx = position_settings[i].dwmx * m_reset_speed_factor;
01288     }
01289   }
01290 
01291 
01292   return position_settings;
01293 }
01294 
01295 void driver_svh::SVHFingerManager::setResetSpeed(const float &speed)
01296 {
01297   if ((speed>= 0.0) && (speed <= 1.0))
01298   {
01299     m_reset_speed_factor = speed;
01300   }
01301   else
01302   {
01303     LOGGING_ERROR_C(DriverSVH, SVHFingerManager, "The reset speed value given: "<< speed << " is not valid. Please provide a value between 0.0 and 1.0, default is 0.2"<< endl);
01304   }
01305 }
01306 
01307 // Converts joint positions of a specific channel from RAD to ticks
01308 int32_t SVHFingerManager::convertRad2Ticks(const SVHChannel &channel,const double &position)
01309 {
01310   int32_t target_position = static_cast<int32_t>(position / m_ticks2rad[channel]);
01311 
01312   if (m_home_settings[channel].direction > 0)
01313   {
01314     target_position += m_position_max[channel];
01315   }
01316   else
01317   {
01318     target_position += m_position_min[channel];
01319   }
01320 
01321   return target_position;
01322 }
01323 
01324 // Converts Joint ticks of a specific channel back to RAD removing its offset in the process
01325 double SVHFingerManager::convertTicks2Rad(const SVHChannel &channel, const int32_t &ticks)
01326 {
01327     int32_t cleared_position_ticks;
01328 
01329     if (m_home_settings[channel].direction > 0)
01330     {
01331       cleared_position_ticks = ticks - m_position_max[channel];
01332     }
01333     else
01334     {
01335       cleared_position_ticks = ticks - m_position_min[channel];
01336     }
01337 
01338     return static_cast<double>(cleared_position_ticks * m_ticks2rad[channel]);
01339 }
01340 
01341 // Check bounds of target positions
01342 bool SVHFingerManager::isInsideBounds(const SVHChannel &channel, const int32_t &target_position)
01343 {
01344 
01345   // Switched off channels will always be reported as inside bounds
01346   if (m_is_switched_off[channel] || ((target_position >= m_position_min[channel]) && (target_position <= m_position_max[channel])))
01347   {
01348       return true;
01349   }
01350   else
01351   {
01352     LOGGING_WARNING_C(DriverSVH, SVHFingerManager, "Channel" << channel << " : " << SVHController::m_channel_description[channel]  << " Target: " << target_position << "(" << convertTicks2Rad(channel,target_position) << "rad)" << " is out of bounds! [" << m_position_min[channel] << "/" << m_position_max[channel] << "]"  << endl);
01353     return false;
01354   }
01355 }
01356 
01357 void SVHFingerManager::requestControllerState()
01358 {
01359   m_controller->requestControllerState();
01360 }
01361 
01362 void SVHFingerManager::setResetTimeout(const int& resetTimeout)
01363 {
01364   m_reset_timeout = (resetTimeout>0)?resetTimeout:0;
01365 }
01366 
01367 SVHFirmwareInfo SVHFingerManager::getFirmwareInfo()
01368 {
01369   // As the firmware info takes longer we need to disable the polling during the request of the firmware informatio
01370   if (m_feedback_thread != NULL)
01371   {
01372     // wait until thread has stopped
01373     m_feedback_thread->stop();
01374     m_feedback_thread->join();
01375   }
01376 
01377   // Tell the hardware to get the newest firmware information
01378   m_controller->requestFirmwareInfo();
01379   // Just wait a tiny amount
01380   icl_core::os::usleep(100);
01381 
01382   // Start the feedback process aggain
01383   if (m_feedback_thread != NULL)
01384   {
01385     // wait until thread has stopped
01386     m_feedback_thread->start();
01387   }
01388 
01389   // Note that the Firmware will also be printed to the console by the controller. So in case you just want to know it no further action is required
01390   return m_controller->getFirmwareInfo();
01391 }
01392 
01393 }


schunk_svh_driver
Author(s): Georg Heppner
autogenerated on Fri Aug 28 2015 12:59:19