From 754d9191ab791744fa2aafbf08eb04b0e4ada5a7 Mon Sep 17 00:00:00 2001 From: Nils-Christian Iseke <48475933+Nils-ChristianIseke@users.noreply.github.com> Date: Tue, 12 Dec 2023 19:54:45 +0100 Subject: [PATCH] Replaced single value joint_limit_margin with list of joint_limit_margin (#2576) * Replaced joint_limit_margin with list of margins: joint_limit_margin. Enabling setting individual margins for each joint. * Dimension comment update * Adding a dimension check within the validateParams() function of servo.cpp to give a clear error message if the size of joint_limit_margis does not match the number of joints of the move_group * Formatting fix Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> * Fix panda_simulated_config.yaml --------- Co-authored-by: AndyZe Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> --- .../config/panda_simulated_config.yaml | 2 +- .../moveit_servo/config/servo_parameters.yaml | 8 ++++---- .../moveit_servo/config/test_config_panda.yaml | 2 +- .../include/moveit_servo/utils/common.hpp | 4 ++-- moveit_ros/moveit_servo/src/servo.cpp | 14 +++++++++++++- moveit_ros/moveit_servo/src/utils/common.cpp | 6 +++--- 6 files changed, 24 insertions(+), 12 deletions(-) diff --git a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml index 9ff8ea16bc..748544e4b6 100644 --- a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml +++ b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml @@ -41,7 +41,7 @@ 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) hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this -joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. +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/ros-planning/moveit2/pull/620) ## Topic names diff --git a/moveit_ros/moveit_servo/config/servo_parameters.yaml b/moveit_ros/moveit_servo/config/servo_parameters.yaml index 56696b1742..1bd2fcef94 100644 --- a/moveit_ros/moveit_servo/config/servo_parameters.yaml +++ b/moveit_ros/moveit_servo/config/servo_parameters.yaml @@ -306,12 +306,12 @@ servo: description: "Halt all joints in cartesian mode, else halt only the joints at their limit" } - joint_limit_margin: { - type: double, - default_value: 0.1, + 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.", validation: { - gt<>: 0.0 + lower_element_bounds<>: 0.0 } } diff --git a/moveit_ros/moveit_servo/config/test_config_panda.yaml b/moveit_ros/moveit_servo/config/test_config_panda.yaml index 987af440fe..d0ad02adf5 100644 --- a/moveit_ros/moveit_servo/config/test_config_panda.yaml +++ b/moveit_ros/moveit_servo/config/test_config_panda.yaml @@ -42,7 +42,7 @@ 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) hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this -joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. +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/ros-planning/moveit2/pull/620) ## Topic names 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 c897fe7106..78904d3863 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp @@ -153,11 +153,11 @@ double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities, * @param positions The joint positions. * @param velocities The current commanded velocities. * @param joint_bounds The allowable limits for the robot joints. - * @param margin Additional buffer on the actual joint limits. + * @param margins Additional buffer on the actual joint limits. * @return The joints that are violating the specified position limits. */ std::vector jointsToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, - const moveit::core::JointBoundsVector& joint_bounds, double margin); + 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 40229ebf28..6310b83d47 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -246,6 +246,18 @@ bool Servo::validateParams(const servo::Params& servo_params) const servo_params.active_subgroup.c_str(), servo_params.move_group_name.c_str()); params_valid = false; } + if (servo_params.joint_limit_margins.size() != + robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveVariableCount()) + { + 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()); + params_valid = false; + } return params_valid; } @@ -503,7 +515,7 @@ KinematicState Servo::getNextJointState(const ServoInput& command) // 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_margin); + jointsToHalt(target_state.positions, target_state.velocities, joint_bounds, servo_params_.joint_limit_margins); // Apply halting if any joints need to be halted. if (!joints_to_halt.empty()) diff --git a/moveit_ros/moveit_servo/src/utils/common.cpp b/moveit_ros/moveit_servo/src/utils/common.cpp index ea630f4ab6..2a4a4817b0 100644 --- a/moveit_ros/moveit_servo/src/utils/common.cpp +++ b/moveit_ros/moveit_servo/src/utils/common.cpp @@ -341,7 +341,7 @@ double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities, } std::vector jointsToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, - const moveit::core::JointBoundsVector& joint_bounds, double margin) + 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++) @@ -349,8 +349,8 @@ std::vector jointsToHalt(const Eigen::VectorXd& positions, const Eigen::Vec const auto joint_bound = (joint_bounds[i])->front(); if (joint_bound.position_bounded_) { - const bool negative_bound = velocities[i] < 0 && positions[i] < (joint_bound.min_position_ + margin); - const bool positive_bound = velocities[i] > 0 && positions[i] > (joint_bound.max_position_ - margin); + 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) { joint_idxs_to_halt.push_back(i);