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 <[email protected]>
ros-industrial · Aug 13, 2024 · 62a904a · 62a904a
1 parent eef67ba
commit 62a904a
Show file tree

Hide file tree

Showing 6 changed files with 77 additions and 2 deletions.
diff --git 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
@@ -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
@@ -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
@@ -49,6 +49,7 @@ class NodeCanopen402Driver : public NodeCanopenProxyDriver<NODETYPE>
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_set_mode_velocity_service;
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_set_mode_cyclic_velocity_service;
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_set_mode_cyclic_position_service;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_set_mode_cyclic_torque_service;
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr handle_set_mode_interpolated_position_service;
   rclcpp::Service<canopen_interfaces::srv::COTargetDouble>::SharedPtr handle_set_target_service;
   rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr publish_joint_state;
@@ -318,6 +319,33 @@ class NodeCanopen402Driver : public NodeCanopenProxyDriver<NODETYPE>
    */
   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/...en_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp b/...en_402_driver/include/canopen_402_driver/node_interfaces/node_canopen_402_driver_impl.hpp
@@ -100,6 +100,12 @@ void NodeCanopen402Driver<rclcpp::Node>::init(bool called_from_base)
       &NodeCanopen402Driver<rclcpp::Node>::handle_set_mode_torque, this, std::placeholders::_1,
       std::placeholders::_2));
 
+  handle_set_mode_cyclic_torque_service = this->node_->create_service<std_srvs::srv::Trigger>(
+    std::string(this->node_->get_name()).append("/cyclic_torque_mode").c_str(),
+    std::bind(
+      &NodeCanopen402Driver<rclcpp::Node>::handle_set_mode_cyclic_torque, this,
+      std::placeholders::_1, std::placeholders::_2));
+
   handle_set_target_service = this->node_->create_service<canopen_interfaces::srv::COTargetDouble>(
     std::string(this->node_->get_name()).append("/target").c_str(),
     std::bind(
@@ -168,6 +174,12 @@ void NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::init(bool called_fro
       &NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::handle_set_mode_torque, this,
       std::placeholders::_1, std::placeholders::_2));
 
+  handle_set_mode_cyclic_torque_service = this->node_->create_service<std_srvs::srv::Trigger>(
+    std::string(this->node_->get_name()).append("/cyclic_torque_mode").c_str(),
+    std::bind(
+      &NodeCanopen402Driver<rclcpp_lifecycle::LifecycleNode>::handle_set_mode_cyclic_torque, this,
+      std::placeholders::_1, std::placeholders::_2));
+
   handle_set_target_service = this->node_->create_service<canopen_interfaces::srv::COTargetDouble>(
     std::string(this->node_->get_name()).append("/target").c_str(),
     std::bind(
@@ -413,6 +425,14 @@ void NodeCanopen402Driver<NODETYPE>::handle_set_mode_torque(
   response->success = set_mode_torque();
 }
 
+template <class NODETYPE>
+void NodeCanopen402Driver<NODETYPE>::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 <class NODETYPE>
 void NodeCanopen402Driver<NODETYPE>::handle_set_target(
   const canopen_interfaces::srv::COTargetDouble::Request::SharedPtr request,
@@ -594,6 +614,26 @@ bool NodeCanopen402Driver<NODETYPE>::set_mode_cyclic_velocity()
   }
 }
 
+template <class NODETYPE>
+bool NodeCanopen402Driver<NODETYPE>::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 <class NODETYPE>
 bool NodeCanopen402Driver<NODETYPE>::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
@@ -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: