Merge pull request #2943 from pleroy/Prepend

Support for prepending a continuous trajectory to another one
mockingbirdnest · Apr 5, 2021 · 12d2e88 · 12d2e88
2 parents d6b57a5 + 1021df0
commit 12d2e88
Show file tree

Hide file tree

Showing 3 changed files with 139 additions and 2 deletions.
diff --git a/physics/continuous_trajectory.hpp b/physics/continuous_trajectory.hpp
@@ -73,10 +73,21 @@ class ContinuousTrajectory : public Trajectory<Frame> {
                 DegreesOfFreedom<Frame> const& degrees_of_freedom)
       EXCLUDES(lock_);
 
+  // Prepends the given |trajectory| to this one.  Ideally the last point of
+  // |trajectory| should match the first point of this object.
+  // Note the rvalue reference: |ContinuousTrajectory| is not moveable and not
+  // copyable, but the |InstantPolynomialPairs| are moveable and we really want
+  // to move them.  We could pass by non-const lvalue reference, but we would
+  // rather make it clear at the calling site that the object is consumed, so
+  // we require the use of std::move.
+  void Prepend(ContinuousTrajectory&& trajectory);
+
   // Implementation of the interface |Trajectory|.
 
-  // |t_max| may be less than the last time passed to Append.  For an empty
-  // trajectory, an infinity with the proper sign is returned.
+  // |t_max| may be less than the last time passed to Append because the
+  // trajectory cannot be evaluated for the last points, for which no polynomial
+  // was constructed.  For an empty trajectory, an infinity with the proper
+  // sign is returned.
   Instant t_min() const override EXCLUDES(lock_);
   Instant t_max() const override EXCLUDES(lock_);
 

diff --git a/physics/continuous_trajectory_body.hpp b/physics/continuous_trajectory_body.hpp
@@ -131,6 +131,46 @@ Status ContinuousTrajectory<Frame>::Append(
   return status;
 }
 
+template<typename Frame>
+void ContinuousTrajectory<Frame>::Prepend(ContinuousTrajectory&& prefix) {
+  absl::MutexLock l1(&lock_);
+  absl::MutexLock l2(&prefix.lock_);
+
+  CHECK_EQ(step_, prefix.step_);
+  CHECK_EQ(tolerance_, prefix.tolerance_);
+
+  if (prefix.polynomials_.empty()) {
+    // Nothing to do.
+  } else if (polynomials_.empty()) {
+    // All the data comes from |prefix|.  This must set all the fields of
+    // this object that are not set at construction.
+    adjusted_tolerance_ = prefix.adjusted_tolerance_;
+    is_unstable_ = prefix.is_unstable_;
+    degree_ = prefix.degree_;
+    degree_age_ = prefix.degree_age_;
+    polynomials_ = std::move(prefix.polynomials_);
+    last_accessed_polynomial_ = prefix.last_accessed_polynomial_;
+    first_time_ = prefix.first_time_;
+    last_points_ = prefix.last_points_;
+  } else {
+    // The polynomials must be aligned, because the time computations only use
+    // basic arithmetic and are platform-independent.  The space computations,
+    // on the other may depend on characteristics of the hardware and/or math
+    // library, so we cannot check that the trajectories are "continuous" at the
+    // junction.
+    CHECK_EQ(*first_time_, prefix.polynomials_.back().t_max);
+    // This operation is in O(prefix.size()).
+    std::move(polynomials_.begin(),
+              polynomials_.end(),
+              std::back_inserter(prefix.polynomials_));
+    polynomials_.swap(prefix.polynomials_);
+    first_time_ = prefix.first_time_;
+    // Note that any |last_points_| in |prefix| are irrelevant because they
+    // correspond to a time interval covered by the first polynomial of this
+    // object.
+  }
+}
+
 template<typename Frame>
 Instant ContinuousTrajectory<Frame>::t_min() const {
   absl::ReaderMutexLock l(&lock_);

diff --git a/physics/continuous_trajectory_test.cpp b/physics/continuous_trajectory_test.cpp
@@ -586,6 +586,92 @@ TEST_F(ContinuousTrajectoryTest, Continuity) {
   EXPECT_THAT(p1, AlmostEquals(p3, 0, 2));
 }
 
+TEST_F(ContinuousTrajectoryTest, Prepend) {
+  int const number_of_steps1 = 20;
+  int const number_of_steps2 = 15;
+  int const number_of_substeps = 50;
+  Time const step = 0.01 * Second;
+  Length const tolerance = 0.1 * Metre;
+
+  // Construct two trajectories with different functions.
+
+  Instant const t1 = t0_;
+  auto position_function1 =
+      [this, t1](Instant const t) {
+        return World::origin +
+            Displacement<World>({(t - t1) * 3 * Metre / Second,
+                                 (t - t1) * 5 * Metre / Second,
+                                 (t - t1) * (-2) * Metre / Second});
+      };
+  auto velocity_function1 =
+      [](Instant const t) {
+        return Velocity<World>({3 * Metre / Second,
+                                5 * Metre / Second,
+                                -2 * Metre / Second});
+      };
+  auto trajectory1 =
+      std::make_unique<ContinuousTrajectory<World>>(step, tolerance);
+  FillTrajectory(number_of_steps1,
+                 step,
+                 position_function1,
+                 velocity_function1,
+                 t1,
+                 *trajectory1);
+  EXPECT_EQ(t1 + step, trajectory1->t_min());
+  EXPECT_EQ(t1 + (((number_of_steps1 - 1) / 8) * 8 + 1) * step,
+            trajectory1->t_max());
+
+  Instant const t2 = trajectory1->t_max();
+  auto position_function2 =
+      [this, &position_function1, t2](Instant const t) {
+        return position_function1(t2) +
+               Displacement<World>({(t - t2) * 6 * Metre / Second,
+                                    (t - t2) * 1.5 * Metre / Second,
+                                    (t - t2) * 7 * Metre / Second});
+      };
+  auto velocity_function2 =
+      [](Instant const t) {
+        return Velocity<World>({6 * Metre / Second,
+                                1.5 * Metre / Second,
+                                7 * Metre / Second});
+      };
+  auto trajectory2 =
+      std::make_unique<ContinuousTrajectory<World>>(step, tolerance);
+  FillTrajectory(number_of_steps2 + 1,
+                 step,
+                 position_function2,
+                 velocity_function2,
+                 t2 - step,  // First point at t2.
+                 *trajectory2);
+  EXPECT_EQ(t2, trajectory2->t_min());
+  EXPECT_EQ(t2 + (number_of_steps2 / 8) * 8 * step,
+            trajectory2->t_max());
+
+  // Prepend one trajectory to the other.
+  trajectory2->Prepend(std::move(*trajectory1));
+
+  // Verify the resulting trajectory.
+  EXPECT_EQ(t1 + step, trajectory2->t_min());
+  EXPECT_EQ(t2 + (number_of_steps2 / 8) * 8 * step,
+            trajectory2->t_max());
+  for (Instant time = trajectory2->t_min();
+       time <= t2;
+       time += step / number_of_substeps) {
+    EXPECT_THAT(trajectory2->EvaluatePosition(time),
+                AlmostEquals(position_function1(time), 0, 10)) << time;
+    EXPECT_THAT(trajectory2->EvaluateVelocity(time),
+                AlmostEquals(velocity_function1(time), 0, 4)) << time;
+  }
+  for (Instant time = t2 + step / number_of_substeps;
+       time <= trajectory2->t_max();
+       time += step / number_of_substeps) {
+    EXPECT_THAT(trajectory2->EvaluatePosition(time),
+                AlmostEquals(position_function2(time), 0, 2816)) << time;
+    EXPECT_THAT(trajectory2->EvaluateVelocity(time),
+                AlmostEquals(velocity_function2(time), 0, 34)) << time;
+  }
+}
+
 TEST_F(ContinuousTrajectoryTest, Serialization) {
   int const number_of_steps = 20;
   int const number_of_substeps = 50;