584 task gripper controller merge into main we want juicy gripper inside of main

gripper_controller/CMakeLists.txt

@@ -0,0 +1,97 @@
+cmake_minimum_required(VERSION 3.8)
+project(gripper_controller)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 20)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(vortex_msgs REQUIRED)
+find_package(vortex_utils REQUIRED)
+find_package(vortex_utils_ros REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(spdlog REQUIRED)
+find_package(fmt REQUIRED)
+
+include_directories(include)
+
+# ---------------------------------------------------------------------------
+# 1. Component Library (Shared)
+# ---------------------------------------------------------------------------
+set(LIB_NAME "${PROJECT_NAME}_component")
+
+add_library(${LIB_NAME} SHARED
+  src/gripper_controller.cpp
+  src/gripper_controller_ros.cpp
+)
+
+ament_target_dependencies(${LIB_NAME} PUBLIC
+  rclcpp
+  rclcpp_components
+  rclcpp_action
+  Eigen3
+  vortex_msgs
+  vortex_utils
+  vortex_utils_ros
+  spdlog
+  fmt
+)
+
+# ---------------------------------------------------------------------------
+# 2. Core Library (Static) - Used for Unit Testing
+# ---------------------------------------------------------------------------
+set(CORE_LIB_NAME "${PROJECT_NAME}_core")
+
+add_library(${CORE_LIB_NAME} STATIC
+  src/gripper_controller.cpp
+)
+
+ament_target_dependencies(${CORE_LIB_NAME} PUBLIC
+  Eigen3
+  vortex_utils
+)
+
+# ---------------------------------------------------------------------------
+# 3. Component Registration
+# ---------------------------------------------------------------------------
+rclcpp_components_register_node(
+  ${LIB_NAME}
+  PLUGIN "vortex::controller::GripperControllerNode"
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+# ---------------------------------------------------------------------------
+# 4. Standalone Executable
+# ---------------------------------------------------------------------------
+add_executable(${PROJECT_NAME}_standalone
+  src/gripper_controller_node.cpp
+)
+target_link_libraries(${PROJECT_NAME}_standalone PUBLIC
+  ${LIB_NAME}
+)
+ament_target_dependencies(${PROJECT_NAME}_standalone PUBLIC
+  rclcpp
+)
+
+ament_export_targets(export_${LIB_NAME})
+install(TARGETS ${LIB_NAME} ${PROJECT_NAME}_standalone
+  EXPORT export_${LIB_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/ DESTINATION include)
+install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}/)
+
+if(BUILD_TESTING)
+  add_subdirectory(test)
+endif()
+
+ament_package()

gripper_controller/config/gripper_controller_params.yaml

@@ -0,0 +1,19 @@
+ /**:
+  ros__parameters:
+    # Control loop timing
+    time_step_ms: 10         # milliseconds — must match reference filter time step
+
+    # Proportional gain — tune after gear ratio calibration
+    # Diagonal P-controller: vel = diag(kp_roll, kp_pinch) * position_error
+    kp:
+      roll:  1.0             # TODO: tune
+      pinch: 1.0             # TODO: tune
+
+    # Topics
+    topics:
+      # Input: smoothed reference from the gripper reference filter
+      reference: "nautilus/gripper/guidance"
+      # Input: raw measured gripper state from the interface node
+      state:     "nautilus/gripper/state"
+      # Output: velocity command to the interface node
+      control:   "nautilus/gripper/control"

gripper_controller/include/gripper_controller/gripper_controller.hpp

@@ -0,0 +1,32 @@
+#ifndef GRIPPER_CONTROLLER__GRIPPER_CONTROLLER_HPP_
+#define GRIPPER_CONTROLLER__GRIPPER_CONTROLLER_HPP_
+
+#include "gripper_controller/gripper_controller_typedefs.hpp"
+
+
+class GripperController {
+   public:
+    GripperController();
+
+    // @brief Calculate velocity command from position error
+    // @param measured_state:  struct containing measured gripper state [roll, pinch]
+    // @param reference_state: struct containing desired gripper state [roll, pinch]
+    // @return 2D vector containing velocity commands [roll_vel, pinch_vel]
+    types::Vector2d calculate_velocity(const types::GripperState& measured_state,
+                                       const types::GripperState& reference_state);
+
+    // @brief Set the proportional gain matrix
+    // @param proportional_gain_matrix: 2x2 matrix containing the proportional gain matrix
+    void set_kp(const types::Matrix2d& proportional_gain_matrix);
+
+    // @brief Set the controller loop period in milliseconds
+    // @param time_step_ms: time step in milliseconds
+    void set_time_step_ms(double time_step_ms);
+
+   private:
+    types::Matrix2d Kp_;
+    double time_step_ms_;
+};
+
+
+#endif  // GRIPPER_CONTROLLER__GRIPPER_CONTROLLER_HPP_

gripper_controller/include/gripper_controller/gripper_controller_ros.hpp

@@ -0,0 +1,64 @@
+#ifndef GRIPPER_CONTROLLER__GRIPPER_CONTROLLER_ROS_HPP_
+#define GRIPPER_CONTROLLER__GRIPPER_CONTROLLER_ROS_HPP_
+
+#include <chrono>
+#include <functional>
+#include <mutex>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_components/register_node_macro.hpp>
+#include <vortex_msgs/msg/gripper_reference_filter.hpp>
+#include <vortex_msgs/msg/gripper_state.hpp>
+#include <vortex_msgs/msg/gripper_state_velocity_command.hpp>
+#include <vortex/utils/ros/qos_profiles.hpp>
+#include "gripper_controller/gripper_controller.hpp"
+#include "gripper_controller/gripper_controller_translator.hpp"
+#include "gripper_controller/gripper_controller_typedefs.hpp"
+
+// ---------------------------------------------------------------------------
+// Responsibility: ROS wiring only — subscriptions, publications, parameter
+// loading, and timer setup. All control mathematics live in GripperController.
+// All message translation lives in gripper_controller::translator.
+// ---------------------------------------------------------------------------
+
+namespace vortex::controller {
+
+class GripperControllerNode : public rclcpp::Node {
+public:
+  explicit GripperControllerNode(const rclcpp::NodeOptions & options);
+
+private:
+  void reference_callback(
+    const vortex_msgs::msg::GripperReferenceFilter::SharedPtr reference_filter_msg);
+
+  void state_callback(
+    const vortex_msgs::msg::GripperState::SharedPtr gripper_state_msg);
+
+  void publish_control();
+
+  void set_controller_params();
+
+  void set_subscribers_and_publisher();
+
+  GripperController controller_;
+
+  rclcpp::Subscription<vortex_msgs::msg::GripperReferenceFilter>::SharedPtr
+    reference_sub_;
+  rclcpp::Subscription<vortex_msgs::msg::GripperState>::SharedPtr
+    state_sub_;
+  rclcpp::Publisher<vortex_msgs::msg::GripperStateVelocityCommand>::SharedPtr
+    control_pub_;
+
+  rclcpp::TimerBase::SharedPtr control_timer_;
+  std::chrono::milliseconds time_step_ms_;
+
+  std::mutex state_mutex_;
+
+  double roll_ref_ = 0.0;
+  double pinch_ref_ = 0.0;
+  double roll_measured_ = 0.0;
+  double pinch_measured_ = 0.0;
+};
+
+} // namespace vortex::controller
+
+#endif // GRIPPER_CONTROLLER__GRIPPER_CONTROLLER_ROS_HPP_

gripper_controller/include/gripper_controller/gripper_controller_translator.hpp

@@ -0,0 +1,49 @@
+#ifndef GRIPPER_CONTROLLER__GRIPPER_CONTROLLER_TRANSLATOR_HPP_
+#define GRIPPER_CONTROLLER__GRIPPER_CONTROLLER_TRANSLATOR_HPP_
+
+#include <vortex_msgs/msg/gripper_reference_filter.hpp>
+#include <vortex_msgs/msg/gripper_state.hpp>
+#include <vortex_msgs/msg/gripper_state_velocity_command.hpp>
+#include "gripper_controller/gripper_controller_typedefs.hpp"
+
+// ---------------------------------------------------------------------------
+// Responsibility: translate between domain structs and ROS message types.
+// This is intentionally a namespace of stateless free functions rather than
+// a class — there is no invariant to maintain, no state to encapsulate.
+// Keeping translation logic here (instead of inside the node) satisfies SRP:
+// the node only orchestrates, the translator only converts.
+// ---------------------------------------------------------------------------
+
+namespace gripper_controller::translator {
+
+/// @brief Extract position-only GripperState from a smoothed reference message.
+inline types::GripperState reference_filter_msg_to_gripper_state(
+  const vortex_msgs::msg::GripperReferenceFilter& reference_filter_msg) {
+  types::GripperState gripper_state;
+  gripper_state.roll = reference_filter_msg.roll;
+  gripper_state.pinch = reference_filter_msg.pinch;
+  return gripper_state;
+}
+
+/// @brief Convert a raw GripperState ROS message to the domain GripperState struct.
+inline types::GripperState gripper_state_msg_to_gripper_state(
+  const vortex_msgs::msg::GripperState& gripper_state_msg) {
+  types::GripperState gripper_state;
+  gripper_state.roll = gripper_state_msg.roll;
+  gripper_state.pinch = gripper_state_msg.pinch;
+  return gripper_state;
+}
+
+/// @brief Pack a 2D velocity command vector into a GripperStateVelocityCommand message.
+inline vortex_msgs::msg::GripperStateVelocityCommand
+velocity_command_to_gripper_velocity_command_msg(
+  const types::Vector2d& velocity_command) {
+  vortex_msgs::msg::GripperStateVelocityCommand velocity_command_msg;
+  velocity_command_msg.roll_velocity = velocity_command(0);
+  velocity_command_msg.pinch_velocity = velocity_command(1);
+  return velocity_command_msg;
+}
+
+} // namespace gripper_controller::translator
+
+#endif // GRIPPER_CONTROLLER__GRIPPER_CONTROLLER_TRANSLATOR_HPP_

gripper_controller/include/gripper_controller/gripper_controller_typedefs.hpp

@@ -0,0 +1,22 @@
+#ifndef GRIPPER_CONTROLLER__GRIPPER_CONTROLLER_TYPEDEFS_HPP_
+#define GRIPPER_CONTROLLER__GRIPPER_CONTROLLER_TYPEDEFS_HPP_
+
+#include <eigen3/Eigen/Dense>
+
+namespace types {
+
+using Vector2d = Eigen::Vector2d;
+using Matrix2d = Eigen::Matrix2d;
+
+struct GripperState {
+    double roll  = 0.0;
+    double pinch = 0.0;
+};
+
+// Velocity saturation limits — tune after gear ratio calibration
+constexpr double MAX_ROLL_VEL  = 1.0;  // TODO: tune
+constexpr double MAX_PINCH_VEL = 1.0;  // TODO: tune
+
+}  // namespace types
+
+#endif  // GRIPPER_CONTROLLER__GRIPPER_CONTROLLER_TYPEDEFS_HPP_

gripper_controller/launch/gripper_controller.launch.py

@@ -0,0 +1,21 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    config_file_path = os.path.join(
+        get_package_share_directory("gripper_controller"),
+        "config",
+        "gripper_controller_params.yaml",
+    )
+
+    gripper_controller_node = Node(
+        package="gripper_controller",
+        executable="gripper_controller_standalone",
+        name="gripper_controller_node",
+        parameters=[config_file_path],
+        output="screen",
+    )
+
+    return LaunchDescription([gripper_controller_node])

gripper_controller/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>gripper_controller</name>
+  <version>0.0.0</version>
+  <description>Package for the control algorithm of the gripper</description>
+  <maintainer email="patricsh@stud.ntnu.no">patricsh</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>rclcpp_components</depend> <depend>geometry_msgs</depend>
+  <depend>vortex_msgs</depend>
+  <depend>vortex_utils</depend>
+  <depend>vortex_utils_ros</depend>
+
+  <depend>eigen</depend>
+  <depend>spdlog</depend>
+  <depend>fmt</depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

gripper_controller/src/gripper_controller.cpp

@@ -0,0 +1,37 @@
+#include "gripper_controller/gripper_controller.hpp"
+
+#include <algorithm>
+
+GripperController::GripperController()
+    : Kp_(types::Matrix2d::Identity()), time_step_ms_(10.0) {}
+
+types::Vector2d GripperController::calculate_velocity(
+    const types::GripperState& measured_state,
+    const types::GripperState& reference_state) {
+
+    const types::Vector2d position_error = [&] {
+        types::Vector2d position_difference;
+        position_difference(0) = reference_state.roll  - measured_state.roll;
+        position_difference(1) = reference_state.pinch - measured_state.pinch;
+        return position_difference;
+    }();
+
+    types::Vector2d velocity_command = Kp_ * position_error;
+
+    velocity_command(0) = std::clamp(velocity_command(0),
+                                     -types::MAX_ROLL_VEL,
+                                      types::MAX_ROLL_VEL);
+    velocity_command(1) = std::clamp(velocity_command(1),
+                                     -types::MAX_PINCH_VEL,
+                                      types::MAX_PINCH_VEL);
+
+    return velocity_command;
+}
+
+void GripperController::set_kp(const types::Matrix2d& proportional_gain_matrix) {
+    Kp_ = proportional_gain_matrix;
+}
+
+void GripperController::set_time_step_ms(double time_step_ms) {
+    time_step_ms_ = time_step_ms;
+}

gripper_controller/src/gripper_controller_node.cpp

@@ -0,0 +1,14 @@
+#include <rclcpp/rclcpp.hpp>
+#include "gripper_controller/gripper_controller_ros.hpp"
+
+int main(int argc, char** argv) {
+  rclcpp::init(argc, argv);
+
+  auto gripper_controller_node =
+    std::make_shared<vortex::controller::GripperControllerNode>(
+      rclcpp::NodeOptions());
+
+  rclcpp::spin(gripper_controller_node);
+  rclcpp::shutdown();
+  return 0;
+}