We inadvertantly were dereferencing a null iterator if the user provi…

…ded an invalid variable, instead of properly catching the bug. This fixes that bug, and updates the error message we check for.
RobotLocomotion · Nov 15, 2024 · ce4ebb6 · ce4ebb6
1 parent d51a9ec
commit ce4ebb6
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 5 deletions.
diff --git a/planning/trajectory_optimization/gcs_trajectory_optimization.cc b/planning/trajectory_optimization/gcs_trajectory_optimization.cc
@@ -842,6 +842,12 @@ Binding<T> SubstituteAllVariables(
         old_variables.begin(), old_variables.end(), [&](const Variable& var) {
           return var.equal_to(new_binding_variables[i]);
         });
+    if (iterator == old_variables.end()) {
+      // We throw an error if the user gave an unknown variable.
+      throw std::runtime_error(
+          fmt::format("Unknown variable with name {} provided.",
+                      new_binding_variables[i].get_name()));
+    }
     size_t index = std::distance(std::begin(old_variables), iterator);
     new_binding_variables[i] = new_variables[index];
   }

diff --git a/planning/trajectory_optimization/gcs_trajectory_optimization.h b/planning/trajectory_optimization/gcs_trajectory_optimization.h
@@ -394,7 +394,7 @@ class GcsTrajectoryOptimization final {
     }
 
     /** Convenience overload of AddVertexConstraint to take in a
-     * Binding<Constraint>. */
+    Binding<Constraint>. */
     void AddVertexConstraint(
         const solvers::Binding<solvers::Constraint>& binding,
         const std::unordered_set<

diff --git a/planning/trajectory_optimization/test/gcs_trajectory_optimization_test.cc b/planning/trajectory_optimization/test/gcs_trajectory_optimization_test.cc
@@ -2702,7 +2702,7 @@ GTEST_TEST(GcsTrajectoryOptimizationTest, GenericSubgraphVertexCostConstraint) {
   DRAKE_EXPECT_THROWS_MESSAGE(middle.AddVertexCost(bad_expression_3),
                               ".*.IsSubsetOf\\(Variables\\(placeholder_x_.*");
   DRAKE_EXPECT_THROWS_MESSAGE(middle.AddVertexCost(ParseCost(bad_expression_3)),
-                              ".*.IsSubsetOf\\(Variables\\(placeholder_x_.*");
+                              ".*Unknown variable.*");
   DRAKE_EXPECT_THROWS_MESSAGE(middle.AddVertexConstraint(bad_formula_1),
                               ".*Edge placeholder variables cannot be used.*");
   DRAKE_EXPECT_THROWS_MESSAGE(
@@ -2717,7 +2717,7 @@ GTEST_TEST(GcsTrajectoryOptimizationTest, GenericSubgraphVertexCostConstraint) {
                               ".*.IsSubsetOf\\(Variables\\(placeholder_x_.*");
   DRAKE_EXPECT_THROWS_MESSAGE(
       middle.AddVertexConstraint(ParseConstraint(bad_formula_3)),
-      ".*.IsSubsetOf\\(Variables\\(placeholder_x_.*");
+      ".*Unknown variable.*");
 }
 
 GTEST_TEST(GcsTrajectoryOptimizationTest, GenericSubgraphEdgeCostConstraint) {
@@ -2842,7 +2842,7 @@ GTEST_TEST(GcsTrajectoryOptimizationTest, GenericSubgraphEdgeCostConstraint) {
   DRAKE_EXPECT_THROWS_MESSAGE(middle.AddEdgeCost(bad_expression_3),
                               ".*IsSubsetOf\\(allowed_vars_\\).*");
   DRAKE_EXPECT_THROWS_MESSAGE(middle.AddEdgeCost(ParseCost(bad_expression_3)),
-                              ".*IsSubsetOf\\(allowed_vars_\\).*");
+                              ".*Unknown variable.*");
   DRAKE_EXPECT_THROWS_MESSAGE(
       middle.AddEdgeConstraint(bad_formula_1),
       ".*Vertex placeholder variables cannot be used.*");
@@ -2859,7 +2859,7 @@ GTEST_TEST(GcsTrajectoryOptimizationTest, GenericSubgraphEdgeCostConstraint) {
                               ".*IsSubsetOf\\(allowed_vars_\\).*");
   DRAKE_EXPECT_THROWS_MESSAGE(
       middle.AddEdgeConstraint(ParseConstraint(bad_formula_3)),
-      ".*IsSubsetOf\\(allowed_vars_\\).*");
+      ".*Unknown variable.*");
 }
 
 GTEST_TEST(GcsTrajectoryOptimizationTest,