remove joint trajectory controller

franka_example_controllers/include/franka_example_controllers/elbow_example_controller.hpp

@@ -47,8 +47,10 @@ class ElbowExampleController : public controller_interface::ControllerInterface
 
   const bool k_elbow_activated_{true};
   std::vector<double> initial_cartesian_velocity_and_elbow;
-
+  bool first_pass_{true};
+  std::array<double, 2> initial_elbow_configuration_{0.0, 0.0};
   double elapsed_time_{0.0};
+  const double traj_frequency_{0.001};
 };
 
 }  // namespace franka_example_controllers

franka_example_controllers/src/cartesian_velocity_example_controller.cpp

@@ -59,7 +59,8 @@ controller_interface::return_type CartesianVelocityExampleController::update(
   if (franka_cartesian_velocity_->setCommand(cartesian_velocity_command)) {
     return controller_interface::return_type::OK;
   } else {
-    std::cout << "SET COMMAND FAILED" << std::endl;
+    RCLCPP_FATAL(get_node()->get_logger(),
+                 "Set command failed. Did you activate the elbow command interface?");
     return controller_interface::return_type::ERROR;
   }
 }

franka_example_controllers/src/elbow_example_controller.cpp

@@ -42,27 +42,30 @@ controller_interface::InterfaceConfiguration ElbowExampleController::state_inter
 controller_interface::return_type ElbowExampleController::update(
     const rclcpp::Time& /*time*/,
     const rclcpp::Duration& /*period*/) {
-  if (first_pass) {
-    // Get start values of the cartesian velocity and elbow command
-    franka_cartesian_velocity_->get_values_command_interfaces(initial_cartesian_velocity_and_elbow);
-    for (auto cart : initial_cartesian_velocity_and_elbow) {
-      std::cout << cart << std::endl;
+  if (first_pass_) {
+    // Get initial elbow configuration values
+    if (!franka_cartesian_velocity_->getCommandedElbowConfiguration(initial_elbow_configuration_)) {
+      RCLCPP_FATAL(get_node()->get_logger(),
+                   "Can't get the initial elbow configuration. Did you activate the elbow in "
+                   "cartesian velocity interface?");
+      return controller_interface::return_type::ERROR;
     }
-    first_pass = false;
-  }
 
-  elapsed_time_ = elapsed_time_ + 0.001;
+    first_pass_ = false;
+  }
+  elapsed_time_ = elapsed_time_ + traj_frequency_;
 
   double angle = M_PI / 10.0 * (1.0 - std::cos(M_PI / 5.0 * elapsed_time_));
 
   std::array<double, 6> cartesian_velocity_command = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
   std::array<double, 2> elbow_command = {
-      {initial_cartesian_velocity_and_elbow[6] + angle, initial_cartesian_velocity_and_elbow[7]}};
+      {initial_elbow_configuration_[0] + angle, initial_elbow_configuration_[1]}};
 
   if (franka_cartesian_velocity_->setCommand(cartesian_velocity_command, elbow_command)) {
     return controller_interface::return_type::OK;
   } else {
-    RCLCPP_FATAL(get_node()->get_logger(), "SET COMMAND FAILED");
+    RCLCPP_FATAL(get_node()->get_logger(),
+                 "Set command failed. Did you activate the elbow command interface?");
     return controller_interface::return_type::ERROR;
   }
 }

franka_hardware/include/franka_hardware/franka_hardware_interface.hpp

@@ -38,8 +38,12 @@ namespace franka_hardware {
 class FrankaHardwareInterface : public hardware_interface::SystemInterface {
  public:
   explicit FrankaHardwareInterface(std::shared_ptr<Robot> robot);
-  FrankaHardwareInterface() = default;
-  virtual ~FrankaHardwareInterface() = default;
+  FrankaHardwareInterface();
+  FrankaHardwareInterface(const FrankaHardwareInterface&) = delete;
+  FrankaHardwareInterface& operator=(const FrankaHardwareInterface& other) = delete;
+  FrankaHardwareInterface& operator=(FrankaHardwareInterface&& other) = delete;
+  FrankaHardwareInterface(FrankaHardwareInterface&& other) = delete;
+  ~FrankaHardwareInterface() override = default;
 
   hardware_interface::return_type prepare_command_mode_switch(
       const std::vector<std::string>& start_interfaces,
@@ -59,12 +63,18 @@ class FrankaHardwareInterface : public hardware_interface::SystemInterface {
   static const size_t kNumberOfJoints = 7;
 
  private:
+  struct InterfaceInfo {
+    std::string interface_type;
+    size_t size;
+    bool& claim_flag;
+  };
+
   std::shared_ptr<Robot> robot_;
   std::shared_ptr<FrankaParamServiceServer> node_;
   std::shared_ptr<FrankaExecutor> executor_;
 
   // Initialize joint position commands in the first pass
-  bool first_pass{true};
+  bool first_pass_{true};
 
   std::array<double, kNumberOfJoints> hw_commands_{0, 0, 0, 0, 0, 0, 0};
 
@@ -98,7 +108,8 @@ class FrankaHardwareInterface : public hardware_interface::SystemInterface {
 
   const std::string k_HW_IF_CARTESIAN_VELOCITY = "cartesian_velocity";
   const std::string k_HW_IF_ELBOW_COMMAND = "elbow_command";
-  const std::string k_joint_velocity_interface = "/velocity";
+
+  const std::vector<InterfaceInfo> command_interfaces_info_;
 
   franka::RobotState hw_franka_robot_state_;
   franka::RobotState* hw_franka_robot_state_addr_ = &hw_franka_robot_state_;

franka_hardware/include/franka_hardware/robot.hpp

@@ -91,6 +91,13 @@ class Robot {
    */
   virtual void writeOnce(const std::array<double, 7>& joint_hardware_command);
 
+  /**
+   * @brief Preprocessing includes rate limiting and low pass filtering, if activated
+   *
+   * @param cartesian_velocities cartesian velocity in libfranka format
+   */
+  virtual void preProcessCartesianVelocities(franka::CartesianVelocities& cartesian_velocities);
+
   /**
    * Cartesian velocity command
    * @param[in] cartesian_velocity_command cartesian level velocity command in format

franka_hardware/src/franka_hardware_interface.cpp

@@ -12,6 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <fmt/core.h>
 #include <algorithm>
 #include <cmath>
 #include <exception>
@@ -33,7 +34,18 @@ using StateInterface = hardware_interface::StateInterface;
 using CommandInterface = hardware_interface::CommandInterface;
 
 FrankaHardwareInterface::FrankaHardwareInterface(std::shared_ptr<Robot> robot)
-    : robot_{std::move(robot)} {}
+    : FrankaHardwareInterface() {
+  robot_ = std::move(robot);  // NOLINT(cppcoreguidelines-prefer-member-initializer)
+}
+
+FrankaHardwareInterface::FrankaHardwareInterface()
+    : command_interfaces_info_({
+          {hardware_interface::HW_IF_EFFORT, kNumberOfJoints, effort_interface_claimed_},
+          {hardware_interface::HW_IF_VELOCITY, kNumberOfJoints, velocity_joint_interface_claimed_},
+          {k_HW_IF_ELBOW_COMMAND, hw_elbow_command_names_.size(), elbow_command_interface_claimed_},
+          {k_HW_IF_CARTESIAN_VELOCITY, hw_cartesian_velocities_.size(),
+           velocity_cartesian_interface_claimed_},
+      }) {}
 
 std::vector<StateInterface> FrankaHardwareInterface::export_state_interfaces() {
   std::vector<StateInterface> state_interfaces;
@@ -69,15 +81,15 @@ std::vector<CommandInterface> FrankaHardwareInterface::export_command_interfaces
 
   // cartesian velocity command interface 6 in order: dx, dy, dz, wx, wy, wz
   for (auto i = 0U; i < hw_cartesian_velocities_.size(); i++) {
-    command_interfaces.emplace_back(CommandInterface(hw_cartesian_velocities_names_[i],
+    command_interfaces.emplace_back(CommandInterface(hw_cartesian_velocities_names_.at(i),
                                                      k_HW_IF_CARTESIAN_VELOCITY,
                                                      &hw_cartesian_velocities_.at(i)));
   }
 
   // elbow command interface
   for (auto i = 0U; i < hw_elbow_command_names_.size(); i++) {
     command_interfaces.emplace_back(CommandInterface(
-        hw_elbow_command_names_[i], k_HW_IF_ELBOW_COMMAND, &hw_elbow_command_.at(i)));
+        hw_elbow_command_names_.at(i), k_HW_IF_ELBOW_COMMAND, &hw_elbow_command_.at(i)));
   }
 
   return command_interfaces;
@@ -108,10 +120,10 @@ hardware_interface::return_type FrankaHardwareInterface::read(const rclcpp::Time
 
   hw_franka_robot_state_ = robot_->readOnce();
 
-  if (first_pass && elbow_command_interface_running_) {
+  if (first_pass_ && elbow_command_interface_running_) {
     // elbow command should be initialized with the first elbow_c read from the robot
     hw_elbow_command_ = hw_franka_robot_state_.elbow_c;
-    first_pass = false;
+    first_pass_ = false;
   }
 
   hw_positions_ = hw_franka_robot_state_.q;
@@ -137,7 +149,7 @@ hardware_interface::return_type FrankaHardwareInterface::write(const rclcpp::Tim
   if (velocity_joint_interface_running_ || effort_interface_running_) {
     robot_->writeOnce(hw_commands_);
   } else if (velocity_cartesian_interface_running_ && elbow_command_interface_running_ &&
-             !first_pass) {
+             !first_pass_) {
     // Wait until the first read pass after robot controller is activated to write the elbow
     // command to the robot
     robot_->writeOnce(hw_cartesian_velocities_, hw_elbow_command_);
@@ -276,65 +288,52 @@ hardware_interface::return_type FrankaHardwareInterface::perform_command_mode_sw
 hardware_interface::return_type FrankaHardwareInterface::prepare_command_mode_switch(
     const std::vector<std::string>& start_interfaces,
     const std::vector<std::string>& stop_interfaces) {
-  auto is_effort_interface = [](const std::string& interface) {
-    return interface.find(hardware_interface::HW_IF_EFFORT) != std::string::npos;
+  auto contains_interface_type = [](const std::string& interface,
+                                    const std::string& interface_type) {
+    size_t slash_position = interface.find('/');
+    if (slash_position != std::string::npos && slash_position + 1 < interface.size()) {
+      std::string after_slash = interface.substr(slash_position + 1);
+      return after_slash == interface_type;
+    }
+    return false;
   };
 
-  auto is_velocity_interface = [=](const std::string& interface) {
-    return interface.find(k_joint_velocity_interface) != std::string::npos;
-  };
+  auto generate_error_message = [this](const std::string& start_stop_command,
+                                       const std::string& interface_name,
+                                       size_t actual_interface_size,
+                                       size_t expected_interface_size) {
+    std::string error_message =
+        fmt::format("Invalid number of {} interfaces to {}. Expected {}, given {}", interface_name,
+                    start_stop_command, expected_interface_size, actual_interface_size);
+    RCLCPP_FATAL(this->getLogger(), "%s", error_message.c_str());
 
-  auto is_cartesian_velocity_interface = [=](const std::string& interface) {
-    return interface.find(k_HW_IF_CARTESIAN_VELOCITY) != std::string::npos;
+    throw std::invalid_argument(error_message);
   };
 
-  auto is_elbow_command_interface = [=](const std::string& interface) {
-    return interface.find(k_HW_IF_ELBOW_COMMAND) != std::string::npos;
-  };
-
-  auto generate_error_message =
-      [&](const std::string& start_stop_command, const std::string& interface_name,
-          int64_t actual_interface_size, int64_t expected_interface_size) {
-        RCLCPP_FATAL(this->getLogger(), "Expected %ld %s interfaces to %s, but got %ld instead.",
-                     expected_interface_size, interface_name.c_str(), start_stop_command.c_str(),
-                     actual_interface_size);
-        std::ostringstream error_message_stream;
-        error_message_stream << "Invalid number of " << interface_name << " interfaces to "
-                             << start_stop_command << ". Expected " << expected_interface_size;
-        std::string error_message = error_message_stream.str();
-
-        throw std::invalid_argument(error_message);
-      };
-
-  auto start_stop_command_interface =
-      [&](const std::function<bool(const std::string& interface)>& find_interface_function,
-          const std::string& interface_name, int64_t interface_size, bool& claim_flag) {
-        int64_t num_stop_interface =
-            std::count_if(stop_interfaces.begin(), stop_interfaces.end(), find_interface_function);
-        int64_t num_start_interface = std::count_if(
-            start_interfaces.begin(), start_interfaces.end(), find_interface_function);
-
-        if (num_stop_interface == interface_size) {
-          claim_flag = false;
-        } else if (num_stop_interface != 0) {
-          generate_error_message("stop", interface_name, num_stop_interface, interface_size);
-        }
-        if (num_start_interface == interface_size) {
-          claim_flag = true;
-        } else if (num_start_interface != 0) {
-          generate_error_message("start", interface_name, num_start_interface, interface_size);
-        }
-      };
-
-  start_stop_command_interface(is_effort_interface, hardware_interface::HW_IF_EFFORT,
-                               kNumberOfJoints, effort_interface_claimed_);
-  start_stop_command_interface(is_velocity_interface, hardware_interface::HW_IF_VELOCITY,
-                               kNumberOfJoints, velocity_joint_interface_claimed_);
-  start_stop_command_interface(is_cartesian_velocity_interface, k_HW_IF_CARTESIAN_VELOCITY,
-                               hw_cartesian_velocities_.size(),
-                               velocity_cartesian_interface_claimed_);
-  start_stop_command_interface(is_elbow_command_interface, k_HW_IF_ELBOW_COMMAND,
-                               hw_elbow_command_.size(), elbow_command_interface_claimed_);
+  for (const auto& interface : command_interfaces_info_) {
+    size_t num_stop_interface =
+        std::count_if(stop_interfaces.begin(), stop_interfaces.end(),
+                      [contains_interface_type, &interface](const std::string& interface_given) {
+                        return contains_interface_type(interface_given, interface.interface_type);
+                      });
+    size_t num_start_interface =
+        std::count_if(start_interfaces.begin(), start_interfaces.end(),
+                      [contains_interface_type, &interface](const std::string& interface_given) {
+                        return contains_interface_type(interface_given, interface.interface_type);
+                      });
+
+    if (num_stop_interface == interface.size) {
+      interface.claim_flag = false;
+    } else if (num_stop_interface != 0U) {
+      generate_error_message("stop", interface.interface_type, num_stop_interface, interface.size);
+    }
+    if (num_start_interface == interface.size) {
+      interface.claim_flag = true;
+    } else if (num_start_interface != 0U) {
+      generate_error_message("start", interface.interface_type, num_start_interface,
+                             interface.size);
+    }
+  }
 
   return hardware_interface::return_type::OK;
 }

franka_hardware/src/robot.cpp

@@ -99,16 +99,17 @@ void Robot::writeOnceJointVelocities(const std::array<double, 7>& velocities) {
   active_control_->writeOnce(velocity_command);
 }
 
-void Robot::writeOnce(const std::array<double, 6>& cartesian_velocities) {
-  std::lock_guard<std::mutex> lock(control_mutex_);
-
-  auto velocity_command = franka::CartesianVelocities(cartesian_velocities);
-
+void Robot::preProcessCartesianVelocities(franka::CartesianVelocities& velocity_command) {
   if (cartesian_velocity_low_pass_filter_active) {
     for (size_t i = 0; i < 6; i++) {
-      velocity_command.O_dP_EE[i] =
-          franka::lowpassFilter(franka::kDeltaT, velocity_command.O_dP_EE[i],
-                                current_state_.O_dP_EE_c[i], low_pass_filter_cut_off_freq);
+      velocity_command.O_dP_EE.at(i) =
+          franka::lowpassFilter(franka::kDeltaT, velocity_command.O_dP_EE.at(i),
+                                current_state_.O_dP_EE_c.at(i), low_pass_filter_cut_off_freq);
+    }
+    if (velocity_command.hasElbow()) {
+      velocity_command.elbow[0] =
+          franka::lowpassFilter(franka::kDeltaT, velocity_command.elbow[0],
+                                current_state_.elbow_c[0], low_pass_filter_cut_off_freq);
     }
   }
 
@@ -120,8 +121,24 @@ void Robot::writeOnce(const std::array<double, 6>& cartesian_velocities) {
         franka::kMaxTranslationalJerk, franka::kMaxRotationalVelocity,
         franka::kMaxRotationalAcceleration, franka::kMaxRotationalJerk, velocity_command.O_dP_EE,
         current_state_.O_dP_EE_c, current_state_.O_ddP_EE_c);
+    if (velocity_command.hasElbow()) {
+      velocity_command.elbow[0] = franka::limitRate(
+          franka::kMaxElbowVelocity, franka::kMaxElbowAcceleration, franka::kMaxElbowJerk,
+          velocity_command.elbow[0], current_state_.elbow_c[0], current_state_.delbow_c[0],
+          current_state_.ddelbow_c[0]);
+    }
   }
   franka::checkFinite(velocity_command.O_dP_EE);
+  if (velocity_command.hasElbow()) {
+    franka::checkElbow(velocity_command.elbow);
+  }
+}
+
+void Robot::writeOnce(const std::array<double, 6>& cartesian_velocities) {
+  std::lock_guard<std::mutex> lock(control_mutex_);
+
+  auto velocity_command = franka::CartesianVelocities(cartesian_velocities);
+  preProcessCartesianVelocities(velocity_command);
 
   active_control_->writeOnce(velocity_command);
 }
@@ -131,32 +148,7 @@ void Robot::writeOnce(const std::array<double, 6>& cartesian_velocities,
   std::lock_guard<std::mutex> lock(control_mutex_);
 
   auto velocity_command = franka::CartesianVelocities(cartesian_velocities, elbow_command);
-
-  if (cartesian_velocity_low_pass_filter_active) {
-    for (size_t i = 0; i < 6; i++) {
-      velocity_command.O_dP_EE[i] =
-          franka::lowpassFilter(franka::kDeltaT, velocity_command.O_dP_EE[i],
-                                current_state_.O_dP_EE_c[i], low_pass_filter_cut_off_freq);
-      velocity_command.elbow[0] =
-          franka::lowpassFilter(franka::kDeltaT, velocity_command.elbow[0],
-                                current_state_.elbow_c[0], low_pass_filter_cut_off_freq);
-    }
-  }
-  // If you are experiencing issues with robot error. You can try activating the rate
-  // limiter. Rate limiter is default deactivated (cartesian_velocity_command_rate_limit_active_)
-  if (cartesian_velocity_command_rate_limit_active_) {
-    velocity_command.O_dP_EE = franka::limitRate(
-        franka::kMaxTranslationalVelocity, franka::kMaxTranslationalAcceleration,
-        franka::kMaxTranslationalJerk, franka::kMaxRotationalVelocity,
-        franka::kMaxRotationalAcceleration, franka::kMaxRotationalJerk, velocity_command.O_dP_EE,
-        current_state_.O_dP_EE_c, current_state_.O_ddP_EE_c);
-    velocity_command.elbow[0] = franka::limitRate(
-        franka::kMaxElbowVelocity, franka::kMaxElbowAcceleration, franka::kMaxElbowJerk,
-        velocity_command.elbow[0], current_state_.elbow_c[0], current_state_.delbow_c[0],
-        current_state_.ddelbow_c[0]);
-  }
-  franka::checkFinite(velocity_command.O_dP_EE);
-  franka::checkElbow(velocity_command.elbow);
+  preProcessCartesianVelocities(velocity_command);
 
   active_control_->writeOnce(velocity_command);
 }