Skip to content

Commit

Permalink
Add use_padding flag + deprecate checkCollisionUnpadded() functions (#…
Browse files Browse the repository at this point in the history 
…3088)

* Add use_padding flag + deprecate Unpadded... functions

* Fix CI

* Remove deprecated functions from python API

* Update #pragmas

* Address review

* Add CHANGELOG comment

* Remove warning

* Update MIGRATION guide

* Update early exit logic in checkCollision

* Update comment
  • Loading branch information
@sjahr
sjahr authored Nov 18, 2024
1 parent 91b47a6 commit d529029
Show file tree
Hide file tree
Showing 7 changed files with 170 additions and 109 deletions.
2 changes: 2 additions & 0 deletions MIGRATION.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,8 @@
API changes in MoveIt releases

## ROS Rolling
- [11/2024] Added flags to control padding to CollisionRequest. This change deprecates PlanningScene::checkCollisionUnpadded(..) functions. Please use PlanningScene::checkCollision(..) with a req.pad_environment_collisions = false;

- [12/2023] `trajectory_processing::Path` and `trajectory_processing::Trajectory` APIs have been updated to prevent misuse.
The constructors have been replaced by builder methods so that errors can be communicated. Paths and trajectories now need to be created with `Path::Create()` and `Trajectory::Create()`. These methods now return an `std::optional` that needs to be checked for a valid value. `Trajectory` no longer has the `isValid()` method. If it's invalid, `Trajectory::Create()` will return `std::nullopt`. Finally, `Path` now takes the list of input waypoints as `std::vector`, instead of `std::list`.
- [12/2023] LMA kinematics plugin is removed to remove the maintenance work because better alternatives exist like KDL or TracIK
Expand Down
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 pad_environment_collisions = true;

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

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

Expand Down
95 changes: 27 additions & 68 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,11 +365,7 @@ 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). */
Expand All @@ -387,99 +376,69 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro
/** \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 d529029

Please sign in to comment.