Derivative Boundary conditions in Toppra::SolvePathParameterization (#…

…21975)

bindings/pydrake/multibody/optimization_py.cc

@@ -193,6 +193,7 @@ PYBIND11_MODULE(optimization, m) {
         .def_static("CalcGridPoints", &Class::CalcGridPoints, py::arg("path"),
             py::arg("options"), cls_doc.CalcGridPoints.doc)
         .def("SolvePathParameterization", &Class::SolvePathParameterization,
+            py::arg("s_dot_start") = 0, py::arg("s_dot_end") = 0,
             cls_doc.SolvePathParameterization.doc)
         .def("AddJointVelocityLimit", &Class::AddJointVelocityLimit,
             py::arg("lower_limit"), py::arg("upper_limit"),

bindings/pydrake/multibody/test/optimization_test.py

@@ -400,5 +400,6 @@ def test_solve(self):
         toppra = Toppra(path=path, plant=plant, gridpoints=gridpoints)
 
         toppra.AddJointAccelerationLimit([-1.], [1.])
-        trajectory = toppra.SolvePathParameterization()
+        trajectory = toppra.SolvePathParameterization(
+            s_dot_start=0, s_dot_end=0)
         self.assertIsInstance(trajectory, PiecewisePolynomial)

multibody/optimization/test/toppra_test.cc

@@ -24,8 +24,8 @@ class IiwaToppraTest : public ::testing::Test {
         .AddModelsFromUrl(
             "package://drake_models/iiwa_description/sdf/"
             "iiwa7_no_collision.sdf");
-    iiwa_plant_->WeldFrames(
-        iiwa_plant_->world_frame(), iiwa_plant_->GetFrameByName("iiwa_link_0"));
+    iiwa_plant_->WeldFrames(iiwa_plant_->world_frame(),
+                            iiwa_plant_->GetFrameByName("iiwa_link_0"));
     iiwa_plant_->Finalize();
     plant_context_ = iiwa_plant_->CreateDefaultContext();
 
@@ -106,6 +106,21 @@ class IiwaToppraTest : public ::testing::Test {
   std::unique_ptr<Toppra> toppra_;
 };
 
+TEST_F(IiwaToppraTest, BoundaryConditions) {
+  toppra_->AddJointVelocityLimit(Eigen::VectorXd::Constant(7, -1),
+                                 Eigen::VectorXd::Constant(7, 1));
+  toppra_->AddJointAccelerationLimit(Eigen::VectorXd::Constant(7, -10),
+                                     Eigen::VectorXd::Constant(7, 10));
+  const double s_dot_start = 0.7;
+  const double s_dot_end = 0.4;
+  auto result = toppra_->SolvePathParameterization(s_dot_start, s_dot_end);
+  ASSERT_TRUE(result);
+  auto s_path = result.value();
+  auto s_path_dot = s_path.derivative();
+  EXPECT_NEAR(s_path_dot.value(s_path.start_time())(0, 0), s_dot_start, 1e-6);
+  EXPECT_NEAR(s_path_dot.value(s_path.end_time())(0, 0), s_dot_end, 1e-6);
+}
+
 TEST_F(IiwaToppraTest, JointVelocityLimit) {
   Eigen::VectorXd lower_bound(7);
   lower_bound << -1.1, -1.2, -1.3, -1.4, -1.5, -1.6, -1.7;
@@ -518,7 +533,7 @@ TEST_F(IiwaToppraTest, FrameAccelerationLimitTrajectoryOverlap) {
 
   const double path_start = path_.start_time();
   const double path_end = path_.end_time();
-  const std::vector<double> big_breaks{path_start - 0.1, path_end +  0.1};
+  const std::vector<double> big_breaks{path_start - 0.1, path_end + 0.1};
 
   const auto lower_bigger =
       PiecewisePolynomial<double>::ZeroOrderHold(big_breaks, lower_samples);
@@ -539,8 +554,8 @@ TEST_F(IiwaToppraTest, FrameAccelerationLimitTrajectoryOverlap) {
         fmt::format(".* can't add a trajectory frame acceleration .* lower "
                     "limit domain \\[{}, {}\\].* upper limit domain "
                     "\\[{}, {}\\] .* path domain \\[{}, {}\\].",
-                    bad_breaks[0], bad_breaks[1], big_breaks[0],
-                    big_breaks[1], path_start, path_end));
+                    bad_breaks[0], bad_breaks[1], big_breaks[0], big_breaks[1],
+                    path_start, path_end));
 
     // Just for upper.
     DRAKE_EXPECT_THROWS_MESSAGE(
@@ -607,7 +622,7 @@ TEST_F(IiwaToppraTest, FrameAccelerationLimitVarying) {
   // Lower limit for the interval [s0, s1).
   const Vector6d lower0 = Vector6d::Constant(-0.5);
   const Vector6d upper0 = -lower0;
-    // Lower limit for the interval [s1, s2).
+  // Lower limit for the interval [s1, s2).
   const Vector6d lower1 = Vector6d::Constant(-7);
   const Vector6d upper1 = -lower1;
 

multibody/optimization/toppra.cc

@@ -602,21 +602,20 @@ Toppra::ComputeForwardPass(double s_dot_0,
   return std::make_pair(xstar, ustar);
 }
 
-std::optional<PiecewisePolynomial<double>> Toppra::SolvePathParameterization() {
-  const double s_dot_0 = 0;
-  const double s_dot_N = 0;
-
+std::optional<PiecewisePolynomial<double>> Toppra::SolvePathParameterization(
+    double s_dot_start, double s_dot_end) {
   // Clp appears to outperform Mosek for these optimizations.
   // TODO(mpetersen94) Add Gurobi in the correct ordering
   auto solver = solvers::MakeFirstAvailableSolver(
       {solvers::ClpSolver::id(), solvers::MosekSolver::id()});
 
   const std::optional<Eigen::Matrix2Xd> K =
-      ComputeBackwardPass(s_dot_0, s_dot_N, *solver);
+      ComputeBackwardPass(s_dot_start, s_dot_end, *solver);
   if (!K) {  // Error occurred in backpass, return nothing
     return std::nullopt;
   }
-  const auto forward_results = ComputeForwardPass(s_dot_0, K.value(), *solver);
+  const auto forward_results =
+      ComputeForwardPass(s_dot_start, K.value(), *solver);
   if (!forward_results) {  // Error occurred in forward pass, return nothing
     return std::nullopt;
   }

multibody/optimization/toppra.h

@@ -112,8 +112,13 @@ class Toppra {
    * generate a time parameterized trajectory.
    * The path parameterization has the same start time as the original path's
    * starting break.
+   * @param s_dot_start ṡ(0). The default value is zero (trajectory starts at
+   * zero velocity).
+   * @param s_dot_end ṡ(T), where T is the end break of the path. The default
+   * value is zero (trajectory ends at zero velocity).
    */
-  std::optional<PiecewisePolynomial<double>> SolvePathParameterization();
+  std::optional<PiecewisePolynomial<double>> SolvePathParameterization(
+      double s_dot_start = 0, double s_dot_end = 0);
 
   /**
    * Adds a velocity limit to all the degrees of freedom in the plant. The
@@ -225,12 +230,11 @@ class Toppra {
    * @pydrake_mkdoc_identifier{const}
    */
   std::pair<Binding<LinearConstraint>, Binding<LinearConstraint>>
-  AddFrameAccelerationLimit(
-      const Frame<double>& constraint_frame,
-      const Eigen::Ref<const Vector6d>& lower_limit,
-      const Eigen::Ref<const Vector6d>& upper_limit,
-      ToppraDiscretization discretization =
-          ToppraDiscretization::kInterpolation);
+  AddFrameAccelerationLimit(const Frame<double>& constraint_frame,
+                            const Eigen::Ref<const Vector6d>& lower_limit,
+                            const Eigen::Ref<const Vector6d>& upper_limit,
+                            ToppraDiscretization discretization =
+                                ToppraDiscretization::kInterpolation);
   /**
    * A version of acceleration limit that uses a trajectory for the upper and
    * lower limits.