Add Force-Gated Position and Velocity Controllers

force_gate_controller/CMakeLists.txt

@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 3.16)
 project(force_gate_controller LANGUAGES CXX)
 
-if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") 
   add_compile_options(-Wall -Wextra -Wconversion)
 endif()
 
@@ -12,13 +12,16 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   controller_interface
   generate_parameter_library
   hardware_interface
+  moveit_ros_control_interface
   pluginlib
+  position_controllers
   rclcpp
   rclcpp_lifecycle
   realtime_tools
   rsl
   tl_expected
   trajectory_msgs
+  velocity_controllers
 )
 
 find_package(ament_cmake REQUIRED)
@@ -31,10 +34,18 @@ generate_parameter_library(force_gate_controller_parameters
   src/force_gate_controller_parameters.yaml
   include/force_gate_controller/validate_jtc_parameters.hpp
 )
+# generate_parameter_library(force_gate_position_velocity_controller_parameters
+#   src/force_gate_position_velocity_controller_parameters.yaml
+# )
 
 add_library(force_gate_controller SHARED
   src/force_gate_controller.cpp
+  src/force_gate_parent.cpp
+  src/force_gate_position_controller.cpp
+  src/force_gate_velocity_controller.cpp
+  src/tolerances.cpp
   src/trajectory.cpp
+  src/validate_jtc_parameters.cpp
 )
 target_compile_features(force_gate_controller PUBLIC cxx_std_17)
 target_include_directories(force_gate_controller PUBLIC
@@ -43,13 +54,15 @@ target_include_directories(force_gate_controller PUBLIC
 )
 target_link_libraries(force_gate_controller PUBLIC
   force_gate_controller_parameters
+  # force_gate_position_velocity_controller_parameters
 )
 ament_target_dependencies(force_gate_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
 target_compile_definitions(force_gate_controller PRIVATE "FORCE_GATE_CONTROLLER_BUILDING_DLL" "_USE_MATH_DEFINES")
-pluginlib_export_plugin_description_file(controller_interface force_gate_plugin.xml)
+pluginlib_export_plugin_description_file(controller_interface force_gate_controller_plugin.xml)
+pluginlib_export_plugin_description_file(moveit_ros_control_interface force_gate_controller_allocator_plugin.xml)
 
 if(BUILD_TESTING)
   find_package(ament_cmake_gmock REQUIRED)
@@ -97,6 +110,7 @@ install(
 install(TARGETS
     force_gate_controller
     force_gate_controller_parameters
+    # force_gate_position_velocity_controller_parameters
   EXPORT export_force_gate_controller
   RUNTIME DESTINATION bin
   ARCHIVE DESTINATION lib

force_gate_controller/force_gate_controller_allocator_plugin.xml

@@ -0,0 +1,7 @@
+<library path="moveit_ros_control_interface_trajectory_plugin">
+  <class name="force_gate_controller/ForceGateController" type="moveit_ros_control_interface::JointTrajectoryControllerAllocator" base_class_type="moveit_ros_control_interface::ControllerHandleAllocator">
+    <description>
+      A ControllerAllocator for the force gates joint trajectory controller, to enable it to be controlled by MoveIt's Ros2ControlManager.
+    </description>
+  </class>
+</library>

force_gate_controller/force_gate_controller_plugin.xml

@@ -0,0 +1,21 @@
+<library path="force_gate_controller">
+  <class name="force_gate_controller/ForceGateController" type="force_gate_controller::ForceGateController" base_class_type="controller_interface::ControllerInterface">
+  <description>
+    The joint trajectory controller executes joint-space trajectories on a set of joints and allows for Force / Torque safety thresholding.
+  </description>
+  </class>
+</library>
+<library path="force_gate_controller">
+  <class name="force_gate_controller/ForceGateVelocityController" type="force_gate_controller::ForceGateVelocityController" base_class_type="controller_interface::ControllerInterface">
+  <description>
+    The joint group velocity controller executes joint-space trajectories on a set of joints and allows for Force / Torque safety thresholding.
+  </description>
+  </class>
+</library>
+<library path="force_gate_controller">
+  <class name="force_gate_controller/ForceGatePositionController" type="force_gate_controller::ForceGatePositionController" base_class_type="controller_interface::ControllerInterface">
+  <description>
+    The joint group position controller executes joint-space trajectories on a set of joints and allows for Force / Torque safety thresholding.
+  </description>
+  </class>
+</library>

force_gate_controller/include/force_gate_controller/force_gate_parent.hpp

@@ -0,0 +1,52 @@
+// Copyright 2020 PAL Robotics S.L.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef FORCE_GATE_CONTROLLER__FORCE_GATE_PARENT_HPP_
+#define FORCE_GATE_CONTROLLER__FORCE_GATE_PARENT_HPP_
+
+// #include <string>
+
+#include "geometry_msgs/msg/wrench_stamped.hpp"
+#include "force_gate_controller/tolerances.hpp"
+#include "rclcpp/time.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "realtime_tools/realtime_buffer.h"
+
+// auto-generated by generate_parameter_library
+#include "force_gate_controller_parameters.hpp"
+
+namespace force_gate_controller
+{
+/**
+ * \brief Defines shared functions and attributes to force-gate a controller.
+ */
+class ForceGateParent
+{
+protected:
+  controller_interface::CallbackReturn read_force_gate_parameters(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node);
+
+  WrenchTolerances wrench_tolerances_;
+  realtime_tools::RealtimeBuffer<std::shared_ptr<geometry_msgs::msg::WrenchStamped>> rt_wrench_stamped_;
+  rclcpp::Subscription<geometry_msgs::msg::WrenchStamped>::SharedPtr wrench_subscriber_ = nullptr;
+
+  // Parameters from ROS for force_gate_controller
+  std::shared_ptr<ParamListener> force_gate_param_listener_;
+  Params force_gate_params_;
+
+  bool check_wrench_threshold(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node, const rclcpp::Time & time);
+};
+
+}  // namespace force_gate_controller
+
+#endif  // FORCE_GATE_CONTROLLER__FORCE_GATE_PARENT_HPP_

force_gate_controller/include/force_gate_controller/force_gate_position_controller.hpp

@@ -0,0 +1,60 @@
+// Copyright 2020 PAL Robotics S.L.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef FORCE_GATE_CONTROLLER__FORCE_GATE_POSITION_CONTROLLER_HPP_
+#define FORCE_GATE_CONTROLLER__FORCE_GATE_POSITION_CONTROLLER_HPP_
+
+// #include <string>
+
+#include "geometry_msgs/msg/wrench_stamped.hpp"
+#include "position_controllers/joint_group_position_controller.hpp"
+#include "force_gate_controller/force_gate_parent.hpp"
+#include "force_gate_controller/visibility_control.h"
+#include "force_gate_controller/tolerances.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+
+namespace force_gate_controller
+{
+/**
+ * \brief A force-gated version of the JointGroupPositionController.
+ * 
+ * Forward command controller for a set of position controlled joints (linear or angular).
+ *
+ * This class forwards the commanded positions down to a set of joints.
+ *
+ * \param joints Names of the joints to control.
+ *
+ * Subscribes to:
+ * - \b commands (std_msgs::msg::Float64MultiArray) : The position commands to apply.
+ */
+class ForceGatePositionController : public position_controllers::JointGroupPositionController, public ForceGateParent
+{
+public:
+  FORCE_GATE_CONTROLLER_PUBLIC
+  ForceGatePositionController();
+
+  FORCE_GATE_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
+
+  FORCE_GATE_CONTROLLER_PUBLIC
+  controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+protected:
+  // Overridden functions to read parameters from ROS.
+  controller_interface::CallbackReturn read_parameters() override;
+};
+
+}  // namespace force_gate_controller
+
+#endif  // FORCE_GATE_CONTROLLER__FORCE_GATE_POSITION_CONTROLLER_HPP_

force_gate_controller/include/force_gate_controller/force_gate_velocity_controller.hpp

@@ -0,0 +1,58 @@
+// Copyright 2020 PAL Robotics S.L.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef FORCE_GATE_CONTROLLER__FORCE_GATE_VELOCITY_CONTROLLER_HPP_
+#define FORCE_GATE_CONTROLLER__FORCE_GATE_VELOCITY_CONTROLLER_HPP_
+
+// #include <string>
+
+#include "geometry_msgs/msg/wrench_stamped.hpp"
+#include "velocity_controllers/joint_group_velocity_controller.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "force_gate_controller/force_gate_parent.hpp"
+#include "force_gate_controller/tolerances.hpp"
+#include "force_gate_controller/visibility_control.h"
+
+namespace force_gate_controller
+{
+/**
+ * \brief Forward command controller for a set of velocity controlled joints (linear or angular).
+ *
+ * This class forwards the commanded velocities down to a set of joints.
+ *
+ * \param joints Names of the joints to control.
+ *
+ * Subscribes to:
+ * - \b command (std_msgs::msg::Float64MultiArray) : The velocity commands to apply.
+ */
+class ForceGateVelocityController : public velocity_controllers::JointGroupVelocityController, public ForceGateParent
+{
+public:
+  FORCE_GATE_CONTROLLER_PUBLIC
+  ForceGateVelocityController();
+
+  FORCE_GATE_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
+
+  FORCE_GATE_CONTROLLER_PUBLIC
+  controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+protected:
+  // Overridden functions to read parameters from ROS.
+  controller_interface::CallbackReturn read_parameters() override;
+};
+
+}  // namespace force_gate_controller
+
+#endif  // FORCE_GATE_CONTROLLER__FORCE_GATE_VELOCITY_CONTROLLER_HPP_

force_gate_controller/include/force_gate_controller/tolerances.hpp

@@ -105,33 +105,7 @@ struct SegmentTolerances
  * \param params The ROS Parameters
  * \return Trajectory segment tolerances.
  */
-SegmentTolerances get_segment_tolerances(Params const & params)
-{
-  auto const & constraints = params.constraints;
-  auto const n_joints = params.joints.size();
-
-  SegmentTolerances tolerances;
-  tolerances.goal_time_tolerance = constraints.goal_time;
-
-  // State and goal state tolerances
-  tolerances.state_tolerance.resize(n_joints);
-  tolerances.goal_state_tolerance.resize(n_joints);
-  for (size_t i = 0; i < n_joints; ++i)
-  {
-    auto const joint = params.joints[i];
-    tolerances.state_tolerance[i].position = constraints.joints_map.at(joint).trajectory;
-    tolerances.goal_state_tolerance[i].position = constraints.joints_map.at(joint).goal;
-    tolerances.goal_state_tolerance[i].velocity = constraints.stopped_velocity_tolerance;
-
-    auto logger = rclcpp::get_logger("tolerance");
-    RCLCPP_DEBUG(
-      logger, "%s %f", (joint + ".trajectory").c_str(), tolerances.state_tolerance[i].position);
-    RCLCPP_DEBUG(
-      logger, "%s %f", (joint + ".goal").c_str(), tolerances.goal_state_tolerance[i].position);
-  }
-
-  return tolerances;
-}
+SegmentTolerances get_segment_tolerances(Params const & params);
 
 /**
  * \param state_error State error to check.
@@ -140,54 +114,9 @@ SegmentTolerances get_segment_tolerances(Params const & params)
  * \param show_errors If the joint that violate its tolerance should be output to console. NOT
  * REALTIME if true \return True if \p state_error fulfills \p state_tolerance.
  */
-inline bool check_state_tolerance_per_joint(
+bool check_state_tolerance_per_joint(
   const trajectory_msgs::msg::JointTrajectoryPoint & state_error, size_t joint_idx,
-  const StateTolerances & state_tolerance, bool show_errors = false)
-{
-  using std::abs;
-  const double error_position = state_error.positions[joint_idx];
-  const double error_velocity =
-    state_error.velocities.empty() ? 0.0 : state_error.velocities[joint_idx];
-  const double error_acceleration =
-    state_error.accelerations.empty() ? 0.0 : state_error.accelerations[joint_idx];
-
-  const bool is_valid =
-    !(state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) &&
-    !(state_tolerance.velocity > 0.0 && abs(error_velocity) > state_tolerance.velocity) &&
-    !(state_tolerance.acceleration > 0.0 && abs(error_acceleration) > state_tolerance.acceleration);
-
-  if (is_valid)
-  {
-    return true;
-  }
-
-  if (show_errors)
-  {
-    const auto logger = rclcpp::get_logger("tolerances");
-    RCLCPP_ERROR(logger, "Path state tolerances failed:");
-
-    if (state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position)
-    {
-      RCLCPP_ERROR(
-        logger, "Position Error: %f, Position Tolerance: %f", error_position,
-        state_tolerance.position);
-    }
-    if (state_tolerance.velocity > 0.0 && abs(error_velocity) > state_tolerance.velocity)
-    {
-      RCLCPP_ERROR(
-        logger, "Velocity Error: %f, Velocity Tolerance: %f", error_velocity,
-        state_tolerance.velocity);
-    }
-    if (
-      state_tolerance.acceleration > 0.0 && abs(error_acceleration) > state_tolerance.acceleration)
-    {
-      RCLCPP_ERROR(
-        logger, "Acceleration Error: %f, Acceleration Tolerance: %f", error_acceleration,
-        state_tolerance.acceleration);
-    }
-  }
-  return false;
-}
+  const StateTolerances & state_tolerance, bool show_errors);
 
 }  // namespace force_gate_controller