Skip to content

Commit

Permalink
Trajectory player fixes (#989)
Browse files Browse the repository at this point in the history
  • Loading branch information
@rjoomen
rjoomen authored Feb 21, 2024
1 parent 26f3e47 commit 7a971fb
Show file tree
Hide file tree
Showing 5 changed files with 140 additions and 56 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
13 changes: 10 additions & 3 deletions tesseract_visualization/include/tesseract_visualization/trajectory_player.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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 };
Expand Down
10 changes: 5 additions & 5 deletions tesseract_visualization/src/trajectory_interpolator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down 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
21 changes: 12 additions & 9 deletions tesseract_visualization/src/trajectory_player.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -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;

Expand All @@ -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_);
Expand All @@ -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_; }

Expand All @@ -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();
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.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);
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 7a971fb

Please sign in to comment.