Added support for Estop (#27)

* Added support for Estop - Disables motors from updating step when Estop is pressed - Adds a new function on teensy source to reset Estop from the host and discard any old command - MT request returns ES response to the teensy_driver, which caches the estop state - Hardware interface resets the estop on activation - Hardware interface disables the interface with an error if estop is set during a write operation - A reset estop script added as part of ar_hardware_interface package - don't re-calibrate after estop reset --------- Co-authored-by: Abhijit Majumdar <abhijit.g.majumdar.com> Co-authored-by: Yifei Cheng <[email protected]>
ycheng517 · Nov 30, 2024 · 1bdf7e0 · 1bdf7e0
1 parent eb87c54
commit 1bdf7e0
Show file tree

Hide file tree

Showing 8 changed files with 124 additions and 11 deletions.
diff --git a/README.md b/README.md
@@ -151,6 +151,15 @@ Available Launch Arguments:
 
 You can now plan in RViz and control the real-world arm. Joint commands and joint states will be updated through the hardware interface.
 
+NOTE: At any point you may interrupt the robot movement by pressing the E-Stop button
+on the robot. This would abruptly stop the robot motion! To reset the E-Stop state of
+the robot use the following command
+
+```bash
+ros2 run ar_hardware_interface reset_estop.sh <AR_MODEL>
+```
+where `<AR_MODEL>` is the model of the AR4, one of `mk1`, `mk2`, or `mk3`
+
 ---
 
 ### Control simulated arm in Gazebo with MoveIt in RViz

diff --git a/ar_hardware_interface/CMakeLists.txt b/ar_hardware_interface/CMakeLists.txt
@@ -69,6 +69,11 @@ install(DIRECTORY config launch urdf
   DESTINATION share/${PROJECT_NAME}
 )
 
+install(PROGRAMS
+  scripts/reset_estop.sh
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 ament_export_targets(export_ar_hardware_interface HAS_LIBRARY_TARGET)
 ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)
 

diff --git a/ar_hardware_interface/include/ar_hardware_interface/ar_hardware_interface.hpp b/ar_hardware_interface/include/ar_hardware_interface/ar_hardware_interface.hpp
@@ -49,6 +49,8 @@ class ARHardwareInterface : public hardware_interface::SystemInterface {
   std::vector<double> joint_effort_commands_;
 
   // Misc
+  bool calibrated_ = false;
+
   void init_variables();
   double degToRad(double deg) { return deg / 180.0 * M_PI; };
   double radToDeg(double rad) { return rad / M_PI * 180.0; };

diff --git a/ar_hardware_interface/include/ar_hardware_interface/teensy_driver.hpp b/ar_hardware_interface/include/ar_hardware_interface/teensy_driver.hpp
@@ -22,6 +22,8 @@ class TeensyDriver {
               std::vector<double>& joint_states);
   void getJointPositions(std::vector<double>& joint_positions);
   bool calibrateJoints();
+  bool resetEStop();
+  bool isEStopped();
 
   TeensyDriver();
 
@@ -34,6 +36,7 @@ class TeensyDriver {
   int num_joints_;
   std::vector<double> joint_positions_deg_;
   std::vector<int> enc_calibrations_;
+  bool is_estopped_;
   rclcpp::Logger logger_ = rclcpp::get_logger("teensy_driver");
   rclcpp::Clock clock_ = rclcpp::Clock(RCL_ROS_TIME);
 
@@ -46,6 +49,7 @@ class TeensyDriver {
   void checkInit(std::string msg);
   void updateEncoderCalibrations(std::string msg);
   void updateJointPositions(std::string msg);
+  void updateEStopStatus(std::string msg);
   bool succeeded(std::string msg);
 };
 

diff --git a/ar_hardware_interface/scripts/reset_estop.sh b/ar_hardware_interface/scripts/reset_estop.sh
@@ -0,0 +1,17 @@
+#!/bin/bash
+
+# This is a convenience script to reset the EStop
+
+set -e # exit if error
+
+if [ -z "$1" ]; then
+  printf "\tUsage: ros2 run ar_hardware_interface reset_estop.sh <AR_MODEL>\n"
+  printf "\twhere <AR_MODEL> is 'mk1', 'mk2' or 'mk3'\n"
+  exit 1
+fi
+
+ros2 control set_hardware_component_state "$1"  active
+ros2 control set_controller_state joint_trajectory_controller active
+ros2 control set_controller_state joint_state_broadcaster active
+
+exit 0
diff --git a/ar_hardware_interface/src/ar_hardware_interface.cpp b/ar_hardware_interface/src/ar_hardware_interface.cpp
@@ -49,14 +49,23 @@ hardware_interface::CallbackReturn ARHardwareInterface::on_activate(
     const rclcpp_lifecycle::State& /*previous_state*/) {
   RCLCPP_INFO(logger_, "Activating hardware interface...");
 
+  // Reset Estop (if any)
+  bool success = driver_.resetEStop();
+  if (!success) {
+    RCLCPP_ERROR(logger_,
+                 "Cannot activate. Hardware E-stop state cannot be reset.");
+    return hardware_interface::CallbackReturn::ERROR;
+  }
+
   // calibrate joints if needed
   bool calibrate = info_.hardware_parameters.at("calibrate") == "True";
-  if (calibrate) {
+  if (calibrate && !calibrated_) {
     // run calibration
     RCLCPP_INFO(logger_, "Running joint calibration...");
     if (!driver_.calibrateJoints()) {
       return hardware_interface::CallbackReturn::ERROR;
     }
+    calibrated_ = true;
   }
 
   // init position commands at current positions
@@ -131,6 +140,14 @@ hardware_interface::return_type ARHardwareInterface::write(
   }
   RCLCPP_DEBUG_THROTTLE(logger_, clock_, 500, logInfo.c_str());
   driver_.update(actuator_commands_, actuator_positions_);
+  if (driver_.isEStopped()) {
+    std::string logWarn =
+        "Hardware in EStop state. To reset the EStop "
+        "reactivate the hardware component using 'ros2 "
+        "run ar_hardware_interface reset_estop.sh'.";
+    RCLCPP_WARN(logger_, logWarn.c_str());
+    return hardware_interface::return_type::ERROR;
+  }
   return hardware_interface::return_type::OK;
 }
 

diff --git a/ar_hardware_interface/src/teensy_driver.cpp b/ar_hardware_interface/src/teensy_driver.cpp
@@ -45,6 +45,7 @@ bool TeensyDriver::init(std::string ar_model, std::string port, int baudrate,
   num_joints_ = num_joints;
   joint_positions_deg_.resize(num_joints_);
   enc_calibrations_.resize(num_joints_);
+  is_estopped_ = false;
   return true;
 }
 
@@ -92,6 +93,16 @@ void TeensyDriver::getJointPositions(std::vector<double>& joint_positions) {
   joint_positions = joint_positions_deg_;
 }
 
+bool TeensyDriver::resetEStop() {
+  std::string msg = "RE\n";
+  exchange(msg);
+  return !is_estopped_;
+}
+
+bool TeensyDriver::isEStopped() {
+  return is_estopped_;
+}
+
 // Send specific commands
 bool TeensyDriver::sendCommand(std::string outMsg) { return exchange(outMsg); }
 
@@ -120,6 +131,9 @@ bool TeensyDriver::exchange(std::string outMsg) {
   } else if (header == "JP") {
     // encoder steps
     updateJointPositions(inMsg);
+  } else if (header == "ES") {
+    // estop status
+    updateEStopStatus(inMsg);
   } else if (header == "DB") {
     // debug message
     RCLCPP_DEBUG(logger_, "Debug message: %s", inMsg.c_str());
@@ -218,6 +232,10 @@ void TeensyDriver::updateJointPositions(std::string msg) {
   joint_positions_deg_[5] = std::stod(msg.substr(idx6));
 }
 
+void TeensyDriver::updateEStopStatus(std::string msg) {
+  is_estopped_ = msg.substr(2) == "1" ? true : false;
+}
+
 bool TeensyDriver::succeeded(std::string msg) {
   size_t res_idx = msg.find("RES", 2) + 3;
   if (res_idx != std::string::npos && msg[res_idx] == '0') {

diff --git a/ar_microcontrollers/AR4_teensy/AR4_teensy.ino b/ar_microcontrollers/AR4_teensy/AR4_teensy.ino
@@ -15,6 +15,7 @@ String MODEL = "";
 // Physical Params
 ///////////////////////////////////////////////////////////////////////////////
 
+const int ESTOP_PIN = 39;
 const int STEP_PINS[] = {0, 2, 4, 6, 8, 10};
 const int DIR_PINS[] = {1, 3, 5, 7, 9, 11};
 const int LIMIT_PINS[] = {26, 27, 28, 29, 30, 31};
@@ -82,6 +83,39 @@ char JOINT_NAMES[] = {'A', 'B', 'C', 'D', 'E', 'F'};
 // num of encoder steps in range of motion of joint
 int ENC_RANGE_STEPS[NUM_JOINTS];
 
+bool estop_pressed = false;
+
+void estopPressed() {
+  estop_pressed = true;
+}
+
+void resetEstop() {
+  // if ESTOP button is pressed still, do not reset the flag!
+  if (digitalRead(ESTOP_PIN) == LOW) {
+    return;
+  }
+
+  // reset any previously set MT commands
+  for (int i = 0; i < NUM_JOINTS; ++i) {
+    // NOTE: This may seem redundant but is the only permitted way to set
+    // _stepInterval and _n to 0, which is required to avoid a jerk resume
+    // when Estop is reset after interruption of an accelerated motion
+    stepperJoints[i].setCurrentPosition(stepperJoints[i].currentPosition());
+  }
+
+  estop_pressed = false;
+}
+
+bool safeRun(AccelStepper& stepperJoint) {
+  if (estop_pressed) return false;
+  return stepperJoint.run();
+}
+
+bool safeRunSpeed(AccelStepper& stepperJoint) {
+  if (estop_pressed) return false;
+  return stepperJoint.runSpeed();
+}
+
 void setup() {
   MOTOR_STEPS_PER_DEG["mk1"] = MOTOR_STEPS_PER_DEG_MK1;
   MOTOR_STEPS_PER_DEG["mk2"] = MOTOR_STEPS_PER_DEG_MK2;
@@ -106,6 +140,9 @@ void setup() {
     pinMode(DIR_PINS[i], OUTPUT);
     pinMode(LIMIT_PINS[i], INPUT);
   }
+
+  pinMode(ESTOP_PIN, INPUT_PULLUP);
+  attachInterrupt(digitalPinToInterrupt(ESTOP_PIN), estopPressed, FALLING);
 }
 
 void setupSteppersMK1() {
@@ -267,7 +304,7 @@ bool calibrateJoints(int* calJoints) {
         // check limit switches
         if (!reachedLimitSwitch(i)) {
           // limit switch not reached, continue moving
-          stepperJoints[i].runSpeed();
+          safeRunSpeed(stepperJoints[i]);
           calAllDone = false;
         } else {
           // limit switch reached
@@ -291,7 +328,7 @@ void moveAwayFromLimitSwitch() {
   }
   for (int j = 0; j < 10000000; j++) {
     for (int i = 0; i < NUM_JOINTS; ++i) {
-      stepperJoints[i].runSpeed();
+      safeRunSpeed(stepperJoints[i]);
     }
   }
   for (int i = 0; i < NUM_JOINTS; i++) {
@@ -356,7 +393,7 @@ void doCalibrationRoutine() {
           (REST_ENC_POSITIONS[MODEL][i] / ENC_MULT[i] - curMotorSteps[i]) *
           ENC_DIR[i];
       stepperJoints[i].move(target_pos);
-      stepperJoints[i].run();
+      safeRun(stepperJoints[i]);
     }
   }
   bool restPosReached = false;
@@ -365,7 +402,7 @@ void doCalibrationRoutine() {
     restPosReached = true;
     for (int i = 0; i < NUM_JOINTS; ++i) {
       if (abs(stepperJoints[i].distanceToGo()) > 5) {
-        stepperJoints[i].run();
+        safeRun(stepperJoints[i]);
         restPosReached = false;
       }
     }
@@ -374,7 +411,7 @@ void doCalibrationRoutine() {
     // set all speeds to zero
     for (int i = 0; i < NUM_JOINTS; ++i) {
       stepperJoints[i].setSpeed(0);
-      stepperJoints[i].run();
+      safeRun(stepperJoints[i]);
     }
     String msg = String("JCRES0MSG") + "Failed to return to rest position.";
     Serial.println(msg);
@@ -430,9 +467,8 @@ void stateTRAJ() {
         // update trajectory information
         readMotorSteps(curMotorSteps);
 
-        // update host with joint positions
-        encStepsToJointPos(curMotorSteps, curJointPos);
-        String msg = String("JP") + JointPosToString(curJointPos);
+        // update the host about estop state
+        String msg = String("ES") + estop_pressed;
         Serial.println(msg);
 
         // get new position commands
@@ -463,7 +499,7 @@ void stateTRAJ() {
             //   diffMotSteps = diffMotSteps / 2;
             // }
             stepperJoints[i].move(diffMotSteps);
-            stepperJoints[i].run();
+            safeRun(stepperJoints[i]);
           }
         }
       } else if (function == "JC") {
@@ -489,12 +525,17 @@ void stateTRAJ() {
         // update host with joint positions
         String msg = String("JP") + JointPosToString(curJointPos);
         Serial.println(msg);
+      } else if (function == "RE") {
+        resetEstop();
+        // update host with Estop status after trying to reset it
+        String msg = String("ES") + estop_pressed;
+        Serial.println(msg);
       }
 
       inData = "";  // clear message
     }
     for (int i = 0; i < NUM_JOINTS; ++i) {
-      stepperJoints[i].run();
+      safeRun(stepperJoints[i]);
     }
   }
 }