diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 5efdd52886..b9c90b7e4f 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -37,6 +37,7 @@ generate_parameter_library(joint_trajectory_controller_parameters add_library(joint_trajectory_controller SHARED src/joint_trajectory_controller.cpp src/trajectory.cpp + src/tolerances.cpp ) target_compile_features(joint_trajectory_controller PUBLIC cxx_std_17) if(WIN32) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index f34c94cf08..f2def01546 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -28,6 +28,7 @@ // POSSIBILITY OF SUCH DAMAGE. /// \author Adolfo Rodriguez Tsouroukdissian +/// \author Suryansh Singh (thedevmystic) #ifndef JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ #define JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ @@ -42,32 +43,67 @@ namespace joint_trajectory_controller { + /** - * \brief Trajectory state tolerances for position, velocity and acceleration variables. + * \brief Trajectory state tolerances for joint state components. * - * A tolerance value of zero means that no tolerance will be applied for that variable. + * A tolerance value of zero means that no tolerance will be applied for that component. */ struct StateTolerances { + /** + * \brief Position component (Base). + */ double position = 0.0; + + /** + * \brief Velocity component (1st Derivative). + */ double velocity = 0.0; + + /** + * \brief Acceleration component (2nd Derivative). + */ double acceleration = 0.0; }; /** - * \brief Trajectory segment tolerances. + * \brief Trajectory segment tolerances for validation during execution and at the endpoint. + * + * This structure defines the allowable deviation from the planned trajectory + * during execution (state_tolerance) and the final required accuracy at the + * segment's endpoint (goal_state_tolerance). */ struct SegmentTolerances { explicit SegmentTolerances(size_t size = 0) : state_tolerance(size), goal_state_tolerance(size) {} - /** State tolerances that apply during segment execution. */ + /** + * \brief State tolerances applied throughout the segment's execution. + * + * If the actual joint state deviates from the desired setpoint by more than + * this tolerance at any time during the segment, the trajectory execution + * may be aborted. This vector contains one entry for each joint. + */ std::vector state_tolerance; - /** State tolerances that apply for the goal state only.*/ + /** + * \brief State tolerances applied only to the final goal state. + * + * These are the tolerances the joint state must meet at the segment's + * end time (or within the goal_time_tolerance window) for the segment + * to be considered successfully completed. This vector contains one entry + * for each joint. + */ std::vector goal_state_tolerance; - /** Extra time after the segment end time allowed to reach the goal state tolerances. */ + /** + * \brief Extra time allowed to reach the goal state tolerances. + * + * This defines a time window (in seconds) after the segment's + * planned end time, during which the goal_state_tolerance must be met. + * This allows the robot to "settle" into the final target position. + */ double goal_time_tolerance = 0.0; }; @@ -75,263 +111,227 @@ struct SegmentTolerances * \brief Populate trajectory segment tolerances using data from the ROS node. * * It is assumed that the following parameter structure is followed on the provided LifecycleNode. - * Unspecified parameters will take the defaults shown in the comments: + * Unspecified parameters will take the defaults shown below: * * \code * constraints: - * goal_time: 1.0 # Defaults to zero - * stopped_velocity_tolerance: 0.02 # Defaults to 0.01 - * foo_joint: - * trajectory: 0.05 # Defaults to zero (ie. the tolerance is not enforced) - * goal: 0.03 # Defaults to zero (ie. the tolerance is not enforced) - * bar_joint: - * goal: 0.01 + * goal_time: 1.0 # Defaults to 0.0 + * stopped_velocity_tolerance: 0.02 # Defaults to 0.01 (Applied to goal state velocity for ALL joints) + * foo_joint: # Joint-specific tolerances + * trajectory: + * position: 0.05 # Defaults to 0.0 (Enforced during execution) + * velocity: 0.1 + * acceleration: 0.2 + * goal: + * position: 0.03 # Defaults to 0.0 (Enforced at final goal state) + * velocity: 0.01 # Note: 'stopped_velocity_tolerance' overrides this if higher. + * acceleration: 0.4 + * bar_joint: + * goal: + * position: 0.01 * \endcode * - * \param jtc_logger The logger to use for output - * \param params The ROS Parameters - * \return Trajectory segment tolerances. + * The `stopped_velocity_tolerance` is applied as the minimum velocity tolerance + * for the goal state of *all* joints, overriding individual `joint.goal.velocity` + * settings if the global value is higher. + * + * \param jtc_logger The logger to use for output. + * \param params The ROS Parameters structure containing joint lists and constraints. + * + * \return Trajectory segment tolerances populated from parameters. */ -SegmentTolerances get_segment_tolerances(rclcpp::Logger & jtc_logger, const Params & params) -{ - auto const & constraints = params.constraints; - auto const n_joints = params.joints.size(); - - SegmentTolerances tolerances; - tolerances.goal_time_tolerance = constraints.goal_time; - static auto logger = jtc_logger.get_child("tolerance"); - RCLCPP_DEBUG(logger, "goal_time %f", constraints.goal_time); - - // State and goal state tolerances - tolerances.state_tolerance.resize(n_joints); - tolerances.goal_state_tolerance.resize(n_joints); - for (size_t i = 0; i < n_joints; ++i) - { - auto const joint = params.joints[i]; - tolerances.state_tolerance[i].position = constraints.joints_map.at(joint).trajectory; - tolerances.goal_state_tolerance[i].position = constraints.joints_map.at(joint).goal; - tolerances.goal_state_tolerance[i].velocity = constraints.stopped_velocity_tolerance; - - RCLCPP_DEBUG( - logger, "%s %f", (joint + ".trajectory.position").c_str(), - tolerances.state_tolerance[i].position); - RCLCPP_DEBUG( - logger, "%s %f", (joint + ".goal.position").c_str(), - tolerances.goal_state_tolerance[i].position); - RCLCPP_DEBUG( - logger, "%s %f", (joint + ".goal.velocity").c_str(), - tolerances.goal_state_tolerance[i].velocity); - } +SegmentTolerances get_segment_tolerances(rclcpp::Logger & jtc_logger, const Params & params); - return tolerances; -} - -double resolve_tolerance_source(const double default_value, const double goal_value) -{ - // from - // https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointTolerance.msg - // There are two special values for tolerances: - // * 0 - The tolerance is unspecified and will remain at whatever the default is - // * -1 - The tolerance is "erased". - // If there was a default, the joint will be allowed to move without restriction. - constexpr double ERASE_VALUE = -1.0; - auto is_erase_value = [=](double value) - { return fabs(value - ERASE_VALUE) < std::numeric_limits::epsilon(); }; - - if (goal_value > 0.0) - { - return goal_value; - } - else if (is_erase_value(goal_value)) - { - return 0.0; - } - else if (goal_value < 0.0) - { - throw std::runtime_error("Illegal tolerance value."); - } - return default_value; -} +/** + * \brief Resolves the final effective tolerance value based on provided goal tolerance. + * + * This function applies logic based on conventions of ROS joint tolerance messages + * (like control_msgs/msg/JointTolerance.msg) where special negative values are used + * to modify or clear the default tolerance. + * + * The logic applied is: + * 1. Positive value: The goal tolerance is explicitly used. + * 2. ERASE_VALUE (-1.0): The tolerance is cleared/erased, returning 0.0, which means the + * tolerance check is disabled or the joint is unrestricted. + * 3. Zero (0.0): The goal tolerance is unspecified, and the default value is returned. + * 4. Other negative values (except ERASE_VALUE): Illegal input, throws an error. + * + * \param default_value The pre-configured or default tolerance value. + * \param goal_value The tolerance value specified in the goal message. + * + * \return The resolved tolerance value to be used for validation. + * \throws std::runtime_error If an illegal negative tolerance value is provided. + */ +double resolve_tolerance_source(const double default_value, const double goal_value); /** * \brief Populate trajectory segment tolerances using data from an action goal. * - * \param jtc_logger The logger to use for output - * \param default_tolerances The default tolerances to use if the action goal does not specify any. - * \param goal The new action goal - * \param joints The joints configured by ROS parameters - * \return Trajectory segment tolerances. + * This function creates a new `SegmentTolerances` object by starting with a copy of + * the `default_tolerances` and then overriding specific values with tolerances + * provided in the `FollowJointTrajectory` action goal. + * + * The `resolve_tolerance_source` function is used to correctly handle special + * tolerance values (0.0 for default, -1.0 for erase). If any provided joint name + * is invalid or a tolerance value is illegal, the function immediately returns the + * unmodified `default_tolerances`. + * + * \param jtc_logger The logger to use for output and debugging messages. + * \param default_tolerances The base tolerances configured via ROS parameters, used if + * the action goal does not specify any, or if the goal is invalid. + * \param goal The new `FollowJointTrajectory` action goal containing optional tolerance overrides + * in `path_tolerance` and `goal_tolerance` fields. + * \param joints The list of joints configured for the controller (used for index mapping). + * + * \return The resolved `SegmentTolerances` object, prioritizing goal values over defaults. */ SegmentTolerances get_segment_tolerances( rclcpp::Logger & jtc_logger, const SegmentTolerances & default_tolerances, const control_msgs::action::FollowJointTrajectory::Goal & goal, - const std::vector & joints) + const std::vector & joints); + +/** + * \brief Calculates the error point (desired - actual) for a trajectory point. + * + * \param desired_state The commanded state point. + * \param current_state The actual state point from the hardware. + * + * \return A JointTrajectoryPoint where positions, velocities, etc., are the difference. + */ +trajectory_msgs::msg::JointTrajectoryPoint create_error_trajectory_point( + const trajectory_msgs::msg::JointTrajectoryPoint & desired_state, + const trajectory_msgs::msg::JointTrajectoryPoint & current_state); + +/** + * \brief Checks if the error for a single joint state component is within the defined tolerance. + * + * This function is used to validate the state error (e.g., actual minus desired) for a + * specific joint against its maximum allowable deviations (tolerance) in position, velocity, + * acceleration, and effort. A tolerance value of 0.0 means that the corresponding check is skipped. + * The check is successful only if the absolute error is less than or equal to the tolerance + * for all components that have a positive tolerance set. + * + * \param state_error The difference (error) between the actual and desired joint state. + * \param joint_idx The index of the joint in the `state_error` message to be checked. + * \param state_tolerance The tolerance structure defining the maximum allowed absolute errors. + * \param show_errors If true, logging messages about the tolerance violation will be shown. + * + * **WARNING: Logging is not real-time safe.** + * + * \return True if the absolute error for all checked components (where tolerance > 0.0) + * is less than or equal to the respective tolerance; False otherwise. + */ +bool check_state_tolerance_per_joint( + const trajectory_msgs::msg::JointTrajectoryPoint & state_error, size_t joint_idx, + const StateTolerances & state_tolerance, bool show_errors = false); + +/** + * \brief Checks if the entire trajectory point error is within the segment tolerances. + * + * This function iterates over all joints and calls check_state_tolerance_per_joint(). + * + * \param state_error Error point (difference between commanded and actual) for all joints. + * \param segment_tolerances StateTolerances for all joints in the trajectory. + * \param show_errors If the joint that violates its tolerance should be output to console (NON-REALTIME if true). + * + * \return True if ALL joints are within their defined tolerances, false otherwise. + */ +bool check_trajectory_point_tolerance( + const trajectory_msgs::msg::JointTrajectoryPoint & state_error, + const std::vector & segment_tolerances, + bool show_errors = false); + + +/** Inline Functions Implementations **/ + +inline trajectory_msgs::msg::JointTrajectoryPoint create_error_trajectory_point( + const trajectory_msgs::msg::JointTrajectoryPoint & desired_state, + const trajectory_msgs::msg::JointTrajectoryPoint & current_state) { - SegmentTolerances active_tolerances(default_tolerances); - static auto logger = jtc_logger.get_child("tolerance"); + trajectory_msgs::msg::JointTrajectoryPoint error_state; + const size_t n_joints = desired_state.positions.size(); - try - { - active_tolerances.goal_time_tolerance = resolve_tolerance_source( - default_tolerances.goal_time_tolerance, rclcpp::Duration(goal.goal_time_tolerance).seconds()); - } - catch (const std::runtime_error & e) - { - RCLCPP_ERROR( - logger, "Specified illegal goal_time_tolerance: %f. Using default tolerances", - rclcpp::Duration(goal.goal_time_tolerance).seconds()); - return default_tolerances; - } - RCLCPP_DEBUG(logger, "%s %f", "goal_time", active_tolerances.goal_time_tolerance); + // Helper lambda for element-wise subtraction (desired - actual) + auto subtract = [](double desired, double actual) { return desired - actual; }; - // State and goal state tolerances - for (auto joint_tol : goal.path_tolerance) - { - auto const joint = joint_tol.name; - // map joint names from goal to active_tolerances - auto it = std::find(joints.begin(), joints.end(), joint); - if (it == joints.end()) - { - RCLCPP_ERROR( - logger, "%s", - ("joint '" + joint + - "' specified in goal.path_tolerance does not exist. " - "Using default tolerances.") - .c_str()); - return default_tolerances; - } - auto i = static_cast(std::distance(joints.cbegin(), it)); - std::string interface = ""; - try - { - interface = "position"; - active_tolerances.state_tolerance[i].position = resolve_tolerance_source( - default_tolerances.state_tolerance[i].position, joint_tol.position); - interface = "velocity"; - active_tolerances.state_tolerance[i].velocity = resolve_tolerance_source( - default_tolerances.state_tolerance[i].velocity, joint_tol.velocity); - interface = "acceleration"; - active_tolerances.state_tolerance[i].acceleration = resolve_tolerance_source( - default_tolerances.state_tolerance[i].acceleration, joint_tol.acceleration); - } - catch (const std::runtime_error & e) - { - RCLCPP_ERROR( - logger, - "joint '%s' specified in goal.path_tolerance has a invalid %s tolerance. Using default " - "tolerances.", - joint.c_str(), interface.c_str()); - return default_tolerances; - } + // Position Error + error_state.positions.resize(n_joints); + std::transform( + desired_state.positions.begin(), desired_state.positions.end(), + current_state.positions.begin(), error_state.positions.begin(), + subtract); - RCLCPP_DEBUG( - logger, "%s %f", (joint + ".state_tolerance.position").c_str(), - active_tolerances.state_tolerance[i].position); - RCLCPP_DEBUG( - logger, "%s %f", (joint + ".state_tolerance.velocity").c_str(), - active_tolerances.state_tolerance[i].velocity); - RCLCPP_DEBUG( - logger, "%s %f", (joint + ".state_tolerance.acceleration").c_str(), - active_tolerances.state_tolerance[i].acceleration); - } - for (auto goal_tol : goal.goal_tolerance) + // Velocity Error (Only if both are available) + if (!desired_state.velocities.empty() && !current_state.velocities.empty()) { - auto const joint = goal_tol.name; - // map joint names from goal to active_tolerances - auto it = std::find(joints.begin(), joints.end(), joint); - if (it == joints.end()) - { - RCLCPP_ERROR( - logger, "%s", - ("joint '" + joint + - "' specified in goal.goal_tolerance does not exist. " - "Using default tolerances.") - .c_str()); - return default_tolerances; - } - auto i = static_cast(std::distance(joints.cbegin(), it)); - std::string interface = ""; - try - { - interface = "position"; - active_tolerances.goal_state_tolerance[i].position = resolve_tolerance_source( - default_tolerances.goal_state_tolerance[i].position, goal_tol.position); - interface = "velocity"; - active_tolerances.goal_state_tolerance[i].velocity = resolve_tolerance_source( - default_tolerances.goal_state_tolerance[i].velocity, goal_tol.velocity); - interface = "acceleration"; - active_tolerances.goal_state_tolerance[i].acceleration = resolve_tolerance_source( - default_tolerances.goal_state_tolerance[i].acceleration, goal_tol.acceleration); - } - catch (const std::runtime_error & e) - { - RCLCPP_ERROR( - logger, - "joint '%s' specified in goal.goal_tolerance has a invalid %s tolerance. Using default " - "tolerances.", - joint.c_str(), interface.c_str()); - return default_tolerances; - } + error_state.velocities.resize(n_joints); + std::transform( + desired_state.velocities.begin(), desired_state.velocities.end(), + current_state.velocities.begin(), error_state.velocities.begin(), + subtract); + } - RCLCPP_DEBUG( - logger, "%s %f", (joint + ".goal_state_tolerance.position").c_str(), - active_tolerances.goal_state_tolerance[i].position); - RCLCPP_DEBUG( - logger, "%s %f", (joint + ".goal_state_tolerance.velocity").c_str(), - active_tolerances.goal_state_tolerance[i].velocity); - RCLCPP_DEBUG( - logger, "%s %f", (joint + ".goal_state_tolerance.acceleration").c_str(), - active_tolerances.goal_state_tolerance[i].acceleration); + // Acceleration Error (Only if both are available) + if (!desired_state.accelerations.empty() && !current_state.accelerations.empty()) + { + error_state.accelerations.resize(n_joints); + std::transform( + desired_state.accelerations.begin(), desired_state.accelerations.end(), + current_state.accelerations.begin(), error_state.accelerations.begin(), + subtract); } - return active_tolerances; + return error_state; } -/** - * \param state_error State error to check. - * \param joint_idx Joint index for the state error - * \param state_tolerance State tolerance of joint to check \p state_error against. - * \param show_errors If the joint that violate its tolerance should be output to console. NOT - * REALTIME if true \return True if \p state_error fulfills \p state_tolerance. - */ inline bool check_state_tolerance_per_joint( const trajectory_msgs::msg::JointTrajectoryPoint & state_error, size_t joint_idx, - const StateTolerances & state_tolerance, bool show_errors = false) + const StateTolerances & state_tolerance, bool show_errors) { using std::abs; + + // Extract joint state components from state_error const double error_position = state_error.positions[joint_idx]; const double error_velocity = state_error.velocities.empty() ? 0.0 : state_error.velocities[joint_idx]; const double error_acceleration = state_error.accelerations.empty() ? 0.0 : state_error.accelerations[joint_idx]; + // Check if the components are valid const bool is_valid = !(state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) && !(state_tolerance.velocity > 0.0 && abs(error_velocity) > state_tolerance.velocity) && !(state_tolerance.acceleration > 0.0 && abs(error_acceleration) > state_tolerance.acceleration); + // If valid, then return true if (is_valid) { return true; } + // Otherwise, if logging errors [NON REALTIME] if (show_errors) { const auto logger = rclcpp::get_logger("tolerances"); RCLCPP_ERROR(logger, "State tolerances failed for joint %zu:", joint_idx); + // Position Error if (state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) { RCLCPP_ERROR( logger, "Position Error: %f, Position Tolerance: %f", error_position, state_tolerance.position); } + + // Velocity Error if (state_tolerance.velocity > 0.0 && abs(error_velocity) > state_tolerance.velocity) { RCLCPP_ERROR( logger, "Velocity Error: %f, Velocity Tolerance: %f", error_velocity, state_tolerance.velocity); } + + // Acceleration Error if ( state_tolerance.acceleration > 0.0 && abs(error_acceleration) > state_tolerance.acceleration) { @@ -340,9 +340,43 @@ inline bool check_state_tolerance_per_joint( state_tolerance.acceleration); } } + + // Return false return false; } +inline bool check_trajectory_point_tolerance( + const trajectory_msgs::msg::JointTrajectoryPoint & state_error, + const std::vector & segment_tolerances, + bool show_errors) +{ + // Check that the error vector size matches the tolerance vector size + if (state_error.positions.size() != segment_tolerances.size()) + { + if (show_errors) + { + const auto logger = rclcpp::get_logger("tolerances"); + RCLCPP_ERROR( + logger, + "Error point joint size (%zu) does not match tolerance joint size (%zu).", + state_error.positions.size(), segment_tolerances.size()); + } + return false; // Cannot perform a valid check + } + + for (size_t i = 0; i < segment_tolerances.size(); ++i) + { + // The check_state_tolerance_per_joint function handles checking for available + // interfaces (position, velocity, and acceleration). + if (!check_state_tolerance_per_joint(state_error, i, segment_tolerances[i], show_errors)) + { + return false; // Found a joint that failed its tolerance + } + } + + return true; // All joints passed the tolerance check +} + } // namespace joint_trajectory_controller #endif // JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 5e7947d15d..a365b8d591 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -216,13 +216,43 @@ joint_trajectory_controller: } } __map_joints: - trajectory: { - type: double, - default_value: 0.0, - description: "Per-joint trajectory offset tolerance during movement.", - } - goal: { - type: double, - default_value: 0.0, - description: "Per-joint trajectory offset tolerance at the goal position.", - } + trajectory: + one_of<>: + - type: double + - type: struct + fields: + position: { + type: double, + default_value: 0.0, + description: "Position tolerance for the trajectory execution." + } + velocity: { + type: double, + default_value: 0.0, + description: "Velocity tolerance for the trajectory execution." + } + acceleration: { + type: double, + default_value: 0.0, + description: "Acceleration tolerance for the trajectory execution." + } + goal: + one_of<>: + - type: double + - type: struct + fields: + position: { + type: double, + default_value: 0.0, + description: "Position tolerance for the final goal state." + } + velocity: { + type: double, + default_value: 0.0, + description: "Velocity tolerance for the final goal state." + } + acceleration: { + type: double, + default_value: 0.0, + description: "Acceleration tolerance for the final goal state." + } diff --git a/joint_trajectory_controller/src/tolerances.cpp b/joint_trajectory_controller/src/tolerances.cpp new file mode 100644 index 0000000000..f1b77cbac2 --- /dev/null +++ b/joint_trajectory_controller/src/tolerances.cpp @@ -0,0 +1,226 @@ +// Copyright 2013 PAL Robotics S.L. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics S.L. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +/// \author Adolfo Rodriguez Tsouroukdissian +/// \author Suryansh Singh (thedevmystic) + +#include "joint_trajectory_controller/tolerances.hpp" + +#include +#include +#include +#include +#include +#include + +namespace joint_trajectory_controller +{ + +double resolve_tolerance_source(const double default_value, const double goal_value) +{ + // Erase value is -1.0 + const double ERASE_VALUE = -1.0; + // Epsilon for comparision + const double EPSILON = std::numeric_limits::epsilon(); + + if (goal_value > 0.0) + { + return goal_value; + } + else if (std::abs(goal_value - ERASE_VALUE) < EPSILON) + { + return 0.0; + } + else if (goal_value < 0.0) + { + throw std::runtime_error("Illegal tolerance value"); + } + + // goal_value is 0.0, use default + return default_value; +} + +SegmentTolerances get_segment_tolerances(rclcpp::Logger & jtc_logger, const Params & params) +{ + auto const & constraints = params.constraints; + auto const n_joints = params.joints.size(); + + SegmentTolerances tolerances; + tolerances.goal_time_tolerance = constraints.goal_time; + static auto logger = jtc_logger.get_child("param_tolerance_loader"); + RCLCPP_DEBUG(logger, "goal_time %f", constraints.goal_time); + + tolerances.state_tolerance.resize(n_joints); + tolerances.goal_state_tolerance.resize(n_joints); + + for (size_t i = 0; i < n_joints; ++i) + { + auto const joint = params.joints[i]; + auto const & joint_constraints = constraints.joints_map.at(joint); + + // Path/Trajectory Tolerances (state_tolerance) + tolerances.state_tolerance[i].position = joint_constraints.trajectory.position; + tolerances.state_tolerance[i].velocity = joint_constraints.trajectory.velocity; + tolerances.state_tolerance[i].acceleration = joint_constraints.trajectory.acceleration; + + // Goal Tolerances (goal_state_tolerance) + tolerances.goal_state_tolerance[i].position = joint_constraints.goal.position; + + // Velocity tolerance must be at least the global stopped_velocity_tolerance + tolerances.goal_state_tolerance[i].velocity = std::max( + joint_constraints.goal.velocity, + constraints.stopped_velocity_tolerance + ); + + tolerances.goal_state_tolerance[i].acceleration = joint_constraints.goal.acceleration; + + // Debug Logging + RCLCPP_DEBUG(logger, "--- Tolerances for Joint: %s ---", joint.c_str()); + RCLCPP_DEBUG(logger, "Path Pos/Vel/Acc: %f / %f / %f", + tolerances.state_tolerance[i].position, + tolerances.state_tolerance[i].velocity, + tolerances.state_tolerance[i].acceleration); + RCLCPP_DEBUG(logger, "Goal Pos/Vel/Acc: %f / %f / %f", + tolerances.goal_state_tolerance[i].position, + tolerances.goal_state_tolerance[i].velocity, + tolerances.goal_state_tolerance[i].acceleration); + } + + return tolerances; +} + +SegmentTolerances get_segment_tolerances( + rclcpp::Logger & jtc_logger, const SegmentTolerances & default_tolerances, + const control_msgs::action::FollowJointTrajectory::Goal & goal, + const std::vector & joints) +{ + SegmentTolerances active_tolerances = default_tolerances; + static auto logger = jtc_logger.get_child("goal_tolerance_loader"); + + // Create a map to look up joint index by name for fast access + std::map joint_to_id; + for (size_t i = 0; i < joints.size(); ++i) + { + joint_to_id[joints[i]] = i; + } + + // Process goal.path_tolerance (Execution Constraints) + for (const auto & joint_tol : goal.path_tolerance) + { + auto it = joint_to_id.find(joint_tol.name); + if (it == joint_to_id.end()) + { + RCLCPP_ERROR( + logger, + "Path tolerance specified for unknown joint '%s'. Using default tolerances.", + joint_tol.name.c_str()); + return default_tolerances; + } + const size_t i = it->second; + std::string interface = "position"; + + try + { + // Position + active_tolerances.state_tolerance[i].position = resolve_tolerance_source( + default_tolerances.state_tolerance[i].position, joint_tol.position); + + // Velocity + interface = "velocity"; + active_tolerances.state_tolerance[i].velocity = resolve_tolerance_source( + default_tolerances.state_tolerance[i].velocity, joint_tol.velocity); + + // Acceleration + interface = "acceleration"; + active_tolerances.state_tolerance[i].acceleration = resolve_tolerance_source( + default_tolerances.state_tolerance[i].acceleration, joint_tol.acceleration); + } + catch (const std::runtime_error & ex) + { + RCLCPP_ERROR( + logger, + "Illegal path tolerance value for joint '%s' and interface '%s': %s. Using default tolerances.", + joint_tol.name.c_str(), interface.c_str(), ex.what()); + return default_tolerances; + } + } + + // Process goal.goal_tolerance (Endpoint Constraints) + for (const auto & goal_tol : goal.goal_tolerance) + { + auto it = joint_to_id.find(goal_tol.name); + if (it == joint_to_id.end()) + { + RCLCPP_ERROR( + logger, + "Goal tolerance specified for unknown joint '%s'. Using default tolerances.", + goal_tol.name.c_str()); + return default_tolerances; + } + const size_t i = it->second; + std::string interface = "position"; + + try + { + // Position + active_tolerances.goal_state_tolerance[i].position = resolve_tolerance_source( + default_tolerances.goal_state_tolerance[i].position, goal_tol.position); + + // Velocity + interface = "velocity"; + active_tolerances.goal_state_tolerance[i].velocity = resolve_tolerance_source( + default_tolerances.goal_state_tolerance[i].velocity, goal_tol.velocity); + + // Acceleration + interface = "acceleration"; + active_tolerances.goal_state_tolerance[i].acceleration = resolve_tolerance_source( + default_tolerances.goal_state_tolerance[i].acceleration, goal_tol.acceleration); + } + catch (const std::runtime_error & ex) + { + RCLCPP_ERROR( + logger, + "Illegal goal tolerance value for joint '%s' and interface '%s': %s. Using default tolerances.", + goal_tol.name.c_str(), interface.c_str(), ex.what()); + return default_tolerances; + } + } + + // Process goal_time_tolerance + if (goal.goal_time_tolerance.sec != 0 || goal.goal_time_tolerance.nanosec != 0) + { + const double goal_time_sec = + static_cast(goal.goal_time_tolerance.sec) + + static_cast(goal.goal_time_tolerance.nanosec) / 1e9; + + active_tolerances.goal_time_tolerance = std::max(0.0, goal_time_sec); + } + + return active_tolerances; +} + +} // namespace joint_trajectory_controller