From a693e3e455f2d23e7b2df40dbbc12aa51c8cad95 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Thu, 15 Aug 2024 02:45:36 -0400 Subject: [PATCH] [Servo] Support single-element joint limit margins vector and fix joint halting logic for multi-DOF Joints (#2970) --- .../config/panda_simulated_config.yaml | 9 ++- .../moveit_servo/config/servo_parameters.yaml | 6 +- .../config/test_config_panda.yaml | 9 ++- .../include/moveit_servo/servo.hpp | 7 ++- .../include/moveit_servo/utils/common.hpp | 9 +-- moveit_ros/moveit_servo/src/servo.cpp | 59 +++++++++---------- moveit_ros/moveit_servo/src/utils/common.cpp | 44 ++++++++++---- 7 files changed, 86 insertions(+), 57 deletions(-) diff --git a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml index de8376e466..980370ca18 100644 --- a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml +++ b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml @@ -38,13 +38,16 @@ is_primary_planning_scene_monitor: true check_octomap_collisions: false # Check collision against the octomap (if a 3D sensor plugin is available) ## MoveIt properties -move_group_name: panda_arm # Often 'manipulator' or 'arm' +move_group_name: panda_arm # Often 'manipulator' or 'arm' ## Configure handling of singularities and joint limits -lower_singularity_threshold: 10.0 # Start decelerating when the condition number hits this (close to singularity) +lower_singularity_threshold: 10.0 # Start decelerating when the condition number hits this (close to singularity) hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this -joint_limit_margins: [0.12, 0.12, 0.12, 0.12, 0.12, 0.12, 0.12] # added as a buffer to joint limits [radians or meters]. If moving quickly, make this larger. leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/moveit/moveit2/pull/620) +# Added as a buffer to joint variable position bounds [in that joint variable's respective units]. +# Can be of size 1, which applies the margin to all joints, or the same size as the number of degrees of freedom of the active joint group. +# If moving quickly, make these values larger. +joint_limit_margins: [0.12] ## Topic names cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands diff --git a/moveit_ros/moveit_servo/config/servo_parameters.yaml b/moveit_ros/moveit_servo/config/servo_parameters.yaml index ddffaf0c97..497252714c 100644 --- a/moveit_ros/moveit_servo/config/servo_parameters.yaml +++ b/moveit_ros/moveit_servo/config/servo_parameters.yaml @@ -326,8 +326,8 @@ servo: joint_limit_margins: { type: double_array, - default_value: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1], - description: "Added as a buffer to joint limits [radians]. If moving quickly, make this larger.", + default_value: [0.1], + description: "Added as a buffer to joint variable position bounds [in that joint variable's respective units]. Can be of size 1, which applies the margin to all joints, or the same size as the number of degrees of freedom of the active joint group. If moving quickly, make these values larger.", validation: { lower_element_bounds<>: 0.0 } @@ -336,7 +336,7 @@ servo: override_velocity_scaling_factor: { type: double, default_value: 0.0, - description: "Scaling factor when servo overrides the velocity (eg: near singularities)", + description: "Scaling factor when servo overrides the velocity (e.g, near singularities)", validation: { bounds<>: [0.0, 1.0] } diff --git a/moveit_ros/moveit_servo/config/test_config_panda.yaml b/moveit_ros/moveit_servo/config/test_config_panda.yaml index 5324ce9bf1..34e735875a 100644 --- a/moveit_ros/moveit_servo/config/test_config_panda.yaml +++ b/moveit_ros/moveit_servo/config/test_config_panda.yaml @@ -37,13 +37,16 @@ smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" is_primary_planning_scene_monitor: true ## MoveIt properties -move_group_name: panda_arm # Often 'manipulator' or 'arm' +move_group_name: panda_arm # Often 'manipulator' or 'arm' ## Configure handling of singularities and joint limits -lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) +lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this -joint_limit_margins: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] # added as a buffer to joint limits [radians or meters]. If moving quickly, make this larger. leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/moveit/moveit2/pull/620) +# Added as a buffer to joint variable position bounds [in that joint variable's respective units]. +# Can be of size 1, which applies the margin to all joints, or the same size as the number of degrees of freedom of the active joint group. +# If moving quickly, make these values larger. +joint_limit_margins: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] ## Topic names cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp index 9d4f836a72..d946bdc833 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp @@ -189,7 +189,7 @@ class Servo * @param servo_params The servo parameters * @return True if parameters are valid, else False */ - bool validateParams(const servo::Params& servo_params) const; + bool validateParams(const servo::Params& servo_params); /** * \brief Updates the servo parameters and performs validations. @@ -208,7 +208,7 @@ class Servo * @param target_state The target kinematic state. * @return The bounded kinematic state. */ - KinematicState haltJoints(const std::vector& joints_to_halt, const KinematicState& current_state, + KinematicState haltJoints(const std::vector& joints_to_halt, const KinematicState& current_state, const KinematicState& target_state) const; // Variables @@ -232,6 +232,9 @@ class Servo // Map between joint subgroup names and corresponding joint name - move group indices map std::unordered_map joint_name_to_index_maps_; + + // The current joint limit safety margins for each active joint position variable. + std::vector joint_limit_margins_; }; } // namespace moveit_servo 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 5fd5091dbd..060a113f56 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp @@ -166,15 +166,16 @@ double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities, const moveit::core::JointBoundsVector& joint_bounds, double scaling_override); /** - * \brief Finds the joints that are exceeding allowable position limits. + * \brief Finds the joint variable indices corresponding 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 joints that are violating 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 8cae5bf842..329e4a84a8 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -182,7 +182,7 @@ void Servo::setCollisionChecking(const bool check_collision) check_collision ? collision_monitor_->start() : collision_monitor_->stop(); } -bool Servo::validateParams(const servo::Params& servo_params) const +bool Servo::validateParams(const servo::Params& servo_params) { bool params_valid = true; auto robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState(); @@ -247,30 +247,27 @@ bool Servo::validateParams(const servo::Params& servo_params) const << servo_params.move_group_name << "'" << check_yaml_string); params_valid = false; } - if (servo_params.joint_limit_margins.size() != - robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveVariableCount()) - { - RCLCPP_ERROR_STREAM( - logger_, - "The parameter 'joint_limit_margins' must have the same number of elements as the number of joints in the " - "move group. The size of 'joint_limit_margins' is '" - << servo_params.joint_limit_margins.size() << "' but the number of joints of the move group '" - << servo_params.move_group_name << "' is '" - << robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveVariableCount() << "'" - << check_yaml_string); - params_valid = false; + const auto num_dofs = robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveVariableCount(); + if (servo_params.joint_limit_margins.size() == 1u) + { + joint_limit_margins_.clear(); + for (size_t idx = 0; idx < num_dofs; ++idx) + { + joint_limit_margins_.push_back(servo_params.joint_limit_margins[0]); + } } - if (servo_params.joint_limit_margins.size() != - robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveVariableCount()) + else if (servo_params.joint_limit_margins.size() == num_dofs) { - RCLCPP_ERROR(logger_, - "Parameter 'joint_limit_margins' must have the same number of elements as the number of joints in the " - "move_group. " - "Size of 'joint_limit_margins' is '%li', but number of joints in '%s' is '%i'. " - "Check the parameters YAML file used to launch this node.", - servo_params.joint_limit_margins.size(), servo_params.move_group_name.c_str(), - robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveVariableCount()); + joint_limit_margins_ = servo_params.joint_limit_margins; + } + else + { + RCLCPP_ERROR_STREAM( + logger_, "The parameter 'joint_limit_margins' must have either a single element or the same number of " + "elements as the degrees of freedom in the active joint group. The size of 'joint_limit_margins' is '" + << servo_params.joint_limit_margins.size() << "' but the number of degrees of freedom in group '" + << servo_params.move_group_name << "' is '" << num_dofs << "'." << check_yaml_string); params_valid = false; } @@ -337,14 +334,14 @@ void Servo::setCommandType(const CommandType& command_type) expected_command_type_ = command_type; } -KinematicState Servo::haltJoints(const std::vector& joints_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 int idx : joints_to_halt) + for (const auto idx : joint_variables_to_halt) { halting_joint_names << bounded_state.joint_names[idx] + " "; } @@ -364,7 +361,7 @@ KinematicState Servo::haltJoints(const std::vector& joints_to_halt, const K // Halt only the joints that are out of bounds bounded_state.positions = target_state.positions; bounded_state.velocities = target_state.velocities; - for (const int idx : joints_to_halt) + for (const auto idx : joint_variables_to_halt) { bounded_state.positions[idx] = current_state.positions[idx]; bounded_state.velocities[idx] = 0.0; @@ -533,15 +530,15 @@ KinematicState Servo::getNextJointState(const moveit::core::RobotStatePtr& robot // Compute velocities based on smoothed joint positions 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 joints_to_halt = - jointsToHalt(target_state.positions, target_state.velocities, joint_bounds, servo_params_.joint_limit_margins); + // 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_); // Apply halting if any joints need to be halted. - if (!joints_to_halt.empty()) + if (!joint_variables_to_halt.empty()) { servo_status_ = StatusCode::JOINT_BOUND; - target_state = haltJoints(joints_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 f7ee5701e2..7ce13c1805 100644 --- a/moveit_ros/moveit_servo/src/utils/common.cpp +++ b/moveit_ros/moveit_servo/src/utils/common.cpp @@ -408,24 +408,46 @@ 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_idxs_to_halt; - for (size_t i = 0; i < joint_bounds.size(); i++) + std::vector variable_indices_to_halt; + + // Now get the scaling factor from joint velocity limits. + size_t variable_idx = 0; + for (const auto& joint_bound : joint_bounds) { - const auto joint_bound = (joint_bounds[i])->front(); - if (joint_bound.position_bounded_) + bool halt_joint = false; + for (const auto& variable_bound : *joint_bound) { - const bool negative_bound = velocities[i] < 0 && positions[i] < (joint_bound.min_position_ + margins[i]); - const bool positive_bound = velocities[i] > 0 && positions[i] > (joint_bound.max_position_ - margins[i]); - if (negative_bound || positive_bound) + // First, loop through all the joint variables to see if the entire joint should be halted. + if (variable_bound.position_bounded_) + { + const bool approaching_negative_bound = + velocities[variable_idx] < 0 && + positions[variable_idx] < (variable_bound.min_position_ + margins[variable_idx]); + const bool approaching_positive_bound = + velocities[variable_idx] > 0 && + positions[variable_idx] > (variable_bound.max_position_ - margins[variable_idx]); + if (approaching_negative_bound || approaching_positive_bound) + { + halt_joint |= true; + } + } + ++variable_idx; + + // If the joint needs to be halted, add all variable indices corresponding to that joint. + if (halt_joint) { - joint_idxs_to_halt.push_back(i); + for (size_t k = variable_idx - joint_bound->size(); k < variable_idx; ++k) + { + variable_indices_to_halt.push_back(k); + } } } } - return joint_idxs_to_halt; + return variable_indices_to_halt; } /** \brief Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped **/