Document how collision checking includes descendent links (#2058)

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))

moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h

@@ -302,36 +302,37 @@ 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);
   }
 
   /** \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();
     return isStateColliding(static_cast<const moveit::core::RobotState&>(state), group, verbose);
   }
 
   /** \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,79 +822,83 @@ 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<std::size_t>* 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<std::size_t>* 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 = "",
                    bool verbose = false, std::vector<std::size_t>* 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::vector<moveit_msgs::msg::Constraints>& goal_constraints, const std::string& group = "",
                    bool verbose = false, std::vector<std::size_t>* 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 robot_trajectory::RobotTrajectory& trajectory,
                    const moveit_msgs::msg::Constraints& path_constraints,
                    const std::vector<moveit_msgs::msg::Constraints>& goal_constraints, const std::string& group = "",
                    bool verbose = false, std::vector<std::size_t>* 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 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<std::size_t>* 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<std::size_t>* 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<std::size_t>* 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<collision_detection::CostSource>& 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<collision_detection::CostSource>& 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<collision_detection::CostSource>& 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<collision_detection::CostSource>& costs) const;