diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index aede22d0ee..d08d0facbc 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -36,6 +36,7 @@ generate_parameter_library(joint_trajectory_controller_parameters add_library(joint_trajectory_controller SHARED src/joint_trajectory_controller.cpp + src/tolerances.cpp src/trajectory.cpp src/validate_jtc_parameters.cpp ) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index f34c94cf08..121768d19c 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -28,20 +28,26 @@ // POSSIBILITY OF SUCH DAMAGE. /// \author Adolfo Rodriguez Tsouroukdissian +/// \author Suryansh Singh (thedevmystic) #ifndef JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ #define JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ +#include +#include #include #include #include #include +#include "angles/angles.h" #include "control_msgs/action/follow_joint_trajectory.hpp" #include "joint_trajectory_controller/joint_trajectory_controller_parameters.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" namespace joint_trajectory_controller { + /** * \brief Trajectory state tolerances for position, velocity and acceleration variables. * @@ -55,19 +61,42 @@ struct StateTolerances }; /** - * \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,7 +104,7 @@ 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: @@ -88,250 +117,260 @@ struct SegmentTolerances * goal: 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[in] jtc_logger The logger to use for output. + * \param[in] 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[in] default_value The pre-configured or default tolerance value. + * \param[in] 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[in] jtc_logger The logger to use for output and debugging messages. + * \param[in] 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[in] goal The new `FollowJointTrajectory` action goal containing optional tolerance + * overrides in `path_tolerance` and `goal_tolerance` fields. + * \param[in] 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) -{ - SegmentTolerances active_tolerances(default_tolerances); - static auto logger = jtc_logger.get_child("tolerance"); + const std::vector & joints); - 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); +/** + * \brief Calculates the error point (desired - actual) for a trajectory point. + * + * \param[in] desired_state The commanded state point. + * \param[in] current_state The actual state point from the hardware. + * \param[in] is_wraparounds A vector indicating which joints are wraparound (e.g., continuous). + * \param[in] show_errors If true, logging messages about size mismatches will be shown. + * \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, + const std::vector & is_wraparounds, bool show_errors = false); + +/** + * \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, and + * acceleration. 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[in] state_error The difference (error) between the actual and desired joint state. + * \param[in] joint_idx The index of the joint in the `state_error` message to be checked. + * \param[in] state_tolerance The tolerance structure defining the maximum allowed absolute errors. + * \param[in] 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[in] state_error Error point (difference between commanded and actual) for all joints. + * \param[in] segment_tolerances StateTolerances for all joints in the trajectory. + * \param[in] 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 **/ - // State and goal state tolerances - for (auto joint_tol : goal.path_tolerance) +inline trajectory_msgs::msg::JointTrajectoryPoint create_error_trajectory_point( + const trajectory_msgs::msg::JointTrajectoryPoint & desired_state, + const trajectory_msgs::msg::JointTrajectoryPoint & current_state, + const std::vector & is_wraparounds, bool show_errors) +{ + trajectory_msgs::msg::JointTrajectoryPoint error_state; + const size_t n_joints = desired_state.positions.size(); + + // Check if vectors are same size before proceeding to prevent undefined behavior + if (current_state.positions.size() != n_joints) { - 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) + if (show_errors) { + const auto logger = rclcpp::get_logger("tolerances"); 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; + "Current state positions size (%zu) does not match desired state positions size (%zu).", + current_state.positions.size(), n_joints); } - - 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); + return error_state; // Return empty error state } - for (auto goal_tol : goal.goal_tolerance) + + // Check if wraparounds size matches before proceeding + if (is_wraparounds.size() != n_joints) { - 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()) + if (show_errors) { + const auto logger = rclcpp::get_logger("tolerances"); RCLCPP_ERROR( - logger, "%s", - ("joint '" + joint + - "' specified in goal.goal_tolerance does not exist. " - "Using default tolerances.") - .c_str()); - return default_tolerances; + logger, "Wraparounds size (%zu) does not match desired state positions size (%zu).", + is_wraparounds.size(), n_joints); } - auto i = static_cast(std::distance(joints.cbegin(), it)); - std::string interface = ""; - try + return error_state; // Return empty error state + } + + // Position Error + error_state.positions.resize(n_joints); + for (size_t i = 0; i < n_joints; ++i) + { + if (is_wraparounds[i]) { - 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); + // Use shortest distance for wraparound joints (e.g., continuous joints) + // Normalized between: [-pi, pi] + error_state.positions[i] = + angles::shortest_angular_distance(current_state.positions[i], desired_state.positions[i]); } - catch (const std::runtime_error & e) + else { - 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; + // Standard Euclidean distance + error_state.positions[i] = desired_state.positions[i] - current_state.positions[i]; } + } + + // Helper lambda for element-wise subtraction (desired - actual) + auto subtract = [](double desired, double actual) { return desired - actual; }; + + // Velocity Error (Only if both are same size) + if (desired_state.velocities.size() == n_joints && current_state.velocities.size() == n_joints) + { + 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 same size) + if ( + desired_state.accelerations.size() == n_joints && + current_state.accelerations.size() == n_joints) + { + 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; + + // Check joint index validity + if (joint_idx >= state_error.positions.size()) + { + if (show_errors) + { + const auto logger = rclcpp::get_logger("tolerances"); + RCLCPP_ERROR( + logger, "Joint index %zu is out of bounds for positions of size %zu.", joint_idx, + state_error.positions.size()); + } + return false; // Invalid joint index + } + + // Helper lambda to check if an index is valid for a vector + auto check_index = [joint_idx](const std::vector & vec) + { return joint_idx < vec.size(); }; + + // 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]; + check_index(state_error.velocities) ? state_error.velocities[joint_idx] : 0.0; const double error_acceleration = - state_error.accelerations.empty() ? 0.0 : state_error.accelerations[joint_idx]; + check_index(state_error.accelerations) ? state_error.accelerations[joint_idx] : 0.0; + // 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 +379,41 @@ 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/tolerances.cpp b/joint_trajectory_controller/src/tolerances.cpp new file mode 100644 index 0000000000..d09794494d --- /dev/null +++ b/joint_trajectory_controller/src/tolerances.cpp @@ -0,0 +1,237 @@ +// Copyright 2013 PAL Robotics S.L. +// All rights reserved. +// +// 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 the 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 HOLDER 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) +{ + constexpr double ERASE_VALUE = -1.0; + constexpr double EPSILON = std::numeric_limits::epsilon(); + + // Helper lambda to check for values + auto is_erase_value = [&](double value) { return std::fabs(value - ERASE_VALUE) < EPSILON; }; + + if (goal_value > 0.0) + { + // Valid +ve tolerance, return by value + return goal_value; + } + else if (is_erase_value(goal_value)) + { + // Valid -ve tolerance (-1), erase value to 0 + return 0.0; + } + else if (goal_value < 0.0) + { + // All other -ve tolerances are invalid. + throw std::runtime_error("Illegal tolerance value."); + } + // Return default value if don't pass any check + 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("tolerance"); + RCLCPP_DEBUG(logger, "Goal Time: %f", constraints.goal_time); + + // State and goal 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]; + auto const & joint_constraints = constraints.joints_map.at(joint); + + // Tolerances + tolerances.state_tolerance[i].position = joint_constraints.trajectory; + tolerances.goal_state_tolerance[i].position = joint_constraints.goal; + tolerances.goal_state_tolerance[i].velocity = constraints.stopped_velocity_tolerance; + + // Debug Logging + RCLCPP_DEBUG(logger, "--- Tolerances for Joint: %s ---", joint.c_str()); + RCLCPP_DEBUG(logger, "Trajectory Position: %f", tolerances.state_tolerance[i].position); + RCLCPP_DEBUG(logger, "Goal Position: %f", tolerances.goal_state_tolerance[i].position); + RCLCPP_DEBUG(logger, "Goal Velocity: %f", tolerances.goal_state_tolerance[i].velocity); + RCLCPP_DEBUG(logger, "---------------------------"); + } + + 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) +{ + static auto logger = jtc_logger.get_child("tolerance"); + SegmentTolerances active_tolerances(default_tolerances); + + // Create a map to look up joint 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_time_tolerance + try + { + double goal_time_sec = rclcpp::Duration(goal.goal_time_tolerance).seconds(); + active_tolerances.goal_time_tolerance = + resolve_tolerance_source(default_tolerances.goal_time_tolerance, goal_time_sec); + } + catch (const std::runtime_error & e) + { + RCLCPP_WARN( + logger, "Illegal goal time tolerance specified: %f. Using default tolerances.", + rclcpp::Duration(goal.goal_time_tolerance).seconds()); + return default_tolerances; + } + + // 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_WARN( + logger, "Path tolerance specified for unknown joint '%s'. Using default tolerances.", + joint_tol.name.c_str()); + return default_tolerances; + } + size_t i = it->second; + std::string interface = ""; // For error tracking + + try + { + // Position + interface = "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 & e) + { + RCLCPP_WARN( + logger, + "Illegal %s path tolerance value specified for joint '%s'. Using default tolerances.", + interface.c_str(), joint_tol.name.c_str()); + return default_tolerances; + } + } + + // Process goal.goal_tolerance (Goal/Endpoint Constraints) + for (const auto & joint_tol : goal.goal_tolerance) + { + auto it = joint_to_id.find(joint_tol.name); + if (it == joint_to_id.end()) + { + RCLCPP_WARN( + logger, "Goal tolerance specified for unknown joint '%s'. Using default tolerances.", + joint_tol.name.c_str()); + return default_tolerances; + } + size_t i = it->second; + std::string interface = ""; // For error tracking + + try + { + // Position + interface = "position"; + active_tolerances.goal_state_tolerance[i].position = resolve_tolerance_source( + default_tolerances.goal_state_tolerance[i].position, joint_tol.position); + + // Velocity + interface = "velocity"; + active_tolerances.goal_state_tolerance[i].velocity = resolve_tolerance_source( + default_tolerances.goal_state_tolerance[i].velocity, joint_tol.velocity); + + // Acceleration + interface = "acceleration"; + active_tolerances.goal_state_tolerance[i].acceleration = resolve_tolerance_source( + default_tolerances.goal_state_tolerance[i].acceleration, joint_tol.acceleration); + } + catch (const std::runtime_error & e) + { + RCLCPP_WARN( + logger, + "Illegal %s goal tolerance value specified for joint '%s'. Using default tolerances.", + interface.c_str(), joint_tol.name.c_str()); + return default_tolerances; + } + } + + // Debug Logging + RCLCPP_DEBUG(logger, "---- Active Tolerances ----"); + RCLCPP_DEBUG(logger, "Goal Time: %f", active_tolerances.goal_time_tolerance); + for (size_t i = 0; i < joints.size(); ++i) + { + RCLCPP_DEBUG(logger, "--- Tolerances for Joint: %s ---", joints[i].c_str()); + RCLCPP_DEBUG(logger, "Trajectory Position: %f", active_tolerances.state_tolerance[i].position); + RCLCPP_DEBUG(logger, "Trajectory Velocity: %f", active_tolerances.state_tolerance[i].velocity); + RCLCPP_DEBUG( + logger, "Trajectory Acceleration: %f", active_tolerances.state_tolerance[i].acceleration); + RCLCPP_DEBUG(logger, "Goal Position: %f", active_tolerances.goal_state_tolerance[i].position); + RCLCPP_DEBUG(logger, "Goal Velocity: %f", active_tolerances.goal_state_tolerance[i].velocity); + RCLCPP_DEBUG( + logger, "Goal Acceleration: %f", active_tolerances.goal_state_tolerance[i].acceleration); + } + RCLCPP_DEBUG(logger, "---------------------------"); + + return active_tolerances; +} + +} // namespace joint_trajectory_controller diff --git a/joint_trajectory_controller/test/test_tolerances.cpp b/joint_trajectory_controller/test/test_tolerances.cpp index 106a9a761f..75c615256a 100644 --- a/joint_trajectory_controller/test/test_tolerances.cpp +++ b/joint_trajectory_controller/test/test_tolerances.cpp @@ -52,6 +52,7 @@ class TestTolerancesFixture : public ::testing::Test SegmentTolerances default_tolerances; joint_trajectory_controller::Params params; std::vector joint_names_; + std::vector is_wraparounds_; rclcpp::Logger logger = rclcpp::get_logger("TestTolerancesFixture"); void SetUp() override @@ -59,6 +60,9 @@ class TestTolerancesFixture : public ::testing::Test // Initialize joint_names_ with some test data joint_names_ = {"joint1", "joint2", "joint3"}; + // No wraparounds + is_wraparounds_ = {false, false, false}; + // Initialize default_tolerances and params with common setup for all tests // TODO(anyone) fill params and use // SegmentTolerances get_segment_tolerances(Params const & params) instead @@ -438,3 +442,86 @@ TEST_F(TestTolerancesFixture, test_invalid_joints_goal_tolerance) logger, default_tolerances, goal_msg, params.joints); expectDefaultTolerances(active_tolerances); } + +// Create Error Point Tests +TEST_F(TestTolerancesFixture, test_create_error_trajectory_point) +{ + trajectory_msgs::msg::JointTrajectoryPoint desired; + trajectory_msgs::msg::JointTrajectoryPoint actual; + + // Joint 0: Standard, Joint 1: Wraparound (Continuous), Joint 2: Standard + is_wraparounds_ = {false, true, false}; + + // Setup desired state + // Joint 1 is desired at -3.14 (near -PI) + desired.positions = {1.0, -3.14, 3.0}; + desired.velocities = {0.1, 0.2, 0.3}; + desired.accelerations = {0.01, 0.02, 0.03}; + + // Setup actual state + // Joint 1 is actually at 3.14 (near +PI). Shortest distance should be near 0. + actual.positions = {0.9, 3.14, 3.0}; + actual.velocities = {0.05, 0.25, 0.3}; + actual.accelerations = {0.0, 0.03, 0.03}; + + // Calculate error: Error = Desired - Actual + auto error_point = joint_trajectory_controller::create_error_trajectory_point( + desired, actual, is_wraparounds_, false); + + // Verify Position Errors + ASSERT_EQ(error_point.positions.size(), 3); + EXPECT_NEAR(error_point.positions[0], 0.1, 1e-6); // Standard: 1.0 - 0.9 + // Wraparound: shortest distance between 3.14 and -3.14 is very small + EXPECT_NEAR(error_point.positions[1], 0.0, 1e-2); + EXPECT_NEAR(error_point.positions[2], 0.0, 1e-6); + + // Verify Velocity Errors (Standard subtraction regardless of wraparound) + ASSERT_EQ(error_point.velocities.size(), 3); + EXPECT_NEAR(error_point.velocities[0], 0.05, 1e-6); + EXPECT_NEAR(error_point.velocities[1], -0.05, 1e-6); + EXPECT_NEAR(error_point.velocities[2], 0.0, 1e-6); + + // Verify Acceleration Errors + ASSERT_EQ(error_point.accelerations.size(), 3); + EXPECT_NEAR(error_point.accelerations[0], 0.01, 1e-6); + EXPECT_NEAR(error_point.accelerations[1], -0.01, 1e-6); + EXPECT_NEAR(error_point.accelerations[2], 0.0, 1e-6); +} + +TEST_F(TestTolerancesFixture, test_create_error_trajectory_point_mismatched_sizes) +{ + { + SCOPED_TRACE("mismatched positions size"); + trajectory_msgs::msg::JointTrajectoryPoint desired; + trajectory_msgs::msg::JointTrajectoryPoint actual; + std::vector wraps = {false, false, false}; + + desired.positions = {1.0, 2.0, 3.0}; + actual.positions = {1.0, 2.0}; + + // Should return empty because positions size (3) != (2) + auto error_point = + joint_trajectory_controller::create_error_trajectory_point(desired, actual, wraps, false); + + EXPECT_TRUE(error_point.positions.empty()); + EXPECT_TRUE(error_point.velocities.empty()); + EXPECT_TRUE(error_point.accelerations.empty()); + } + { + SCOPED_TRACE("mismatched wraps size"); + trajectory_msgs::msg::JointTrajectoryPoint desired; + trajectory_msgs::msg::JointTrajectoryPoint actual; + std::vector mismatch_wraps = {false}; + + desired.positions = {1.0, 2.0}; + actual.positions = {1.0, 2.0}; + + // Should return empty because mismatch_wraps size (1) != joints positions size (2) + auto error_point = joint_trajectory_controller::create_error_trajectory_point( + desired, actual, mismatch_wraps, false); + + EXPECT_TRUE(error_point.positions.empty()); + EXPECT_TRUE(error_point.velocities.empty()); + EXPECT_TRUE(error_point.accelerations.empty()); + } +}