Trajectory player fixes

tesseract_visualization/include/tesseract_visualization/trajectory_interpolator.h

@@ -67,7 +67,7 @@ class TrajectoryInterpolator
 
   static tesseract_common::JointState interpolate(const tesseract_common::JointState& start,
                                                   const tesseract_common::JointState& end,
-                                                  double t);
+                                                  double f);
 };
 }  // namespace tesseract_visualization
 #endif  // TESSERACT_VISUALIZATION_TRAJECTORY_INTERPOLATOR_H

tesseract_visualization/include/tesseract_visualization/trajectory_player.h

@@ -87,10 +87,16 @@ class TrajectoryPlayer
   double currentDuration() const;
 
   /**
-   * @brief Get the trajectory duration
+   * @brief Get the trajectory duration at the begin state
    * @return The trajectory duration
    */
-  double trajectoryDuration() const;
+  double trajectoryDurationBegin() const;
+
+  /**
+   * @brief Get the trajectory duration at the end state
+   * @return The trajectory duration
+   */
+  double trajectoryDurationEnd() const;
 
   /**
    * @brief Check if the player has the reached the end of the trajectory
@@ -118,7 +124,8 @@ class TrajectoryPlayer
 
 private:
   TrajectoryInterpolator::UPtr trajectory_{ nullptr };
-  double trajectory_duration_{ 0 };
+  double trajectory_duration_start_{ 0 };
+  double trajectory_duration_end_{ 0 };
   double current_duration_{ 0 };
   double scale_{ 1 };
   bool loop_{ false };

tesseract_visualization/src/trajectory_interpolator.cpp

@@ -38,7 +38,7 @@ TrajectoryInterpolator::TrajectoryInterpolator(tesseract_common::JointTrajectory
   double total_time = 0;
   bool overwrite_dt = false;
   // Check if time is populated
-  if (!trajectory_.empty() && (trajectory_.back().time - trajectory_.front().time) < 1e-3)
+  if ((trajectory_.size() > 1) && (trajectory_.back().time - trajectory_.front().time) < 1e-3)
     overwrite_dt = true;
 
   bool initial_state = true;
@@ -77,7 +77,7 @@ void TrajectoryInterpolator::findStateIndices(const double& duration, long& befo
     return;
   }
 
-  // Find indicies
+  // Find indices
   std::size_t index = 0;
   std::size_t num_points = trajectory_.size();
   double running_duration = 0.0;
@@ -144,19 +144,19 @@ long TrajectoryInterpolator::getStateCount() const { return static_cast<long>(tr
 
 tesseract_common::JointState TrajectoryInterpolator::interpolate(const tesseract_common::JointState& start,
                                                                  const tesseract_common::JointState& end,
-                                                                 double t)
+                                                                 double f)
 {
   assert(!start.joint_names.empty());
   assert(!end.joint_names.empty());
   assert(start.position.rows() != 0);
   assert(end.position.rows() != 0);
   tesseract_common::JointState out;
-  out.time = start.time + t;
+  out.time = start.time + (end.time - start.time) * f;
   out.joint_names = start.joint_names;
   out.position.resize(static_cast<long>(out.joint_names.size()));
 
   for (long i = 0; i < static_cast<long>(out.joint_names.size()); ++i)
-    out.position[i] = start.position[i] + (end.position[i] - start.position[i]) * t;
+    out.position[i] = start.position[i] + (end.position[i] - start.position[i]) * f;
 
   return out;
 }

tesseract_visualization/src/trajectory_player.cpp

@@ -39,7 +39,8 @@ void TrajectoryPlayer::setTrajectory(const tesseract_common::JointTrajectory& tr
   trajectory_ = std::make_unique<TrajectoryInterpolator>(trajectory);
 
   // Get the duration
-  trajectory_duration_ = trajectory_->getStateDuration(trajectory_->getStateCount() - 1);
+  trajectory_duration_start_ = trajectory_->getStateDuration(0);
+  trajectory_duration_end_ = trajectory_->getStateDuration(trajectory_->getStateCount() - 1);
 
   // Reset state
   reset();
@@ -74,13 +75,13 @@ tesseract_common::JointState TrajectoryPlayer::setCurrentDuration(double duratio
     throw std::runtime_error("Trajectory is empty!");
 
   finished_ = false;
-  if (duration > trajectory_duration_)
+  if (duration > trajectory_duration_end_)
   {
-    current_duration_ = trajectory_duration_;
+    current_duration_ = trajectory_duration_end_;
     finished_ = true;
   }
-  else if (duration < 0)
-    current_duration_ = 0;
+  else if (duration < trajectory_duration_start_)
+    current_duration_ = trajectory_duration_start_;
   else
     current_duration_ = duration;
 
@@ -100,9 +101,9 @@ tesseract_common::JointState TrajectoryPlayer::getNext()
   auto current_time = std::chrono::high_resolution_clock::now();
   current_duration_ = (scale_ * std::chrono::duration<double>(current_time - start_time_).count());
 
-  if (current_duration_ > trajectory_duration_)
+  if (current_duration_ > trajectory_duration_end_)
   {
-    current_duration_ = trajectory_duration_;
+    current_duration_ = trajectory_duration_end_;
 
     // Compute the interpolated state
     auto mi = trajectory_->getState(current_duration_);
@@ -126,7 +127,9 @@ tesseract_common::JointState TrajectoryPlayer::getByIndex(long index) const
 
 double TrajectoryPlayer::currentDuration() const { return current_duration_; }
 
-double TrajectoryPlayer::trajectoryDuration() const { return trajectory_duration_; }
+double TrajectoryPlayer::trajectoryDurationBegin() const { return trajectory_duration_start_; }
+
+double TrajectoryPlayer::trajectoryDurationEnd() const { return trajectory_duration_end_; }
 
 bool TrajectoryPlayer::isFinished() const { return finished_; }
 
@@ -137,7 +140,7 @@ bool TrajectoryPlayer::isLoopEnabled() const { return loop_; }
 void TrajectoryPlayer::reset()
 {
   // Reset state associated with trajectory playback
-  current_duration_ = 0.0;
+  current_duration_ = trajectory_duration_start_;
 
   // Get the chrono time
   start_time_ = std::chrono::high_resolution_clock::now();

tesseract_visualization/test/trajectory_player_unit.cpp

@@ -32,94 +32,168 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_visualization/trajectory_player.h>
 #include <tesseract_visualization/trajectory_interpolator.h>
 
-TEST(TesseracTrajectoryPlayerUnit, TrajectoryTest)  // NOLINT
+void CheckTrajectory(const tesseract_common::JointTrajectory& trajectory, int first, int last)
 {
   using namespace tesseract_visualization;
   using namespace tesseract_common;
 
-  std::vector<std::string> joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" };
-  JointTrajectory trajectory;
-
-  // Define trajectory
-  for (long i = 0; i < 10; ++i)
-  {
-    Eigen::VectorXd p = Eigen::VectorXd::Zero(6);
-    p(0) = static_cast<double>(i);
-    trajectory.push_back(JointState(joint_names, p));
-    trajectory.back().time = static_cast<double>(i);
-  }
-
   TrajectoryPlayer player;
   player.setTrajectory(trajectory);
 
-  EXPECT_NEAR(player.trajectoryDuration(), 9, 1e-5);
-  EXPECT_NEAR(player.currentDuration(), 0, 1e-5);
+  EXPECT_NEAR(player.trajectoryDurationBegin(), first, 1e-5);
+  EXPECT_NEAR(player.trajectoryDurationEnd(), last, 1e-5);
+  EXPECT_NEAR(player.currentDuration(), first, 1e-5);
 
-  for (long i = 0; i < 10; ++i)
+  // Advance by index
+  for (long i = first; i <= last; ++i)
   {
-    JointState s = player.setCurrentDurationByIndex(i);
+    JointState s = player.setCurrentDurationByIndex(i - first);
     EXPECT_NEAR(s.time, static_cast<double>(i), 1e-5);
     EXPECT_NEAR(s.position(0), static_cast<double>(i), 1e-5);
   }
 
-  for (long i = 0; i < 10; ++i)
+  // Advance by time
+  for (long i = first; i <= last; ++i)
   {
     JointState s = player.setCurrentDuration(static_cast<double>(i));
     EXPECT_NEAR(s.time, static_cast<double>(i), 1e-5);
     EXPECT_NEAR(s.position(0), static_cast<double>(i), 1e-5);
   }
 
   {
-    JointState s = player.setCurrentDurationByIndex(10);
-    EXPECT_NEAR(s.time, 9, 1e-5);
-    EXPECT_NEAR(s.position(0), 9, 1e-5);
+    JointState s = player.setCurrentDurationByIndex((last - first) + 1);
+    EXPECT_NEAR(s.time, last, 1e-5);
+    EXPECT_NEAR(s.position(0), last, 1e-5);
   }
 
   {
-    JointState s = player.setCurrentDuration(10);
-    EXPECT_NEAR(s.time, 9, 1e-5);
-    EXPECT_NEAR(s.position(0), 9, 1e-5);
+    JointState s = player.setCurrentDuration(last + 1);
+    EXPECT_NEAR(s.time, last, 1e-5);
+    EXPECT_NEAR(s.position(0), last, 1e-5);
     EXPECT_TRUE(player.isFinished());
   }
 
   {
-    JointState s = player.setCurrentDuration(11);
-    EXPECT_NEAR(s.time, 9, 1e-5);
-    EXPECT_NEAR(s.position(0), 9, 1e-5);
+    JointState s = player.setCurrentDuration(last + 2);
+    EXPECT_NEAR(s.time, last, 1e-5);
+    EXPECT_NEAR(s.position(0), last, 1e-5);
     EXPECT_TRUE(player.isFinished());
-    player.setCurrentDuration(0);
+    player.setCurrentDuration(first);
     EXPECT_FALSE(player.isFinished());
   }
 
   {
     JointState s = player.setCurrentDurationByIndex(-1);
-    EXPECT_NEAR(s.time, 0, 1e-5);
-    EXPECT_NEAR(s.position(0), 0, 1e-5);
+    EXPECT_NEAR(s.time, first, 1e-5);
+    EXPECT_NEAR(s.position(0), first, 1e-5);
   }
 
   {
-    JointState s = player.setCurrentDuration(-1);
-    EXPECT_NEAR(s.time, 0, 1e-5);
-    EXPECT_NEAR(s.position(0), 0, 1e-5);
+    JointState s = player.setCurrentDuration(first - 1);
+    EXPECT_NEAR(s.time, first, 1e-5);
+    EXPECT_NEAR(s.position(0), first, 1e-5);
     EXPECT_FALSE(player.isFinished());
   }
 }
 
+TEST(TesseracTrajectoryPlayerUnit, TrajectoryUntimedTest)  // NOLINT
+{
+  using namespace tesseract_visualization;
+  using namespace tesseract_common;
+
+  std::vector<std::string> joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" };
+  JointTrajectory trajectory;
+  int first = 0;
+  int last = 9;
+  double auto_dt = 0.1;  // Time step assigned by interpolator to untimed trajectory
+
+  // Define trajectory without timing
+  for (long i = first; i <= last; ++i)
+  {
+    Eigen::VectorXd p = Eigen::VectorXd::Zero(6);
+    p(0) = static_cast<double>(i);
+    trajectory.push_back(JointState(joint_names, p));
+  }
+
+  TrajectoryPlayer player;
+  player.setTrajectory(trajectory);
+
+  EXPECT_NEAR(player.trajectoryDurationBegin(), first, 1e-5);
+  EXPECT_NEAR(player.trajectoryDurationEnd(), last * auto_dt, 1e-5);
+  EXPECT_NEAR(player.currentDuration(), first, 1e-5);
+
+  {
+    JointState s = player.setCurrentDuration(0.5);
+    EXPECT_NEAR(s.position(0), 0.5 / auto_dt, 1e-5);
+  }
+
+  {
+    JointState s = player.setCurrentDurationByIndex(6);
+    EXPECT_NEAR(s.time, 6 * auto_dt, 1e-5);
+  }
+}
+
+TEST(TesseracTrajectoryPlayerUnit, TrajectoryTimedTest)  // NOLINT
+{
+  using namespace tesseract_visualization;
+  using namespace tesseract_common;
+
+  std::vector<std::string> joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" };
+  JointTrajectory trajectory;
+  int first = 0;
+  int last = 9;
+
+  // Define trajectory with timing
+  trajectory.clear();
+  for (long i = first; i <= last; ++i)
+  {
+    Eigen::VectorXd p = Eigen::VectorXd::Zero(6);
+    p(0) = static_cast<double>(i);
+    trajectory.push_back(JointState(joint_names, p));
+    trajectory.back().time = static_cast<double>(i);
+  }
+
+  CheckTrajectory(trajectory, first, last);
+}
+
+TEST(TesseracTrajectoryPlayerUnit, TrajectoryNonzeroStartTest)  // NOLINT
+{
+  using namespace tesseract_visualization;
+  using namespace tesseract_common;
+
+  std::vector<std::string> joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" };
+  JointTrajectory trajectory;
+  int first = 5;
+  int last = 14;
+
+  // Define trajectory with timing and a non-zero begin
+  for (long i = first; i <= last; ++i)
+  {
+    Eigen::VectorXd p = Eigen::VectorXd::Zero(6);
+    p(0) = static_cast<double>(i);
+    trajectory.push_back(JointState(joint_names, p));
+    trajectory.back().time = static_cast<double>(i);
+  }
+
+  CheckTrajectory(trajectory, first, last);
+}
+
 TEST(TesseracTrajectoryInterpolatorUnit, TrajectoryInterpolatorTest)  // NOLINT
 {
   using namespace tesseract_visualization;
   using namespace tesseract_common;
 
   std::vector<std::string> joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" };
   JointTrajectory trajectory;
+  double time_scale = 0.5;
 
   // Define trajectory
   for (long i = 0; i < 10; ++i)
   {
     Eigen::VectorXd p = Eigen::VectorXd::Zero(6);
     p(0) = static_cast<double>(i);
     trajectory.push_back(JointState(joint_names, p));
-    trajectory.back().time = static_cast<double>(i);
+    trajectory.back().time = static_cast<double>(i) * time_scale;
   }
 
   TrajectoryInterpolator interpolator(trajectory);
@@ -128,21 +202,21 @@ TEST(TesseracTrajectoryInterpolatorUnit, TrajectoryInterpolatorTest)  // NOLINT
 
   for (long i = 0; i < 19; ++i)
   {
-    JointState s = interpolator.getState(static_cast<double>(i) * 0.5);
-    EXPECT_NEAR(s.time, static_cast<double>(i) * 0.5, 1e-5);
+    JointState s = interpolator.getState(static_cast<double>(i) * 0.5 * time_scale);
+    EXPECT_NEAR(s.time, static_cast<double>(i) * 0.5 * time_scale, 1e-5);
     EXPECT_NEAR(s.position(0), static_cast<double>(i) * 0.5, 1e-5);
   }
 
   // Test above max duration
   JointState s = interpolator.getState(10);
-  EXPECT_NEAR(s.time, 9, 1e-5);
+  EXPECT_NEAR(s.time, 9 * time_scale, 1e-5);
   EXPECT_NEAR(s.position(0), 9, 1e-5);
 
   // Test get instruction duration
   for (long i = 0; i < 10; ++i)
   {
     double duration = interpolator.getStateDuration(i);
-    EXPECT_NEAR(duration, static_cast<double>(i), 1e-5);
+    EXPECT_NEAR(duration, static_cast<double>(i) * time_scale, 1e-5);
   }
 }