From 62a904a7011c60541066ca7771d14bcbdc682e17 Mon Sep 17 00:00:00 2001 From: Christoph Hellmann Santos <51296695+hellantos@users.noreply.github.com> Date: Tue, 13 Aug 2024 09:30:25 +0200 Subject: [PATCH] Add cyclic torque mode to cia402 driver and robot system controller (#293) * Add base functions for switching to cyclic torque mode * Add cyclic torque mode as effort interface to robot_system controller * Add documentation about cyclic torque mode. --------- Co-authored-by: Christoph Hellmann Santos --- canopen/sphinx/user-guide/cia402-driver.rst | 3 ++ .../canopen_402_driver/cia402_driver.hpp | 2 + .../lifecycle_cia402_driver.hpp | 2 + .../node_canopen_402_driver.hpp | 28 +++++++++++++ .../node_canopen_402_driver_impl.hpp | 40 +++++++++++++++++++ .../canopen_ros2_control/cia402_data.hpp | 4 +- 6 files changed, 77 insertions(+), 2 deletions(-) diff --git a/canopen/sphinx/user-guide/cia402-driver.rst b/canopen/sphinx/user-guide/cia402-driver.rst index 7f3e998a2..29df0c302 100644 --- a/canopen/sphinx/user-guide/cia402-driver.rst +++ b/canopen/sphinx/user-guide/cia402-driver.rst @@ -55,6 +55,9 @@ Services * - ~/cyclic_velocity_mode - Trigger - Switches to cyclic velocity mode + * - ~/cyclic_torque_mode + - Trigger + - Switches to cyclic torque mode * - ~/interpolated_position_mode - Trigger - Switches to interpolated position mode, only linear mode with fixed time is supported diff --git a/canopen_402_driver/include/canopen_402_driver/cia402_driver.hpp b/canopen_402_driver/include/canopen_402_driver/cia402_driver.hpp index d54eecc8e..8a6505da9 100644 --- a/canopen_402_driver/include/canopen_402_driver/cia402_driver.hpp +++ b/canopen_402_driver/include/canopen_402_driver/cia402_driver.hpp @@ -91,6 +91,8 @@ class Cia402Driver : public ros2_canopen::CanopenDriver bool set_mode_torque() { return node_canopen_402_driver_->set_mode_torque(); } + bool set_mode_cyclic_torque() { return node_canopen_402_driver_->set_mode_cyclic_torque(); } + bool set_mode_interpolated_position() { return node_canopen_402_driver_->set_mode_interpolated_position(); diff --git a/canopen_402_driver/include/canopen_402_driver/lifecycle_cia402_driver.hpp b/canopen_402_driver/include/canopen_402_driver/lifecycle_cia402_driver.hpp index 2ccef5ca3..4d1009f0f 100644 --- a/canopen_402_driver/include/canopen_402_driver/lifecycle_cia402_driver.hpp +++ b/canopen_402_driver/include/canopen_402_driver/lifecycle_cia402_driver.hpp @@ -93,6 +93,8 @@ class LifecycleCia402Driver : public ros2_canopen::LifecycleCanopenDriver bool set_mode_torque() { return node_canopen_402_driver_->set_mode_torque(); } + bool set_mode_cyclic_torque() { return node_canopen_402_driver_->set_mode_cyclic_torque(); } + bool set_mode_interpolated_position() { return node_canopen_402_driver_->set_mode_interpolated_position(); diff --git a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp index 1e38588ed..13a01a013 100644 --- a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp +++ b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver.hpp @@ -49,6 +49,7 @@ class NodeCanopen402Driver : public NodeCanopenProxyDriver rclcpp::Service::SharedPtr handle_set_mode_velocity_service; rclcpp::Service::SharedPtr handle_set_mode_cyclic_velocity_service; rclcpp::Service::SharedPtr handle_set_mode_cyclic_position_service; + rclcpp::Service::SharedPtr handle_set_mode_cyclic_torque_service; rclcpp::Service::SharedPtr handle_set_mode_interpolated_position_service; rclcpp::Service::SharedPtr handle_set_target_service; rclcpp::Publisher::SharedPtr publish_joint_state; @@ -318,6 +319,33 @@ class NodeCanopen402Driver : public NodeCanopenProxyDriver */ bool set_mode_torque(); + /** + * @brief Service Callback to set cyclic torque mode + * + * Calls Motor402::enterModeAndWait with Cyclic Torque Mode as + * Target Operation Mode. If successful, the motor was transitioned + * to Cyclic Torque Mode. + * + * @param [in] request + * @param [out] response + */ + void handle_set_mode_cyclic_torque( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response); + + /** + * @brief Method to set cyclic torque mode + * + * Calls Motor402::enterModeAndWait with Profiled Torque Mode as + * Target Operation Mode. If successful, the motor was transitioned + * to Profiled Torque Mode. + * + * @param [in] void + * + * @return bool + */ + bool set_mode_cyclic_torque(); + /** * @brief Service Callback to set target * diff --git a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp index 48cc5e1b6..0df269647 100644 --- a/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp +++ b/canopen_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp @@ -100,6 +100,12 @@ void NodeCanopen402Driver::init(bool called_from_base) &NodeCanopen402Driver::handle_set_mode_torque, this, std::placeholders::_1, std::placeholders::_2)); + handle_set_mode_cyclic_torque_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/cyclic_torque_mode").c_str(), + std::bind( + &NodeCanopen402Driver::handle_set_mode_cyclic_torque, this, + std::placeholders::_1, std::placeholders::_2)); + handle_set_target_service = this->node_->create_service( std::string(this->node_->get_name()).append("/target").c_str(), std::bind( @@ -168,6 +174,12 @@ void NodeCanopen402Driver::init(bool called_fro &NodeCanopen402Driver::handle_set_mode_torque, this, std::placeholders::_1, std::placeholders::_2)); + handle_set_mode_cyclic_torque_service = this->node_->create_service( + std::string(this->node_->get_name()).append("/cyclic_torque_mode").c_str(), + std::bind( + &NodeCanopen402Driver::handle_set_mode_cyclic_torque, this, + std::placeholders::_1, std::placeholders::_2)); + handle_set_target_service = this->node_->create_service( std::string(this->node_->get_name()).append("/target").c_str(), std::bind( @@ -413,6 +425,14 @@ void NodeCanopen402Driver::handle_set_mode_torque( response->success = set_mode_torque(); } +template +void NodeCanopen402Driver::handle_set_mode_cyclic_torque( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) +{ + response->success = set_mode_cyclic_torque(); +} + template void NodeCanopen402Driver::handle_set_target( const canopen_interfaces::srv::COTargetDouble::Request::SharedPtr request, @@ -594,6 +614,26 @@ bool NodeCanopen402Driver::set_mode_cyclic_velocity() } } +template +bool NodeCanopen402Driver::set_mode_cyclic_torque() +{ + if (this->activated_.load()) + { + if (motor_->getMode() != MotorBase::Cyclic_Synchronous_Torque) + { + return motor_->enterModeAndWait(MotorBase::Cyclic_Synchronous_Torque); + } + else + { + return false; + } + } + else + { + return false; + } +} + template bool NodeCanopen402Driver::set_mode_torque() { diff --git a/canopen_ros2_control/include/canopen_ros2_control/cia402_data.hpp b/canopen_ros2_control/include/canopen_ros2_control/cia402_data.hpp index 3615caf2d..20eb070e0 100644 --- a/canopen_ros2_control/include/canopen_ros2_control/cia402_data.hpp +++ b/canopen_ros2_control/include/canopen_ros2_control/cia402_data.hpp @@ -135,8 +135,7 @@ struct Cia402Data command_interface_to_operation_mode.end()) { // target effort - command_interfaces.emplace_back( - joint_name, hardware_interface::HW_IF_EFFORT, &target_position); + command_interfaces.emplace_back(joint_name, hardware_interface::HW_IF_EFFORT, &target_torque); interfaces.push_back(joint_name + "/" + "effort"); } } @@ -164,6 +163,7 @@ struct Cia402Data driver->set_target(target_velocity); break; case MotorBase::Profiled_Torque: + case MotorBase::Cyclic_Synchronous_Torque: driver->set_target(target_torque); break; default: