diff --git a/gripper_controller/CMakeLists.txt b/gripper_controller/CMakeLists.txt new file mode 100644 index 0000000..e37362d --- /dev/null +++ b/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() diff --git a/gripper_controller/config/gripper_controller_params.yaml b/gripper_controller/config/gripper_controller_params.yaml new file mode 100644 index 0000000..2efa89a --- /dev/null +++ b/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" diff --git a/gripper_controller/include/gripper_controller/gripper_controller.hpp b/gripper_controller/include/gripper_controller/gripper_controller.hpp new file mode 100644 index 0000000..d969e7b --- /dev/null +++ b/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_ diff --git a/gripper_controller/include/gripper_controller/gripper_controller_ros.hpp b/gripper_controller/include/gripper_controller/gripper_controller_ros.hpp new file mode 100644 index 0000000..ae1be93 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include +#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::SharedPtr + reference_sub_; + rclcpp::Subscription::SharedPtr + state_sub_; + rclcpp::Publisher::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_ diff --git a/gripper_controller/include/gripper_controller/gripper_controller_translator.hpp b/gripper_controller/include/gripper_controller/gripper_controller_translator.hpp new file mode 100644 index 0000000..99f3126 --- /dev/null +++ b/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 +#include +#include +#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_ diff --git a/gripper_controller/include/gripper_controller/gripper_controller_typedefs.hpp b/gripper_controller/include/gripper_controller/gripper_controller_typedefs.hpp new file mode 100644 index 0000000..002ba5a --- /dev/null +++ b/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 + +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_ diff --git a/gripper_controller/launch/gripper_controller.launch.py b/gripper_controller/launch/gripper_controller.launch.py new file mode 100644 index 0000000..f5d76e9 --- /dev/null +++ b/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]) diff --git a/gripper_controller/package.xml b/gripper_controller/package.xml new file mode 100644 index 0000000..076f4cf --- /dev/null +++ b/gripper_controller/package.xml @@ -0,0 +1,28 @@ + + + + gripper_controller + 0.0.0 + Package for the control algorithm of the gripper + patricsh + MIT + + ament_cmake + + rclcpp + rclcpp_action + rclcpp_components geometry_msgs + vortex_msgs + vortex_utils + vortex_utils_ros + + eigen + spdlog + fmt + + ament_cmake_gtest + + + ament_cmake + + diff --git a/gripper_controller/src/gripper_controller.cpp b/gripper_controller/src/gripper_controller.cpp new file mode 100644 index 0000000..203f149 --- /dev/null +++ b/gripper_controller/src/gripper_controller.cpp @@ -0,0 +1,37 @@ +#include "gripper_controller/gripper_controller.hpp" + +#include + +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; +} diff --git a/gripper_controller/src/gripper_controller_node.cpp b/gripper_controller/src/gripper_controller_node.cpp new file mode 100644 index 0000000..6d356d5 --- /dev/null +++ b/gripper_controller/src/gripper_controller_node.cpp @@ -0,0 +1,14 @@ +#include +#include "gripper_controller/gripper_controller_ros.hpp" + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + + auto gripper_controller_node = + std::make_shared( + rclcpp::NodeOptions()); + + rclcpp::spin(gripper_controller_node); + rclcpp::shutdown(); + return 0; +} diff --git a/gripper_controller/src/gripper_controller_ros.cpp b/gripper_controller/src/gripper_controller_ros.cpp new file mode 100644 index 0000000..3936f5b --- /dev/null +++ b/gripper_controller/src/gripper_controller_ros.cpp @@ -0,0 +1,130 @@ +#include "gripper_controller/gripper_controller_ros.hpp" + +#include +#include +#include +#include +#include "gripper_controller/gripper_controller_translator.hpp" + +const auto start_message = R"( + + ____ _ ____ _ _ _ +/ ___|_ __ (_)_ __ _ __ ___ _ __ / ___|___ _ __ | |_ _ __ ___ | | | ___ _ __ +| | _| '_ \| | '_ \| '_ \ / _ \ '__|| | / _ \| '_ \| __| '__/ _ \| | |/ _ \ '__| +| |_| | |_) | | |_) | |_) | __/ | | |__| (_) | | | | |_| | | (_) | | | __/ | + \____| .__/|_| .__/| .__/ \___|_| \____\___/|_| |_|\__|_| \___/|_|_|\___|_| + |_| |_| |_| + +)"; + +namespace vortex::controller { + +GripperControllerNode::GripperControllerNode(const rclcpp::NodeOptions & options) +: Node("gripper_controller_node", options) { + set_controller_params(); + + set_subscribers_and_publisher(); + + spdlog::info(start_message); +} + +void GripperControllerNode::set_controller_params() { + const int time_step_ms_param = + this->declare_parameter("time_step_ms", 10); + time_step_ms_ = std::chrono::milliseconds(time_step_ms_param); + + const double kp_roll = + this->declare_parameter("kp.roll", 1.0); + const double kp_pinch = + this->declare_parameter("kp.pinch", 1.0); + + const types::Matrix2d proportional_gain_matrix = [&] { + types::Matrix2d gain_matrix = types::Matrix2d::Zero(); + gain_matrix(0, 0) = kp_roll; + gain_matrix(1, 1) = kp_pinch; + return gain_matrix; + }(); + + controller_.set_kp(proportional_gain_matrix); + controller_.set_time_step_ms(static_cast(time_step_ms_param)); + + spdlog::info("GripperController: kp_roll={:.3f} kp_pinch={:.3f} dt={}ms", + kp_roll, kp_pinch, time_step_ms_param); +} + +void GripperControllerNode::set_subscribers_and_publisher() { + const std::string reference_topic = + this->declare_parameter("topics.reference"); + const std::string state_topic = + this->declare_parameter("topics.state"); + const std::string control_topic = + this->declare_parameter("topics.control"); + + const auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); + + reference_sub_ = + this->create_subscription( + reference_topic, qos_sensor_data, + [this](const vortex_msgs::msg::GripperReferenceFilter::SharedPtr reference_filter_msg) { + reference_callback(reference_filter_msg); + }); + + state_sub_ = + this->create_subscription( + state_topic, qos_sensor_data, + [this](const vortex_msgs::msg::GripperState::SharedPtr gripper_state_msg) { + state_callback(gripper_state_msg); + }); + + control_pub_ = + this->create_publisher( + control_topic, qos_sensor_data); + + // Timer is created last so that controller_ and the publisher are fully + // initialised before the first publish_control() fires. + control_timer_ = this->create_wall_timer( + time_step_ms_, + [this]() { publish_control(); }); +} + +void GripperControllerNode::reference_callback( + const vortex_msgs::msg::GripperReferenceFilter::SharedPtr reference_filter_msg) { + std::lock_guard lock(state_mutex_); + roll_ref_ = reference_filter_msg->roll; + pinch_ref_ = reference_filter_msg->pinch; +} + +void GripperControllerNode::state_callback( + const vortex_msgs::msg::GripperState::SharedPtr gripper_state_msg) { + std::lock_guard lock(state_mutex_); + roll_measured_ = gripper_state_msg->roll; + pinch_measured_ = gripper_state_msg->pinch; +} + +void GripperControllerNode::publish_control() { + const auto [measured_state, reference_state] = [this] { + std::lock_guard lock(state_mutex_); + types::GripperState measured_gripper_state; + types::GripperState reference_gripper_state; + measured_gripper_state.roll = roll_measured_; + measured_gripper_state.pinch = pinch_measured_; + reference_gripper_state.roll = roll_ref_; + reference_gripper_state.pinch = pinch_ref_; + return std::pair{measured_gripper_state, reference_gripper_state}; + }(); + + const types::Vector2d velocity_command = + controller_.calculate_velocity(measured_state, reference_state); + + auto velocity_command_msg = + std::make_unique( + gripper_controller::translator::velocity_command_to_gripper_velocity_command_msg( + velocity_command)); + velocity_command_msg->header.stamp = this->now(); + + control_pub_->publish(std::move(velocity_command_msg)); +} + + RCLCPP_COMPONENTS_REGISTER_NODE(GripperControllerNode) + +} // namespace vortex::controller diff --git a/gripper_controller/test/CMakeLists.txt b/gripper_controller/test/CMakeLists.txt new file mode 100644 index 0000000..5441a5c --- /dev/null +++ b/gripper_controller/test/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.8) + +find_package(GTest REQUIRED) +include(GoogleTest) + +set(TEST_BINARY_NAME ${PROJECT_NAME}_test) + +add_executable( + ${TEST_BINARY_NAME} + test_gripper_controller.cpp +) + +target_link_libraries( + ${TEST_BINARY_NAME} + PRIVATE + ${CORE_LIB_NAME} + GTest::GTest +) + +ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3) + +gtest_discover_tests(${TEST_BINARY_NAME}) diff --git a/gripper_controller/test/test_gripper_controller.cpp b/gripper_controller/test/test_gripper_controller.cpp new file mode 100644 index 0000000..22f71ec --- /dev/null +++ b/gripper_controller/test/test_gripper_controller.cpp @@ -0,0 +1,210 @@ +#include +#include "gripper_controller/gripper_controller.hpp" +#include "gripper_controller/gripper_controller_typedefs.hpp" + +// --------------------------------------------------------------------------- +// Test fixture +// Sets up a GripperController with identity Kp (gain = 1.0 on both axes) +// so that expected output equals the position error exactly, up to saturation. +// --------------------------------------------------------------------------- +class GripperControllerTest : public ::testing::Test { + protected: + void SetUp() override { + types::Matrix2d identity_gain = types::Matrix2d::Identity(); + controller_.set_kp(identity_gain); + controller_.set_time_step_ms(10.0); + } + + // @brief Helper: run calculate_velocity with explicit scalar inputs. + types::Vector2d compute_velocity(double measured_roll, + double measured_pinch, + double reference_roll, + double reference_pinch) { + types::GripperState measured_state; + measured_state.roll = measured_roll; + measured_state.pinch = measured_pinch; + + types::GripperState reference_state; + reference_state.roll = reference_roll; + reference_state.pinch = reference_pinch; + + return controller_.calculate_velocity(measured_state, reference_state); + } + + GripperController controller_; +}; + +// --------------------------------------------------------------------------- +// Zero error +// --------------------------------------------------------------------------- + +TEST_F(GripperControllerTest, ZeroError_ProducesZeroVelocity) { + const types::Vector2d velocity_command = compute_velocity(0.0, 0.0, 0.0, 0.0); + + EXPECT_DOUBLE_EQ(velocity_command(0), 0.0); + EXPECT_DOUBLE_EQ(velocity_command(1), 0.0); +} + +TEST_F(GripperControllerTest, StateEqualsReference_ProducesZeroVelocity) { + const types::Vector2d velocity_command = compute_velocity(0.5, 0.3, 0.5, 0.3); + + EXPECT_DOUBLE_EQ(velocity_command(0), 0.0); + EXPECT_DOUBLE_EQ(velocity_command(1), 0.0); +} + +// --------------------------------------------------------------------------- +// Correct sign and axis isolation +// --------------------------------------------------------------------------- + +TEST_F(GripperControllerTest, PositiveRollError_ProducesPositiveRollVelocity) { + const types::Vector2d velocity_command = compute_velocity(0.0, 0.0, 0.5, 0.0); + + EXPECT_GT(velocity_command(0), 0.0); + EXPECT_DOUBLE_EQ(velocity_command(1), 0.0); +} + +TEST_F(GripperControllerTest, NegativeRollError_ProducesNegativeRollVelocity) { + const types::Vector2d velocity_command = compute_velocity(0.5, 0.0, 0.0, 0.0); + + EXPECT_LT(velocity_command(0), 0.0); + EXPECT_DOUBLE_EQ(velocity_command(1), 0.0); +} + +TEST_F(GripperControllerTest, PositivePinchError_ProducesPositivePinchVelocity) { + const types::Vector2d velocity_command = compute_velocity(0.0, 0.0, 0.0, 0.5); + + EXPECT_DOUBLE_EQ(velocity_command(0), 0.0); + EXPECT_GT(velocity_command(1), 0.0); +} + +TEST_F(GripperControllerTest, NegativePinchError_ProducesNegativePinchVelocity) { + const types::Vector2d velocity_command = compute_velocity(0.0, 0.5, 0.0, 0.0); + + EXPECT_DOUBLE_EQ(velocity_command(0), 0.0); + EXPECT_LT(velocity_command(1), 0.0); +} + +// --------------------------------------------------------------------------- +// Proportionality — with identity Kp, output equals error exactly +// (as long as error stays within saturation limits) +// --------------------------------------------------------------------------- + +TEST_F(GripperControllerTest, OutputProportionalToError_WithinSaturationLimits) { + const double roll_error = 0.3; + const double pinch_error = 0.6; + + const types::Vector2d velocity_command = + compute_velocity(0.0, 0.0, roll_error, pinch_error); + + EXPECT_NEAR(velocity_command(0), roll_error, 1e-9); + EXPECT_NEAR(velocity_command(1), pinch_error, 1e-9); +} + +// --------------------------------------------------------------------------- +// Velocity saturation +// --------------------------------------------------------------------------- + +TEST_F(GripperControllerTest, LargePositiveRollError_SaturatesAtMaxRollVelocity) { + // Error of 100.0 with gain 1.0 would produce 100.0 — well above MAX_ROLL_VEL + const types::Vector2d velocity_command = compute_velocity(0.0, 0.0, 100.0, 0.0); + + EXPECT_DOUBLE_EQ(velocity_command(0), types::MAX_ROLL_VEL); +} + +TEST_F(GripperControllerTest, LargeNegativeRollError_SaturatesAtNegativeMaxRollVelocity) { + const types::Vector2d velocity_command = compute_velocity(100.0, 0.0, 0.0, 0.0); + + EXPECT_DOUBLE_EQ(velocity_command(0), -types::MAX_ROLL_VEL); +} + +TEST_F(GripperControllerTest, LargePositivePinchError_SaturatesAtMaxPinchVelocity) { + const types::Vector2d velocity_command = compute_velocity(0.0, 0.0, 0.0, 100.0); + + EXPECT_DOUBLE_EQ(velocity_command(1), types::MAX_PINCH_VEL); +} + +TEST_F(GripperControllerTest, LargeNegativePinchError_SaturatesAtNegativeMaxPinchVelocity) { + const types::Vector2d velocity_command = compute_velocity(0.0, 100.0, 0.0, 0.0); + + EXPECT_DOUBLE_EQ(velocity_command(1), -types::MAX_PINCH_VEL); +} + +TEST_F(GripperControllerTest, SaturationIsSymmetric_RollAxis) { + const types::Vector2d positive_saturation = compute_velocity(0.0, 0.0, 100.0, 0.0); + const types::Vector2d negative_saturation = compute_velocity(100.0, 0.0, 0.0, 0.0); + + EXPECT_DOUBLE_EQ(positive_saturation(0), -negative_saturation(0)); +} + +TEST_F(GripperControllerTest, SaturationIsSymmetric_PinchAxis) { + const types::Vector2d positive_saturation = compute_velocity(0.0, 0.0, 0.0, 100.0); + const types::Vector2d negative_saturation = compute_velocity(0.0, 100.0, 0.0, 0.0); + + EXPECT_DOUBLE_EQ(positive_saturation(1), -negative_saturation(1)); +} + +// --------------------------------------------------------------------------- +// Gain matrix effects +// --------------------------------------------------------------------------- + +TEST_F(GripperControllerTest, ZeroGainMatrix_AlwaysProducesZeroVelocity) { + controller_.set_kp(types::Matrix2d::Zero()); + + const types::Vector2d velocity_command = compute_velocity(0.0, 0.0, 1.0, 1.0); + + EXPECT_DOUBLE_EQ(velocity_command(0), 0.0); + EXPECT_DOUBLE_EQ(velocity_command(1), 0.0); +} + +TEST_F(GripperControllerTest, ScaledDiagonalGain_ScalesOutputCorrectly) { + types::Matrix2d scaled_gain = types::Matrix2d::Zero(); + scaled_gain(0, 0) = 0.5; // roll gain + scaled_gain(1, 1) = 2.0; // pinch gain + controller_.set_kp(scaled_gain); + + // Error of 0.4 on both axes — both stay within saturation after scaling + const types::Vector2d velocity_command = compute_velocity(0.0, 0.0, 0.4, 0.4); + + EXPECT_NEAR(velocity_command(0), 0.5 * 0.4, 1e-9); + EXPECT_NEAR(velocity_command(1), 2.0 * 0.4, 1e-9); +} + +TEST_F(GripperControllerTest, OffDiagonalGain_CrossCouplingPropagatesCorrectly) { + // Set cross-coupling: roll error should also drive pinch output + types::Matrix2d cross_coupled_gain = types::Matrix2d::Identity(); + cross_coupled_gain(1, 0) = 0.5; // pinch output gains from roll error + controller_.set_kp(cross_coupled_gain); + + // Pure roll error of 0.4 + const types::Vector2d velocity_command = compute_velocity(0.0, 0.0, 0.4, 0.0); + + EXPECT_NEAR(velocity_command(0), 0.4, 1e-9); // direct roll term + EXPECT_NEAR(velocity_command(1), 0.5 * 0.4, 1e-9); // cross-coupled pinch term +} + +// --------------------------------------------------------------------------- +// set_kp takes effect on the next call +// --------------------------------------------------------------------------- + +TEST_F(GripperControllerTest, UpdatedGain_TakesEffectImmediately) { + // First call with identity gain + const types::Vector2d first_velocity = compute_velocity(0.0, 0.0, 0.4, 0.0); + EXPECT_NEAR(first_velocity(0), 0.4, 1e-9); + + // Double the roll gain + types::Matrix2d doubled_gain = types::Matrix2d::Identity(); + doubled_gain(0, 0) = 2.0; + controller_.set_kp(doubled_gain); + + const types::Vector2d second_velocity = compute_velocity(0.0, 0.0, 0.4, 0.0); + EXPECT_NEAR(second_velocity(0), 0.8, 1e-9); +} + +// --------------------------------------------------------------------------- +// Entry point +// --------------------------------------------------------------------------- + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/gripper_guidance/CMakeLists.txt b/gripper_guidance/CMakeLists.txt new file mode 100644 index 0000000..e873456 --- /dev/null +++ b/gripper_guidance/CMakeLists.txt @@ -0,0 +1,77 @@ +cmake_minimum_required(VERSION 3.8) +project(gripper_reference_filter) + +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(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(spdlog REQUIRED) +find_package(fmt REQUIRED) + +include_directories(include) + +set(LIB_NAME "${PROJECT_NAME}_component") + +add_library(${LIB_NAME} SHARED + src/gripper_reference_filter.cpp + src/gripper_reference_filter_ros.cpp) + +ament_target_dependencies(${LIB_NAME} PUBLIC + rclcpp + rclcpp_components + rclcpp_action + geometry_msgs + nav_msgs + Eigen3 + vortex_msgs + vortex_utils + vortex_utils_ros + spdlog + fmt +) + +rclcpp_components_register_node( + ${LIB_NAME} + PLUGIN "vortex::guidance::GripperReferenceFilterNode" + EXECUTABLE ${PROJECT_NAME}_node +) + +ament_export_targets(export_${LIB_NAME}) + +install(TARGETS ${LIB_NAME} + EXPORT export_${LIB_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME}/ +) + +if(BUILD_TESTING) + add_subdirectory(test) +endif() + +ament_package() diff --git a/gripper_guidance/README.md b/gripper_guidance/README.md new file mode 100644 index 0000000..0ccfbf2 --- /dev/null +++ b/gripper_guidance/README.md @@ -0,0 +1,42 @@ +## Reference filter + +The reference filter (or reference model) is of third order and provides reference for pose, velocity and acceleration. So that $x_d := [\eta_d^\top, \dot{\eta}_d^\top, \ddot{\eta}_d^\top] \in \mathbb{R}^{3n}$. + +The state-space representation is, according to (Fossen, 2021), +```math +\dot{x}_d = A_d x_d + B_d r +``` +where +```math +A_d = \begin{bmatrix} +0_{n\times n} & I_n & 0_{n\times n} \\ +0_{n\times n} & 0_{n \times n} & I_n \\ +-\Omega^3 & -(2 \Delta + I_n) \Omega^2 & -(2 \Delta + I_n) \Omega +\end{bmatrix} +``` +and +```math +B_d = \begin{bmatrix} +0_{n \times n} \\ +0_{n \times n} \\ +\Omega^3 +\end{bmatrix}. +``` + +The steady-state pose for a constant reference signal $r$ is +```math +\lim_{t \to \infty} \eta_d = r +``` + +The state at the next time step is calculated using forward Euler integration +```math +x_{i+1} = x_i + \dot{x}_i * dt +``` + +## Action Server +The action server is responsible for handling goal requests and publishing guidance commands. The server will always prioritize new goal request, and will abort ongoing request when getting a new request. The action definition can be found [here](https://github.com/vortexntnu/vortex-msgs/blob/main/action/ReferenceFilterWaypoint.action). + +- Action name: /reference_filter +- Goal type: PoseStamped +- Result type: bool +- Guidance topic: /dp/reference diff --git a/gripper_guidance/config/gripper_reference_filter_params.yaml b/gripper_guidance/config/gripper_reference_filter_params.yaml new file mode 100644 index 0000000..6e93944 --- /dev/null +++ b/gripper_guidance/config/gripper_reference_filter_params.yaml @@ -0,0 +1,17 @@ + /**: + ros__parameters: + # Integration loop period in milliseconds + time_step_ms: 10 + + # Filter dynamics parameters + zeta: [1.0, 1.0] + omega: [0.4, 0.4] + + # Topics + topics: + guidance_gripper: "nautilus/gripper/guidance" + gripper_state: "nautilus/gripper/state" + + # Action servers + action_servers: + gripper_reference_filter: "nautilus/gripper/reference_filter" diff --git a/gripper_guidance/include/gripper_reference_filter/gripper_eigen_typedefs.hpp b/gripper_guidance/include/gripper_reference_filter/gripper_eigen_typedefs.hpp new file mode 100644 index 0000000..44e1dc0 --- /dev/null +++ b/gripper_guidance/include/gripper_reference_filter/gripper_eigen_typedefs.hpp @@ -0,0 +1,21 @@ +/** + * @file gripper_eigen_typedefs.hpp + * @brief Contains Eigen typedefs used in this package. + */ + +#ifndef GRIPPER_REFERENCE_FILTER__GRIPPER_EIGEN_TYPEDEFS_HPP_ +#define GRIPPER_REFERENCE_FILTER__GRIPPER_EIGEN_TYPEDEFS_HPP_ + +#include + +namespace Eigen { + +typedef Eigen::Matrix Matrix6d; +typedef Eigen::Matrix Matrix6x2d; +typedef Eigen::Matrix Matrix2d; +typedef Eigen::Matrix Vector2d; +typedef Eigen::Matrix Vector6d; + +} // namespace Eigen + +#endif // GRIPPER_REFERENCE_FILTER__GRIPPER_EIGEN_TYPEDEFS_HPP_ diff --git a/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter.hpp b/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter.hpp new file mode 100644 index 0000000..c1ce725 --- /dev/null +++ b/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter.hpp @@ -0,0 +1,69 @@ +#ifndef GRIPPER_REFERENCE_FILTER__GRIPPER_REFERENCE_FILTER_HPP_ +#define GRIPPER_REFERENCE_FILTER__GRIPPER_REFERENCE_FILTER_HPP_ + +#include "gripper_reference_filter/gripper_eigen_typedefs.hpp" + +namespace vortex::guidance { + +struct GripperReferenceFilterParams { + Eigen::Vector2d omega = Eigen::Vector2d::Zero(); + Eigen::Vector2d zeta = Eigen::Vector2d::Zero(); +}; + +class GripperReferenceFilter { + public: + explicit GripperReferenceFilter(const GripperReferenceFilterParams& params); + + // @brief Reset the internal 6D filter state. Position components are seeded + // from the supplied reference; velocity and acceleration components + // are zeroed. + // @param initial_reference 2D vector [roll, pinch] + void reset(const Eigen::Vector2d& initial_reference); + + // @brief Integrate the filter one time step toward the goal reference. + // @param goal_reference 2D vector [roll, pinch] + // @param time_step_seconds integration step in seconds + void step(const Eigen::Vector2d& goal_reference, double time_step_seconds); + + // @brief Snap the position components of the filter state to the goal + // reference. Used at convergence so that the published output + // exactly matches the requested goal. + // @param goal_reference 2D vector [roll, pinch] + void snap_to(const Eigen::Vector2d& goal_reference); + + // @brief Return the position output of the filter (roll, pinch). The + // higher-order state components are intentionally not exposed on + // the wire. + // @return 2D vector [roll, pinch] + Eigen::Vector2d reference_output() const; + + // @brief Calculate the state transition matrix + // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen + // 2021 p. 336 eq: 12.6 + void calculate_Ad(const Eigen::Vector2d& omega, + const Eigen::Vector2d& zeta); + + // @brief Calculate the input matrix + // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen + // 2021 p. 336 eq: 12.6 + void calculate_Bd(const Eigen::Vector2d& omega); + + private: + // @brief Calculate the state derivative + // @param state The state vector 6x1 + // @param reference The reference vector 2x1 + // @return The state derivative 6x1 + // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen + // 2021 p. 336 eq: 12.5 + Eigen::Vector6d calculate_state_derivative( + const Eigen::Vector6d& state, + const Eigen::Vector2d& reference) const; + + Eigen::Matrix6d Ad_; + Eigen::Matrix6x2d Bd_; + Eigen::Vector6d filter_state_; +}; + +} // namespace vortex::guidance + +#endif // GRIPPER_REFERENCE_FILTER__GRIPPER_REFERENCE_FILTER_HPP_ diff --git a/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp b/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp new file mode 100644 index 0000000..88020e6 --- /dev/null +++ b/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp @@ -0,0 +1,101 @@ +#ifndef GRIPPER_REFERENCE_FILTER__GRIPPER_REFERENCE_FILTER_ROS_HPP_ +#define GRIPPER_REFERENCE_FILTER__GRIPPER_REFERENCE_FILTER_ROS_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "gripper_reference_filter/gripper_eigen_typedefs.hpp" +#include "gripper_reference_filter/gripper_reference_filter.hpp" + +namespace vortex::guidance { + +class GripperReferenceFilterNode : public rclcpp::Node { + public: + explicit GripperReferenceFilterNode( + const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + + ~GripperReferenceFilterNode(); + + private: + // @brief Declare ROS parameters and create the subscription/publisher pair. + void set_subscribers_and_publisher(); + + // @brief Declare ROS parameters and create the action server. + void set_action_server(); + + // @brief Declare ROS parameters and construct the reference filter instance. + void set_refererence_filter(); + + // @brief Latch the measured gripper state for later use as the initial + // reference seed and for mode-masking the goal. + void reference_callback( + const vortex_msgs::msg::GripperState::SharedPtr state_msg); + + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID& /*uuid*/, + std::shared_ptr< + const vortex_msgs::action::GripperReferenceFilterWaypoint::Goal> + /*goal*/); + + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr> + /*goal_handle*/); + + void handle_accepted( + const std::shared_ptr> goal_handle); + + void execute( + const std::shared_ptr> goal_handle); + + // @brief Latch the filter's current 2D output as the held reference and + // publish it once. Called whenever a goal ends so that the next + // republish_held_reference_tick has a non-default value to send. + void latch_current_state_as_held_reference(); + + // @brief Periodically republish the held reference when no goal is active. + // This keeps downstream consumers receiving the last committed + // reference rather than a default-zero message, which prevented + // the convergence check from spuriously succeeding on a new + // zero-valued goal. + void republish_held_reference_tick(); + + rclcpp_action::Server< + vortex_msgs::action::GripperReferenceFilterWaypoint>::SharedPtr + action_server_; + + std::unique_ptr gripper_reference_filter_{}; + + rclcpp::Publisher::SharedPtr + reference_pub_; + + rclcpp::Subscription::SharedPtr + reference_sub_; + + std::chrono::milliseconds time_step_ms_{}; + + Eigen::Vector2d measured_reference_{Eigen::Vector2d::Zero()}; + + std::mutex mutex_; + + rclcpp::TimerBase::SharedPtr held_reference_republish_timer_; + vortex_msgs::msg::GripperReferenceFilter last_published_reference_; + bool holding_reference_{false}; + + std::atomic preempted_{false}; + std::mutex execute_mutex_; + std::thread execute_thread_; +}; + +} // namespace vortex::guidance + +#endif // GRIPPER_REFERENCE_FILTER__GRIPPER_REFERENCE_FILTER_ROS_HPP_ diff --git a/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros_utils.hpp b/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros_utils.hpp new file mode 100644 index 0000000..9314c34 --- /dev/null +++ b/gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros_utils.hpp @@ -0,0 +1,86 @@ +#ifndef GRIPPER_REFERENCE_FILTER__GRIPPER_REFERENCE_FILTER_ROS_UTILS_HPP_ +#define GRIPPER_REFERENCE_FILTER__GRIPPER_REFERENCE_FILTER_ROS_UTILS_HPP_ + +#include +#include +#include "gripper_reference_filter/gripper_eigen_typedefs.hpp" + +namespace vortex::guidance { + +// @brief Extract the [roll, pinch] reference from a GripperWaypoint. +// @param waypoint_goal The action goal waypoint. +// @return 2D vector [roll, pinch]. +inline Eigen::Vector2d fill_reference_goal( + const vortex_msgs::msg::GripperWaypoint& waypoint_goal) { + Eigen::Vector2d reference; + reference << waypoint_goal.roll, waypoint_goal.pinch; + return reference; +} + +// @brief Build the wire-format GripperReferenceFilter message from a 2D +// reference output. Only the position components (roll, pinch) are +// published; higher-order filter state is intentionally not exposed. +// @param reference_output 2D vector [roll, pinch]. +// @return Message ready for publication. +inline vortex_msgs::msg::GripperReferenceFilter fill_reference_msg( + const Eigen::Vector2d& reference_output) { + vortex_msgs::msg::GripperReferenceFilter reference_msg; + reference_msg.roll = reference_output(0); + reference_msg.pinch = reference_output(1); + return reference_msg; +} + +// @brief Apply mode logic to a requested reference. Axes excluded by the mode +// are pinned to the corresponding component of current_reference so +// that the unaffected axis does not drift. +// @param requested_reference 2D vector [roll, pinch] from the action goal. +// @param current_reference 2D vector [roll, pinch] currently being held. +// @param mode GripperWaypoint mode constant. +// @return The resolved reference after applying the mode mask. +inline Eigen::Vector2d apply_mode_logic( + const Eigen::Vector2d& requested_reference, + const Eigen::Vector2d& current_reference, + uint8_t mode) { + Eigen::Vector2d resolved_reference = requested_reference; + switch (mode) { + case vortex_msgs::msg::GripperWaypoint::ROLL_AND_PINCH: + break; + case vortex_msgs::msg::GripperWaypoint::ONLY_ROLL: + resolved_reference(1) = current_reference(1); + break; + case vortex_msgs::msg::GripperWaypoint::ONLY_PINCH: + resolved_reference(0) = current_reference(0); + break; + } + return resolved_reference; +} + +// @brief Compute the convergence error between two references, with the axis +// excluded by the mode masked to zero so it does not contribute to the +// norm-based convergence check. +// @param current_reference 2D vector [roll, pinch] currently produced by the filter. +// @param goal_reference 2D vector [roll, pinch] requested by the goal. +// @param mode GripperWaypoint mode constant. +// @return 2D error vector with masked axes set to zero. +inline Eigen::Vector2d compute_convergence_error( + const Eigen::Vector2d& current_reference, + const Eigen::Vector2d& goal_reference, + uint8_t mode) { + Eigen::Vector2d convergence_error = current_reference - goal_reference; + switch (mode) { + case vortex_msgs::msg::GripperWaypoint::ONLY_ROLL: + convergence_error(1) = 0.0; + break; + case vortex_msgs::msg::GripperWaypoint::ONLY_PINCH: + convergence_error(0) = 0.0; + break; + case vortex_msgs::msg::GripperWaypoint::ROLL_AND_PINCH: + default: + break; + } + return convergence_error; +} + +} // namespace vortex::guidance + +#endif // GRIPPER_REFERENCE_FILTER__GRIPPER_REFERENCE_FILTER_ROS_UTILS_HPP_ diff --git a/gripper_guidance/launch/gripper_reference_filter.launch.py b/gripper_guidance/launch/gripper_reference_filter.launch.py new file mode 100644 index 0000000..d1f7fab --- /dev/null +++ b/gripper_guidance/launch/gripper_reference_filter.launch.py @@ -0,0 +1,32 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def generate_launch_description(): + # Path to the specific gripper reference filter parameters + config_file_path = os.path.join( + get_package_share_directory("gripper_reference_filter"), + "config", + "gripper_reference_filter_params.yaml", + ) + + container = ComposableNodeContainer( + name='gripper_reference_filter_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='gripper_reference_filter', + # THIS PLUGIN NAME MUST MATCH YOUR NAMESPACE AND CLASS NAME EXACTLY + plugin='vortex::guidance::GripperReferenceFilterNode', + name='gripper_reference_filter_node', + parameters=[config_file_path], + ) + ], + output='screen', + ) + + return LaunchDescription([container]) diff --git a/gripper_guidance/package.xml b/gripper_guidance/package.xml new file mode 100644 index 0000000..baef78d --- /dev/null +++ b/gripper_guidance/package.xml @@ -0,0 +1,24 @@ + + + + gripper_reference_filter + 0.0.0 + Reference filter for the gripper + patricsh + MIT + + ament_cmake + + rclcpp + rclcpp_action + geometry_msgs + vortex_msgs + vortex_utils + vortex_utils_ros + nav_msgs + eigen + + + ament_cmake + + diff --git a/gripper_guidance/src/gripper_reference_filter.cpp b/gripper_guidance/src/gripper_reference_filter.cpp new file mode 100644 index 0000000..1235785 --- /dev/null +++ b/gripper_guidance/src/gripper_reference_filter.cpp @@ -0,0 +1,64 @@ +#include "gripper_reference_filter/gripper_reference_filter.hpp" + +namespace vortex::guidance { + +GripperReferenceFilter::GripperReferenceFilter(const GripperReferenceFilterParams& params) { + Ad_.setZero(); + Bd_.setZero(); + filter_state_.setZero(); + calculate_Ad(params.omega, params.zeta); + calculate_Bd(params.omega); +} + +void GripperReferenceFilter::reset(const Eigen::Vector2d& initial_reference) { + filter_state_.setZero(); + filter_state_(0) = initial_reference(0); + filter_state_(1) = initial_reference(1); +} + +void GripperReferenceFilter::step(const Eigen::Vector2d& goal_reference, + double time_step_seconds) { + const Eigen::Vector6d state_derivative = + calculate_state_derivative(filter_state_, goal_reference); + filter_state_ += state_derivative * time_step_seconds; +} + +void GripperReferenceFilter::snap_to(const Eigen::Vector2d& goal_reference) { + filter_state_.head(2) = goal_reference; +} + +Eigen::Vector2d GripperReferenceFilter::reference_output() const { + return filter_state_.head(2); +} + +Eigen::Vector6d GripperReferenceFilter::calculate_state_derivative( + const Eigen::Vector6d& state, const Eigen::Vector2d& reference) const { + const Eigen::Vector6d state_derivative = Ad_ * state + Bd_ * reference; + return state_derivative; +} + +void GripperReferenceFilter::calculate_Ad(const Eigen::Vector2d& omega, + const Eigen::Vector2d& zeta) { + const Eigen::Matrix2d omega_diag = omega.asDiagonal(); + const Eigen::Matrix2d zeta_diag = zeta.asDiagonal(); + const Eigen::Matrix2d omega_diag_squared = omega_diag * omega_diag; + const Eigen::Matrix2d omega_diag_cubed = omega_diag_squared * omega_diag; + + Ad_.block<2,2>(0, 2) = Eigen::Matrix2d::Identity(); + + Ad_.block<2,2>(2, 4) = Eigen::Matrix2d::Identity(); + + Ad_.block<2,2>(4, 0) = -omega_diag_cubed; + Ad_.block<2,2>(4, 2) = -2 * (2*zeta_diag + Eigen::Matrix2d::Identity()) * omega_diag_squared; + Ad_.block<2,2>(4, 4) = -2 * (2*zeta_diag + Eigen::Matrix2d::Identity()) * omega_diag; +} + +void GripperReferenceFilter::calculate_Bd(const Eigen::Vector2d& omega) { + const Eigen::Matrix2d omega_diag = omega.asDiagonal(); + const Eigen::Matrix2d omega_diag_squared = omega_diag * omega_diag; + const Eigen::Matrix2d omega_diag_cubed = omega_diag_squared * omega_diag; + + Bd_.block<2,2>(4, 0) = omega_diag_cubed; +} + +} // namespace vortex::guidance diff --git a/gripper_guidance/src/gripper_reference_filter_ros.cpp b/gripper_guidance/src/gripper_reference_filter_ros.cpp new file mode 100644 index 0000000..1ee7a43 --- /dev/null +++ b/gripper_guidance/src/gripper_reference_filter_ros.cpp @@ -0,0 +1,266 @@ +#include "gripper_reference_filter/gripper_reference_filter_ros.hpp" +#include +#include +#include +#include +#include "gripper_reference_filter/gripper_reference_filter_ros_utils.hpp" + +const auto start_message = R"( + + ____ _ ____ __ _____ _ _ _ + / ___|_ __ (_)_ __ _ __ ___ _ __ | _ \ ___ / _| ___ _ __ ___ _ __ ___ ___ | ___(_) | |_ ___ _ __ +| | _| '_ \| | '_ \| '_ \ / _ \ '__| | |_) / _ \ |_ / _ \ '__/ _ \ '_ \ / __/ _ \ | |_ | | | __/ _ \ '__| +| |_| | |_) | | |_) | |_) | __/ | | _ < __/ _| __/ | | __/ | | | (_| __/ | _| | | | || __/ | + \____| .__/|_| .__/| .__/ \___|_| |_| \_\___|_| \___|_| \___|_| |_|\___\___| |_| |_|_|\__\\\___|_| + |_| |_| |_| + + )"; + +namespace vortex::guidance { + +GripperReferenceFilterNode::GripperReferenceFilterNode(const rclcpp::NodeOptions& options) + : Node("gripper_reference_filter_node", options) { + set_subscribers_and_publisher(); + + set_action_server(); + + set_refererence_filter(); + + held_reference_republish_timer_ = this->create_wall_timer( + time_step_ms_, + [this]() { republish_held_reference_tick(); }); + + spdlog::info(start_message); +} + +GripperReferenceFilterNode::~GripperReferenceFilterNode() { + preempted_ = true; + if (execute_thread_.joinable()) { + execute_thread_.join(); + } +} + +void GripperReferenceFilterNode::set_subscribers_and_publisher() { + const std::string guidance_topic = + this->declare_parameter("topics.guidance_gripper"); + const std::string gripper_state_topic = + this->declare_parameter("topics.gripper_state"); + + const auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); + + reference_pub_ = this->create_publisher( + guidance_topic, qos_sensor_data); + + reference_sub_ = this->create_subscription( + gripper_state_topic, qos_sensor_data, + [this](const vortex_msgs::msg::GripperState::SharedPtr state_msg) { + reference_callback(state_msg); + }); +} + +void GripperReferenceFilterNode::set_action_server() { + this->declare_parameter("action_servers.gripper_reference_filter"); + const std::string action_server_name = + this->get_parameter("action_servers.gripper_reference_filter").as_string(); + + action_server_ = rclcpp_action::create_server< + vortex_msgs::action::GripperReferenceFilterWaypoint>( + this, action_server_name, + [this](const auto& uuid, auto goal) { + return handle_goal(uuid, std::move(goal)); + }, + [this](auto goal_handle) { return handle_cancel(goal_handle); }, + [this](auto goal_handle) { handle_accepted(goal_handle); }); +} + +void GripperReferenceFilterNode::set_refererence_filter() { + const int time_step_ms_param = + this->declare_parameter("time_step_ms", 10); + time_step_ms_ = std::chrono::milliseconds(time_step_ms_param); + + this->declare_parameter>("zeta"); + this->declare_parameter>("omega"); + + const std::vector zeta = this->get_parameter("zeta").as_double_array(); + const std::vector omega = this->get_parameter("omega").as_double_array(); + + const Eigen::Vector2d zeta_eigen = Eigen::Map(zeta.data()); + const Eigen::Vector2d omega_eigen = Eigen::Map(omega.data()); + + const GripperReferenceFilterParams filter_params{omega_eigen, zeta_eigen}; + gripper_reference_filter_ = std::make_unique(filter_params); +} + +void GripperReferenceFilterNode::reference_callback( + const vortex_msgs::msg::GripperState::SharedPtr state_msg) { + std::lock_guard lock(mutex_); + measured_reference_ << state_msg->roll, state_msg->pinch; +} + +rclcpp_action::GoalResponse GripperReferenceFilterNode::handle_goal( + const rclcpp_action::GoalUUID& /*uuid*/, + std::shared_ptr + /*goal*/) { + { + std::lock_guard lock(mutex_); + holding_reference_ = false; + } + spdlog::info("Accepted goal request"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse GripperReferenceFilterNode::handle_cancel( + const std::shared_ptr> /*goal_handle*/) { + spdlog::info("Received request to cancel goal"); + return rclcpp_action::CancelResponse::ACCEPT; +} + +void GripperReferenceFilterNode::handle_accepted( + const std::shared_ptr> goal_handle) { + std::lock_guard lock(execute_mutex_); + preempted_ = true; + if (execute_thread_.joinable()) { + execute_thread_.join(); + } + preempted_ = false; + + execute_thread_ = + std::thread([this, goal_handle]() { execute(goal_handle); }); +} + +void GripperReferenceFilterNode::latch_current_state_as_held_reference() { + if (!reference_pub_) { + return; + } + auto held_message = std::make_unique( + fill_reference_msg(gripper_reference_filter_->reference_output())); + { + std::lock_guard lock(mutex_); + last_published_reference_ = *held_message; + holding_reference_ = true; + } + + reference_pub_->publish(std::move(held_message)); +} + +void GripperReferenceFilterNode::republish_held_reference_tick() { + auto held_message = std::make_unique(); + { + std::lock_guard lock(mutex_); + if (!holding_reference_ || !reference_pub_) { + return; + } + *held_message = last_published_reference_; + } + + reference_pub_->publish(std::move(held_message)); +} + +void GripperReferenceFilterNode::execute( + const std::shared_ptr> goal_handle) { + spdlog::info("Executing goal"); + + const Eigen::Vector2d filter_seed = [this] { + std::lock_guard lock(mutex_); + if (holding_reference_) { + Eigen::Vector2d seed; + seed << last_published_reference_.roll, + last_published_reference_.pinch; + return seed; + } + return measured_reference_; + }(); + gripper_reference_filter_->reset(filter_seed); + + const vortex_msgs::msg::GripperWaypoint waypoint_goal = + goal_handle->get_goal()->waypoint; + + const uint8_t mode = waypoint_goal.mode; + + double convergence_threshold = goal_handle->get_goal()->convergence_threshold; + if (convergence_threshold <= 0.0) { + convergence_threshold = 0.1; + spdlog::warn( + "GripperReferenceFilter: Invalid convergence_threshold received (<= 0). " + "Using default 0.1"); + } + + const Eigen::Vector2d goal_reference = + apply_mode_logic(fill_reference_goal(waypoint_goal), filter_seed, mode); + + const double time_step_seconds = time_step_ms_.count() / 1000.0; + + auto result = std::make_shared< + vortex_msgs::action::GripperReferenceFilterWaypoint::Result>(); + + rclcpp::Rate loop_rate(1000.0 / time_step_ms_.count()); + + while (rclcpp::ok()) { + if (preempted_.load()) { + latch_current_state_as_held_reference(); + result->success = false; + goal_handle->abort(result); + spdlog::info("Goal preempted by newer goal"); + return; + } + + if (goal_handle->is_canceling()) { + latch_current_state_as_held_reference(); + result->success = false; + goal_handle->canceled(result); + spdlog::info("Goal canceled"); + return; + } + + gripper_reference_filter_->step(goal_reference, time_step_seconds); + + const Eigen::Vector2d filter_output = gripper_reference_filter_->reference_output(); + + auto reference_message = + std::make_unique( + fill_reference_msg(filter_output)); + { + std::lock_guard lock(mutex_); + last_published_reference_ = *reference_message; + } + reference_pub_->publish(std::move(reference_message)); + + const Eigen::Vector2d convergence_error = + compute_convergence_error(filter_output, goal_reference, mode); + + if (convergence_error.norm() < convergence_threshold) { + gripper_reference_filter_->snap_to(goal_reference); + auto final_message = + std::make_unique( + fill_reference_msg(gripper_reference_filter_->reference_output())); + { + std::lock_guard lock(mutex_); + last_published_reference_ = *final_message; + holding_reference_ = true; + } + reference_pub_->publish(std::move(final_message)); + result->success = true; + goal_handle->succeed(result); + spdlog::info("Goal reached"); + return; + } + + loop_rate.sleep(); + } + + if (!rclcpp::ok() && goal_handle->is_active()) { + result->success = false; + try { + goal_handle->abort(result); + } catch (...) { + // Ignore exceptions during shutdown + } + } +} + +} // namespace vortex::guidance + +RCLCPP_COMPONENTS_REGISTER_NODE(vortex::guidance::GripperReferenceFilterNode) diff --git a/gripper_guidance/test/CMakeLists.txt b/gripper_guidance/test/CMakeLists.txt new file mode 100644 index 0000000..99ed167 --- /dev/null +++ b/gripper_guidance/test/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.8) + +find_package(GTest REQUIRED) +include(GoogleTest) + +set(TEST_BINARY_NAME ${PROJECT_NAME}_test) +add_executable( + ${TEST_BINARY_NAME} + test_gripper_reference_filter.cpp +) + +target_link_libraries( + ${TEST_BINARY_NAME} + PRIVATE + ${LIB_NAME} + GTest::GTest +) + +ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3) + +gtest_discover_tests(${TEST_BINARY_NAME}) diff --git a/gripper_guidance/test/test_gripper_reference_filter.cpp b/gripper_guidance/test/test_gripper_reference_filter.cpp new file mode 100644 index 0000000..5dc5632 --- /dev/null +++ b/gripper_guidance/test/test_gripper_reference_filter.cpp @@ -0,0 +1,91 @@ +#include +#include "gripper_reference_filter/gripper_reference_filter.hpp" + +using namespace vortex::guidance; + +class ReferenceFilterTest : public ::testing::Test { +protected: + void SetUp() override { + GripperReferenceFilterParams params; + params.omega << 1.0, 1.0; + params.zeta << 1.0, 1.0; + filter = std::make_unique(params); + } + + Eigen::Vector2d integrate_to_steady_state(const Eigen::Vector2d& initial_reference, + const Eigen::Vector2d& goal_reference, + int steps, + double time_step_seconds = 0.01) { + filter->reset(initial_reference); + for (int i = 0; i < steps; ++i) { + filter->step(goal_reference, time_step_seconds); + } + return filter->reference_output(); + } + + bool position_converged_to_reference(const Eigen::Vector2d& output, + const Eigen::Vector2d& goal_reference, + double tol = 1e-2) { + return std::abs(output(0) - goal_reference(0)) < tol && + std::abs(output(1) - goal_reference(1)) < tol; + } + + bool no_overshoot(const Eigen::Vector2d& output, + const Eigen::Vector2d& goal_reference, + double tol = 1e-6) { + return output(0) <= goal_reference(0) + tol && + output(1) <= goal_reference(1) + tol; + } + + std::unique_ptr filter; +}; + +// Reset seeds the position output but produces no motion until step() is called. +TEST_F(ReferenceFilterTest, ResetSeedsPositionOutput) { + Eigen::Vector2d seed; + seed << 0.5, -0.25; + + filter->reset(seed); + + EXPECT_TRUE(filter->reference_output().isApprox(seed, 1e-9)); +} + +// Step response: filter output converges to the goal reference over time +// (Fossen Eq. 12.11-12.12). +TEST_F(ReferenceFilterTest, StepResponseConverges) { + Eigen::Vector2d goal; + goal << 1.0, 0.5; + + const Eigen::Vector2d output = + integrate_to_steady_state(Eigen::Vector2d::Zero(), goal, 2000); + + EXPECT_TRUE(position_converged_to_reference(output, goal)); +} + +// Critically damped (zeta=1) should produce no overshoot. +TEST_F(ReferenceFilterTest, NoCriticallyDampedOvershoot) { + Eigen::Vector2d goal; + goal << 1.0, 1.0; + + filter->reset(Eigen::Vector2d::Zero()); + for (int i = 0; i < 2000; ++i) { + filter->step(goal, 0.01); + EXPECT_TRUE(no_overshoot(filter->reference_output(), goal)); + } +} + +// snap_to forces the position output to match the goal exactly. +TEST_F(ReferenceFilterTest, SnapToMatchesGoal) { + Eigen::Vector2d goal; + goal << 3.14, -0.7; + + filter->reset(Eigen::Vector2d::Zero()); + filter->snap_to(goal); + + EXPECT_TRUE(filter->reference_output().isApprox(goal, 1e-9)); +} + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/gripper_interface/CMakeLists.txt b/gripper_interface/CMakeLists.txt index 246f39c..853212c 100644 --- a/gripper_interface/CMakeLists.txt +++ b/gripper_interface/CMakeLists.txt @@ -9,36 +9,74 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -include_directories(include) - find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -find_package(sensor_msgs REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(vortex_utils_ros REQUIRED) find_package(spdlog REQUIRED) -find_package(fmt REQUIRED) -add_executable(${PROJECT_NAME}_node - src/gripper_interface_node.cpp - src/gripper_interface_driver.cpp +# Global include — inherited by all targets in this directory AND add_subdirectory(test) +include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) + +# --------------------------------------------------------------------------- +# Component library (composable node) +# --------------------------------------------------------------------------- +set(COMPONENT_LIB ${PROJECT_NAME}_component) + +add_library(${COMPONENT_LIB} SHARED + src/can_interface.cpp + src/gripper_interface_ros.cpp ) -target_link_libraries(${PROJECT_NAME}_node fmt::fmt) +target_include_directories(${COMPONENT_LIB} PUBLIC + $ + $ +) -ament_target_dependencies(${PROJECT_NAME}_node +ament_target_dependencies(${COMPONENT_LIB} PUBLIC rclcpp - sensor_msgs + rclcpp_components + vortex_msgs + vortex_utils_ros spdlog - fmt ) -install(DIRECTORY - launch - config - DESTINATION share/${PROJECT_NAME} +rclcpp_components_register_node( + ${COMPONENT_LIB} + PLUGIN "vortex::interface::GripperInterfaceNode" + EXECUTABLE ${PROJECT_NAME}_node ) +# --------------------------------------------------------------------------- +# Standalone executable +# --------------------------------------------------------------------------- +add_executable(${PROJECT_NAME}_standalone + src/gripper_interface_node.cpp +) + +target_link_libraries(${PROJECT_NAME}_standalone PUBLIC + ${COMPONENT_LIB} + fmt::fmt +) +ament_target_dependencies(${PROJECT_NAME}_standalone PUBLIC rclcpp) + +# --------------------------------------------------------------------------- +# Install +# --------------------------------------------------------------------------- install(TARGETS - ${PROJECT_NAME}_node - DESTINATION lib/${PROJECT_NAME}) + ${COMPONENT_LIB} + ${PROJECT_NAME}_standalone + EXPORT export_${COMPONENT_LIB} + 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() diff --git a/gripper_interface/config/gripper_interface_params.yaml b/gripper_interface/config/gripper_interface_params.yaml new file mode 100644 index 0000000..e16cfa9 --- /dev/null +++ b/gripper_interface/config/gripper_interface_params.yaml @@ -0,0 +1,5 @@ +gripper_interface_node: + ros__parameters: + can_interface: "vcan0" + topics.gripper_state: "nautilus/gripper/state" + topics.velocity_command: "nautilus/gripper/control" diff --git a/gripper_interface/config/params.yaml b/gripper_interface/config/params.yaml deleted file mode 100644 index a95a1f7..0000000 --- a/gripper_interface/config/params.yaml +++ /dev/null @@ -1,14 +0,0 @@ -/gripper_interface_node: - ros__parameters: - topics: - joy: "/joy" - pwm: "/gripper_pwm" - joint_state: "/joint_state_gripper" - - pwm: - gain: 200 # value range: pwm_idle +/- pwm_gain - idle: 1500 - - i2c: - bus: 7 - address: 0x31 diff --git a/gripper_interface/include/gripper_interface/can_interface.hpp b/gripper_interface/include/gripper_interface/can_interface.hpp new file mode 100644 index 0000000..ff7f4a9 --- /dev/null +++ b/gripper_interface/include/gripper_interface/can_interface.hpp @@ -0,0 +1,54 @@ +#ifndef CAN_INTERFACE_H +#define CAN_INTERFACE_H + +#include +#include +#include +#include +#include +#include + +enum class can_status { + OK = 0, + ERR_ALREADY_INITIALIZED = -1, + ERR_SOCKET = -2, + ERR_CANFD_SUPPORT = -3, + ERR_LOOPBACK = -4, + ERR_INTERFACE_INDEX = -5, + ERR_BIND = -6, + ERR_NOT_INITIALIZED = -7, + ERR_DATA_LENGTH = -8, + ERR_SEND = -9, + ERR_RECEIVE = -10, + ERR_SETTING_TIMEOUT = -11, + ERR_SETTING_FILTER = -12, + ERR_CLEARING_FILTERS = -13 +}; + +class can_interface { +private: + int socket_fd_; + bool is_initialized_; + std::string interface_name_; + + std::thread receive_thread_; + std::atomic receiving_ = false; + +public: + can_interface(); + ~can_interface(); + + can_status init(const std::string& ifname = "can0"); + can_status send(uint32_t can_id, const uint8_t* data, uint8_t len, bool use_brs = true); + can_status receive(struct canfd_frame& frame); + can_status receive(struct canfd_frame& frame, int timeout_ms); + can_status start_async_receive(std::function callback); + void stop_async_receive(); + can_status set_filter(uint32_t can_id, uint32_t can_mask = 0x7FF); + can_status set_filters(const struct can_filter* filters, size_t num_filters); + can_status clear_filters(); + bool is_initialized() const; + std::string get_interface_name() const; +}; + +#endif // CAN_INTERFACE_H diff --git a/gripper_interface/include/gripper_interface/gripper_interface_driver.hpp b/gripper_interface/include/gripper_interface/gripper_interface_driver.hpp deleted file mode 100644 index 60416c2..0000000 --- a/gripper_interface/include/gripper_interface/gripper_interface_driver.hpp +++ /dev/null @@ -1,111 +0,0 @@ -#ifndef GRIPPER_INTERFACE_DRIVER_HPP -#define GRIPPER_INTERFACE_DRIVER_HPP - -#include -#include -#include -#include -#include -#include // for std::transform -#include -#include -#include -#include -#include -#include -#include // for std::iota -#include -#include -#include - -/** - * @brief Class for interfacing with the gripper. - */ -class GripperInterfaceDriver { - public: - ~GripperInterfaceDriver(); - - /** - * @brief Constructor for the GripperInterfaceDriver class. - * @param i2c_bus The I2C bus number. - * @param i2c_address The I2C address of the microcontroller. - * @param pwm_gain The gain for converting joystick values to PWM values. - * @param pwm_idle The idle PWM value. - */ - GripperInterfaceDriver(short i2c_bus, - int i2c_address, - int pwm_gain, - int pwm_idle); - - /** - * @brief Convert joystick value to PWM value. - * @param joy_value The joystick value. - * @return The PWM value. - */ - std::uint16_t joy_to_pwm(const double joy_value); - - /** - * @brief Send PWM values to the gripper. - * @param pwm_values The PWM values. - */ - void send_pwm(const std::vector& pwm_values); - - /** - * @brief Start gripper by sending 0x02 first byte - * @param None - */ - void start_gripper(); - - /** - * @brief Stop gripper by sending 0x01 first byte - * @param None - */ - void stop_gripper(); - - /** - * @brief Reads the raw angle of each encoder - * @param None - * @Return vector containing the angles of the shoulder, wrist and grip in - * order - */ - - std::vector encoder_read(); - - private: - int bus_fd_; // File descriptor for I2C bus - int i2c_bus_; // I2C bus number - int i2c_address_; // I2C address of the microcontroller - int pwm_gain_; - int pwm_idle_; - - /** - * @brief Convert PWM value to I2C data. - * @param pwm The PWM value. - * @return The I2C data. - */ - static constexpr std::array pwm_to_i2c_data( - std::uint16_t pwm) { - return {static_cast((pwm >> 8) & 0xFF), - static_cast(pwm & 0xFF)}; - } - - /** - * @brief Converts two uint8_t to uint16_t - *@param Array containing to uint8_t - *@return Encoder angles - */ - static constexpr std::uint16_t i2c_to_encoder_angles( - std::array data) { - return (static_cast(data[0]) << 8) | data[1]; - } - /** - *@brief Converts raw encoder angle to radians - *@param Raw encoder angle (uint16_t) - *@return angle in radians (double) - */ - static constexpr double raw_angle_to_radians(std::uint16_t raw_angle) { - return (static_cast(raw_angle) / 0x3FFF) * (2.0 * M_PI); - } -}; // class GripperInterfaceDriver - -#endif // GRIPPER_INTERFACE_DRIVER_HPP diff --git a/gripper_interface/include/gripper_interface/gripper_interface_node.hpp b/gripper_interface/include/gripper_interface/gripper_interface_node.hpp deleted file mode 100644 index 627e26e..0000000 --- a/gripper_interface/include/gripper_interface/gripper_interface_node.hpp +++ /dev/null @@ -1,59 +0,0 @@ -#ifndef GRIPPER_INTERFACE_HPP -#define GRIPPER_INTERFACE_HPP - -#include -#include -#include -#include -#include -#include -#include -#include -#include "gripper_interface/gripper_interface_driver.hpp" - -class GripperInterface : public rclcpp::Node { - public: - GripperInterface(); - - private: - /** - * @brief Extract parameters from the config file. - */ - void extract_parameters(); - - void set_publisher_and_subsribers(); - - /** - * @brief Callback function for the joystick message. - * @param msg The joystick message. - */ - void joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg); - - void encoder_angles_callback(); - - /** - * @brief Convert a vector of PWM values to a ROS message. - * @param vec The vector of PWM values. - * @return The ROS message. - */ - std_msgs::msg::Int16MultiArray vec_to_msg(std::vector vec); - - std::string joy_topic_; - std::string pwm_topic_; - std::string joint_state_topic_; - int i2c_bus_; - int i2c_address_; - int pwm_gain_; - int pwm_idle_; - - std::unique_ptr gripper_driver_; - - rclcpp::Publisher::SharedPtr joint_state_pub_; - - rclcpp::Subscription::SharedPtr joy_sub_; - rclcpp::Publisher::SharedPtr pwm_pub_; - rclcpp::TimerBase::SharedPtr watchdog_timer_; - rclcpp::Time last_msg_time_; -}; - -#endif // GRIPPER_INTERFACE_HPP diff --git a/gripper_interface/include/gripper_interface/gripper_interface_ros.hpp b/gripper_interface/include/gripper_interface/gripper_interface_ros.hpp new file mode 100644 index 0000000..439d6f5 --- /dev/null +++ b/gripper_interface/include/gripper_interface/gripper_interface_ros.hpp @@ -0,0 +1,38 @@ +#pragma once + +#include +#include +#include +#include +#include +#include "gripper_interface/can_interface.hpp" +#include "gripper_interface/gripper_interface_translator.hpp" + +#include +#include + +namespace vortex::interface { + +class GripperInterfaceNode : public rclcpp::Node { +public: + explicit GripperInterfaceNode(const rclcpp::NodeOptions& options); + ~GripperInterfaceNode() override; + +private: + void declare_and_load_parameters(); + void init_can(); + void setup_pub_sub(); + + void on_can_frame(const struct canfd_frame& frame, can_status status); + void velocity_command_callback( + const vortex_msgs::msg::GripperStateVelocityCommand::SharedPtr msg); + + can_interface can_; + std::string can_interface_name_; + std::mutex pub_mutex_; + + rclcpp::Publisher::SharedPtr state_pub_; + rclcpp::Subscription::SharedPtr velocity_cmd_sub_; +}; + +} // namespace vortex::interface diff --git a/gripper_interface/include/gripper_interface/gripper_interface_translator.hpp b/gripper_interface/include/gripper_interface/gripper_interface_translator.hpp new file mode 100644 index 0000000..796adc6 --- /dev/null +++ b/gripper_interface/include/gripper_interface/gripper_interface_translator.hpp @@ -0,0 +1,98 @@ +#ifndef GRIPPER_INTERFACE__GRIPPER_INTERFACE_TRANSLATOR_HPP_ +#define GRIPPER_INTERFACE__GRIPPER_INTERFACE_TRANSLATOR_HPP_ + +#include +#include +#include + +#include +#include + +#include "gripper_interface/gripper_interface_typedefs.hpp" + +// --------------------------------------------------------------------------- +// Responsibility: pure, stateless conversions between raw CAN byte payloads +// and ROS/domain types. No I2C, no file descriptors, no ROS dependencies +// beyond message types. +// +// Encoding conventions (matching MCU firmware): +// - Encoder values: big-endian uint16_t, 14-bit (mask with 0x3FFF) +// - PWM values: big-endian uint16_t, microseconds +// --------------------------------------------------------------------------- + +namespace gripper_interface::translator { + +// --------------------------------------------------------------------------- +// Encoder helpers (mirroring MCU's raw_angle_to_radians / i2c_to_encoder_angles) +// --------------------------------------------------------------------------- + +inline uint16_t bytes_to_uint16(uint8_t msb, uint8_t lsb) { + return static_cast((static_cast(msb) << 8) | lsb); +} + +inline double raw_angle_to_radians(uint16_t raw) { + return (static_cast(raw & 0x3FFF) / types::ENCODER_COUNTS) * types::TWO_PI; +} + +// --------------------------------------------------------------------------- +// RawEncoderFrame → GripperState (position, radians) +// +// Byte layout from MCU (NUM_ENCODERS=2 path): +// bytes[0..1] = wrist encoder → roll +// bytes[2..3] = grip encoder → pinch +// bytes[4..5] = unused +// +// WARNING: firmware read_encoders() uses `buf = out + enc_num` instead of +// `out + enc_num * 2`. Verify byte layout on hardware — if enc_num indexing +// is wrong the bytes[2..3] pinch data may be corrupted. +// --------------------------------------------------------------------------- +inline vortex_msgs::msg::GripperState +encoder_frame_to_gripper_state(const types::RawEncoderFrame& frame) { + const uint16_t raw_roll = bytes_to_uint16(frame.bytes[0], frame.bytes[1]); + const uint16_t raw_pinch = bytes_to_uint16(frame.bytes[2], frame.bytes[3]); + + vortex_msgs::msg::GripperState msg; + msg.roll = raw_angle_to_radians(raw_roll) * types::GEAR_RATIO_ROLL; + msg.pinch = raw_angle_to_radians(raw_pinch) * types::GEAR_RATIO_PINCH; + return msg; +} + +// --------------------------------------------------------------------------- +// VelocityCommand → RawPwmFrame +// +// Velocity is normalised by MAX_VEL, then mapped to [PWM_MIN_US, PWM_MAX_US]. +// Idle (zero velocity) = PWM_IDLE_US = 1500 µs. +// +// Formula: duty_us = IDLE + clamp(vel / MAX_VEL, -1, 1) * RANGE +// +// The 4-byte payload sent on CAN SET_PWM: +// bytes[0..1] = roll duty (µs) big-endian uint16_t +// bytes[2..3] = pinch duty (µs) big-endian uint16_t +// --------------------------------------------------------------------------- +inline uint16_t velocity_to_duty_us(double velocity, double max_velocity) { + const double normalised = std::clamp(velocity / max_velocity, -1.0, 1.0); + const double duty = types::PWM_IDLE_US + normalised * types::PWM_RANGE_US; + return static_cast( + std::clamp(duty, + static_cast(types::PWM_MIN_US), + static_cast(types::PWM_MAX_US))); +} + +inline types::RawPwmFrame velocity_command_to_pwm_frame( + const vortex_msgs::msg::GripperStateVelocityCommand& cmd) { + + const uint16_t roll_us = velocity_to_duty_us(cmd.roll_velocity, types::MAX_ROLL_VEL); + const uint16_t pinch_us = velocity_to_duty_us(cmd.pinch_velocity, types::MAX_PINCH_VEL); + + types::RawPwmFrame frame; + // Big-endian packing — MSB first, matching MCU memcpy expectation + frame.bytes[0] = static_cast((roll_us >> 8) & 0xFF); + frame.bytes[1] = static_cast( roll_us & 0xFF); + frame.bytes[2] = static_cast((pinch_us >> 8) & 0xFF); + frame.bytes[3] = static_cast( pinch_us & 0xFF); + return frame; +} + +} // namespace gripper_interface::translator + +#endif // GRIPPER_INTERFACE__GRIPPER_INTERFACE_TRANSLATOR_HPP_ diff --git a/gripper_interface/include/gripper_interface/gripper_interface_typedefs.hpp b/gripper_interface/include/gripper_interface/gripper_interface_typedefs.hpp new file mode 100644 index 0000000..83f0683 --- /dev/null +++ b/gripper_interface/include/gripper_interface/gripper_interface_typedefs.hpp @@ -0,0 +1,65 @@ +#ifndef GRIPPER_INTERFACE__GRIPPER_INTERFACE_TYPEDEFS_HPP_ +#define GRIPPER_INTERFACE__GRIPPER_INTERFACE_TYPEDEFS_HPP_ + +#include +#include + +namespace gripper_interface::types { + +// --------------------------------------------------------------------------- +// Raw CAN payloads — exactly what crosses the CAN bus. +// All multi-byte fields are big-endian (MCU packs MSB first). +// --------------------------------------------------------------------------- + +// MCU → Host: 6 bytes, indices [0..1]=wrist, [2..3]=grip, [4..5]=unused +// Raw 14-bit encoder value (AS5048 / compatible) packed in uint16_t. +// NOTE: firmware bug suspected in read_encoders() — buf = out + enc_num +// should likely be out + enc_num*2. Verify on hardware before trusting +// bytes 2-3 when running NUM_ENCODERS=2. +struct RawEncoderFrame { + std::array bytes{}; +}; + +// Host → MCU: 4 bytes = uint16_t[2], duty cycle in microseconds [roll, pinch] +// Servo range: 700 µs (full reverse) – 1500 µs (idle) – 2300 µs (full forward) +struct RawPwmFrame { + std::array bytes{}; +}; + +// Decoded encoder angles in radians +struct EncoderAngles { + double roll = 0.0; // wrist encoder → roll + double pinch = 0.0; // grip encoder → pinch +}; + +// Velocity command from controller (rad/s) +struct VelocityCommand { + double roll_vel = 0.0; + double pinch_vel = 0.0; +}; + +// --------------------------------------------------------------------------- +// Servo / encoder constants +// --------------------------------------------------------------------------- +constexpr double ENCODER_COUNTS = 16383.0; // 0x3FFF — 14-bit AS5048 +constexpr double TWO_PI = 6.283185307179586; + +// Servo PWM limits (microseconds) — from tcc.c comments +constexpr uint16_t PWM_IDLE_US = 1500; +constexpr uint16_t PWM_MAX_US = 2300; +constexpr uint16_t PWM_MIN_US = 700; +constexpr uint16_t PWM_RANGE_US = PWM_MAX_US - PWM_IDLE_US; // 800 + +// Gear ratios — both assumed 1.0 until experimentally calibrated +// TODO: measure worm/leadscrew reduction after servo installation +constexpr double GEAR_RATIO_ROLL = 1.0; +constexpr double GEAR_RATIO_PINCH = 1.0; + +// Velocity saturation — must match types::MAX_ROLL_VEL / MAX_PINCH_VEL +// in gripper_controller_typedefs.hpp +constexpr double MAX_ROLL_VEL = 1.0; // rad/s +constexpr double MAX_PINCH_VEL = 1.0; // rad/s + +} // namespace gripper_interface::types + +#endif // GRIPPER_INTERFACE__GRIPPER_INTERFACE_TYPEDEFS_HPP_ diff --git a/gripper_interface/interface_to_gripper_state/interface_to_gripper_state.cpp b/gripper_interface/interface_to_gripper_state/interface_to_gripper_state.cpp new file mode 100644 index 0000000..4c6e352 --- /dev/null +++ b/gripper_interface/interface_to_gripper_state/interface_to_gripper_state.cpp @@ -0,0 +1,32 @@ +#include +#include +#include "sensor_msgs/msg/joint_state.hpp" +#include "vortex_msgs/msg/gripper_state.hpp" + +namespace vortex::guidance { + +// TODO: Calibrate gear ratios experimentally before deployment. +// Roll: suspect direct drive but may have a worm gear reduction. +// Pinch: leadscrew/rack — gear_ratio_pinch converts encoder radians +// to jaw displacement (metres or radians depending on convention). +constexpr double GEAR_RATIO_ROLL = 1.0; // TODO: measure — direct drive assumed for now +constexpr double GEAR_RATIO_PINCH = 1.0; // TODO: measure — leadscrew pitch unknown + +constexpr std::size_t ENCODER_INDEX_ROLL = 0; // wrist → roll +constexpr std::size_t ENCODER_INDEX_PINCH = 1; // grip → pinch +constexpr std::size_t ENCODER_MIN_SIZE = 2; + +inline vortex_msgs::msg::GripperState translate_to_gripper_state( + const sensor_msgs::msg::JointState::SharedPtr& msg) + { + if (msg->position.size() < ENCODER_MIN_SIZE) { + throw std::runtime_error( + "JointState has fewer than 2 positions — expected [roll, grip]"); + } + + vortex_msgs::msg::GripperState state; + state.roll = msg->position[ENCODER_INDEX_ROLL] * GEAR_RATIO_ROLL; + state.pinch = msg->position[ENCODER_INDEX_PINCH] * GEAR_RATIO_PINCH; + return state; + } +} diff --git a/gripper_interface/launch/gripper_interface.launch.py b/gripper_interface/launch/gripper_interface.launch.py index b005602..efb5273 100644 --- a/gripper_interface/launch/gripper_interface.launch.py +++ b/gripper_interface/launch/gripper_interface.launch.py @@ -1,20 +1,22 @@ -from os import path - +import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node -params = path.join( - get_package_share_directory('gripper_interface'), 'config', 'params.yaml' -) - def generate_launch_description(): + config_file_path = os.path.join( + get_package_share_directory('gripper_interface'), + 'config', + 'gripper_interface_params.yaml', + ) + gripper_interface_node = Node( package='gripper_interface', - executable='gripper_interface_node', + executable='gripper_interface_standalone', name='gripper_interface_node', - parameters=[params], + parameters=[config_file_path], output='screen', ) + return LaunchDescription([gripper_interface_node]) diff --git a/gripper_interface/package.xml b/gripper_interface/package.xml index 303a612..7d16668 100644 --- a/gripper_interface/package.xml +++ b/gripper_interface/package.xml @@ -2,15 +2,22 @@ gripper_interface - 0.0.0 - Package to take joy inputs and turn them into pwm signals for the gripper servos - andeshog + 0.0.1 + CAN-bus interface node between ROS 2 and the gripper MCU firmware. + Vortex NTNU MIT ament_cmake rclcpp - sensor_msgs + rclcpp_components + vortex_msgs + vortex_utils_ros + spdlog + fmt + + ament_lint_auto + ament_lint_common ament_cmake diff --git a/gripper_interface/src/can_interface.cpp b/gripper_interface/src/can_interface.cpp new file mode 100644 index 0000000..ec6da01 --- /dev/null +++ b/gripper_interface/src/can_interface.cpp @@ -0,0 +1,199 @@ +#include "gripper_interface/can_interface.hpp" +#include +#include +#include +#include +#include +#include +#include + +can_interface::can_interface() : socket_fd_(-1), is_initialized_(false) {} + +can_interface::~can_interface() { + if (is_initialized_) { + close(socket_fd_); + } +} + +can_status can_interface::init(const std::string& ifname) { + interface_name_ = ifname; + + socket_fd_ = socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (socket_fd_ < 0) { + return can_status::ERR_SOCKET; + } + + int enable_canfd = 1; + if (setsockopt(socket_fd_, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, + &enable_canfd, sizeof(enable_canfd)) < 0) { + close(socket_fd_); + return can_status::ERR_CANFD_SUPPORT; + } + + // Enable loopback (receive own frames). Only use for vcan testing + //int recv_own_msgs = 1; + //if (setsockopt(socket_fd_, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, + // &recv_own_msgs, sizeof(recv_own_msgs)) < 0) { + // close(socket_fd_); + // return can_status::ERR_LOOPBACK; + //} + + struct ifreq ifr; + std::strcpy(ifr.ifr_name, interface_name_.c_str()); + if (ioctl(socket_fd_, SIOCGIFINDEX, &ifr) < 0) { + close(socket_fd_); + return can_status::ERR_INTERFACE_INDEX; + } + + struct sockaddr_can addr; + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + if (bind(socket_fd_, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + close(socket_fd_); + return can_status::ERR_BIND; + } + + is_initialized_ = true; + + return can_status::OK; +} + +can_status can_interface::send(uint32_t can_id, const uint8_t* data, uint8_t len, bool use_brs) { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + if (len > 64) { + return can_status::ERR_DATA_LENGTH; + } + + struct canfd_frame frame; + memset(&frame, 0, sizeof(frame)); + + frame.can_id = can_id; + frame.len = len; + frame.flags = use_brs ? CANFD_BRS : 0; + + if (data != nullptr && len > 0) { + memcpy(frame.data, data, len); + } + + if (write(socket_fd_, &frame, sizeof(frame)) != sizeof(frame)) { + return can_status::ERR_SEND; + } + + return can_status::OK; +} + +can_status can_interface::receive(struct canfd_frame& frame) { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + ssize_t nbytes = read(socket_fd_, &frame, sizeof(frame)); + + if (nbytes != CANFD_MTU) { + return can_status::ERR_RECEIVE; + } + + return can_status::OK; +} + +can_status can_interface::receive(struct canfd_frame& frame, int timeout_ms) { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + struct timeval tv; + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000; + + if (setsockopt(socket_fd_, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0) { + return can_status::ERR_SETTING_TIMEOUT; + } + + ssize_t nbytes = read(socket_fd_, &frame, sizeof(frame)); + + if (nbytes != CANFD_MTU) { + return can_status::ERR_RECEIVE; + } + + return can_status::OK; +} + +can_status can_interface::start_async_receive(std::function callback) { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + receiving_ = true; + + receive_thread_ = std::thread([this, callback]() { + while (receiving_) { + struct canfd_frame frame; + can_status status = receive(frame, 1000); + callback(frame, status); + } + }); + return can_status::OK; +} + +void can_interface::stop_async_receive() { + receiving_ = false; + if (receive_thread_.joinable()) { + receive_thread_.join(); + } +} + +can_status can_interface::set_filter(uint32_t can_id, uint32_t can_mask) { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + struct can_filter filter; + filter.can_id = can_id; + filter.can_mask = can_mask; + + if (setsockopt(socket_fd_, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, sizeof(filter)) < 0) { + return can_status::ERR_SETTING_FILTER; + } + + return can_status::OK; +} + +can_status can_interface::set_filters(const struct can_filter* filters, size_t num_filters) { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + if (setsockopt(socket_fd_, SOL_CAN_RAW, CAN_RAW_FILTER, filters, num_filters * sizeof(struct can_filter)) < 0) { + return can_status::ERR_SETTING_FILTER; + } + + return can_status::OK; +} + +can_status can_interface::clear_filters() { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + struct can_filter filter; + filter.can_id = 0x0; + filter.can_mask = 0x0; + + if (setsockopt(socket_fd_, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, sizeof(filter)) < 0) { + return can_status::ERR_CLEARING_FILTERS; + } + + return can_status::OK; +} + +bool can_interface::is_initialized() const { + return is_initialized_; +} + +std::string can_interface::get_interface_name() const { + return interface_name_; +} diff --git a/gripper_interface/src/gripper_interface_driver.cpp b/gripper_interface/src/gripper_interface_driver.cpp deleted file mode 100644 index 28d61be..0000000 --- a/gripper_interface/src/gripper_interface_driver.cpp +++ /dev/null @@ -1,131 +0,0 @@ -#include "gripper_interface/gripper_interface_driver.hpp" -#include - -GripperInterfaceDriver::GripperInterfaceDriver(short i2c_bus, - int i2c_address, - int pwm_gain, - int pwm_idle) - : i2c_bus_(i2c_bus), - i2c_address_(i2c_address), - pwm_gain_(pwm_gain), - pwm_idle_(pwm_idle) { - std::string i2c_filename = std::format("/dev/i2c-{}", i2c_bus_); - bus_fd_ = - open(i2c_filename.c_str(), - O_RDWR); // Open the I2C bus for reading and writing (O_RDWR) - if (bus_fd_ < 0) { - throw std::runtime_error( - std::format("ERROR: Failed to open I2C bus {} : {}", i2c_bus_, - strerror(errno))); - } - - if (ioctl(bus_fd_, I2C_SLAVE, i2c_address_) < 0) { - throw std::runtime_error(std::format("Failed to open I2C bus {} : {}", - i2c_bus_, strerror(errno))); - return; - } -} - -GripperInterfaceDriver::~GripperInterfaceDriver() { - if (bus_fd_ >= 0) { - send_pwm(std::vector(3, pwm_idle_)); - close(bus_fd_); - } -} - -std::uint16_t GripperInterfaceDriver::joy_to_pwm(const double joy_value) { - return static_cast(pwm_idle_ + pwm_gain_ * joy_value); -} - -void GripperInterfaceDriver::send_pwm( - const std::vector& pwm_values) { - try { - constexpr std::size_t i2c_data_size = - 1 + 3 * 2; // 3 thrusters * (1xMSB + 1xLSB) - std::array i2c_data_array; - - i2c_data_array.at(0) = 0x00; // "Start" byte - - for (std::size_t i = 1; i < 4; i++) { - i2c_data_array[2 * i - 1] = - static_cast((pwm_values[i] >> 8) & 0xFF); - i2c_data_array[2 * i] = static_cast(pwm_values[i] & 0xFF); - } - - if (write(bus_fd_, i2c_data_array.data(), i2c_data_size) != - i2c_data_size) { - throw std::runtime_error(std::format( - "Error: Failed to write to I2C device : {}", strerror(errno))); - } - } catch (const std::exception& e) { - spdlog::error("ERROR: Failed to send PWM values - {}", e.what()); - } catch (...) { - spdlog::error("ERROR: Failed to send PWM values - unknown error"); - } -} - -void GripperInterfaceDriver::stop_gripper() { - try { - constexpr std::size_t i2c_data_size = 1; - std::uint8_t i2c_message = 0x01; - - if (write(bus_fd_, &i2c_message, i2c_data_size) != i2c_data_size) { - throw std::runtime_error(std::format( - "Error: Failed to write to I2C device : {}", strerror(errno))); - } - } catch (const std::exception& e) { - spdlog::error("ERROR: Failed to send stop gripper command - {}", - e.what()); - } catch (...) { - spdlog::error( - "ERROR: Failed to send stop gripper command - unknown error"); - } -} - -void GripperInterfaceDriver::start_gripper() { - try { - constexpr std::size_t i2c_data_size = 1; - std::uint8_t i2c_message = 0x02; - - if (write(bus_fd_, &i2c_message, i2c_data_size) != i2c_data_size) { - throw std::runtime_error(std::format( - "Error: Failed to write to I2C device : {}", strerror(errno))); - } - } catch (const std::exception& e) { - spdlog::error("ERROR: Failed to send start gripper command - {}", - e.what()); - } catch (...) { - spdlog::error( - "ERROR: Failed to send start gripper command - unknown error"); - } -} - -std::vector GripperInterfaceDriver::encoder_read() { - constexpr std::size_t i2c_data_size = 6; // 6 bytes -> 3 angles. - constexpr std::size_t num_angles = i2c_data_size / 2; - std::array i2c_data_array; - std::vector encoder_angles; - encoder_angles.reserve(num_angles); - - try { - if (read(bus_fd_, i2c_data_array.data(), i2c_data_size) != - static_cast(i2c_data_size)) { - throw std::runtime_error(std::format( - "Error: Failed to read from I2C device: {}", strerror(errno))); - } - - for (std::size_t i = 0; i < num_angles; ++i) { - std::array pair = {i2c_data_array[2 * i], - i2c_data_array[2 * i + 1]}; - std::uint16_t raw_angle = i2c_to_encoder_angles(pair); - encoder_angles.push_back(raw_angle_to_radians(raw_angle)); - } - - return encoder_angles; - } catch (const std::exception& e) { - spdlog::error("ERROR: Failed to read encoder values - {}", e.what()); - } catch (...) { - spdlog::error("ERROR: Failed to read encoder values - unknown error"); - } - return {}; -} diff --git a/gripper_interface/src/gripper_interface_node.cpp b/gripper_interface/src/gripper_interface_node.cpp index bedeb3d..b8ff36e 100644 --- a/gripper_interface/src/gripper_interface_node.cpp +++ b/gripper_interface/src/gripper_interface_node.cpp @@ -1,99 +1,22 @@ -#include "gripper_interface/gripper_interface_node.hpp" +#include +#include +#include +#include "gripper_interface/gripper_interface_ros.hpp" -GripperInterface::GripperInterface() : Node("gripper_interface_node") { - extract_parameters(); - joy_sub_ = this->create_subscription( - joy_topic_, 10, - std::bind(&GripperInterface::joy_callback, this, - std::placeholders::_1)); - pwm_pub_ = - this->create_publisher(pwm_topic_, 10); - joint_state_pub_ = this->create_publisher( - joint_state_topic_, 10); - gripper_driver_ = std::make_unique( - i2c_bus_, i2c_address_, pwm_gain_, pwm_idle_); - - watchdog_timer_ = this->create_wall_timer( - std::chrono::milliseconds(500), - std::bind(&GripperInterface::encoder_angles_callback, this)); - - last_msg_time_ = this->now(); - spdlog::info("Gripper interface node started"); -} - -void GripperInterface::extract_parameters() { - this->declare_parameter("topics.joy"); - this->declare_parameter("topics.pwm"); - this->declare_parameter("topics.joint_state"); - this->declare_parameter("pwm.gain"); - this->declare_parameter("pwm.idle"); - this->declare_parameter("i2c.bus"); - this->declare_parameter("i2c.address"); - - this->joy_topic_ = this->get_parameter("topics.joy").as_string(); - this->pwm_topic_ = this->get_parameter("topics.pwm").as_string(); - this->joint_state_topic_ = - this->get_parameter("topics.joint_state").as_string(); - this->pwm_gain_ = this->get_parameter("pwm.gain").as_int(); - this->pwm_idle_ = this->get_parameter("pwm.idle").as_int(); - this->i2c_bus_ = this->get_parameter("i2c.bus").as_int(); - this->i2c_address_ = this->get_parameter("i2c.address").as_int(); -} - -void GripperInterface::joy_callback( - const sensor_msgs::msg::Joy::SharedPtr msg) { - std::vector pwm_values; - double shoulder_value = msg->axes[1]; - double wrist_value = msg->axes[0]; - double grip_value = msg->axes[3]; - - std::uint16_t shoulder_pwm = gripper_driver_->joy_to_pwm(shoulder_value); - std::uint16_t wrist_pwm = gripper_driver_->joy_to_pwm(wrist_value); - std::uint16_t grip_pwm = gripper_driver_->joy_to_pwm(grip_value); - - pwm_values.push_back(shoulder_pwm); - pwm_values.push_back(wrist_pwm); - pwm_values.push_back(grip_pwm); - - std_msgs::msg::Int16MultiArray pwm_msg = vec_to_msg(pwm_values); - pwm_pub_->publish(pwm_msg); - - gripper_driver_->send_pwm(pwm_values); - - if (msg->buttons[0]) { - gripper_driver_->start_gripper(); - } else if (msg->buttons[1]) { - gripper_driver_->stop_gripper(); - } -} - -void GripperInterface::encoder_angles_callback() { - std::vector angles = gripper_driver_->encoder_read(); - if (angles.empty()) { - return; - } - - auto joint_state_msg = sensor_msgs::msg::JointState(); +int main(int argc, char* argv[]) { + // Force spdlog to stdout at debug level before anything else runs + spdlog::set_default_logger(spdlog::stdout_color_mt("gripper")); + spdlog::set_level(spdlog::level::debug); + spdlog::info("main: starting gripper_interface_node"); - joint_state_msg.header.stamp = this->now(); - joint_state_msg.name = {"shoulder", "wrist", "grip"}; - joint_state_msg.position = angles; + rclcpp::init(argc, argv); - joint_state_pub_->publish(joint_state_msg); -} + rclcpp::NodeOptions options; + auto node = std::make_shared(options); -std_msgs::msg::Int16MultiArray GripperInterface::vec_to_msg( - std::vector vec) { - std_msgs::msg::Int16MultiArray msg; - for (std::uint16_t value : vec) { - msg.data.push_back(value); - } - return msg; -} + spdlog::info("main: node constructed, spinning"); + rclcpp::spin(node); -int main(int argc, char* argv[]) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } diff --git a/gripper_interface/src/gripper_interface_ros.cpp b/gripper_interface/src/gripper_interface_ros.cpp new file mode 100644 index 0000000..7428c1b --- /dev/null +++ b/gripper_interface/src/gripper_interface_ros.cpp @@ -0,0 +1,147 @@ +#include "gripper_interface/gripper_interface_ros.hpp" + +#include + +const auto start_message = R"( + + ____ _ ___ _ __ + / ___|_ __ (_)_ __ _ __ ___|_ _|_ __ | |_ ___ _ __ / _| __ _ ___ ___ +| | _| '__| | '_ \| '_ \ / _ \| || '_ \| __/ _ \ '__| |_ / _` |/ __/ _ \ +| |_| | | | | |_) | |_) | __/| || | | | || __/ | | _| (_| | (_| __/ + \____|_| |_| .__/| .__/ \___|___|_| |_|\__\___|_| |_| \__,_|\___\___| + |_| |_| + +)"; + +namespace vortex::interface { + +// --------------------------------------------------------------------------- +// Construction / Destruction +// --------------------------------------------------------------------------- + +GripperInterfaceNode::GripperInterfaceNode(const rclcpp::NodeOptions& options) +: Node("gripper_interface_node", options) { + declare_and_load_parameters(); + init_can(); + setup_pub_sub(); + spdlog::info(start_message); +} + +GripperInterfaceNode::~GripperInterfaceNode() { + spdlog::info("GripperInterfaceNode: shutting down"); + can_.stop_async_receive(); + can_.send(0x45b, nullptr, 0); // STOP_GRIPPER +} + +// --------------------------------------------------------------------------- +// Setup +// --------------------------------------------------------------------------- + +void GripperInterfaceNode::declare_and_load_parameters() { + can_interface_name_ = this->declare_parameter("can_interface", "can0"); + spdlog::info("GripperInterfaceNode: can_interface={}", can_interface_name_); +} + +void GripperInterfaceNode::init_can() { + const can_status status = can_.init(can_interface_name_); + if (status != can_status::OK) { + throw std::runtime_error( + "GripperInterfaceNode: failed to initialise CAN on '" + + can_interface_name_ + "' (status " + + std::to_string(static_cast(status)) + ")"); + } + + + // Filter to SEND_ANGLES (0x46a) only — discard all other bus traffic. + can_.set_filter(0x46a, CAN_SFF_MASK); + + // Start receive thread. on_can_frame is called for every frame that passes + // the filter, plus ERR_RECEIVE on 1-second timeouts (handled gracefully). + can_.start_async_receive( + [this](const struct canfd_frame& frame, can_status s) { + on_can_frame(frame, s); + }); + + can_.send(0x45a, nullptr, 0); // START_GRIPPER + spdlog::info("GripperInterfaceNode: CAN initialised on {}", can_interface_name_); +} + +void GripperInterfaceNode::setup_pub_sub() { + const std::string state_topic = + this->declare_parameter("topics.gripper_state", "/gripper/state"); + const std::string cmd_topic = + this->declare_parameter("topics.velocity_command", "/gripper/velocity_command"); + + auto qos = vortex::utils::qos_profiles::sensor_data_profile(1); + + state_pub_ = this->create_publisher(state_topic, qos); + + velocity_cmd_sub_ = + this->create_subscription( + cmd_topic, qos, + std::bind(&GripperInterfaceNode::velocity_command_callback, + this, std::placeholders::_1)); + + spdlog::info("GripperInterfaceNode: publishing GripperState on '{}'", state_topic); + spdlog::info("GripperInterfaceNode: subscribing velocity commands on '{}'", cmd_topic); +} + +// --------------------------------------------------------------------------- +// CAN receive (called from can_interface's internal thread) +// --------------------------------------------------------------------------- + +void GripperInterfaceNode::on_can_frame(const struct canfd_frame& frame, + can_status status) { + if (status == can_status::ERR_RECEIVE) { + return; // 1-second timeout, not an error + } + + if (status != can_status::OK) { + spdlog::warn("GripperInterfaceNode: CAN receive error (status {})", + static_cast(status)); + return; + } + + if ((frame.can_id & CAN_SFF_MASK) != 0x46a) { // not SEND_ANGLES + return; + } + + if (frame.len < 4) { + spdlog::warn("GripperInterfaceNode: SEND_ANGLES frame too short (len={})", + static_cast(frame.len)); + return; + } + + gripper_interface::types::RawEncoderFrame encoder_frame{}; + std::memcpy(encoder_frame.bytes.data(), frame.data, + std::min(frame.len, encoder_frame.bytes.size())); + + vortex_msgs::msg::GripperState state_msg = + gripper_interface::translator::encoder_frame_to_gripper_state(encoder_frame); + state_msg.header.stamp = this->now(); + + { + std::lock_guard lock(pub_mutex_); + state_pub_->publish(state_msg); + } +} + +// --------------------------------------------------------------------------- +// Velocity command → CAN (called from ROS executor thread) +// --------------------------------------------------------------------------- + +void GripperInterfaceNode::velocity_command_callback( + const vortex_msgs::msg::GripperStateVelocityCommand::SharedPtr msg) { + + const gripper_interface::types::RawPwmFrame pwm_frame = + gripper_interface::translator::velocity_command_to_pwm_frame(*msg); + + can_.send(0x469, // SET_PWM + pwm_frame.bytes.data(), + static_cast(pwm_frame.bytes.size()), + /*use_brs=*/false); +} + +RCLCPP_COMPONENTS_REGISTER_NODE(GripperInterfaceNode) + +} // namespace vortex::interface diff --git a/gripper_interface/test/CMakeLists.txt b/gripper_interface/test/CMakeLists.txt new file mode 100644 index 0000000..71ea0e8 --- /dev/null +++ b/gripper_interface/test/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.8) + +find_package(GTest REQUIRED) +find_package(vortex_msgs REQUIRED) +include(GoogleTest) + +set(TEST_BINARY_NAME ${PROJECT_NAME}_test) + +add_executable( + ${TEST_BINARY_NAME} + test_gripper_interface_translator.cpp +) + +# Inherit include dirs from the component lib (PUBLIC target_include_directories +# on COMPONENT_LIB propagates here) + the directory-level include_directories +# set in the parent CMakeLists.txt +target_link_libraries( + ${TEST_BINARY_NAME} + PRIVATE + ${PROJECT_NAME}_component + GTest::GTest +) + +ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC vortex_msgs) + +gtest_discover_tests(${TEST_BINARY_NAME}) diff --git a/gripper_interface/test/test_gripper_interface_translator.cpp b/gripper_interface/test/test_gripper_interface_translator.cpp new file mode 100644 index 0000000..09279b8 --- /dev/null +++ b/gripper_interface/test/test_gripper_interface_translator.cpp @@ -0,0 +1,410 @@ +#include +#include +#include + +#include "gripper_interface/gripper_interface_translator.hpp" +#include "gripper_interface/gripper_interface_typedefs.hpp" + +using namespace gripper_interface; + +// --------------------------------------------------------------------------- +// Helper: decode a big-endian uint16_t from two consecutive bytes +// --------------------------------------------------------------------------- +static uint16_t decode_be(uint8_t msb, uint8_t lsb) +{ + return static_cast((static_cast(msb) << 8) | lsb); +} + +// =========================================================================== +// bytes_to_uint16 +// =========================================================================== + +TEST(BytesToUint16, Zero_ProducesZero) +{ + EXPECT_EQ(translator::bytes_to_uint16(0x00, 0x00), 0u); +} + +TEST(BytesToUint16, MsbOnly_ShiftedCorrectly) +{ + EXPECT_EQ(translator::bytes_to_uint16(0x01, 0x00), 256u); +} + +TEST(BytesToUint16, LsbOnly_NotShifted) +{ + EXPECT_EQ(translator::bytes_to_uint16(0x00, 0xFF), 255u); +} + +TEST(BytesToUint16, BothBytes_BigEndianAssembly) +{ + // 0x12 << 8 | 0x34 = 0x1234 = 4660 + EXPECT_EQ(translator::bytes_to_uint16(0x12, 0x34), 0x1234u); +} + +TEST(BytesToUint16, MaxValue_ProducesFFFF) +{ + EXPECT_EQ(translator::bytes_to_uint16(0xFF, 0xFF), 0xFFFFu); +} + +// =========================================================================== +// raw_angle_to_radians +// =========================================================================== + +TEST(RawAngleToRadians, Zero_ProducesZero) +{ + EXPECT_DOUBLE_EQ(translator::raw_angle_to_radians(0), 0.0); +} + +TEST(RawAngleToRadians, HalfScale_ProducesPi) +{ + // 8192 / 16384 * 2π = π + const double expected = (8192.0 / 16384.0) * 2.0 * M_PI; + EXPECT_NEAR(translator::raw_angle_to_radians(8192), expected, 1e-10); +} + +TEST(RawAngleToRadians, FullScale14Bit_JustUnderTwoPi) +{ + // 0x3FFF = 16383 — largest valid 14-bit value + const double expected = (16383.0 / 16384.0) * 2.0 * M_PI; + EXPECT_NEAR(translator::raw_angle_to_radians(0x3FFF), expected, 1e-10); +} + +TEST(RawAngleToRadians, Bit14Set_MaskedToZero) +{ + // 0x4000 — bit 14 set, after 0x3FFF mask → 0 → 0 radians + EXPECT_DOUBLE_EQ(translator::raw_angle_to_radians(0x4000), 0.0); +} + +TEST(RawAngleToRadians, UpperBitsMasked_EquivalentToMaskedValue) +{ + // 0xFFFF masked with 0x3FFF == 0x3FFF + EXPECT_DOUBLE_EQ(translator::raw_angle_to_radians(0xFFFF), + translator::raw_angle_to_radians(0x3FFF)); +} + +TEST(RawAngleToRadians, OutputAlwaysNonNegative) +{ + // Masking ensures raw is never negative after cast + EXPECT_GE(translator::raw_angle_to_radians(0x8000), 0.0); + EXPECT_GE(translator::raw_angle_to_radians(0xFFFF), 0.0); +} + +// =========================================================================== +// encoder_frame_to_gripper_state +// =========================================================================== + +TEST(EncoderFrameToGripperState, ZeroFrame_ProducesZeroState) +{ + types::RawEncoderFrame frame{}; + auto state = translator::encoder_frame_to_gripper_state(frame); + + EXPECT_DOUBLE_EQ(state.roll, 0.0); + EXPECT_DOUBLE_EQ(state.pinch, 0.0); +} + +TEST(EncoderFrameToGripperState, RollBytesOnly_PinchRemainsZero) +{ + types::RawEncoderFrame frame{}; + frame.bytes[0] = 0x20; + frame.bytes[1] = 0x00; + + auto state = translator::encoder_frame_to_gripper_state(frame); + + EXPECT_GT(state.roll, 0.0); + EXPECT_DOUBLE_EQ(state.pinch, 0.0); +} + +TEST(EncoderFrameToGripperState, PinchBytesOnly_RollRemainsZero) +{ + types::RawEncoderFrame frame{}; + frame.bytes[2] = 0x20; + frame.bytes[3] = 0x00; + + auto state = translator::encoder_frame_to_gripper_state(frame); + + EXPECT_DOUBLE_EQ(state.roll, 0.0); + EXPECT_GT(state.pinch, 0.0); +} + +TEST(EncoderFrameToGripperState, IdenticalRawValues_ProducesIdenticalAngles) +{ + // With GEAR_RATIO_ROLL == GEAR_RATIO_PINCH == 1.0, same raw → same output + types::RawEncoderFrame frame{}; + frame.bytes[0] = 0x10; + frame.bytes[1] = 0x00; + frame.bytes[2] = 0x10; + frame.bytes[3] = 0x00; + + auto state = translator::encoder_frame_to_gripper_state(frame); + + EXPECT_DOUBLE_EQ(state.roll, state.pinch); +} + +TEST(EncoderFrameToGripperState, RollMatchesManualCalculation) +{ + types::RawEncoderFrame frame{}; + frame.bytes[0] = 0x10; + frame.bytes[1] = 0x80; + + const uint16_t raw = translator::bytes_to_uint16(0x10, 0x80); + const double expected = translator::raw_angle_to_radians(raw) * types::GEAR_RATIO_ROLL; + + auto state = translator::encoder_frame_to_gripper_state(frame); + + EXPECT_DOUBLE_EQ(state.roll, expected); +} + +TEST(EncoderFrameToGripperState, PinchMatchesManualCalculation) +{ + types::RawEncoderFrame frame{}; + frame.bytes[2] = 0x08; + frame.bytes[3] = 0x40; + + const uint16_t raw = translator::bytes_to_uint16(0x08, 0x40); + const double expected = translator::raw_angle_to_radians(raw) * types::GEAR_RATIO_PINCH; + + auto state = translator::encoder_frame_to_gripper_state(frame); + + EXPECT_DOUBLE_EQ(state.pinch, expected); +} + +TEST(EncoderFrameToGripperState, UnusedBytes_DoNotAffectOutput) +{ + types::RawEncoderFrame frame_a{}; + frame_a.bytes[0] = 0x10; + frame_a.bytes[1] = 0x00; + frame_a.bytes[2] = 0x08; + frame_a.bytes[3] = 0x00; + frame_a.bytes[4] = 0x00; + frame_a.bytes[5] = 0x00; + + types::RawEncoderFrame frame_b = frame_a; + frame_b.bytes[4] = 0xFF; + frame_b.bytes[5] = 0xFF; + + auto state_a = translator::encoder_frame_to_gripper_state(frame_a); + auto state_b = translator::encoder_frame_to_gripper_state(frame_b); + + EXPECT_DOUBLE_EQ(state_a.roll, state_b.roll); + EXPECT_DOUBLE_EQ(state_a.pinch, state_b.pinch); +} + +// =========================================================================== +// velocity_to_duty_us +// =========================================================================== + +TEST(VelocityToDutyUs, ZeroVelocity_ProducesIdlePwm) +{ + EXPECT_EQ(translator::velocity_to_duty_us(0.0, 1.0), + static_cast(types::PWM_IDLE_US)); +} + +TEST(VelocityToDutyUs, MaxPositiveVelocity_ProducesMaxPwm) +{ + EXPECT_EQ(translator::velocity_to_duty_us(1.0, 1.0), + static_cast(types::PWM_MAX_US)); +} + +TEST(VelocityToDutyUs, MaxNegativeVelocity_ProducesMinPwm) +{ + EXPECT_EQ(translator::velocity_to_duty_us(-1.0, 1.0), + static_cast(types::PWM_MIN_US)); +} + +TEST(VelocityToDutyUs, HalfPositiveVelocity_MidpointBetweenIdleAndMax) +{ + const uint16_t expected = + static_cast(types::PWM_IDLE_US + 0.5 * types::PWM_RANGE_US); + + EXPECT_EQ(translator::velocity_to_duty_us(0.5, 1.0), expected); +} + +TEST(VelocityToDutyUs, HalfNegativeVelocity_MidpointBetweenMinAndIdle) +{ + const uint16_t expected = + static_cast(types::PWM_IDLE_US - 0.5 * types::PWM_RANGE_US); + + EXPECT_EQ(translator::velocity_to_duty_us(-0.5, 1.0), expected); +} + +TEST(VelocityToDutyUs, OverspeedPositive_ClampsToMaxPwm) +{ + EXPECT_EQ(translator::velocity_to_duty_us(999.0, 1.0), + static_cast(types::PWM_MAX_US)); +} + +TEST(VelocityToDutyUs, OverspeedNegative_ClampsToMinPwm) +{ + EXPECT_EQ(translator::velocity_to_duty_us(-999.0, 1.0), + static_cast(types::PWM_MIN_US)); +} + +TEST(VelocityToDutyUs, ClampingIsSymmetricAboutIdle) +{ + const uint16_t pos = translator::velocity_to_duty_us( 999.0, 1.0); + const uint16_t neg = translator::velocity_to_duty_us(-999.0, 1.0); + + EXPECT_EQ(pos + neg, static_cast(2 * types::PWM_IDLE_US)); +} + +TEST(VelocityToDutyUs, OutputAlwaysWithinHardwareLimits) +{ + for (double v : {-10.0, -1.0, -0.5, 0.0, 0.5, 1.0, 10.0}) { + const uint16_t duty = translator::velocity_to_duty_us(v, 1.0); + EXPECT_GE(duty, static_cast(types::PWM_MIN_US)); + EXPECT_LE(duty, static_cast(types::PWM_MAX_US)); + } +} + +// =========================================================================== +// velocity_command_to_pwm_frame +// =========================================================================== + +TEST(VelocityCommandToPwmFrame, ZeroCommand_AllBytesEncodeIdlePwm) +{ + vortex_msgs::msg::GripperStateVelocityCommand cmd; + cmd.roll_velocity = 0.0; + cmd.pinch_velocity = 0.0; + + auto frame = translator::velocity_command_to_pwm_frame(cmd); + + EXPECT_EQ(decode_be(frame.bytes[0], frame.bytes[1]), + static_cast(types::PWM_IDLE_US)); + EXPECT_EQ(decode_be(frame.bytes[2], frame.bytes[3]), + static_cast(types::PWM_IDLE_US)); +} + +TEST(VelocityCommandToPwmFrame, FullPositiveRoll_RollBytesEncodeMaxPwm_PinchIdle) +{ + vortex_msgs::msg::GripperStateVelocityCommand cmd; + cmd.roll_velocity = types::MAX_ROLL_VEL; + cmd.pinch_velocity = 0.0; + + auto frame = translator::velocity_command_to_pwm_frame(cmd); + + EXPECT_EQ(decode_be(frame.bytes[0], frame.bytes[1]), + static_cast(types::PWM_MAX_US)); + EXPECT_EQ(decode_be(frame.bytes[2], frame.bytes[3]), + static_cast(types::PWM_IDLE_US)); +} + +TEST(VelocityCommandToPwmFrame, FullNegativeRoll_RollBytesEncodeMinPwm_PinchIdle) +{ + vortex_msgs::msg::GripperStateVelocityCommand cmd; + cmd.roll_velocity = -types::MAX_ROLL_VEL; + cmd.pinch_velocity = 0.0; + + auto frame = translator::velocity_command_to_pwm_frame(cmd); + + EXPECT_EQ(decode_be(frame.bytes[0], frame.bytes[1]), + static_cast(types::PWM_MIN_US)); + EXPECT_EQ(decode_be(frame.bytes[2], frame.bytes[3]), + static_cast(types::PWM_IDLE_US)); +} + +TEST(VelocityCommandToPwmFrame, FullPositivePinch_PinchBytesEncodeMaxPwm_RollIdle) +{ + vortex_msgs::msg::GripperStateVelocityCommand cmd; + cmd.roll_velocity = 0.0; + cmd.pinch_velocity = types::MAX_PINCH_VEL; + + auto frame = translator::velocity_command_to_pwm_frame(cmd); + + EXPECT_EQ(decode_be(frame.bytes[0], frame.bytes[1]), + static_cast(types::PWM_IDLE_US)); + EXPECT_EQ(decode_be(frame.bytes[2], frame.bytes[3]), + static_cast(types::PWM_MAX_US)); +} + +TEST(VelocityCommandToPwmFrame, FullNegativePinch_PinchBytesEncodeMinPwm_RollIdle) +{ + vortex_msgs::msg::GripperStateVelocityCommand cmd; + cmd.roll_velocity = 0.0; + cmd.pinch_velocity = -types::MAX_PINCH_VEL; + + auto frame = translator::velocity_command_to_pwm_frame(cmd); + + EXPECT_EQ(decode_be(frame.bytes[0], frame.bytes[1]), + static_cast(types::PWM_IDLE_US)); + EXPECT_EQ(decode_be(frame.bytes[2], frame.bytes[3]), + static_cast(types::PWM_MIN_US)); +} + +TEST(VelocityCommandToPwmFrame, AxesAreIndependent_OppositeExtremes) +{ + vortex_msgs::msg::GripperStateVelocityCommand cmd; + cmd.roll_velocity = types::MAX_ROLL_VEL; + cmd.pinch_velocity = -types::MAX_PINCH_VEL; + + auto frame = translator::velocity_command_to_pwm_frame(cmd); + + EXPECT_EQ(decode_be(frame.bytes[0], frame.bytes[1]), + static_cast(types::PWM_MAX_US)); + EXPECT_EQ(decode_be(frame.bytes[2], frame.bytes[3]), + static_cast(types::PWM_MIN_US)); +} + +TEST(VelocityCommandToPwmFrame, BigEndianPacking_MsbInLowerByteIndex) +{ + // 0.5 * MAX_ROLL_VEL → duty = 1750 µs = 0x06D6 + // bytes[0] must be 0x06, bytes[1] must be 0xD6 + vortex_msgs::msg::GripperStateVelocityCommand cmd; + cmd.roll_velocity = 0.5 * types::MAX_ROLL_VEL; + cmd.pinch_velocity = 0.0; + + auto frame = translator::velocity_command_to_pwm_frame(cmd); + const uint16_t expected = + static_cast(types::PWM_IDLE_US + 0.5 * types::PWM_RANGE_US); + + EXPECT_EQ(frame.bytes[0], static_cast((expected >> 8) & 0xFF)); + EXPECT_EQ(frame.bytes[1], static_cast( expected & 0xFF)); +} + +TEST(VelocityCommandToPwmFrame, PwmSymmetry_PositiveAndNegativeSymmetricAboutIdle) +{ + vortex_msgs::msg::GripperStateVelocityCommand pos_cmd; + pos_cmd.roll_velocity = 0.7 * types::MAX_ROLL_VEL; + pos_cmd.pinch_velocity = 0.0; + + vortex_msgs::msg::GripperStateVelocityCommand neg_cmd; + neg_cmd.roll_velocity = -0.7 * types::MAX_ROLL_VEL; + neg_cmd.pinch_velocity = 0.0; + + auto pos_frame = translator::velocity_command_to_pwm_frame(pos_cmd); + auto neg_frame = translator::velocity_command_to_pwm_frame(neg_cmd); + + const uint16_t pos_us = decode_be(pos_frame.bytes[0], pos_frame.bytes[1]); + const uint16_t neg_us = decode_be(neg_frame.bytes[0], neg_frame.bytes[1]); + + EXPECT_EQ(pos_us + neg_us, static_cast(2 * types::PWM_IDLE_US)); +} + +TEST(VelocityCommandToPwmFrame, AllOutputBytesWithinHardwarePwmRange) +{ + for (double rv : {-2.0, -1.0, 0.0, 1.0, 2.0}) { + for (double pv : {-2.0, -1.0, 0.0, 1.0, 2.0}) { + vortex_msgs::msg::GripperStateVelocityCommand cmd; + cmd.roll_velocity = rv; + cmd.pinch_velocity = pv; + + auto frame = translator::velocity_command_to_pwm_frame(cmd); + + const uint16_t roll_us = decode_be(frame.bytes[0], frame.bytes[1]); + const uint16_t pinch_us = decode_be(frame.bytes[2], frame.bytes[3]); + + EXPECT_GE(roll_us, static_cast(types::PWM_MIN_US)); + EXPECT_LE(roll_us, static_cast(types::PWM_MAX_US)); + EXPECT_GE(pinch_us, static_cast(types::PWM_MIN_US)); + EXPECT_LE(pinch_us, static_cast(types::PWM_MAX_US)); + } + } +} + +// --------------------------------------------------------------------------- +// Entry point +// --------------------------------------------------------------------------- + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/gripper_open_loop_controller/CMakeLists.txt b/gripper_open_loop_controller/CMakeLists.txt new file mode 100644 index 0000000..c975b6b --- /dev/null +++ b/gripper_open_loop_controller/CMakeLists.txt @@ -0,0 +1,77 @@ +cmake_minimum_required(VERSION 3.8) +project(gripper_open_loop_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(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_open_loop_controller_ros.cpp +) + +ament_target_dependencies(${LIB_NAME} PUBLIC + rclcpp + rclcpp_components + rclcpp_action + vortex_msgs + vortex_utils + vortex_utils_ros + spdlog + fmt +) + +# --------------------------------------------------------------------------- +# 2. Component Registration +# --------------------------------------------------------------------------- +rclcpp_components_register_node( + ${LIB_NAME} + PLUGIN "vortex::controller::GripperOpenLoopControllerNode" + EXECUTABLE ${PROJECT_NAME}_node +) + +# --------------------------------------------------------------------------- +# 3. Standalone Executable +# --------------------------------------------------------------------------- +add_executable(${PROJECT_NAME}_standalone + src/gripper_open_loop_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}/) + +ament_package() diff --git a/gripper_open_loop_controller/config/gripper_open_loop_controller_params.yaml b/gripper_open_loop_controller/config/gripper_open_loop_controller_params.yaml new file mode 100644 index 0000000..c351a5f --- /dev/null +++ b/gripper_open_loop_controller/config/gripper_open_loop_controller_params.yaml @@ -0,0 +1,13 @@ +/**: + ros__parameters: + # Publish loop period in milliseconds + time_step_ms: 10 + + # Topics + topics: + # Output: velocity command to the gripper interface node + control: "nautilus/gripper/control" + + # Action servers + action_servers: + gripper_open_loop_controller: "nautilus/gripper/open_loop_controller" diff --git a/gripper_open_loop_controller/include/gripper_open_loop_controller/gripper_open_loop_controller_ros.hpp b/gripper_open_loop_controller/include/gripper_open_loop_controller/gripper_open_loop_controller_ros.hpp new file mode 100644 index 0000000..c0e3d95 --- /dev/null +++ b/gripper_open_loop_controller/include/gripper_open_loop_controller/gripper_open_loop_controller_ros.hpp @@ -0,0 +1,70 @@ +#ifndef GRIPPER_OPEN_LOOP_CONTROLLER__GRIPPER_OPEN_LOOP_CONTROLLER_ROS_HPP_ +#define GRIPPER_OPEN_LOOP_CONTROLLER__GRIPPER_OPEN_LOOP_CONTROLLER_ROS_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vortex::controller { + +// @brief Open-loop gripper controller. Owns an action server that accepts a +// velocity command for a fixed duration and republishes that command +// on a velocity topic at a fixed rate until the duration elapses. +// Intended as a feedback-free fallback for cases where the gripper +// state is not observable. +class GripperOpenLoopControllerNode : public rclcpp::Node { + public: + explicit GripperOpenLoopControllerNode(const rclcpp::NodeOptions& options); + + ~GripperOpenLoopControllerNode(); + + private: + void set_controller_params(); + + void set_publisher(); + + void set_action_server(); + + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID& /*uuid*/, + std::shared_ptr + /*goal*/); + + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr> /*goal_handle*/); + + void handle_accepted( + const std::shared_ptr> goal_handle); + + void execute( + const std::shared_ptr> goal_handle); + + // @brief Publish a single zero-velocity command. Called after the goal + // ends so that downstream consumers do not continue actuating. + void publish_zero_velocity(); + + rclcpp_action::Server::SharedPtr + action_server_; + + rclcpp::Publisher::SharedPtr + control_pub_; + + std::chrono::milliseconds time_step_ms_{}; + + std::atomic preempted_{false}; + std::mutex execute_mutex_; + std::thread execute_thread_; +}; + +} // namespace vortex::controller + +#endif // GRIPPER_OPEN_LOOP_CONTROLLER__GRIPPER_OPEN_LOOP_CONTROLLER_ROS_HPP_ diff --git a/gripper_open_loop_controller/launch/gripper_open_loop_controller.launch.py b/gripper_open_loop_controller/launch/gripper_open_loop_controller.launch.py new file mode 100644 index 0000000..bc2fb4c --- /dev/null +++ b/gripper_open_loop_controller/launch/gripper_open_loop_controller.launch.py @@ -0,0 +1,22 @@ +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_open_loop_controller"), + "config", + "gripper_open_loop_controller_params.yaml", + ) + + gripper_open_loop_controller_node = Node( + package="gripper_open_loop_controller", + executable="gripper_open_loop_controller_standalone", + name="gripper_open_loop_controller_node", + parameters=[config_file_path], + output="screen", + ) + + return LaunchDescription([gripper_open_loop_controller_node]) diff --git a/gripper_open_loop_controller/package.xml b/gripper_open_loop_controller/package.xml new file mode 100644 index 0000000..cb4c5e2 --- /dev/null +++ b/gripper_open_loop_controller/package.xml @@ -0,0 +1,27 @@ + + + + gripper_open_loop_controller + 0.0.0 + Open-loop gripper controller: executes a velocity command for a fixed duration via an action server. Intended as a feedback-free fallback for cases where measured gripper state is unavailable. + patricsh + MIT + + ament_cmake + + rclcpp + rclcpp_action + rclcpp_components + vortex_msgs + vortex_utils + vortex_utils_ros + + spdlog + fmt + + ament_cmake_gtest + + + ament_cmake + + diff --git a/gripper_open_loop_controller/src/gripper_open_loop_controller_node.cpp b/gripper_open_loop_controller/src/gripper_open_loop_controller_node.cpp new file mode 100644 index 0000000..2b665fa --- /dev/null +++ b/gripper_open_loop_controller/src/gripper_open_loop_controller_node.cpp @@ -0,0 +1,13 @@ +#include +#include "gripper_open_loop_controller/gripper_open_loop_controller_ros.hpp" + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + + auto node = std::make_shared( + rclcpp::NodeOptions()); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/gripper_open_loop_controller/src/gripper_open_loop_controller_ros.cpp b/gripper_open_loop_controller/src/gripper_open_loop_controller_ros.cpp new file mode 100644 index 0000000..ccc68c4 --- /dev/null +++ b/gripper_open_loop_controller/src/gripper_open_loop_controller_ros.cpp @@ -0,0 +1,207 @@ +#include "gripper_open_loop_controller/gripper_open_loop_controller_ros.hpp" + +#include +#include +#include +#include +#include + +const auto start_message = R"( + + ____ _ ___ _ ____ _ _ _ +/ ___|_ __ (_)_ __ _ __ ___ _ __ / _ \ _ __ ___ _ __ | | ___ ___ _ __ / ___|___ _ __ | |_ _ __ ___ | | | ___ _ __ +| | _| '_ \| | '_ \| '_ \ / _ \ '__|| | | | '_ \ / _ \ '_ \| | / _ \ / _ \| '_ \ | | / _ \| '_ \| __| '__/ _ \| | |/ _ \ '__| +| |_| | |_) | | |_) | |_) | __/ | | |_| | |_) | __/ | | | |__| (_) | (_) | |_) | | |__| (_) | | | | |_| | | (_) | | | __/ | + \____| .__/|_| .__/| .__/ \___|_| \___/| .__/ \___|_| |_|_____\___/ \___/| .__/ \____\___/|_| |_|\__|_| \___/|_|_|\___|_| + |_| |_| |_| |_| |_| + +)"; + +namespace vortex::controller { + +GripperOpenLoopControllerNode::GripperOpenLoopControllerNode( + const rclcpp::NodeOptions& options) +: Node("gripper_open_loop_controller_node", options) { + set_controller_params(); + + set_publisher(); + + set_action_server(); + + spdlog::info(start_message); +} + +GripperOpenLoopControllerNode::~GripperOpenLoopControllerNode() { + preempted_ = true; + if (execute_thread_.joinable()) { + execute_thread_.join(); + } +} + +void GripperOpenLoopControllerNode::set_controller_params() { + const int time_step_ms_param = + this->declare_parameter("time_step_ms", 10); + time_step_ms_ = std::chrono::milliseconds(time_step_ms_param); + + spdlog::info("GripperOpenLoopController: dt={}ms", time_step_ms_param); +} + +void GripperOpenLoopControllerNode::set_publisher() { + const std::string control_topic = + this->declare_parameter("topics.control"); + + const auto qos_sensor_data = + vortex::utils::qos_profiles::sensor_data_profile(1); + + control_pub_ = + this->create_publisher( + control_topic, qos_sensor_data); +} + +void GripperOpenLoopControllerNode::set_action_server() { + const std::string action_server_name = + this->declare_parameter( + "action_servers.gripper_open_loop_controller"); + + action_server_ = rclcpp_action::create_server< + vortex_msgs::action::GripperOpenLoopCommand>( + this, action_server_name, + [this](const auto& uuid, auto goal) { + return handle_goal(uuid, std::move(goal)); + }, + [this](auto goal_handle) { return handle_cancel(goal_handle); }, + [this](auto goal_handle) { handle_accepted(goal_handle); }); +} + +rclcpp_action::GoalResponse GripperOpenLoopControllerNode::handle_goal( + const rclcpp_action::GoalUUID& /*uuid*/, + std::shared_ptr + /*goal*/) { + spdlog::info("Accepted open-loop goal request"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse GripperOpenLoopControllerNode::handle_cancel( + const std::shared_ptr> /*goal_handle*/) { + spdlog::info("Received request to cancel open-loop goal"); + return rclcpp_action::CancelResponse::ACCEPT; +} + +void GripperOpenLoopControllerNode::handle_accepted( + const std::shared_ptr> goal_handle) { + std::lock_guard lock(execute_mutex_); + preempted_ = true; + if (execute_thread_.joinable()) { + execute_thread_.join(); + } + preempted_ = false; + + execute_thread_ = + std::thread([this, goal_handle]() { execute(goal_handle); }); +} + +void GripperOpenLoopControllerNode::publish_zero_velocity() { + if (!control_pub_) { + return; + } + auto stop_message = + std::make_unique(); + stop_message->header.stamp = this->now(); + stop_message->roll_velocity = 0.0; + stop_message->pinch_velocity = 0.0; + control_pub_->publish(std::move(stop_message)); +} + +void GripperOpenLoopControllerNode::execute( + const std::shared_ptr> goal_handle) { + const auto goal = goal_handle->get_goal(); + + double requested_duration_seconds = goal->duration_seconds; + if (requested_duration_seconds <= 0.0) { + spdlog::warn( + "GripperOpenLoopController: Invalid duration_seconds received " + "(<= 0). Aborting goal."); + auto failure_result = + std::make_shared(); + failure_result->success = false; + goal_handle->abort(failure_result); + return; + } + + using GoalT = vortex_msgs::action::GripperOpenLoopCommand::Goal; + const double requested_roll_velocity = + (goal->mode == GoalT::ONLY_PINCH) ? 0.0 : goal->roll_velocity; + const double requested_pinch_velocity = + (goal->mode == GoalT::ONLY_ROLL) ? 0.0 : goal->pinch_velocity; + + spdlog::info( + "GripperOpenLoopController: roll_vel={:.3f} pinch_vel={:.3f} " + "duration={:.3f}s mode={}", + requested_roll_velocity, requested_pinch_velocity, + requested_duration_seconds, goal->mode); + + auto feedback = + std::make_shared(); + auto result = + std::make_shared(); + + rclcpp::Rate loop_rate(1000.0 / time_step_ms_.count()); + const auto start_time = this->now(); + + while (rclcpp::ok()) { + const double elapsed_seconds = + (this->now() - start_time).seconds(); + + if (preempted_.load()) { + publish_zero_velocity(); + result->success = false; + goal_handle->abort(result); + spdlog::info("Open-loop goal preempted by newer goal"); + return; + } + + if (goal_handle->is_canceling()) { + publish_zero_velocity(); + result->success = false; + goal_handle->canceled(result); + spdlog::info("Open-loop goal canceled"); + return; + } + + if (elapsed_seconds >= requested_duration_seconds) { + publish_zero_velocity(); + result->success = true; + goal_handle->succeed(result); + spdlog::info("Open-loop goal duration reached"); + return; + } + + auto velocity_command_msg = + std::make_unique(); + velocity_command_msg->header.stamp = this->now(); + velocity_command_msg->roll_velocity = requested_roll_velocity; + velocity_command_msg->pinch_velocity = requested_pinch_velocity; + control_pub_->publish(std::move(velocity_command_msg)); + + feedback->elapsed_seconds = elapsed_seconds; + goal_handle->publish_feedback(feedback); + + loop_rate.sleep(); + } + + if (!rclcpp::ok() && goal_handle->is_active()) { + result->success = false; + try { + goal_handle->abort(result); + } catch (...) { + // Ignore exceptions during shutdown + } + } +} + +} // namespace vortex::controller + +RCLCPP_COMPONENTS_REGISTER_NODE(vortex::controller::GripperOpenLoopControllerNode) diff --git a/gripper_sim_interface/gripper_sim_interface/__init__.py b/gripper_sim_interface/gripper_sim_interface/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/gripper_sim_interface/gripper_sim_interface/gripper_sim_bridge.py b/gripper_sim_interface/gripper_sim_interface/gripper_sim_bridge.py new file mode 100644 index 0000000..fc13265 --- /dev/null +++ b/gripper_sim_interface/gripper_sim_interface/gripper_sim_bridge.py @@ -0,0 +1,122 @@ +#!/usr/bin/env python3 +""" +Gripper Sim Bridge +Rewires the vortex gripper pipeline to use Stonefish/Nautilus simulator topics. + +Joystick reference (joystick_interface_auv, nautilus branch): + /nautilus/gripper_servos <- JointState + name = [arm_joint, finger_joint1, finger_joint2] + velocity = [roll, grip, grip ] + frame_id = "base_link" + +This bridge mirrors that format exactly, driven by the gripper pipeline: + /vortex/gripper/control (GripperStateVelocityCommand) + roll_dot -> arm_joint velocity (≡ rotate) + pinch_dot -> finger_joint1/2 velocity (≡ grip) + +State feedback — raw radians, no normalisation: + /nautilus/servo_state (JointState) -> /vortex/gripper/state (GripperState) + Finger avg: -0.33 rad = fully open, ~0.0 rad = fully closed. + +NOTE: Keep the AUV in AUTONOMOUS mode during gripper pipeline tests to avoid +a topic conflict — the joystick also publishes to /nautilus/gripper_servos in +MANUAL and REFERENCE modes. +""" + +import rclpy +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy +from sensor_msgs.msg import JointState +from vortex_msgs.msg import GripperState, GripperStateVelocityCommand + +BEST_EFFORT_QOS = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + depth=1, + durability=DurabilityPolicy.VOLATILE, +) + +RELIABLE_QOS = QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + depth=10, + durability=DurabilityPolicy.VOLATILE, +) + + +class GripperSimBridge(Node): + def __init__(self): + super().__init__('gripper_sim_bridge') + + self.create_subscription( + JointState, + '/nautilus/servo_state', + self.on_servo_state, + BEST_EFFORT_QOS, + ) + self.create_subscription( + GripperStateVelocityCommand, + '/vortex/gripper/control', + self.on_velocity_command, + BEST_EFFORT_QOS, + ) + + self.state_pub = self.create_publisher( + GripperState, '/vortex/gripper/state', RELIABLE_QOS + ) + self.cmd_pub = self.create_publisher( + JointState, '/nautilus/gripper_servos', BEST_EFFORT_QOS + ) + + self.get_logger().info('GripperSimBridge: online') + self.get_logger().info(' IN /nautilus/servo_state -> /vortex/gripper/state') + self.get_logger().info(' OUT /vortex/gripper/control -> /nautilus/gripper_servos') + self.get_logger().warn( + 'Keep AUV in AUTONOMOUS mode to avoid joystick conflict on /nautilus/gripper_servos' + ) + + def on_servo_state(self, msg: JointState): + try: + idx_arm = msg.name.index('nautilus/arm_joint') + idx_f1 = msg.name.index('nautilus/finger_joint1') + idx_f2 = msg.name.index('nautilus/finger_joint2') + except ValueError as e: + self.get_logger().warn(f'Joint name missing: {e}', throttle_duration_sec=5.0) + return + + state = GripperState() + state.header.stamp = self.get_clock().now().to_msg() + state.roll = msg.position[idx_arm] + state.pinch = (msg.position[idx_f1] + msg.position[idx_f2]) / 2.0 + + self.state_pub.publish(state) + + def on_velocity_command(self, msg: GripperStateVelocityCommand): + cmd = JointState() + cmd.header.stamp = self.get_clock().now().to_msg() + cmd.header.frame_id = 'base_link' + cmd.name = [ + 'nautilus/arm_joint', + 'nautilus/finger_joint1', + 'nautilus/finger_joint2', + ] + + cmd.position = [] + cmd.velocity = [msg.roll_velocity, msg.pinch_velocity, msg.pinch_velocity] + cmd.effort = [] + self.cmd_pub.publish(cmd) + +def main(): + rclpy.init() + node = GripperSimBridge() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/gripper_sim_interface/launch/gripper_sim_interface.launch.py b/gripper_sim_interface/launch/gripper_sim_interface.launch.py new file mode 100644 index 0000000..4ecf5f7 --- /dev/null +++ b/gripper_sim_interface/launch/gripper_sim_interface.launch.py @@ -0,0 +1,13 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='gripper_sim_interface', + executable='gripper_sim_bridge', + name='gripper_sim_bridge', + output='screen', + ) + ]) diff --git a/gripper_sim_interface/package.xml b/gripper_sim_interface/package.xml new file mode 100644 index 0000000..56b6f4a --- /dev/null +++ b/gripper_sim_interface/package.xml @@ -0,0 +1,22 @@ + + + + gripper_sim_interface + 0.1.0 + Sim bridge between the vortex gripper pipeline and Nautilus/Stonefish topics + patricsh + MIT + + rclpy + sensor_msgs + vortex_msgs + + ament_python + ament_copyright + ament_flake8 + ament_pep257 + + + ament_python + + diff --git a/gripper_sim_interface/resource/gripper_sim_interface b/gripper_sim_interface/resource/gripper_sim_interface new file mode 100644 index 0000000..e69de29 diff --git a/gripper_sim_interface/setup.cfg b/gripper_sim_interface/setup.cfg new file mode 100644 index 0000000..7d6b7df --- /dev/null +++ b/gripper_sim_interface/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/gripper_sim_interface +[install] +install_scripts=$base/lib/gripper_sim_interface diff --git a/gripper_sim_interface/setup.py b/gripper_sim_interface/setup.py new file mode 100644 index 0000000..bf3a0f3 --- /dev/null +++ b/gripper_sim_interface/setup.py @@ -0,0 +1,23 @@ +from setuptools import find_packages, setup +import os +from glob import glob + +package_name = 'gripper_sim_interface' + +setup( + name=package_name, + version='0.1.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.py')), + ], + install_requires=['setuptools'], + zip_safe=True, + entry_points={ + 'console_scripts': [ + 'gripper_sim_bridge = gripper_sim_interface.gripper_sim_bridge:main', + ], + }, +) diff --git a/guidance/test/test_gripper_reference_filter.cpp b/guidance/test/test_gripper_reference_filter.cpp new file mode 100644 index 0000000..a4fc23e --- /dev/null +++ b/guidance/test/test_gripper_reference_filter.cpp @@ -0,0 +1,83 @@ + #include +#include "gripper_reference_filter/gripper_reference_filter.hpp" + +using namespace vortex::guidance; + +class ReferenceFilterTest : public ::testing::Test { +protected: + void SetUp() override { + GripperReferenceFilterParams params; + params.omega << 1.0, 1.0; + params.zeta << 1.0, 1.0; + filter = std::make_unique(params); + } + + Eigen::Vector6d integrate_steps(Eigen::Vector6d x, const Eigen::Vector2d& r, int steps, double dt = 0.01) { + for (int i = 0; i < steps; ++i) { + x += dt * filter->calculate_x_dot(x, r); + } + return x; + } + + bool position_converged_to_reference(const Eigen::Vector6d& x, const Eigen::Vector2d& r, double tol = 1e-2) { + return std::abs(x(0) - r(0)) < tol && std::abs(x(1) - r(1)) < tol; + } + + bool velocity_within_bounds(const Eigen::Vector6d& x, double vel_bound = 10.0, double acc_bound = 100.0) { + return std::abs(x(2)) < vel_bound && std::abs(x(4)) < acc_bound; + } + + bool no_overshoot(const Eigen::Vector6d& x, const Eigen::Vector2d& r, double tol = 1e-6) { + return x(0) <= r(0) + tol && x(1) <= r(1) + tol; + } + + std::unique_ptr filter; +}; + +// Zero input, zero state -> zero derivative +TEST_F(ReferenceFilterTest, ZeroInputZeroState) { + Eigen::Vector6d x_dot = filter->calculate_x_dot(Eigen::Vector6d::Zero(), Eigen::Vector2d::Zero()); + EXPECT_TRUE(x_dot.isZero(1e-9)); +} + +// Step response: x_d should converge to r over time (Fossen Eq. 12.11-12.12) +TEST_F(ReferenceFilterTest, StepResponseConverges) { + Eigen::Vector2d r; + r << 1.0, 0.5; + + Eigen::Vector6d x = integrate_steps(Eigen::Vector6d::Zero(), r, 2000); + + EXPECT_TRUE(position_converged_to_reference(x, r)); +} + +// Velocity and acceleration states should stay bounded throughout transient +TEST_F(ReferenceFilterTest, VelocityAndAccelerationBounded) { + Eigen::Vector2d r; + r << 1.0, 1.0; + Eigen::Vector6d x = Eigen::Vector6d::Zero(); + + for (int i = 0; i < 1000; ++i) { + x += 0.01 * filter->calculate_x_dot(x, r); + EXPECT_TRUE(velocity_within_bounds(x)); + } +} + +// Critically damped (zeta=1) should produce no overshoot +TEST_F(ReferenceFilterTest, NoCriticallyDampedOvershoot) { + Eigen::Vector2d r; + r << 1.0, 1.0; + Eigen::Vector6d x = Eigen::Vector6d::Zero(); + + for (int i = 0; i < 2000; ++i) { + x += 0.01 * filter->calculate_x_dot(x, r); + EXPECT_TRUE(no_overshoot(x, r)); + } +} + +// namespace vortex::guidance + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +}