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..167db54c53 100644 --- a/moveit_ros/moveit_servo/src/utils/common.cpp +++ b/moveit_ros/moveit_servo/src/utils/common.cpp @@ -408,14 +408,15 @@ 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; + size_t joint_idx = 0; + size_t variable_idx = 0; for (const auto& joint_bound : joint_bounds) { for (const auto& variable_bound : *joint_bound) @@ -423,18 +424,22 @@ std::vector jointVariablesToHalt(const Eigen::VectorXd& positions, const if (variable_bound.position_bounded_) { const bool approaching_negative_bound = - velocities[idx] < 0 && positions[idx] < (variable_bound.min_position_ + margins[idx]); + velocities[variable_idx] < 0 && + positions[variable_idx] < (variable_bound.min_position_ + margins[variable_idx]); const bool approaching_positive_bound = - velocities[idx] > 0 && positions[idx] > (variable_bound.max_position_ - margins[idx]); + velocities[variable_idx] > 0 && + positions[variable_idx] > (variable_bound.max_position_ - margins[variable_idx]); if (approaching_negative_bound || approaching_positive_bound) { - variable_idxs_to_halt.push_back(idx); + joint_indices_to_halt.push_back(joint_idx); + break; // No need to add the same joint index more than once. } } - ++idx; + ++variable_idx; } + ++joint_idx; } - return variable_idxs_to_halt; + return joint_indices_to_halt; } /** \brief Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped **/