diff --git a/bindings/pydrake/multibody/optimization_py.cc b/bindings/pydrake/multibody/optimization_py.cc index 6d32cb412cb4..5fbd0dfdbbe8 100644 --- a/bindings/pydrake/multibody/optimization_py.cc +++ b/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"), diff --git a/bindings/pydrake/multibody/test/optimization_test.py b/bindings/pydrake/multibody/test/optimization_test.py index 342f2399ed51..6b8ae0311379 100644 --- a/bindings/pydrake/multibody/test/optimization_test.py +++ b/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) diff --git a/multibody/optimization/test/toppra_test.cc b/multibody/optimization/test/toppra_test.cc index 3558358fe93f..0622d39e4c0b 100644 --- a/multibody/optimization/test/toppra_test.cc +++ b/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_; }; +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 big_breaks{path_start - 0.1, path_end + 0.1}; + const std::vector big_breaks{path_start - 0.1, path_end + 0.1}; const auto lower_bigger = PiecewisePolynomial::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; diff --git a/multibody/optimization/toppra.cc b/multibody/optimization/toppra.cc index e0b3b0fc3428..a7f87b226c70 100644 --- a/multibody/optimization/toppra.cc +++ b/multibody/optimization/toppra.cc @@ -602,21 +602,20 @@ Toppra::ComputeForwardPass(double s_dot_0, return std::make_pair(xstar, ustar); } -std::optional> Toppra::SolvePathParameterization() { - const double s_dot_0 = 0; - const double s_dot_N = 0; - +std::optional> 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 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; } diff --git a/multibody/optimization/toppra.h b/multibody/optimization/toppra.h index 9a480aeea5a3..4239cac5791c 100644 --- a/multibody/optimization/toppra.h +++ b/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> SolvePathParameterization(); + std::optional> 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> - AddFrameAccelerationLimit( - const Frame& constraint_frame, - const Eigen::Ref& lower_limit, - const Eigen::Ref& upper_limit, - ToppraDiscretization discretization = - ToppraDiscretization::kInterpolation); + AddFrameAccelerationLimit(const Frame& constraint_frame, + const Eigen::Ref& lower_limit, + const Eigen::Ref& upper_limit, + ToppraDiscretization discretization = + ToppraDiscretization::kInterpolation); /** * A version of acceleration limit that uses a trajectory for the upper and * lower limits.