diff --git a/README.md b/README.md index 97566e3..2ba664e 100644 --- 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 +``` +where `` 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 index 17399ea..fe132f6 100644 --- 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 index 7394b3f..4f2f809 100644 --- 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 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 index 656d59a..7c921f3 100644 --- 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& joint_states); void getJointPositions(std::vector& joint_positions); bool calibrateJoints(); + bool resetEStop(); + bool isEStopped(); TeensyDriver(); @@ -34,6 +36,7 @@ class TeensyDriver { int num_joints_; std::vector joint_positions_deg_; std::vector 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 new file mode 100755 index 0000000..f311677 --- /dev/null +++ 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 \n" + printf "\twhere 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 index e6e8100..f9370f1 100644 --- 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 index 08cc409..9866643 100644 --- 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& 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 index 3dd1812..2d832e7 100644 --- 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]); } } }