Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Document how collision checking includes descendent links #2058

Merged
merged 2 commits into from
Apr 4, 2023
Merged

Document how collision checking includes descendent links #2058

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
3d33991
Update planning_scene.h
AndyZe Mar 30, 2023
46ad2b4
Same in collision_common.h
AndyZe Mar 30, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 4 additions & 2 deletions moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 */
Expand Down Expand Up @@ -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))
Expand Down
61 changes: 33 additions & 28 deletions moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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
{
Expand All @@ -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
{
Expand All @@ -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
Expand All @@ -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,
Expand Down Expand Up @@ -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;

Expand All @@ -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;
Expand All @@ -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;

Expand Down