diff --git a/planning/iris/BUILD.bazel b/planning/iris/BUILD.bazel index d14ec172c037..6d1acec085b0 100644 --- a/planning/iris/BUILD.bazel +++ b/planning/iris/BUILD.bazel @@ -37,6 +37,130 @@ drake_cc_library( ], ) +drake_cc_library( + name = "iris_interface_options", + srcs = ["iris_interface_options.cc"], + hdrs = ["iris_interface_options.h"], + deps = [ + "//geometry/optimization:convex_set", + "//solvers:solver_options", + "//geometry:meshcat", + "//common:random" + ], + implementation_deps = [ + ], +) + + +drake_cc_library( + name = "iris_via_collisions_and_ellipsoid_interface_options", + srcs = ["iris_via_collisions_and_ellipsoid_interface_options.cc"], + hdrs = ["iris_via_collisions_and_ellipsoid_interface_options.h"], + deps = [ + "//planning/iris:iris_interface", + "//planning/iris:iris_interface_options" + ], +) + +drake_cc_library( + name = "iris_via_collisions_and_ellipsoid_interface", + srcs = ["iris_via_collisions_and_ellipsoid_interface.cc"], + hdrs = ["iris_via_collisions_and_ellipsoid_interface.h"], + deps = [ + "//planning/iris:iris_via_collisions_and_ellipsoid_interface_options", + "//planning/iris:iris_interface", + "//planning:collision_checker", + "//planning/iris:iris_interface_options" + ], +) + +drake_cc_library( + name = "iris_interface", + srcs = ["iris_interface.cc"], + hdrs = ["iris_interface.h"], + deps = [ + "//planning:collision_checker", + ":iris_interface_options", + "//geometry/optimization:convex_set", + ], + implementation_deps = [ + ], +) + +drake_cc_library( + name = "iris_np", + srcs = ["iris_np.cc"], + hdrs = ["iris_np.h"], + deps = [ + "//planning:scene_graph_collision_checker", + "//planning/iris:iris_via_collisions_and_ellipsoid_interface", + "//planning/iris:iris_via_collisions_and_ellipsoid_interface_options", + "//geometry/optimization:convex_set", + ], + implementation_deps = [ + ], +) + +drake_cc_library( + name = "iris_np2", + srcs = ["iris_np2.cc"], + hdrs = ["iris_np2.h"], + deps = [ + "//planning:scene_graph_collision_checker", + "//planning/iris:iris_via_collisions_and_ellipsoid_interface", + "//planning/iris:iris_via_collisions_and_ellipsoid_interface_options", + "//geometry/optimization:convex_set", + ], + implementation_deps = [ + ], +) + +drake_cc_library( + name = "iris_zo", + srcs = ["iris_zo.cc"], + hdrs = ["iris_zo.h"], + deps = [ + "//planning:collision_checker", + "//planning/iris:iris_via_collisions_and_ellipsoid_interface", + "//planning/iris:iris_via_collisions_and_ellipsoid_interface_options", + "//geometry/optimization:convex_set", + ], + implementation_deps = [ + ], +) + + +drake_cc_library( + name = "c_iris", + srcs = ["c_iris.cc"], + hdrs = ["c_iris.h"], + deps = [ + "//planning:scene_graph_collision_checker", + "//planning/iris:iris_interface", + "//planning/iris:iris_interface_options", + "//geometry/optimization:convex_set", + "//geometry/optimization:cspace_free_polytope", + "//geometry/optimization:cspace_free_polytope_base", + "//geometry/optimization:cspace_free_structs", + ], + implementation_deps = [ + ], +) + +drake_cc_library( + name = "vanilla_iris", + srcs = ["vanilla_iris.cc"], + hdrs = ["vanilla_iris.h"], + deps = [ + "//planning:collision_checker", + "//planning/iris:iris_via_collisions_and_ellipsoid_interface", + "//planning/iris:iris_via_collisions_and_ellipsoid_interface_options", + "//geometry/optimization:convex_set", + ], + implementation_deps = [ + ], +) + drake_cc_googletest( name = "iris_from_clique_cover_test", timeout = "moderate", diff --git a/planning/iris/c_iris.cc b/planning/iris/c_iris.cc new file mode 100644 index 000000000000..b7d94c113336 --- /dev/null +++ b/planning/iris/c_iris.cc @@ -0,0 +1,43 @@ +#include "drake/planning/iris/c_iris.h" + +namespace drake { +namespace planning { +using geometry::optimization::CspaceFreePolytope; + +C_Iris::C_Iris( + const SceneGraphCollisionChecker& checker, + geometry::optimization::SeparatingPlaneOrder plane_order, + const Eigen::Ref& q_star, + const geometry::optimization::CspaceFreePolytopeBase::Options& options) + : IrisInterface(checker), + set_builder_{&(checker_->plant()), &(checker_->model().scene_graph()), + plane_order, q_star, options} {}; + +void C_Iris::DoSetup(const C_IrisOptions& options, + geometry::optimization::HPolyhedron* set) { + *set = options.seed_region; + std::optional search_result = + set_builder_.BinarySearch(options.ignored_collision_pairs, set->A(), + set->b(), set->ChebyshevCenter(), + options.binary_search_options); + throw std::logic_error("unimplemented"); +} + +void C_Iris::DoImproveRegionHyperplanes( + const C_IrisOptions& options, geometry::optimization::HPolyhedron* set) { + // This should implement the step which pushes the hyperplanes away from the + // max volume inscribed ellipse. + unused(options, set); + throw std::logic_error("unimplemented"); +} + +void C_Iris::DoUpdateMetric(const C_IrisOptions& options, + const geometry::optimization::HPolyhedron& set) { + // This should compute the new inscribed ellipse and the new certificate after + // the hyperplanes have been pushed back. + unused(options, set); + throw std::logic_error("unimplemented"); +} + +} // namespace planning +} // namespace drake diff --git a/planning/iris/c_iris.h b/planning/iris/c_iris.h new file mode 100644 index 000000000000..a55dc4e5ef2b --- /dev/null +++ b/planning/iris/c_iris.h @@ -0,0 +1,68 @@ +#pragma once + +#include "drake/geometry/optimization/cspace_free_polytope.h" +#include "drake/geometry/optimization/cspace_free_polytope_base.h" +#include "drake/geometry/optimization/cspace_free_structs.h" +#include "drake/geometry/optimization/hpolyhedron.h" +#include "drake/planning/iris/iris_interface.h" +#include "drake/planning/iris/iris_interface_options.h" +#include "drake/planning/scene_graph_collision_checker.h" + +namespace drake { +namespace planning { + +struct C_IrisOptions : IrisInterfaceOptions { + // TODO add all the unique options. + + // Options for running binary search during DoSetup which expands the seed + // polytope. + geometry::optimization::CspaceFreePolytope::BinarySearchOptions + binary_search_options; + + // Options for running binary search during DoSetup which expands the seed + // polytope. + geometry::optimization::CspaceFreePolytope::BilinearAlternationOptions + bilinearSearchOptions; + + geometry::optimization::HPolyhedron seed_region; + + geometry::optimization::CspaceFreePolytopeBase::IgnoredCollisionPairs + ignored_collision_pairs; +}; + +class C_Iris final : public IrisInterface { + public: + C_Iris( + const SceneGraphCollisionChecker& checker, + geometry::optimization::SeparatingPlaneOrder plane_order, + const Eigen::Ref& q_star, + const geometry::optimization::CspaceFreePolytopeBase::Options& options = + geometry::optimization::CspaceFreePolytopeBase::Options{}); + + private: + // Do not hide IrisInterface's pure virtual functions. + using IrisInterface::DoImproveRegionHyperplanes; + using IrisInterface::DoUpdateMetric; + using IrisInterface::DoSetup; + + /** Runs Binary Search on the seed polytope. */ + void DoSetup(const C_IrisOptions& options, + geometry::optimization::HPolyhedron* set); + + /** Runs the step which pushes the hyperplanes away from the max volume + * inscribed ellipse. */ + void DoImproveRegionHyperplanes(const C_IrisOptions& options, + geometry::optimization::HPolyhedron* set); + + /** Runs the step which computes the max volume inscribed ellipse and a new + * certificate. */ + void DoUpdateMetric(const C_IrisOptions& options, + const geometry::optimization::HPolyhedron& set); + + // Currently we use the set_builder_ to implement the above methods, but a + // better rewrite will condense this into one file. + geometry::optimization::CspaceFreePolytope set_builder_; +}; + +} // namespace planning +} // namespace drake diff --git a/planning/iris/iris_interface.cc b/planning/iris/iris_interface.cc new file mode 100644 index 000000000000..338d295cd0df --- /dev/null +++ b/planning/iris/iris_interface.cc @@ -0,0 +1 @@ +#include "drake/planning/iris/iris_interface.h" diff --git a/planning/iris/iris_interface.h b/planning/iris/iris_interface.h new file mode 100644 index 000000000000..b366712c2f8b --- /dev/null +++ b/planning/iris/iris_interface.h @@ -0,0 +1,100 @@ +#pragma once + +#include + +#include "drake/common/drake_copyable.h" +#include "drake/geometry/optimization/hpolyhedron.h" +#include "drake/planning/collision_checker.h" +#include "drake/planning/iris/iris_interface_options.h" + +namespace drake { +namespace planning { + +// SFINAE that the options subclass must derive from the IrisInterfaceOptions. +template ::value>> +class IrisInterface { + /** + * A class for implementing various Iris-type algorithms. Note that this + * interface is NOT thread-safe in the sense that one IrisInterface object + * cannot be used to construct multiple regions in parallel. + */ + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(IrisInterface); + virtual ~IrisInterface(); + + geometry::optimization::HPolyhedron BuildSet( + const IrisInterfaceOptionsSubclass& options) { + geometry::optimization::HPolyhedron set = + options.domain.value_or(geometry::optimization::HPolyhedron::MakeBox( + checker_->plant().GetPositionLowerLimits(), + checker_->plant().GetPositionUpperLimits())); + DRAKE_THROW_UNLESS(set.IsBounded()); + + Setup(options, &set); + + int iteration = 0; + while (iteration < options.iteration_limit && !CheckTermination(set)) { + ImproveRegionHyperplanes(options, &set); + UpdateMetric(options, set); + ++iteration; + } + return set; + }; + + void Setup(const IrisInterfaceOptionsSubclass& options, + geometry::optimization::HPolyhedron* set) { + DRAKE_THROW_UNLESS(set != nullptr); + DoSetup(options, set); + } + + void ImproveRegionHyperplanes(const IrisInterfaceOptionsSubclass& options, + geometry::optimization::HPolyhedron* set) { + DRAKE_THROW_UNLESS(set != nullptr); + DoImproveRegionHyperplanes(options, set); + } + + void UpdateMetric(const IrisInterfaceOptionsSubclass& options, + const geometry::optimization::HPolyhedron& set) { + DoUpdateMetric(options, set); + } + + bool CheckTermination(const IrisInterfaceOptionsSubclass& options, + const geometry::optimization::HPolyhedron& set) { + bool ret{false}; + if (options.termination_func) { + ret = options.termination_func(set); + } + return ret || DoCheckTermination(options, set); + } + + protected: + IrisInterface(const CollisionChecker& checker) + : checker_{std::move(checker.Clone())} {}; + + /** Runs any additional set up code before the set building loop starts. */ + virtual void DoSetup(const IrisInterfaceOptionsSubclass& options, + geometry::optimization::HPolyhedron* set) = 0; + + /** Given a proposed region, modify it to a better region, e.g. by adding + * hyperplanes so that less of the set is in collision. */ + virtual void DoImproveRegionHyperplanes( + const IrisInterfaceOptionsSubclass& options, + geometry::optimization::HPolyhedron* set) = 0; + + /** Updates the metric used to find an improvement of the set */ + virtual void DoUpdateMetric( + const IrisInterfaceOptionsSubclass& options, + const geometry::optimization::HPolyhedron& set) = 0; + + /** Returns true if the set construct loop should terminate. */ + virtual bool DoCheckTermination( + const IrisInterfaceOptionsSubclass& options, + const geometry::optimization::HPolyhedron& set) = 0; + + std::unique_ptr checker_; +}; + +} // namespace planning +} // namespace drake \ No newline at end of file diff --git a/planning/iris/iris_interface_options.cc b/planning/iris/iris_interface_options.cc new file mode 100644 index 000000000000..8b281991c516 --- /dev/null +++ b/planning/iris/iris_interface_options.cc @@ -0,0 +1 @@ +#include "drake/planning/iris/iris_interface_options.h" diff --git a/planning/iris/iris_interface_options.h b/planning/iris/iris_interface_options.h new file mode 100644 index 000000000000..2c5b8f2a252d --- /dev/null +++ b/planning/iris/iris_interface_options.h @@ -0,0 +1,34 @@ +#pragma once + +#include "drake/common/random.h" +#include "drake/geometry/meshcat.h" +#include "drake/geometry/optimization/hpolyhedron.h" + +namespace drake { +namespace planning { + +struct IrisInterfaceOptions { + /** Maximum number of iterations. */ + int iteration_limit{100}; + + /** A function which returns true when a set is done being constructed.*/ + std::function + termination_func; + + /** Solver options used by any internal solvers used in the loop. */ + std::optional solver_options; + + /** The domain in which the set is constructed. If unset, the plant's joint + * limits are used. */ + std::optional domain; + + /** A default randomness generator for source of randomness which may occur in + * the algorithm. */ + RandomGenerator random_generator{0}; + + /** Passing a meshcat instance may enable debugging visualizations. */ + std::shared_ptr meshcat{}; +}; + +} // namespace planning +} // namespace drake \ No newline at end of file diff --git a/planning/iris/iris_np.cc b/planning/iris/iris_np.cc new file mode 100644 index 000000000000..d345c9287663 --- /dev/null +++ b/planning/iris/iris_np.cc @@ -0,0 +1,26 @@ +#include "drake/planning/iris/iris_np.h" + +namespace drake { +namespace planning { + +IrisNp::IrisNp(const SceneGraphCollisionChecker& checker) + : IrisViaCollisionsAndEllipsoidInterface(checker){}; + +Eigen::MatrixXd IrisNp::FindCollisionPoints( + const IrisNpOptions& options, + const geometry::optimization::HPolyhedron& set) { + unused(options, set); + throw std::logic_error("unimplemented"); + return Eigen::VectorXd::Zero(checker_->plant().num_positions()); +} + +void IrisNp::AddHyperplanesAtPoints( + const Eigen::Ref& points, + const IrisNpOptions& options, + geometry::optimization::HPolyhedron* set) const { + unused(points, options, set); + throw std::logic_error("unimplemented"); +} + +} // namespace planning +} // namespace drake diff --git a/planning/iris/iris_np.h b/planning/iris/iris_np.h new file mode 100644 index 000000000000..b8da7460c859 --- /dev/null +++ b/planning/iris/iris_np.h @@ -0,0 +1,40 @@ +#pragma once + +#include "drake/geometry/optimization/hpolyhedron.h" +#include "drake/planning/scene_graph_collision_checker.h" +#include "drake/planning/iris/iris_via_collisions_and_ellipsoid_interface.h" +#include "drake/planning/iris/iris_via_collisions_and_ellipsoid_interface_options.h" + +namespace drake { +namespace planning { + +struct IrisNpOptions + : internal::IrisViaCollisionsAndEllipsoidInterfaceOptions { + // TODO add all the unique options. +}; + +class IrisNp final + : public internal::IrisViaCollisionsAndEllipsoidInterface { + public: + explicit IrisNp(const SceneGraphCollisionChecker& checker); + + private: + // Do not hide IrisViaCollisionsAndEllipsoidInterface's pure virtual + // functions. + using internal::IrisViaCollisionsAndEllipsoidInterface< + IrisNpOptions>::FindCollisionPoints; + using internal::IrisViaCollisionsAndEllipsoidInterface< + IrisNpOptions>::AddHyperplanesAtPoints; + + Eigen::MatrixXd FindCollisionPoints( + const IrisNpOptions& options, + const geometry::optimization::HPolyhedron& set); + + void AddHyperplanesAtPoints(const Eigen::Ref& points, + const IrisNpOptions& options, + geometry::optimization::HPolyhedron* set) const; +}; + +} // namespace planning + +} // namespace drake \ No newline at end of file diff --git a/planning/iris/iris_np2.cc b/planning/iris/iris_np2.cc new file mode 100644 index 000000000000..7c331acc3849 --- /dev/null +++ b/planning/iris/iris_np2.cc @@ -0,0 +1,26 @@ +#include "drake/planning/iris/iris_np2.h" + +namespace drake { +namespace planning { + +IrisNp2::IrisNp2(const SceneGraphCollisionChecker& checker) + : IrisViaCollisionsAndEllipsoidInterface(checker){}; + +Eigen::MatrixXd IrisNp2::FindCollisionPoints( + const IrisNp2Options& options, + const geometry::optimization::HPolyhedron& set) { + unused(options, set); + throw std::logic_error("unimplemented"); + return Eigen::VectorXd::Zero(checker_->plant().num_positions()); +} + +void IrisNp2::AddHyperplanesAtPoints( + const Eigen::Ref& points, + const IrisNp2Options& options, + geometry::optimization::HPolyhedron* set) const { + unused(points, options, set); + throw std::logic_error("unimplemented"); +} + +} // namespace planning +} // namespace drake diff --git a/planning/iris/iris_np2.h b/planning/iris/iris_np2.h new file mode 100644 index 000000000000..00f00a210df3 --- /dev/null +++ b/planning/iris/iris_np2.h @@ -0,0 +1,39 @@ +#pragma once + +#include "drake/geometry/optimization/hpolyhedron.h" +#include "drake/planning/scene_graph_collision_checker.h" +#include "drake/planning/iris/iris_via_collisions_and_ellipsoid_interface.h" +#include "drake/planning/iris/iris_via_collisions_and_ellipsoid_interface_options.h" + +namespace drake { +namespace planning { + +struct IrisNp2Options + : internal::IrisViaCollisionsAndEllipsoidInterfaceOptions { + // TODO add all the unique options. +}; + +class IrisNp2 final + : public internal::IrisViaCollisionsAndEllipsoidInterface { + public: + explicit IrisNp2(const SceneGraphCollisionChecker& checker); + + private: + // Do not hide IrisViaCollisionsAndEllipsoidInterface's pure virtual + // functions. + using internal::IrisViaCollisionsAndEllipsoidInterface< + IrisNp2Options>::FindCollisionPoints; + using internal::IrisViaCollisionsAndEllipsoidInterface< + IrisNp2Options>::AddHyperplanesAtPoints; + + Eigen::MatrixXd FindCollisionPoints( + const IrisNp2Options& options, + const geometry::optimization::HPolyhedron& set); + + void AddHyperplanesAtPoints(const Eigen::Ref& points, + const IrisNp2Options& options, + geometry::optimization::HPolyhedron* set) const; +}; + +} // namespace planning +} // namespace drake diff --git a/planning/iris/iris_via_collisions_and_ellipsoid_interface.cc b/planning/iris/iris_via_collisions_and_ellipsoid_interface.cc new file mode 100644 index 000000000000..23ea109e7427 --- /dev/null +++ b/planning/iris/iris_via_collisions_and_ellipsoid_interface.cc @@ -0,0 +1 @@ +#include "planning/iris/iris_via_collisions_and_ellipsoid_interface.h" diff --git a/planning/iris/iris_via_collisions_and_ellipsoid_interface.h b/planning/iris/iris_via_collisions_and_ellipsoid_interface.h new file mode 100644 index 000000000000..b8fb899f61ab --- /dev/null +++ b/planning/iris/iris_via_collisions_and_ellipsoid_interface.h @@ -0,0 +1,107 @@ +#pragma once + +#include + +#include "drake/common/drake_copyable.h" +#include "drake/common/drake_throw.h" +#include "drake/geometry/optimization/hpolyhedron.h" +#include "drake/geometry/optimization/hyperellipsoid.h" +#include "drake/planning/collision_checker.h" +#include "drake/planning/iris/iris_interface.h" +#include "drake/planning/iris/iris_interface_options.h" +#include "drake/planning/iris/iris_via_collisions_and_ellipsoid_interface_options.h" + +namespace drake { +namespace planning { +namespace internal { + +// SFINAE that the options subclass must derive from the +// IrisViaCollisionsAndEllipsoidInterfaceOptions. +template ::value>> +// template +class IrisViaCollisionsAndEllipsoidInterface + : public IrisInterface< + IrisViaCollisionsAndEllipsoidInterfaceOptionsSubclass> { + public: + virtual ~IrisViaCollisionsAndEllipsoidInterface(); + + protected: + // Do not hide IrisInterface's pure virtual functions. + using IrisInterface:: + DoImproveRegionHyperplanes; + using IrisInterface< + IrisViaCollisionsAndEllipsoidInterfaceOptionsSubclass>::DoUpdateMetric; + using IrisInterface< + IrisViaCollisionsAndEllipsoidInterfaceOptionsSubclass>::DoSetup; + + IrisViaCollisionsAndEllipsoidInterface(const CollisionChecker& checker) + : IrisInterface( + checker){ + // TODO complete constructor. This will essentially implement + // MakeIrisObstacles from geometry/optimization/iris.h + }; + + /** Each column of the returned matrix is a point in set which contains a + * collision */ + virtual Eigen::MatrixXd FindCollisionPoints( + const IrisViaCollisionsAndEllipsoidInterfaceOptions& options, + const geometry::optimization::HPolyhedron& set) = 0; + + /** Given a set of points in collision that are inside sets, add hyperplanes + * to set to exclude those collisions. */ + virtual void AddHyperplanesAtPoints( + const Eigen::Ref& points, + const IrisViaCollisionsAndEllipsoidInterfaceOptions& options, + geometry::optimization::HPolyhedron* set) const = 0; + + private: + void DoImproveRegionHyperplanes( + const IrisViaCollisionsAndEllipsoidInterfaceOptions& options, + geometry::optimization::HPolyhedron* set) { + while (HPolyhedronIsCollisionFreeViaUnadaptiveTest(*set, options)) { + const Eigen::MatrixXd collision_points = + FindCollisionPoints(options, *set); + AddHyperplanesAtPoints(collision_points, options, set); + } + }; + + void DoUpdateMetric( + const IrisViaCollisionsAndEllipsoidInterfaceOptions& options, + const geometry::optimization::HPolyhedron& set) { + unused(options); + ellipsoid_metric = set.MaximumVolumeInscribedEllipsoid(); + }; + + void DoSetup(const IrisViaCollisionsAndEllipsoidInterfaceOptions& options, + geometry::optimization::HPolyhedron* set) { + unused(set); + DRAKE_THROW_UNLESS(options.seed.rows() == + this->checker_->plant().num_positions()); + for (int i = 0; i < this->checker_->num_allocated_contexts(); ++i) { + this->checker_->UpdatePositions(options.seed, i); + } + const double kEpsilonEllipsoid = 1e-2; + ellipsoid_metric = options.starting_ellipse.value_or( + geometry::optimization::Hyperellipsoid::MakeHypersphere( + kEpsilonEllipsoid, options.seed)); + }; + + bool HPolyhedronIsCollisionFreeViaUnadaptiveTest( + const geometry::optimization::HPolyhedron& set, + const IrisViaCollisionsAndEllipsoidInterfaceOptions& options) const { + unused(set); + unused(options); + // TODO implement + throw std::logic_error("Unimplemented"); + return true; + }; + + geometry::optimization::Hyperellipsoid ellipsoid_metric; +}; + +} // namespace internal +} // namespace planning +} // namespace drake \ No newline at end of file diff --git a/planning/iris/iris_via_collisions_and_ellipsoid_interface_options.cc b/planning/iris/iris_via_collisions_and_ellipsoid_interface_options.cc new file mode 100644 index 000000000000..27726cb65742 --- /dev/null +++ b/planning/iris/iris_via_collisions_and_ellipsoid_interface_options.cc @@ -0,0 +1 @@ +#include "drake/planning/iris/iris_via_collisions_and_ellipsoid_interface_options.h" \ No newline at end of file diff --git a/planning/iris/iris_via_collisions_and_ellipsoid_interface_options.h b/planning/iris/iris_via_collisions_and_ellipsoid_interface_options.h new file mode 100644 index 000000000000..92dbb6682e47 --- /dev/null +++ b/planning/iris/iris_via_collisions_and_ellipsoid_interface_options.h @@ -0,0 +1,85 @@ +#pragma once + +#include + +#include "drake/common/drake_copyable.h" +#include "drake/geometry/optimization/hpolyhedron.h" +#include "drake/planning/collision_checker.h" +#include "drake/planning/iris/iris_interface.h" +#include "drake/planning/iris/iris_interface_options.h" + +namespace drake { +namespace planning { +namespace internal { + +struct IrisViaCollisionsAndEllipsoidInterfaceOptions + : public IrisInterfaceOptions { + /** IRIS will terminate if the change in the *volume* of the hyperellipsoid + between iterations is less that this threshold. This termination condition can + be disabled by setting to a negative value. + + \b Algorithms: All */ + double absolute_termination_threshold{2e-2}; // from rdeits/iris-distro. + + /** IRIS will terminate if the change in the *volume* of the hyperellipsoid + between iterations is less that this percent of the previous best volume. + This termination condition can be disabled by setting to a negative value. + + \b Algorithms: All */ + double relative_termination_threshold{1e-3}; // from rdeits/iris-distro. + + // TODO(russt): Improve the implementation so that we can clearly document the + // units for this margin. + /** When adding hyperplanes, we retreat by this margin from each + C-space obstacle in order to avoid the possibility of requiring an infinite + number of faces to approximate a curved boundary. + + \b Algorithms: NP, NP2, and ZO */ + double configuration_space_margin{1e-2}; + + /** For each possible collision, IRIS will search for a counter-example by + formulating a (likely nonconvex) optimization problem. The initial guess for + this optimization is taken by sampling uniformly inside the current IRIS + region. This option controls the termination condition for that + counter-example search, defining the number of consecutive failures to find a + counter-example requested before moving on to the next constraint. + + \b Algorithms: NP */ + int num_collision_infeasible_samples{5}; + + /** For IRIS in configuration space, it can be beneficial to not only specify + task-space obstacles (passed in through the plant) but also obstacles that are + defined by convex sets in the configuration space. This option can be used to + pass in such configuration space obstacles. + + \b Algorithms: All */ + geometry::optimization::ConvexSets configuration_obstacles{}; + + /** The current seed point around which a region is grown. */ + Eigen::VectorXd seed{}; + + /** The initial hyperellipsoid used for calculating hyperplanes + in the first iteration. If no hyperellipsoid is provided, a small hypershpere + centered at the current seed will be used. */ + std::optional starting_ellipse{}; + + /** The p in the unadpative test from Faster Algorithms for Growing + * Collision-Free Convex Polytopes*/ + double p{0.9}; + + /** The delta in the unadpative test from Faster Algorithms for Growing + * Collision-Free Convex Polytopes*/ + double delta{0.9}; + + /** The tau in the unadpative test from Faster Algorithms for Growing + * Collision-Free Convex Polytopes*/ + double tau{0.9}; + + /** Number of mixing steps used when sampling from the set during the + * unadpative test. */ + int mixing_steps{10}; +}; + +} // namespace internal +} // namespace planning +} // namespace drake \ No newline at end of file diff --git a/planning/iris/iris_zo.cc b/planning/iris/iris_zo.cc new file mode 100644 index 000000000000..564520cf8fe9 --- /dev/null +++ b/planning/iris/iris_zo.cc @@ -0,0 +1,26 @@ +#include "drake/planning/iris/iris_zo.h" + +namespace drake { +namespace planning { + +IrisZo::IrisZo(const CollisionChecker& checker) + : IrisViaCollisionsAndEllipsoidInterface(checker){}; + +Eigen::MatrixXd IrisZo::FindCollisionPoints( + const IrisZoOptions& options, + const geometry::optimization::HPolyhedron& set) { + unused(options, set); + throw std::logic_error("unimplemented"); + return Eigen::VectorXd::Zero(checker_->plant().num_positions()); +} + +void IrisZo::AddHyperplanesAtPoints( + const Eigen::Ref& points, + const IrisZoOptions& options, + geometry::optimization::HPolyhedron* set) const { + unused(points, options, set); + throw std::logic_error("unimplemented"); +} + +} // namespace planning +} // namespace drake diff --git a/planning/iris/iris_zo.h b/planning/iris/iris_zo.h new file mode 100644 index 000000000000..1731959fd47a --- /dev/null +++ b/planning/iris/iris_zo.h @@ -0,0 +1,39 @@ +#pragma once + +#include "drake/geometry/optimization/hpolyhedron.h" +#include "drake/planning/collision_checker.h" +#include "drake/planning/iris/iris_via_collisions_and_ellipsoid_interface.h" +#include "drake/planning/iris/iris_via_collisions_and_ellipsoid_interface_options.h" + +namespace drake { +namespace planning { + +struct IrisZoOptions + : internal::IrisViaCollisionsAndEllipsoidInterfaceOptions { + // TODO add all the unique options. +}; + +class IrisZo final + : public internal::IrisViaCollisionsAndEllipsoidInterface { + public: + explicit IrisZo(const CollisionChecker& checker); + + private: + // Do not hide IrisViaCollisionsAndEllipsoidInterface's pure virtual + // functions. + using internal::IrisViaCollisionsAndEllipsoidInterface< + IrisZoOptions>::FindCollisionPoints; + using internal::IrisViaCollisionsAndEllipsoidInterface< + IrisZoOptions>::AddHyperplanesAtPoints; + + Eigen::MatrixXd FindCollisionPoints( + const IrisZoOptions& options, + const geometry::optimization::HPolyhedron& set); + + void AddHyperplanesAtPoints(const Eigen::Ref& points, + const IrisZoOptions& options, + geometry::optimization::HPolyhedron* set) const; +}; + +} // namespace planning +} // namespace drake diff --git a/planning/iris/vanilla_iris.cc b/planning/iris/vanilla_iris.cc new file mode 100644 index 000000000000..b3abcc5e7564 --- /dev/null +++ b/planning/iris/vanilla_iris.cc @@ -0,0 +1,21 @@ +#include "drake/planning/iris/vanilla_iris.h" +namespace drake { +namespace planning { + +Eigen::MatrixXd VanillaIris::FindCollisionPoints( + const VanillaIrisOptions& options, + const geometry::optimization::HPolyhedron& set) { + unused(options, set); + throw std::logic_error("unimplemented"); +} + +void VanillaIris::AddHyperplanesAtPoints( + const Eigen::Ref& points, + const VanillaIrisOptions& options, + geometry::optimization::HPolyhedron* set) const { + unused(points, options, set); + throw std::logic_error("unimplemented"); +} + +} // namespace planning +} // namespace drake \ No newline at end of file diff --git a/planning/iris/vanilla_iris.h b/planning/iris/vanilla_iris.h new file mode 100644 index 000000000000..0b7d1019a1ae --- /dev/null +++ b/planning/iris/vanilla_iris.h @@ -0,0 +1,43 @@ +#pragma once + +#include "drake/geometry/optimization/hpolyhedron.h" +#include "drake/planning/collision_checker.h" +#include "drake/planning/iris/iris_via_collisions_and_ellipsoid_interface.h" +#include "drake/planning/iris/iris_via_collisions_and_ellipsoid_interface_options.h" + +namespace drake { +namespace planning { + +struct VanillaIrisOptions + : internal::IrisViaCollisionsAndEllipsoidInterfaceOptions { + // TODO add all the unique options. +}; + +class VanillaIris final + : public internal::IrisViaCollisionsAndEllipsoidInterface< + VanillaIrisOptions> { + public: + explicit VanillaIris(const CollisionChecker& checker); + + private: + // Do not hide IrisViaCollisionsAndEllipsoidInterface's pure virtual + // functions. + using internal::IrisViaCollisionsAndEllipsoidInterface< + VanillaIrisOptions>::FindCollisionPoints; + using internal::IrisViaCollisionsAndEllipsoidInterface< + VanillaIrisOptions>::AddHyperplanesAtPoints; + + // Finds the closest point on the closest obstacle to the seed point that is + // inside set. + Eigen::MatrixXd FindCollisionPoints( + const VanillaIrisOptions& options, + const geometry::optimization::HPolyhedron& set); + + // Adds a hyperplane tangent to the ellipsoid at points. + void AddHyperplanesAtPoints(const Eigen::Ref& points, + const VanillaIrisOptions& options, + geometry::optimization::HPolyhedron* set) const; +}; + +} // namespace planning +} // namespace drake