riq_hand.cpp

Go to the documentation of this file.

#include "riq_hand_ethercat_hardware/riq_hand.h"
#include "riq_hand_ethercat_hardware/riq_hand_state.h"
#include "riq_hand_ethercat_hardware/riq_hand_command.h"

#include <iomanip>

#include <boost/static_assert.hpp>

#include <ros/console.h>

#include <math.h>
#include <algorithm>

// Product code = 11
PLUGINLIB_REGISTER_CLASS(11, riq_hand_ethercat_hardware::RIQHand, EthercatDevice);

namespace riq_hand_ethercat_hardware
{

RIQHandDiagnostics::RIQHandDiagnostics() :
  al_status_good_(false)
{
  // empty
}

void RIQHand::sizeAssert()
{
  BOOST_STATIC_ASSERT(sizeof(RIQHandStateEcat) == STATUS_SIZE);
  BOOST_STATIC_ASSERT(sizeof(RIQHandCommandEcat) == COMMAND_SIZE);
}

RIQHand::RIQHand() :
  state_cycle_count_(0),
  should_publish_(true),
  needs_watchdog_reset_(true),
  needs_reset_(false),
  needs_reset_countdown_(0),
  latched_fault_code_(RIQHandStateEcat::NO_FAULT),
  after_reset_countdown_(0),
  after_watchdog_reset_countdown_(0),
  halted_(false)
{
  //empty
}
 
RIQHand::~RIQHand()
{
  delete sh_->get_fmmu_config();
  delete sh_->get_pd_config();
}

void RIQHand::construct(EtherCAT_SlaveHandler *sh, int &start_address)
{
  EthercatDevice::construct(sh, start_address);

  command_size_ = COMMAND_SIZE;
  status_size_  = STATUS_SIZE;

  EtherCAT_FMMU_Config *fmmu = new EtherCAT_FMMU_Config(2);
  (*fmmu)[0] = EC_FMMU(start_address, // Logical start address
                       command_size_, // Logical length
                       0x00,  // Logical StartBit
                       0x07,  // Logical EndBit
                       COMMAND_PHY_ADDR, // Physical Start address
                       0x00,   // Physical StartBit
                       false,  // Read Enable
                       true,   // Write Enable
                       true);  // Enable  

  start_address += command_size_;

  (*fmmu)[1] = EC_FMMU(start_address, // Logical start address
                       status_size_,  // Logical length
                       0x00,  // Logical StartBit
                       0x07,  // Logical EndBit
                       STATUS_PHY_ADDR, // Physical Start address
                       0x00,  // Physical StartBit
                       true,  // Read Enable
                       false, // Write Enable
                       true); // Enable

  start_address += status_size_;

  sh->set_fmmu_config(fmmu);


  EtherCAT_PD_Config *pd = new EtherCAT_PD_Config(2);

  // Sync manager for command data - SyncMan Control Register
  // <Sm MinSize="0" MaxSize="1024" DefaultSize="200" StartAddress="#x1100" ControlByte="#x74" Enable="1">Outputs</Sm>
  (*pd)[0] =
    EC_SyncMan (
                COMMAND_PHY_ADDR,         // 0-1   Physical Addr    -
                command_size_,            // 2-3   Length           - 
                EC_BUFFERED,              // 4.1:0 Buffer type      -  Operation Mode (Buffered/Mailbox)
                EC_WRITTEN_FROM_MASTER,   // 4.3:2 Direction        -  
                                          //          Read: ECAT read, PDI write  or  
                                          //          Write: ECAT write, PDI read
                true,                     // 4.5   AL event enable  -  Interrupt ECAT Event Request
                true,                     // 4.6   Watchdog enable  -  Watchdog Trigger Enable
                false,                    // 5.0   Write event      -  Interrupt Write
                false,                    // 5.1   Read event       -  Interrupt Read
                false,                    // 5.2   Watchdog trigger -  ?? Reserved ??
                false,                    // 5.5:3 Queued state     -  Mailbox empty/full  
                EC_FIRST_BUFFER,          // 5.5:4 BufferedState    -  Last written buffer
                true);                    // 7.0   Channel Enable   -  
  (*pd)[0].ECATEventEnable = 1;


  // Sync manager for status data - SyncMan Control Register = 0x20
  // <Sm MinSize="0" MaxSize="1024" DefaultSize="200" StartAddress="#x1D00" ControlByte="#x30" Enable="1">Inputs</Sm>
  (*pd)[1] =
    EC_SyncMan (
                STATUS_PHY_ADDR,          // 0-1   Physical Addr    -
                status_size_,             // 2-3   Length           - 
                EC_BUFFERED,              // 4.1:0 Buffer type      -  Operation Mode (Buffered/Mailbox)
                EC_READ_FROM_MASTER,      // 4.3:2 Direction        -  
                                          //          Read: ECAT read, PDI write  or  
                                          //          Write: ECAT write, PDI read
                true,                     // 4.5   AL event enable  -  Interrupt ECAT Event Request
                false,                    // 4.6   Watchdog enable  -  Watchdog Trigger Enable
                false,                    // 5.0   Write event      -  Interrupt Write
                false,                    // 5.1   Read event       -  Interrupt Read
                false,                    // 5.2   Watchdog trigger -  ?? Reserved ??
                false,                    // 5.5:3 Queued state     -  Mailbox empty/full  
                EC_FIRST_BUFFER,          // 5.5:4 BufferedState    -  Last written buffer
                true);                    // 7.0   Channel Enable   -  
  (*pd)[1].ECATEventEnable = 1;

  sh->set_pd_config(pd);


  EtherCAT_MbxConfig *mbx = new EtherCAT_MbxConfig;

  // Sync manager for Master->Slave Mailbox - SyncMan Control Register = 0x26
  mbx->SM0 =
    EC_SyncMan(
               MBX_COMMAND_PHY_ADDR,
               MBX_COMMAND_SIZE,
               EC_QUEUED,                //
               EC_WRITTEN_FROM_MASTER,     //
               true,                       // Trigger AL interupt (to uC/FPGA slave) when buffer is writen by master
               false,                      // no watchdog
               false,                      // (ro)
               false,                      // (ro)
               false,                      // (ro)
               false,                      // (ro)
               EC_FIRST_BUFFER,            // (ro) which buffer was last written
               true);                      // enable syncmanager
  mbx->SM0.ECATEventEnable = 1;

  // Sync manager for Slave->Master Mailbox - SyncMan Control Register = 0x22
  mbx->SM1 =
    EC_SyncMan(
               MBX_STATUS_PHY_ADDR,
               MBX_STATUS_SIZE,
               EC_QUEUED,                  //
               EC_READ_FROM_MASTER,        //
               true,                       // Don't AL interupt (to uC/FPGA slave) when buffer is read by master
               false,                      // no watchdog
               false,                      // (ro)
               false,                      // (ro)
               false,                      // (ro)
               false,                      // (ro)
               EC_FIRST_BUFFER,            // which buffer was last written (ro)
               true);
  //mbx->SM1.ECATEventEnable = 1;

  sh->set_mbx_config(mbx);

  // Disable watchdog as early as possible (may not be needed any more)
  // disableWatchdog(sh);
}

int RIQHand::initialize(pr2_hardware_interface::HardwareInterface *, bool)
{
  ROS_DEBUG("Device #%02d: RIQ Hand (%#08x)", sh_->get_ring_position(), sh_->get_product_code());

  if (use_ros_)
  {
    ros::NodeHandle nh;

    // Register ROS subscriber for hand command
    commandSubscriber_ = nh.subscribe("riq_hand_command", 2, &RIQHand::commandCallback, this);
    
    // Intialize realtime publisher
    publisher_.init(nh, "riq_hand_state", 1);
  }

  return 0;
}

void RIQHand::commandCallback(const riq_msgs::RIQHandCommand::ConstPtr& msg)
{
  command_mutex_.lock();
  new_command_ = *msg;    
  command_mutex_.unlock();
}


unsigned RIQHand::scaleAndSaturate255(double value)
{
  value = double(value)*255.0;
  value = roundf(value);
  value = std::max(0.0, std::min( 255.0, value ) );
  return unsigned(value); 
}  

void RIQHand::packCommand(unsigned char *buffer, bool halt, bool reset)
{
  // Get newest command data (if possible)
  if (command_mutex_.try_lock())
  {
    command_ = new_command_;
    command_mutex_.unlock();
  } 

  // Request watch reset (by needs to be done by non-realtime thread)
  if (reset)
  {
    needs_watchdog_reset_ = true;
    latched_fault_code_ = 0;
    if (needs_reset_)
    {
      // When device need reset (re initialization) keep reset bit low for a couple of cycles
      needs_reset_countdown_ = 10;
      needs_reset_ = false;
    }
  }    

  RIQHandCommandEcat *command = (RIQHandCommandEcat *)(buffer);  
  RIQActionRequest &action_request(command->action_request_);

  command->zero();

  if (needs_reset_countdown_ > 0)
  {
    --needs_reset_countdown_;
    action_request.initialize_ = 0;
  }
  else 
  {
    action_request.initialize_ = 1;
  }  

  switch(command_.action)
  {
  case riq_msgs::RIQHandCommand::OPEN :
    action_request.grip_ = RIQActionRequest::GRIP_OPEN;
    break;
  case riq_msgs::RIQHandCommand::CLOSE :
    action_request.grip_ = RIQActionRequest::GRIP_CLOSE;
    break;
  case riq_msgs::RIQHandCommand::STOP :
    action_request.grip_ = RIQActionRequest::GRIP_STOP;
    break;
  default:
    action_request.grip_ = RIQActionRequest::GRIP_STOP;
  }

  switch(command_.mode)
  {
  case riq_msgs::RIQHandCommand::CYLINDRICAL:
    action_request.mode_ = RIQActionRequest::CYLINDRICAL_MODE;
    break;
  case riq_msgs::RIQHandCommand::PINCH:
    action_request.mode_ = RIQActionRequest::PINCH_MODE;
    break;
  case riq_msgs::RIQHandCommand::SPHERIOD:
    action_request.mode_ = RIQActionRequest::SPHERIOD_MODE;
    break;
  case riq_msgs::RIQHandCommand::SCISSORS:
    action_request.mode_ = RIQActionRequest::SCISSORS_MODE;
    break;
  }
  
  command->force_    = scaleAndSaturate255(command_.force);
  command->velocity_ = scaleAndSaturate255(command_.velocity);

  if (halt)
  {
    action_request.grip_ = RIQActionRequest::GRIP_STOP;
  }
  halted_ = halt;

  command->safety_shutdown_.timeout_ = RIQSafetyShutdown::SHUTDOWN_TIMEOUT_10240ms;
}


bool RIQHand::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
{
  // Nothing for now
  const RIQHandStateEcat *state = (const RIQHandStateEcat *)(this_buffer + command_size_);
  const RIQHandStateEcat *prev_state = (const RIQHandStateEcat *)(prev_buffer + command_size_);
  const RIQGripperStatus &gripper_status(state->gripper_status_);
  const RIQObjectStatus  &object_status(state->object_status_);
  
  if (++state_cycle_count_ >= STATE_PUBLISH_PERIOD)
  {
    state_cycle_count_ = 0;
    should_publish_ = true;
  }

  if (should_publish_ && use_ros_ && publisher_.trylock())
  {
    should_publish_ = false;

    riq_msgs::RIQHandState     &msg(publisher_.msg_);
    riq_msgs::RIQActuatorState &thumb(msg.thumb);
    riq_msgs::RIQActuatorState &right(msg.right_finger);
    riq_msgs::RIQActuatorState &left(msg.left_finger);
    riq_msgs::RIQActuatorState &scissors(msg.scissors);
    
    msg.operational = 
      (gripper_status.initialization_ == 1) &&
      (state->fault_status_ == RIQHandStateEcat::NO_FAULT) && 
      !halted_;

    // status, mode, and object field constants are defined to be the same as EtherCAT interface values
    msg.status = gripper_status.status_;
    msg.mode   = gripper_status.mode_;
    msg.grip   = gripper_status.grip_;
    msg.object = object_status.detected_;
  
    thumb.object_detected     = object_status.thumb_detected_;
    right.object_detected     = object_status.right_finger_detected_;
    left.object_detected      = object_status.left_finger_detected_;
    scissors.object_detected  = object_status.scissors_detected_;

    thumb.position    = RIQHandStateEcat::convertPosition(state->thumb_position_);
    right.position    = RIQHandStateEcat::convertPosition(state->right_finger_position_);
    left.position     = RIQHandStateEcat::convertPosition(state->left_finger_position_);
    scissors.position = RIQHandStateEcat::convertPosition(state->scissors_position_);

    thumb.current     = RIQHandStateEcat::convertCurrent(state->thumb_current_);
    right.current     = RIQHandStateEcat::convertCurrent(state->right_finger_current_);
    left.current      = RIQHandStateEcat::convertCurrent(state->left_finger_current_);
    scissors.current  = RIQHandStateEcat::convertCurrent(state->scissors_current_);

    publisher_.unlockAndPublish();
  }

  bool rv = true;

  if (needs_watchdog_reset_)
  {
    after_watchdog_reset_countdown_  = 100;
  }
  else if (after_watchdog_reset_countdown_ > 0)
  {
    --after_watchdog_reset_countdown_;
    if ((prev_state->fault_status_ == RIQHandStateEcat::FAULT_COMMUNICATION_TIMEOUT) && 
        (state->fault_status_      == RIQHandStateEcat::NO_FAULT))
    {
      ROS_INFO("Communication fault cleared at %d", after_watchdog_reset_countdown_);
    }
  }    
  
  if (needs_reset_countdown_ > 0)
  {
    after_reset_countdown_ = 100;
  }
  else if (after_reset_countdown_ > 0)
  {
    --after_reset_countdown_;
    bool was_major_fault = 
      (prev_state->fault_status_ == RIQHandStateEcat::FAULT_MAJOR_SCISSORS_DISPLACEMENT) || 
      (prev_state->fault_status_ == RIQHandStateEcat::FAULT_MAJOR_SCISSORS_DISPLACEMENT);
    if (was_major_fault && (state->fault_status_ == RIQHandStateEcat::NO_FAULT))
    {
      ROS_INFO("Major fault cleared at %d", after_reset_countdown_);
    }
  }

  // When certain faults occur, halt motors and save fault code
  if (!halted_  &&  (state->fault_status_ != RIQHandStateEcat::NO_FAULT))
  {
    switch (state->fault_status_)
    {
    case RIQHandStateEcat::FAULT_COMMUNICATION_TIMEOUT:
      // Ignore this error, for a few cycles after has been cleared
      if (after_watchdog_reset_countdown_ == 0)
      {
        rv = false;
        latched_fault_code_ = state->fault_status_;
        ROS_ERROR("Watchdog");
      }
      break;
    case RIQHandStateEcat::FAULT_INSUFFICIENT_SUPPLY_VOLTAGE:
      rv = false;
      latched_fault_code_ = state->fault_status_;
      break;
    case RIQHandStateEcat::FAULT_MAJOR_SCISSORS_DISPLACEMENT:
    case RIQHandStateEcat::FAULT_MAJOR_FINGER_DISPLACEMENT:
      // Ignore these errors for a while after resetting
      if (after_reset_countdown_ == 0)
      {
        needs_reset_ = true;
        rv = false;
        latched_fault_code_ = state->fault_status_;
      }
      break;
    default:
      // Do nothing for other fault codes
      break;
    }
  }

  return rv;
}



void RIQHand::disableWatchdog(EtherCAT_SlaveHandler *sh)
{
  // The ethercat process data watchdog timeout on set to 100ms after reset.
  // This disables watchdog by change timeout to special value of 0.
  EthercatDirectCom com(EtherCAT_DataLinkLayer::instance());
  static const unsigned WATCHDOG_TIME_PDI_ADDR = 0x410;
  static const unsigned WATCHDOG_TIME_PROCESS_DATA_ADDR = 0x420;  
  uint16_t timeout = 0;
  if (writeData(&com, sh, WATCHDOG_TIME_PDI_ADDR, &timeout, 2, FIXED_ADDR) != 0)
  {
    ROS_ERROR("Error disabling PDI watchdog\n");
  }
  timeout = 0;
  if (writeData(&com, sh, WATCHDOG_TIME_PROCESS_DATA_ADDR, &timeout, 2, FIXED_ADDR) != 0)
  {
    ROS_ERROR("Error disabling process data watchdog\n");
  }
}


void RIQHand::collectDiagnostics(EthercatCom *com)
{
  EthercatDevice::collectDiagnostics(com);
  
  ECatALStatusAll al_status;
  bool al_status_good = al_status.readData(com,sh_);

  { boost::unique_lock<boost::mutex> lock(diagnostics_mutex_);
    collect_diagnostics_.al_status_good_ = al_status_good;
    collect_diagnostics_.al_status_ = al_status;
  }

  // Use diagnostics collection thread to clear watchdog timeout error (when requested by reset_motors)
  // When the EtherCAT master does not update slave process data every 100ms, the watchdog times out.
  // When this occcurs the device automatically goes into the SAFE-OP state.
  // To get device operational again, the AL error is acknowledged, 
  // and the device is put back into the OPerational state
  if (needs_watchdog_reset_)
  {
    if (!al_status_good)
    {
      // Without status data, it is not possible to determine if
      // watchdog error does indeed need to cleared.
      //ROS_WARN("Cannot reset watchdog, no status data");
    }
    else if (al_status.status_code_.error_code_ == ECatALStatusCode::SYNC_MANAGER_WATCHDOG)
    {
      // reset error code and move back into operational state
      ECatALControl al_control;
      al_control.raw_ = 0;
      al_control.ack_error_ = 1;
      al_control.state_ = ECatStates::OP;
      if (al_control.writeData(com, sh_))
      {
        // watchdog error has been cleared
        //ROS_WARN("Watchdog reset");
        needs_watchdog_reset_ = false;        
      }
      else
      {
        //ROS_WARN("Watchdog reset failed");
      }
    }
    else
    {
      // No watchdog error to clear
      //ROS_WARN("Watchdog reset uneeded");
      needs_watchdog_reset_ = false;      
    }
  }
}


void RIQHand::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
{
  const RIQHandStateEcat *status = (const RIQHandStateEcat *)(buffer + command_size_);
  const RIQHandCommandEcat *command = (const RIQHandCommandEcat *)(buffer);  

  // Get copy of diagnostics data collected with another thread
  if (diagnostics_mutex_.try_lock())
  { 
    publish_diagnostics_ = collect_diagnostics_;
    diagnostics_mutex_.unlock();
  }

  stringstream str;
  str << "EtherCAT Device #" << setw(2) << setfill('0') << sh_->get_ring_position() << " (RIQHand)";
  d.name = str.str();
  d.summary(d.OK, "OK");
  char serial[32];
  snprintf(serial, sizeof(serial), "%05d-%05d", sh_->get_product_code(), sh_->get_serial());
  d.hardware_id = serial;

  d.clear();
  d.addf("Product code", "RIQ Hand (%d), Revision %d",
         sh_->get_product_code(), sh_->get_revision());
  d.addf("Serial Number", "%s", serial);

  const RIQGripperStatus &gripper_status(status->gripper_status_);
  d.addf("Initialization", "%s (%d)", (gripper_status.initialization_ ? "Initialized" : "Reset"),
         int(gripper_status.initialization_) );
  d.addf("Gripper Mode", "%s (%d)", gripper_status.modeString(), gripper_status.mode_);
  d.addf("Gripper Status", "%s (%d)", gripper_status.statusString(), gripper_status.status_);
  bool in_scissors_mode = gripper_status.inScissorsMode();

  const RIQObjectStatus &object_status(status->object_status_);
  d.addf("Object Detected", "%s (%d)", 
         object_status.detectedString(in_scissors_mode), 
         object_status.detected_);

  d.addf("Fault", "%s (%d)", status->faultString(), int(status->fault_status_));

  if (latched_fault_code_ != 0)
  {
    // If fault was latched, it is error that requires motor reset
    // Examples : communication timeout, undervoltage
    d.mergeSummary(d.ERROR, RIQHandStateEcat::faultString(latched_fault_code_));
  }
  else if (status->fault_status_ != status->NO_FAULT)
  {
    // If fault was not latched
    d.mergeSummary(d.WARN, status->faultString());
  }

  d.addf("Thumb position", "%f (%u)", 
         status->convertPosition(status->thumb_position_), 
         unsigned(status->thumb_position_));
  d.addf("Thumb position", "%fmA (%u)", 
         1000.0 * status->convertCurrent(status->thumb_current_),
         unsigned(status->thumb_current_));

  d.addf("Right finger position", "%f (%u)", 
         status->convertPosition(status->right_finger_position_), 
         unsigned(status->right_finger_position_));
  d.addf("Right finger current", "%fmA (%u)", 
         1000.0 * status->convertCurrent(status->right_finger_current_),
         unsigned(status->right_finger_current_));

  d.addf("Left finger position", "%f (%u)", 
         status->convertPosition(status->left_finger_position_), 
         unsigned(status->left_finger_position_));
  d.addf("Left finger current", "%fmA (%u)", 
         1000.0 * status->convertCurrent(status->left_finger_current_),
         unsigned(status->left_finger_current_));

  d.addf("Scissors position", "%f (%u)", 
         status->convertPosition(status->scissors_position_), 
         unsigned(status->scissors_position_));
  d.addf("Scissors current", "%fmA (%u)", 
         1000.0 * status->convertCurrent(status->scissors_current_),
         unsigned(status->scissors_current_));


  const RIQActionRequest &action_request(command->action_request_);

  d.addf("Initialization request", "%s (%d)", 
         (action_request.initialize_ ? "Initialize" : "Reset"),
         unsigned(action_request.initialize_));
  d.addf("Mode request", "%s (%d)", action_request.modeString(),
         unsigned(action_request.mode_));
  d.addf("Grip request", "%s (%d)", action_request.gripString(),
         unsigned(action_request.grip_));
  d.addf("Force", "%f (%d)", (double(command->force_)/255.0), 
         unsigned(command->force_));
  d.addf("Velocity", "%f (%d)", (double(command->velocity_)/255.0),
         unsigned(command->velocity_));

    
  if (publish_diagnostics_.al_status_good_)
  {
    const ECatALStatusAll &al_status(publish_diagnostics_.al_status_);
    d.addf("AL Status", "%s : %s", 
           (al_status.status_.error_?"Error":"Good"),
           al_status.status_code_.errorCodeString());
    d.addf("AL State", "%s (%d)", 
           al_status.status_.stateString(),
           al_status.status_.state_);
  }
  else 
  {
    d.addf("AL Status", "INVALID"); 
  }  

  // TODO : RIQHand RX error counters do not seem to work. 
  //  Get EtherCAT support document to figure out what slave device registers are supported
  EthercatDevice::ethercatDiagnostics(d, 1);   // RIQHand has only a single port
}


}; //namespace riq_hand_ethercat_hardware