From b25f9b86e73524b325c2a830d99e3d6ab11553fb Mon Sep 17 00:00:00 2001 From: Andy Zelenak Date: Tue, 4 Apr 2023 14:50:00 -0500 Subject: [PATCH] Timestep bug --- .../src/time_optimal_trajectory_generation.cpp | 2 +- .../test_time_optimal_trajectory_generation.cpp | 16 ++++++++-------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp index 0a837f2229..1e95a96530 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -868,7 +868,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_; 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; @@ -876,6 +875,7 @@ Eigen::VectorXd Trajectory::getAcceleration(double time) const (path_.getTangent(path_pos) * path_vel - path_.getTangent(previous->path_pos_) * previous->path_vel_); if (time_step > 0.0) path_acc /= time_step; + std::cerr << "Getting acceleration: " << acceleration << " vector: " << path_acc[0] << std::endl; return path_acc; } diff --git a/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp index 4a07168b14..b93563449b 100644 --- a/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp @@ -380,20 +380,17 @@ TEST(time_optimal_trajectory_generation, testSingleDofDiscontinuity) // Start matches EXPECT_DOUBLE_EQ(start_position, trajectory.getPosition(0.0)[0]); - std::stringstream buffer; - // Redirect std::cerr to buffer - std::streambuf* cerr_buf = std::cerr.rdbuf(buffer.rdbuf()); + std::cerr << "Getting accel at 0.01: " << std::endl; + trajectory.getAcceleration(0.01)[0]; + std::cerr << "Getting accel at 0.02: " << std::endl; + trajectory.getAcceleration(0.02)[0]; // 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 == 0) - { - EXPECT_NEAR(trajectory.getAcceleration(time)[0], 0, 1e-3) << "Time: " << time; - } - else if (time > 0 && time < t_switch) + if (time < t_switch) { EXPECT_NEAR(trajectory.getAcceleration(time)[0], max_accelerations[0], 1e-3) << "Time: " << time; } @@ -403,6 +400,9 @@ TEST(time_optimal_trajectory_generation, testSingleDofDiscontinuity) } } + std::stringstream buffer; + // Redirect std::cerr to buffer + std::streambuf* cerr_buf = std::cerr.rdbuf(buffer.rdbuf()); std::cerr.rdbuf(cerr_buf); }