From d6908d5edfc655fe721245c4a8e00132436c80e2 Mon Sep 17 00:00:00 2001 From: Russ Tedrake Date: Sun, 25 Jul 2021 17:38:31 -0400 Subject: [PATCH] WIP: getting snopt version ready to PR --- geometry/optimization/iris.cc | 64 ++++++++++++------------- geometry/optimization/iris.h | 29 +++++++++-- geometry/optimization/test/iris_test.cc | 48 +++++++------------ 3 files changed, 73 insertions(+), 68 deletions(-) diff --git a/geometry/optimization/iris.cc b/geometry/optimization/iris.cc index 4d0aa14f93a7..bb24d72eba0d 100644 --- a/geometry/optimization/iris.cc +++ b/geometry/optimization/iris.cc @@ -221,21 +221,23 @@ bool FindClosestCollision(const multibody::MultibodyPlant& plant, 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); + if (solver.solver_id() == solvers::IbexSolver::id()) { + // Use kNonconvex instead of the default kConvexSmooth. + 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); + } + } else { + // Help nonlinear optimizers (e.g. SNOPT) avoid trivial local minima at the + // origin. + prog.SetInitialGuess(q, VectorXd::Constant(plant.num_positions(), .01)); + prog.SetInitialGuess(p_AA, Vector3d::Constant(.01)); + prog.SetInitialGuess(p_BB, Vector3d::Constant(.01)); } solvers::MathematicalProgramResult result = solver.Solve(prog); @@ -243,15 +245,16 @@ bool FindClosestCollision(const multibody::MultibodyPlant& plant, *closest = result.GetSolution(q); return true; } - std::cout << prog << std::endl; + //std::cout << prog << std::endl; return false; } } // namespace HPolyhedron IrisInConfigurationSpace( - const multibody::MultibodyPlant& plant, - const QueryObject& query_object, + const multibody::MultibodyPlant& plant, + const SceneGraph& scene_graph, + const systems::Context& root_context, const Eigen::Ref& sample, const IrisOptions& options) { const int nq = plant.num_positions(); @@ -268,6 +271,11 @@ HPolyhedron IrisInConfigurationSpace( const double kEpsilonEllipsoid = 1e-2; Hyperellipsoid E = Hyperellipsoid::MakeHypersphere(kEpsilonEllipsoid, sample); + std::unique_ptr> + symbolic_plant = systems::System::ToSymbolic(plant); + auto query_object = + scene_graph.get_query_output_port().Eval>( + scene_graph.GetMyContextFromRoot(root_context)); const SceneGraphInspector& inspector = query_object.inspector(); // Make all of the convex sets. TODO(russt): Consider factoring this out so @@ -285,7 +293,7 @@ HPolyhedron IrisInConfigurationSpace( 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)); + bodies.emplace(geom_id, *symbolic_plant->GetBodyFromFrameId(frame_id)); } // TODO(russt): As a surrogate for the true objective, we could use convex @@ -309,10 +317,10 @@ HPolyhedron IrisInConfigurationSpace( int iteration = 0; MatrixXd tangent_matrix; - //solvers::IbexSolver solver; + // solvers::IbexSolver solver; solvers::SnoptSolver solver; DRAKE_DEMAND(solver.is_available() && solver.is_enabled()); - auto context = plant.CreateDefaultContext(); + auto symbolic_context = symbolic_plant->CreateDefaultContext(); VectorXd closest(nq); while (true) { @@ -322,13 +330,10 @@ HPolyhedron IrisInConfigurationSpace( 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); + *symbolic_plant, bodies.at(geomA), bodies.at(geomB), + *sets.at(geomA), *sets.at(geomB), E, A.topRows(num_constraints), + b.head(num_constraints), solver, symbolic_context.get(), &closest); 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()) { @@ -341,13 +346,8 @@ HPolyhedron IrisInConfigurationSpace( (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; } } diff --git a/geometry/optimization/iris.h b/geometry/optimization/iris.h index ef6a58a1a044..a8707e8fb691 100644 --- a/geometry/optimization/iris.h +++ b/geometry/optimization/iris.h @@ -93,11 +93,32 @@ 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. +/** A variation of the Iris (Iterative Region Inflation by Semidefinite +programming) algorithm which finds collision-free regions in the *configuration +space* of @p plant. @see Iris for details on the original algorithm. +The possibility of this configuration-space variant was suggested in the +original IRIS paper, but substantial new ideas have been employed here to +address the non-convexity of configuration-space obstacles. + +@param plant describes the kinematics of configuration space. +@param scene_graph describes the geometry; it must be connected to @p plant in a +systems::Diagram. +@param root_context is a context of the systems::Diagram containing the +systems::Context for both @p plant and @p scene_graph. +@param sample is a vector of size plant.num_positions() representing the initial +IRIS seed configuration. +@param options provides additional configuration options. + +Note: This initial implementation **does not** yet provide a rigorous guarantee +that the returned region is collision free. The certification step will be +contributed in a follow-up PR. + +@ingroup geometry_optimization +*/ HPolyhedron IrisInConfigurationSpace( - const multibody::MultibodyPlant& plant, - const QueryObject& query_object, + const multibody::MultibodyPlant& plant, + const SceneGraph& scene_graph, + const systems::Context& root_context, const Eigen::Ref& sample, const IrisOptions& options = IrisOptions()); diff --git a/geometry/optimization/test/iris_test.cc b/geometry/optimization/test/iris_test.cc index 4e5873f49a01..e461f4e6bf75 100644 --- a/geometry/optimization/test/iris_test.cc +++ b/geometry/optimization/test/iris_test.cc @@ -248,13 +248,8 @@ HPolyhedron IrisFromUrdf(const std::string urdf, 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); + return IrisInConfigurationSpace(plant, scene_graph, *context, sample, + options); } } // namespace @@ -496,27 +491,6 @@ GTEST_TEST(IrisInConfigurationSpaceTest, DoublePendulum) { 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); @@ -524,12 +498,22 @@ GTEST_TEST(IrisInConfigurationSpaceTest, DoublePendulum) { // 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; + // 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); + // 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})); } } // namespace