Added support for Estop #27
Conversation
| @@ -489,12 +517,17 @@ void stateTRAJ() { | |||
| // update host with joint positions | |||
| String msg = String("JP") + JointPosToString(curJointPos); | |||
| Serial.println(msg); | |||
| } else if (function == "RE") { | |||
There was a problem hiding this comment.
should there be a more automatic way of resetting after e-stop? i.e. maybe before starting any command we can check if the estop is still active, and clear the estop accordingly. Currently, it looks like e-stop can only be cleared by sending an RE command and there's no easy way to do that other than by shutting down the ros2_control node and connect to the serial monitor via the arduino IDE and issue the command.
There was a problem hiding this comment.
Fair point, I agree the current way to reset the estop is not very comfortable to use.
If we use automatic resetting (or "run only if estop not pressed") through the "MT" command, the problem is that it is published by the control node at the configured update frequency (100Hz), which would reset the Estop as soon as the user releases the estop push button. Since the button (at least the one available in the robot kits) is a momentary switch, the robot would resume the potentially error prone movement after the user lets go the button. Conventionally for industrial robots, the Estop once set, stays in that state unless its twisted and pulled. This gives the user the changes to ascertain no problems exists before resuming the robot operations.
Potential solutions I can think of is to add a ros service that sends the "RE" command to the robot to reset the estop, which the user could be guided to call when they want to do so, without having to restart anything.
Alternatively, the estop reset can be something that is run when the robot is initialized using the "ST" command or immediately after calibration. That way if the user presses the estop, they would require shutting down the ros control node and restarting it to reset the estop.
What do you think?
There was a problem hiding this comment.
I see, yeah I like the convention to give user the chance to ascertain that no problems exist before resuming operations. I think we could surface the e-stop to the hardware interface level through the teensy driver (i.e. send the teensy driver an ES reply instead of the typical reply for commands like MT) and then the hardware interface can return error (which I think makes the controller deactivated?). When the e-stop is cleared and we're ready to resume, we activate the driver via ros2 control set_controller_state active which calls the RE command to reset the e-stop. This way on the ROS side we stay in an established error handling paradigm, and the user is aware that the software is in an e-stop state. Below are example code snippets illustrating the concept:
returning error during read:
hardware_interface::return_type MyHardwareInterface::read()
{
if (e_stop_condition) {
return hardware_interface::return_type::ERROR;
}
// Normal read operation
return hardware_interface::return_type::OK;
}
resetting e-stop on activate:
controller_interface::return_type MyController::on_activate(
const rclcpp_lifecycle::State & /*previous_state*/)
{
bool sucess = driver_.reset_estop();
if (!success) {
RCLCPP_ERROR(get_node()->get_logger(), "Cannot activate. Hardware in E-stop state.");
return controller_interface::return_type::ERROR;
}
RCLCPP_INFO(get_node()->get_logger(), "Controller activated.");
return controller_interface::return_type::OK;
}
Thoughts? I think both of your ideas should work as well and are quicker to implement, but with this way the user is more informed of the software state, and through log messages we can inform the user how to reset back to a working state.
There was a problem hiding this comment.
I like your idea! It gives more streamlined control to the user and the ros2 controller is kept informed of the state of the hardware through the interface. I'll add in those changes and run some tests to make sure its functional 🤖
There was a problem hiding this comment.
great, btw I have a sizable change coming up in #28 that makes the robot arm move much faster amongst other improvements. Feel free to try it out!
There was a problem hiding this comment.
Implemented the feature as we discussed, with some changes:
- The teensy driver receives the ES status as a response to any MT commands, as a result the hardware interface deactivates when estop is detected within its
write()method (notread()) - Upon returning
hardware_interface::return_type::ERRORfrom thewrite()method, the hardware interface is disabled along with the joint_state controller and broadcaster as well (see image and video below). To reset the estop, I have to set all 3 to active usingros2 control set_hardware_component_state mk3 active ros2 control set_controller_state joint_trajectory_controller active ros2 control set_controller_state joint_state_broadcaster active
- When estop is pressed during a motion, and then reset by activating the interfaces using instructions above, the robot jerks, indicative of trying to resume its acceleration/deceleration profiles. To avoid this, all joints accelerations and step intervals are reset using
stepperJoints[i].setCurrentPosition(stepperJoints[i].currentPosition()), since that is the onlypublicmethod available to do that
With these changes though, the estop functionality seems to work well IMO. I'd like to know if it works for you too
Before estop:
After estop:
video: estop pressed during motion, interfaces are disabled, then estop reset by activating the interfaces back up, which enables using the driver again
There was a problem hiding this comment.
great problem solving to get this working well!
abhijitmajumdar
force-pushed
the
estop-support
branch
from
ce64f7f
to
77c16e7
Compare
| @@ -131,6 +138,18 @@ 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 " | |||
There was a problem hiding this comment.
For convenience, we could put the 3 commands in a bash script in src/ar_hardware_interface/scripts and do something like ros2 run ar_hardware_interface reset_estop.sh
There was a problem hiding this comment.
This is a great idea! I added a script to do that.
I also added some notes about using EStop (and resetting it) in the readme. Feel free to edit/remove those changes as you see fit
- 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
abhijitmajumdar
force-pushed
the
estop-support
branch
from
77c16e7
to
52f4aa8
Compare
There was a problem hiding this comment.
LGTM and thank you very much for the contribution! I added a couple of minor fixes to:
- don't re-calibrate after resetting e-stop, since recalibration takes too long and causes the
ros2 control set_hardware_component_statecommand to fail - update the warning message to tell the user to re-run the reset_estop.sh script
What does this do
Why
I almost broke my robot during calibration because I had noise on the encoder inputs 😅 (ref: https://www.anninrobotics.com/forum/questions/encoders-start-counting-automatically-when-powering-the-motor)