Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix: TOTG can return vels/accels greater than the limits #2084

Merged
merged 2 commits into from
Apr 6, 2023
Merged

Fix: TOTG can return vels/accels greater than the limits #2084

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
a1e90ac
Add test and debug issue where TOTG returns accels > limit
AndyZe Apr 5, 2023
1929dd8
Merge branch 'main' into andyz/totg_jump
AndyZe Apr 6, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 8 additions & 0 deletions ...tory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,15 @@ class Path
Eigen::VectorXd getConfig(double s) const;
Eigen::VectorXd getTangent(double s) const;
Eigen::VectorXd getCurvature(double s) const;

/** @brief Get the next switching point.
* @param[in] s Arc length traveled so far
* @param[out] discontinuity True if this switching point is a discontinuity
* @return arc length to the switching point
**/
double getNextSwitchingPoint(double s, bool& discontinuity) const;

/// @brief Return a list of all switching points as a pair (arc length to switching point, discontinuity)
std::list<std::pair<double, bool>> getSwitchingPoints() const;

private:
Expand Down
2 changes: 0 additions & 2 deletions moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -831,7 +831,6 @@ Eigen::VectorXd Trajectory::getVelocity(double time) const
const double acceleration =
2.0 * (it->path_pos_ - previous->path_pos_ - time_step * previous->path_vel_) / (time_step * time_step);

time_step = time - previous->time_;
const double path_pos =
previous->path_pos_ + time_step * previous->path_vel_ + 0.5 * time_step * time_step * acceleration;
const double path_vel = previous->path_vel_ + time_step * acceleration;
Expand All @@ -849,7 +848,6 @@ Eigen::VectorXd Trajectory::getAcceleration(double time) const
const double acceleration =
2.0 * (it->path_pos_ - previous->path_pos_ - time_step * previous->path_vel_) / (time_step * time_step);

time_step = time - previous->time_;
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Notice that a different time_step is used previously at L848. The time_step at L848 is exactly at one of the waypoints that TOTG used in calculation so vel/accel limits aren't exceeded.

The time_step at L852 is not exactly on one of the waypoints that TOTG used in calculation and that's why it exceeds the vel/accel limits.

There will be a very small loss in accuracy by removing this but it should be fine if TOTG is run with a small delta_t. Since the default delta_t is 0.001sec, that's certainly true.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks for the explanation!

const double path_pos =
previous->path_pos_ + time_step * previous->path_vel_ + 0.5 * time_step * time_step * acceleration;
const double path_vel = previous->path_vel_ + time_step * acceleration;
Expand Down
62 changes: 62 additions & 0 deletions moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -97,6 +97,11 @@ TEST(time_optimal_trajectory_generation, test1)
EXPECT_DOUBLE_EQ(985.000244140625, trajectory.getPosition(trajectory.getDuration())[1]);
EXPECT_DOUBLE_EQ(2126.0, trajectory.getPosition(trajectory.getDuration())[2]);
EXPECT_DOUBLE_EQ(0.0, trajectory.getPosition(trajectory.getDuration())[3]);

// Start at rest and end at rest
const double traj_duration = trajectory.getDuration();
EXPECT_NEAR(0.0, trajectory.getVelocity(0.0)[0], 0.1);
EXPECT_NEAR(0.0, trajectory.getVelocity(traj_duration)[0], 0.1);
}

TEST(time_optimal_trajectory_generation, test2)
Expand Down Expand Up @@ -135,6 +140,11 @@ TEST(time_optimal_trajectory_generation, test2)
EXPECT_DOUBLE_EQ(533.0, trajectory.getPosition(trajectory.getDuration())[1]);
EXPECT_DOUBLE_EQ(951.0, trajectory.getPosition(trajectory.getDuration())[2]);
EXPECT_DOUBLE_EQ(90.0, trajectory.getPosition(trajectory.getDuration())[3]);

// Start at rest and end at rest
const double traj_duration = trajectory.getDuration();
EXPECT_NEAR(0.0, trajectory.getVelocity(0.0)[0], 0.1);
EXPECT_NEAR(0.0, trajectory.getVelocity(traj_duration)[0], 0.1);
}

TEST(time_optimal_trajectory_generation, test3)
Expand Down Expand Up @@ -173,6 +183,11 @@ TEST(time_optimal_trajectory_generation, test3)
EXPECT_DOUBLE_EQ(533.0, trajectory.getPosition(trajectory.getDuration())[1]);
EXPECT_DOUBLE_EQ(951.0, trajectory.getPosition(trajectory.getDuration())[2]);
EXPECT_DOUBLE_EQ(90.0, trajectory.getPosition(trajectory.getDuration())[3]);

// Start at rest and end at rest
const double traj_duration = trajectory.getDuration();
EXPECT_NEAR(0.0, trajectory.getVelocity(0.0)[0], 0.1);
EXPECT_NEAR(0.0, trajectory.getVelocity(traj_duration)[0], 0.1);
}

// Test the version of computeTimeStamps that takes custom velocity/acceleration limits
Expand Down Expand Up @@ -476,6 +491,53 @@ TEST(time_optimal_trajectory_generation, testFixedNumWaypoints)
EXPECT_NEAR(trajectory.getWayPointCount(), desired_num_waypoints, 1);
}

TEST(time_optimal_trajectory_generation, testSingleDofDiscontinuity)
{
// Test a (prior) specific failure case
Eigen::VectorXd waypoint(1);
std::list<Eigen::VectorXd> waypoints;

const double start_position = 1.881943;
waypoint << start_position;
waypoints.push_back(waypoint);
waypoint << 2.600542;
waypoints.push_back(waypoint);

Eigen::VectorXd max_velocities(1);
max_velocities << 4.54;
Eigen::VectorXd max_accelerations(1);
max_accelerations << 28.0;

Trajectory trajectory(Path(waypoints, 0.1 /* path tolerance */), max_velocities, max_accelerations,
0.001 /* timestep */);
EXPECT_TRUE(trajectory.isValid());

EXPECT_GT(trajectory.getDuration(), 0.0);
const double traj_duration = trajectory.getDuration();
EXPECT_NEAR(0.320681, traj_duration, 1e-3);

// Start matches
EXPECT_DOUBLE_EQ(start_position, trajectory.getPosition(0.0)[0]);
// Start at rest and end at rest
EXPECT_NEAR(0.0, trajectory.getVelocity(0.0)[0], 0.1);
EXPECT_NEAR(0.0, trajectory.getVelocity(traj_duration)[0], 0.1);

// Check vels and accels at all points
for (double time = 0; time < traj_duration; time += 0.01)
{
// This trajectory has a single switching point
double t_switch = 0.1603407;
if (time < t_switch)
{
EXPECT_NEAR(trajectory.getAcceleration(time)[0], max_accelerations[0], 1e-3) << "Time: " << time;
}
else if (time > t_switch)
{
EXPECT_NEAR(trajectory.getAcceleration(time)[0], -max_accelerations[0], 1e-3) << "Time: " << time;
}
}
}

int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
Expand Down