From 6363af3b40e0ce8eafd76aab2a1a6c3d01638da3 Mon Sep 17 00:00:00 2001 From: Hongkai Dai Date: Thu, 2 Jan 2025 10:46:22 -0800 Subject: [PATCH] KinematicTrajectoryOptimization adds a linear constraint on velocity. This imposes a linear constraint on our decision variables. --- .../planning_py_trajectory_optimization.cc | 4 + .../test/trajectory_optimization_test.py | 3 + .../kinematic_trajectory_optimization.cc | 75 ++++++++++++++++++- .../kinematic_trajectory_optimization.h | 14 ++++ .../kinematic_trajectory_optimization_test.cc | 46 ++++++++++++ 5 files changed, 140 insertions(+), 2 deletions(-) diff --git a/bindings/pydrake/planning/planning_py_trajectory_optimization.cc b/bindings/pydrake/planning/planning_py_trajectory_optimization.cc index 564a6caa6a80..da5926d776b8 100644 --- a/bindings/pydrake/planning/planning_py_trajectory_optimization.cc +++ b/bindings/pydrake/planning/planning_py_trajectory_optimization.cc @@ -319,6 +319,10 @@ void DefinePlanningTrajectoryOptimization(py::module m) { &Class::AddVelocityConstraintAtNormalizedTime, py::arg("constraint"), py::arg("s"), cls_doc.AddVelocityConstraintAtNormalizedTime.doc) + .def("AddVelocityLinearConstraintAtNormalizedTime", + &Class::AddVelocityLinearConstraintAtNormalizedTime, + py::arg("constraint"), py::arg("s"), + cls_doc.AddVelocityLinearConstraintAtNormalizedTime.doc) .def("AddPathAccelerationConstraint", &Class::AddPathAccelerationConstraint, py::arg("lb"), py::arg("ub"), py::arg("s"), cls_doc.AddPathAccelerationConstraint.doc) diff --git a/bindings/pydrake/planning/test/trajectory_optimization_test.py b/bindings/pydrake/planning/test/trajectory_optimization_test.py index aba718ac6ced..cdac4d5b6ce8 100644 --- a/bindings/pydrake/planning/test/trajectory_optimization_test.py +++ b/bindings/pydrake/planning/test/trajectory_optimization_test.py @@ -247,6 +247,9 @@ def test_kinematic_trajectory_optimization(self): lb=np.zeros((4, 1)), ub=np.zeros((4, 1))) trajopt.AddVelocityConstraintAtNormalizedTime(velocity_constraint, s=0) + trajopt.AddVelocityLinearConstraintAtNormalizedTime( + constraint=mp.LinearConstraint( + np.eye(2), lb=np.zeros((2,)), ub=np.ones((2,))), s=0) trajopt.AddPathAccelerationConstraint(lb=b, ub=b, s=0) trajopt.AddDurationConstraint(1, 1) trajopt.AddPositionBounds(lb=b, ub=b) diff --git a/planning/trajectory_optimization/kinematic_trajectory_optimization.cc b/planning/trajectory_optimization/kinematic_trajectory_optimization.cc index d593eb8509b2..9fdbcb8cfd1a 100644 --- a/planning/trajectory_optimization/kinematic_trajectory_optimization.cc +++ b/planning/trajectory_optimization/kinematic_trajectory_optimization.cc @@ -22,6 +22,8 @@ namespace drake { namespace planning { namespace trajectory_optimization { +const double kInf = std::numeric_limits::infinity(); + using math::BsplineBasis; using math::EigenToStdVector; using math::ExtractValue; @@ -205,8 +207,8 @@ KinematicTrajectoryOptimization::KinematicTrajectoryOptimization( num_positions_, num_control_points_, "control_point"); duration_ = prog_.NewContinuousVariables(1, "duration")[0]; // duration >= 0. - auto duration_bbox_binding = prog_.AddBoundingBoxConstraint( - 0, std::numeric_limits::infinity(), duration_); + auto duration_bbox_binding = + prog_.AddBoundingBoxConstraint(0, kInf, duration_); duration_bbox_binding.evaluator()->set_description( "positive duration constraint"); @@ -331,6 +333,75 @@ KinematicTrajectoryOptimization::AddVelocityConstraintAtNormalizedTime( return binding; } +Binding +KinematicTrajectoryOptimization::AddVelocityLinearConstraintAtNormalizedTime( + const std::shared_ptr& constraint, double s) { + DRAKE_DEMAND(constraint->num_vars() == num_positions_); + DRAKE_DEMAND(0 <= s && s <= 1); + const VectorX rdot = sym_rdot_->value(s); + // `constraint` is + // lb <= A * qdot <= ub + // we have qdot = rdot / T + // So the linear constraint we impose is + // lb <= A * rdot/T <= ub + // Namely + // A * rdot - lb * T >= 0 + // A * rdot - ub * T <= 0 + auto [vars_vel, _] = symbolic::ExtractVariablesFromExpression(rdot); + // TODO(hongkai.dai) call AsLinearInControlPoints in bsplinebasis when it is + // ready to compute M_vel. + Eigen::MatrixXd M_vel(num_positions(), vars_vel.size()); + symbolic::DecomposeLinearExpressions(rdot, vars_vel, &M_vel); + // The constraint is + // A * M_vel * vars_vel - lb * T >= 0 + // A * M_vel * vars_vel - ub * T <= 0 + // We call this new constraint as + // lb_new <= A_new * vars_all <= ub_new + // where vars_all = [vars_vel, T] + Eigen::MatrixXd A_new(2 * constraint->num_constraints(), vars_vel.rows() + 1); + Eigen::VectorXd lb_new(A_new.rows()); + Eigen::VectorXd ub_new(A_new.rows()); + int A_new_row_count = 0; + const auto& A = constraint->GetDenseA(); + for (int i = 0; i < constraint->num_constraints(); ++i) { + // If lb(i) == ub(i), then we only add a linear equality constraint + // A.row(i) * M_vel * vars_vel - lb(i) * T = 0. + // Otherwise, we add the constraint + // A.row(i) * M_vel * vars_vel - lb(i) * T >= 0 + // and + // A.row(i) * M_vel * vars_vel - ub(i) * T <= 0 + if (constraint->lower_bound()(i) == constraint->upper_bound()(i)) { + DRAKE_DEMAND(!std::isinf(constraint->lower_bound()(i))); + A_new.row(A_new_row_count) << A.row(i) * M_vel, + -constraint->lower_bound()(i); + lb_new(A_new_row_count) = 0; + ub_new(A_new_row_count) = 0; + ++A_new_row_count; + } else { + if (!std::isinf(constraint->lower_bound()(i))) { + A_new.row(A_new_row_count) << constraint->GetDenseA().row(i) * M_vel, + -constraint->lower_bound()(i); + lb_new(A_new_row_count) = 0; + ub_new(A_new_row_count) = kInf; + ++A_new_row_count; + } + if (!std::isinf(constraint->upper_bound()(i))) { + A_new.row(A_new_row_count) << constraint->GetDenseA().row(i) * M_vel, + -constraint->upper_bound()(i); + lb_new(A_new_row_count) = -kInf; + ub_new(A_new_row_count) = 0; + ++A_new_row_count; + } + } + } + auto binding = prog_.AddLinearConstraint( + A_new.topRows(A_new_row_count), lb_new.topRows(A_new_row_count), + ub_new.topRows(A_new_row_count), + {vars_vel, Vector1(duration_)}); + binding.evaluator()->set_description("velocity linear constraint"); + return binding; +} + Binding KinematicTrajectoryOptimization::AddPathAccelerationConstraint( const Eigen::Ref& lb, diff --git a/planning/trajectory_optimization/kinematic_trajectory_optimization.h b/planning/trajectory_optimization/kinematic_trajectory_optimization.h index fb99ffe57ce8..9a361f1aefd0 100644 --- a/planning/trajectory_optimization/kinematic_trajectory_optimization.h +++ b/planning/trajectory_optimization/kinematic_trajectory_optimization.h @@ -142,6 +142,20 @@ class KinematicTrajectoryOptimization { solvers::Binding AddVelocityConstraintAtNormalizedTime( const std::shared_ptr& constraint, double s); + /** Adds a linear constraint on trajectory velocity `q̇(t)`, evaluated at + `s`. The constraint will be evaluated as if it is bound with variables + corresponding to `q̇(T*s)` (as opposed to [q(T*s), q̇(T*s)] in + AddVelocityConstraintAtNormalizedTime()). + + This method should be compared with AddPathVelocityConstraint, which only + constrains ṙ(s) because it does not reason about the time scaling, T. + + @pre constraint.num_vars() == num_positions() + @pre 0 <= `s` <= 1. */ + solvers::Binding + AddVelocityLinearConstraintAtNormalizedTime( + const std::shared_ptr& constraint, double s); + /** Adds a linear constraint on the second derivative of the path, `lb` ≤ r̈(s) ≤ `ub`. Note that this does NOT directly constrain q̈(t). @pre 0 <= `s` <= 1. */ diff --git a/planning/trajectory_optimization/test/kinematic_trajectory_optimization_test.cc b/planning/trajectory_optimization/test/kinematic_trajectory_optimization_test.cc index f555d5a001fb..24e0b453117e 100644 --- a/planning/trajectory_optimization/test/kinematic_trajectory_optimization_test.cc +++ b/planning/trajectory_optimization/test/kinematic_trajectory_optimization_test.cc @@ -23,6 +23,8 @@ namespace drake { namespace planning { namespace trajectory_optimization { +const double kInf = std::numeric_limits::infinity(); + using solvers::ExpressionConstraint; using solvers::MathematicalProgramResult; using solvers::OsqpSolver; @@ -154,6 +156,50 @@ TEST_F(KinematicTrajectoryOptimizationTest, EXPECT_TRUE(CompareMatrices(qdot->value(0.2), x_desired.tail<3>(), kTol)); } +TEST_F(KinematicTrajectoryOptimizationTest, + AddVelocityLinearConstraintAtNormalizedTime) { + EXPECT_EQ(trajopt_.prog().generic_constraints().size(), 0); + int num_linear_constraints = + static_cast(trajopt_.prog().linear_constraints().size()); + const Vector3d v_desired_lb1(0.4, -kInf, 0.6); + const Vector3d v_desired_ub1(0.4, 0.7, kInf); + const double s1 = 0.2; + auto binding1 = trajopt_.AddVelocityLinearConstraintAtNormalizedTime( + std::make_shared(v_desired_lb1, + v_desired_ub1), + s1); + const Vector3d v_desired_lb2(-kInf, 0.5, 0.6); + const Vector3d v_desired_ub2(kInf, 0.7, 0.6); + const double s2 = 0.7; + auto binding2 = trajopt_.AddVelocityLinearConstraintAtNormalizedTime( + std::make_shared(v_desired_lb2, + v_desired_ub2), + s2); + EXPECT_THAT(binding1.to_string(), HasSubstr("velocity linear constraint")); + EXPECT_THAT(binding2.to_string(), HasSubstr("velocity linear constraint")); + EXPECT_EQ(trajopt_.prog().generic_constraints().size(), 0); + EXPECT_EQ(trajopt_.prog().linear_constraints().size(), + num_linear_constraints + 2); + + trajopt_.AddDurationConstraint(1.5, 2); + trajopt_.SetInitialGuess(BsplineTrajectory( + trajopt_.basis(), math::EigenToStdVector( + MatrixXd::Ones(3, trajopt_.num_control_points())))); + auto result = Solve(trajopt_.prog()); + EXPECT_TRUE(result.is_success()); + auto q = trajopt_.ReconstructTrajectory(result); + auto qdot = q.MakeDerivative(); + const double kTol = 1e-6; + const double T = result.GetSolution(trajopt_.duration()); + const Eigen::VectorXd qdot_val1 = qdot->value(s1 * T); + const Eigen::VectorXd qdot_val2 = qdot->value(s2 * T); + + EXPECT_TRUE((qdot_val1.array() >= v_desired_lb1.array() - kTol).all()); + EXPECT_TRUE((qdot_val1.array() <= v_desired_ub1.array() + kTol).all()); + EXPECT_TRUE((qdot_val2.array() >= v_desired_lb2.array() - kTol).all()); + EXPECT_TRUE((qdot_val2.array() <= v_desired_ub2.array() + kTol).all()); +} + TEST_F(KinematicTrajectoryOptimizationTest, AddPathAccelerationConstraint) { EXPECT_EQ(trajopt_.prog().linear_constraints().size(), 0); VectorXd desired = VectorXd::Ones(num_positions_);