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/src/trajectory_interpolator.cpp b/tesseract_visualization/src/trajectory_interpolator.cpp index f08142c20e5..ecb24f41bc8 100644 --- a/tesseract_visualization/src/trajectory_interpolator.cpp +++ b/tesseract_visualization/src/trajectory_interpolator.cpp @@ -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/test/trajectory_player_unit.cpp b/tesseract_visualization/test/trajectory_player_unit.cpp index 5f6ef32d5cc..e3abba380ac 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.trajectoryDurationEnd(), 9, 1e-5); - EXPECT_NEAR(player.currentDuration(), 0, 1e-5); + EXPECT_NEAR(player.trajectoryDurationStart(), 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.trajectoryDurationStart(), 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); } }