Add twist to hardware interface

Author: urfeex

ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp

@@ -83,6 +83,7 @@ enum StoppingInterface
   STOP_FREEDRIVE,
   STOP_TOOL_CONTACT,
   STOP_TORQUE,
+  STOP_TWIST,
 };
 
 // We define our own quaternion to use it as a buffer, since we need to pass pointers to the state
@@ -179,6 +180,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   urcl::vector6d_t urcl_position_commands_old_;
   urcl::vector6d_t urcl_velocity_commands_;
   urcl::vector6d_t urcl_torque_commands_;
+  urcl::vector6d_t urcl_twist_commands_;
   urcl::vector6d_t urcl_joint_positions_;
   urcl::vector6d_t urcl_joint_velocities_;
   urcl::vector6d_t urcl_joint_efforts_;
@@ -260,6 +262,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   urcl::vector6d_t passthrough_trajectory_accelerations_;
   double passthrough_trajectory_time_from_start_;
 
+  bool twist_controller_running_;
+
   // payload stuff
   urcl::vector3d_t payload_center_of_gravity_;
   double payload_mass_;
@@ -322,6 +326,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   const std::string FORCE_MODE_GPIO = "force_mode";
   const std::string FREEDRIVE_MODE_GPIO = "freedrive_mode";
   const std::string TOOL_CONTACT_GPIO = "tool_contact";
+  const std::string TWIST_GPIO = "twist";
 
   std::unordered_map<std::string, std::unordered_map<std::string, bool>> mode_compatibility_;
 };

ur_robot_driver/src/hardware_interface.cpp

@@ -65,13 +65,15 @@ URPositionHardwareInterface::URPositionHardwareInterface()
   mode_compatibility_[hardware_interface::HW_IF_POSITION][PASSTHROUGH_GPIO] = false;
   mode_compatibility_[hardware_interface::HW_IF_POSITION][FREEDRIVE_MODE_GPIO] = false;
   mode_compatibility_[hardware_interface::HW_IF_POSITION][TOOL_CONTACT_GPIO] = true;
+  mode_compatibility_[hardware_interface::HW_IF_POSITION][TWIST_GPIO] = false;
 
   mode_compatibility_[hardware_interface::HW_IF_VELOCITY][hardware_interface::HW_IF_POSITION] = false;
   mode_compatibility_[hardware_interface::HW_IF_VELOCITY][hardware_interface::HW_IF_EFFORT] = false;
   mode_compatibility_[hardware_interface::HW_IF_VELOCITY][FORCE_MODE_GPIO] = false;
   mode_compatibility_[hardware_interface::HW_IF_VELOCITY][PASSTHROUGH_GPIO] = false;
   mode_compatibility_[hardware_interface::HW_IF_VELOCITY][FREEDRIVE_MODE_GPIO] = false;
   mode_compatibility_[hardware_interface::HW_IF_VELOCITY][TOOL_CONTACT_GPIO] = true;
+  mode_compatibility_[hardware_interface::HW_IF_VELOCITY][TWIST_GPIO] = true;
 
   mode_compatibility_[hardware_interface::HW_IF_EFFORT][hardware_interface::HW_IF_POSITION] = false;
   mode_compatibility_[hardware_interface::HW_IF_EFFORT][hardware_interface::HW_IF_VELOCITY] = false;
@@ -86,27 +88,38 @@ URPositionHardwareInterface::URPositionHardwareInterface()
   mode_compatibility_[FORCE_MODE_GPIO][PASSTHROUGH_GPIO] = true;
   mode_compatibility_[FORCE_MODE_GPIO][FREEDRIVE_MODE_GPIO] = false;
   mode_compatibility_[FORCE_MODE_GPIO][TOOL_CONTACT_GPIO] = false;
+  mode_compatibility_[FORCE_MODE_GPIO][TWIST_GPIO] = false;
 
   mode_compatibility_[PASSTHROUGH_GPIO][hardware_interface::HW_IF_POSITION] = false;
   mode_compatibility_[PASSTHROUGH_GPIO][hardware_interface::HW_IF_VELOCITY] = false;
   mode_compatibility_[PASSTHROUGH_GPIO][hardware_interface::HW_IF_EFFORT] = false;
   mode_compatibility_[PASSTHROUGH_GPIO][FORCE_MODE_GPIO] = true;
   mode_compatibility_[PASSTHROUGH_GPIO][FREEDRIVE_MODE_GPIO] = false;
   mode_compatibility_[PASSTHROUGH_GPIO][TOOL_CONTACT_GPIO] = true;
+  mode_compatibility_[PASSTHROUGH_GPIO][TWIST_GPIO] = false;
 
   mode_compatibility_[FREEDRIVE_MODE_GPIO][hardware_interface::HW_IF_POSITION] = false;
   mode_compatibility_[FREEDRIVE_MODE_GPIO][hardware_interface::HW_IF_VELOCITY] = false;
   mode_compatibility_[FREEDRIVE_MODE_GPIO][hardware_interface::HW_IF_EFFORT] = false;
   mode_compatibility_[FREEDRIVE_MODE_GPIO][FORCE_MODE_GPIO] = false;
   mode_compatibility_[FREEDRIVE_MODE_GPIO][PASSTHROUGH_GPIO] = false;
   mode_compatibility_[FREEDRIVE_MODE_GPIO][TOOL_CONTACT_GPIO] = false;
+  mode_compatibility_[FREEDRIVE_MODE_GPIO][TWIST_GPIO] = false;
 
   mode_compatibility_[TOOL_CONTACT_GPIO][hardware_interface::HW_IF_POSITION] = true;
   mode_compatibility_[TOOL_CONTACT_GPIO][hardware_interface::HW_IF_VELOCITY] = true;
   mode_compatibility_[TOOL_CONTACT_GPIO][hardware_interface::HW_IF_EFFORT] = true;
   mode_compatibility_[TOOL_CONTACT_GPIO][FORCE_MODE_GPIO] = false;
   mode_compatibility_[TOOL_CONTACT_GPIO][PASSTHROUGH_GPIO] = true;
   mode_compatibility_[TOOL_CONTACT_GPIO][FREEDRIVE_MODE_GPIO] = false;
+  mode_compatibility_[TOOL_CONTACT_GPIO][TWIST_GPIO] = true;
+
+  mode_compatibility_[TWIST_GPIO][hardware_interface::HW_IF_POSITION] = false;
+  mode_compatibility_[TWIST_GPIO][hardware_interface::HW_IF_VELOCITY] = false;
+  mode_compatibility_[TWIST_GPIO][FORCE_MODE_GPIO] = false;
+  mode_compatibility_[TWIST_GPIO][PASSTHROUGH_GPIO] = false;
+  mode_compatibility_[TWIST_GPIO][FREEDRIVE_MODE_GPIO] = false;
+  mode_compatibility_[TWIST_GPIO][TOOL_CONTACT_GPIO] = true;
 }
 
 URPositionHardwareInterface::~URPositionHardwareInterface()
@@ -131,12 +144,14 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareComponent
   urcl_position_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
   urcl_position_commands_old_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
   urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
+  urcl_twist_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
   position_controller_running_ = false;
   velocity_controller_running_ = false;
   torque_controller_running_ = false;
   freedrive_mode_controller_running_ = false;
   passthrough_trajectory_controller_running_ = false;
   tool_contact_controller_running_ = false;
+  twist_controller_running_ = false;
   runtime_state_ = static_cast<uint32_t>(rtde::RUNTIME_STATE::STOPPED);
   pausing_state_ = PausingState::RUNNING;
   pausing_ramp_up_increment_ = 0.01;
@@ -482,6 +497,19 @@ std::vector<hardware_interface::CommandInterface> URPositionHardwareInterface::e
                                                                          &passthrough_trajectory_accelerations_[i]));
   }
 
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface(tf_prefix + TWIST_GPIO, "linear_velocity_x", &urcl_twist_commands_[0]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface(tf_prefix + TWIST_GPIO, "linear_velocity_y", &urcl_twist_commands_[1]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface(tf_prefix + TWIST_GPIO, "linear_velocity_z", &urcl_twist_commands_[2]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface(tf_prefix + TWIST_GPIO, "angular_velocity_x", &urcl_twist_commands_[3]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface(tf_prefix + TWIST_GPIO, "angular_velocity_y", &urcl_twist_commands_[4]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface(tf_prefix + TWIST_GPIO, "angular_velocity_z", &urcl_twist_commands_[5]));
+
   command_interfaces.emplace_back(hardware_interface::CommandInterface(
       tf_prefix + TOOL_CONTACT_GPIO, "tool_contact_set_state", &tool_contact_set_state_));
 
@@ -884,6 +912,8 @@ hardware_interface::return_type URPositionHardwareInterface::write(const rclcpp:
           urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, 0,
           urcl::RobotReceiveTimeout::millisec(1000 * 5.0 / static_cast<double>(info_.rw_rate)));
       check_passthrough_trajectory_controller();
+    } else if (twist_controller_running_) {
+      ur_driver_->writeJointCommand(urcl_twist_commands_, urcl::comm::ControlMode::MODE_SPEEDL, receive_timeout_);
     } else {
       ur_driver_->writeKeepalive();
     }
@@ -1170,6 +1200,9 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
     if (tool_contact_controller_running_) {
       control_modes[i].push_back(TOOL_CONTACT_GPIO);
     }
+    if (twist_controller_running_) {
+      control_modes[i].push_back(TWIST_GPIO);
+    }
   }
 
   auto is_mode_compatible = [this](const std::string& mode, const std::vector<std::string>& other_modes) {
@@ -1197,7 +1230,8 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
         { tf_prefix + FORCE_MODE_GPIO + "/type", FORCE_MODE_GPIO },
         { tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i), PASSTHROUGH_GPIO },
         { tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success", FREEDRIVE_MODE_GPIO },
-        { tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_set_state", TOOL_CONTACT_GPIO }
+        { tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_set_state", TOOL_CONTACT_GPIO },
+        { tf_prefix + TWIST_GPIO + "/linear_velocity_x", TWIST_GPIO }
       };
 
       for (auto& item : start_modes_to_check) {
@@ -1247,7 +1281,8 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
           StoppingInterface::STOP_PASSTHROUGH },
         { tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success", FREEDRIVE_MODE_GPIO, StoppingInterface::STOP_FREEDRIVE },
         { tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_set_state", TOOL_CONTACT_GPIO,
-          StoppingInterface::STOP_TOOL_CONTACT }
+          StoppingInterface::STOP_TOOL_CONTACT },
+        { tf_prefix + TWIST_GPIO + "/linear_velocity_x", TWIST_GPIO, StoppingInterface::STOP_TWIST }
       };
       for (auto& item : stop_modes_to_check) {
         if (key == std::get<0>(item)) {
@@ -1312,13 +1347,20 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod
     freedrive_activated_ = false;
     freedrive_mode_abort_ = 1.0;
   }
+
   if (stop_modes_.size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(),
                                            StoppingInterface::STOP_TOOL_CONTACT) != stop_modes_[0].end()) {
     tool_contact_controller_running_ = false;
     tool_contact_result_ = 3.0;
     ur_driver_->endToolContact();
   }
 
+  if (stop_modes_.size() != 0 &&
+      std::find(stop_modes_[0].begin(), stop_modes_[0].end(), StoppingInterface::STOP_TWIST) != stop_modes_[0].end()) {
+    twist_controller_running_ = false;
+    urcl_twist_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
+  }
+
   if (start_modes_.size() != 0 && std::find(start_modes_[0].begin(), start_modes_[0].end(),
                                             hardware_interface::HW_IF_POSITION) != start_modes_[0].end()) {
     velocity_controller_running_ = false;
@@ -1366,6 +1408,12 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod
       std::find(start_modes_[0].begin(), start_modes_[0].end(), TOOL_CONTACT_GPIO) != start_modes_[0].end()) {
     tool_contact_controller_running_ = true;
   }
+  if (start_modes_[0].size() != 0 &&
+      std::find(start_modes_[0].begin(), start_modes_[0].end(), TWIST_GPIO) != start_modes_[0].end()) {
+    velocity_controller_running_ = false;
+    position_controller_running_ = false;
+    twist_controller_running_ = true;
+  }
   start_modes_.clear();
   stop_modes_.clear();
 

ur_robot_driver/urdf/ur.ros2_control.xacro

@@ -298,6 +298,14 @@
         <command_interface name="trajectory_size"/>
       </gpio>
 
+      <gpio name="${tf_prefix}twist">
+        <command_interface name="linear_velocity_x"/>
+        <command_interface name="linear_velocity_y"/>
+        <command_interface name="linear_velocity_z"/>
+        <command_interface name="angular_velocity_x"/>
+        <command_interface name="angular_velocity_y"/>
+        <command_interface name="angular_velocity_z"/>
+      </gpio>
 
       <gpio name="${tf_prefix}get_robot_software_version">
         <state_interface name="get_version_major">