Skip to content

Commit

Permalink
- Added more extensive unit tests for the trajectory player
Browse files Browse the repository at this point in the history 
- Fixed a bug in the trajectory interpolator
- Renamed parameter t to f in the interpolator, as this is a fraction (blend), not time, to prevent confusion
  • Loading branch information
@rjoomen
rjoomen committed Feb 21, 2024
1 parent 187c905 commit ff2da54
Show file tree
Hide file tree
Showing 3 changed files with 117 additions and 43 deletions.
2 changes: 1 addition & 1 deletion tesseract_visualization/include/tesseract_visualization/trajectory_interpolator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
8 changes: 4 additions & 4 deletions tesseract_visualization/src/trajectory_interpolator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}
Expand Down
150 changes: 112 additions & 38 deletions tesseract_visualization/test/trajectory_player_unit.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.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<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.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<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);
Expand All @@ -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);
}
}

Expand Down

0 comments on commit ff2da54

Please sign in to comment.