KinematicTrajectoryOptimization adds a linear constraint on velocity.

This imposes a linear constraint on our decision variables.

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)

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)

planning/trajectory_optimization/kinematic_trajectory_optimization.cc

@@ -22,6 +22,8 @@ namespace drake {
 namespace planning {
 namespace trajectory_optimization {
 
+const double kInf = std::numeric_limits<double>::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<double>::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<LinearConstraint>
+KinematicTrajectoryOptimization::AddVelocityLinearConstraintAtNormalizedTime(
+    const std::shared_ptr<solvers::LinearConstraint>& constraint, double s) {
+  DRAKE_DEMAND(constraint->num_vars() == num_positions_);
+  DRAKE_DEMAND(0 <= s && s <= 1);
+  const VectorX<Expression> 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<symbolic::Variable>(duration_)});
+  binding.evaluator()->set_description("velocity linear constraint");
+  return binding;
+}
+
 Binding<LinearConstraint>
 KinematicTrajectoryOptimization::AddPathAccelerationConstraint(
     const Eigen::Ref<const Eigen::VectorXd>& lb,

planning/trajectory_optimization/kinematic_trajectory_optimization.h

@@ -142,6 +142,20 @@ class KinematicTrajectoryOptimization {
   solvers::Binding<solvers::Constraint> AddVelocityConstraintAtNormalizedTime(
       const std::shared_ptr<solvers::Constraint>& 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<solvers::LinearConstraint>
+  AddVelocityLinearConstraintAtNormalizedTime(
+      const std::shared_ptr<solvers::LinearConstraint>& 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. */

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<double>::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<int>(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<solvers::BoundingBoxConstraint>(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<solvers::BoundingBoxConstraint>(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<double>(
+                            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_);