diff --git a/tesseract_visualization/include/tesseract_visualization/trajectory_interpolator.h b/tesseract_visualization/include/tesseract_visualization/trajectory_interpolator.h index 67b113314f3..44fcb9f785a 100644 --- a/tesseract_visualization/include/tesseract_visualization/trajectory_interpolator.h +++ b/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 diff --git a/tesseract_visualization/include/tesseract_visualization/trajectory_player.h b/tesseract_visualization/include/tesseract_visualization/trajectory_player.h index e3c117c6780..92c1ee52dd0 100644 --- a/tesseract_visualization/include/tesseract_visualization/trajectory_player.h +++ b/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 }; diff --git a/tesseract_visualization/src/trajectory_interpolator.cpp b/tesseract_visualization/src/trajectory_interpolator.cpp index 921649c9f12..ecb24f41bc8 100644 --- a/tesseract_visualization/src/trajectory_interpolator.cpp +++ b/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(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(out.joint_names.size())); for (long i = 0; i < static_cast(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; } diff --git a/tesseract_visualization/src/trajectory_player.cpp b/tesseract_visualization/src/trajectory_player.cpp index 79442e206fb..caf9f0c4330 100644 --- a/tesseract_visualization/src/trajectory_player.cpp +++ b/tesseract_visualization/src/trajectory_player.cpp @@ -39,7 +39,8 @@ void TrajectoryPlayer::setTrajectory(const tesseract_common::JointTrajectory& tr trajectory_ = std::make_unique(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(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(); diff --git a/tesseract_visualization/test/trajectory_player_unit.cpp b/tesseract_visualization/test/trajectory_player_unit.cpp index a162f27246f..5114eb9c755 100644 --- a/tesseract_visualization/test/trajectory_player_unit.cpp +++ b/tesseract_visualization/test/trajectory_player_unit.cpp @@ -32,37 +32,28 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -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 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(i); - trajectory.push_back(JointState(joint_names, p)); - trajectory.back().time = static_cast(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(i), 1e-5); EXPECT_NEAR(s.position(0), static_cast(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(i)); EXPECT_NEAR(s.time, static_cast(i), 1e-5); @@ -70,41 +61,123 @@ TEST(TesseracTrajectoryPlayerUnit, TrajectoryTest) // NOLINT } { - 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 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(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 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(i); + trajectory.push_back(JointState(joint_names, p)); + trajectory.back().time = static_cast(i); + } + + CheckTrajectory(trajectory, first, last); +} + +TEST(TesseracTrajectoryPlayerUnit, TrajectoryNonzeroStartTest) // NOLINT +{ + using namespace tesseract_visualization; + using namespace tesseract_common; + + std::vector 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(i); + trajectory.push_back(JointState(joint_names, p)); + trajectory.back().time = static_cast(i); + } + + CheckTrajectory(trajectory, first, last); +} + TEST(TesseracTrajectoryInterpolatorUnit, TrajectoryInterpolatorTest) // NOLINT { using namespace tesseract_visualization; @@ -112,6 +185,7 @@ TEST(TesseracTrajectoryInterpolatorUnit, TrajectoryInterpolatorTest) // NOLINT std::vector 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) @@ -119,7 +193,7 @@ TEST(TesseracTrajectoryInterpolatorUnit, TrajectoryInterpolatorTest) // NOLINT Eigen::VectorXd p = Eigen::VectorXd::Zero(6); p(0) = static_cast(i); trajectory.push_back(JointState(joint_names, p)); - trajectory.back().time = static_cast(i); + trajectory.back().time = static_cast(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(i) * 0.5); - EXPECT_NEAR(s.time, static_cast(i) * 0.5, 1e-5); + JointState s = interpolator.getState(static_cast(i) * 0.5 * time_scale); + EXPECT_NEAR(s.time, static_cast(i) * 0.5 * time_scale, 1e-5); EXPECT_NEAR(s.position(0), static_cast(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(i), 1e-5); + EXPECT_NEAR(duration, static_cast(i) * time_scale, 1e-5); } }