diff --git a/bindings/pydrake/geometry/test/optimization_test.py b/bindings/pydrake/geometry/test/optimization_test.py index 5df6f22fa7d4..984610435348 100644 --- a/bindings/pydrake/geometry/test/optimization_test.py +++ b/bindings/pydrake/geometry/test/optimization_test.py @@ -531,7 +531,7 @@ def test_graph_of_convex_sets(self): self.assertIsNone(options.max_rounded_paths) options.convex_relaxation = True options.preprocessing = False - options.max_rounded_paths = 2 + options.max_rounded_paths = 0 options.max_rounding_trials = 5 options.flow_tolerance = 1e-6 options.rounding_seed = 1 diff --git a/geometry/optimization/BUILD.bazel b/geometry/optimization/BUILD.bazel index 9ccc0a3cc218..2bab1fc3f3da 100644 --- a/geometry/optimization/BUILD.bazel +++ b/geometry/optimization/BUILD.bazel @@ -167,7 +167,9 @@ drake_cc_library( "//common/symbolic:expression", "//solvers:choose_best_solver", "//solvers:create_cost", + "//solvers:get_program_type", "//solvers:mathematical_program_result", + "//solvers:mosek_solver", "//solvers:solver_interface", ], ) diff --git a/geometry/optimization/graph_of_convex_sets.cc b/geometry/optimization/graph_of_convex_sets.cc index 93d1d05022d0..c30c9ed358ba 100644 --- a/geometry/optimization/graph_of_convex_sets.cc +++ b/geometry/optimization/graph_of_convex_sets.cc @@ -15,7 +15,10 @@ #include "drake/math/quadratic_form.h" #include "drake/solvers/choose_best_solver.h" +#include "drake/solvers/create_constraint.h" #include "drake/solvers/create_cost.h" +#include "drake/solvers/get_program_type.h" +#include "drake/solvers/mosek_solver.h" namespace drake { namespace geometry { @@ -44,7 +47,9 @@ using solvers::MathematicalProgram; using solvers::MathematicalProgramResult; using solvers::MatrixXDecisionVariable; using solvers::PerspectiveQuadraticCost; +using solvers::ProgramType; using solvers::QuadraticCost; +using solvers::RotatedLorentzConeConstraint; using solvers::SolutionResult; using solvers::VariableRefList; using solvers::VectorXDecisionVariable; @@ -54,14 +59,10 @@ using symbolic::Variables; namespace { MathematicalProgramResult Solve(const MathematicalProgram& prog, - const GraphOfConvexSetsOptions& options, - bool rounding) { + const GraphOfConvexSetsOptions& options) { MathematicalProgramResult result; - auto solver_options = (rounding && options.rounding_solver_options) - ? options.rounding_solver_options - : options.solver_options; if (options.solver) { - options.solver->Solve(prog, {}, solver_options, &result); + options.solver->Solve(prog, {}, options.solver_options, &result); } else { std::unique_ptr solver{}; try { @@ -69,7 +70,7 @@ MathematicalProgramResult Solve(const MathematicalProgram& prog, solver = solvers::MakeSolver(solver_id); } catch (const std::exception&) { // We should only get here if the user is trying to solve the MIP. - DRAKE_DEMAND(options.convex_relaxation == false && rounding == false); + DRAKE_DEMAND(options.convex_relaxation == false); // TODO(russt): Consider calling MixedIntegerBranchAndBound automatically // here. The small trick is that we need to pass the SolverId into that @@ -89,7 +90,7 @@ MathematicalProgramResult Solve(const MathematicalProgram& prog, "GraphOfConvexSetsOptions for more details."); } DRAKE_DEMAND(solver != nullptr); - solver->Solve(prog, {}, solver_options, &result); + solver->Solve(prog, {}, options.solver_options, &result); } return result; } @@ -380,10 +381,13 @@ std::string GraphOfConvexSets::GetGraphvizString( if (show_slacks) { graphviz << ",\n"; graphviz << "ϕ = " << result->GetSolution(e->phi()) << ",\n"; - graphviz << "ϕ xᵤ = [" << e->GetSolutionPhiXu(*result).transpose() - << "],\n"; - graphviz << "ϕ xᵥ = [" << e->GetSolutionPhiXv(*result).transpose() - << "]"; + if (result->get_decision_variable_index()->count(e->y_[0].get_id()) != + 0) { + graphviz << "ϕ xᵤ = [" << e->GetSolutionPhiXu(*result).transpose() + << "],\n"; + graphviz << "ϕ xᵥ = [" << e->GetSolutionPhiXv(*result).transpose() + << "]"; + } } } graphviz << "\"];\n"; @@ -527,7 +531,7 @@ std::set GraphOfConvexSets::PreprocessShortestPath( degree.at(e->v().id()).evaluator()->set_bounds(Vector1d(0), Vector1d(0)); // Check if edge e = (u,v) could be on a path from start to goal. - auto result = Solve(prog, options, false); + auto result = Solve(prog, options); if (!result.is_success()) { unusable_edges.insert(edge_id); } @@ -1020,7 +1024,7 @@ MathematicalProgramResult GraphOfConvexSets::SolveShortestPath( } } - MathematicalProgramResult result = Solve(prog, options, false); + MathematicalProgramResult result = Solve(prog, options); log()->info( "Solved GCS shortest path using {} with convex_relaxation={} and " "preprocessing={}{}.", @@ -1028,12 +1032,19 @@ MathematicalProgramResult GraphOfConvexSets::SolveShortestPath( *options.preprocessing, *options.max_rounded_paths > 0 ? " and rounding" : " and no rounding"); + bool found_rounded_result = false; // Implements the rounding scheme put forth in Section 4.2 of // "Motion Planning around Obstacles with Convex Optimization": // https://arxiv.org/abs/2205.04422 if (*options.convex_relaxation && *options.max_rounded_paths > 0 && result.is_success()) { DRAKE_THROW_UNLESS(options.max_rounding_trials > 0); + GraphOfConvexSetsOptions rounding_options = options; + if (rounding_options.rounding_solver_options) { + rounding_options.solver_options = + *rounding_options.rounding_solver_options; + } + RandomGenerator generator(options.rounding_seed); std::uniform_real_distribution uniform; std::vector> paths; @@ -1099,28 +1110,8 @@ MathematicalProgramResult GraphOfConvexSets::SolveShortestPath( paths.push_back(new_path); // Optimize path - std::vector> added_constraints; - for (const auto& [edge_id, e] : edges_) { - if (e->phi_value_.has_value() || unusable_edges.count(edge_id)) { - continue; - } - if (std::find(new_path.begin(), new_path.end(), e.get()) != - new_path.end()) { - added_constraints.push_back( - prog.AddBoundingBoxConstraint(1, 1, relaxed_phi.at(edge_id))); - } else { - added_constraints.push_back( - prog.AddBoundingBoxConstraint(0, 0, relaxed_phi.at(edge_id))); - added_constraints.push_back(prog.AddLinearEqualityConstraint( - e->y_.cast(), VectorXd::Zero(e->y_.size()))); - added_constraints.push_back(prog.AddLinearEqualityConstraint( - e->z_.cast(), VectorXd::Zero(e->z_.size()))); - added_constraints.push_back(prog.AddLinearEqualityConstraint( - e->ell_.cast(), VectorXd::Zero(e->ell_.size()))); - } - } - - MathematicalProgramResult rounded_result = Solve(prog, options, true); + MathematicalProgramResult rounded_result = + SolveConvexRestriction(new_path, rounding_options); // Check path quality. if (rounded_result.is_success() && @@ -1129,104 +1120,103 @@ MathematicalProgramResult GraphOfConvexSets::SolveShortestPath( best_rounded_result.get_optimal_cost())) { best_rounded_result = rounded_result; } - - for (Binding& con : added_constraints) { - prog.RemoveConstraint(con); - } } if (best_rounded_result.is_success()) { result = best_rounded_result; + found_rounded_result = true; } else { result.set_solution_result(SolutionResult::kIterationLimit); } log()->info("Finished {} rounding trials.", num_trials); } - - // Push the placeholder variables and excluded edge variables into the result, - // so that they can be accessed as if they were variables included in the - // optimization. - int num_placeholder_vars = relaxed_phi.size(); - for (const std::pair>& vpair : - vertices_) { - num_placeholder_vars += vpair.second->ambient_dimension(); - num_placeholder_vars += vpair.second->ell_.size(); - } - for (const Edge* e : excluded_edges) { - num_placeholder_vars += e->y_.size() + e->z_.size() + e->ell_.size() + 1; - } - num_placeholder_vars += excluded_phi.size(); - std::unordered_map decision_variable_index = - prog.decision_variable_index(); - int count = result.get_x_val().size(); - Eigen::VectorXd x_val(count + num_placeholder_vars); - x_val.head(count) = result.get_x_val(); - for (const Edge* e : excluded_edges) { - for (int i = 0; i < e->y_.size(); ++i) { - decision_variable_index.emplace(e->y_[i].get_id(), count); - x_val[count++] = 0; + if (!found_rounded_result) { + // Push the placeholder variables and excluded edge variables into the + // result, so that they can be accessed as if they were variables included + // in the optimization. + int num_placeholder_vars = relaxed_phi.size(); + for (const std::pair>& vpair : + vertices_) { + num_placeholder_vars += vpair.second->ambient_dimension(); + num_placeholder_vars += vpair.second->ell_.size(); + } + for (const Edge* e : excluded_edges) { + num_placeholder_vars += e->y_.size() + e->z_.size() + e->ell_.size() + 1; } - for (int i = 0; i < e->z_.size(); ++i) { - decision_variable_index.emplace(e->z_[i].get_id(), count); + num_placeholder_vars += excluded_phi.size(); + std::unordered_map decision_variable_index = + prog.decision_variable_index(); + int count = result.get_x_val().size(); + Eigen::VectorXd x_val(count + num_placeholder_vars); + x_val.head(count) = result.get_x_val(); + for (const Edge* e : excluded_edges) { + for (int i = 0; i < e->y_.size(); ++i) { + decision_variable_index.emplace(e->y_[i].get_id(), count); + x_val[count++] = 0; + } + for (int i = 0; i < e->z_.size(); ++i) { + decision_variable_index.emplace(e->z_[i].get_id(), count); + x_val[count++] = 0; + } + for (int i = 0; i < e->ell_.size(); ++i) { + decision_variable_index.emplace(e->ell_[i].get_id(), count); + x_val[count++] = 0; + } + decision_variable_index.emplace(e->phi_.get_id(), count); x_val[count++] = 0; } - for (int i = 0; i < e->ell_.size(); ++i) { - decision_variable_index.emplace(e->ell_[i].get_id(), count); + for (const Variable& phi : excluded_phi) { + decision_variable_index.emplace(phi.get_id(), count); x_val[count++] = 0; } - decision_variable_index.emplace(e->phi_.get_id(), count); - x_val[count++] = 0; - } - for (const Variable& phi : excluded_phi) { - decision_variable_index.emplace(phi.get_id(), count); - x_val[count++] = 0; - } - for (const std::pair>& vpair : - vertices_) { - const Vertex* v = vpair.second.get(); - const bool is_target = (target_id == v->id()); - VectorXd x_v = VectorXd::Zero(v->ambient_dimension()); - double sum_phi = 0; - if (is_target) { - sum_phi = 1.0; - for (const auto& e : incoming_edges[v->id()]) { - x_v += result.GetSolution(e->z_); + for (const std::pair>& vpair : + vertices_) { + const Vertex* v = vpair.second.get(); + const bool is_target = (target_id == v->id()); + VectorXd x_v = VectorXd::Zero(v->ambient_dimension()); + double sum_phi = 0; + if (is_target) { + sum_phi = 1.0; + for (const auto& e : incoming_edges[v->id()]) { + x_v += result.GetSolution(e->z_); + } + } else { + for (const auto& e : outgoing_edges[v->id()]) { + x_v += result.GetSolution(e->y_); + sum_phi += result.GetSolution( + *options.convex_relaxation ? relaxed_phi.at(e->id()) : e->phi_); + } } - } else { - for (const auto& e : outgoing_edges[v->id()]) { - x_v += result.GetSolution(e->y_); - sum_phi += result.GetSolution( - *options.convex_relaxation ? relaxed_phi.at(e->id()) : e->phi_); + // In the convex relaxation, sum_relaxed_phi may not be one even for + // vertices in the shortest path. We undo yₑ = ϕₑ xᵤ here to ensure that + // xᵤ is in v->set(). If ∑ ϕₑ is small enough that numerical errors + // prevent the projection back into the Xᵤ, then we prefer to return NaN. + if (sum_phi < 100.0 * std::numeric_limits::epsilon()) { + x_v = VectorXd::Constant(v->ambient_dimension(), + std::numeric_limits::quiet_NaN()); + } else if (*options.convex_relaxation) { + x_v /= sum_phi; + } + for (int i = 0; i < v->ambient_dimension(); ++i) { + decision_variable_index.emplace(v->x()[i].get_id(), count); + x_val[count++] = x_v[i]; + } + for (int ii = 0; ii < v->ell_.size(); ++ii) { + decision_variable_index.emplace(v->ell_[ii].get_id(), count); + x_val[count++] = + result.GetSolution(vertex_edge_ell.at(v->id()).col(ii)).sum(); } } - // In the convex relaxation, sum_relaxed_phi may not be one even for - // vertices in the shortest path. We undo yₑ = ϕₑ xᵤ here to ensure that - // xᵤ is in v->set(). If ∑ ϕₑ is small enough that numerical errors - // prevent the projection back into the Xᵤ, then we prefer to return NaN. - if (sum_phi < 100.0 * std::numeric_limits::epsilon()) { - x_v = VectorXd::Constant(v->ambient_dimension(), - std::numeric_limits::quiet_NaN()); - } else if (*options.convex_relaxation) { - x_v /= sum_phi; - } - for (int i = 0; i < v->ambient_dimension(); ++i) { - decision_variable_index.emplace(v->x()[i].get_id(), count); - x_val[count++] = x_v[i]; - } - for (int ii = 0; ii < v->ell_.size(); ++ii) { - decision_variable_index.emplace(v->ell_[ii].get_id(), count); - x_val[count++] = - result.GetSolution(vertex_edge_ell.at(v->id()).col(ii)).sum(); - } - } - if (*options.convex_relaxation) { - // Write the value of the relaxed phi into the phi placeholder. - for (const auto& [edge_id, relaxed_phi_var] : relaxed_phi) { - decision_variable_index.emplace(edges_.at(edge_id)->phi_.get_id(), count); - x_val[count++] = result.GetSolution(relaxed_phi_var); + if (*options.convex_relaxation) { + // Write the value of the relaxed phi into the phi placeholder. + for (const auto& [edge_id, relaxed_phi_var] : relaxed_phi) { + decision_variable_index.emplace(edges_.at(edge_id)->phi_.get_id(), + count); + x_val[count++] = result.GetSolution(relaxed_phi_var); + } } + result.set_decision_variable_index(decision_variable_index); + result.set_x_val(x_val); } - result.set_decision_variable_index(decision_variable_index); - result.set_x_val(x_val); return result; } @@ -1298,6 +1288,104 @@ std::vector GraphOfConvexSets::GetSolutionPath( return path_edges; } +namespace { + +// TODO(russt): Move this into the public API. + +// TODO(russt): Return a map from the removed cost to the new slack variable +// and constraint. + +/* Most convex solvers require only support linear and quadratic costs when +operating with nonlinear constraints. This removes costs and adds variables and +constraints as needed by the solvers. */ +void RewriteForConvexSolver(MathematicalProgram* prog) { + // Use Mosek's requirements to test the program attributes. + solvers::MosekSolver mosek; + if (mosek.AreProgramAttributesSatisfied(*prog)) { + return; + } + + const double kInf = std::numeric_limits::infinity(); + + // Loop through all unsupported costs and rewrite them into constraints. + std::unordered_set> to_remove; + + for (const auto& binding : prog->l2norm_costs()) { + const int m = binding.evaluator()->A().rows(); + const int n = binding.evaluator()->A().cols(); + auto slack = prog->NewContinuousVariables<1>("slack"); + prog->AddLinearCost(Vector1d::Ones(), slack); + // |Ax+b|² ≤ slack, written as a Lorentz cone with z = [slack; Ax+b]. + MatrixXd A = MatrixXd::Zero(m + 1, n + 1); + A(0, 0) = 1; + A.bottomRightCorner(m, n) = binding.evaluator()->A(); + VectorXd b(m + 1); + b << 0, binding.evaluator()->b(); + prog->AddLorentzConeConstraint(A, b, {slack, binding.variables()}); + to_remove.insert(binding); + } + + for (const auto& binding : prog->generic_costs()) { + const Cost* cost = binding.evaluator().get(); + if (const auto* l1c = dynamic_cast(cost)) { + const int m = l1c->A().rows(); + const int n = l1c->A().cols(); + auto slack = prog->NewContinuousVariables(m, "slack"); + prog->AddLinearCost(VectorXd::Ones(m), slack); + // Ax + b ≤ slack, written as [A,-I][x;slack] ≤ -b. + MatrixXd A = MatrixXd::Zero(m, m + n); + A << l1c->A(), -MatrixXd::Identity(m, m); + prog->AddLinearConstraint(A, VectorXd::Constant(m, -kInf), -l1c->b(), + {binding.variables(), slack}); + // -(Ax + b) ≤ slack, written as [A,I][x;slack] ≥ -b. + A.rightCols(m) = MatrixXd::Identity(m, m); + prog->AddLinearConstraint(A, -l1c->b(), VectorXd::Constant(m, kInf), + {binding.variables(), slack}); + to_remove.insert(binding); + } else if (const auto* linfc = dynamic_cast(cost)) { + const int m = linfc->A().rows(); + const int n = linfc->A().cols(); + auto slack = prog->NewContinuousVariables<1>("slack"); + prog->AddLinearCost(Vector1d::Ones(), slack); + // ∀i, aᵢᵀx + bᵢ ≤ slack, written as [A,-1][x;slack] ≤ -b. + MatrixXd A = MatrixXd::Zero(m, n + 1); + A << linfc->A(), VectorXd::Constant(m, -1); + prog->AddLinearConstraint(A, VectorXd::Constant(m, -kInf), -linfc->b(), + {binding.variables(), slack}); + // ∀i, -(aᵢᵀx + bᵢ) ≤ slack, written as [A,1][x;slack] ≥ -b. + A.col(A.cols() - 1) = VectorXd::Ones(m); + prog->AddLinearConstraint(A, -linfc->b(), VectorXd::Constant(m, kInf), + {binding.variables(), slack}); + to_remove.insert(binding); + } else if (const auto* pqc = + dynamic_cast(cost)) { + const int m = pqc->A().rows(); + const int n = pqc->A().cols(); + auto slack = prog->NewContinuousVariables<1>("slack"); + prog->AddLinearCost(Vector1d::Ones(), slack); + // Written as rotated Lorentz cone with z = [slack; Ax+b]. + MatrixXd A = MatrixXd::Zero(m + 1, n + 1); + A(0, 0) = 1; + A.bottomRightCorner(m, n) = pqc->A(); + VectorXd b(m + 1); + b << 0, pqc->b(); + prog->AddRotatedLorentzConeConstraint(A, b, {slack, binding.variables()}); + to_remove.insert(binding); + } + } + for (const auto& b : to_remove) { + prog->RemoveCost(b); + } + + if (!mosek.AreProgramAttributesSatisfied(*prog)) { + throw std::runtime_error(fmt::format( + "SolveConvexRestriction failed to generate a convex problem: {}", + mosek.ExplainUnsatisfiedProgramAttributes(*prog))); + } +} + +} // namespace + MathematicalProgramResult GraphOfConvexSets::SolveConvexRestriction( const std::vector& active_edges, const GraphOfConvexSetsOptions& options) const { @@ -1314,6 +1402,9 @@ MathematicalProgramResult GraphOfConvexSets::SolveConvexRestriction( } for (const auto* v : vertices) { + if (v->set().ambient_dimension() == 0) { + continue; + } prog.AddDecisionVariables(v->x()); v->set().AddPointInSetConstraints(&prog, v->x()); @@ -1338,10 +1429,15 @@ MathematicalProgramResult GraphOfConvexSets::SolveConvexRestriction( } } - MathematicalProgramResult result = Solve(prog, options, false); + RewriteForConvexSolver(&prog); + MathematicalProgramResult result = Solve(prog, options); + + // TODO(russt): Add the dual variables back in for the rewritten costs. + + // Add phi vars. + int num_excluded_vars = edges_.size(); // Add any excluded vertices to the result. - int num_excluded_vars = 0; std::vector excluded_vertices; for (const auto& pair : vertices_) { const Vertex* v = pair.second.get(); @@ -1355,6 +1451,14 @@ MathematicalProgramResult GraphOfConvexSets::SolveConvexRestriction( x_val.head(count) = result.get_x_val(); std::unordered_map decision_variable_index = prog.decision_variable_index(); + for (const auto& pair : edges_) { + const Edge* e = pair.second.get(); + decision_variable_index.emplace(e->phi_.get_id(), count); + x_val[count++] = std::find(active_edges.begin(), active_edges.end(), e) != + active_edges.end() + ? 1.0 + : 0.0; + } for (const Vertex* v : excluded_vertices) { for (int i = 0; i < v->x().size(); ++i) { decision_variable_index.emplace(v->x()[i].get_id(), count); diff --git a/geometry/optimization/graph_of_convex_sets.h b/geometry/optimization/graph_of_convex_sets.h index 707d6ca2d07f..f020c45345cc 100644 --- a/geometry/optimization/graph_of_convex_sets.h +++ b/geometry/optimization/graph_of_convex_sets.h @@ -554,14 +554,15 @@ class GraphOfConvexSets { be written using less decision variables, and needs only to include the vertices associated with at least one of the active edges. Decision variables for all other convex sets will be set to NaN. - */ + + @throws std::exception if the program cannot be written as a convex + optimization consumable by one of the standard solvers. */ solvers::MathematicalProgramResult SolveConvexRestriction( const std::vector& active_edges, const GraphOfConvexSetsOptions& options = GraphOfConvexSetsOptions()) const; - private: - /* Facilitates testing. */ + private: /* Facilitates testing. */ friend class PreprocessShortestPathTest; std::set PreprocessShortestPath( diff --git a/geometry/optimization/test/graph_of_convex_sets_test.cc b/geometry/optimization/test/graph_of_convex_sets_test.cc index 5ab1540bf738..f852389ffa06 100644 --- a/geometry/optimization/test/graph_of_convex_sets_test.cc +++ b/geometry/optimization/test/graph_of_convex_sets_test.cc @@ -367,6 +367,27 @@ class ThreePoints : public ::testing::Test { options_.convex_relaxation = true; } + void CheckConvexRestriction(const MathematicalProgramResult& result) { + MathematicalProgramResult restriction_result = + g_.SolveConvexRestriction(std::vector({e_on_}), options_); + log()->info("Solved convex restriction with {}", + restriction_result.get_solver_id().name()); + // Confirm that we get a convex solver (not an NLP solver). + if (MixedIntegerSolverAvailable()) { + EXPECT_TRUE( + restriction_result.get_solver_id() == solvers::MosekSolver::id() || + restriction_result.get_solver_id() == solvers::GurobiSolver::id()); + } + EXPECT_TRUE(restriction_result.is_success()); + EXPECT_NEAR(result.get_optimal_cost(), + restriction_result.get_optimal_cost(), 1e-4); + for (const auto* v : {source_, target_, sink_}) { + EXPECT_TRUE(CompareMatrices(result.GetSolution(v->x()), + restriction_result.GetSolution(v->x()), + 1e-6)); + } + } + GraphOfConvexSets g_; Point p_source_; Point p_target_; @@ -426,6 +447,7 @@ TEST_F(ThreePoints, LinearCost1) { 0 * p_source_.x(), 1e-6)); EXPECT_TRUE( CompareMatrices(e_off_->GetSolutionPhiXv(result), 0 * p_sink_.x(), 1e-6)); + CheckConvexRestriction(result); // Alternative signatures. auto result2 = g_.SolveShortestPath(*source_, *target_, options_); @@ -511,6 +533,7 @@ TEST_F(ThreePoints, LinearCost2) { EXPECT_NEAR(source_->GetSolutionCost(result), b, 1e-6); EXPECT_NEAR(target_->GetSolutionCost(result), 0.0, 1e-6); EXPECT_NEAR(sink_->GetSolutionCost(result), 0.0, 1e-6); + CheckConvexRestriction(result); } TEST_F(ThreePoints, LinearCost3) { @@ -526,6 +549,7 @@ TEST_F(ThreePoints, LinearCost3) { EXPECT_NEAR(source_->GetSolutionCost(result), a.dot(p_source_.x()) + b, 1e-6); EXPECT_NEAR(target_->GetSolutionCost(result), 0.0, 1e-6); EXPECT_NEAR(sink_->GetSolutionCost(result), 0.0, 1e-6); + CheckConvexRestriction(result); } TEST_F(ThreePoints, LinearCost4) { @@ -568,6 +592,7 @@ TEST_F(ThreePoints, QuadraticCost) { 1e-6); EXPECT_NEAR(target_->GetSolutionCost(result), 0.0, 1e-6); EXPECT_NEAR(sink_->GetSolutionCost(result), 0.0, 1e-6); + CheckConvexRestriction(result); } TEST_F(ThreePoints, QuadraticCost2) { @@ -597,6 +622,7 @@ TEST_F(ThreePoints, QuadraticCost2) { 1e-6); EXPECT_NEAR(target_->GetSolutionCost(result), 0.0, 1e-6); EXPECT_NEAR(sink_->GetSolutionCost(result), 0.0, 1e-6); + CheckConvexRestriction(result); } TEST_F(ThreePoints, QuadraticCost3) { @@ -623,6 +649,7 @@ TEST_F(ThreePoints, QuadraticCost3) { 1e-6); EXPECT_NEAR(target_->GetSolutionCost(result), 0.0, 1e-6); EXPECT_NEAR(sink_->GetSolutionCost(result), 0.0, 1e-6); + CheckConvexRestriction(result); } TEST_F(ThreePoints, QuadraticCost4) { @@ -650,6 +677,7 @@ TEST_F(ThreePoints, QuadraticCost4) { (R_v * p_source_.x() + d_v).squaredNorm(), 1e-5); EXPECT_NEAR(target_->GetSolutionCost(result), 0.0, 1e-6); EXPECT_NEAR(sink_->GetSolutionCost(result), 0.0, 1e-6); + CheckConvexRestriction(result); } // Costs must be strictly positive. @@ -695,6 +723,7 @@ TEST_F(ThreePoints, L1NormCost) { EXPECT_NEAR(source_->GetSolutionCost(result), abs(A_v * p_source_.x()), 1e-6); EXPECT_NEAR(target_->GetSolutionCost(result), 0.0, 1e-6); EXPECT_NEAR(sink_->GetSolutionCost(result), 0.0, 1e-6); + CheckConvexRestriction(result); } TEST_F(ThreePoints, L1NormCost2) { @@ -727,6 +756,7 @@ TEST_F(ThreePoints, L1NormCost2) { EXPECT_NEAR(source_->GetSolutionCost(result), abs(A_v * p_source_.x()), 1e-6); EXPECT_NEAR(target_->GetSolutionCost(result), 0.0, 1e-6); EXPECT_NEAR(sink_->GetSolutionCost(result), 0.0, 1e-6); + CheckConvexRestriction(result); } TEST_F(ThreePoints, L2NormCost) { @@ -753,6 +783,7 @@ TEST_F(ThreePoints, L2NormCost) { 1e-6); EXPECT_NEAR(target_->GetSolutionCost(result), 0.0, 1e-6); EXPECT_NEAR(sink_->GetSolutionCost(result), 0.0, 1e-6); + CheckConvexRestriction(result); } TEST_F(ThreePoints, L2NormCost2) { @@ -785,6 +816,7 @@ TEST_F(ThreePoints, L2NormCost2) { 1e-6); EXPECT_NEAR(target_->GetSolutionCost(result), 0.0, 1e-6); EXPECT_NEAR(sink_->GetSolutionCost(result), 0.0, 1e-6); + CheckConvexRestriction(result); } TEST_F(ThreePoints, LInfNormCost) { @@ -810,6 +842,7 @@ TEST_F(ThreePoints, LInfNormCost) { EXPECT_NEAR(source_->GetSolutionCost(result), abs(A_v * p_source_.x()), 1e-6); EXPECT_NEAR(target_->GetSolutionCost(result), 0.0, 1e-6); EXPECT_NEAR(sink_->GetSolutionCost(result), 0.0, 1e-6); + CheckConvexRestriction(result); } TEST_F(ThreePoints, LInfNormCost2) { @@ -842,6 +875,7 @@ TEST_F(ThreePoints, LInfNormCost2) { EXPECT_NEAR(source_->GetSolutionCost(result), abs(A_v * p_source_.x()), 1e-6); EXPECT_NEAR(target_->GetSolutionCost(result), 0.0, 1e-6); EXPECT_NEAR(sink_->GetSolutionCost(result), 0.0, 1e-6); + CheckConvexRestriction(result); } TEST_F(ThreePoints, PerspectiveQuadraticCost) { @@ -875,6 +909,12 @@ TEST_F(ThreePoints, PerspectiveQuadraticCost) { EXPECT_NEAR(source_->GetSolutionCost(result), vertex_expected_cost, 1e-6); EXPECT_NEAR(target_->GetSolutionCost(result), 0.0, 1e-6); EXPECT_NEAR(sink_->GetSolutionCost(result), 0.0, 1e-6); + if (result.get_solver_id() == solvers::CsdpSolver::id()) { + // CSDP 6.2.0 gets the wrong cost -- but correct solution -- in the convex + // restriction (on linux only). + return; + } + CheckConvexRestriction(result); } TEST_F(ThreePoints, GetSolutionPath) { @@ -918,6 +958,25 @@ class ThreeBoxes : public ::testing::Test { options_.convex_relaxation = true; } + void CheckConvexRestriction(const MathematicalProgramResult& result) { + MathematicalProgramResult restriction_result = + g_.SolveConvexRestriction(std::vector({e_on_}), options_); + // Confirm that we get a convex solver (not an NLP solver). + if (MixedIntegerSolverAvailable()) { + EXPECT_TRUE( + restriction_result.get_solver_id() == solvers::MosekSolver::id() || + restriction_result.get_solver_id() == solvers::GurobiSolver::id()); + } + EXPECT_TRUE(restriction_result.is_success()); + EXPECT_NEAR(result.get_optimal_cost(), + restriction_result.get_optimal_cost(), 1e-6); + for (const auto* v : {source_, target_, sink_}) { + EXPECT_TRUE(CompareMatrices(result.GetSolution(v->x()), + restriction_result.GetSolution(v->x()), + 1e-6)); + } + } + GraphOfConvexSets g_; Edge* e_on_{nullptr}; Edge* e_off_{nullptr}; @@ -961,6 +1020,7 @@ TEST_F(ThreeBoxes, LinearEqualityConstraint) { EXPECT_TRUE(CompareMatrices(source_->GetSolution(result), -b, 1e-6)); EXPECT_TRUE(CompareMatrices(target_->GetSolution(result), b, 1e-6)); EXPECT_TRUE(sink_->GetSolution(result).hasNaN()); + CheckConvexRestriction(result); } TEST_F(ThreeBoxes, LinearEqualityConstraint2) { @@ -989,6 +1049,7 @@ TEST_F(ThreeBoxes, LinearEqualityConstraint2) { Aeq.rightCols(2) * target_->GetSolution(result), beq, 1e-6)); EXPECT_TRUE(sink_->GetSolution(result).hasNaN()); + CheckConvexRestriction(result); } TEST_F(ThreeBoxes, LinearConstraint) { @@ -1001,6 +1062,7 @@ TEST_F(ThreeBoxes, LinearConstraint) { EXPECT_TRUE((source_->GetSolution(result).array() <= b.array() - 1e-6).all()); EXPECT_TRUE((target_->GetSolution(result).array() >= b.array() - 1e-6).all()); EXPECT_TRUE(sink_->GetSolution(result).hasNaN()); + CheckConvexRestriction(result); } TEST_F(ThreeBoxes, LinearConstraint2) { @@ -1043,6 +1105,7 @@ TEST_F(ThreeBoxes, LinearConstraint2) { CompareMatrices(source_->GetSolution(result), Vector2d::Zero(), 1e-6)); EXPECT_FALSE( CompareMatrices(target_->GetSolution(result), Vector2d::Zero(), 1e-6)); + CheckConvexRestriction(result); } TEST_F(ThreeBoxes, SolveConvexRestriction) { @@ -2002,6 +2065,13 @@ GTEST_TEST(ShortestPathTest, Graphviz) { EXPECT_THAT(g.GetGraphvizString(result), AllOf(HasSubstr("x ="), HasSubstr("cost ="), HasSubstr("ϕ ="), HasSubstr("ϕ xᵤ ="), HasSubstr("ϕ xᵥ ="))); + + // With a rounded result. + options.max_rounded_paths = 1; + result = g.SolveShortestPath(*source, *target, options); + EXPECT_THAT(g.GetGraphvizString(result), + AllOf(HasSubstr("x ="), HasSubstr("cost ="), HasSubstr("ϕ ="))); + // No slack variables. EXPECT_THAT( g.GetGraphvizString(result, false), diff --git a/solvers/cost.h b/solvers/cost.h index 58be46836021..a2eb1a59946c 100644 --- a/solvers/cost.h +++ b/solvers/cost.h @@ -386,11 +386,15 @@ class LInfNormCost : public Cost { }; /** - * If z = Ax + b, implements a cost of the form: + * If \f$ z = Ax + b,\f$ implements a cost of the form: + * @f[ * (z_1^2 + z_2^2 + ... + z_{n-1}^2) / z_0. + * @f] * Note that this cost is convex when we additionally constrain z_0 > 0. It is * treated as a generic nonlinear objective by most solvers. * + * Costs of this form are sometimes referred to as "quadratic over linear". + * * @ingroup solver_evaluators */ class PerspectiveQuadraticCost : public Cost {