ConfigurationSpaceObstacleCollisionChecker

bindings/pydrake/planning/planning_py_collision_checker.cc

@@ -1,10 +1,13 @@
 #include "drake/bindings/pydrake/common/wrap_pybind.h"
 #include "drake/bindings/pydrake/documentation_pybind.h"
 #include "drake/bindings/pydrake/planning/planning_py.h"
+#include "drake/bindings/pydrake/geometry/optimization_pybind.h"
+
 #include "drake/bindings/pydrake/pydrake_pybind.h"
 #include "drake/planning/collision_checker.h"
 #include "drake/planning/distance_and_interpolation_provider.h"
 #include "drake/planning/scene_graph_collision_checker.h"
+#include "drake/planning/configuration_space_obstacle_collision_checker.h"
 #include "drake/planning/unimplemented_collision_checker.h"
 
 namespace drake {
@@ -311,6 +314,26 @@ void DefinePlanningCollisionChecker(py::module m) {
  .c_str());
  }
 
+ {
+ using Class = ConfigurationSpaceObstacleCollisionChecker;
+
+ py::class_<Class, drake::planning::CollisionChecker>(m, 
+ "ConfigurationSpaceObstacleCollisionChecker")
+ .def(py::init([](const drake::planning::CollisionChecker& checker,
+ const std::vector<drake::geometry::optimization::ConvexSet*>& configuration_obstacles) {
+ return std::make_unique<Class>(copyable_unique_ptr<drake::planning::CollisionChecker> (checker.Clone()), CloneConvexSets(configuration_obstacles));
+ }),
+ py::arg("checker"), py::arg("configuration_obstacles"),
+ "Creates a new ConfigurationSpaceObstacleCollisionChecker with "
+ "the given checker and configuration space obstacles.")
+ .def("add_configuration_space_obstacles", 
+ &Class::AddConfigurationSpaceObstacles, py::arg("new_obstacles"),
+ "Adds new configuration space obstacles.")
+ .def("set_configuration_space_obstacles", 
+ &Class::SetConfigurationSpaceObstacles, py::arg("new_obstacles"),
+ "Sets the configuration space obstacles.");
+ }
+
  {
  using Class = UnimplementedCollisionChecker;
  constexpr auto& cls_doc = doc.UnimplementedCollisionChecker;

bindings/pydrake/planning/planning_py_iris_from_clique_cover.cc

@@ -18,6 +18,10 @@ void DefinePlanningIrisFromCliqueCover(py::module m) {
  .def(py::init<>())
  .def_readwrite("iris_options", &IrisFromCliqueCoverOptions::iris_options,
  cls_doc.iris_options.doc)
+ .def_readwrite("fast_iris_options", &IrisFromCliqueCoverOptions::fast_iris_options,
+ cls_doc.fast_iris_options.doc)
+ .def_readwrite("use_fast_iris", &IrisFromCliqueCoverOptions::use_fast_iris,
+ cls_doc.use_fast_iris.doc)
  .def_readwrite("coverage_termination_threshold",
  &IrisFromCliqueCoverOptions::coverage_termination_threshold,
  cls_doc.coverage_termination_threshold.doc)
@@ -41,7 +45,10 @@ void DefinePlanningIrisFromCliqueCover(py::module m) {
  cls_doc.rank_tol_for_minimum_volume_circumscribed_ellipsoid.doc)
  .def_readwrite("point_in_set_tol",
  &IrisFromCliqueCoverOptions::point_in_set_tol,
- cls_doc.point_in_set_tol.doc);
+ cls_doc.point_in_set_tol.doc)
+ .def_readwrite("sample_with_margin",
+ &IrisFromCliqueCoverOptions::sample_with_margin,
+ cls_doc.sample_with_margin.doc);
 
  m.def(
  "IrisInConfigurationSpaceFromCliqueCover",

bindings/pydrake/planning/test/collision_checker_test.py

@@ -9,6 +9,7 @@
 
 from pydrake.common.test_utilities import numpy_compare
 from pydrake.geometry import Sphere
+from pydrake.geometry.optimization import HPolyhedron
 from pydrake.math import RigidTransform
 from pydrake.multibody.plant import MultibodyPlant
 from pydrake.multibody.tree import (
@@ -461,6 +462,15 @@ def test_scene_graph_collision_checker(self):
  False, True)
  self._test_collision_checker_base_class(function_checker, False)
 
+ def test_configuration_space_obstacle_collision_checker(self):
+ default_checker = self._make_scene_graph_collision_checker(
+ False, False)
+ A = np.eye(default_checker.plant().num_positions())
+ b = np.ones(default_checker.plant().num_positions())
+ sets = [HPolyhedron(A, b)]
+
+ checker2 = mut.ConfigurationSpaceObstacleCollisionChecker(default_checker, sets)
+
  def test_visibility_graph(self):
  checker = self._make_scene_graph_collision_checker(True, False)
  plant = checker.model().plant()

planning/BUILD.bazel

@@ -17,6 +17,7 @@ drake_cc_package_library(
  ":collision_checker",
  ":collision_checker_context",
  ":collision_checker_params",
+ ":configuration_space_obstacle_collision_checker",
  ":distance_and_interpolation_provider",
  ":linear_distance_and_interpolation_provider",
  ":robot_clearance",
@@ -177,6 +178,16 @@ drake_cc_library(
  ],
 )
 
+drake_cc_library(
+ name = "configuration_space_obstacle_collision_checker",
+ srcs = ["configuration_space_obstacle_collision_checker.cc"],
+ hdrs = ["configuration_space_obstacle_collision_checker.h"],
+ deps = [
+ ":collision_checker",
+ "//geometry/optimization:convex_set",
+ ],
+)
+
 drake_cc_library(
  name = "unimplemented_collision_checker",
  srcs = ["unimplemented_collision_checker.cc"],

planning/collision_checker.h

@@ -1251,6 +1251,8 @@ class CollisionChecker {
  */
  std::string CriticizePaddingMatrix() const;
 
+ friend class ConfigurationSpaceObstacleCollisionChecker;
+
  private:
  /* @param context_number Optional implicit context number.
  @see @ref ccb_implicit_contexts "Implicit Context Parallelism". */

planning/configuration_space_obstacle_collision_checker.cc

@@ -0,0 +1,129 @@
+#include "drake/planning/configuration_space_obstacle_collision_checker.h"
+
+#include <algorithm>
+#include <functional>
+#include <set>
+#include <utility>
+#include <iterator>
+
+#include "drake/common/fmt_eigen.h"
+#include "drake/common/copyable_unique_ptr.h"
+#include "drake/geometry/collision_filter_manager.h"
+#include "drake/geometry/geometry_instance.h"
+#include "drake/geometry/scene_graph.h"
+#include "drake/multibody/plant/multibody_plant.h"
+#include "drake/planning/robot_diagram.h"
+#include "drake/geometry/optimization/convex_set.h"
+
+namespace drake {
+namespace planning {
+
+using Eigen::RowVectorXd;
+using Eigen::Vector3d;
+using geometry::CollisionFilterDeclaration;
+using geometry::FrameId;
+using geometry::GeometryId;
+using geometry::GeometryInstance;
+using geometry::GeometrySet;
+using geometry::QueryObject;
+using geometry::SceneGraph;
+using geometry::SceneGraphInspector;
+using geometry::Shape;
+using geometry::SignedDistancePair;
+using math::RigidTransform;
+using multibody::BodyIndex;
+using multibody::Frame;
+using multibody::JacobianWrtVariable;
+using multibody::MultibodyPlant;
+using multibody::RigidBody;
+using systems::Context;
+
+using geometry::optimization::ConvexSets;
+
+ConfigurationSpaceObstacleCollisionChecker::ConfigurationSpaceObstacleCollisionChecker(
+ copyable_unique_ptr<CollisionChecker> checker, 
+ ConvexSets configuration_obstacles) : CollisionChecker(*checker), 
+ checker_(std::move(checker)), 
+ configuration_obstacles_(configuration_obstacles) {
+}
+
+ConfigurationSpaceObstacleCollisionChecker::ConfigurationSpaceObstacleCollisionChecker(
+ const ConfigurationSpaceObstacleCollisionChecker&) = default;
+
+void ConfigurationSpaceObstacleCollisionChecker::AddConfigurationSpaceObstacles(
+ ConvexSets new_obstacles) {
+ std::move(new_obstacles.begin(), new_obstacles.end(), 
+ std::back_inserter(configuration_obstacles_));
+}
+
+void ConfigurationSpaceObstacleCollisionChecker::SetConfigurationSpaceObstacles(
+ ConvexSets new_obstacles) {
+ configuration_obstacles_.assign(
+ std::make_move_iterator(new_obstacles.begin()), 
+ std::make_move_iterator(new_obstacles.end()));
+}
+
+std::unique_ptr<CollisionChecker> ConfigurationSpaceObstacleCollisionChecker::
+ DoClone() const {
+ // N.B. We cannot use make_unique due to private-only access.
+ return std::unique_ptr<ConfigurationSpaceObstacleCollisionChecker>(
+ new ConfigurationSpaceObstacleCollisionChecker(*this));
+}
+
+void ConfigurationSpaceObstacleCollisionChecker::DoUpdateContextPositions(
+ CollisionCheckerContext*) const {
+ // No additional actions are required to update positions.
+}
+
+bool ConfigurationSpaceObstacleCollisionChecker::DoCheckContextConfigCollisionFree(
+ const CollisionCheckerContext& model_context) const {
+ if (not checker_->DoCheckContextConfigCollisionFree(model_context)) {
+ return false;
+ };
+
+ for (const auto& obstacle_ptr : configuration_obstacles_) {
+ if (obstacle_ptr->PointInSet(plant().GetPositions(
+ model_context.plant_context()))) {
+ return false;
+ }
+ }
+
+ return true;
+}
+
+std::optional<GeometryId> ConfigurationSpaceObstacleCollisionChecker::
+ DoAddCollisionShapeToBody(const std::string& group_name, 
+ const RigidBody<double>& bodyA, const Shape& shape, 
+ const RigidTransform<double>& X_AG) {
+ return checker_->DoAddCollisionShapeToBody(group_name, bodyA, shape, X_AG);
+}
+
+void ConfigurationSpaceObstacleCollisionChecker::RemoveAddedGeometries(
+ const std::vector<CollisionChecker::AddedShape>& shapes) {
+ checker_->RemoveAddedGeometries(shapes);
+}
+
+void ConfigurationSpaceObstacleCollisionChecker::UpdateCollisionFilters() {
+ checker_->UpdateCollisionFilters();
+}
+
+RobotClearance ConfigurationSpaceObstacleCollisionChecker::
+ DoCalcContextRobotClearance(const CollisionCheckerContext& model_context,
+ const double influence_distance) const {
+ return checker_->DoCalcContextRobotClearance(model_context, 
+ influence_distance);
+}
+
+std::vector<RobotCollisionType>
+ConfigurationSpaceObstacleCollisionChecker::DoClassifyContextBodyCollisions(
+ const CollisionCheckerContext& model_context) const {
+ return checker_->DoClassifyContextBodyCollisions(model_context);
+}
+
+int ConfigurationSpaceObstacleCollisionChecker::DoMaxContextNumDistances(
+ const CollisionCheckerContext& model_context) const {
+ return checker_->DoMaxContextNumDistances(model_context);
+}
+
+} // namespace planning
+} // namespace drake

planning/configuration_space_obstacle_collision_checker.h

@@ -0,0 +1,78 @@
+#pragma once
+
+#include <memory>
+#include <optional>
+#include <string>
+#include <vector>
+
+#include "drake/common/copyable_unique_ptr.h"
+#include "drake/planning/collision_checker.h"
+#include "drake/planning/collision_checker_params.h"
+#include "drake/geometry/optimization/convex_set.h"
+
+namespace drake {
+namespace planning {
+
+/** An implementation of CollisionChecker that accepts user-specified
+configuration-space obstacles in its constructor and respsects collisions
+against those configuration-space obstacles.
+
+@ingroup planning_collision_checker
+*/
+class ConfigurationSpaceObstacleCollisionChecker final : public CollisionChecker {
+ public:
+ /** @name Does not allow copy, move, or assignment. */
+ /** @{ */
+ // N.B. The copy constructor is private for use in implementing Clone().
+ void operator=(const ConfigurationSpaceObstacleCollisionChecker&) = delete;
+ /** @} */
+
+ /** Creates a new checker with the given params. */
+ ConfigurationSpaceObstacleCollisionChecker(
+ copyable_unique_ptr<CollisionChecker> checker,
+ geometry::optimization::ConvexSets configuration_obstacles);
+
+ void AddConfigurationSpaceObstacles(
+ geometry::optimization::ConvexSets new_obstacles);
+
+ void SetConfigurationSpaceObstacles(
+ geometry::optimization::ConvexSets new_obstacles);
+
+ private:
+ // To support Clone(), allow copying (but not move nor assign).
+ explicit ConfigurationSpaceObstacleCollisionChecker(
+ const ConfigurationSpaceObstacleCollisionChecker&);
+
+ std::unique_ptr<CollisionChecker> DoClone() const final;
+
+ void DoUpdateContextPositions(CollisionCheckerContext*) const final;
+
+ bool DoCheckContextConfigCollisionFree(
+ const CollisionCheckerContext& model_context) const final;
+
+ std::optional<geometry::GeometryId> DoAddCollisionShapeToBody(
+ const std::string& group_name, const multibody::RigidBody<double>& bodyA,
+ const geometry::Shape& shape,
+ const math::RigidTransform<double>& X_AG) final;
+
+ void RemoveAddedGeometries(
+ const std::vector<CollisionChecker::AddedShape>& shapes) final;
+
+ void UpdateCollisionFilters() final;
+
+ RobotClearance DoCalcContextRobotClearance(
+ const CollisionCheckerContext& model_context,
+ double influence_distance) const final;
+
+ std::vector<RobotCollisionType> DoClassifyContextBodyCollisions(
+ const CollisionCheckerContext& model_context) const final;
+
+ int DoMaxContextNumDistances(
+ const CollisionCheckerContext& model_context) const final;
+
+ const copyable_unique_ptr<CollisionChecker> checker_;
+ geometry::optimization::ConvexSets configuration_obstacles_;
+};
+
+} // namespace planning
+} // namespace drake

planning/test/configuration_space_obstacle_collision_checker_test.cc

@@ -0,0 +1,28 @@
+#include "drake/planning/configuration_space_obstacle_collision_checker.h"
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+
+#include "drake/common/test_utilities/eigen_matrix_compare.h"
+#include "drake/planning/linear_distance_and_interpolation_provider.h"
+#include "drake/planning/robot_diagram_builder.h"
+#include "drake/planning/test/planning_test_helpers.h"
+#include "drake/planning/test_utilities/collision_checker_abstract_test_suite.h"
+
+namespace drake {
+namespace planning {
+namespace test {
+namespace {
+
+using Eigen::Matrix3d;
+using Eigen::MatrixXd;
+using Eigen::Vector3d;
+using Eigen::VectorXd;
+
+
+
+
+}
+}
+}
+}