From 3d3399169759b7e707b99db5b6965cfb415bb870 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 30 Mar 2023 15:03:54 -0500 Subject: [PATCH 1/2] Update planning_scene.h --- .../moveit/planning_scene/planning_scene.h | 61 ++++++++++--------- 1 file changed, 33 insertions(+), 28 deletions(-) diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h index 02818427a0..9911a250d1 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -302,13 +302,14 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro /**@{*/ /** \brief Check if the current state is in collision (with the environment or self collision). - If a group name is specified, collision checking is done for that group only. + If a group name is specified, collision checking is done for that group only (plus descendent links). Since the function is non-const, the current state transforms are updated before the collision check. */ bool isStateColliding(const std::string& group = "", bool verbose = false); /** \brief Check if the current state is in collision (with the environment or self collision). If a group name is specified, - collision checking is done for that group only. It is expected the current state transforms are up to date. */ + collision checking is done for that group only (plus descendent links). It is expected the current state + transforms are up to date. */ bool isStateColliding(const std::string& group = "", bool verbose = false) const { return isStateColliding(getCurrentState(), group, verbose); @@ -316,8 +317,8 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro /** \brief Check if a given state is in collision (with the environment or self collision) If a group name is specified, - collision checking is done for that group only. The link transforms for \e state are updated before the collision - check. */ + collision checking is done for that group only (plus descendent links). The link transforms for \e state are + updated before the collision check. */ bool isStateColliding(moveit::core::RobotState& state, const std::string& group = "", bool verbose = false) const { state.updateCollisionBodyTransforms(); @@ -325,13 +326,13 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro } /** \brief Check if a given state is in collision (with the environment or self collision) - If a group name is specified, collision checking is done for that group only. It is expected that the link - transforms of \e state are up to date. */ + If a group name is specified, collision checking is done for that group only (plus descendent links). It is + expected that the link transforms of \e state are up to date. */ bool isStateColliding(const moveit::core::RobotState& state, const std::string& group = "", bool verbose = false) const; /** \brief Check if a given state is in collision (with the environment or self collision) - If a group name is specified, collision checking is done for that group only. */ + If a group name is specified, collision checking is done for that group only (plus descendent links). */ bool isStateColliding(const moveit_msgs::msg::RobotState& state, const std::string& group = "", bool verbose = false) const; @@ -522,7 +523,7 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro } /** \brief Get the names of the links that are involved in collisions for the state \e robot_state. - * Can be restricted to links part of or updated by \e group_name */ + * Can be restricted to links part of or updated by \e group_name (plus descendent links) */ void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, const moveit::core::RobotState& robot_state, const std::string& group_name = "") const { @@ -531,7 +532,7 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro /** \brief Get the names of the links that are involved in collisions for the state \e robot_state. Update the link transforms for \e robot_state if needed. - Can be restricted to links part of or updated by \e group_name */ + Can be restricted to links part of or updated by \e group_name (plus descendent links) */ void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, moveit::core::RobotState& robot_state, const std::string& group_name = "") const { @@ -542,7 +543,7 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro /** \brief Get the names of the links that are involved in collisions for the state \e robot_state given the allowed collision matrix (\e acm). Update the link transforms for \e robot_state if needed. - Can be restricted to links part of or updated by \e group_name*/ + Can be restricted to links part of or updated by \e group_name (plus descendent links) */ void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm, const std::string& group_name = "") const @@ -552,7 +553,7 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro } /** \brief Get the names of the links that are involved in collisions for the state \e robot_state given the - allowed collision matrix (\e acm). Can be restricted to links part of or updated by \e group_name */ + allowed collision matrix (\e acm). Can be restricted to links part of or updated by \e group_name (plus descendent links) */ void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts, const moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm, @@ -821,43 +822,46 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro bool isStateConstrained(const moveit::core::RobotState& state, const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const; - /** \brief Check if a given state is valid. This means checking for collisions and feasibility */ + /** \brief Check if a given state is valid. This means checking for collisions and feasibility. Includes descendent + * links of \e group. */ bool isStateValid(const moveit_msgs::msg::RobotState& state, const std::string& group = "", bool verbose = false) const; - /** \brief Check if a given state is valid. This means checking for collisions and feasibility */ + /** \brief Check if a given state is valid. This means checking for collisions and feasibility. Includes descendent + * links of \e group. */ bool isStateValid(const moveit::core::RobotState& state, const std::string& group = "", bool verbose = false) const; /** \brief Check if a given state is valid. This means checking for collisions, feasibility and whether the user - * specified validity conditions hold as well */ + * specified validity conditions hold as well. Includes descendent links of \e group. */ bool isStateValid(const moveit_msgs::msg::RobotState& state, const moveit_msgs::msg::Constraints& constr, const std::string& group = "", bool verbose = false) const; /** \brief Check if a given state is valid. This means checking for collisions, feasibility and whether the user - * specified validity conditions hold as well */ + * specified validity conditions hold as well. Includes descendent links of \e group. */ bool isStateValid(const moveit::core::RobotState& state, const moveit_msgs::msg::Constraints& constr, const std::string& group = "", bool verbose = false) const; /** \brief Check if a given state is valid. This means checking for collisions, feasibility and whether the user - * specified validity conditions hold as well */ + * specified validity conditions hold as well. Includes descendent links of \e group. */ bool isStateValid(const moveit::core::RobotState& state, const kinematic_constraints::KinematicConstraintSet& constr, const std::string& group = "", bool verbose = false) const; - /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance and feasibility) */ + /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance and feasibility). + * Includes descendent links of \e group. */ bool isPathValid(const moveit_msgs::msg::RobotState& start_state, const moveit_msgs::msg::RobotTrajectory& trajectory, const std::string& group = "", bool verbose = false, std::vector* invalid_index = nullptr) const; /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the - * passed in trajectory. */ + * passed in trajectory. Includes descendent links of \e group. */ bool isPathValid(const moveit_msgs::msg::RobotState& start_state, const moveit_msgs::msg::RobotTrajectory& trajectory, const moveit_msgs::msg::Constraints& path_constraints, const std::string& group = "", bool verbose = false, std::vector* invalid_index = nullptr) const; /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the - * passed in trajectory. */ + * passed in trajectory. Includes descendent links of \e group. */ bool isPathValid(const moveit_msgs::msg::RobotState& start_state, const moveit_msgs::msg::RobotTrajectory& trajectory, const moveit_msgs::msg::Constraints& path_constraints, const moveit_msgs::msg::Constraints& goal_constraints, const std::string& group = "", @@ -865,7 +869,7 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the - * passed in trajectory. */ + * passed in trajectory. Includes descendent links of \e group. */ bool isPathValid(const moveit_msgs::msg::RobotState& start_state, const moveit_msgs::msg::RobotTrajectory& trajectory, const moveit_msgs::msg::Constraints& path_constraints, const std::vector& goal_constraints, const std::string& group = "", @@ -873,7 +877,7 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the - * passed in trajectory. */ + * passed in trajectory. Includes descendent links of \e group. */ bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const moveit_msgs::msg::Constraints& path_constraints, const std::vector& goal_constraints, const std::string& group = "", @@ -881,19 +885,20 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the - * passed in trajectory. */ + * passed in trajectory. Includes descendent links of \e group. */ bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const moveit_msgs::msg::Constraints& path_constraints, const moveit_msgs::msg::Constraints& goal_constraints, const std::string& group = "", bool verbose = false, std::vector* invalid_index = nullptr) const; /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and - * constraint satisfaction). */ + * constraint satisfaction). Includes descendent links of \e group. */ bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const moveit_msgs::msg::Constraints& path_constraints, const std::string& group = "", bool verbose = false, std::vector* invalid_index = nullptr) const; - /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance and feasibility) */ + /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance and feasibility). + * Includes descendent links of \e group. */ bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const std::string& group = "", bool verbose = false, std::vector* invalid_index = nullptr) const; @@ -902,8 +907,8 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro void getCostSources(const robot_trajectory::RobotTrajectory& trajectory, std::size_t max_costs, std::set& costs, double overlap_fraction = 0.9) const; - /** \brief Get the top \e max_costs cost sources for a specified trajectory, but only for group \e group_name. The - * resulting costs are stored in \e costs */ + /** \brief Get the top \e max_costs cost sources for a specified trajectory, but only for group \e group_name (plus + * descendent links). The resulting costs are stored in \e costs */ void getCostSources(const robot_trajectory::RobotTrajectory& trajectory, std::size_t max_costs, const std::string& group_name, std::set& costs, double overlap_fraction = 0.9) const; @@ -912,8 +917,8 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro void getCostSources(const moveit::core::RobotState& state, std::size_t max_costs, std::set& costs) const; - /** \brief Get the top \e max_costs cost sources for a specified state, but only for group \e group_name. The - * resulting costs are stored in \e costs */ + /** \brief Get the top \e max_costs cost sources for a specified state, but only for group \e group_name (plus + * descendent links). The resulting costs are stored in \e costs */ void getCostSources(const moveit::core::RobotState& state, std::size_t max_costs, const std::string& group_name, std::set& costs) const; From 46ad2b4d10369709b838f914f35ddaba1a68e306 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 30 Mar 2023 15:15:36 -0500 Subject: [PATCH 2/2] Same in collision_common.h --- .../include/moveit/collision_detection/collision_common.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h index c553f2c136..a5098ac99d 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h @@ -197,7 +197,7 @@ struct CollisionRequest { } - /** \brief The group name to check collisions for (optional; if empty, assume the complete robot) */ + /** \brief The group name to check collisions for (optional; if empty, assume the complete robot). Descendent links are included. */ std::string group_name; /** \brief If true, compute proximity distance */ @@ -254,7 +254,9 @@ struct DistanceRequest { } - /// Compute \e active_components_only_ based on \e req_ + /*** \brief Compute \e active_components_only_ based on \e req_. This + includes links that are in the kinematic model but outside this group, if those links are descendants of + joints in this group that have their values updated. */ void enableGroup(const moveit::core::RobotModelConstPtr& robot_model) { if (robot_model->hasJointModelGroup(group_name))