From 8555da57726824485191225e945b0f4253fa9df6 Mon Sep 17 00:00:00 2001 From: Peter Date: Wed, 13 Nov 2024 10:56:29 -0500 Subject: [PATCH 01/13] Iris ZO pulled over from devel branch --- bindings/pydrake/planning/BUILD.bazel | 1 + bindings/pydrake/planning/planning_py.cc | 1 + bindings/pydrake/planning/planning_py.h | 3 + .../pydrake/planning/planning_py_iris_zo.cc | 98 ++++ planning/iris/BUILD.bazel | 21 + planning/iris/iris_zo.cc | 507 ++++++++++++++++ planning/iris/iris_zo.h | 171 ++++++ planning/iris/test/iris_zo_test.cc | 549 ++++++++++++++++++ 8 files changed, 1351 insertions(+) create mode 100644 bindings/pydrake/planning/planning_py_iris_zo.cc create mode 100644 planning/iris/iris_zo.cc create mode 100644 planning/iris/iris_zo.h create mode 100644 planning/iris/test/iris_zo_test.cc diff --git a/bindings/pydrake/planning/BUILD.bazel b/bindings/pydrake/planning/BUILD.bazel index 23168b282df5..917e10457e12 100644 --- a/bindings/pydrake/planning/BUILD.bazel +++ b/bindings/pydrake/planning/BUILD.bazel @@ -41,6 +41,7 @@ drake_pybind_library( "planning_py_trajectory_optimization.cc", "planning_py_visibility_graph.cc", "planning_py_iris_from_clique_cover.cc", + "planning_py_iris_zo.cc", "planning_py_zmp_planner.cc", ], package_info = PACKAGE_INFO, diff --git a/bindings/pydrake/planning/planning_py.cc b/bindings/pydrake/planning/planning_py.cc index 87edf2d1301f..2d4884b46c8d 100644 --- a/bindings/pydrake/planning/planning_py.cc +++ b/bindings/pydrake/planning/planning_py.cc @@ -28,6 +28,7 @@ and/or trajectories of dynamical systems. internal::DefinePlanningTrajectoryOptimization(m); internal::DefinePlanningVisibilityGraph(m); internal::DefinePlanningIrisFromCliqueCover(m); + internal::DefinePlanningIrisZo(m); internal::DefinePlanningZmpPlanner(m); } diff --git a/bindings/pydrake/planning/planning_py.h b/bindings/pydrake/planning/planning_py.h index cd7c6b66d69e..e0a1cfda22e1 100644 --- a/bindings/pydrake/planning/planning_py.h +++ b/bindings/pydrake/planning/planning_py.h @@ -26,6 +26,9 @@ void DefinePlanningGraphAlgorithms(py::module m); /* Defines bindings per planning_py_iris_from_clique_cover.cc. */ void DefinePlanningIrisFromCliqueCover(py::module m); +/* Defines bindings per planning_py_iris_zo.cc. */ +void DefinePlanningIrisZo(py::module m); + /* Defines bindings per planning_py_robot_diagram.cc. */ void DefinePlanningRobotDiagram(py::module m); diff --git a/bindings/pydrake/planning/planning_py_iris_zo.cc b/bindings/pydrake/planning/planning_py_iris_zo.cc new file mode 100644 index 000000000000..1cc4e742a428 --- /dev/null +++ b/bindings/pydrake/planning/planning_py_iris_zo.cc @@ -0,0 +1,98 @@ +#include "drake/bindings/pydrake/documentation_pybind.h" +#include "drake/bindings/pydrake/planning/planning_py.h" +#include "drake/bindings/pydrake/pydrake_pybind.h" +#include "drake/planning/iris/iris_zo.h" + +namespace drake { +namespace pydrake { +namespace internal { + +void DefinePlanningIrisZo(py::module m) { + // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. + using namespace drake::planning; + constexpr auto& doc = pydrake_doc.drake.planning; + + // IrisZoOptions + const auto& cls_doc = doc.IrisZoOptions; + py::class_(m, "IrisZoOptions", cls_doc.doc) + .def(py::init<>()) + .def_readwrite("num_particles", &IrisZoOptions::num_particles, + cls_doc.num_particles.doc) + .def_readwrite("tau", &IrisZoOptions::tau, cls_doc.tau.doc) + .def_readwrite("delta", &IrisZoOptions::delta, cls_doc.delta.doc) + .def_readwrite("epsilon", &IrisZoOptions::epsilon, cls_doc.epsilon.doc) + .def_readwrite("containment_points", &IrisZoOptions::containment_points, + cls_doc.containment_points.doc) + .def_readwrite("force_containment_points", + &IrisZoOptions::force_containment_points, + cls_doc.force_containment_points.doc) + .def_readwrite("max_iterations", &IrisZoOptions::max_iterations, + cls_doc.max_iterations.doc) + .def_readwrite("max_iterations_separating_planes", + &IrisZoOptions::max_iterations_separating_planes, + cls_doc.max_iterations_separating_planes.doc) + .def_readwrite("max_separating_planes_per_iteration", + &IrisZoOptions::max_separating_planes_per_iteration, + cls_doc.max_separating_planes_per_iteration.doc) + .def_readwrite("bisection_steps", &IrisZoOptions::bisection_steps, + cls_doc.bisection_steps.doc) + .def_readwrite( + "parallelize", &IrisZoOptions::parallelize, cls_doc.parallelize.doc) + .def_readwrite("verbose", &IrisZoOptions::verbose, cls_doc.verbose.doc) + .def_readwrite("require_sample_point_is_contained", + &IrisZoOptions::require_sample_point_is_contained, + cls_doc.require_sample_point_is_contained.doc) + .def_readwrite("configuration_space_margin", + &IrisZoOptions::configuration_space_margin, + cls_doc.configuration_space_margin.doc) + .def_readwrite("termination_threshold", + &IrisZoOptions::termination_threshold, + cls_doc.termination_threshold.doc) + .def_readwrite("relative_termination_threshold", + &IrisZoOptions::relative_termination_threshold, + cls_doc.relative_termination_threshold.doc) + .def_readwrite( + "random_seed", &IrisZoOptions::random_seed, cls_doc.random_seed.doc) + .def_readwrite("mixing_steps", &IrisZoOptions::mixing_steps, + cls_doc.mixing_steps.doc) + .def("__repr__", [](const IrisZoOptions& self) { + return py::str( + "IrisZoOptions(" + "num_particles={}, " + "tau={}, " + "delta={}, " + "epsilon={}, " + "force_containment_points={}, " + "num_consecutive_failures={}, " + "max_iterations={}, " + "max_iterations_separating_planes={}, " + "max_separating_planes_per_iteration={}, " + "bisection_steps={}, " + "parallelize={}, " + "verbose={}, " + "require_sample_point_is_contained={}, " + "configuration_space_margin={}, " + "termination_threshold={}, " + "relative_termination_threshold={}, " + "random_seed={}, " + "mixing_steps={}, " + ")") + .format(self.num_particles, self.tau, self.delta, self.epsilon, + self.containment_points, self.force_containment_points, + self.max_iterations, self.max_iterations_separating_planes, + self.max_separating_planes_per_iteration, self.bisection_steps, + self.parallelize, self.verbose, + self.require_sample_point_is_contained, + self.configuration_space_margin, self.termination_threshold, + self.relative_termination_threshold, self.random_seed, + self.mixing_steps); + }); + + m.def("IrisZo", &IrisZo, py::arg("checker"), py::arg("starting_ellipsoid"), + py::arg("doma din"), py::arg("options") = IrisZoOptions(), + doc.IrisZo.doc); +} + +} // namespace internal +} // namespace pydrake +} // namespace drake diff --git a/planning/iris/BUILD.bazel b/planning/iris/BUILD.bazel index d14ec172c037..77c2a0835b2f 100644 --- a/planning/iris/BUILD.bazel +++ b/planning/iris/BUILD.bazel @@ -14,6 +14,7 @@ drake_cc_package_library( visibility = ["//visibility:public"], deps = [ ":iris_from_clique_cover", + ":iris_zo", ], ) @@ -37,6 +38,26 @@ drake_cc_library( ], ) +drake_cc_library( + name = "iris_zo", + srcs = ["iris_zo.cc"], + hdrs = ["iris_zo.h"], + deps = [ + "//common:parallelism", + "//geometry:meshcat", + "//geometry/optimization:convex_set", + "//planning:collision_checker", + "//solvers:choose_best_solver", + "//solvers:clarabel_solver", + "//solvers:mathematical_program", + "//solvers:mosek_solver", + "//solvers:solve", + ], + implementation_deps = [ + "@common_robotics_utilities", + ], +) + drake_cc_googletest( name = "iris_from_clique_cover_test", timeout = "moderate", diff --git a/planning/iris/iris_zo.cc b/planning/iris/iris_zo.cc new file mode 100644 index 000000000000..383016317154 --- /dev/null +++ b/planning/iris/iris_zo.cc @@ -0,0 +1,507 @@ +#include "drake/planning/iris/iris_zo.h" + +#include +#include +#include + +#include + +#include "drake/common/fmt_eigen.h" +#include "drake/geometry/optimization/convex_set.h" +#include "drake/geometry/optimization/hpolyhedron.h" +#include "drake/geometry/optimization/vpolytope.h" +#include "drake/solvers/choose_best_solver.h" +#include "drake/solvers/clarabel_solver.h" +#include "drake/solvers/mosek_solver.h" +#include "drake/solvers/solve.h" + +namespace drake { +namespace planning { + +using common_robotics_utilities::parallelism::DegreeOfParallelism; +using common_robotics_utilities::parallelism::DynamicParallelForIndexLoop; +using common_robotics_utilities::parallelism::ParallelForBackend; +using common_robotics_utilities::parallelism::StaticParallelForIndexLoop; +using geometry::Meshcat; +using geometry::Sphere; +using geometry::optimization::HPolyhedron; +using geometry::optimization::Hyperellipsoid; +using geometry::optimization::VPolytope; +using math::RigidTransform; +using solvers::MathematicalProgram; + +namespace { + +using values_t = std::vector; +using index_t = std::vector; + +index_t argsort(values_t const& values) { + index_t index(values.size()); + std::iota(index.begin(), index.end(), 0); + std::sort(index.begin(), index.end(), [&values](uint8_t a, uint8_t b) { + return values[a] < values[b]; + }); + return index; +} + +Eigen::VectorXd compute_face_tangent_to_dist_cvxh( + const Hyperellipsoid& E, const Eigen::Ref& point, + const VPolytope& cvxh_vpoly) { + Eigen::VectorXd a_face = E.A().transpose() * E.A() * (point - E.center()); + double b_face = a_face.transpose() * point; + double worst_case = + (a_face.transpose() * cvxh_vpoly.vertices()).maxCoeff() - b_face; + // Return standard iris face if either the face does not chopp off any + // containment points or collision is in convex hull. + if (cvxh_vpoly.PointInSet(point) || worst_case <= 0) { + return a_face; + } else { + MathematicalProgram prog; + int dim = cvxh_vpoly.ambient_dimension(); + std::vector preferred_solvers{ + solvers::MosekSolver::id(), solvers::ClarabelSolver::id()}; + auto x = prog.NewContinuousVariables(dim); + cvxh_vpoly.AddPointInSetConstraints(&prog, x); + Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(dim, dim); + prog.AddQuadraticErrorCost(identity, point, x); + auto solver = solvers::MakeFirstAvailableSolver(preferred_solvers); + solvers::MathematicalProgramResult result; + solver->Solve(prog, std::nullopt, std::nullopt, &result); + DRAKE_THROW_UNLESS(result.is_success()); + a_face = point - result.GetSolution(x); + return a_face; + } +} + +int unadaptive_test_samples(double p, double delta, double tau) { + return static_cast(-2 * std::log(delta) / (tau * tau * p) + 0.5); +} + +} // namespace + +HPolyhedron IrisZo(const planning::CollisionChecker& checker, + const Hyperellipsoid& starting_ellipsoid, + const HPolyhedron& domain, const IrisZoOptions& options) { + auto start = std::chrono::high_resolution_clock::now(); + const auto parallelism = Parallelism::Max(); + const int num_threads_to_use = + checker.SupportsParallelChecking() && options.parallelize + ? std::min(parallelism.num_threads(), + checker.num_allocated_contexts()) + : 1; + + RandomGenerator generator(options.random_seed); + + const Eigen::VectorXd starting_ellipsoid_center = starting_ellipsoid.center(); + + Hyperellipsoid current_ellipsoid = starting_ellipsoid; + Eigen::VectorXd current_ellipsoid_center = starting_ellipsoid.center(); + Eigen::MatrixXd current_ellipsoid_A = starting_ellipsoid.A(); + double previous_volume = 0; + + const int dim = starting_ellipsoid.ambient_dimension(); + int current_num_faces = domain.A().rows(); + + DRAKE_THROW_UNLESS(num_threads_to_use > 0); + DRAKE_THROW_UNLESS(domain.ambient_dimension() == dim); + DRAKE_THROW_UNLESS(domain.IsBounded()); + DRAKE_THROW_UNLESS(domain.PointInSet(current_ellipsoid_center)); + + VPolytope cvxh_vpoly(options.containment_points); + if (options.force_containment_points) { + DRAKE_THROW_UNLESS(domain.ambient_dimension() == + options.containment_points.rows()); + cvxh_vpoly = cvxh_vpoly.GetMinimalRepresentation(); + + std::vector cont_vec; + cont_vec.reserve((options.containment_points.cols())); + + for (int col = 0; col < options.containment_points.cols(); ++col) { + Eigen::VectorXd conf = options.containment_points.col(col); + cont_vec.emplace_back(conf); + } + + std::vector containment_point_col_free = + checker.CheckConfigsCollisionFree(cont_vec, parallelism); + for (const auto col_free : containment_point_col_free) { + if (!col_free) { + throw std::runtime_error( + "One or more containment points are in collision!"); + } + } + } + // For debugging visualization. + Eigen::Vector3d point_to_draw = Eigen::Vector3d::Zero(); + if (options.meshcat && dim <= 3) { + std::string path = "seedpoint"; + options.meshcat->SetObject(path, Sphere(0.06), + geometry::Rgba(0.1, 1, 1, 1.0)); + point_to_draw.head(dim) = current_ellipsoid_center; + options.meshcat->SetTransform(path, RigidTransform(point_to_draw)); + } + + std::vector particles; + + // upper bound on number of particles required if we hit max iterations + double outer_delta_min = + options.delta * 6 / + (M_PI * M_PI * options.max_iterations * options.max_iterations); + + double delta_min = outer_delta_min * 6 / + (M_PI * M_PI * options.max_iterations_separating_planes * + options.max_iterations_separating_planes); + + int N_max = unadaptive_test_samples(options.epsilon, delta_min, options.tau); + + if (options.verbose) { + log()->info( + "IrisZo finding region that is {} collision free with {} certainty " + "using {} particles.", + options.epsilon, 1 - options.delta, options.num_particles); + log()->info("IrisZo worst case test requires {} samples.", N_max); + } + + particles.reserve(N_max); + for (int i = 0; i < N_max; ++i) { + particles.emplace_back(Eigen::VectorXd::Zero(dim)); + } + + int iteration = 0; + HPolyhedron P = domain; + HPolyhedron P_prev = domain; + + // pre-allocate memory for the polyhedron we are going to construct + // TODO(wernerpe): find better solution than hardcoding 300 + Eigen::Matrix A( + P.A().rows() + 300, dim); + Eigen::VectorXd b(P.A().rows() + 300); + + // if (options.verbose) { + // log()->info("IrisZo requires {}/{} particles to be collision free ", + // bernoulli_threshold, options.num_particles); + // } + + while (true) { + log()->info("IrisZo iteration {}", iteration); + + Eigen::MatrixXd ATA = current_ellipsoid_A.transpose() * current_ellipsoid_A; + // rescaling makes max step computations more stable + ATA = (dim / ATA.trace()) * ATA; + + // initialize polytope with domain + A.topRows(domain.A().rows()) = domain.A(); + b.head(domain.A().rows()) = domain.b(); + + // Separating Planes Step + int num_iterations_separating_planes = 0; + + // track maximum relaxation of cspace margin if containment of points is + // requested + double max_relaxation = 0; + double outer_delta = + options.delta * 6 / (M_PI * M_PI * (iteration + 1) * (iteration + 1)); + + // No need for decaying outer delta if we are guaranteed to terminate after + // one step. In this case we can be less conservative and set it to our + // total accepted error probability. + if (options.max_iterations == 1) { + outer_delta = options.delta; + } + + while (num_iterations_separating_planes < + options.max_iterations_separating_planes) { + int k_squared = num_iterations_separating_planes + 1; + k_squared *= k_squared; + double delta_k = outer_delta * 6 / (M_PI * M_PI * k_squared); + int N_k = unadaptive_test_samples(options.epsilon, delta_k, options.tau); + + particles.at(0) = P.UniformSample(&generator, current_ellipsoid_center, + options.mixing_steps); + // populate particles by uniform sampling + for (int i = 1; i < N_k; ++i) { + particles.at(i) = P.UniformSample(&generator, particles.at(i - 1), + options.mixing_steps); + } + + // Copy top slice of particles, due to collision checker only accepting + // vectors of configurations. + // TODO(wernerpe): Remove this copy operation. + std::vector particles_step(particles.begin(), + particles.begin() + N_k); + + // Find all particles in collision + std::vector particle_col_free = + checker.CheckConfigsCollisionFree(particles_step, parallelism); + std::vector particles_in_collision; + int number_particles_in_collision_unadaptive_test = 0; + int number_particles_in_collision = 0; + for (size_t i = 0; i < particle_col_free.size(); ++i) { + if (particle_col_free[i] == 0) { + // only push back a maximum of num_particles for optimization of the + // faces + if (options.num_particles > number_particles_in_collision) { + // starting index is always 0, therefore particles[i+start] + // =particles[i] + particles_in_collision.push_back(particles[i]); + ++number_particles_in_collision; + } + ++number_particles_in_collision_unadaptive_test; + } + } + if (options.verbose) { + log()->info("IrisZo N_k {}, N_col {}, thresh {}", N_k, + number_particles_in_collision_unadaptive_test, + (1 - options.tau) * options.epsilon * N_k); + } + + // break if threshold is passed + if (number_particles_in_collision_unadaptive_test <= + (1 - options.tau) * options.epsilon * N_k) { + break; + } + + // warn user if test fails on last iteration + if (num_iterations_separating_planes == + options.max_iterations_separating_planes - 1) { + log()->warn( + "IrisZo WARNING, separating planes hit max iterations without " + "passing the bernoulli test, this voids the probabilistic " + "guarantees!"); + } + + // Update particle positions + std::vector particles_in_collision_updated; + particles_in_collision_updated.reserve(particles_in_collision.size()); + for (auto p : particles_in_collision) { + particles_in_collision_updated.emplace_back(p); + } + + const auto particle_update_work = + [&checker, &particles_in_collision_updated, &particles_in_collision, + ¤t_ellipsoid_center, + &options](const int thread_num, const int64_t index) { + const int point_idx = static_cast(index); + auto start_point = particles_in_collision[point_idx]; + + Eigen::VectorXd current_point = start_point; + + // update particles via gradient descent and bisection + // find newton descent direction + Eigen::VectorXd grad = (current_point - current_ellipsoid_center); + double max_distance = grad.norm(); + grad.normalize(); + + Eigen::VectorXd curr_pt_lower = current_point - max_distance * grad; + // update current point using bisection + if (!checker.CheckConfigCollisionFree(curr_pt_lower, thread_num)) { + // directly set to lowerbound + current_point = curr_pt_lower; + } else { + // bisect to find closest point in collision + Eigen::VectorXd curr_pt_upper = current_point; + for (int i = 0; i < options.bisection_steps; ++i) { + Eigen::VectorXd query = 0.5 * (curr_pt_upper + curr_pt_lower); + if (checker.CheckConfigCollisionFree(query, thread_num)) { + // config is collision free, increase lower bound + curr_pt_lower = query; + } else { + // config is in collision, decrease upper bound + curr_pt_upper = query; + current_point = query; + } + } + } + //} + particles_in_collision_updated[point_idx] = current_point; + }; + // update all particles in parallel + DynamicParallelForIndexLoop(DegreeOfParallelism(num_threads_to_use), 0, + number_particles_in_collision, + particle_update_work, + ParallelForBackend::BEST_AVAILABLE); + + // Resampling particles around found collisions + // TODO(wernerpe): implement optional resampling step + + // Place Hyperplanes + std::vector particle_distances; + particle_distances.reserve(number_particles_in_collision); + + for (auto particle : particles_in_collision_updated) { + particle_distances.emplace_back( + (particle - current_ellipsoid_center).transpose() * ATA * + (particle - current_ellipsoid_center)); + } + + // returned in ascending order + auto indices_sorted = argsort(particle_distances); + + // bools are not threadsafe - using uint8_t instead to accomondate for + // parallel checking + std::vector particle_is_redundant; + + for (int i = 0; i < number_particles_in_collision; ++i) { + particle_is_redundant.push_back(0); + } + + // add separating planes step + int hyperplanes_added = 0; + for (auto i : indices_sorted) { + // add nearest face + auto nearest_particle = particles_in_collision_updated[i]; + if (!particle_is_redundant[i]) { + // compute face + Eigen::VectorXd a_face; + if (options.force_containment_points && + options.containment_points.size()) { + a_face = compute_face_tangent_to_dist_cvxh( + current_ellipsoid, nearest_particle, cvxh_vpoly); + // std::cout< 0) { + b_face += relaxation; + if (max_relaxation < relaxation) max_relaxation = relaxation; + } + } + A.row(current_num_faces) = a_face.transpose(); + b(current_num_faces) = b_face; + ++current_num_faces; + ++hyperplanes_added; + + // resize A matrix if we need more faces + if (A.rows() <= current_num_faces) { + A.conservativeResize(A.rows() * 2, A.cols()); + b.conservativeResize(b.rows() * 2); + } + + // debugging visualization + if (options.meshcat && dim <= 3) { + for (int pt_to_draw = 0; pt_to_draw < number_particles_in_collision; + ++pt_to_draw) { + std::string path = fmt::format( + "face_pt/iteration{:02}/sepit{:02}/{:03}/pt", iteration, + num_iterations_separating_planes, current_num_faces); + options.meshcat->SetObject(path, Sphere(0.03), + geometry::Rgba(1, 1, 0.1, 1.0)); + point_to_draw.head(dim) = nearest_particle; + options.meshcat->SetTransform( + path, RigidTransform(point_to_draw)); + } + } + + if (hyperplanes_added == + options.max_separating_planes_per_iteration && + options.max_separating_planes_per_iteration > 0) + break; + + // set used particle to redundant + particle_is_redundant.at(i) = true; + +// loop over remaining non-redundant particles and check for +// redundancy +#if defined(_OPENMP) +#pragma omp parallel for num_threads(num_threads_to_use) +#endif + for (int particle_index = 0; + particle_index < number_particles_in_collision; + ++particle_index) { + if (!particle_is_redundant[particle_index]) { + if (a_face.transpose() * + particles_in_collision_updated[particle_index] - + b_face >= + 0) { + particle_is_redundant[particle_index] = 1; + } + } + } + } + } + + // update current polyhedron + P = HPolyhedron(A.topRows(current_num_faces), b.head(current_num_faces)); + if (max_relaxation > 0) { + log()->info( + fmt::format("IrisZo Warning relaxing cspace margin by {:03} to " + "ensure point containment", + max_relaxation)); + } + // resampling particles in current polyhedron for next iteration + particles[0] = P.UniformSample(&generator, options.mixing_steps); + for (int j = 1; j < options.num_particles; ++j) { + particles[j] = + P.UniformSample(&generator, particles[j - 1], options.mixing_steps); + } + ++num_iterations_separating_planes; + if (num_iterations_separating_planes - + 1 % static_cast( + 0.2 * options.max_iterations_separating_planes) == + 0 && + options.verbose) { + log()->info("SeparatingPlanes iteration: {} faces: {}", + num_iterations_separating_planes, current_num_faces); + } + } + + current_ellipsoid = P.MaximumVolumeInscribedEllipsoid(); + current_ellipsoid_A = current_ellipsoid.A(); + current_ellipsoid_center = current_ellipsoid.center(); + + const double volume = current_ellipsoid.Volume(); + const double delta_volume = volume - previous_volume; + if (delta_volume <= options.termination_threshold) { + log()->info("IrisZo delta vol {}, threshold {}", delta_volume, + options.termination_threshold); + break; + } + if (delta_volume / (previous_volume + 1e-6) <= + options.relative_termination_threshold) { + log()->info("IrisZo reldelta vol {}, threshold {}", + delta_volume / previous_volume, + options.relative_termination_threshold); + break; + } + ++iteration; + if (!(iteration < options.max_iterations)) { + log()->info("IrisZo iter {}, iter limit {}", iteration, + options.max_iterations); + break; + } + + if (options.require_sample_point_is_contained) { + if (!(P.PointInSet(starting_ellipsoid_center))) { + log()->info("IrisZo ERROR initial seed point not contained in region."); + return P_prev; + } + } + previous_volume = volume; + // reset polytope to domain, store previous iteration + P_prev = P; + P = domain; + current_num_faces = P.A().rows(); + } + auto stop = std::chrono::high_resolution_clock::now(); + if (options.verbose) { + log()->info( + "IrisZo execution time : {} ms", + std::chrono::duration_cast(stop - start) + .count()); + } + return P; +} + +} // namespace planning +} // namespace drake diff --git a/planning/iris/iris_zo.h b/planning/iris/iris_zo.h new file mode 100644 index 000000000000..963c619d44f5 --- /dev/null +++ b/planning/iris/iris_zo.h @@ -0,0 +1,171 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include + +#include "drake/common/parallelism.h" +#include "drake/geometry/meshcat.h" +#include "drake/geometry/optimization/hpolyhedron.h" +#include "drake/geometry/optimization/hyperellipsoid.h" +#include "drake/planning/collision_checker.h" + +namespace drake { +namespace planning { + +struct IrisZoOptions { + /** Passes this object to an Archive. + Refer to @ref yaml_serialization "YAML Serialization" for background. + Note: This only serializes options that are YAML built-in types. */ + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(num_particles)); + a->Visit(DRAKE_NVP(tau)); + a->Visit(DRAKE_NVP(delta)); + a->Visit(DRAKE_NVP(epsilon)); + a->Visit(DRAKE_NVP(containment_points)); + a->Visit(DRAKE_NVP(force_containment_points)); + a->Visit(DRAKE_NVP(max_iterations)); + a->Visit(DRAKE_NVP(max_iterations_separating_planes)); + a->Visit(DRAKE_NVP(max_separating_planes_per_iteration)); + a->Visit(DRAKE_NVP(bisection_steps)); + a->Visit(DRAKE_NVP(parallelize)); + a->Visit(DRAKE_NVP(verbose)); + a->Visit(DRAKE_NVP(require_sample_point_is_contained)); + a->Visit(DRAKE_NVP(configuration_space_margin)); + a->Visit(DRAKE_NVP(termination_threshold)); + a->Visit(DRAKE_NVP(relative_termination_threshold)); + a->Visit(DRAKE_NVP(random_seed)); + a->Visit(DRAKE_NVP(mixing_steps)); + } + + IrisZoOptions() = default; + + /** Number of particles used to estimate the closest collision*/ + int num_particles = 1e3; + + /** Descision threshold for unadaptive test.*/ + double tau = 0.5; + + /** Upper bound on the error probability that the fraction-in-collision + * `epsilon` is not met.*/ + double delta = 5e-2; + + /** Admissible fraction of the region volume allowed to be in collision.*/ + double epsilon = 1e-2; + + /** Points that are guaranteed to be contained in the final region + * provided their convex hull is collision free.*/ + Eigen::MatrixXd containment_points; + + /** If true, sets faces tangent to the sublevelsets of dist(C), where + * c is the convex hull of the points passed in `containment_points`. + */ + bool force_containment_points{false}; + + /** Number of resampling steps for the gradient updates*/ + // int num_resampling_steps = 1; + + /** Number Iris Iterations*/ + int max_iterations{2}; + + /** Maximum number of rounds of adding faces to the polytope*/ + int max_iterations_separating_planes{20}; + + /** Maximum number of faces to add per round of samples*/ + int max_separating_planes_per_iteration{-1}; + + /** Maximum number of bisection steps per gradient step*/ + int bisection_steps{10}; + + /** Parallelize the updates of the particles*/ + bool parallelize{true}; + + /* Enables print statements indicating the progress of fast iris**/ + bool verbose{false}; + + /** The initial polytope is guaranteed to contain the point if that point is + collision-free. */ + bool require_sample_point_is_contained{true}; + + /** 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. + */ + double configuration_space_margin{1e-2}; + + /** 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. */ + double termination_threshold{2e-2}; + + /** 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. */ + double relative_termination_threshold{1e-3}; // from rdeits/iris-distro. + + /** 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. */ + // ConvexSets configuration_obstacles{}; + + /** The only randomization in IRIS is the random sampling done to find + counter-examples for the additional constraints using in + IrisInConfigurationSpace. Use this option to set the initial seed. */ + int random_seed{1234}; + + // number of mixing steps used for hit and run sampling + int mixing_steps{50}; + + /** Passing a meshcat instance may enable debugging visualizations; this + currently and when the + configuration space is <= 3 dimensional.*/ + std::shared_ptr meshcat{}; +}; + +/** The IRIS-ZO (Iterative Regional Inflation by Semidefinite programming - Zero +Order) algorithm, as described in + +P. Werner, T. Cohn, R. H. Jiang, T. Seyde, M. Simchowitz, R. Tedrake, and D. +Rus, "Faster Algorithms for Growing Collision-Free Convex Polytopes in Robot +Configuration Space," + +https://groups.csail.mit.edu/robotics-center/public_papers/Werner24.pdf + +This algorithm constructs probabilistically collision-free polytopes, in robot +configuration space while only relying on a collision checker. The sets are +constructed using a simple parallel zero-order optimization strategy. The +produced polytope P is probabilistically collision-free in the sense that one +gets to control the probability δ that the fraction of the volume-in-collision +is larger than ε + +Pr[λ(P\Cfree)/λ(P) > ε] ⋞ δ. + +@param starting_ellipsoid provides the initial ellipsoid around which to grow +the region. This is typically a small ball around a collision-free +configuration. The center of this ellipsoid is required to be collision-free. +@param domain describes the total region of interest; computed IRIS regions will +be inside this domain. It must be bounded, and is typically a simple bounding +box representing joint limits (e.g. from HPolyhedron::MakeBox). +@param options contains algorithm parameters such as the desired collision-free +fraction, confidence level, and various algorithmic settings. + +The @p starting_ellipsoid and @p domain must describe elements in the same +ambient dimension as the configuration space of the robot. +@return A HPolyhedron representing the computed collision-free region in +configuration space. +@ingroup robot_planning +*/ + +geometry::optimization::HPolyhedron IrisZo( + const CollisionChecker& checker, + const geometry::optimization::Hyperellipsoid& starting_ellipsoid, + const geometry::optimization::HPolyhedron& domain, + const IrisZoOptions& options = IrisZoOptions()); +} // namespace planning +} // namespace drake diff --git a/planning/iris/test/iris_zo_test.cc b/planning/iris/test/iris_zo_test.cc new file mode 100644 index 000000000000..5b9889cdfff4 --- /dev/null +++ b/planning/iris/test/iris_zo_test.cc @@ -0,0 +1,549 @@ +#include "drake/planning/iris/iris_zo.h" + +#include +#include + +#include + +#include "drake/common/find_resource.h" +#include "drake/common/test_utilities/expect_throws_message.h" +#include "drake/common/test_utilities/maybe_pause_for_user.h" +#include "drake/geometry/meshcat.h" +#include "drake/geometry/optimization/hpolyhedron.h" +#include "drake/geometry/optimization/vpolytope.h" +#include "drake/geometry/test_utilities/meshcat_environment.h" +#include "drake/multibody/inverse_kinematics/inverse_kinematics.h" +#include "drake/planning/robot_diagram_builder.h" +#include "drake/planning/scene_graph_collision_checker.h" + +namespace drake { +namespace planning { +namespace { + +using common::MaybePauseForUser; +using Eigen::Vector2d; +using geometry::Meshcat; +using geometry::Rgba; +using geometry::Sphere; +using geometry::optimization::HPolyhedron; +using geometry::optimization::Hyperellipsoid; +using geometry::optimization::VPolytope; +using symbolic::Variable; + +const double kInf = std::numeric_limits::infinity(); + +// Helper method for testing FastIris from a urdf string. +HPolyhedron IrisZoFromUrdf(const std::string urdf, + const Hyperellipsoid& starting_ellipsoid, + const IrisZoOptions& options) { + CollisionCheckerParams params; + RobotDiagramBuilder builder(0.0); + + builder.parser().package_map().AddPackageXml(FindResourceOrThrow( + "drake/multibody/parsing/test/box_package/package.xml")); + params.robot_model_instances = + builder.parser().AddModelsFromString(urdf, "urdf"); + + auto plant_ptr = &(builder.plant()); + plant_ptr->Finalize(); + + params.model = builder.Build(); + params.edge_step_size = 0.01; + + HPolyhedron domain = HPolyhedron::MakeBox( + plant_ptr->GetPositionLowerLimits(), plant_ptr->GetPositionUpperLimits()); + + planning::SceneGraphCollisionChecker checker(std::move(params)); + // plant.SetPositions(&plant.GetMyMutableContextFromRoot(context.get()), + // sample); + return IrisZo(checker, starting_ellipsoid, domain, options); +} + +// One prismatic link with joint limits. Iris should return the joint limits. +GTEST_TEST(IrisZoTest, JointLimits) { + const std::string limits_urdf = R"( + + + + + + + + + + + + + +)"; + + const Vector1d sample = Vector1d::Zero(); + Hyperellipsoid starting_ellipsoid = + Hyperellipsoid::MakeHypersphere(1e-2, sample); + IrisZoOptions options; + options.verbose = true; + HPolyhedron region = IrisZoFromUrdf(limits_urdf, starting_ellipsoid, options); + + EXPECT_EQ(region.ambient_dimension(), 1); + + const double kTol = 1e-5; + const double qmin = -2.0, qmax = 2.0; + EXPECT_TRUE(region.PointInSet(Vector1d{qmin + kTol})); + EXPECT_TRUE(region.PointInSet(Vector1d{qmax - kTol})); + EXPECT_FALSE(region.PointInSet(Vector1d{qmin - kTol})); + EXPECT_FALSE(region.PointInSet(Vector1d{qmax + kTol})); +} + +// A simple double pendulum with link lengths `l1` and `l2` with a sphere at the +// tip of radius `r` between two (fixed) walls at `w` from the origin. The +// true configuration space is - w + r ≤ l₁s₁ + l₂s₁₊₂ ≤ w - r. These regions +// are visualized at https://www.desmos.com/calculator/ff0hbnkqhm. +GTEST_TEST(IrisZoTest, DoublePendulum) { + const double l1 = 2.0; + const double l2 = 1.0; + const double r = .5; + const double w = 1.83; + const std::string double_pendulum_urdf = fmt::format( + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)", + fmt::arg("w_plus_one_half", w + .5), fmt::arg("l1", l1), + fmt::arg("l2", l2), fmt::arg("r", r)); + + const Vector2d sample = Vector2d::Zero(); + std::shared_ptr meshcat = geometry::GetTestEnvironmentMeshcat(); + meshcat->Delete("face_pt"); + IrisZoOptions options; + options.verbose = true; + options.meshcat = meshcat; + Hyperellipsoid starting_ellipsoid = + Hyperellipsoid::MakeHypersphere(1e-2, sample); + HPolyhedron region = + IrisZoFromUrdf(double_pendulum_urdf, starting_ellipsoid, options); + + EXPECT_EQ(region.ambient_dimension(), 2); + // Confirm that we've found a substantial region. + EXPECT_GE(region.MaximumVolumeInscribedEllipsoid().Volume(), 2.0); + + EXPECT_TRUE(region.PointInSet(Vector2d{.4, 0.0})); + EXPECT_FALSE(region.PointInSet(Vector2d{.5, 0.0})); + EXPECT_TRUE(region.PointInSet(Vector2d{.3, .3})); + EXPECT_FALSE(region.PointInSet(Vector2d{.4, .3})); + EXPECT_TRUE(region.PointInSet(Vector2d{-.4, 0.0})); + EXPECT_FALSE(region.PointInSet(Vector2d{-.5, 0.0})); + EXPECT_TRUE(region.PointInSet(Vector2d{-.3, -.3})); + EXPECT_FALSE(region.PointInSet(Vector2d{-.4, -.3})); + + { + meshcat->Set2dRenderMode(math::RigidTransformd(Eigen::Vector3d{0, 0, 1}), + -3.25, 3.25, -3.25, 3.25); + meshcat->SetProperty("/Grid", "visible", true); + Eigen::RowVectorXd theta2s = + Eigen::RowVectorXd::LinSpaced(100, -1.57, 1.57); + Eigen::Matrix3Xd points = Eigen::Matrix3Xd::Zero(3, 2 * theta2s.size() + 1); + const double c = -w + r; + for (int i = 0; i < theta2s.size(); ++i) { + const double a = l1 + l2 * std::cos(theta2s[i]), + b = l2 * std::sin(theta2s[i]); + // wolfram solve a*sin(q) + b*cos(q) = c for q + points(0, i) = + 2 * std::atan((std::sqrt(a * a + b * b - c * c) + a) / (b + c)) + + M_PI; + points(1, i) = theta2s[i]; + points(0, points.cols() - i - 2) = + 2 * std::atan((std::sqrt(a * a + b * b - c * c) + a) / (b - c)) - + M_PI; + points(1, points.cols() - i - 2) = theta2s[i]; + } + points.col(points.cols() - 1) = points.col(0); + meshcat->SetLine("True C_free", points, 2.0, Rgba(0, 0, 1)); + VPolytope vregion = VPolytope(region).GetMinimalRepresentation(); + points.resize(3, vregion.vertices().cols() + 1); + points.topLeftCorner(2, vregion.vertices().cols()) = vregion.vertices(); + points.topRightCorner(2, 1) = vregion.vertices().col(0); + points.bottomRows<1>().setZero(); + meshcat->SetLine("IRIS Region", points, 2.0, Rgba(0, 1, 0)); + + MaybePauseForUser(); + } +} + +const char block_urdf[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +// A block on a vertical track, free to rotate (in the plane) with width `w` of +// 2 and height `h` of 1, plus a ground plane at z=0. The true configuration +// space is min(q₀ ± .5w sin(q₁) ± .5h cos(q₁)) ≥ 0, where the min is over the +// ±. This region is also visualized at +// https://www.desmos.com/calculator/ok5ckpa1kp. +GTEST_TEST(IrisZoTest, BlockOnGround) { + const Vector2d sample{1.0, 0.0}; + std::shared_ptr meshcat = geometry::GetTestEnvironmentMeshcat(); + meshcat->Delete("face_pt"); + IrisZoOptions options; + options.verbose = true; + options.meshcat = meshcat; + Hyperellipsoid starting_ellipsoid = + Hyperellipsoid::MakeHypersphere(1e-2, sample); + HPolyhedron region = IrisZoFromUrdf(block_urdf, starting_ellipsoid, options); + + EXPECT_EQ(region.ambient_dimension(), 2); + // Confirm that we've found a substantial region. + EXPECT_GE(region.MaximumVolumeInscribedEllipsoid().Volume(), 2.0); + + { + meshcat->Set2dRenderMode(math::RigidTransformd(Eigen::Vector3d{0, 0, 1}), 0, + 3.25, -3.25, 3.25); + meshcat->SetProperty("/Grid", "visible", true); + Eigen::RowVectorXd thetas = Eigen::RowVectorXd::LinSpaced(100, -M_PI, M_PI); + const double w = 2, h = 1; + Eigen::Matrix3Xd points = Eigen::Matrix3Xd::Zero(3, 2 * thetas.size() + 1); + for (int i = 0; i < thetas.size(); ++i) { + const double a = 0.5 * + (-w * std::sin(thetas[i]) - h * std::cos(thetas[i])), + b = 0.5 * + (-w * std::sin(thetas[i]) + h * std::cos(thetas[i])), + c = 0.5 * + (+w * std::sin(thetas[i]) - h * std::cos(thetas[i])), + d = 0.5 * + (+w * std::sin(thetas[i]) + h * std::cos(thetas[i])); + points(0, i) = std::max({a, b, c, d}); + points(1, i) = thetas[i]; + points(0, points.cols() - i - 2) = 3.0; + points(1, points.cols() - i - 2) = thetas[i]; + } + points.col(points.cols() - 1) = points.col(0); + meshcat->SetLine("True C_free", points, 2.0, Rgba(0, 0, 1)); + VPolytope vregion = VPolytope(region).GetMinimalRepresentation(); + points.resize(3, vregion.vertices().cols() + 1); + points.topLeftCorner(2, vregion.vertices().cols()) = vregion.vertices(); + points.topRightCorner(2, 1) = vregion.vertices().col(0); + points.bottomRows<1>().setZero(); + meshcat->SetLine("IRIS Region", points, 2.0, Rgba(0, 1, 0)); + + MaybePauseForUser(); + } +} + +// A (somewhat contrived) example of a concave configuration-space obstacle +// (resulting in a convex configuration-space, which we approximate with +// polytopes): A simple pendulum of length `l` with a sphere at the tip of +// radius `r` on a vertical track, plus a ground plane at z=0. The +// configuration space is given by the joint limits and z + l*cos(theta) >= r. +// The region is also visualized at +// https://www.desmos.com/calculator/flshvay78b. In addition to testing the +// convex space, this was originally a test for which Ibex found +// counter-examples that Snopt missed; now Snopt succeeds due to having +// options.num_collision_infeasible_samples > 1. +GTEST_TEST(IrisZoTest, ConvexConfigurationSpace) { + const double l = 1.5; + const double r = 0.1; + + std::shared_ptr meshcat = geometry::GetTestEnvironmentMeshcat(); + meshcat->Delete("face_pt"); + meshcat->Set2dRenderMode(math::RigidTransformd(Eigen::Vector3d{0, 0, 1}), + -3.25, 3.25, -3.25, 3.25); + meshcat->SetProperty("/Grid", "visible", true); + Eigen::RowVectorXd theta1s = Eigen::RowVectorXd::LinSpaced(100, -1.5, 1.5); + Eigen::Matrix3Xd points = Eigen::Matrix3Xd::Zero(3, 2 * theta1s.size()); + for (int i = 0; i < theta1s.size(); ++i) { + points(0, i) = r - l * cos(theta1s[i]); + points(1, i) = theta1s[i]; + points(0, points.cols() - i - 1) = 0; + points(1, points.cols() - i - 1) = theta1s[i]; + } + meshcat->SetLine("True C_free", points, 2.0, Rgba(0, 0, 1)); + + const std::string convex_urdf = fmt::format( + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)", + fmt::arg("l", l), fmt::arg("r", r)); + + const Vector2d sample{-0.5, 0.0}; + IrisZoOptions options; + + // This point should be outside of the configuration space (in collision). + // The particular value was found by visual inspection using meshcat. + const double z_test = 0, theta_test = -1.55; + // Confirm that the pendulum is colliding with the wall with true kinematics: + EXPECT_LE(z_test + l * std::cos(theta_test), r); + + // Turn on meshcat for addition debugging visualizations. + // This example is truly adversarial for IRIS. After one iteration, the + // maximum-volume inscribed ellipse is approximately centered in C-free. So + // finding a counter-example in the bottom corner (near the test point) is + // not only difficult because we need to sample in a corner of the polytope, + // but because the objective is actually pulling the counter-example search + // away from that corner. Open the meshcat visualization to step through the + // details! + options.meshcat = meshcat; + options.verbose = true; + Hyperellipsoid starting_ellipsoid = + Hyperellipsoid::MakeHypersphere(1e-2, sample); + // std::this_thread::sleep_for(std::chrono::milliseconds(100)); + HPolyhedron region = IrisZoFromUrdf(convex_urdf, starting_ellipsoid, options); + // TODO(russt): Expecting the test point to be outside the verified region is + // too strong of a requirement right now. If we can improve the algorithm then + // we should make this EXPECT_FALSE. + if (!region.PointInSet(Vector2d{z_test, theta_test})) { + log()->info("Our test point is not in the set"); + } + + EXPECT_EQ(region.ambient_dimension(), 2); + // Confirm that we've found a substantial region. + EXPECT_GE(region.MaximumVolumeInscribedEllipsoid().Volume(), 0.5); + + { + VPolytope vregion = VPolytope(region).GetMinimalRepresentation(); + points.resize(3, vregion.vertices().cols() + 1); + points.topLeftCorner(2, vregion.vertices().cols()) = vregion.vertices(); + points.topRightCorner(2, 1) = vregion.vertices().col(0); + points.bottomRows<1>().setZero(); + meshcat->SetLine("IRIS Region", points, 2.0, Rgba(0, 1, 0)); + + meshcat->SetObject("Test point", Sphere(0.03), Rgba(1, 0, 0)); + meshcat->SetTransform("Test point", math::RigidTransform(Eigen::Vector3d( + z_test, theta_test, 0))); + + MaybePauseForUser(); + } +} +/* A movable sphere with fixed boxes in all corners. +┌───────────────┐ +│┌────┐ ┌────┐│ +││ │ │ ││ +│└────┘ └────┘│ +│ o │ +│┌────┐ ┌────┐│ +││ │ │ ││ +│└────┘ └────┘│ +└───────────────┘ */ +const char boxes_in_corners_urdf[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; +GTEST_TEST(IrisZoTest, ForceContainmentPointsTest) { + std::shared_ptr meshcat; + meshcat = geometry::GetTestEnvironmentMeshcat(); + meshcat->Delete("face_pt"); + meshcat->Delete("True C_free"); + meshcat->Delete("Test point"); + meshcat->Set2dRenderMode(math::RigidTransformd(Eigen::Vector3d{0, 0, 1}), + -3.25, 3.25, -3.25, 3.25); + meshcat->SetProperty("/Grid", "visible", true); + // Draw the true cspace. + Eigen::Matrix3Xd env_points(3, 5); + // clang-format off + env_points << -2, 2, 2, -2, -2, + 2, 2, -2, -2, 2, + 0, 0, 0, 0, 0; + // clang-format on + meshcat->SetLine("Domain", env_points, 8.0, Rgba(0, 0, 0)); + Eigen::Matrix3Xd centers(3, 4); + double c = 1.0; + // clang-format off + centers << -c, c, c, -c, + c, c, -c, -c, + 0, 0, 0, 0; + // clang-format on + Eigen::Matrix3Xd obs_points(3, 5); + // approximating offset due to sphere radius with fixed offset + double s = 0.7 + 0.01; + // clang-format off + obs_points << -s, s, s, -s, -s, + s, s, -s, -s, s, + s, 0, 0, 0, 0; + // clang-format on + for (int obstacle_idx = 0; obstacle_idx < 4; ++obstacle_idx) { + Eigen::Matrix3Xd obstacle = obs_points; + obstacle.colwise() += centers.col(obstacle_idx); + meshcat->SetLine(fmt::format("/obstacles/obs_{}", obstacle_idx), obstacle, + 8.0, Rgba(0, 0, 0)); + } + const Vector2d sample{0.0, 0.0}; + Hyperellipsoid starting_ellipsoid = + Hyperellipsoid::MakeHypersphere(1e-2, sample); + // std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + Eigen::Matrix3Xd cont_points(3, 4); + double xw, yw; + xw = 0.4; + yw = 0.28; + // clang-format off + cont_points << -xw, xw, xw, -xw, + yw, yw, -yw, -yw, + 0, 0, 0, 0; + // clang-format on + IrisZoOptions options; + options.verbose = true; + options.meshcat = meshcat; + options.configuration_space_margin = 0.04; + options.containment_points = cont_points.topRows(2); + options.force_containment_points = true; + HPolyhedron region = + IrisZoFromUrdf(boxes_in_corners_urdf, starting_ellipsoid, options); + EXPECT_EQ(region.ambient_dimension(), 2); + { + for (int pt_to_draw = 0; pt_to_draw < cont_points.cols(); ++pt_to_draw) { + Eigen::Vector3d point_to_draw = Eigen::Vector3d::Zero(); + std::string path = fmt::format("cont_pt/{}", pt_to_draw); + options.meshcat->SetObject(path, Sphere(0.04), + geometry::Rgba(1, 0, 0.0, 1.0)); + point_to_draw.head(2) = cont_points.col(pt_to_draw); + options.meshcat->SetTransform( + path, math::RigidTransform(point_to_draw)); + EXPECT_TRUE(region.PointInSet(point_to_draw.head(2))); + } + Eigen::Matrix3Xd points = Eigen::Matrix3Xd::Zero(3, 20); + + VPolytope vregion = VPolytope(region).GetMinimalRepresentation(); + points.resize(3, vregion.vertices().cols() + 1); + points.topLeftCorner(2, vregion.vertices().cols()) = vregion.vertices(); + points.topRightCorner(2, 1) = vregion.vertices().col(0); + points.bottomRows<1>().setZero(); + meshcat->SetLine("IRIS Region", points, 2.0, Rgba(0, 1, 0)); + + MaybePauseForUser(); + } +} + +} // namespace +} // namespace planning +} // namespace drake From fd815094ad41de07fcbe1bd87c3f4aff59975bd9 Mon Sep 17 00:00:00 2001 From: Peter Date: Wed, 13 Nov 2024 11:30:32 -0500 Subject: [PATCH 02/13] first iteration of python test --- bindings/pydrake/planning/BUILD.bazel | 8 ++ .../pydrake/planning/planning_py_iris_zo.cc | 3 +- .../pydrake/planning/test/iris_zo_test.py | 77 +++++++++++++++++++ 3 files changed, 86 insertions(+), 2 deletions(-) create mode 100644 bindings/pydrake/planning/test/iris_zo_test.py diff --git a/bindings/pydrake/planning/BUILD.bazel b/bindings/pydrake/planning/BUILD.bazel index 917e10457e12..b60c6ba9e653 100644 --- a/bindings/pydrake/planning/BUILD.bazel +++ b/bindings/pydrake/planning/BUILD.bazel @@ -131,6 +131,14 @@ drake_py_unittest( ], ) +drake_py_unittest( + name = "iris_zo_test", + num_threads = 2, + deps = [ + ":planning", + ], +) + drake_py_unittest( name = "zmp_planner_test", deps = [ diff --git a/bindings/pydrake/planning/planning_py_iris_zo.cc b/bindings/pydrake/planning/planning_py_iris_zo.cc index 1cc4e742a428..07e675bd42fe 100644 --- a/bindings/pydrake/planning/planning_py_iris_zo.cc +++ b/bindings/pydrake/planning/planning_py_iris_zo.cc @@ -89,8 +89,7 @@ void DefinePlanningIrisZo(py::module m) { }); m.def("IrisZo", &IrisZo, py::arg("checker"), py::arg("starting_ellipsoid"), - py::arg("doma din"), py::arg("options") = IrisZoOptions(), - doc.IrisZo.doc); + py::arg("domain"), py::arg("options") = IrisZoOptions(), doc.IrisZo.doc); } } // namespace internal diff --git a/bindings/pydrake/planning/test/iris_zo_test.py b/bindings/pydrake/planning/test/iris_zo_test.py new file mode 100644 index 000000000000..f17fd6c2639a --- /dev/null +++ b/bindings/pydrake/planning/test/iris_zo_test.py @@ -0,0 +1,77 @@ +import unittest + +import pydrake.planning as mut +from pydrake.common import RandomGenerator, Parallelism +from pydrake.geometry.optimization import Hyperellipsoid, HPolyhedron +from pydrake.planning import ( + RobotDiagramBuilder, + SceneGraphCollisionChecker, + CollisionCheckerParams, +) + +import numpy as np + + +class TestIrisZo(unittest.TestCase): + def test_iris_zo(self): + # Taken from iris_from_clique_cover_test.py + cross_cspace_urdf = """ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +""" + params = dict(edge_step_size=0.1) + builder = RobotDiagramBuilder() + params["robot_model_instances"] = builder.parser().AddModelsFromString( + cross_cspace_urdf, "urdf" + ) + params["model"] = builder.Build() + plant = params["model"].plant() + checker = SceneGraphCollisionChecker(**params) + options = mut.IrisZoOptions() + seed_point = np.zeros((2,)) + starting_ellipsoid = Hyperellipsoid.MakeHypersphere(0.01, seed_point) + domain = HPolyhedron.MakeBox(plant.GetPositionLowerLimits(), + plant.GetPositionUpperLimits()) + region = mut.IrisZo(checker, starting_ellipsoid, domain, options) + test_point = np.array([0.0, 0.5]) + self.assertTrue(region.PointInSet(test_point)) From 3f7aa6bc563eef79e972a856d8cf9f20585edf85 Mon Sep 17 00:00:00 2001 From: Peter Date: Wed, 13 Nov 2024 12:11:19 -0500 Subject: [PATCH 03/13] first round of comments --- .../pydrake/planning/planning_py_iris_zo.cc | 6 +- planning/iris/iris_zo.cc | 34 ++++---- planning/iris/iris_zo.h | 77 +++++++++---------- planning/iris/test/iris_zo_test.cc | 1 - 4 files changed, 52 insertions(+), 66 deletions(-) diff --git a/bindings/pydrake/planning/planning_py_iris_zo.cc b/bindings/pydrake/planning/planning_py_iris_zo.cc index 07e675bd42fe..f56b7440f02d 100644 --- a/bindings/pydrake/planning/planning_py_iris_zo.cc +++ b/bindings/pydrake/planning/planning_py_iris_zo.cc @@ -23,9 +23,6 @@ void DefinePlanningIrisZo(py::module m) { .def_readwrite("epsilon", &IrisZoOptions::epsilon, cls_doc.epsilon.doc) .def_readwrite("containment_points", &IrisZoOptions::containment_points, cls_doc.containment_points.doc) - .def_readwrite("force_containment_points", - &IrisZoOptions::force_containment_points, - cls_doc.force_containment_points.doc) .def_readwrite("max_iterations", &IrisZoOptions::max_iterations, cls_doc.max_iterations.doc) .def_readwrite("max_iterations_separating_planes", @@ -78,8 +75,7 @@ void DefinePlanningIrisZo(py::module m) { "mixing_steps={}, " ")") .format(self.num_particles, self.tau, self.delta, self.epsilon, - self.containment_points, self.force_containment_points, - self.max_iterations, self.max_iterations_separating_planes, + self.containment_points, self.max_iterations, self.max_iterations_separating_planes, self.max_separating_planes_per_iteration, self.bisection_steps, self.parallelize, self.verbose, self.require_sample_point_is_contained, diff --git a/planning/iris/iris_zo.cc b/planning/iris/iris_zo.cc index 383016317154..71378ddbf832 100644 --- a/planning/iris/iris_zo.cc +++ b/planning/iris/iris_zo.cc @@ -83,12 +83,9 @@ HPolyhedron IrisZo(const planning::CollisionChecker& checker, const Hyperellipsoid& starting_ellipsoid, const HPolyhedron& domain, const IrisZoOptions& options) { auto start = std::chrono::high_resolution_clock::now(); - const auto parallelism = Parallelism::Max(); - const int num_threads_to_use = - checker.SupportsParallelChecking() && options.parallelize - ? std::min(parallelism.num_threads(), - checker.num_allocated_contexts()) - : 1; + const int num_threads_to_use = checker.SupportsParallelChecking() && + std::min(options.parallelism.num_threads(), + checker.num_allocated_contexts()); RandomGenerator generator(options.random_seed); @@ -107,22 +104,23 @@ HPolyhedron IrisZo(const planning::CollisionChecker& checker, DRAKE_THROW_UNLESS(domain.IsBounded()); DRAKE_THROW_UNLESS(domain.PointInSet(current_ellipsoid_center)); - VPolytope cvxh_vpoly(options.containment_points); - if (options.force_containment_points) { + VPolytope cvxh_vpoly; + if (options.containment_points.has_value()) { + cvxh_vpoly = VPolytope(options.containment_points.value()); DRAKE_THROW_UNLESS(domain.ambient_dimension() == - options.containment_points.rows()); + options.containment_points->rows()); cvxh_vpoly = cvxh_vpoly.GetMinimalRepresentation(); std::vector cont_vec; - cont_vec.reserve((options.containment_points.cols())); + cont_vec.reserve((options.containment_points->cols())); - for (int col = 0; col < options.containment_points.cols(); ++col) { - Eigen::VectorXd conf = options.containment_points.col(col); + for (int col = 0; col < options.containment_points->cols(); ++col) { + Eigen::VectorXd conf = options.containment_points->col(col); cont_vec.emplace_back(conf); } std::vector containment_point_col_free = - checker.CheckConfigsCollisionFree(cont_vec, parallelism); + checker.CheckConfigsCollisionFree(cont_vec, options.parallelism); for (const auto col_free : containment_point_col_free) { if (!col_free) { throw std::runtime_error( @@ -231,7 +229,8 @@ HPolyhedron IrisZo(const planning::CollisionChecker& checker, // Find all particles in collision std::vector particle_col_free = - checker.CheckConfigsCollisionFree(particles_step, parallelism); + checker.CheckConfigsCollisionFree(particles_step, + options.parallelism); std::vector particles_in_collision; int number_particles_in_collision_unadaptive_test = 0; int number_particles_in_collision = 0; @@ -352,8 +351,7 @@ HPolyhedron IrisZo(const planning::CollisionChecker& checker, if (!particle_is_redundant[i]) { // compute face Eigen::VectorXd a_face; - if (options.force_containment_points && - options.containment_points.size()) { + if (options.containment_points.has_value()) { a_face = compute_face_tangent_to_dist_cvxh( current_ellipsoid, nearest_particle, cvxh_vpoly); // std::cout< 0) { b_face += relaxation; diff --git a/planning/iris/iris_zo.h b/planning/iris/iris_zo.h index 963c619d44f5..3948ec0ef6da 100644 --- a/planning/iris/iris_zo.h +++ b/planning/iris/iris_zo.h @@ -28,12 +28,11 @@ struct IrisZoOptions { a->Visit(DRAKE_NVP(delta)); a->Visit(DRAKE_NVP(epsilon)); a->Visit(DRAKE_NVP(containment_points)); - a->Visit(DRAKE_NVP(force_containment_points)); a->Visit(DRAKE_NVP(max_iterations)); a->Visit(DRAKE_NVP(max_iterations_separating_planes)); a->Visit(DRAKE_NVP(max_separating_planes_per_iteration)); a->Visit(DRAKE_NVP(bisection_steps)); - a->Visit(DRAKE_NVP(parallelize)); + a->Visit(DRAKE_NVP(parallelism)); a->Visit(DRAKE_NVP(verbose)); a->Visit(DRAKE_NVP(require_sample_point_is_contained)); a->Visit(DRAKE_NVP(configuration_space_margin)); @@ -45,57 +44,56 @@ struct IrisZoOptions { IrisZoOptions() = default; - /** Number of particles used to estimate the closest collision*/ + /** Number of particles used to estimate the closest collision. */ int num_particles = 1e3; - /** Descision threshold for unadaptive test.*/ + /** Descision threshold for the unadaptive test. Chosing a small value + * increases both the cost and the power statistical test. Increasing the + * value of `tau` makes running an individual test cheaper but decreases its + * power to accept a polytope. We chosing a value of 0.5 is a good + * trade-off. + */ double tau = 0.5; - /** Upper bound on the error probability that the fraction-in-collision - * `epsilon` is not met.*/ + /** Upper bound on the probability the returned region has a + * fraction-in-collision greater than `epsilon`. */ double delta = 5e-2; - /** Admissible fraction of the region volume allowed to be in collision.*/ + /** Admissible fraction of the region volume allowed to be in collision. */ double epsilon = 1e-2; /** Points that are guaranteed to be contained in the final region - * provided their convex hull is collision free.*/ - Eigen::MatrixXd containment_points; - - /** If true, sets faces tangent to the sublevelsets of dist(C), where - * c is the convex hull of the points passed in `containment_points`. - */ - bool force_containment_points{false}; + * provided their convex hull is collision free. */ + std::optional containment_points{std::nullopt}; - /** Number of resampling steps for the gradient updates*/ - // int num_resampling_steps = 1; + /** Maximum number of alternations between the ellipsoid and the separating + * planes step (a.k.a. outer iterations). */ + int max_iterations{3}; - /** Number Iris Iterations*/ - int max_iterations{2}; - - /** Maximum number of rounds of adding faces to the polytope*/ + /** Maximum number of rounds of adding faces to the polytope per outer + * iteration. */ int max_iterations_separating_planes{20}; - /** Maximum number of faces to add per round of samples*/ + /** Maximum number of faces to add per inner iteration. Setting the value to + * -1 means there is no limit to the number of faces that can be added. */ int max_separating_planes_per_iteration{-1}; - /** Maximum number of bisection steps per gradient step*/ + /** Maximum number of bisection steps. */ int bisection_steps{10}; - /** Parallelize the updates of the particles*/ - bool parallelize{true}; + /** Parallelize the updates of the particles. */ + Parallelism parallelism{Parallelism::Max()}; - /* Enables print statements indicating the progress of fast iris**/ + /** Enables print statements indicating the progress of IrisZo. */ bool verbose{false}; /** The initial polytope is guaranteed to contain the point if that point is - collision-free. */ + * collision-free. */ bool require_sample_point_is_contained{true}; /** 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. - */ + * possibility of requiring an infinite number of faces to approximate a + * curved boundary. */ double configuration_space_margin{1e-2}; /** IRIS will terminate if the change in the *volume* of the hyperellipsoid @@ -106,20 +104,13 @@ struct IrisZoOptions { /** 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. */ - double relative_termination_threshold{1e-3}; // from rdeits/iris-distro. - - /** 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. */ - // ConvexSets configuration_obstacles{}; + double relative_termination_threshold{1e-3}; - /** The only randomization in IRIS is the random sampling done to find - counter-examples for the additional constraints using in - IrisInConfigurationSpace. Use this option to set the initial seed. */ + /** This option to sets the random seed for random sampling throughout the + * algorithm. */ int random_seed{1234}; - // number of mixing steps used for hit and run sampling + /** Number of mixing steps used for hit-and-run sampling. */ int mixing_steps{50}; /** Passing a meshcat instance may enable debugging visualizations; this @@ -131,9 +122,10 @@ struct IrisZoOptions { /** The IRIS-ZO (Iterative Regional Inflation by Semidefinite programming - Zero Order) algorithm, as described in -P. Werner, T. Cohn, R. H. Jiang, T. Seyde, M. Simchowitz, R. Tedrake, and D. +P. Werner, T. Cohn\*, R. H. Jiang\*, T. Seyde, M. Simchowitz, R. Tedrake, and D. Rus, "Faster Algorithms for Growing Collision-Free Convex Polytopes in Robot Configuration Space," +\* Denotes equal contribution. https://groups.csail.mit.edu/robotics-center/public_papers/Werner24.pdf @@ -148,7 +140,8 @@ Pr[λ(P\Cfree)/λ(P) > ε] ⋞ δ. @param starting_ellipsoid provides the initial ellipsoid around which to grow the region. This is typically a small ball around a collision-free -configuration. The center of this ellipsoid is required to be collision-free. +configuration (e.g. Hyperellipsoid::MakeHyperSphere(radius, seed_point)). The +center of this ellipsoid is required to be collision-free. @param domain describes the total region of interest; computed IRIS regions will be inside this domain. It must be bounded, and is typically a simple bounding box representing joint limits (e.g. from HPolyhedron::MakeBox). diff --git a/planning/iris/test/iris_zo_test.cc b/planning/iris/test/iris_zo_test.cc index 5b9889cdfff4..4b28f8856a78 100644 --- a/planning/iris/test/iris_zo_test.cc +++ b/planning/iris/test/iris_zo_test.cc @@ -516,7 +516,6 @@ GTEST_TEST(IrisZoTest, ForceContainmentPointsTest) { options.meshcat = meshcat; options.configuration_space_margin = 0.04; options.containment_points = cont_points.topRows(2); - options.force_containment_points = true; HPolyhedron region = IrisZoFromUrdf(boxes_in_corners_urdf, starting_ellipsoid, options); EXPECT_EQ(region.ambient_dimension(), 2); From 7098bf48cdb0552b771aea55d3365d1f83a3e873 Mon Sep 17 00:00:00 2001 From: Peter Date: Wed, 13 Nov 2024 12:21:31 -0500 Subject: [PATCH 04/13] more comments --- planning/iris/iris_zo.cc | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/planning/iris/iris_zo.cc b/planning/iris/iris_zo.cc index 71378ddbf832..e58578801752 100644 --- a/planning/iris/iris_zo.cc +++ b/planning/iris/iris_zo.cc @@ -49,11 +49,12 @@ Eigen::VectorXd compute_face_tangent_to_dist_cvxh( const VPolytope& cvxh_vpoly) { Eigen::VectorXd a_face = E.A().transpose() * E.A() * (point - E.center()); double b_face = a_face.transpose() * point; - double worst_case = - (a_face.transpose() * cvxh_vpoly.vertices()).maxCoeff() - b_face; - // Return standard iris face if either the face does not chopp off any - // containment points or collision is in convex hull. - if (cvxh_vpoly.PointInSet(point) || worst_case <= 0) { + + // Return standard iris face if either the face does not chop off any + // containment points or collision lies inside of the convex hull of the + // containment points. + if (cvxh_vpoly.PointInSet(point) || + (a_face.transpose() * cvxh_vpoly.vertices()).maxCoeff() - b_face <= 0) { return a_face; } else { MathematicalProgram prog; @@ -64,17 +65,16 @@ Eigen::VectorXd compute_face_tangent_to_dist_cvxh( cvxh_vpoly.AddPointInSetConstraints(&prog, x); Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(dim, dim); prog.AddQuadraticErrorCost(identity, point, x); - auto solver = solvers::MakeFirstAvailableSolver(preferred_solvers); - solvers::MathematicalProgramResult result; - solver->Solve(prog, std::nullopt, std::nullopt, &result); + auto result = solvers::Solve(prog); DRAKE_THROW_UNLESS(result.is_success()); a_face = point - result.GetSolution(x); return a_face; } } -int unadaptive_test_samples(double p, double delta, double tau) { - return static_cast(-2 * std::log(delta) / (tau * tau * p) + 0.5); +// See Definition 1 in the paper. +int unadaptive_test_samples(double epsilon, double delta, double tau) { + return static_cast(-2 * std::log(delta) / (tau * tau * epsilon) + 0.5); } } // namespace @@ -94,6 +94,8 @@ HPolyhedron IrisZo(const planning::CollisionChecker& checker, Hyperellipsoid current_ellipsoid = starting_ellipsoid; Eigen::VectorXd current_ellipsoid_center = starting_ellipsoid.center(); Eigen::MatrixXd current_ellipsoid_A = starting_ellipsoid.A(); + + // Prevent directly terminating if the ellipsoid is too large. double previous_volume = 0; const int dim = starting_ellipsoid.ambient_dimension(); From 272c7bdf99abc77c8b2e5ddb55218434efcea700 Mon Sep 17 00:00:00 2001 From: Peter Date: Wed, 13 Nov 2024 13:34:42 -0500 Subject: [PATCH 05/13] even more comments --- planning/iris/iris_zo.cc | 60 ++++++++++++++++------------------------ planning/iris/iris_zo.h | 4 ++- 2 files changed, 27 insertions(+), 37 deletions(-) diff --git a/planning/iris/iris_zo.cc b/planning/iris/iris_zo.cc index e58578801752..f71e6dd5b4cd 100644 --- a/planning/iris/iris_zo.cc +++ b/planning/iris/iris_zo.cc @@ -1,7 +1,6 @@ #include "drake/planning/iris/iris_zo.h" #include -#include #include #include @@ -140,9 +139,7 @@ HPolyhedron IrisZo(const planning::CollisionChecker& checker, options.meshcat->SetTransform(path, RigidTransform(point_to_draw)); } - std::vector particles; - - // upper bound on number of particles required if we hit max iterations + // Upper bound on number of particles required if we hit max iterations. double outer_delta_min = options.delta * 6 / (M_PI * M_PI * options.max_iterations * options.max_iterations); @@ -161,43 +158,33 @@ HPolyhedron IrisZo(const planning::CollisionChecker& checker, log()->info("IrisZo worst case test requires {} samples.", N_max); } - particles.reserve(N_max); - for (int i = 0; i < N_max; ++i) { - particles.emplace_back(Eigen::VectorXd::Zero(dim)); - } + std::vector particles(N_max, Eigen::VectorXd::Zero(dim)); int iteration = 0; HPolyhedron P = domain; HPolyhedron P_prev = domain; - // pre-allocate memory for the polyhedron we are going to construct + // Pre-allocate memory for the polyhedron we are going to construct. // TODO(wernerpe): find better solution than hardcoding 300 Eigen::Matrix A( P.A().rows() + 300, dim); Eigen::VectorXd b(P.A().rows() + 300); - // if (options.verbose) { - // log()->info("IrisZo requires {}/{} particles to be collision free ", - // bernoulli_threshold, options.num_particles); - // } - while (true) { - log()->info("IrisZo iteration {}", iteration); + log()->info("IrisZo outer iteration {}", iteration); Eigen::MatrixXd ATA = current_ellipsoid_A.transpose() * current_ellipsoid_A; - // rescaling makes max step computations more stable - ATA = (dim / ATA.trace()) * ATA; - // initialize polytope with domain + // Initialize polytope with domain. A.topRows(domain.A().rows()) = domain.A(); b.head(domain.A().rows()) = domain.b(); - // Separating Planes Step int num_iterations_separating_planes = 0; - // track maximum relaxation of cspace margin if containment of points is - // requested + // Track maximum relaxation of cspace margin if containment_points are + // requested. double max_relaxation = 0; + double outer_delta = options.delta * 6 / (M_PI * M_PI * (iteration + 1) * (iteration + 1)); @@ -208,6 +195,7 @@ HPolyhedron IrisZo(const planning::CollisionChecker& checker, outer_delta = options.delta; } + // Separating Planes Step. while (num_iterations_separating_planes < options.max_iterations_separating_planes) { int k_squared = num_iterations_separating_planes + 1; @@ -266,7 +254,7 @@ HPolyhedron IrisZo(const planning::CollisionChecker& checker, options.max_iterations_separating_planes - 1) { log()->warn( "IrisZo WARNING, separating planes hit max iterations without " - "passing the bernoulli test, this voids the probabilistic " + "passing the unadaptive test, this voids the probabilistic " "guarantees!"); } @@ -277,6 +265,8 @@ HPolyhedron IrisZo(const planning::CollisionChecker& checker, particles_in_collision_updated.emplace_back(p); } + // For each particle in collision, we run a bisection search to find a + // configuration on the boundary of the obstacle. const auto particle_update_work = [&checker, &particles_in_collision_updated, &particles_in_collision, ¤t_ellipsoid_center, @@ -312,7 +302,7 @@ HPolyhedron IrisZo(const planning::CollisionChecker& checker, } } } - //} + particles_in_collision_updated[point_idx] = current_point; }; // update all particles in parallel @@ -321,10 +311,10 @@ HPolyhedron IrisZo(const planning::CollisionChecker& checker, particle_update_work, ParallelForBackend::BEST_AVAILABLE); - // Resampling particles around found collisions - // TODO(wernerpe): implement optional resampling step + // Resampling particles around found collisions. + // TODO(wernerpe): Implement optional resampling step. - // Place Hyperplanes + // Place Hyperplanes. std::vector particle_distances; particle_distances.reserve(number_particles_in_collision); @@ -334,31 +324,26 @@ HPolyhedron IrisZo(const planning::CollisionChecker& checker, (particle - current_ellipsoid_center)); } - // returned in ascending order + // The indices are returned in ascending order. auto indices_sorted = argsort(particle_distances); - // bools are not threadsafe - using uint8_t instead to accomondate for - // parallel checking + // Bools are not threadsafe - using uint8_t instead to accomondate for + // parallel checking. std::vector particle_is_redundant; for (int i = 0; i < number_particles_in_collision; ++i) { particle_is_redundant.push_back(0); } - // add separating planes step + // Add separating planes. int hyperplanes_added = 0; for (auto i : indices_sorted) { - // add nearest face auto nearest_particle = particles_in_collision_updated[i]; if (!particle_is_redundant[i]) { - // compute face Eigen::VectorXd a_face; if (options.containment_points.has_value()) { a_face = compute_face_tangent_to_dist_cvxh( current_ellipsoid, nearest_particle, cvxh_vpoly); - // std::cout<( 0.2 * options.max_iterations_separating_planes) == diff --git a/planning/iris/iris_zo.h b/planning/iris/iris_zo.h index 3948ec0ef6da..ee0dc9b0d9f2 100644 --- a/planning/iris/iris_zo.h +++ b/planning/iris/iris_zo.h @@ -63,7 +63,9 @@ struct IrisZoOptions { double epsilon = 1e-2; /** Points that are guaranteed to be contained in the final region - * provided their convex hull is collision free. */ + * provided their convex hull is collision free. Note that if the containment + * points are closer than configuration_margin to an obstacle we will relax + * the margin in favor of including the containment points.*/ std::optional containment_points{std::nullopt}; /** Maximum number of alternations between the ellipsoid and the separating From 10dcdd332a7f9b8112e7f2824d2f74b5094620df Mon Sep 17 00:00:00 2001 From: Peter Date: Wed, 13 Nov 2024 15:32:56 -0500 Subject: [PATCH 06/13] fixed test, fixed bindings --- bindings/pydrake/planning/BUILD.bazel | 14 +++++------ .../pydrake/planning/planning_py_iris_zo.cc | 15 ++++++------ planning/iris/BUILD.bazel | 23 +++++++++++++++++++ planning/iris/iris_zo.h | 2 +- planning/iris/test/iris_zo_test.cc | 11 ++++----- 5 files changed, 42 insertions(+), 23 deletions(-) diff --git a/bindings/pydrake/planning/BUILD.bazel b/bindings/pydrake/planning/BUILD.bazel index b60c6ba9e653..0832099f9e27 100644 --- a/bindings/pydrake/planning/BUILD.bazel +++ b/bindings/pydrake/planning/BUILD.bazel @@ -131,13 +131,13 @@ drake_py_unittest( ], ) -drake_py_unittest( - name = "iris_zo_test", - num_threads = 2, - deps = [ - ":planning", - ], -) +# drake_py_unittest( +# name = "iris_zo_test", +# num_threads = 2, +# deps = [ +# ":planning", +# ], +# ) drake_py_unittest( name = "zmp_planner_test", diff --git a/bindings/pydrake/planning/planning_py_iris_zo.cc b/bindings/pydrake/planning/planning_py_iris_zo.cc index f56b7440f02d..fedc8afb4a17 100644 --- a/bindings/pydrake/planning/planning_py_iris_zo.cc +++ b/bindings/pydrake/planning/planning_py_iris_zo.cc @@ -34,7 +34,7 @@ void DefinePlanningIrisZo(py::module m) { .def_readwrite("bisection_steps", &IrisZoOptions::bisection_steps, cls_doc.bisection_steps.doc) .def_readwrite( - "parallelize", &IrisZoOptions::parallelize, cls_doc.parallelize.doc) + "parallelism", &IrisZoOptions::parallelism, cls_doc.parallelism.doc) .def_readwrite("verbose", &IrisZoOptions::verbose, cls_doc.verbose.doc) .def_readwrite("require_sample_point_is_contained", &IrisZoOptions::require_sample_point_is_contained, @@ -59,13 +59,11 @@ void DefinePlanningIrisZo(py::module m) { "tau={}, " "delta={}, " "epsilon={}, " - "force_containment_points={}, " - "num_consecutive_failures={}, " "max_iterations={}, " "max_iterations_separating_planes={}, " "max_separating_planes_per_iteration={}, " "bisection_steps={}, " - "parallelize={}, " + // "parallelism={}, " "verbose={}, " "require_sample_point_is_contained={}, " "configuration_space_margin={}, " @@ -75,17 +73,18 @@ void DefinePlanningIrisZo(py::module m) { "mixing_steps={}, " ")") .format(self.num_particles, self.tau, self.delta, self.epsilon, - self.containment_points, self.max_iterations, self.max_iterations_separating_planes, + self.max_iterations, self.max_iterations_separating_planes, self.max_separating_planes_per_iteration, self.bisection_steps, - self.parallelize, self.verbose, - self.require_sample_point_is_contained, + // self.parallelism, + self.verbose, self.require_sample_point_is_contained, self.configuration_space_margin, self.termination_threshold, self.relative_termination_threshold, self.random_seed, self.mixing_steps); }); m.def("IrisZo", &IrisZo, py::arg("checker"), py::arg("starting_ellipsoid"), - py::arg("domain"), py::arg("options") = IrisZoOptions(), doc.IrisZo.doc); + py::arg("domain"), py::arg("options") = IrisZoOptions(), + py::call_guard(), doc.IrisZo.doc); } } // namespace internal diff --git a/planning/iris/BUILD.bazel b/planning/iris/BUILD.bazel index 77c2a0835b2f..59c8978cce5e 100644 --- a/planning/iris/BUILD.bazel +++ b/planning/iris/BUILD.bazel @@ -90,4 +90,27 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "iris_zo_test", + timeout = "moderate", + data = ["//multibody/parsing:test_models"], + num_threads = 8, + # Most of these tests take an exceptionally long time under + # instrumentation, resulting in timeouts, and so are excluded. + tags = [ + "no_asan", + "no_memcheck", + ], + deps = [ + ":iris_zo", + "//common/test_utilities:expect_throws_message", + "//common/test_utilities:maybe_pause_for_user", + "//geometry:meshcat", + "//geometry/test_utilities:meshcat_environment", + "//multibody/inverse_kinematics", + "//planning:robot_diagram_builder", + "//planning:scene_graph_collision_checker", + ], +) + add_lint_tests() diff --git a/planning/iris/iris_zo.h b/planning/iris/iris_zo.h index ee0dc9b0d9f2..c4d32f4d099c 100644 --- a/planning/iris/iris_zo.h +++ b/planning/iris/iris_zo.h @@ -32,7 +32,7 @@ struct IrisZoOptions { a->Visit(DRAKE_NVP(max_iterations_separating_planes)); a->Visit(DRAKE_NVP(max_separating_planes_per_iteration)); a->Visit(DRAKE_NVP(bisection_steps)); - a->Visit(DRAKE_NVP(parallelism)); + // a->Visit(DRAKE_NVP(parallelism)); a->Visit(DRAKE_NVP(verbose)); a->Visit(DRAKE_NVP(require_sample_point_is_contained)); a->Visit(DRAKE_NVP(configuration_space_margin)); diff --git a/planning/iris/test/iris_zo_test.cc b/planning/iris/test/iris_zo_test.cc index 4b28f8856a78..a2c00d01250c 100644 --- a/planning/iris/test/iris_zo_test.cc +++ b/planning/iris/test/iris_zo_test.cc @@ -54,11 +54,10 @@ HPolyhedron IrisZoFromUrdf(const std::string urdf, plant_ptr->GetPositionLowerLimits(), plant_ptr->GetPositionUpperLimits()); planning::SceneGraphCollisionChecker checker(std::move(params)); - // plant.SetPositions(&plant.GetMyMutableContextFromRoot(context.get()), - // sample); return IrisZo(checker, starting_ellipsoid, domain, options); } +// Reproduced from the IrisInConfigurationSpace unit tests. // One prismatic link with joint limits. Iris should return the joint limits. GTEST_TEST(IrisZoTest, JointLimits) { const std::string limits_urdf = R"( @@ -94,6 +93,7 @@ GTEST_TEST(IrisZoTest, JointLimits) { EXPECT_FALSE(region.PointInSet(Vector1d{qmax + kTol})); } +// Reproduced from the IrisInConfigurationSpace unit tests. // A simple double pendulum with link lengths `l1` and `l2` with a sphere at the // tip of radius `r` between two (fixed) walls at `w` from the origin. The // true configuration space is - w + r ≤ l₁s₁ + l₂s₁₊₂ ≤ w - r. These regions @@ -236,6 +236,7 @@ const char block_urdf[] = R"( )"; +// Reproduced from the IrisInConfigurationSpace unit tests. // A block on a vertical track, free to rotate (in the plane) with width `w` of // 2 and height `h` of 1, plus a ground plane at z=0. The true configuration // space is min(q₀ ± .5w sin(q₁) ± .5h cos(q₁)) ≥ 0, where the min is over the @@ -290,6 +291,7 @@ GTEST_TEST(IrisZoTest, BlockOnGround) { } } +// Reproduced from the IrisInConfigurationSpace unit tests. // A (somewhat contrived) example of a concave configuration-space obstacle // (resulting in a convex configuration-space, which we approximate with // polytopes): A simple pendulum of length `l` with a sphere at the tip of @@ -377,17 +379,12 @@ GTEST_TEST(IrisZoTest, ConvexConfigurationSpace) { options.verbose = true; Hyperellipsoid starting_ellipsoid = Hyperellipsoid::MakeHypersphere(1e-2, sample); - // std::this_thread::sleep_for(std::chrono::milliseconds(100)); HPolyhedron region = IrisZoFromUrdf(convex_urdf, starting_ellipsoid, options); - // TODO(russt): Expecting the test point to be outside the verified region is - // too strong of a requirement right now. If we can improve the algorithm then - // we should make this EXPECT_FALSE. if (!region.PointInSet(Vector2d{z_test, theta_test})) { log()->info("Our test point is not in the set"); } EXPECT_EQ(region.ambient_dimension(), 2); - // Confirm that we've found a substantial region. EXPECT_GE(region.MaximumVolumeInscribedEllipsoid().Volume(), 0.5); { From bd2b3decb6328b91b71cb7c24e0be77ef5a9172e Mon Sep 17 00:00:00 2001 From: Peter Date: Wed, 13 Nov 2024 18:03:23 -0500 Subject: [PATCH 07/13] remove kinf --- bindings/pydrake/planning/planning_py_iris_zo.cc | 6 +++--- planning/iris/test/iris_zo_test.cc | 2 -- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/bindings/pydrake/planning/planning_py_iris_zo.cc b/bindings/pydrake/planning/planning_py_iris_zo.cc index fedc8afb4a17..6879b16f953e 100644 --- a/bindings/pydrake/planning/planning_py_iris_zo.cc +++ b/bindings/pydrake/planning/planning_py_iris_zo.cc @@ -63,7 +63,7 @@ void DefinePlanningIrisZo(py::module m) { "max_iterations_separating_planes={}, " "max_separating_planes_per_iteration={}, " "bisection_steps={}, " - // "parallelism={}, " + "parallelism={}, " "verbose={}, " "require_sample_point_is_contained={}, " "configuration_space_margin={}, " @@ -75,8 +75,8 @@ void DefinePlanningIrisZo(py::module m) { .format(self.num_particles, self.tau, self.delta, self.epsilon, self.max_iterations, self.max_iterations_separating_planes, self.max_separating_planes_per_iteration, self.bisection_steps, - // self.parallelism, - self.verbose, self.require_sample_point_is_contained, + self.parallelism, self.verbose, + self.require_sample_point_is_contained, self.configuration_space_margin, self.termination_threshold, self.relative_termination_threshold, self.random_seed, self.mixing_steps); diff --git a/planning/iris/test/iris_zo_test.cc b/planning/iris/test/iris_zo_test.cc index a2c00d01250c..0d8f253b498c 100644 --- a/planning/iris/test/iris_zo_test.cc +++ b/planning/iris/test/iris_zo_test.cc @@ -30,8 +30,6 @@ using geometry::optimization::Hyperellipsoid; using geometry::optimization::VPolytope; using symbolic::Variable; -const double kInf = std::numeric_limits::infinity(); - // Helper method for testing FastIris from a urdf string. HPolyhedron IrisZoFromUrdf(const std::string urdf, const Hyperellipsoid& starting_ellipsoid, From e276a0f7ea31e03a99df8489d04b3f1a07472c36 Mon Sep 17 00:00:00 2001 From: Peter Date: Wed, 13 Nov 2024 18:05:11 -0500 Subject: [PATCH 08/13] delete parallelism object --- planning/iris/iris_zo.h | 1 - 1 file changed, 1 deletion(-) diff --git a/planning/iris/iris_zo.h b/planning/iris/iris_zo.h index c4d32f4d099c..8570a06ac413 100644 --- a/planning/iris/iris_zo.h +++ b/planning/iris/iris_zo.h @@ -32,7 +32,6 @@ struct IrisZoOptions { a->Visit(DRAKE_NVP(max_iterations_separating_planes)); a->Visit(DRAKE_NVP(max_separating_planes_per_iteration)); a->Visit(DRAKE_NVP(bisection_steps)); - // a->Visit(DRAKE_NVP(parallelism)); a->Visit(DRAKE_NVP(verbose)); a->Visit(DRAKE_NVP(require_sample_point_is_contained)); a->Visit(DRAKE_NVP(configuration_space_margin)); From 230c6e33d8f189ada2fa471b9b962b8de49e8a93 Mon Sep 17 00:00:00 2001 From: Peter Date: Thu, 14 Nov 2024 10:11:50 -0500 Subject: [PATCH 09/13] print statements added in order to try to catch CI fail --- planning/iris/iris_zo.h | 6 ++++-- planning/iris/test/iris_zo_test.cc | 8 +++++++- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/planning/iris/iris_zo.h b/planning/iris/iris_zo.h index 8570a06ac413..fb14d17b7d23 100644 --- a/planning/iris/iris_zo.h +++ b/planning/iris/iris_zo.h @@ -49,7 +49,7 @@ struct IrisZoOptions { /** Descision threshold for the unadaptive test. Chosing a small value * increases both the cost and the power statistical test. Increasing the * value of `tau` makes running an individual test cheaper but decreases its - * power to accept a polytope. We chosing a value of 0.5 is a good + * power to accept a polytope. We find chosing a value of 0.5 a good * trade-off. */ double tau = 0.5; @@ -64,7 +64,9 @@ struct IrisZoOptions { /** Points that are guaranteed to be contained in the final region * provided their convex hull is collision free. Note that if the containment * points are closer than configuration_margin to an obstacle we will relax - * the margin in favor of including the containment points.*/ + * the margin in favor of including the containment points. The matrix + * `containment_points` is expected to be of the shape dimension times number + * of points.*/ std::optional containment_points{std::nullopt}; /** Maximum number of alternations between the ellipsoid and the separating diff --git a/planning/iris/test/iris_zo_test.cc b/planning/iris/test/iris_zo_test.cc index 0d8f253b498c..305c73b33af3 100644 --- a/planning/iris/test/iris_zo_test.cc +++ b/planning/iris/test/iris_zo_test.cc @@ -1,6 +1,7 @@ #include "drake/planning/iris/iris_zo.h" #include +#include #include #include @@ -151,8 +152,10 @@ GTEST_TEST(IrisZoTest, DoublePendulum) { options.meshcat = meshcat; Hyperellipsoid starting_ellipsoid = Hyperellipsoid::MakeHypersphere(1e-2, sample); + std::cout << "pre iris zo call \n"; HPolyhedron region = IrisZoFromUrdf(double_pendulum_urdf, starting_ellipsoid, options); + std::cout << "post iris zo call \n"; EXPECT_EQ(region.ambient_dimension(), 2); // Confirm that we've found a substantial region. @@ -495,7 +498,6 @@ GTEST_TEST(IrisZoTest, ForceContainmentPointsTest) { const Vector2d sample{0.0, 0.0}; Hyperellipsoid starting_ellipsoid = Hyperellipsoid::MakeHypersphere(1e-2, sample); - // std::this_thread::sleep_for(std::chrono::milliseconds(100)); Eigen::Matrix3Xd cont_points(3, 4); double xw, yw; @@ -511,8 +513,10 @@ GTEST_TEST(IrisZoTest, ForceContainmentPointsTest) { options.meshcat = meshcat; options.configuration_space_margin = 0.04; options.containment_points = cont_points.topRows(2); + std::cout << "pre iris zo call \n"; HPolyhedron region = IrisZoFromUrdf(boxes_in_corners_urdf, starting_ellipsoid, options); + std::cout << "post iris zo call \n"; EXPECT_EQ(region.ambient_dimension(), 2); { for (int pt_to_draw = 0; pt_to_draw < cont_points.cols(); ++pt_to_draw) { @@ -528,10 +532,12 @@ GTEST_TEST(IrisZoTest, ForceContainmentPointsTest) { Eigen::Matrix3Xd points = Eigen::Matrix3Xd::Zero(3, 20); VPolytope vregion = VPolytope(region).GetMinimalRepresentation(); + std::cout << "drawing region \n"; points.resize(3, vregion.vertices().cols() + 1); points.topLeftCorner(2, vregion.vertices().cols()) = vregion.vertices(); points.topRightCorner(2, 1) = vregion.vertices().col(0); points.bottomRows<1>().setZero(); + std::cout << "drawing region2 \n"; meshcat->SetLine("IRIS Region", points, 2.0, Rgba(0, 1, 0)); MaybePauseForUser(); From 6dafa3e5a0e41b9a21bea0bb748d954f0183b7b0 Mon Sep 17 00:00:00 2001 From: Peter Date: Thu, 14 Nov 2024 17:53:55 -0500 Subject: [PATCH 10/13] more cleanup --- planning/iris/iris_zo.cc | 16 ++++++++-------- planning/iris/iris_zo.h | 4 ++-- planning/iris/test/iris_zo_test.cc | 7 ++----- 3 files changed, 12 insertions(+), 15 deletions(-) diff --git a/planning/iris/iris_zo.cc b/planning/iris/iris_zo.cc index f71e6dd5b4cd..1f061939cdec 100644 --- a/planning/iris/iris_zo.cc +++ b/planning/iris/iris_zo.cc @@ -82,9 +82,11 @@ HPolyhedron IrisZo(const planning::CollisionChecker& checker, const Hyperellipsoid& starting_ellipsoid, const HPolyhedron& domain, const IrisZoOptions& options) { auto start = std::chrono::high_resolution_clock::now(); - const int num_threads_to_use = checker.SupportsParallelChecking() && - std::min(options.parallelism.num_threads(), - checker.num_allocated_contexts()); + const int num_threads_to_use = + checker.SupportsParallelChecking() + ? std::min(options.parallelism.num_threads(), + checker.num_allocated_contexts()) + : 1; RandomGenerator generator(options.random_seed); @@ -396,11 +398,9 @@ HPolyhedron IrisZo(const planning::CollisionChecker& checker, // set used particle to redundant particle_is_redundant.at(i) = true; -// loop over remaining non-redundant particles and check for -// redundancy -#if defined(_OPENMP) -#pragma omp parallel for num_threads(num_threads_to_use) -#endif + // loop over remaining non-redundant particles and check for + // redundancy + // TODO(cohnt): Revert this back to parallel but with CRU for (int particle_index = 0; particle_index < number_particles_in_collision; ++particle_index) { diff --git a/planning/iris/iris_zo.h b/planning/iris/iris_zo.h index fb14d17b7d23..03c39c6658db 100644 --- a/planning/iris/iris_zo.h +++ b/planning/iris/iris_zo.h @@ -109,7 +109,7 @@ struct IrisZoOptions { This termination condition can be disabled by setting to a negative value. */ double relative_termination_threshold{1e-3}; - /** This option to sets the random seed for random sampling throughout the + /** This option sets the random seed for random sampling throughout the * algorithm. */ int random_seed{1234}; @@ -128,7 +128,7 @@ Order) algorithm, as described in P. Werner, T. Cohn\*, R. H. Jiang\*, T. Seyde, M. Simchowitz, R. Tedrake, and D. Rus, "Faster Algorithms for Growing Collision-Free Convex Polytopes in Robot Configuration Space," -\* Denotes equal contribution. + * Denotes equal contribution. https://groups.csail.mit.edu/robotics-center/public_papers/Werner24.pdf diff --git a/planning/iris/test/iris_zo_test.cc b/planning/iris/test/iris_zo_test.cc index 305c73b33af3..b6ded29dd216 100644 --- a/planning/iris/test/iris_zo_test.cc +++ b/planning/iris/test/iris_zo_test.cc @@ -513,10 +513,8 @@ GTEST_TEST(IrisZoTest, ForceContainmentPointsTest) { options.meshcat = meshcat; options.configuration_space_margin = 0.04; options.containment_points = cont_points.topRows(2); - std::cout << "pre iris zo call \n"; HPolyhedron region = IrisZoFromUrdf(boxes_in_corners_urdf, starting_ellipsoid, options); - std::cout << "post iris zo call \n"; EXPECT_EQ(region.ambient_dimension(), 2); { for (int pt_to_draw = 0; pt_to_draw < cont_points.cols(); ++pt_to_draw) { @@ -524,7 +522,8 @@ GTEST_TEST(IrisZoTest, ForceContainmentPointsTest) { std::string path = fmt::format("cont_pt/{}", pt_to_draw); options.meshcat->SetObject(path, Sphere(0.04), geometry::Rgba(1, 0, 0.0, 1.0)); - point_to_draw.head(2) = cont_points.col(pt_to_draw); + point_to_draw(0) = cont_points(0, pt_to_draw); + point_to_draw(1) = cont_points(1, pt_to_draw); options.meshcat->SetTransform( path, math::RigidTransform(point_to_draw)); EXPECT_TRUE(region.PointInSet(point_to_draw.head(2))); @@ -532,12 +531,10 @@ GTEST_TEST(IrisZoTest, ForceContainmentPointsTest) { Eigen::Matrix3Xd points = Eigen::Matrix3Xd::Zero(3, 20); VPolytope vregion = VPolytope(region).GetMinimalRepresentation(); - std::cout << "drawing region \n"; points.resize(3, vregion.vertices().cols() + 1); points.topLeftCorner(2, vregion.vertices().cols()) = vregion.vertices(); points.topRightCorner(2, 1) = vregion.vertices().col(0); points.bottomRows<1>().setZero(); - std::cout << "drawing region2 \n"; meshcat->SetLine("IRIS Region", points, 2.0, Rgba(0, 1, 0)); MaybePauseForUser(); From 9a472fb71d791f2cdd30ae39fb1166006a9d6023 Mon Sep 17 00:00:00 2001 From: Peter Date: Thu, 14 Nov 2024 17:55:13 -0500 Subject: [PATCH 11/13] more cleanup --- planning/iris/test/iris_zo_test.cc | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/planning/iris/test/iris_zo_test.cc b/planning/iris/test/iris_zo_test.cc index b6ded29dd216..ba283c5c79b6 100644 --- a/planning/iris/test/iris_zo_test.cc +++ b/planning/iris/test/iris_zo_test.cc @@ -1,7 +1,6 @@ #include "drake/planning/iris/iris_zo.h" #include -#include #include #include @@ -152,10 +151,9 @@ GTEST_TEST(IrisZoTest, DoublePendulum) { options.meshcat = meshcat; Hyperellipsoid starting_ellipsoid = Hyperellipsoid::MakeHypersphere(1e-2, sample); - std::cout << "pre iris zo call \n"; + HPolyhedron region = IrisZoFromUrdf(double_pendulum_urdf, starting_ellipsoid, options); - std::cout << "post iris zo call \n"; EXPECT_EQ(region.ambient_dimension(), 2); // Confirm that we've found a substantial region. From 234a73c9c49089fb2ef62fabe20eff0f268c442b Mon Sep 17 00:00:00 2001 From: Peter Date: Thu, 14 Nov 2024 17:57:13 -0500 Subject: [PATCH 12/13] missed minor change --- planning/iris/iris_zo.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/iris/iris_zo.cc b/planning/iris/iris_zo.cc index 1f061939cdec..3fdbc8c6ca06 100644 --- a/planning/iris/iris_zo.cc +++ b/planning/iris/iris_zo.cc @@ -400,7 +400,7 @@ HPolyhedron IrisZo(const planning::CollisionChecker& checker, // loop over remaining non-redundant particles and check for // redundancy - // TODO(cohnt): Revert this back to parallel but with CRU + // TODO(cohnt): Revert this back to parallel but with CRU. for (int particle_index = 0; particle_index < number_particles_in_collision; ++particle_index) { From 8b80a4ccf7c96c1b23a72a79f6072d723e392954 Mon Sep 17 00:00:00 2001 From: Peter Date: Fri, 15 Nov 2024 18:47:15 -0500 Subject: [PATCH 13/13] broken state, tests seem to segfault in the lsan mode before even entering the test case.added print statement right after the GTEST_TEST() and it does not print when it errors out. --- planning/iris/BUILD.bazel | 3 +- planning/iris/iris_zo.cc | 16 +- planning/iris/test/iris_zo_test.cc | 792 +++++++++++++++-------------- 3 files changed, 409 insertions(+), 402 deletions(-) diff --git a/planning/iris/BUILD.bazel b/planning/iris/BUILD.bazel index 59c8978cce5e..c9ee70967bc4 100644 --- a/planning/iris/BUILD.bazel +++ b/planning/iris/BUILD.bazel @@ -94,7 +94,8 @@ drake_cc_googletest( name = "iris_zo_test", timeout = "moderate", data = ["//multibody/parsing:test_models"], - num_threads = 8, + num_threads = 1, + shard_count = 1, # Most of these tests take an exceptionally long time under # instrumentation, resulting in timeouts, and so are excluded. tags = [ diff --git a/planning/iris/iris_zo.cc b/planning/iris/iris_zo.cc index 3fdbc8c6ca06..8e6e7c9a895f 100644 --- a/planning/iris/iris_zo.cc +++ b/planning/iris/iris_zo.cc @@ -87,7 +87,9 @@ HPolyhedron IrisZo(const planning::CollisionChecker& checker, ? std::min(options.parallelism.num_threads(), checker.num_allocated_contexts()) : 1; - + log()->info( + "IrisZo using {} threads.", + num_threads_to_use); RandomGenerator generator(options.random_seed); const Eigen::VectorXd starting_ellipsoid_center = starting_ellipsoid.center(); @@ -278,14 +280,13 @@ HPolyhedron IrisZo(const planning::CollisionChecker& checker, Eigen::VectorXd current_point = start_point; - // update particles via gradient descent and bisection - // find newton descent direction - Eigen::VectorXd grad = (current_point - current_ellipsoid_center); - double max_distance = grad.norm(); - grad.normalize(); + // update particles via bisection + Eigen::VectorXd curr_pt_lower = current_ellipsoid_center; - Eigen::VectorXd curr_pt_lower = current_point - max_distance * grad; // update current point using bisection + if(checker.num_allocated_contexts()<=thread_num){ + throw std::runtime_error(fmt::format("number of threads exceeds maximum {}, {}", thread_num, checker.num_allocated_contexts())); + } if (!checker.CheckConfigCollisionFree(curr_pt_lower, thread_num)) { // directly set to lowerbound current_point = curr_pt_lower; @@ -307,6 +308,7 @@ HPolyhedron IrisZo(const planning::CollisionChecker& checker, particles_in_collision_updated[point_idx] = current_point; }; + // update all particles in parallel DynamicParallelForIndexLoop(DegreeOfParallelism(num_threads_to_use), 0, number_particles_in_collision, diff --git a/planning/iris/test/iris_zo_test.cc b/planning/iris/test/iris_zo_test.cc index ba283c5c79b6..b2ebe679512f 100644 --- a/planning/iris/test/iris_zo_test.cc +++ b/planning/iris/test/iris_zo_test.cc @@ -1,6 +1,7 @@ #include "drake/planning/iris/iris_zo.h" #include +#include #include #include @@ -47,248 +48,249 @@ HPolyhedron IrisZoFromUrdf(const std::string urdf, params.model = builder.Build(); params.edge_step_size = 0.01; - + std::cout<<"plant built \n"; HPolyhedron domain = HPolyhedron::MakeBox( plant_ptr->GetPositionLowerLimits(), plant_ptr->GetPositionUpperLimits()); - + std::cout<<"constructing checker \n"; planning::SceneGraphCollisionChecker checker(std::move(params)); + std::cout<<"Calling iris zo \n"; return IrisZo(checker, starting_ellipsoid, domain, options); } // Reproduced from the IrisInConfigurationSpace unit tests. // One prismatic link with joint limits. Iris should return the joint limits. -GTEST_TEST(IrisZoTest, JointLimits) { - const std::string limits_urdf = R"( - - - - - - - - - - - - - -)"; - - const Vector1d sample = Vector1d::Zero(); - Hyperellipsoid starting_ellipsoid = - Hyperellipsoid::MakeHypersphere(1e-2, sample); - IrisZoOptions options; - options.verbose = true; - HPolyhedron region = IrisZoFromUrdf(limits_urdf, starting_ellipsoid, options); - - EXPECT_EQ(region.ambient_dimension(), 1); - - const double kTol = 1e-5; - const double qmin = -2.0, qmax = 2.0; - EXPECT_TRUE(region.PointInSet(Vector1d{qmin + kTol})); - EXPECT_TRUE(region.PointInSet(Vector1d{qmax - kTol})); - EXPECT_FALSE(region.PointInSet(Vector1d{qmin - kTol})); - EXPECT_FALSE(region.PointInSet(Vector1d{qmax + kTol})); -} - -// Reproduced from the IrisInConfigurationSpace unit tests. -// A simple double pendulum with link lengths `l1` and `l2` with a sphere at the -// tip of radius `r` between two (fixed) walls at `w` from the origin. The -// true configuration space is - w + r ≤ l₁s₁ + l₂s₁₊₂ ≤ w - r. These regions -// are visualized at https://www.desmos.com/calculator/ff0hbnkqhm. -GTEST_TEST(IrisZoTest, DoublePendulum) { - const double l1 = 2.0; - const double l2 = 1.0; - const double r = .5; - const double w = 1.83; - const std::string double_pendulum_urdf = fmt::format( - R"( - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -)", - fmt::arg("w_plus_one_half", w + .5), fmt::arg("l1", l1), - fmt::arg("l2", l2), fmt::arg("r", r)); - - const Vector2d sample = Vector2d::Zero(); - std::shared_ptr meshcat = geometry::GetTestEnvironmentMeshcat(); - meshcat->Delete("face_pt"); - IrisZoOptions options; - options.verbose = true; - options.meshcat = meshcat; - Hyperellipsoid starting_ellipsoid = - Hyperellipsoid::MakeHypersphere(1e-2, sample); - - HPolyhedron region = - IrisZoFromUrdf(double_pendulum_urdf, starting_ellipsoid, options); - - EXPECT_EQ(region.ambient_dimension(), 2); - // Confirm that we've found a substantial region. - EXPECT_GE(region.MaximumVolumeInscribedEllipsoid().Volume(), 2.0); - - EXPECT_TRUE(region.PointInSet(Vector2d{.4, 0.0})); - EXPECT_FALSE(region.PointInSet(Vector2d{.5, 0.0})); - EXPECT_TRUE(region.PointInSet(Vector2d{.3, .3})); - EXPECT_FALSE(region.PointInSet(Vector2d{.4, .3})); - EXPECT_TRUE(region.PointInSet(Vector2d{-.4, 0.0})); - EXPECT_FALSE(region.PointInSet(Vector2d{-.5, 0.0})); - EXPECT_TRUE(region.PointInSet(Vector2d{-.3, -.3})); - EXPECT_FALSE(region.PointInSet(Vector2d{-.4, -.3})); - - { - meshcat->Set2dRenderMode(math::RigidTransformd(Eigen::Vector3d{0, 0, 1}), - -3.25, 3.25, -3.25, 3.25); - meshcat->SetProperty("/Grid", "visible", true); - Eigen::RowVectorXd theta2s = - Eigen::RowVectorXd::LinSpaced(100, -1.57, 1.57); - Eigen::Matrix3Xd points = Eigen::Matrix3Xd::Zero(3, 2 * theta2s.size() + 1); - const double c = -w + r; - for (int i = 0; i < theta2s.size(); ++i) { - const double a = l1 + l2 * std::cos(theta2s[i]), - b = l2 * std::sin(theta2s[i]); - // wolfram solve a*sin(q) + b*cos(q) = c for q - points(0, i) = - 2 * std::atan((std::sqrt(a * a + b * b - c * c) + a) / (b + c)) + - M_PI; - points(1, i) = theta2s[i]; - points(0, points.cols() - i - 2) = - 2 * std::atan((std::sqrt(a * a + b * b - c * c) + a) / (b - c)) - - M_PI; - points(1, points.cols() - i - 2) = theta2s[i]; - } - points.col(points.cols() - 1) = points.col(0); - meshcat->SetLine("True C_free", points, 2.0, Rgba(0, 0, 1)); - VPolytope vregion = VPolytope(region).GetMinimalRepresentation(); - points.resize(3, vregion.vertices().cols() + 1); - points.topLeftCorner(2, vregion.vertices().cols()) = vregion.vertices(); - points.topRightCorner(2, 1) = vregion.vertices().col(0); - points.bottomRows<1>().setZero(); - meshcat->SetLine("IRIS Region", points, 2.0, Rgba(0, 1, 0)); - - MaybePauseForUser(); - } -} - -const char block_urdf[] = R"( - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -)"; - -// Reproduced from the IrisInConfigurationSpace unit tests. -// A block on a vertical track, free to rotate (in the plane) with width `w` of -// 2 and height `h` of 1, plus a ground plane at z=0. The true configuration -// space is min(q₀ ± .5w sin(q₁) ± .5h cos(q₁)) ≥ 0, where the min is over the -// ±. This region is also visualized at -// https://www.desmos.com/calculator/ok5ckpa1kp. -GTEST_TEST(IrisZoTest, BlockOnGround) { - const Vector2d sample{1.0, 0.0}; - std::shared_ptr meshcat = geometry::GetTestEnvironmentMeshcat(); - meshcat->Delete("face_pt"); - IrisZoOptions options; - options.verbose = true; - options.meshcat = meshcat; - Hyperellipsoid starting_ellipsoid = - Hyperellipsoid::MakeHypersphere(1e-2, sample); - HPolyhedron region = IrisZoFromUrdf(block_urdf, starting_ellipsoid, options); - - EXPECT_EQ(region.ambient_dimension(), 2); - // Confirm that we've found a substantial region. - EXPECT_GE(region.MaximumVolumeInscribedEllipsoid().Volume(), 2.0); - - { - meshcat->Set2dRenderMode(math::RigidTransformd(Eigen::Vector3d{0, 0, 1}), 0, - 3.25, -3.25, 3.25); - meshcat->SetProperty("/Grid", "visible", true); - Eigen::RowVectorXd thetas = Eigen::RowVectorXd::LinSpaced(100, -M_PI, M_PI); - const double w = 2, h = 1; - Eigen::Matrix3Xd points = Eigen::Matrix3Xd::Zero(3, 2 * thetas.size() + 1); - for (int i = 0; i < thetas.size(); ++i) { - const double a = 0.5 * - (-w * std::sin(thetas[i]) - h * std::cos(thetas[i])), - b = 0.5 * - (-w * std::sin(thetas[i]) + h * std::cos(thetas[i])), - c = 0.5 * - (+w * std::sin(thetas[i]) - h * std::cos(thetas[i])), - d = 0.5 * - (+w * std::sin(thetas[i]) + h * std::cos(thetas[i])); - points(0, i) = std::max({a, b, c, d}); - points(1, i) = thetas[i]; - points(0, points.cols() - i - 2) = 3.0; - points(1, points.cols() - i - 2) = thetas[i]; - } - points.col(points.cols() - 1) = points.col(0); - meshcat->SetLine("True C_free", points, 2.0, Rgba(0, 0, 1)); - VPolytope vregion = VPolytope(region).GetMinimalRepresentation(); - points.resize(3, vregion.vertices().cols() + 1); - points.topLeftCorner(2, vregion.vertices().cols()) = vregion.vertices(); - points.topRightCorner(2, 1) = vregion.vertices().col(0); - points.bottomRows<1>().setZero(); - meshcat->SetLine("IRIS Region", points, 2.0, Rgba(0, 1, 0)); - - MaybePauseForUser(); - } -} +// GTEST_TEST(IrisZoTest, JointLimits) { +// const std::string limits_urdf = R"( +// +// +// +// +// +// +// +// +// +// +// +// +// +// )"; + +// const Vector1d sample = Vector1d::Zero(); +// Hyperellipsoid starting_ellipsoid = +// Hyperellipsoid::MakeHypersphere(1e-2, sample); +// IrisZoOptions options; +// options.verbose = true; +// HPolyhedron region = IrisZoFromUrdf(limits_urdf, starting_ellipsoid, options); + +// EXPECT_EQ(region.ambient_dimension(), 1); + +// const double kTol = 1e-5; +// const double qmin = -2.0, qmax = 2.0; +// EXPECT_TRUE(region.PointInSet(Vector1d{qmin + kTol})); +// EXPECT_TRUE(region.PointInSet(Vector1d{qmax - kTol})); +// EXPECT_FALSE(region.PointInSet(Vector1d{qmin - kTol})); +// EXPECT_FALSE(region.PointInSet(Vector1d{qmax + kTol})); +// } + +// // Reproduced from the IrisInConfigurationSpace unit tests. +// // A simple double pendulum with link lengths `l1` and `l2` with a sphere at the +// // tip of radius `r` between two (fixed) walls at `w` from the origin. The +// // true configuration space is - w + r ≤ l₁s₁ + l₂s₁₊₂ ≤ w - r. These regions +// // are visualized at https://www.desmos.com/calculator/ff0hbnkqhm. +// GTEST_TEST(IrisZoTest, DoublePendulum) { +// const double l1 = 2.0; +// const double l2 = 1.0; +// const double r = .5; +// const double w = 1.83; +// const std::string double_pendulum_urdf = fmt::format( +// R"( +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// )", +// fmt::arg("w_plus_one_half", w + .5), fmt::arg("l1", l1), +// fmt::arg("l2", l2), fmt::arg("r", r)); + +// const Vector2d sample = Vector2d::Zero(); +// std::shared_ptr meshcat = geometry::GetTestEnvironmentMeshcat(); +// meshcat->Delete("face_pt"); +// IrisZoOptions options; +// options.verbose = true; +// options.meshcat = meshcat; +// Hyperellipsoid starting_ellipsoid = +// Hyperellipsoid::MakeHypersphere(1e-2, sample); + +// HPolyhedron region = +// IrisZoFromUrdf(double_pendulum_urdf, starting_ellipsoid, options); + +// EXPECT_EQ(region.ambient_dimension(), 2); +// // Confirm that we've found a substantial region. +// EXPECT_GE(region.MaximumVolumeInscribedEllipsoid().Volume(), 2.0); + +// EXPECT_TRUE(region.PointInSet(Vector2d{.4, 0.0})); +// EXPECT_FALSE(region.PointInSet(Vector2d{.5, 0.0})); +// EXPECT_TRUE(region.PointInSet(Vector2d{.3, .3})); +// EXPECT_FALSE(region.PointInSet(Vector2d{.4, .3})); +// EXPECT_TRUE(region.PointInSet(Vector2d{-.4, 0.0})); +// EXPECT_FALSE(region.PointInSet(Vector2d{-.5, 0.0})); +// EXPECT_TRUE(region.PointInSet(Vector2d{-.3, -.3})); +// EXPECT_FALSE(region.PointInSet(Vector2d{-.4, -.3})); + +// { +// meshcat->Set2dRenderMode(math::RigidTransformd(Eigen::Vector3d{0, 0, 1}), +// -3.25, 3.25, -3.25, 3.25); +// meshcat->SetProperty("/Grid", "visible", true); +// Eigen::RowVectorXd theta2s = +// Eigen::RowVectorXd::LinSpaced(100, -1.57, 1.57); +// Eigen::Matrix3Xd points = Eigen::Matrix3Xd::Zero(3, 2 * theta2s.size() + 1); +// const double c = -w + r; +// for (int i = 0; i < theta2s.size(); ++i) { +// const double a = l1 + l2 * std::cos(theta2s[i]), +// b = l2 * std::sin(theta2s[i]); +// // wolfram solve a*sin(q) + b*cos(q) = c for q +// points(0, i) = +// 2 * std::atan((std::sqrt(a * a + b * b - c * c) + a) / (b + c)) + +// M_PI; +// points(1, i) = theta2s[i]; +// points(0, points.cols() - i - 2) = +// 2 * std::atan((std::sqrt(a * a + b * b - c * c) + a) / (b - c)) - +// M_PI; +// points(1, points.cols() - i - 2) = theta2s[i]; +// } +// points.col(points.cols() - 1) = points.col(0); +// meshcat->SetLine("True C_free", points, 2.0, Rgba(0, 0, 1)); +// VPolytope vregion = VPolytope(region).GetMinimalRepresentation(); +// points.resize(3, vregion.vertices().cols() + 1); +// points.topLeftCorner(2, vregion.vertices().cols()) = vregion.vertices(); +// points.topRightCorner(2, 1) = vregion.vertices().col(0); +// points.bottomRows<1>().setZero(); +// meshcat->SetLine("IRIS Region", points, 2.0, Rgba(0, 1, 0)); + +// MaybePauseForUser(); +// } +// } + +// const char block_urdf[] = R"( +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// )"; + +// // Reproduced from the IrisInConfigurationSpace unit tests. +// // A block on a vertical track, free to rotate (in the plane) with width `w` of +// // 2 and height `h` of 1, plus a ground plane at z=0. The true configuration +// // space is min(q₀ ± .5w sin(q₁) ± .5h cos(q₁)) ≥ 0, where the min is over the +// // ±. This region is also visualized at +// // https://www.desmos.com/calculator/ok5ckpa1kp. +// GTEST_TEST(IrisZoTest, BlockOnGround) { +// const Vector2d sample{1.0, 0.0}; +// std::shared_ptr meshcat = geometry::GetTestEnvironmentMeshcat(); +// meshcat->Delete("face_pt"); +// IrisZoOptions options; +// options.verbose = true; +// options.meshcat = meshcat; +// Hyperellipsoid starting_ellipsoid = +// Hyperellipsoid::MakeHypersphere(1e-2, sample); +// HPolyhedron region = IrisZoFromUrdf(block_urdf, starting_ellipsoid, options); + +// EXPECT_EQ(region.ambient_dimension(), 2); +// // Confirm that we've found a substantial region. +// EXPECT_GE(region.MaximumVolumeInscribedEllipsoid().Volume(), 2.0); + +// { +// meshcat->Set2dRenderMode(math::RigidTransformd(Eigen::Vector3d{0, 0, 1}), 0, +// 3.25, -3.25, 3.25); +// meshcat->SetProperty("/Grid", "visible", true); +// Eigen::RowVectorXd thetas = Eigen::RowVectorXd::LinSpaced(100, -M_PI, M_PI); +// const double w = 2, h = 1; +// Eigen::Matrix3Xd points = Eigen::Matrix3Xd::Zero(3, 2 * thetas.size() + 1); +// for (int i = 0; i < thetas.size(); ++i) { +// const double a = 0.5 * +// (-w * std::sin(thetas[i]) - h * std::cos(thetas[i])), +// b = 0.5 * +// (-w * std::sin(thetas[i]) + h * std::cos(thetas[i])), +// c = 0.5 * +// (+w * std::sin(thetas[i]) - h * std::cos(thetas[i])), +// d = 0.5 * +// (+w * std::sin(thetas[i]) + h * std::cos(thetas[i])); +// points(0, i) = std::max({a, b, c, d}); +// points(1, i) = thetas[i]; +// points(0, points.cols() - i - 2) = 3.0; +// points(1, points.cols() - i - 2) = thetas[i]; +// } +// points.col(points.cols() - 1) = points.col(0); +// meshcat->SetLine("True C_free", points, 2.0, Rgba(0, 0, 1)); +// VPolytope vregion = VPolytope(region).GetMinimalRepresentation(); +// points.resize(3, vregion.vertices().cols() + 1); +// points.topLeftCorner(2, vregion.vertices().cols()) = vregion.vertices(); +// points.topRightCorner(2, 1) = vregion.vertices().col(0); +// points.bottomRows<1>().setZero(); +// meshcat->SetLine("IRIS Region", points, 2.0, Rgba(0, 1, 0)); + +// MaybePauseForUser(); +// } +// } // Reproduced from the IrisInConfigurationSpace unit tests. // A (somewhat contrived) example of a concave configuration-space obstacle @@ -301,106 +303,106 @@ GTEST_TEST(IrisZoTest, BlockOnGround) { // convex space, this was originally a test for which Ibex found // counter-examples that Snopt missed; now Snopt succeeds due to having // options.num_collision_infeasible_samples > 1. -GTEST_TEST(IrisZoTest, ConvexConfigurationSpace) { - const double l = 1.5; - const double r = 0.1; - - std::shared_ptr meshcat = geometry::GetTestEnvironmentMeshcat(); - meshcat->Delete("face_pt"); - meshcat->Set2dRenderMode(math::RigidTransformd(Eigen::Vector3d{0, 0, 1}), - -3.25, 3.25, -3.25, 3.25); - meshcat->SetProperty("/Grid", "visible", true); - Eigen::RowVectorXd theta1s = Eigen::RowVectorXd::LinSpaced(100, -1.5, 1.5); - Eigen::Matrix3Xd points = Eigen::Matrix3Xd::Zero(3, 2 * theta1s.size()); - for (int i = 0; i < theta1s.size(); ++i) { - points(0, i) = r - l * cos(theta1s[i]); - points(1, i) = theta1s[i]; - points(0, points.cols() - i - 1) = 0; - points(1, points.cols() - i - 1) = theta1s[i]; - } - meshcat->SetLine("True C_free", points, 2.0, Rgba(0, 0, 1)); - - const std::string convex_urdf = fmt::format( - R"( - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -)", - fmt::arg("l", l), fmt::arg("r", r)); - - const Vector2d sample{-0.5, 0.0}; - IrisZoOptions options; - - // This point should be outside of the configuration space (in collision). - // The particular value was found by visual inspection using meshcat. - const double z_test = 0, theta_test = -1.55; - // Confirm that the pendulum is colliding with the wall with true kinematics: - EXPECT_LE(z_test + l * std::cos(theta_test), r); - - // Turn on meshcat for addition debugging visualizations. - // This example is truly adversarial for IRIS. After one iteration, the - // maximum-volume inscribed ellipse is approximately centered in C-free. So - // finding a counter-example in the bottom corner (near the test point) is - // not only difficult because we need to sample in a corner of the polytope, - // but because the objective is actually pulling the counter-example search - // away from that corner. Open the meshcat visualization to step through the - // details! - options.meshcat = meshcat; - options.verbose = true; - Hyperellipsoid starting_ellipsoid = - Hyperellipsoid::MakeHypersphere(1e-2, sample); - HPolyhedron region = IrisZoFromUrdf(convex_urdf, starting_ellipsoid, options); - if (!region.PointInSet(Vector2d{z_test, theta_test})) { - log()->info("Our test point is not in the set"); - } - - EXPECT_EQ(region.ambient_dimension(), 2); - EXPECT_GE(region.MaximumVolumeInscribedEllipsoid().Volume(), 0.5); - - { - VPolytope vregion = VPolytope(region).GetMinimalRepresentation(); - points.resize(3, vregion.vertices().cols() + 1); - points.topLeftCorner(2, vregion.vertices().cols()) = vregion.vertices(); - points.topRightCorner(2, 1) = vregion.vertices().col(0); - points.bottomRows<1>().setZero(); - meshcat->SetLine("IRIS Region", points, 2.0, Rgba(0, 1, 0)); - - meshcat->SetObject("Test point", Sphere(0.03), Rgba(1, 0, 0)); - meshcat->SetTransform("Test point", math::RigidTransform(Eigen::Vector3d( - z_test, theta_test, 0))); - - MaybePauseForUser(); - } -} +// GTEST_TEST(IrisZoTest, ConvexConfigurationSpace) { +// const double l = 1.5; +// const double r = 0.1; + +// std::shared_ptr meshcat = geometry::GetTestEnvironmentMeshcat(); +// meshcat->Delete("face_pt"); +// meshcat->Set2dRenderMode(math::RigidTransformd(Eigen::Vector3d{0, 0, 1}), +// -3.25, 3.25, -3.25, 3.25); +// meshcat->SetProperty("/Grid", "visible", true); +// Eigen::RowVectorXd theta1s = Eigen::RowVectorXd::LinSpaced(100, -1.5, 1.5); +// Eigen::Matrix3Xd points = Eigen::Matrix3Xd::Zero(3, 2 * theta1s.size()); +// for (int i = 0; i < theta1s.size(); ++i) { +// points(0, i) = r - l * cos(theta1s[i]); +// points(1, i) = theta1s[i]; +// points(0, points.cols() - i - 1) = 0; +// points(1, points.cols() - i - 1) = theta1s[i]; +// } +// meshcat->SetLine("True C_free", points, 2.0, Rgba(0, 0, 1)); + +// const std::string convex_urdf = fmt::format( +// R"( +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// +// )", +// fmt::arg("l", l), fmt::arg("r", r)); + +// const Vector2d sample{-0.5, 0.0}; +// IrisZoOptions options; + +// // This point should be outside of the configuration space (in collision). +// // The particular value was found by visual inspection using meshcat. +// const double z_test = 0, theta_test = -1.55; +// // Confirm that the pendulum is colliding with the wall with true kinematics: +// EXPECT_LE(z_test + l * std::cos(theta_test), r); + +// // Turn on meshcat for addition debugging visualizations. +// // This example is truly adversarial for IRIS. After one iteration, the +// // maximum-volume inscribed ellipse is approximately centered in C-free. So +// // finding a counter-example in the bottom corner (near the test point) is +// // not only difficult because we need to sample in a corner of the polytope, +// // but because the objective is actually pulling the counter-example search +// // away from that corner. Open the meshcat visualization to step through the +// // details! +// options.meshcat = meshcat; +// options.verbose = true; +// Hyperellipsoid starting_ellipsoid = +// Hyperellipsoid::MakeHypersphere(1e-2, sample); +// HPolyhedron region = IrisZoFromUrdf(convex_urdf, starting_ellipsoid, options); +// if (!region.PointInSet(Vector2d{z_test, theta_test})) { +// log()->info("Our test point is not in the set"); +// } + +// EXPECT_EQ(region.ambient_dimension(), 2); +// EXPECT_GE(region.MaximumVolumeInscribedEllipsoid().Volume(), 0.5); + +// { +// VPolytope vregion = VPolytope(region).GetMinimalRepresentation(); +// points.resize(3, vregion.vertices().cols() + 1); +// points.topLeftCorner(2, vregion.vertices().cols()) = vregion.vertices(); +// points.topRightCorner(2, 1) = vregion.vertices().col(0); +// points.bottomRows<1>().setZero(); +// meshcat->SetLine("IRIS Region", points, 2.0, Rgba(0, 1, 0)); + +// meshcat->SetObject("Test point", Sphere(0.03), Rgba(1, 0, 0)); +// meshcat->SetTransform("Test point", math::RigidTransform(Eigen::Vector3d( +// z_test, theta_test, 0))); + +// MaybePauseForUser(); +// } +// } /* A movable sphere with fixed boxes in all corners. ┌───────────────┐ │┌────┐ ┌────┐│ @@ -456,43 +458,44 @@ const char boxes_in_corners_urdf[] = R"( )"; GTEST_TEST(IrisZoTest, ForceContainmentPointsTest) { - std::shared_ptr meshcat; - meshcat = geometry::GetTestEnvironmentMeshcat(); - meshcat->Delete("face_pt"); - meshcat->Delete("True C_free"); - meshcat->Delete("Test point"); - meshcat->Set2dRenderMode(math::RigidTransformd(Eigen::Vector3d{0, 0, 1}), - -3.25, 3.25, -3.25, 3.25); - meshcat->SetProperty("/Grid", "visible", true); + std::cout<<"starting test \n"; +// std::shared_ptr meshcat; +// meshcat = geometry::GetTestEnvironmentMeshcat(); +// meshcat->Delete("face_pt"); +// meshcat->Delete("True C_free"); +// meshcat->Delete("Test point"); +// meshcat->Set2dRenderMode(math::RigidTransformd(Eigen::Vector3d{0, 0, 1}), +// -3.25, 3.25, -3.25, 3.25); +// meshcat->SetProperty("/Grid", "visible", true); // Draw the true cspace. - Eigen::Matrix3Xd env_points(3, 5); - // clang-format off - env_points << -2, 2, 2, -2, -2, - 2, 2, -2, -2, 2, - 0, 0, 0, 0, 0; - // clang-format on - meshcat->SetLine("Domain", env_points, 8.0, Rgba(0, 0, 0)); - Eigen::Matrix3Xd centers(3, 4); - double c = 1.0; - // clang-format off - centers << -c, c, c, -c, - c, c, -c, -c, - 0, 0, 0, 0; - // clang-format on - Eigen::Matrix3Xd obs_points(3, 5); - // approximating offset due to sphere radius with fixed offset - double s = 0.7 + 0.01; - // clang-format off - obs_points << -s, s, s, -s, -s, - s, s, -s, -s, s, - s, 0, 0, 0, 0; - // clang-format on - for (int obstacle_idx = 0; obstacle_idx < 4; ++obstacle_idx) { - Eigen::Matrix3Xd obstacle = obs_points; - obstacle.colwise() += centers.col(obstacle_idx); - meshcat->SetLine(fmt::format("/obstacles/obs_{}", obstacle_idx), obstacle, - 8.0, Rgba(0, 0, 0)); - } +// Eigen::Matrix3Xd env_points(3, 5); +// // clang-format off +// env_points << -2, 2, 2, -2, -2, +// 2, 2, -2, -2, 2, +// 0, 0, 0, 0, 0; +// // clang-format on +// meshcat->SetLine("Domain", env_points, 8.0, Rgba(0, 0, 0)); +// Eigen::Matrix3Xd centers(3, 4); +// double c = 1.0; +// // clang-format off +// centers << -c, c, c, -c, +// c, c, -c, -c, +// 0, 0, 0, 0; +// // clang-format on +// Eigen::Matrix3Xd obs_points(3, 5); +// // approximating offset due to sphere radius with fixed offset +// double s = 0.7 + 0.01; +// // clang-format off +// obs_points << -s, s, s, -s, -s, +// s, s, -s, -s, s, +// s, 0, 0, 0, 0; +// // clang-format on +// for (int obstacle_idx = 0; obstacle_idx < 4; ++obstacle_idx) { +// Eigen::Matrix3Xd obstacle = obs_points; +// obstacle.colwise() += centers.col(obstacle_idx); +// meshcat->SetLine(fmt::format("/obstacles/obs_{}", obstacle_idx), obstacle, +// 8.0, Rgba(0, 0, 0)); +// } const Vector2d sample{0.0, 0.0}; Hyperellipsoid starting_ellipsoid = Hyperellipsoid::MakeHypersphere(1e-2, sample); @@ -508,35 +511,36 @@ GTEST_TEST(IrisZoTest, ForceContainmentPointsTest) { // clang-format on IrisZoOptions options; options.verbose = true; - options.meshcat = meshcat; +// options.meshcat = meshcat; options.configuration_space_margin = 0.04; options.containment_points = cont_points.topRows(2); + std::cout<<"calling fn \n"; HPolyhedron region = IrisZoFromUrdf(boxes_in_corners_urdf, starting_ellipsoid, options); EXPECT_EQ(region.ambient_dimension(), 2); - { - for (int pt_to_draw = 0; pt_to_draw < cont_points.cols(); ++pt_to_draw) { - Eigen::Vector3d point_to_draw = Eigen::Vector3d::Zero(); - std::string path = fmt::format("cont_pt/{}", pt_to_draw); - options.meshcat->SetObject(path, Sphere(0.04), - geometry::Rgba(1, 0, 0.0, 1.0)); - point_to_draw(0) = cont_points(0, pt_to_draw); - point_to_draw(1) = cont_points(1, pt_to_draw); - options.meshcat->SetTransform( - path, math::RigidTransform(point_to_draw)); - EXPECT_TRUE(region.PointInSet(point_to_draw.head(2))); - } - Eigen::Matrix3Xd points = Eigen::Matrix3Xd::Zero(3, 20); - - VPolytope vregion = VPolytope(region).GetMinimalRepresentation(); - points.resize(3, vregion.vertices().cols() + 1); - points.topLeftCorner(2, vregion.vertices().cols()) = vregion.vertices(); - points.topRightCorner(2, 1) = vregion.vertices().col(0); - points.bottomRows<1>().setZero(); - meshcat->SetLine("IRIS Region", points, 2.0, Rgba(0, 1, 0)); - - MaybePauseForUser(); - } +// { +// for (int pt_to_draw = 0; pt_to_draw < cont_points.cols(); ++pt_to_draw) { +// Eigen::Vector3d point_to_draw = Eigen::Vector3d::Zero(); +// std::string path = fmt::format("cont_pt/{}", pt_to_draw); +// options.meshcat->SetObject(path, Sphere(0.04), +// geometry::Rgba(1, 0, 0.0, 1.0)); +// point_to_draw(0) = cont_points(0, pt_to_draw); +// point_to_draw(1) = cont_points(1, pt_to_draw); +// options.meshcat->SetTransform( +// path, math::RigidTransform(point_to_draw)); +// EXPECT_TRUE(region.PointInSet(point_to_draw.head(2))); +// } +// Eigen::Matrix3Xd points = Eigen::Matrix3Xd::Zero(3, 20); + +// VPolytope vregion = VPolytope(region).GetMinimalRepresentation(); +// points.resize(3, vregion.vertices().cols() + 1); +// points.topLeftCorner(2, vregion.vertices().cols()) = vregion.vertices(); +// points.topRightCorner(2, 1) = vregion.vertices().col(0); +// points.bottomRows<1>().setZero(); +// meshcat->SetLine("IRIS Region", points, 2.0, Rgba(0, 1, 0)); + +// MaybePauseForUser(); +// } } } // namespace