diff --git a/geometry/optimization/BUILD.bazel b/geometry/optimization/BUILD.bazel index 733d67e26d4a..34f7e1990ef0 100644 --- a/geometry/optimization/BUILD.bazel +++ b/geometry/optimization/BUILD.bazel @@ -49,6 +49,11 @@ drake_cc_library( deps = [ ":convex_set", "//geometry:scene_graph", + "//multibody/parsing:parser", + "//multibody/plant", + "//solvers:ibex_solver", + "//solvers:snopt_solver", + "//systems/framework:diagram_builder", ], ) diff --git a/geometry/optimization/iris.cc b/geometry/optimization/iris.cc index 0fbbeecebceb..4d0aa14f93a7 100644 --- a/geometry/optimization/iris.cc +++ b/geometry/optimization/iris.cc @@ -1,12 +1,17 @@ #include "drake/geometry/optimization/iris.h" #include +#include #include +#include #include #include #include #include "drake/geometry/optimization/convex_set.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/solvers/ibex_solver.h" +#include "drake/solvers/snopt_solver.h" namespace drake { namespace geometry { @@ -14,7 +19,9 @@ namespace optimization { using Eigen::MatrixXd; using Eigen::Ref; +using Eigen::Vector3d; using Eigen::VectorXd; +using symbolic::Expression; HPolyhedron Iris(const ConvexSets& obstacles, const Ref& sample, const HPolyhedron& domain, const IrisOptions& options) { @@ -106,43 +113,47 @@ class IrisConvexSetMaker final : public ShapeReifier { std::optional reference_frame) : query_{query}, reference_frame_{reference_frame} {}; + void set_reference_frame(const FrameId& reference_frame) { + DRAKE_DEMAND(reference_frame.is_valid()); + *reference_frame_ = reference_frame; + } + + void set_geometry_id(const GeometryId& geom_id) { geom_id_ = geom_id; } + using ShapeReifier::ImplementGeometry; void ImplementGeometry(const Sphere&, void* data) { - GeometryId& geom_id = *static_cast(data); - sets_.emplace_back( - std::make_unique(query_, geom_id, reference_frame_)); + DRAKE_DEMAND(geom_id_.is_valid()); + auto& set = *static_cast*>(data); + set = std::make_unique(query_, geom_id_, reference_frame_); } void ImplementGeometry(const HalfSpace&, void* data) { - GeometryId& geom_id = *static_cast(data); - sets_.emplace_back( - std::make_unique(query_, geom_id, reference_frame_)); + DRAKE_DEMAND(geom_id_.is_valid()); + auto& set = *static_cast*>(data); + set = std::make_unique(query_, geom_id_, reference_frame_); } void ImplementGeometry(const Box&, void* data) { - GeometryId& geom_id = *static_cast(data); + DRAKE_DEMAND(geom_id_.is_valid()); + auto& set = *static_cast*>(data); // Note: We choose HPolyhedron over VPolytope here, but the IRIS paper // discusses a significant performance improvement using a "least-distance // programming" instance from CVXGEN that exploited the VPolytope // representation. So we may wish to revisit this. - sets_.emplace_back( - std::make_unique(query_, geom_id, reference_frame_)); + set = std::make_unique(query_, geom_id_, reference_frame_); } void ImplementGeometry(const Ellipsoid&, void* data) { - GeometryId& geom_id = *static_cast(data); - sets_.emplace_back( - std::make_unique(query_, geom_id, reference_frame_)); + DRAKE_DEMAND(geom_id_.is_valid()); + auto& set = *static_cast*>(data); + set = std::make_unique(query_, geom_id_, reference_frame_); } - // This must be called last; it transfers ownership. - ConvexSets claim_sets() { return std::move(sets_); } - private: const QueryObject& query_{}; std::optional reference_frame_{}; - ConvexSets sets_{}; + GeometryId geom_id_{}; }; } // namespace @@ -153,12 +164,216 @@ ConvexSets MakeIrisObstacles(const QueryObject& query_object, const GeometrySet all_ids(inspector.GetAllGeometryIds()); const std::unordered_set geom_ids = inspector.GetGeometryIds(all_ids, Role::kProximity); + ConvexSets sets(geom_ids.size()); IrisConvexSetMaker maker(query_object, reference_frame); + int count = 0; for (GeometryId geom_id : geom_ids) { - inspector.GetShape(geom_id).Reify(&maker, &geom_id); + maker.set_geometry_id(geom_id); + inspector.GetShape(geom_id).Reify(&maker, &sets[count++]); + } + return sets; +} + +namespace { + +// Solves the optimization +// min_q (q-d)*CᵀC(q-d) +// s.t. setA on bodyA and setB on bodyB are in collision in q. +// Aq ≤ b. +// where C, d are the center and matrix from the hyperellipsoid E. +// Returns true iff a collision is found. +// Sets `closest` to an optimizing solution q*, if a solution is found. +bool FindClosestCollision(const multibody::MultibodyPlant& plant, + const multibody::Body& bodyA, + const multibody::Body& bodyB, + const ConvexSet& setA, const ConvexSet& setB, + const Hyperellipsoid& E, + const Eigen::Ref& A, + const Eigen::Ref& b, + const solvers::SolverBase& solver, + systems::Context* context, + VectorXd* closest) { + solvers::MathematicalProgram prog; + auto q = prog.NewContinuousVariables(plant.num_positions(), "q"); + + prog.AddLinearConstraint( + A, VectorXd::Constant(b.size(), -std::numeric_limits::infinity()), + b, q); + // Scale the objective so the eigenvalues are close to 1, using + // scale*lambda_min = 1/scale*lambda_max. + const MatrixXd Asq = E.A().transpose() * E.A(); + Eigen::SelfAdjointEigenSolver es(Asq); + const double scale = 1.0 / std::sqrt(es.eigenvalues().maxCoeff() * + es.eigenvalues().minCoeff()); + prog.AddQuadraticErrorCost(scale * Asq, E.center(), q); + + auto p_AA = prog.NewContinuousVariables<3>("p_AA"); + auto p_BB = prog.NewContinuousVariables<3>("p_BB"); + setA.AddPointInSetConstraints(&prog, p_AA); + setB.AddPointInSetConstraints(&prog, p_BB); + + plant.SetPositions(context, q); + const math::RigidTransform X_WA = + plant.EvalBodyPoseInWorld(*context, bodyA); + const math::RigidTransform X_WB = + plant.EvalBodyPoseInWorld(*context, bodyB); + prog.AddConstraint(X_WA * p_AA.cast() == + X_WB * p_BB.cast()); + + // Help nonlinear optimizers (e.g. SNOPT) avoid trivial local minima at the + // origin. + prog.SetSolverOption(solvers::SnoptSolver::id(), "Print file", + "/tmp/iris_snopt.out"); + prog.SetInitialGuess(q, VectorXd::Constant(plant.num_positions(), .01)); + prog.SetInitialGuess(p_AA, Vector3d::Constant(.01)); + prog.SetInitialGuess(p_BB, Vector3d::Constant(.01)); + for (const auto binding : prog.lorentz_cone_constraints()) { + const auto c = binding.evaluator(); + prog.AddConstraint( + std::make_shared( + c->A_dense(), c->b(), + solvers::LorentzConeConstraint::EvalType::kNonconvex), + binding.variables()); + prog.RemoveConstraint(binding); + } + + solvers::MathematicalProgramResult result = solver.Solve(prog); + if (result.is_success()) { + *closest = result.GetSolution(q); + return true; + } + std::cout << prog << std::endl; + return false; +} + +} // namespace + +HPolyhedron IrisInConfigurationSpace( + const multibody::MultibodyPlant& plant, + const QueryObject& query_object, + const Eigen::Ref& sample, + const IrisOptions& options) { + const int nq = plant.num_positions(); + DRAKE_DEMAND(sample.size() == nq); + + // Note: We require finite joint limits to define the bounding box for the + // IRIS algorithm. + DRAKE_DEMAND(plant.GetPositionLowerLimits().array().isFinite().all()); + DRAKE_DEMAND(plant.GetPositionUpperLimits().array().isFinite().all()); + HPolyhedron P = HPolyhedron::MakeBox(plant.GetPositionLowerLimits(), + plant.GetPositionUpperLimits()); + DRAKE_DEMAND(P.A().rows() == 2 * nq); + + const double kEpsilonEllipsoid = 1e-2; + Hyperellipsoid E = Hyperellipsoid::MakeHypersphere(kEpsilonEllipsoid, sample); + + const SceneGraphInspector& inspector = query_object.inspector(); + + // Make all of the convex sets. TODO(russt): Consider factoring this out so + // that I don't have to redo the work for every new region. + IrisConvexSetMaker maker(query_object, inspector.world_frame_id()); + std::unordered_map> sets{}; + std::unordered_map&> bodies{}; + const std::unordered_set geom_ids = inspector.GetGeometryIds( + GeometrySet(inspector.GetAllGeometryIds()), Role::kProximity); + copyable_unique_ptr temp_set; + for (GeometryId geom_id : geom_ids) { + // Make all sets in the local geometry frame. + FrameId frame_id = inspector.GetFrameId(geom_id); + maker.set_reference_frame(frame_id); + maker.set_geometry_id(geom_id); + inspector.GetShape(geom_id).Reify(&maker, &temp_set); + sets.emplace(geom_id, std::move(temp_set)); + bodies.emplace(geom_id, *plant.GetBodyFromFrameId(frame_id)); + } + + // TODO(russt): As a surrogate for the true objective, we could use convex + // optimization to compute the (squared?) distance between each collision pair + // from the sample point configuration, then sort the pairs by that distance. + // This could improve computation times in Ibex here and produce regions with + // less faces. + auto pairs = inspector.GetCollisionCandidates(); + const int N = static_cast(pairs.size()); + + // On each iteration, we will build the collision-free polytope represented as + // {x | A * x <= b}. Here we pre-allocate matrices with a generous maximum + // size. + Eigen::Matrix A( + P.A().rows() + 2*N, nq); + VectorXd b(P.A().rows() + 2*N); + A.topRows(P.A().rows()) = P.A(); + b.head(P.A().rows()) = P.b(); + + double best_volume = E.Volume(); + int iteration = 0; + MatrixXd tangent_matrix; + + //solvers::IbexSolver solver; + solvers::SnoptSolver solver; + DRAKE_DEMAND(solver.is_available() && solver.is_enabled()); + auto context = plant.CreateDefaultContext(); + VectorXd closest(nq); + + while (true) { + tangent_matrix = 2.0 * E.A().transpose() * E.A(); + int num_constraints = 2 * nq; // Start with just the joint limits. + // Find separating hyperplanes + for (const auto [geomA, geomB] : pairs) { + while (true) { + const bool collision = FindClosestCollision( + plant, bodies.at(geomA), bodies.at(geomB), *sets.at(geomA), + *sets.at(geomB), E, A.topRows(num_constraints), + b.head(num_constraints), solver, context.get(), &closest); + std::cout << "geomA=" << inspector.GetName(geomA) + << ", geomB=" << inspector.GetName(geomB); + if (collision) { + std::cout << ", closest=" << closest.transpose() << std::endl; + // Add the tangent to the (scaled) ellipsoid at this point as a + // constraint. + if (num_constraints >= A.rows()) { + // Increase pre-allocated polytope size. + A.conservativeResize(A.rows() + N, nq); + b.conservativeResize(b.rows() + N); + } + + A.row(num_constraints) = + (tangent_matrix * (closest - E.center())).normalized(); + b[num_constraints] = A.row(num_constraints) * closest - + options.configuration_space_margin; + std::cout << " " + << A.row(num_constraints) * + symbolic::MakeVectorVariable(nq, "q") + << " ≤ " << b[num_constraints] << std::endl; + num_constraints++; + } else { + std::cout << ". No collisions." << std::endl; + break; + } + } + } + + if (options.require_sample_point_is_contained && + ((A.topRows(num_constraints) * sample).array() >= + b.head(num_constraints).array()) + .any()) { + break; + } + P = HPolyhedron(A.topRows(num_constraints), b.head(num_constraints)); + + iteration++; + if (iteration >= options.iteration_limit) { + break; + } + + E = P.MaximumVolumeInscribedEllipsoid(); + const double volume = E.Volume(); + if (volume - best_volume <= options.termination_threshold) { + break; + } + best_volume = volume; } - return maker.claim_sets(); + return P; } } // namespace optimization diff --git a/geometry/optimization/iris.h b/geometry/optimization/iris.h index 30bffe95ecb5..ef6a58a1a044 100644 --- a/geometry/optimization/iris.h +++ b/geometry/optimization/iris.h @@ -4,8 +4,10 @@ #include #include +#include "drake/common/symbolic.h" #include "drake/geometry/optimization/convex_set.h" #include "drake/geometry/optimization/hpolyhedron.h" +#include "drake/multibody/plant/multibody_plant.h" namespace drake { namespace geometry { @@ -32,6 +34,12 @@ struct IrisOptions { /** IRIS will terminate if the change in the *volume* of the hyperellipsoid between iterations is less that this threshold. */ double termination_threshold{2e-2}; // from rdeits/iris-distro. + + /** For IRIS in configuration space, 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. TODO: clarify the units. + */ + double configuration_space_margin{1e-2}; }; /** The IRIS (Iterative Region Inflation by Semidefinite programming) algorithm, @@ -85,6 +93,14 @@ ConvexSets MakeIrisObstacles( const QueryObject& query_object, std::optional reference_frame = std::nullopt); +// TODO(russt): Pass a context of the plant if/when we support setting kinematic +// parameters. +HPolyhedron IrisInConfigurationSpace( + const multibody::MultibodyPlant& plant, + const QueryObject& query_object, + const Eigen::Ref& sample, + const IrisOptions& options = IrisOptions()); + } // namespace optimization } // namespace geometry } // namespace drake diff --git a/geometry/optimization/test/iris_test.cc b/geometry/optimization/test/iris_test.cc index 818bfe5c701f..4e5873f49a01 100644 --- a/geometry/optimization/test/iris_test.cc +++ b/geometry/optimization/test/iris_test.cc @@ -2,11 +2,14 @@ #include +#include "drake/common/temp_directory.h" #include "drake/common/test_utilities/eigen_matrix_compare.h" #include "drake/geometry/optimization/point.h" #include "drake/geometry/optimization/vpolytope.h" #include "drake/geometry/scene_graph.h" #include "drake/math/rigid_transform.h" +#include "drake/multibody/parsing/parser.h" +#include "drake/systems/framework/diagram_builder.h" namespace drake { namespace geometry { @@ -110,8 +113,7 @@ GTEST_TEST(IrisTest, BallInBox) { └─────┴─┴─────┘ */ GTEST_TEST(IrisTest, MultipleBoxes) { ConvexSets obstacles; - obstacles.emplace_back( - VPolytope::MakeBox(Vector2d(.1, .5), Vector2d(1, 1))); + obstacles.emplace_back(VPolytope::MakeBox(Vector2d(.1, .5), Vector2d(1, 1))); obstacles.emplace_back( VPolytope::MakeBox(Vector2d(-1, -1), Vector2d(-.1, -.5))); obstacles.emplace_back( @@ -232,6 +234,304 @@ TEST_F(SceneGraphTester, MultipleBoxes) { EXPECT_TRUE(region.PointInSet(Vector3d(0.0, 0.0, -0.99))); } +namespace { + +// Helper method for testing IrisInConfigurationSpace from a urdf string. +HPolyhedron IrisFromUrdf(const std::string urdf, + const Eigen::Ref& sample, + const IrisOptions& options) { + systems::DiagramBuilder builder; + auto [plant, scene_graph] = + multibody::AddMultibodyPlantSceneGraph(&builder, 0.0); + multibody::Parser(&plant).AddModelFromString(urdf, "urdf"); + plant.Finalize(); + auto diagram = builder.Build(); + + auto context = diagram->CreateDefaultContext(); + auto query_object = + scene_graph.get_query_output_port().Eval>( + scene_graph.GetMyContextFromRoot(*context)); + + std::unique_ptr> + symbolic_plant = systems::System::ToSymbolic(plant); + return IrisInConfigurationSpace(*symbolic_plant, query_object, sample); +} + +} // namespace + +// One prismatic link with joint limits. Iris should return the joint limits. +GTEST_TEST(IrisInConfigurationSpaceTest, JointLimits) { + const std::string limits_urdf = R"( + + + + + + + + + + + + + +)"; + + const Vector1d sample = Vector1d::Zero(); + IrisOptions options; + HPolyhedron region = IrisFromUrdf(limits_urdf, sample, 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})); +} + +// Three boxes. Two on the outside are fixed. One in the middle on a prismatic +// joint. The configuration space is a (convex) line segment q ∈ (−1,1). +GTEST_TEST(IrisInConfigurationSpaceTest, BoxesPrismatic) { + const std::string boxes_urdf = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + + const Vector1d sample = Vector1d::Zero(); + IrisOptions options; + HPolyhedron region = IrisFromUrdf(boxes_urdf, sample, options); + + EXPECT_EQ(region.ambient_dimension(), 1); + + const double kTol = 1e-5; + const double qmin = -1.0 + options.configuration_space_margin, + qmax = 1.0 - options.configuration_space_margin; + 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})); +} + +// Three spheres. Two on the outside are fixed. One in the middle on a +// prismatic joint. The configuration space is a (convex) line segment q ∈ +// (−1,1). +GTEST_TEST(IrisInConfigurationSpaceTest, SpheresPrismatic) { + const std::string spheres_urdf = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + + const Vector1d sample = Vector1d::Zero(); + IrisOptions options; + HPolyhedron region = IrisFromUrdf(spheres_urdf, sample, options); + + EXPECT_EQ(region.ambient_dimension(), 1); + + const double kTol = 1e-5; + const double qmin = -1.0 + options.configuration_space_margin, + qmax = 1.0 - options.configuration_space_margin; + 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 pendulum of length `l` with a sphere at the tip of radius `r` +// between two (fixed) walls at `w` from the origin. The configuration space +// is a (convex) line segment q ∈ (sin⁻¹((-w+r)/l), sin((w-r)/l)). +GTEST_TEST(IrisInConfigurationSpaceTest, SinglePendulum) { + const double l = 2.0; + const double r = .5; + const double w = 1.0; + const std::string pendulum_urdf = fmt::format( + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)", + fmt::arg("w_plus_one_half", w + .5), fmt::arg("l", l), fmt::arg("r", r)); + + const Vector1d sample = Vector1d::Zero(); + IrisOptions options; + HPolyhedron region = IrisFromUrdf(pendulum_urdf, sample, options); + + EXPECT_EQ(region.ambient_dimension(), 1); + + const double kTol = 1e-5; + const double qmin = + std::asin((-w + r) / l) + options.configuration_space_margin, + qmax = + std::asin((w - r) / l) - options.configuration_space_margin; + 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(IrisInConfigurationSpaceTest, 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)); + + std::cout << double_pendulum_urdf << std::endl; + + systems::DiagramBuilder builder; + auto [plant, scene_graph] = + multibody::AddMultibodyPlantSceneGraph(&builder, 0.0); + multibody::Parser(&plant).AddModelFromString(double_pendulum_urdf, "urdf"); + plant.Finalize(); + auto diagram = builder.Build(); + + auto context = diagram->CreateDefaultContext(); + +// const double kTol = 1e-5; + + // Confirm the configuration space boundary via SceneGraph. + auto& plant_context = plant.GetMyMutableContextFromRoot(context.get()); + plant.SetPositions(&plant_context, Vector2d(.5, .5)); + auto query_object = + scene_graph.get_query_output_port().Eval>( + scene_graph.GetMyContextFromRoot(*context)); + EXPECT_TRUE(query_object.HasCollisions()); + + const Vector2d sample = Vector2d::Zero(); + IrisOptions options; + HPolyhedron region = IrisFromUrdf(double_pendulum_urdf, sample, options); + + // Note: You may use this to plot the solution in the desmos graphing + // calculator link above. Just copy each equation in the printed formula into + // a desmos cell. The intersection is the computed region. + const Vector2 xy{symbolic::Variable("x"), + symbolic::Variable("y")}; + std::cout << (region.A()*xy <= region.b()) << std::endl; + + EXPECT_EQ(region.ambient_dimension(), 2); + EXPECT_TRUE(false); +} + } // namespace } // namespace optimization } // namespace geometry diff --git a/solvers/ibex_solver.cc b/solvers/ibex_solver.cc index d879388e5e41..09ded7aa6dfb 100644 --- a/solvers/ibex_solver.cc +++ b/solvers/ibex_solver.cc @@ -127,7 +127,7 @@ void IbexSolver::DoSolve(const MathematicalProgram& prog, std::vector> expr_ctrs; // Add constraints. - for (const auto& b : prog.linear_constraints()) { + for (const auto& b : prog.GetAllConstraints()) { Formula f{b.evaluator()->CheckSatisfied(b.variables())}; std::cerr << "Add constraint: " << f << "\n"; if (symbolic::is_conjunction(f)) { @@ -174,6 +174,7 @@ void IbexSolver::DoSolve(const MathematicalProgram& prog, std::cerr << sys << "\n"; + // TODO: This appears to crash if there is no objective. ibex::DefaultOptimizer o(sys, rel_eps_f, abs_eps_f, eps_h, rigor, inHC4, random_seed, eps_x); o.trace = trace;