FixUp GCS PhiConstraint test (RobotLocomotion#21330)

PR 21053 to upgrade clarabel fell over on some numerical problems in
the PhiConstraint unit test.  I've captured that failure and reported
it in oxfordcontrol/Clarabel.rs#96 .

In examining the problem, I found a number of small errors in the
existing test. Resolving those errors allows clarabel 7.1 to pass the
tests without any upstream changes required.

The changes in the PhiConstraint test are:

- QuadraticCost was being used incorrectly (we forgot the factor of
  2). It was also documented as being the L2 norm, instead of the L2
  norm squared.

- Added a diagram to the doc and minor code cleanup.

In GCS, I made two changes, both of which have no functional impact:

- The bounding box constraint with lb==ub was replaced with a linear
  equality constraint (which should be preferred).

- I've slightly improved the variable names used in the mathematical
  program, for readability during debugging. This program is
  intentionally not exposed to the user, and all of the changes were
  to internally defined variables. Although a user _could_ have gotten
  to these by mucking around in the MathematicalProgramResult, there
  is nothing in the API contract that is changing on them.

geometry/optimization/graph_of_convex_sets.cc

@@ -184,10 +184,12 @@ Edge::Edge(const EdgeId& id, Vertex* u, Vertex* v, std::string name)
       u_{u},
       v_{v},
       allowed_vars_{u_->x()},
-      phi_{"phi", symbolic::Variable::Type::BINARY},
+      phi_{name + "phi", symbolic::Variable::Type::BINARY},
       name_(std::move(name)),
-      y_{symbolic::MakeVectorContinuousVariable(u_->ambient_dimension(), "y")},
-      z_{symbolic::MakeVectorContinuousVariable(v_->ambient_dimension(), "z")},
+      y_{symbolic::MakeVectorContinuousVariable(u_->ambient_dimension(),
+                                                name_ + "y")},
+      z_{symbolic::MakeVectorContinuousVariable(v_->ambient_dimension(),
+                                                name_ + "z")},
       x_to_yz_{static_cast<size_t>(y_.size() + z_.size())} {
   DRAKE_DEMAND(u_ != nullptr);
   DRAKE_DEMAND(v_ != nullptr);
@@ -211,7 +213,8 @@ std::pair<Variable, Binding<Cost>> Edge::AddCost(const Binding<Cost>& binding) {
   DRAKE_THROW_UNLESS(Variables(binding.variables()).IsSubsetOf(allowed_vars_));
   const int n = ell_.size();
   ell_.conservativeResize(n + 1);
-  ell_[n] = Variable(fmt::format("ell{}", n), Variable::Type::CONTINUOUS);
+  ell_[n] =
+      Variable(fmt::format("{}ell{}", name_, n), Variable::Type::CONTINUOUS);
   costs_.emplace_back(binding);
   return std::pair<Variable, Binding<Cost>>(ell_[n], costs_.back());
 }
@@ -855,7 +858,7 @@ MathematicalProgramResult GraphOfConvexSets::SolveShortestPath(
 
     Variable phi;
     if (*options.convex_relaxation) {
-      phi = prog.NewContinuousVariables<1>("phi")[0];
+      phi = prog.NewContinuousVariables<1>(e->name() + "phi")[0];
       prog.AddBoundingBoxConstraint(0, 1, phi);
       relaxed_phi.emplace(edge_id, phi);
     } else {
@@ -865,7 +868,8 @@ MathematicalProgramResult GraphOfConvexSets::SolveShortestPath(
     if (e->phi_value_.has_value()) {
       DRAKE_DEMAND(*e->phi_value_);
       double phi_value = *e->phi_value_ ? 1.0 : 0.0;
-      prog.AddBoundingBoxConstraint(phi_value, phi_value, phi);
+      prog.AddLinearEqualityConstraint(Vector1d(1.0), phi_value,
+                                       Vector1<Variable>(phi));
     }
     prog.AddDecisionVariables(e->y_);
     prog.AddDecisionVariables(e->z_);

geometry/optimization/test/graph_of_convex_sets_test.cc

@@ -1391,6 +1391,17 @@ GTEST_TEST(ShortestPathTest, TwoStepLoopConstraint) {
 
 // Tests that all optimization variables are properly set, even when constrained
 // to be on or off.
+//            ┌──┐
+//   ┌───────►│v2├──────┐
+//   │        └──┘      │
+//   │                  ▼
+// ┌─┴┐                ┌──┐
+// │v0│                │v3│
+// └─┬┘                └──┘
+//   │                  ▲
+//   │        ┌──┐      │
+//   └───────►│v1├──────┘
+//            └──┘
 GTEST_TEST(ShortestPathTest, PhiConstraint) {
   GraphOfConvexSets spp;
 
@@ -1401,21 +1412,20 @@ GTEST_TEST(ShortestPathTest, PhiConstraint) {
 
   auto v = spp.Vertices();
 
-  Edge* edge_01 = spp.AddEdge(v[0], v[1]);
-  Edge* edge_02 = spp.AddEdge(v[0], v[2]);
+  spp.AddEdge(v[0], v[1]);
+  spp.AddEdge(v[0], v[2]);
   Edge* edge_13 = spp.AddEdge(v[1], v[3]);
-  Edge* edge_23 = spp.AddEdge(v[2], v[3]);
+  spp.AddEdge(v[2], v[3]);
 
-  // |xu - xv|₂
+  // |xu - xv|₂²
   Matrix<double, 4, 4> A = Matrix<double, 4, 4>::Identity();
   A.block(0, 2, 2, 2) = -Matrix2d::Identity();
   A.block(2, 0, 2, 2) = -Matrix2d::Identity();
-  auto cost = std::make_shared<solvers::QuadraticCost>(A, Vector4d::Zero());
-
-  edge_01->AddCost(solvers::Binding(cost, {v[0]->x(), v[1]->x()}));
-  edge_02->AddCost(solvers::Binding(cost, {v[0]->x(), v[2]->x()}));
-  edge_13->AddCost(solvers::Binding(cost, {v[1]->x(), v[3]->x()}));
-  edge_23->AddCost(solvers::Binding(cost, {v[2]->x(), v[3]->x()}));
+  auto cost =
+      std::make_shared<solvers::QuadraticCost>(2.0 * A, Vector4d::Zero());
+  for (auto e : spp.Edges()) {
+    e->AddCost(solvers::Binding(cost, {e->xu(), e->xv()}));
+  }
 
   GraphOfConvexSetsOptions options;
   options.preprocessing = false;
@@ -1433,7 +1443,7 @@ GTEST_TEST(ShortestPathTest, PhiConstraint) {
                                 0.5 * Vector2d(1, -1), 1e-6));
     EXPECT_TRUE(CompareMatrices(edge_13->GetSolutionPhiXv(result),
                                 0.5 * Vector2d(2, 0), 1e-6));
-    EXPECT_NEAR(edge_13->GetSolutionCost(result), 0.5 * 1, 1e-6);
+    EXPECT_NEAR(edge_13->GetSolutionCost(result), 1.0, 1e-6);
     EXPECT_NEAR(result.GetSolution(edge_13->phi()), 0.5, 1e-6);
     EXPECT_TRUE(
         CompareMatrices(v[1]->GetSolution(result), Vector2d(1, -1), 1e-6));
@@ -1466,8 +1476,8 @@ GTEST_TEST(ShortestPathTest, PhiConstraint) {
                                 Vector2d(1, -1), 1e-6));
     EXPECT_TRUE(CompareMatrices(edge_13->GetSolutionPhiXv(result),
                                 Vector2d(2, 0), 1e-6));
-    EXPECT_NEAR(edge_13->GetSolutionCost(result), 1, 1e-6);
-    EXPECT_NEAR(result.GetSolution(edge_13->phi()), 1, 1e-6);
+    EXPECT_NEAR(edge_13->GetSolutionCost(result), 2.0, 1e-4);
+    EXPECT_NEAR(result.GetSolution(edge_13->phi()), 1.0, 1e-6);
   }
 }