trajectories: Adds PiecewisePolynomial::AppendFirstOrderSegment

for incremental construction of first-order polynomial trajectories.

bindings/pydrake/test/trajectories_test.py

@@ -63,6 +63,7 @@ def test_first_order_hold(self):
 
         deriv = pp.MakeDerivative(derivative_order=1)
         np.testing.assert_equal(np.array([[2.], [2.]]), deriv.value(.5))
+        pp.AppendFirstOrderSegment(time=3., sample=[-0.4, .57])
 
     def test_hermite(self):
         t = [0., 1., 2.]

bindings/pydrake/trajectories_py.cc

@@ -147,6 +147,10 @@ PYBIND11_MODULE(trajectories, m) {
           &PiecewisePolynomial<T>::AppendCubicHermiteSegment, py::arg("time"),
           py::arg("sample"), py::arg("sample_dot"),
           doc.PiecewisePolynomial.AppendCubicHermiteSegment.doc)
+      .def("AppendFirstOrderSegment",
+          &PiecewisePolynomial<T>::AppendFirstOrderSegment, py::arg("time"),
+          py::arg("sample"),
+          doc.PiecewisePolynomial.AppendFirstOrderSegment.doc)
       .def("RemoveFinalSegment", &PiecewisePolynomial<T>::RemoveFinalSegment,
           doc.PiecewisePolynomial.RemoveFinalSegment.doc)
       .def("ReverseTime", &PiecewisePolynomial<T>::ReverseTime,

common/trajectories/piecewise_polynomial.cc

@@ -337,6 +337,31 @@ void PiecewisePolynomial<T>::AppendCubicHermiteSegment(
   this->get_mutable_breaks().push_back(time);
 }
 
+template <typename T>
+void PiecewisePolynomial<T>::AppendFirstOrderSegment(
+    const T& time, const Eigen::Ref<const MatrixX<T>>& sample) {
+  DRAKE_DEMAND(!empty());
+  DRAKE_DEMAND(time > this->end_time());
+  DRAKE_DEMAND(sample.rows() == rows());
+  DRAKE_DEMAND(sample.cols() == cols());
+
+  const int segment_index = polynomials_.size() - 1;
+  const T dt = time - this->end_time();
+
+  PolynomialMatrix matrix(rows(), cols());
+
+  for (int row = 0; row < rows(); ++row) {
+    for (int col = 0; col < cols(); ++col) {
+      const T start = EvaluateSegmentAbsoluteTime(
+          segment_index, this->end_time(), row, col);
+      matrix(row, col) = Polynomial<T>(
+          Eigen::Matrix<T, 2, 1>(start, (sample(row, col) - start) / dt));
+    }
+  }
+  polynomials_.push_back(matrix);
+  this->get_mutable_breaks().push_back(time);
+}
+
 template <typename T>
 void PiecewisePolynomial<T>::RemoveFinalSegment() {
   DRAKE_DEMAND(!empty());

common/trajectories/piecewise_polynomial.h

@@ -702,6 +702,13 @@ class PiecewisePolynomial final : public PiecewiseTrajectory<T> {
       const T& time, const Eigen::Ref<const MatrixX<T>>& sample,
       const Eigen::Ref<const MatrixX<T>>& sample_dot);
 
+  /**
+   * Given a new sample, this method adds one segment to the end of `this` using
+   * a first-order hold, where the start sample is taken as the value at the
+   * final break of `this`. */
+  void AppendFirstOrderSegment(
+      const T& time, const Eigen::Ref<const MatrixX<T>>& sample);
+
   /** Removes the final segment from the trajectory, reducing the number of
    * segments by 1.
    * @pre `this` is not empty()

common/trajectories/test/piecewise_polynomial_generation_test.cc

@@ -331,6 +331,15 @@ GTEST_TEST(SplineTests, RandomizedLinearSplineTest) {
     EXPECT_TRUE(CheckContinuity(spline, 1e-12, 0));
     EXPECT_TRUE(CheckValues(spline, {Y}, 1e-12));
     EXPECT_TRUE(CheckInterpolatedValuesAtBreakTime(spline, T, Y, 1e-12));
+
+    // Now test that we could have constructed the same trajectory
+    // incrementally.
+    PiecewisePolynomial<double> incremental =
+        PiecewisePolynomial<double>::FirstOrderHold({T[0], T[1]}, {Y[0], Y[1]});
+    for (int i = 2; i < N; i++) {
+      incremental.AppendFirstOrderSegment(T[i], Y[i]);
+    }
+    EXPECT_TRUE(spline.isApprox(incremental, 1e-10));
   }
 }