Skip to content

Commit

Permalink
Add use_padding flag + deprecate Unpadded... functions
Browse files Browse the repository at this point in the history
  • Loading branch information
@sjahr
sjahr committed Nov 14, 2024
1 parent 5049e4d commit 0fb1280
Show file tree
Hide file tree
Showing 5 changed files with 169 additions and 107 deletions.
6 changes: 6 additions & 0 deletions moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -150,6 +150,12 @@ struct CollisionRequest
* are included. */
std::string group_name = "";

/** \brief If true, use padded collision environment */
bool use_padded_collision_environment = true;

/** \brief If true, do self collision check with padded robot links */
bool use_padded_self_collision = false;

/** \brief If true, compute proximity distance */
bool distance = false;

Expand Down
97 changes: 28 additions & 69 deletions moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -347,19 +347,12 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro
void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res);

/** \brief Check whether the current state is in collision. The current state is expected to be updated. */
void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res) const
{
checkCollision(req, res, getCurrentState());
}
void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res) const;

/** \brief Check whether a specified state (\e robot_state) is in collision. This variant of the function takes
a non-const \e robot_state and calls updateCollisionBodyTransforms() on it. */
void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
moveit::core::RobotState& robot_state) const
{
robot_state.updateCollisionBodyTransforms();
checkCollision(req, res, static_cast<const moveit::core::RobotState&>(robot_state));
}
moveit::core::RobotState& robot_state) const;

/** \brief Check whether a specified state (\e robot_state) is in collision. The collision transforms of \e
* robot_state are
Expand All @@ -372,114 +365,80 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro
a non-const \e robot_state and updates its link transforms if needed. */
void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
moveit::core::RobotState& robot_state,
const collision_detection::AllowedCollisionMatrix& acm) const
{
robot_state.updateCollisionBodyTransforms();
checkCollision(req, res, static_cast<const moveit::core::RobotState&>(robot_state), acm);
}
const collision_detection::AllowedCollisionMatrix& acm) const;

/** \brief Check whether a specified state (\e robot_state) is in collision, with respect to a given
allowed collision matrix (\e acm). */
void checkCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
const moveit::core::RobotState& robot_state,
const collision_detection::AllowedCollisionMatrix& acm) const;
const collision_detection::AllowedCollisionMatrix& acm, bool validate_transforms = true) const;

/** \brief Check whether the current state is in collision,
but use a collision_detection::CollisionRobot instance that has no padding.
Since the function is non-const, the current state transforms are also updated if needed. */
void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
collision_detection::CollisionResult& res);
[[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void
checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res);

/** \brief Check whether the current state is in collision,
but use a collision_detection::CollisionRobot instance that has no padding. */
void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
collision_detection::CollisionResult& res) const
{
checkCollisionUnpadded(req, res, getCurrentState(), getAllowedCollisionMatrix());
}
[[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void
checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
collision_detection::CollisionResult& res) const;

/** \brief Check whether a specified state (\e robot_state) is in collision,
but use a collision_detection::CollisionRobot instance that has no padding. */
void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
collision_detection::CollisionResult& res,
const moveit::core::RobotState& robot_state) const
{
checkCollisionUnpadded(req, res, robot_state, getAllowedCollisionMatrix());
}
[[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void
checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
const moveit::core::RobotState& robot_state) const;

/** \brief Check whether a specified state (\e robot_state) is in collision,
but use a collision_detection::CollisionRobot instance that has no padding.
Update the link transforms of \e robot_state if needed. */
void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
collision_detection::CollisionResult& res, moveit::core::RobotState& robot_state) const
{
robot_state.updateCollisionBodyTransforms();
checkCollisionUnpadded(req, res, static_cast<const moveit::core::RobotState&>(robot_state),
getAllowedCollisionMatrix());
}
[[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void
checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
moveit::core::RobotState& robot_state) const;

/** \brief Check whether a specified state (\e robot_state) is in collision, with respect to a given
allowed collision matrix (\e acm), but use a collision_detection::CollisionRobot instance that has no padding.
This variant of the function takes a non-const \e robot_state and calls updates the link transforms if needed. */
void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
collision_detection::CollisionResult& res, moveit::core::RobotState& robot_state,
const collision_detection::AllowedCollisionMatrix& acm) const
{
robot_state.updateCollisionBodyTransforms();
checkCollisionUnpadded(req, res, static_cast<const moveit::core::RobotState&>(robot_state), acm);
}
[[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void
checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
moveit::core::RobotState& robot_state,
const collision_detection::AllowedCollisionMatrix& acm) const;

/** \brief Check whether a specified state (\e robot_state) is in collision, with respect to a given
allowed collision matrix (\e acm), but use a collision_detection::CollisionRobot instance that has no padding. */
void checkCollisionUnpadded(const collision_detection::CollisionRequest& req,
collision_detection::CollisionResult& res, const moveit::core::RobotState& robot_state,
const collision_detection::AllowedCollisionMatrix& acm) const;
[[deprecated("Use new CollisionRequest flags to enable/ disable padding instead.")]] void
checkCollisionUnpadded(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
const moveit::core::RobotState& robot_state,
const collision_detection::AllowedCollisionMatrix& acm) const;

/** \brief Check whether the current state is in self collision */
void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res);

/** \brief Check whether the current state is in self collision */
void checkSelfCollision(const collision_detection::CollisionRequest& req,
collision_detection::CollisionResult& res) const
{
checkSelfCollision(req, res, getCurrentState());
}
collision_detection::CollisionResult& res) const;

/** \brief Check whether a specified state (\e robot_state) is in self collision */
void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
moveit::core::RobotState& robot_state) const
{
robot_state.updateCollisionBodyTransforms();
checkSelfCollision(req, res, static_cast<const moveit::core::RobotState&>(robot_state), getAllowedCollisionMatrix());
}
moveit::core::RobotState& robot_state) const;

/** \brief Check whether a specified state (\e robot_state) is in self collision */
void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
const moveit::core::RobotState& robot_state) const
{
// do self-collision checking with the unpadded version of the robot
getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, getAllowedCollisionMatrix());
}
const moveit::core::RobotState& robot_state) const;

/** \brief Check whether a specified state (\e robot_state) is in self collision, with respect to a given
allowed collision matrix (\e acm). The link transforms of \e robot_state are updated if needed. */
void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
moveit::core::RobotState& robot_state,
const collision_detection::AllowedCollisionMatrix& acm) const
{
robot_state.updateCollisionBodyTransforms();
checkSelfCollision(req, res, static_cast<const moveit::core::RobotState&>(robot_state), acm);
}
const collision_detection::AllowedCollisionMatrix& acm) const;

/** \brief Check whether a specified state (\e robot_state) is in self collision, with respect to a given
allowed collision matrix (\e acm) */
void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
const moveit::core::RobotState& robot_state,
const collision_detection::AllowedCollisionMatrix& acm) const
{
// do self-collision checking with the unpadded version of the robot
getCollisionEnvUnpadded()->checkSelfCollision(req, res, robot_state, acm);
}
const collision_detection::AllowedCollisionMatrix& acm) const;

/** \brief Get the names of the links that are involved in collisions for the current state */
void getCollidingLinks(std::vector<std::string>& links);
Expand Down
Loading

0 comments on commit 0fb1280

Please sign in to comment.