From 0cc137a1878fa9e4f6cd35b7003c7f1ad76f402f Mon Sep 17 00:00:00 2001 From: amice Date: Wed, 4 Sep 2024 13:41:22 -0400 Subject: [PATCH 1/5] same point and ellipsoid done --- planning/iris/BUILD.bazel | 69 +++++++++++++++ planning/iris/c_iris.cc | 5 ++ planning/iris/c_iris.h | 14 ++++ planning/iris/internal/BUILD.bazel | 34 ++++++++ ..._via_collisions_and_ellipsoid_interface.cc | 60 +++++++++++++ ...s_via_collisions_and_ellipsoid_interface.h | 64 ++++++++++++++ ...lisions_and_ellipsoid_interface_options.cc | 1 + ...llisions_and_ellipsoid_interface_options.h | 84 +++++++++++++++++++ planning/iris/iris_interface.cc | 36 ++++++++ planning/iris/iris_interface.h | 64 ++++++++++++++ planning/iris/iris_interface_options.cc | 5 ++ planning/iris/iris_interface_options.h | 34 ++++++++ planning/iris/iris_np.cc | 3 + planning/iris/iris_np2.cc | 5 ++ planning/iris/iris_np2.h | 17 ++++ planning/iris/iris_zo.cc | 5 ++ planning/iris/iris_zo.h | 14 ++++ 17 files changed, 514 insertions(+) create mode 100644 planning/iris/c_iris.cc create mode 100644 planning/iris/c_iris.h create mode 100644 planning/iris/internal/BUILD.bazel create mode 100644 planning/iris/internal/iris_via_collisions_and_ellipsoid_interface.cc create mode 100644 planning/iris/internal/iris_via_collisions_and_ellipsoid_interface.h create mode 100644 planning/iris/internal/iris_via_collisions_and_ellipsoid_interface_options.cc create mode 100644 planning/iris/internal/iris_via_collisions_and_ellipsoid_interface_options.h create mode 100644 planning/iris/iris_interface.cc create mode 100644 planning/iris/iris_interface.h create mode 100644 planning/iris/iris_interface_options.cc create mode 100644 planning/iris/iris_interface_options.h create mode 100644 planning/iris/iris_np.cc create mode 100644 planning/iris/iris_np2.cc create mode 100644 planning/iris/iris_np2.h create mode 100644 planning/iris/iris_zo.cc create mode 100644 planning/iris/iris_zo.h diff --git a/planning/iris/BUILD.bazel b/planning/iris/BUILD.bazel index d14ec172c037..e932f0c9d098 100644 --- a/planning/iris/BUILD.bazel +++ b/planning/iris/BUILD.bazel @@ -37,6 +37,75 @@ 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_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:collision_checker", + "//planning/iris/internal:iris_via_collisions_and_ellipsoid_interface", + "//planning/iris/internal: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:collision_checker", + "//planning/iris/internal:iris_via_collisions_and_ellipsoid_interface", + "//planning/iris/internal: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/internal:iris_via_collisions_and_ellipsoid_interface", + "//planning/iris/internal: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..10a89995dcef --- /dev/null +++ b/planning/iris/c_iris.cc @@ -0,0 +1,5 @@ +// +// Created by amice on 9/4/24. +// + +#include "c_iris.h" diff --git a/planning/iris/c_iris.h b/planning/iris/c_iris.h new file mode 100644 index 000000000000..00b131be4c69 --- /dev/null +++ b/planning/iris/c_iris.h @@ -0,0 +1,14 @@ +// +// Created by amice on 9/4/24. +// + +#ifndef DRAKE_PLANNING_IRIS_C_IRIS_H_ +#define DRAKE_PLANNING_IRIS_C_IRIS_H_ + + +class c_iris { + +}; + + +#endif //DRAKE_PLANNING_IRIS_C_IRIS_H_ diff --git a/planning/iris/internal/BUILD.bazel b/planning/iris/internal/BUILD.bazel new file mode 100644 index 000000000000..1c3bc31a29fa --- /dev/null +++ b/planning/iris/internal/BUILD.bazel @@ -0,0 +1,34 @@ +load("//tools/lint:lint.bzl", "add_lint_tests") +load( + "//tools/skylark:drake_cc.bzl", + "drake_cc_googletest", + "drake_cc_library", + "drake_cc_package_library", +) +load( + "//tools/skylark:test_tags.bzl", + "mosek_test_tags", +) + +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/internal:iris_via_collisions_and_ellipsoid_interface_options", + "//planning/iris:iris_interface", + "//planning/iris:iris_interface_options" + ], +) +# test +add_lint_tests(enable_clang_format_lint = False) diff --git a/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface.cc b/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface.cc new file mode 100644 index 000000000000..ce4380df3de4 --- /dev/null +++ b/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface.cc @@ -0,0 +1,60 @@ +#include "planning/iris/internal/iris_via_collisions_and_ellipsoid_interface.h" + +#include "planning/iris/internal/iris_via_collisions_and_ellipsoid_interface_options.h" + +#include "drake/common/drake_throw.h" +#include "drake/geometry/optimization/hyperellipsoid.h" + +namespace drake { +namespace planning { +namespace internal { +IrisViaCollisionsAndEllipsoidInterface::IrisViaCollisionsAndEllipsoidInterface( + const CollisionChecker& checker) + : IrisInterface(checker){ + // TODO complete constructor. This will essentially implement + // MakeIrisObstacles from geometry/optimization/iris.h + }; + +bool IrisViaCollisionsAndEllipsoidInterface:: + HPolyhedronIsCollisionFreeViaUnadaptiveTest( + const geometry::optimization::HPolyhedron& set, + const IrisViaCollisionsAndEllipsoidInterfaceOptions& options) const { + unused(set); + unused(options); + // TODO implement + throw std::logic_error("Unimplemented"); + return true; +}; + +void IrisViaCollisionsAndEllipsoidInterface::DoAddPlanesToSet( + 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 IrisViaCollisionsAndEllipsoidInterface::DoUpdateMetric( + const IrisViaCollisionsAndEllipsoidInterfaceOptions& options, + const geometry::optimization::HPolyhedron& set) { + unused(options); + ellipsoid_metric = set.MaximumVolumeInscribedEllipsoid(); +} + +void IrisViaCollisionsAndEllipsoidInterface::DoSetup( + const IrisViaCollisionsAndEllipsoidInterfaceOptions& options, + geometry::optimization::HPolyhedron* set) { + unused(set); + DRAKE_THROW_UNLESS(options.seed.rows() == checker_->plant().num_positions()); + for (int i = 0; i < checker_->num_allocated_contexts(); ++i) { + checker_->UpdatePositions(options.seed, i); + } + const double kEpsilonEllipsoid = 1e-2; + ellipsoid_metric = options.starting_ellipse.value_or( + geometry::optimization::Hyperellipsoid::MakeHypersphere(kEpsilonEllipsoid, + options.seed)); +} +} // namespace internal +} // namespace planning +} // namespace drake \ No newline at end of file diff --git a/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface.h b/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface.h new file mode 100644 index 000000000000..2c16530c159d --- /dev/null +++ b/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface.h @@ -0,0 +1,64 @@ +#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/internal/iris_via_collisions_and_ellipsoid_interface_options.h" +#include "drake/planning/iris/iris_interface.h" +#include "drake/planning/iris/iris_interface_options.h" + +namespace drake { +namespace planning { +namespace internal { + +// SFINAE that the options subclass must derive from the IrisInterfaceOptions. +template ::value>> +class IrisViaCollisionsAndEllipsoidInterface + : public IrisInterface< + IrisViaCollisionsAndEllipsoidInterfaceOptionsSubclass> { + public: + explicit IrisViaCollisionsAndEllipsoidInterface( + const CollisionChecker& checker); + + virtual ~IrisViaCollisionsAndEllipsoidInterface(); + + protected: + /** 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 DoAddPlanesToSet( + const IrisViaCollisionsAndEllipsoidInterfaceOptions& options, + geometry::optimization::HPolyhedron* set); + + void DoUpdateMetric( + const IrisViaCollisionsAndEllipsoidInterfaceOptions& options, + const geometry::optimization::HPolyhedron& set); + + void DoSetup(const IrisViaCollisionsAndEllipsoidInterfaceOptions& options, + geometry::optimization::HPolyhedron* set); + bool HPolyhedronIsCollisionFreeViaUnadaptiveTest( + const geometry::optimization::HPolyhedron& set, + const IrisViaCollisionsAndEllipsoidInterfaceOptions& options) const; + + geometry::optimization::Hyperellipsoid ellipsoid_metric; +}; + +} // namespace internal +} // namespace planning +} // namespace drake \ No newline at end of file diff --git a/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface_options.cc b/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface_options.cc new file mode 100644 index 000000000000..c7d23389b693 --- /dev/null +++ b/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface_options.cc @@ -0,0 +1 @@ +#include "drake/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface_options.h" \ No newline at end of file diff --git a/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface_options.h b/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface_options.h new file mode 100644 index 000000000000..2c64c34b99e3 --- /dev/null +++ b/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface_options.h @@ -0,0 +1,84 @@ +#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 : 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_interface.cc b/planning/iris/iris_interface.cc new file mode 100644 index 000000000000..665eb9eecf06 --- /dev/null +++ b/planning/iris/iris_interface.cc @@ -0,0 +1,36 @@ +#include "drake/planning/iris/iris_interface.h" + +#include "drake/planning/collision_checker.h" + +namespace drake { +namespace planning { +// using geometry::optimization::HPolyhedron; +// +//// template +// IrisInterface::IrisInterface(const CollisionChecker& checker) +// : checker_{std::move(checker.Clone())} {}; +// +//// template +//// IrisInterface::IrisInterface( +//// const CollisionChecker& checker) +//// : checker_{std::move(checker.Clone())} {}; +// +// template +// HPolyhedron IrisInterface::BuildSet( +// const IrisInterfaceOptionsSubclass& options) const { +// HPolyhedron set = options.domain.value_or( +// HPolyhedron::MakeBox(checker_->plant().GetPositionLowerLimits(), +// checker_->plant().GetPositionUpperLimits())); +// DRAKE_THROW_UNLESS(set.IsBounded()); +// +// DoSetup(options, &set); +// +// while (!options.termination_function(set)) { +// DoUpdateMetric(options, set); +// DoAddPlanesToSet(options, &set); +// } +// return set; +//} + +} // namespace planning +} // namespace drake diff --git a/planning/iris/iris_interface.h b/planning/iris/iris_interface.h new file mode 100644 index 000000000000..6282c70070a0 --- /dev/null +++ b/planning/iris/iris_interface.h @@ -0,0 +1,64 @@ +#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. + */ + 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()); + + DoSetup(options, &set); + + while (!options.termination_function(set)) { + DoUpdateMetric(options, set); + DoAddPlanesToSet(options, &set); + } + return set; + }; + + protected: + explicit 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 DoAddPlanesToSet(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; + + 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..1599d9ccbf7c --- /dev/null +++ b/planning/iris/iris_interface_options.cc @@ -0,0 +1,5 @@ +// +// Created by amice on 9/3/24. +// + +#include "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..ed8d3a7c0da0 --- /dev/null +++ b/planning/iris/iris_interface_options.h @@ -0,0 +1,34 @@ +#pragma once + +#include "drake/common/random.h" +#include "drake/geometry/optimization/hpolyhedron.h" +#include "drake/geometry/meshcat.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_function_; + + /** 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..b5ff67f92d8a --- /dev/null +++ b/planning/iris/iris_np.cc @@ -0,0 +1,3 @@ +// +// Created by amice on 9/4/24. +// diff --git a/planning/iris/iris_np2.cc b/planning/iris/iris_np2.cc new file mode 100644 index 000000000000..9d5bc432a5e2 --- /dev/null +++ b/planning/iris/iris_np2.cc @@ -0,0 +1,5 @@ +// +// Created by amice on 9/4/24. +// + +#include "iris_np2.h" diff --git a/planning/iris/iris_np2.h b/planning/iris/iris_np2.h new file mode 100644 index 000000000000..a925b3d438ee --- /dev/null +++ b/planning/iris/iris_np2.h @@ -0,0 +1,17 @@ +#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/internal/iris_via_collisions_and_ellipsoid_interface.h" +#include "drake/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface_options.h" + +namespace drake { +namespace planning { + +class iris_np2 {}; + +} +} \ 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..3be01972050b --- /dev/null +++ b/planning/iris/iris_zo.cc @@ -0,0 +1,5 @@ +// +// Created by amice on 9/4/24. +// + +#include "iris_zo.h" diff --git a/planning/iris/iris_zo.h b/planning/iris/iris_zo.h new file mode 100644 index 000000000000..493ac1cb1dd6 --- /dev/null +++ b/planning/iris/iris_zo.h @@ -0,0 +1,14 @@ +// +// Created by amice on 9/4/24. +// + +#ifndef DRAKE_PLANNING_IRIS_ZO_H_ +#define DRAKE_PLANNING_IRIS_ZO_H_ + + +class iris_zo { + +}; + + +#endif //DRAKE_PLANNING_IRIS_ZO_H_ From 9e8a0e4c7efae683f2221b289e8c42ac870d13a3 Mon Sep 17 00:00:00 2001 From: amice Date: Wed, 4 Sep 2024 14:19:23 -0400 Subject: [PATCH 2/5] same point and ellipsoid done --- ...s_via_collisions_and_ellipsoid_interface.h | 6 ++-- planning/iris/iris_np.h | 3 ++ planning/iris/iris_np2.cc | 30 ++++++++++++++++--- planning/iris/iris_np2.h | 28 +++++++++++++---- 4 files changed, 54 insertions(+), 13 deletions(-) create mode 100644 planning/iris/iris_np.h diff --git a/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface.h b/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface.h index 2c16530c159d..ffc2e05944cf 100644 --- a/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface.h +++ b/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface.h @@ -22,12 +22,12 @@ class IrisViaCollisionsAndEllipsoidInterface : public IrisInterface< IrisViaCollisionsAndEllipsoidInterfaceOptionsSubclass> { public: - explicit IrisViaCollisionsAndEllipsoidInterface( - const CollisionChecker& checker); - virtual ~IrisViaCollisionsAndEllipsoidInterface(); protected: + explicit IrisViaCollisionsAndEllipsoidInterface( + const CollisionChecker& checker); + /** Each column of the returned matrix is a point in set which contains a * collision */ virtual Eigen::MatrixXd FindCollisionPoints( diff --git a/planning/iris/iris_np.h b/planning/iris/iris_np.h new file mode 100644 index 000000000000..b5ff67f92d8a --- /dev/null +++ b/planning/iris/iris_np.h @@ -0,0 +1,3 @@ +// +// Created by amice on 9/4/24. +// diff --git a/planning/iris/iris_np2.cc b/planning/iris/iris_np2.cc index 9d5bc432a5e2..932abfd87fcf 100644 --- a/planning/iris/iris_np2.cc +++ b/planning/iris/iris_np2.cc @@ -1,5 +1,27 @@ -// -// Created by amice on 9/4/24. -// +#include "drake/planning/iris/iris_np2.h" -#include "iris_np2.h" +namespace drake { +namespace planning { + +IrisNp2::IrisNp2(const CollisionChecker& checker) + : IrisViaCollisionsAndEllipsoidInterface(checker); + +IrisNp2::FindCollisionPoints( + const IrisViaCollisionsAndEllipsoidInterfaceOptions& options, + const geometry::optimization::HPolyhedron& set) { + unused(options); + unused(set); + throw std::logic_error("unimplemented"); + return Eigen::VectorXd::Zeros(checker_->plant().num_positions()); +} + +IrisNp2::AddHyperplanesAtPoints( + const Eigen::Ref& points, + const IrisViaCollisionsAndEllipsoidInterfaceOptions& 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 index a925b3d438ee..53efaabc921c 100644 --- a/planning/iris/iris_np2.h +++ b/planning/iris/iris_np2.h @@ -1,8 +1,5 @@ #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/internal/iris_via_collisions_and_ellipsoid_interface.h" @@ -11,7 +8,26 @@ namespace drake { namespace planning { -class iris_np2 {}; +struct IrisNp2Options : IrisViaCollisionsAndEllipsoidInterfaceOptions { + // TODO add all the unique options. +}; + +class IrisNp2 : IrisViaCollisionsAndEllipsoidInterface< + IrisViaCollisionsAndEllipsoidInterfaceOptions> { + public: + explicit IrisNp2(const CollisionChecker& checker); + + private: + Eigen::VectorXd FindCollisionPoints( + const IrisViaCollisionsAndEllipsoidInterfaceOptions& options, + const geometry::optimization::HPolyhedron& set); + + void AddHyperplanesAtPoints( + const Eigen::Ref& points, + const IrisViaCollisionsAndEllipsoidInterfaceOptions& options, + geometry::optimization::HPolyhedron* set) const; +}; + +} // namespace planning -} -} \ No newline at end of file +} // namespace drake \ No newline at end of file From 0d1fb142039f4fe4921e2025c687731ce8a27bb8 Mon Sep 17 00:00:00 2001 From: amice Date: Wed, 4 Sep 2024 17:32:10 -0400 Subject: [PATCH 3/5] framework complete. lots of unimplementeds| --- planning/iris/BUILD.bazel | 65 +++++++++-- planning/iris/c_iris.cc | 46 +++++++- planning/iris/c_iris.h | 68 +++++++++-- planning/iris/internal/BUILD.bazel | 34 ------ ..._via_collisions_and_ellipsoid_interface.cc | 60 ---------- ...s_via_collisions_and_ellipsoid_interface.h | 64 ----------- ...lisions_and_ellipsoid_interface_options.cc | 1 - planning/iris/iris_interface.cc | 35 ------ planning/iris/iris_interface.h | 52 +++++++-- planning/iris/iris_interface_options.h | 8 +- planning/iris/iris_np.cc | 29 ++++- planning/iris/iris_np.h | 43 ++++++- planning/iris/iris_np2.cc | 19 ++-- planning/iris/iris_np2.h | 36 +++--- ..._via_collisions_and_ellipsoid_interface.cc | 1 + ...s_via_collisions_and_ellipsoid_interface.h | 107 ++++++++++++++++++ ...lisions_and_ellipsoid_interface_options.cc | 1 + ...llisions_and_ellipsoid_interface_options.h | 3 +- planning/iris/iris_zo.cc | 29 ++++- planning/iris/iris_zo.h | 41 +++++-- planning/iris/test/tmp.cc | 3 + 21 files changed, 476 insertions(+), 269 deletions(-) delete mode 100644 planning/iris/internal/BUILD.bazel delete mode 100644 planning/iris/internal/iris_via_collisions_and_ellipsoid_interface.cc delete mode 100644 planning/iris/internal/iris_via_collisions_and_ellipsoid_interface.h delete mode 100644 planning/iris/internal/iris_via_collisions_and_ellipsoid_interface_options.cc create mode 100644 planning/iris/iris_via_collisions_and_ellipsoid_interface.cc create mode 100644 planning/iris/iris_via_collisions_and_ellipsoid_interface.h create mode 100644 planning/iris/iris_via_collisions_and_ellipsoid_interface_options.cc rename planning/iris/{internal => }/iris_via_collisions_and_ellipsoid_interface_options.h (97%) create mode 100644 planning/iris/test/tmp.cc diff --git a/planning/iris/BUILD.bazel b/planning/iris/BUILD.bazel index e932f0c9d098..84e5cd5d235a 100644 --- a/planning/iris/BUILD.bazel +++ b/planning/iris/BUILD.bazel @@ -51,6 +51,29 @@ drake_cc_library( ], ) + +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"], @@ -69,9 +92,9 @@ drake_cc_library( srcs = ["iris_np.cc"], hdrs = ["iris_np.h"], deps = [ - "//planning:collision_checker", - "//planning/iris/internal:iris_via_collisions_and_ellipsoid_interface", - "//planning/iris/internal:iris_via_collisions_and_ellipsoid_interface_options", + "//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 = [ @@ -83,9 +106,9 @@ drake_cc_library( srcs = ["iris_np2.cc"], hdrs = ["iris_np2.h"], deps = [ - "//planning:collision_checker", - "//planning/iris/internal:iris_via_collisions_and_ellipsoid_interface", - "//planning/iris/internal:iris_via_collisions_and_ellipsoid_interface_options", + "//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 = [ @@ -98,9 +121,27 @@ drake_cc_library( hdrs = ["iris_zo.h"], deps = [ "//planning:collision_checker", - "//planning/iris/internal:iris_via_collisions_and_ellipsoid_interface", - "//planning/iris/internal:iris_via_collisions_and_ellipsoid_interface_options", + "//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 = [ ], @@ -138,4 +179,12 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name="tmp", + deps=[ + ":iris_via_collisions_and_ellipsoid_interface", + ":iris_np2" + ] +) + add_lint_tests() diff --git a/planning/iris/c_iris.cc b/planning/iris/c_iris.cc index 10a89995dcef..b7d94c113336 100644 --- a/planning/iris/c_iris.cc +++ b/planning/iris/c_iris.cc @@ -1,5 +1,43 @@ -// -// Created by amice on 9/4/24. -// +#include "drake/planning/iris/c_iris.h" -#include "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 index 00b131be4c69..a55dc4e5ef2b 100644 --- a/planning/iris/c_iris.h +++ b/planning/iris/c_iris.h @@ -1,14 +1,68 @@ -// -// Created by amice on 9/4/24. -// +#pragma once -#ifndef DRAKE_PLANNING_IRIS_C_IRIS_H_ -#define DRAKE_PLANNING_IRIS_C_IRIS_H_ +#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 { -class c_iris { +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_; +}; -#endif //DRAKE_PLANNING_IRIS_C_IRIS_H_ +} // namespace planning +} // namespace drake diff --git a/planning/iris/internal/BUILD.bazel b/planning/iris/internal/BUILD.bazel deleted file mode 100644 index 1c3bc31a29fa..000000000000 --- a/planning/iris/internal/BUILD.bazel +++ /dev/null @@ -1,34 +0,0 @@ -load("//tools/lint:lint.bzl", "add_lint_tests") -load( - "//tools/skylark:drake_cc.bzl", - "drake_cc_googletest", - "drake_cc_library", - "drake_cc_package_library", -) -load( - "//tools/skylark:test_tags.bzl", - "mosek_test_tags", -) - -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/internal:iris_via_collisions_and_ellipsoid_interface_options", - "//planning/iris:iris_interface", - "//planning/iris:iris_interface_options" - ], -) -# test -add_lint_tests(enable_clang_format_lint = False) diff --git a/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface.cc b/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface.cc deleted file mode 100644 index ce4380df3de4..000000000000 --- a/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface.cc +++ /dev/null @@ -1,60 +0,0 @@ -#include "planning/iris/internal/iris_via_collisions_and_ellipsoid_interface.h" - -#include "planning/iris/internal/iris_via_collisions_and_ellipsoid_interface_options.h" - -#include "drake/common/drake_throw.h" -#include "drake/geometry/optimization/hyperellipsoid.h" - -namespace drake { -namespace planning { -namespace internal { -IrisViaCollisionsAndEllipsoidInterface::IrisViaCollisionsAndEllipsoidInterface( - const CollisionChecker& checker) - : IrisInterface(checker){ - // TODO complete constructor. This will essentially implement - // MakeIrisObstacles from geometry/optimization/iris.h - }; - -bool IrisViaCollisionsAndEllipsoidInterface:: - HPolyhedronIsCollisionFreeViaUnadaptiveTest( - const geometry::optimization::HPolyhedron& set, - const IrisViaCollisionsAndEllipsoidInterfaceOptions& options) const { - unused(set); - unused(options); - // TODO implement - throw std::logic_error("Unimplemented"); - return true; -}; - -void IrisViaCollisionsAndEllipsoidInterface::DoAddPlanesToSet( - 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 IrisViaCollisionsAndEllipsoidInterface::DoUpdateMetric( - const IrisViaCollisionsAndEllipsoidInterfaceOptions& options, - const geometry::optimization::HPolyhedron& set) { - unused(options); - ellipsoid_metric = set.MaximumVolumeInscribedEllipsoid(); -} - -void IrisViaCollisionsAndEllipsoidInterface::DoSetup( - const IrisViaCollisionsAndEllipsoidInterfaceOptions& options, - geometry::optimization::HPolyhedron* set) { - unused(set); - DRAKE_THROW_UNLESS(options.seed.rows() == checker_->plant().num_positions()); - for (int i = 0; i < checker_->num_allocated_contexts(); ++i) { - checker_->UpdatePositions(options.seed, i); - } - const double kEpsilonEllipsoid = 1e-2; - ellipsoid_metric = options.starting_ellipse.value_or( - geometry::optimization::Hyperellipsoid::MakeHypersphere(kEpsilonEllipsoid, - options.seed)); -} -} // namespace internal -} // namespace planning -} // namespace drake \ No newline at end of file diff --git a/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface.h b/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface.h deleted file mode 100644 index ffc2e05944cf..000000000000 --- a/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface.h +++ /dev/null @@ -1,64 +0,0 @@ -#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/internal/iris_via_collisions_and_ellipsoid_interface_options.h" -#include "drake/planning/iris/iris_interface.h" -#include "drake/planning/iris/iris_interface_options.h" - -namespace drake { -namespace planning { -namespace internal { - -// SFINAE that the options subclass must derive from the IrisInterfaceOptions. -template ::value>> -class IrisViaCollisionsAndEllipsoidInterface - : public IrisInterface< - IrisViaCollisionsAndEllipsoidInterfaceOptionsSubclass> { - public: - virtual ~IrisViaCollisionsAndEllipsoidInterface(); - - protected: - explicit IrisViaCollisionsAndEllipsoidInterface( - const CollisionChecker& checker); - - /** 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 DoAddPlanesToSet( - const IrisViaCollisionsAndEllipsoidInterfaceOptions& options, - geometry::optimization::HPolyhedron* set); - - void DoUpdateMetric( - const IrisViaCollisionsAndEllipsoidInterfaceOptions& options, - const geometry::optimization::HPolyhedron& set); - - void DoSetup(const IrisViaCollisionsAndEllipsoidInterfaceOptions& options, - geometry::optimization::HPolyhedron* set); - bool HPolyhedronIsCollisionFreeViaUnadaptiveTest( - const geometry::optimization::HPolyhedron& set, - const IrisViaCollisionsAndEllipsoidInterfaceOptions& options) const; - - geometry::optimization::Hyperellipsoid ellipsoid_metric; -}; - -} // namespace internal -} // namespace planning -} // namespace drake \ No newline at end of file diff --git a/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface_options.cc b/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface_options.cc deleted file mode 100644 index c7d23389b693..000000000000 --- a/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface_options.cc +++ /dev/null @@ -1 +0,0 @@ -#include "drake/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface_options.h" \ No newline at end of file diff --git a/planning/iris/iris_interface.cc b/planning/iris/iris_interface.cc index 665eb9eecf06..338d295cd0df 100644 --- a/planning/iris/iris_interface.cc +++ b/planning/iris/iris_interface.cc @@ -1,36 +1 @@ #include "drake/planning/iris/iris_interface.h" - -#include "drake/planning/collision_checker.h" - -namespace drake { -namespace planning { -// using geometry::optimization::HPolyhedron; -// -//// template -// IrisInterface::IrisInterface(const CollisionChecker& checker) -// : checker_{std::move(checker.Clone())} {}; -// -//// template -//// IrisInterface::IrisInterface( -//// const CollisionChecker& checker) -//// : checker_{std::move(checker.Clone())} {}; -// -// template -// HPolyhedron IrisInterface::BuildSet( -// const IrisInterfaceOptionsSubclass& options) const { -// HPolyhedron set = options.domain.value_or( -// HPolyhedron::MakeBox(checker_->plant().GetPositionLowerLimits(), -// checker_->plant().GetPositionUpperLimits())); -// DRAKE_THROW_UNLESS(set.IsBounded()); -// -// DoSetup(options, &set); -// -// while (!options.termination_function(set)) { -// DoUpdateMetric(options, set); -// DoAddPlanesToSet(options, &set); -// } -// return set; -//} - -} // namespace planning -} // namespace drake diff --git a/planning/iris/iris_interface.h b/planning/iris/iris_interface.h index 6282c70070a0..b366712c2f8b 100644 --- a/planning/iris/iris_interface.h +++ b/planning/iris/iris_interface.h @@ -16,7 +16,9 @@ template ::value>> class IrisInterface { /** - * A class for implementing various Iris-type algorithms. + * 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); @@ -30,17 +32,45 @@ class IrisInterface { checker_->plant().GetPositionUpperLimits())); DRAKE_THROW_UNLESS(set.IsBounded()); - DoSetup(options, &set); + Setup(options, &set); - while (!options.termination_function(set)) { - DoUpdateMetric(options, set); - DoAddPlanesToSet(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: - explicit IrisInterface(const CollisionChecker& checker) + IrisInterface(const CollisionChecker& checker) : checker_{std::move(checker.Clone())} {}; /** Runs any additional set up code before the set building loop starts. */ @@ -49,14 +79,20 @@ class IrisInterface { /** 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 DoAddPlanesToSet(const IrisInterfaceOptionsSubclass& options, - geometry::optimization::HPolyhedron* set) = 0; + 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_; }; diff --git a/planning/iris/iris_interface_options.h b/planning/iris/iris_interface_options.h index ed8d3a7c0da0..2c5b8f2a252d 100644 --- a/planning/iris/iris_interface_options.h +++ b/planning/iris/iris_interface_options.h @@ -1,19 +1,19 @@ #pragma once #include "drake/common/random.h" -#include "drake/geometry/optimization/hpolyhedron.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}; + int iteration_limit{100}; /** A function which returns true when a set is done being constructed.*/ std::function - termination_function_; + termination_func; /** Solver options used by any internal solvers used in the loop. */ std::optional solver_options; @@ -24,7 +24,7 @@ struct IrisInterfaceOptions { /** A default randomness generator for source of randomness which may occur in * the algorithm. */ - RandomGenerator random_generator_{0}; + RandomGenerator random_generator{0}; /** Passing a meshcat instance may enable debugging visualizations. */ std::shared_ptr meshcat{}; diff --git a/planning/iris/iris_np.cc b/planning/iris/iris_np.cc index b5ff67f92d8a..d345c9287663 100644 --- a/planning/iris/iris_np.cc +++ b/planning/iris/iris_np.cc @@ -1,3 +1,26 @@ -// -// Created by amice on 9/4/24. -// +#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 index b5ff67f92d8a..b8da7460c859 100644 --- a/planning/iris/iris_np.h +++ b/planning/iris/iris_np.h @@ -1,3 +1,40 @@ -// -// Created by amice on 9/4/24. -// +#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 index 932abfd87fcf..7c331acc3849 100644 --- a/planning/iris/iris_np2.cc +++ b/planning/iris/iris_np2.cc @@ -3,21 +3,20 @@ namespace drake { namespace planning { -IrisNp2::IrisNp2(const CollisionChecker& checker) - : IrisViaCollisionsAndEllipsoidInterface(checker); +IrisNp2::IrisNp2(const SceneGraphCollisionChecker& checker) + : IrisViaCollisionsAndEllipsoidInterface(checker){}; -IrisNp2::FindCollisionPoints( - const IrisViaCollisionsAndEllipsoidInterfaceOptions& options, +Eigen::MatrixXd IrisNp2::FindCollisionPoints( + const IrisNp2Options& options, const geometry::optimization::HPolyhedron& set) { - unused(options); - unused(set); + unused(options, set); throw std::logic_error("unimplemented"); - return Eigen::VectorXd::Zeros(checker_->plant().num_positions()); + return Eigen::VectorXd::Zero(checker_->plant().num_positions()); } -IrisNp2::AddHyperplanesAtPoints( - const Eigen::Ref& points, - const IrisViaCollisionsAndEllipsoidInterfaceOptions& options, +void IrisNp2::AddHyperplanesAtPoints( + const Eigen::Ref& points, + const IrisNp2Options& options, geometry::optimization::HPolyhedron* set) const { unused(points, options, set); throw std::logic_error("unimplemented"); diff --git a/planning/iris/iris_np2.h b/planning/iris/iris_np2.h index 53efaabc921c..00f00a210df3 100644 --- a/planning/iris/iris_np2.h +++ b/planning/iris/iris_np2.h @@ -1,33 +1,39 @@ #pragma once #include "drake/geometry/optimization/hpolyhedron.h" -#include "drake/planning/collision_checker.h" -#include "drake/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface.h" -#include "drake/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface_options.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 : IrisViaCollisionsAndEllipsoidInterfaceOptions { +struct IrisNp2Options + : internal::IrisViaCollisionsAndEllipsoidInterfaceOptions { // TODO add all the unique options. }; -class IrisNp2 : IrisViaCollisionsAndEllipsoidInterface< - IrisViaCollisionsAndEllipsoidInterfaceOptions> { +class IrisNp2 final + : public internal::IrisViaCollisionsAndEllipsoidInterface { public: - explicit IrisNp2(const CollisionChecker& checker); + explicit IrisNp2(const SceneGraphCollisionChecker& checker); private: - Eigen::VectorXd FindCollisionPoints( - const IrisViaCollisionsAndEllipsoidInterfaceOptions& options, + // 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 IrisViaCollisionsAndEllipsoidInterfaceOptions& options, - geometry::optimization::HPolyhedron* set) const; + void AddHyperplanesAtPoints(const Eigen::Ref& points, + const IrisNp2Options& options, + geometry::optimization::HPolyhedron* set) const; }; } // namespace planning - -} // namespace drake \ No newline at end of file +} // 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/internal/iris_via_collisions_and_ellipsoid_interface_options.h b/planning/iris/iris_via_collisions_and_ellipsoid_interface_options.h similarity index 97% rename from planning/iris/internal/iris_via_collisions_and_ellipsoid_interface_options.h rename to planning/iris/iris_via_collisions_and_ellipsoid_interface_options.h index 2c64c34b99e3..92dbb6682e47 100644 --- a/planning/iris/internal/iris_via_collisions_and_ellipsoid_interface_options.h +++ b/planning/iris/iris_via_collisions_and_ellipsoid_interface_options.h @@ -12,7 +12,8 @@ namespace drake { namespace planning { namespace internal { -struct IrisViaCollisionsAndEllipsoidInterfaceOptions : IrisInterfaceOptions { +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. diff --git a/planning/iris/iris_zo.cc b/planning/iris/iris_zo.cc index 3be01972050b..564520cf8fe9 100644 --- a/planning/iris/iris_zo.cc +++ b/planning/iris/iris_zo.cc @@ -1,5 +1,26 @@ -// -// Created by amice on 9/4/24. -// +#include "drake/planning/iris/iris_zo.h" -#include "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 index 493ac1cb1dd6..1731959fd47a 100644 --- a/planning/iris/iris_zo.h +++ b/planning/iris/iris_zo.h @@ -1,14 +1,39 @@ -// -// Created by amice on 9/4/24. -// +#pragma once -#ifndef DRAKE_PLANNING_IRIS_ZO_H_ -#define DRAKE_PLANNING_IRIS_ZO_H_ +#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 { -class iris_zo { - +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; +}; -#endif //DRAKE_PLANNING_IRIS_ZO_H_ +} // namespace planning +} // namespace drake diff --git a/planning/iris/test/tmp.cc b/planning/iris/test/tmp.cc new file mode 100644 index 000000000000..b5ff67f92d8a --- /dev/null +++ b/planning/iris/test/tmp.cc @@ -0,0 +1,3 @@ +// +// Created by amice on 9/4/24. +// From fa7f73047757e6ed4660f11e8b250fd615989300 Mon Sep 17 00:00:00 2001 From: amice Date: Wed, 4 Sep 2024 17:42:21 -0400 Subject: [PATCH 4/5] add vanilla iris --- planning/iris/BUILD.bazel | 14 ++++++++++++ planning/iris/vanilla_iris.cc | 21 +++++++++++++++++ planning/iris/vanilla_iris.h | 43 +++++++++++++++++++++++++++++++++++ 3 files changed, 78 insertions(+) create mode 100644 planning/iris/vanilla_iris.cc create mode 100644 planning/iris/vanilla_iris.h diff --git a/planning/iris/BUILD.bazel b/planning/iris/BUILD.bazel index 84e5cd5d235a..4c86c795200a 100644 --- a/planning/iris/BUILD.bazel +++ b/planning/iris/BUILD.bazel @@ -147,6 +147,20 @@ drake_cc_library( ], ) +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/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 From 1965a2e479d3508ae9d0749322810a9cf21a5a24 Mon Sep 17 00:00:00 2001 From: amice Date: Wed, 4 Sep 2024 17:47:32 -0400 Subject: [PATCH 5/5] some cleanup --- planning/iris/BUILD.bazel | 8 -------- planning/iris/iris_interface_options.cc | 6 +----- planning/iris/test/tmp.cc | 3 --- 3 files changed, 1 insertion(+), 16 deletions(-) delete mode 100644 planning/iris/test/tmp.cc diff --git a/planning/iris/BUILD.bazel b/planning/iris/BUILD.bazel index 4c86c795200a..6d1acec085b0 100644 --- a/planning/iris/BUILD.bazel +++ b/planning/iris/BUILD.bazel @@ -193,12 +193,4 @@ drake_cc_googletest( ], ) -drake_cc_googletest( - name="tmp", - deps=[ - ":iris_via_collisions_and_ellipsoid_interface", - ":iris_np2" - ] -) - add_lint_tests() diff --git a/planning/iris/iris_interface_options.cc b/planning/iris/iris_interface_options.cc index 1599d9ccbf7c..8b281991c516 100644 --- a/planning/iris/iris_interface_options.cc +++ b/planning/iris/iris_interface_options.cc @@ -1,5 +1 @@ -// -// Created by amice on 9/3/24. -// - -#include "iris_interface_options.h" +#include "drake/planning/iris/iris_interface_options.h" diff --git a/planning/iris/test/tmp.cc b/planning/iris/test/tmp.cc deleted file mode 100644 index b5ff67f92d8a..000000000000 --- a/planning/iris/test/tmp.cc +++ /dev/null @@ -1,3 +0,0 @@ -// -// Created by amice on 9/4/24. -//