Implement Bspline "linear in control points" logic

For BsplineBasis and BsplineTrajectory.

Towards RobotLocomotion#22533 which is towards RobotLocomotion#22500.

common/trajectories/bezier_curve.h

@@ -70,9 +70,10 @@ class BezierCurve final : public trajectories::Trajectory<T> {
    @code
    auto M = curve.AsLinearInControlPoints(n);
    for (int i=0; i<curve.rows(); ++i) {
-     auto c = std::make_shared<solvers::LinearConstraint>(
-       M.col(k).transpose(), Vector1d(lb(i)), Vector1d(ub(i)));
-     prog.AddConstraint(c, curve.row(i).transpose());
+    prog.AddLinearConstraint(M.col(i).transpose(),
+                             Vector1d(lb(i)),
+                             Vector1d(ub(i)),
+                             curve.row(i).transpose());
    }
    @endcode
    Iterating over the rows of the control points is the natural sparsity pattern

common/trajectories/bspline_trajectory.cc

@@ -29,6 +29,85 @@ BsplineTrajectory<T>::BsplineTrajectory(BsplineBasis<T> basis,
 template <typename T>
 BsplineTrajectory<T>::~BsplineTrajectory() = default;
 
+template <typename T>
+Eigen::SparseMatrix<T> BsplineTrajectory<T>::AsLinearInControlPoints(
+    int derivative_order) const {
+  DRAKE_THROW_UNLESS(derivative_order >= 0);
+  if (derivative_order == 0) {
+    Eigen::SparseMatrix<T> M(num_control_points(), num_control_points());
+    M.setIdentity();
+    return M;
+  } else if (derivative_order >= basis_.order()) {
+    // In this case, MakeDerivative will return a zero trajectory using a
+    // single control point.
+    return Eigen::SparseMatrix<T>(num_control_points(), 1);
+  } else {
+    // First compute the control points of the kth derivative, pᵏ, relative
+    // to the original control points, p: pᵏ = p * Mᵏ.
+    Eigen::SparseMatrix<T> M_k(num_control_points(), num_control_points());
+    M_k.setIdentity();
+    for (int j = 1; j <= derivative_order; ++j) {
+      for (int i = 0; i < num_control_points() - j; ++i) {
+        // pᵢᵏ⁺¹ = αᵢ * (pᵢ₊₁ᵏ - pᵢᵏ), and pᵢᵏ = p * Mᵢᵏ, where the i subscript
+        // denotes the ith column. so p * Mᵢᵏ⁺¹ = αᵢ * (p * Mᵢ₊₁ᵏ - p * Mᵢᵏ), or
+        // Mᵢᵏ⁺¹ = αᵢ * (Mᵢ₊₁ᵏ - Mᵢᵏ).
+        M_k.col(i) =
+            (basis_.order() - j) /
+            (basis_.knots()[i + basis_.order()] - basis_.knots()[i + j]) *
+            (M_k.col(i + 1) - M_k.col(i));
+      }
+    }
+    return M_k.leftCols(num_control_points() - derivative_order);
+  }
+}
+
+template <typename T>
+VectorX<T> BsplineTrajectory<T>::EvaluateLinearInControlPoints(
+    const T& t, int derivative_order) const {
+  using std::clamp;
+  T clamped_time = clamp(t, this->start_time(), this->end_time());
+  DRAKE_THROW_UNLESS(derivative_order >= 0);
+  DRAKE_THROW_UNLESS(this->cols() == 1);
+  if (derivative_order == 0) {
+    return basis_.EvaluateLinearInControlPoints(clamped_time);
+  } else if (derivative_order >= basis_.order()) {
+    return VectorX<T>::Zero(num_control_points());
+  } else {
+    // First compute the control points of the kth derivative, pᵏ, relative
+    // to the original control points, p: pᵏ = p * Mᵏ.
+    std::vector<T> derivative_knots(basis_.knots().begin() + derivative_order,
+                                    basis_.knots().end() - derivative_order);
+    BsplineBasis<T> lower_order_basis =
+        BsplineBasis<T>(basis_.order() - derivative_order, derivative_knots);
+    MatrixX<T> M_k =
+        MatrixX<T>::Identity(num_control_points(), num_control_points());
+    // This is similar to the code in AsLinearInControlPoints, but here we can
+    // restrict ourselves to only computing the terms for the active basis
+    // functions.
+    std::vector<int> base_indices =
+        basis_.ComputeActiveBasisFunctionIndices(clamped_time);
+    for (int j = 1; j <= derivative_order; ++j) {
+      for (int i = base_indices.front(); i <= base_indices.back() - j; ++i) {
+        // pᵢᵏ⁺¹ = αᵢ * (pᵢ₊₁ᵏ - pᵢᵏ), and pᵢᵏ = p * Mᵢᵏ, where the i subscript
+        // denotes the ith column. so p * Mᵢᵏ⁺¹ = αᵢ * (p * Mᵢ₊₁ᵏ - p * Mᵢᵏ), or
+        // Mᵢᵏ⁺¹ = αᵢ * (Mᵢ₊₁ᵏ - Mᵢᵏ).
+        M_k.col(i) =
+            (basis_.order() - j) /
+            (basis_.knots()[i + basis_.order()] - basis_.knots()[i + j]) *
+            (M_k.col(i + 1) - M_k.col(i));
+      }
+    }
+    // Now value = p * M_to_deriv * M_deriv
+    VectorX<T> M_deriv = lower_order_basis.EvaluateLinearInControlPoints(clamped_time);
+    VectorX<T> M = VectorX<T>::Zero(num_control_points());
+    for (int i :
+         lower_order_basis.ComputeActiveBasisFunctionIndices(clamped_time)) {
+      M += M_k.col(i)*M_deriv(i);
+    }
+    return M;
+  }
+}
+
 template <typename T>
 std::unique_ptr<Trajectory<T>> BsplineTrajectory<T>::DoClone() const {
   return std::make_unique<BsplineTrajectory<T>>(*this);

common/trajectories/bspline_trajectory.h

@@ -3,6 +3,8 @@
 #include <memory>
 #include <vector>
 
+#include <Eigen/Sparse>
+
 #include "drake/common/drake_bool.h"
 #include "drake/common/drake_copyable.h"
 #include "drake/common/drake_throw.h"
@@ -60,6 +62,28 @@ class BsplineTrajectory final : public trajectories::Trajectory<T> {
     return Trajectory<T>::value(t);
   }
 
+  /** Supports writing optimizations using the control points as decision
+  variables.  This method returns the matrix, `M`, defining the control points of
+  the `order` derivative in the form:
+  <pre>
+  derivative.control_points() = this.control_points() * M
+  </pre>
+  See `BezierCurve::AsLinearInControlPoints()` for more details.
+  @pre derivative_order >= 0. */
+  Eigen::SparseMatrix<T> AsLinearInControlPoints(int derivative_order = 1) const;
+
+  /** Returns the vector, M, such that
+  @verbatim
+  EvalDerivative(t, derivative_order) = control_points() * M
+  @endverbatim
+  where cols()==1 (so control_points() is a matrix). This is
+  useful for writing linear constraints on the control points.
+
+  @pre t ≥ start_time()
+  @pre t ≤ end_time() */
+  VectorX<T> EvaluateLinearInControlPoints(const T& t,
+                                           int derivative_order = 0) const;
+
   /** Returns the number of control points in this curve. */
   int num_control_points() const { return basis_.num_basis_functions(); }
 

common/trajectories/test/bspline_trajectory_test.cc

@@ -10,6 +10,7 @@
 #include "drake/common/proto/call_python.h"
 #include "drake/common/test_utilities/eigen_matrix_compare.h"
 #include "drake/common/test_utilities/expect_throws_message.h"
+#include "drake/common/text_logging.h"
 #include "drake/common/trajectories/trajectory.h"
 #include "drake/common/yaml/yaml_io.h"
 #include "drake/math/autodiff_gradient.h"
@@ -309,6 +310,39 @@ TYPED_TEST(BsplineTrajectoryTests, InsertKnotsTest) {
   }
 }
 
+TYPED_TEST(BsplineTrajectoryTests, LinearInControlPointsTest) {
+  using T = TypeParam;
+  BsplineTrajectory<T> dut = MakeCircleTrajectory<T>();
+
+  MatrixX<T> control_points(dut.rows(), dut.num_control_points());
+  for (int i = 0; i < dut.num_control_points(); ++i) {
+    control_points.col(i) = dut.control_points()[i].col(0);
+  }
+
+  // Note: we intentionally test order > basis.order().
+  for (int order = 0; order < dut.basis().order() + 1; ++order) {
+    log()->info("order = {}", order);
+    // EvaluateLinearInControlPoints()
+    for (T t = dut.start_time(); t < dut.end_time() - 0.05; t += 0.05) {
+      const VectorX<T> value = dut.EvalDerivative(t, order);
+      const VectorX<T> M = dut.EvaluateLinearInControlPoints(t, order);
+      EXPECT_TRUE(CompareMatrices(value, control_points * M, 1e-12));
+    }
+    // AsLinearInControlPoints()
+    const Eigen::SparseMatrix<T> M = dut.AsLinearInControlPoints(order);
+    auto derivative = std::unique_ptr<BsplineTrajectory<T>>(
+        dynamic_cast<BsplineTrajectory<T>*>(
+            dut.MakeDerivative(order).release()));
+    MatrixX<T> derivative_control_points(dut.rows(),
+                                         derivative->num_control_points());
+    for (int i = 0; i < derivative->num_control_points(); ++i) {
+      derivative_control_points.col(i) = derivative->control_points()[i].col(0);
+    }
+    EXPECT_TRUE(
+        CompareMatrices(derivative_control_points, control_points * M, 1e-12));
+  }
+}
+
 // Verifies that the derivatives obtained by evaluating a
 // `BsplineTrajectory<AutoDiffXd>` and extracting the gradient of the result
 // match those obtained by taking the derivative of the whole trajectory and

math/bspline_basis.cc

@@ -13,6 +13,8 @@ namespace drake {
 namespace math {
 namespace {
 
+using Eigen::VectorXd;
+
 template <typename T>
 std::vector<T> MakeKnotVector(int order, int num_basis_functions,
                               KnotVectorType type,
@@ -103,10 +105,10 @@ T_control_point BsplineBasis<T>::EvaluateCurve(
     [2] De Boor, Carl. "On calculating with B-splines." Journal of
         Approximation theory 6.1 (1972): 50-62.
   */
-  DRAKE_DEMAND(static_cast<int>(control_points.size()) ==
-               num_basis_functions());
-  DRAKE_DEMAND(parameter_value >= initial_parameter_value());
-  DRAKE_DEMAND(parameter_value <= final_parameter_value());
+  DRAKE_THROW_UNLESS(static_cast<int>(control_points.size()) ==
+                     num_basis_functions());
+  DRAKE_THROW_UNLESS(parameter_value >= initial_parameter_value());
+  DRAKE_THROW_UNLESS(parameter_value <= final_parameter_value());
 
   // Define short names to match notation in [1].
   const std::vector<T>& t = knots();
@@ -137,6 +139,49 @@ T_control_point BsplineBasis<T>::EvaluateCurve(
   return p.front();
 }
 
+template <typename T>
+VectorX<T> BsplineBasis<T>::EvaluateLinearInControlPoints(
+    const T& parameter_value) const {
+  DRAKE_THROW_UNLESS(parameter_value >= initial_parameter_value());
+  DRAKE_THROW_UNLESS(parameter_value <= final_parameter_value());
+
+  // The recipe from EvaluateCurve (and the reference [1] cited therein) is
+  // pᵢ⁰ = pᵢ, for i ∈ [ell - k + 1, ell] (zero otherwise),
+  // pᵢʲ = (1 - αᵢʲ) pᵢ₋₁ʲ⁻¹ + αᵢʲ pᵢʲ⁻¹.
+  // We compute instead, Mʲ, such that pʲ = p Mʲ, and therefore pᵢʲ = p Mᵢʲ
+  // where Mᵢʲ is the ith column of Mʲ. The corresponding recursion is
+  // M⁰[i,i] = 1 iff i ∈ [ell - k + 1, ell], so that p⁰ = p M⁰,
+  // p Mᵢʲ = (1 - αᵢʲ) p Mᵢ₋₁ʲ⁻¹ + αᵢʲ p Mᵢʲ⁻¹, or
+  // Mᵢʲ = (1 - αᵢʲ) Mᵢ₋₁ʲ⁻¹ + αᵢʲ Mᵢʲ⁻¹.
+
+  const std::vector<T>& t = knots();
+  const T& t_bar = parameter_value;
+  const int k = order();
+
+  /* Find the index, 𝑙, of the greatest knot that is less than or equal to
+  t_bar and strictly less than final_parameter_value(). */
+  const int ell = FindContainingInterval(t_bar);
+  // Following EvaluateCurve, Mʲ only includes the active control points.
+  std::vector<VectorX<T>> Mj(order(), VectorX<T>::Zero(num_basis_functions()));
+  /* For j = 0, i goes from ell down to ell - (k - 1). Define r such that
+  i = ell - r. */
+  for (int r = 0; r < k; ++r) {
+    const int i = ell - r;
+    Mj.at(r)(i) = 1.0;
+  }
+  /* For j = 1, ..., k - 1, i goes from ell down to ell - (k - j - 1). Again,
+  i = ell - r. */
+  for (int j = 1; j < k; ++j) {
+    for (int r = 0; r < k - j; ++r) {
+      const int i = ell - r;
+      // α = (t_bar - t[i]) / (t[i + k - j] - t[i]);
+      const T alpha = (t_bar - t.at(i)) / (t.at(i + k - j) - t.at(i));
+      Mj.at(r) = (1.0 - alpha) * Mj.at(r + 1) + alpha * Mj.at(r);
+    }
+  }
+  return Mj.front();
+}
+
 template <typename T>
 std::vector<int> BsplineBasis<T>::ComputeActiveBasisFunctionIndices(
     const T& parameter_value) const {

math/bspline_basis.h

@@ -7,6 +7,7 @@
 #include "drake/common/drake_bool.h"
 #include "drake/common/drake_copyable.h"
 #include "drake/common/drake_throw.h"
+#include "drake/common/eigen_types.h"
 #include "drake/common/name_value.h"
 #include "drake/math/knot_vector_type.h"
 
@@ -142,6 +143,17 @@ class BsplineBasis final {
       const std::vector<T_control_point>& control_points,
       const T& parameter_value) const;
 
+  /** Returns the vector, M, such that
+  @verbatim
+  EvaluateCurve(control_points, parameter_value) = control_points * M
+  @endverbatim
+  where T_control_points==VectorX<T> (so control_points is a matrix). This is
+  useful for writing linear constraints on the control points.
+
+  @pre parameter_value ≥ initial_parameter_value()
+  @pre parameter_value ≤ final_parameter_value() */
+  VectorX<T> EvaluateLinearInControlPoints(const T& parameter_value) const;
+
   /** Returns the value of the `i`-th basis function evaluated at
   `parameter_value`. */
   T EvaluateBasisFunctionI(int i, const T& parameter_value) const;

math/test/bspline_basis_test.cc

@@ -7,6 +7,7 @@
 
 #include "drake/common/default_scalars.h"
 #include "drake/common/extract_double.h"
+#include "drake/common/test_utilities/eigen_matrix_compare.h"
 #include "drake/common/test_utilities/expect_throws_message.h"
 #include "drake/common/yaml/yaml_io.h"
 
@@ -310,6 +311,27 @@ TYPED_TEST(BsplineBasisTests, OperatorEqualsTest) {
             BsplineBasis<T>(order + 1, other_knots));
 }
 
+TYPED_TEST(BsplineBasisTests, EvaluateLinearInControlPointsTest) {
+  using T = TypeParam;
+  const int order = 4;
+  const std::vector<T> arbitrary_knots = {-5, 0, 0.1, 0.2, 0.4, 0.9, 1.2, 4.0};
+  BsplineBasis<T> dut(order, arbitrary_knots);
+
+  const int num_control_points = arbitrary_knots.size() - order;
+  std::vector<VectorX<T>> control_points(num_control_points);
+  MatrixX<T> control_points_matrix(2, num_control_points);
+  for (int i = 0; i < num_control_points; ++i) {
+    control_points[i] = Vector2<T>{2.4 * i, 3.1 * i};
+    control_points_matrix.col(i) = control_points[i];
+  }
+  for (T s = dut.initial_parameter_value();
+       s < dut.final_parameter_value() - 0.05; s += 0.05) {
+    const VectorX<T> value = dut.EvaluateCurve(control_points, s);
+    const VectorX<T> M = dut.EvaluateLinearInControlPoints(s);
+    EXPECT_TRUE(CompareMatrices(value, control_points_matrix * M, 1e-14));
+  }
+}
+
 const char* const good = R"""(
 order: 3
 knots: [0., 1., 1.5, 1.6, 2., 2.5, 3.]