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 638aa04a8f..aa4d78df1d 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 indices that are exceeding allowable position limits in at least one variable. + * \brief Finds the joint variable indices correspond to joints exceeding allowable position limits. * @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 indices that violate the specified position limits. + * @return The joint variable indices that violate the specified position limits. */ -std::vector jointsToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, - const moveit::core::JointBoundsVector& joint_bounds, - const std::vector& margins); +std::vector jointVariablesToHalt(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 38737403ab..280baa3a8f 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_indices_to_halt, const KinematicState& current_state, - const KinematicState& target_state) const +KinematicState Servo::haltJoints(const std::vector& joint_variables_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_indices_to_halt) + for (const auto idx : joint_variables_to_halt) { halting_joint_names << bounded_state.joint_names[idx] + " "; } @@ -361,7 +361,7 @@ KinematicState Servo::haltJoints(const std::vector& joint_indices_to_hal // 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_indices_to_halt) + for (const auto idx : joint_variables_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_indices_to_halt = - jointsToHalt(target_state.positions, target_state.velocities, joint_bounds, joint_limit_margins_); + const std::vector joint_variables_to_halt = + jointVariablesToHalt(target_state.positions, target_state.velocities, joint_bounds, joint_limit_margins_); // Apply halting if any joints need to be halted. - if (!joint_indices_to_halt.empty()) + if (!joint_variables_to_halt.empty()) { servo_status_ = StatusCode::JOINT_BOUND; - target_state = haltJoints(joint_indices_to_halt, current_state, target_state); + target_state = haltJoints(joint_variables_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 167db54c53..ba63cea053 100644 --- a/moveit_ros/moveit_servo/src/utils/common.cpp +++ b/moveit_ros/moveit_servo/src/utils/common.cpp @@ -408,19 +408,21 @@ double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities, return min_scaling_factor; } -std::vector jointsToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, - const moveit::core::JointBoundsVector& joint_bounds, - const std::vector& margins) +std::vector jointVariablesToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, + const moveit::core::JointBoundsVector& joint_bounds, + const std::vector& margins) { - std::vector joint_indices_to_halt; + std::vector variable_indices_to_halt; // Now get the scaling factor from joint velocity limits. size_t joint_idx = 0; size_t variable_idx = 0; for (const auto& joint_bound : joint_bounds) { + bool halt_joint = false; for (const auto& variable_bound : *joint_bound) { + // First, loop through all the variables to see if if (variable_bound.position_bounded_) { const bool approaching_negative_bound = @@ -431,15 +433,24 @@ std::vector jointsToHalt(const Eigen::VectorXd& positions, const Eigen:: positions[variable_idx] > (variable_bound.max_position_ - margins[variable_idx]); if (approaching_negative_bound || approaching_positive_bound) { - joint_indices_to_halt.push_back(joint_idx); - break; // No need to add the same joint index more than once. + halt_joint |= true; } } ++variable_idx; + + // If the joint needs to be halted, add all variable indices corresponding to that joint. + if (halt_joint) + { + for (size_t k = variable_idx - joint_bound->size(); k < variable_idx; ++k) + { + variable_indices_to_halt.push_back(k); + } + } } + ++joint_idx; } - return joint_indices_to_halt; + return variable_indices_to_halt; } /** \brief Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped **/