From 16efe91908b63224d36f812adff5f5a3307f16c7 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Mon, 12 Aug 2024 20:57:14 -0400 Subject: [PATCH] We actually do want to halt joint indices, not variable indices --- .../include/moveit_servo/utils/common.hpp | 10 +++++----- moveit_ros/moveit_servo/src/servo.cpp | 16 ++++++++-------- moveit_ros/moveit_servo/src/utils/common.cpp | 15 ++++++++------- 3 files changed, 21 insertions(+), 20 deletions(-) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp b/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp index 96e25269b0..638aa04a8f 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp @@ -166,16 +166,16 @@ double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities, const moveit::core::JointBoundsVector& joint_bounds, double scaling_override); /** - * \brief Finds the joint variables that are exceeding allowable position limits. + * \brief Finds the joint indices that are exceeding allowable position limits in at least one variable. * @param positions The joint positions. * @param velocities The current commanded velocities. * @param joint_bounds The allowable limits for the robot joints. * @param margins Additional buffer on the actual joint limits. - * @return The joint variables that are violating the specified position limits. + * @return The joint indices that violate the specified position limits. */ -std::vector jointVariablesToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, - const moveit::core::JointBoundsVector& joint_bounds, - const std::vector& margins); +std::vector jointsToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, + const moveit::core::JointBoundsVector& joint_bounds, + const std::vector& margins); /** * \brief Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped. diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index 280baa3a8f..38737403ab 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -334,14 +334,14 @@ void Servo::setCommandType(const CommandType& command_type) expected_command_type_ = command_type; } -KinematicState Servo::haltJoints(const std::vector& joint_variables_to_halt, - const KinematicState& current_state, const KinematicState& target_state) const +KinematicState Servo::haltJoints(const std::vector& joint_indices_to_halt, const KinematicState& current_state, + const KinematicState& target_state) const { KinematicState bounded_state(target_state.joint_names.size()); bounded_state.joint_names = target_state.joint_names; std::stringstream halting_joint_names; - for (const auto idx : joint_variables_to_halt) + for (const auto idx : joint_indices_to_halt) { halting_joint_names << bounded_state.joint_names[idx] + " "; } @@ -361,7 +361,7 @@ KinematicState Servo::haltJoints(const std::vector& joint_variables_to_h // Halt only the joints that are out of bounds bounded_state.positions = target_state.positions; bounded_state.velocities = target_state.velocities; - for (const auto idx : joint_variables_to_halt) + for (const auto idx : joint_indices_to_halt) { bounded_state.positions[idx] = current_state.positions[idx]; bounded_state.velocities[idx] = 0.0; @@ -534,14 +534,14 @@ KinematicState Servo::getNextJointState(const moveit::core::RobotStatePtr& robot target_state.velocities = (target_state.positions - current_state.positions) / servo_params_.publish_period; // Check if any joints are going past joint position limits. - const std::vector joint_variables_to_halt = - jointVariablesToHalt(target_state.positions, target_state.velocities, joint_bounds, joint_limit_margins_); + const std::vector joint_indices_to_halt = + jointsToHalt(target_state.positions, target_state.velocities, joint_bounds, joint_limit_margins_); // Apply halting if any joints need to be halted. - if (!joint_variables_to_halt.empty()) + if (!joint_indices_to_halt.empty()) { servo_status_ = StatusCode::JOINT_BOUND; - target_state = haltJoints(joint_variables_to_halt, current_state, target_state); + target_state = haltJoints(joint_indices_to_halt, current_state, target_state); } } diff --git a/moveit_ros/moveit_servo/src/utils/common.cpp b/moveit_ros/moveit_servo/src/utils/common.cpp index 5e56c52bdc..7e1828c31e 100644 --- a/moveit_ros/moveit_servo/src/utils/common.cpp +++ b/moveit_ros/moveit_servo/src/utils/common.cpp @@ -408,11 +408,11 @@ double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities, return min_scaling_factor; } -std::vector jointVariablesToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, - const moveit::core::JointBoundsVector& joint_bounds, - const std::vector& margins) +std::vector jointsToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, + const moveit::core::JointBoundsVector& joint_bounds, + const std::vector& margins) { - std::vector variable_idxs_to_halt; + std::vector joint_indices_to_halt; // Now get the scaling factor from joint velocity limits. size_t idx = 0; @@ -428,13 +428,14 @@ std::vector jointVariablesToHalt(const Eigen::VectorXd& positions, const velocities[idx] > 0 && positions[idx] > (variable_bound.max_position_ - margins[idx]); if (approaching_negative_bound || approaching_positive_bound) { - variable_idxs_to_halt.push_back(idx); + joint_indices_to_halt.push_back(idx); + break; // No need to add the same joint index more than once. } } - ++idx; } + ++idx; } - return variable_idxs_to_halt; + return joint_indices_to_halt; } /** \brief Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped **/