From ecff43d2f32d4d31a14485c11e52d4a7d25ba5ad Mon Sep 17 00:00:00 2001 From: Vedant87 Date: Sun, 11 May 2025 06:07:09 +0000 Subject: [PATCH] JTC add more options for tolerance configuration --- .../joint_trajectory_controller.hpp | 7 +++++++ .../src/joint_trajectory_controller.cpp | 4 ++++ .../src/joint_trajectory_controller_parameters.yaml | 10 ++++++++++ .../test/test_joint_trajectory_controller_params.yaml | 4 ++++ 4 files changed, 25 insertions(+) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index ad4abb6335..da94c945c5 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -81,6 +81,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa controller_interface::CallbackReturn on_error( const rclcpp_lifecycle::State & previous_state) override; + // New methods to handle velocity and acceleration tolerances + void setVelocityTolerance(double tolerance); + void setAccelerationTolerance(double tolerance); + protected: // To reduce number of variables and to make the code shorter the interfaces are ordered in types // as the following constants @@ -290,6 +294,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa joint_interface[index].get().set_value(trajectory_point_interface[map_cmd_to_joints_[index]]); } } + + double velocity_tolerance_; + double acceleration_tolerance_; }; } // namespace joint_trajectory_controller diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 6890cea55e..3f7c4bb492 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -55,6 +55,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() // Create the parameter listener and get the parameters param_listener_ = std::make_shared(get_node()); params_ = param_listener_->get_params(); + // Read new velocity and acceleration tolerances + velocity_tolerance_ = get_node()->get_parameter("constraints.velocity_tolerance").as_double(); + acceleration_tolerance_ = + get_node()->get_parameter("constraints.acceleration_tolerance").as_double(); } catch (const std::exception & e) { diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 926366cde6..1b828c9b5e 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -166,3 +166,13 @@ joint_trajectory_controller: default_value: 0.0, description: "Per-joint trajectory offset tolerance at the goal position.", } + velocity_tolerance: { + type: double, + default_value: 0.0, + description: "Velocity tolerance for trajectory segments.", + } + acceleration_tolerance: { + type: double, + default_value: 0.0, + description: "Acceleration tolerance for trajectory segments.", + } diff --git a/joint_trajectory_controller/test/test_joint_trajectory_controller_params.yaml b/joint_trajectory_controller/test/test_joint_trajectory_controller_params.yaml index 356f82d685..a5a2c321d3 100644 --- a/joint_trajectory_controller/test/test_joint_trajectory_controller_params.yaml +++ b/joint_trajectory_controller/test/test_joint_trajectory_controller_params.yaml @@ -10,3 +10,7 @@ test_joint_trajectory_controller: state_interfaces: - position + + constraints: + velocity_tolerance: 0.05 + acceleration_tolerance: 0.1