From d05f8c270de09da109e00aad9fa052d1b9d3d4a0 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 22 Apr 2020 11:55:38 -0500 Subject: [PATCH 01/38] A new funciton, calculateDerivativesFromVelocity() --- include/trackjoint/single_joint_generator.h | 5 +++- src/single_joint_generator.cpp | 29 ++++++++++++--------- test/trajectory_generation_test.cpp | 4 +-- 3 files changed, 23 insertions(+), 15 deletions(-) diff --git a/include/trackjoint/single_joint_generator.h b/include/trackjoint/single_joint_generator.h index 315b8ea5..27b6bb6c 100644 --- a/include/trackjoint/single_joint_generator.h +++ b/include/trackjoint/single_joint_generator.h @@ -108,7 +108,10 @@ class SingleJointGenerator ErrorCodeEnum predictTimeToReach(); /** \brief Calculate vel/accel/jerk from position */ - void calculateDerivatives(); + void calculateDerivativesFromPosition(); + + /** \brief Calculate accel/jerk from velocity */ + void calculateDerivativesFromVelocity(); const size_t kNumWaypointsThreshold, kMaxNumWaypointsFullTrajectory; diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index 1d0e729f..65db8bc4 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -56,7 +56,7 @@ ErrorCodeEnum SingleJointGenerator::generateTrajectory() waypoints_.positions = interpolate(nominal_times_); waypoints_.elapsed_times.setLinSpaced(waypoints_.positions.size(), 0., desired_duration_); - calculateDerivatives(); + calculateDerivativesFromPosition(); ErrorCodeEnum error_code = positionVectorLimitLookAhead(&index_last_successful_); if (error_code) @@ -76,7 +76,7 @@ ErrorCodeEnum SingleJointGenerator::extendTrajectoryDuration() size_t expected_num_waypoints = 1 + desired_duration_ / timestep_; waypoints_.elapsed_times.setLinSpaced(expected_num_waypoints, 0., desired_duration_); waypoints_.positions = interpolate(waypoints_.elapsed_times); - calculateDerivatives(); + calculateDerivativesFromPosition(); ErrorCodeEnum error_code = forwardLimitCompensation(&index_last_successful_); return error_code; } @@ -162,7 +162,7 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde 0.5 * waypoints_.jerks(index) * timestep_ * timestep_; // Re-calculate derivatives from the updated velocity vector - calculateDerivatives(); + calculateDerivativesFromVelocity(); delta_v = 0.5 * delta_j * timestep_ * timestep_; @@ -205,7 +205,7 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde waypoints_.accelerations(index - 1) * timestep_ + 0.5 * waypoints_.jerks(index) * timestep_ * timestep_; // Re-calculate derivatives from the updated velocity vector - calculateDerivatives(); + calculateDerivativesFromVelocity(); } else { @@ -247,7 +247,7 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde delta_v = std::copysign(velocity_limit, waypoints_.velocities(index)) - waypoints_.velocities(index); waypoints_.velocities(index) = std::copysign(velocity_limit, waypoints_.velocities(index)); // Re-calculate derivatives from the updated velocity vector - calculateDerivatives(); + calculateDerivativesFromVelocity(); // Try adjusting the velocity in previous timesteps to compensate for this limit. // Try to account for position error, too. @@ -393,8 +393,7 @@ inline bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index inline ErrorCodeEnum SingleJointGenerator::predictTimeToReach() { // Take a trajectory that could not reach the desired position in time. - // Try increasing the duration until it is interpolated without violating - // limits. + // Try increasing the duration until it is interpolated without violating limits. // This gives a new duration estimate. ErrorCodeEnum error_code = ErrorCodeEnum::kNoError; @@ -431,7 +430,7 @@ inline ErrorCodeEnum SingleJointGenerator::predictTimeToReach() // Try to create the trajectory again, with the new duration //////////////////////////////////////////////////////////// waypoints_.positions = interpolate(waypoints_.elapsed_times); - calculateDerivatives(); + calculateDerivativesFromPosition(); positionVectorLimitLookAhead(&index_last_successful_); } } @@ -509,17 +508,23 @@ inline ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* return error_code; } -inline void SingleJointGenerator::calculateDerivatives() +inline void SingleJointGenerator::calculateDerivativesFromPosition() { - // From position vector, approximate velocity and acceleration. - // velocity = (difference between adjacent position elements) / delta_t - // acceleration = (difference between adjacent velocity elements) / delta_t + // From position vector, approximate vel/accel/jerk. waypoints_.velocities = DiscreteDifferentiation(waypoints_.positions, timestep_, current_joint_state_.velocity); waypoints_.accelerations = DiscreteDifferentiation(waypoints_.velocities, timestep_, current_joint_state_.acceleration); waypoints_.jerks = DiscreteDifferentiation(waypoints_.accelerations, timestep_, 0); } +inline void SingleJointGenerator::calculateDerivativesFromVelocity() +{ + // From velocity vector, approximate accel/jerk. + waypoints_.accelerations = + DiscreteDifferentiation(waypoints_.velocities, timestep_, current_joint_state_.acceleration); + waypoints_.jerks = DiscreteDifferentiation(waypoints_.accelerations, timestep_, 0); +} + void SingleJointGenerator::updateTrajectoryDuration(double new_trajectory_duration) { // The trajectory will be forced to have this duration (or fail) because diff --git a/test/trajectory_generation_test.cpp b/test/trajectory_generation_test.cpp index 535a8375..c040806f 100644 --- a/test/trajectory_generation_test.cpp +++ b/test/trajectory_generation_test.cpp @@ -844,7 +844,7 @@ TEST_F(TrajectoryGenerationTest, TimestepDidNotMatch) EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep_, timestep_tolerance); } - +/* TEST_F(TrajectoryGenerationTest, CustomerStreaming) { // A customer-requested streaming test. @@ -929,7 +929,7 @@ TEST_F(TrajectoryGenerationTest, CustomerStreaming) // If the test gets here, it passed. } - +*/ TEST_F(TrajectoryGenerationTest, StreamingTooFewTimesteps) { // An error should be thrown if streaming mode is enabled with a desired duration < kMinNumTimesteps From 37aee8367a2ffcbd90aa47441584910340e9847b Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 22 Apr 2020 15:38:23 -0500 Subject: [PATCH 02/38] Fix sign of delta_v to backwardLimitComp() --- include/trackjoint/single_joint_generator.h | 2 +- src/single_joint_generator.cpp | 18 +++++++++--------- src/trajectory_generator.cpp | 2 +- 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/include/trackjoint/single_joint_generator.h b/include/trackjoint/single_joint_generator.h index 27b6bb6c..8a4f1a21 100644 --- a/include/trackjoint/single_joint_generator.h +++ b/include/trackjoint/single_joint_generator.h @@ -95,7 +95,7 @@ class SingleJointGenerator /** \brief Start looking back through a velocity vector to calculate for an * excess velocity at limited_index. */ - bool backwardLimitCompensation(size_t limited_index, double* excess_velocity); + bool backwardLimitCompensation(size_t limited_index, double excess_velocity); /** \brief This uses backwardLimitCompensation() but it starts from a position * vector */ diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index 65db8bc4..42508f9d 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -167,7 +167,7 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde delta_v = 0.5 * delta_j * timestep_ * timestep_; // Try adjusting the velocity in previous timesteps to compensate for this limit, if needed - successful_compensation = backwardLimitCompensation(index, &delta_v); + successful_compensation = backwardLimitCompensation(index, -delta_v); if (!successful_compensation) { position_error = position_error + delta_v * timestep_; @@ -218,7 +218,7 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde // Try adjusting the velocity in previous timesteps to compensate for this limit, if needed delta_v = delta_a * timestep_; - successful_compensation = backwardLimitCompensation(index, &delta_v); + successful_compensation = backwardLimitCompensation(index, -delta_v); if (!successful_compensation) { position_error = position_error + delta_v * timestep_; @@ -252,7 +252,7 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde // Try adjusting the velocity in previous timesteps to compensate for this limit. // Try to account for position error, too. delta_v += position_error / timestep_; - successful_compensation = backwardLimitCompensation(index, &delta_v); + successful_compensation = backwardLimitCompensation(index, -delta_v); if (!successful_compensation) { position_error = position_error + delta_v * timestep_; @@ -285,7 +285,7 @@ inline void SingleJointGenerator::recordFailureTime(size_t current_index, size_t } } -inline bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, double* excess_velocity) +inline bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, double excess_velocity) { // The algorithm: // 1) check jerk limits, from beginning to end of trajectory. Don't bother @@ -306,10 +306,10 @@ inline bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index if (fabs(waypoints_.velocities(index)) < limits_.velocity_limit) { // If the full change can be made in this timestep - if ((*excess_velocity > 0 && waypoints_.velocities(index) <= limits_.velocity_limit - *excess_velocity) || - (*excess_velocity < 0 && waypoints_.velocities(index) >= -limits_.velocity_limit - *excess_velocity)) + if ((excess_velocity > 0 && waypoints_.velocities(index) <= limits_.velocity_limit - excess_velocity) || + (excess_velocity < 0 && waypoints_.velocities(index) >= -limits_.velocity_limit - excess_velocity)) { - double new_velocity = waypoints_.velocities(index) + *excess_velocity; + double new_velocity = waypoints_.velocities(index) + excess_velocity; // Accel and jerk, calculated from the previous waypoints double backward_accel = (new_velocity - waypoints_.velocities(index - 1)) / timestep_; double backward_jerk = @@ -348,7 +348,7 @@ inline bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index { // This is what accel and jerk would be if we set velocity(index) to the // limit - double new_velocity = std::copysign(1.0, *excess_velocity) * limits_.velocity_limit; + double new_velocity = std::copysign(1.0, excess_velocity) * limits_.velocity_limit; // Accel and jerk, calculated from the previous waypoints double backward_accel = (new_velocity - waypoints_.velocities(index - 1)) / timestep_; double backward_jerk = @@ -378,7 +378,7 @@ inline bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index waypoints_.accelerations(index) = backward_accel; waypoints_.jerks(index) = backward_jerk; } - *excess_velocity = *excess_velocity - delta_v; + excess_velocity -= delta_v; } } } diff --git a/src/trajectory_generator.cpp b/src/trajectory_generator.cpp index a3779cd3..423351a1 100644 --- a/src/trajectory_generator.cpp +++ b/src/trajectory_generator.cpp @@ -425,4 +425,4 @@ ErrorCodeEnum TrajectoryGenerator::generateTrajectories(std::vector Date: Mon, 27 Apr 2020 13:37:49 -0500 Subject: [PATCH 03/38] Fix the streaming test/example --- src/streaming_example.cpp | 2 +- test/trajectory_generation_test.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/streaming_example.cpp b/src/streaming_example.cpp index 04d095da..34574314 100644 --- a/src/streaming_example.cpp +++ b/src/streaming_example.cpp @@ -39,7 +39,7 @@ int main(int argc, char** argv) constexpr double final_acceleration_tolerance = 1e-1; // For streaming mode, it is important to keep the desired duration >=10 timesteps. // Otherwise, an error will be thrown. This helps with accuracy - constexpr double min_desired_duration = 10 * timestep; + constexpr double min_desired_duration = 1 * timestep; // Between TrackJoint iterations, move ahead this many waypoints along the trajectory. constexpr std::size_t next_waypoint = 1; diff --git a/test/trajectory_generation_test.cpp b/test/trajectory_generation_test.cpp index c040806f..26017577 100644 --- a/test/trajectory_generation_test.cpp +++ b/test/trajectory_generation_test.cpp @@ -844,7 +844,7 @@ TEST_F(TrajectoryGenerationTest, TimestepDidNotMatch) EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep_, timestep_tolerance); } -/* + TEST_F(TrajectoryGenerationTest, CustomerStreaming) { // A customer-requested streaming test. @@ -862,7 +862,7 @@ TEST_F(TrajectoryGenerationTest, CustomerStreaming) constexpr double final_position_tolerance = 1e-5; constexpr double final_velocity_tolerance = 1e-3; constexpr double final_acceleration_tolerance = 1e-2; - const double min_desired_duration = 10 * timestep_; + const double min_desired_duration = timestep_; // Between iterations, skip this many waypoints. // Take next_waypoint from the previous trajectory to start the new trajectory. // Minimum is 1. @@ -929,7 +929,7 @@ TEST_F(TrajectoryGenerationTest, CustomerStreaming) // If the test gets here, it passed. } -*/ + TEST_F(TrajectoryGenerationTest, StreamingTooFewTimesteps) { // An error should be thrown if streaming mode is enabled with a desired duration < kMinNumTimesteps From 19c1c6f90658b5de4f863880b50ac73acc3cc3a6 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 22 Apr 2020 13:27:22 -0500 Subject: [PATCH 04/38] Fix the skipping of some elements in downSample() --- src/trajectory_generator.cpp | 24 +++++++++++++++--------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/src/trajectory_generator.cpp b/src/trajectory_generator.cpp index 423351a1..f0f47b25 100644 --- a/src/trajectory_generator.cpp +++ b/src/trajectory_generator.cpp @@ -134,32 +134,38 @@ void TrajectoryGenerator::downSample(Eigen::VectorXd* time_vector, Eigen::Vector size_t num_elements_filled_in_new_vector = 0; // Number of elements traversed in the upsampled vector size_t num_up_sampled_elements_traversed = 0; + // The last index taken from the beginning of the upsampled vector + size_t last_element_pulled_from_beginning = 0; + // The last index taken from the end of the upsampled vector + size_t last_element_pulled_from_end = position_vector->size() - 1; + for (size_t count = 1; num_elements_filled_in_new_vector < new_vector_size - 2; ++count) { // Update num_elements_to_skip based on: - // (num_elements_remaining_in_upsampled_vector) / - // (num_elements_remaining_in_new_vector) + // (num_elements_remaining_in_upsampled_vector) / (num_elements_remaining_in_new_vector) num_elements_to_skip = (position_vector->size() - 2 - num_up_sampled_elements_traversed) / - (new_vector_size - 1 - num_elements_filled_in_new_vector); + (new_vector_size - 2 - num_elements_filled_in_new_vector); - new_positions[count] = (*position_vector)[count * num_elements_to_skip]; - new_velocities[count] = (*velocity_vector)[count * num_elements_to_skip]; - new_accelerations[count] = (*acceleration_vector)[count * num_elements_to_skip]; + new_positions[count] = (*position_vector)[last_element_pulled_from_beginning + num_elements_to_skip]; + new_velocities[count] = (*velocity_vector)[last_element_pulled_from_beginning + num_elements_to_skip]; + new_accelerations[count] = (*acceleration_vector)[last_element_pulled_from_beginning + num_elements_to_skip]; ++num_elements_filled_in_new_vector; num_up_sampled_elements_traversed = num_up_sampled_elements_traversed + num_elements_to_skip; + last_element_pulled_from_beginning = last_element_pulled_from_beginning + num_elements_to_skip; // Count down if we need to fill more elements. Subtract two because first and last element are already filled. if (num_elements_filled_in_new_vector < new_vector_size - 2) { // Start filling at (end-1) because the last element is already filled new_positions[new_positions.size() - 1 - count] = - (*position_vector)[position_vector->size() - 1 - count * num_elements_to_skip]; + (*position_vector)[last_element_pulled_from_end - num_elements_to_skip]; new_velocities[new_velocities.size() - 1 - count] = - (*velocity_vector)[velocity_vector->size() - 1 - count * num_elements_to_skip]; + (*velocity_vector)[last_element_pulled_from_end - num_elements_to_skip]; new_accelerations[new_accelerations.size() - 1 - count] = - (*acceleration_vector)[acceleration_vector->size() - 1 - count * num_elements_to_skip]; + (*acceleration_vector)[last_element_pulled_from_end - num_elements_to_skip]; ++num_elements_filled_in_new_vector; num_up_sampled_elements_traversed = num_up_sampled_elements_traversed + num_elements_to_skip; + last_element_pulled_from_end = last_element_pulled_from_end - num_elements_to_skip; } } From 030b21430a94b8aa53d2bd5ee02cb2537e824999 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 4 May 2020 11:24:51 -0500 Subject: [PATCH 05/38] Add a new single-joint oscillation test --- test/trajectory_generation_test.cpp | 47 +++++++++++++++++++++++++++++ 1 file changed, 47 insertions(+) diff --git a/test/trajectory_generation_test.cpp b/test/trajectory_generation_test.cpp index 26017577..ff4b590b 100644 --- a/test/trajectory_generation_test.cpp +++ b/test/trajectory_generation_test.cpp @@ -942,6 +942,53 @@ TEST_F(TrajectoryGenerationTest, StreamingTooFewTimesteps) EXPECT_EQ(ErrorCodeEnum::kLessThanTenTimestepsForStreamingMode, traj_gen.inputChecking(current_joint_states_, goal_joint_states_, limits_, timestep_)); } + +TEST_F(TrajectoryGenerationTest, SingleJointOscillation) +{ + // This test comes from MoveIt. Originally, this joint's trajectory oscillated + // Fixed by setting a shorter desired_duration_ + + timestep_ = 0.0075; + desired_duration_ = 0.1; + num_dof_ = 1; + + current_joint_states_.resize(num_dof_); + KinematicState joint_state; + joint_state.position = -0.00397532; + joint_state.velocity = -0.169886; + joint_state.acceleration = -0.84943; + current_joint_states_[0] = joint_state; + + goal_joint_states_.resize(num_dof_); + joint_state.position = -0.025211; + joint_state.velocity = -0.253157; + joint_state.acceleration = -0.87268; + goal_joint_states_[0] = joint_state; + + limits_.resize(num_dof_); + Limits single_joint_limits; + single_joint_limits.velocity_limit = 3.15; + single_joint_limits.acceleration_limit = 5; + single_joint_limits.jerk_limit = 5000; + limits_[0] = single_joint_limits; + + TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration_, max_duration_, current_joint_states_, + goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); + output_trajectories_.resize(num_dof_); + + EXPECT_EQ(ErrorCodeEnum::kNoError, + traj_gen.inputChecking(current_joint_states_, goal_joint_states_, limits_, timestep_)); + EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + + // Position error + const double position_tolerance = 2e-3; + const double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); + EXPECT_LT(position_error, position_tolerance); + // Timestep + const double timestep_tolerance = 0.0005; + EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep_, + timestep_tolerance); +} } // namespace trackjoint int main(int argc, char** argv) From 46731ef91f0068d0bce0cef055873d6b2cfa62cd Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 4 May 2020 17:17:40 -0500 Subject: [PATCH 06/38] It works! Probably could be implemented more efficiently --- include/trackjoint/single_joint_generator.h | 11 +++++----- src/single_joint_generator.cpp | 24 ++++++++++++++++++--- src/three_dof_examples.cpp | 4 ++-- 3 files changed, 29 insertions(+), 10 deletions(-) diff --git a/include/trackjoint/single_joint_generator.h b/include/trackjoint/single_joint_generator.h index 8a4f1a21..b7f92b34 100644 --- a/include/trackjoint/single_joint_generator.h +++ b/include/trackjoint/single_joint_generator.h @@ -13,6 +13,7 @@ #pragma once #include // copysign +#include // Spline-fitting is used to extend trajectory duration #include "trackjoint/error_codes.h" #include "trackjoint/joint_trajectory.h" #include "trackjoint/kinematic_state.h" @@ -21,6 +22,9 @@ namespace trackjoint { +typedef Eigen::Spline Spline1D; +typedef Eigen::SplineFitting SplineFitting1D; + class SingleJointGenerator { public: @@ -58,11 +62,8 @@ class SingleJointGenerator */ ErrorCodeEnum generateTrajectory(); - /** \brief Calculate a trajectory once duration is known. Similar to generateTrajectory minus predictTimeToReach(). - * - * return a TrackJoint status code - */ - ErrorCodeEnum extendTrajectoryDuration(); + /** \brief Extend a trajectory to a new duration. Magnitudes of vel/accel/jerk will be decreased. */ + void extendTrajectoryDuration(); /** \brief Get the generated trajectory * diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index 42508f9d..c5d1e9bd 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -69,16 +69,34 @@ ErrorCodeEnum SingleJointGenerator::generateTrajectory() return error_code; } -ErrorCodeEnum SingleJointGenerator::extendTrajectoryDuration() +void SingleJointGenerator::extendTrajectoryDuration() { + // Fit and generate a spline function to the original (time, position) + Eigen::RowVectorXd time(waypoints_.elapsed_times); + Eigen::RowVectorXd position(waypoints_.positions); + + const auto fit = SplineFitting1D::Interpolate(position, 2, time); + Spline1D spline(fit); + std::cout << "DONE FITTING SPLINE!" << std::endl; + + // New times, with the extended duration + size_t expected_num_waypoints = 1 + desired_duration_ / timestep_; + waypoints_.elapsed_times.setLinSpaced(expected_num_waypoints, 0., desired_duration_); + // Retrieve new positions at the new times + Eigen::RowVectorXd new_positions; + waypoints_.positions.resize(expected_num_waypoints); + for (size_t idx = 0; idx < waypoints_.elapsed_times.size(); ++idx) + waypoints_.positions[idx] = spline(waypoints_.elapsed_times(idx)).coeff(0); + +/* // Clear previous results waypoints_ = JointTrajectory(); - size_t expected_num_waypoints = 1 + desired_duration_ / timestep_; + expected_num_waypoints = 1 + desired_duration_ / timestep_; waypoints_.elapsed_times.setLinSpaced(expected_num_waypoints, 0., desired_duration_); waypoints_.positions = interpolate(waypoints_.elapsed_times); calculateDerivativesFromPosition(); ErrorCodeEnum error_code = forwardLimitCompensation(&index_last_successful_); - return error_code; +*/ } JointTrajectory SingleJointGenerator::getTrajectory() diff --git a/src/three_dof_examples.cpp b/src/three_dof_examples.cpp index 2d5a2d53..1e825013 100644 --- a/src/three_dof_examples.cpp +++ b/src/three_dof_examples.cpp @@ -124,7 +124,7 @@ int main(int argc, char** argv) } } std::cout << "============================================" << std::endl; - +/* ///////////////////////////////// // Second example - large motions ///////////////////////////////// @@ -217,6 +217,6 @@ int main(int argc, char** argv) } } std::cout << "============================================" << std::endl; - +*/ return 0; } From 58201d47b90030024da0190ff87e795fffb244ae Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 5 May 2020 15:59:43 -0500 Subject: [PATCH 07/38] Delete commented block of code from previous implementation --- src/single_joint_generator.cpp | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index c5d1e9bd..5a0a35ee 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -77,26 +77,14 @@ void SingleJointGenerator::extendTrajectoryDuration() const auto fit = SplineFitting1D::Interpolate(position, 2, time); Spline1D spline(fit); - std::cout << "DONE FITTING SPLINE!" << std::endl; // New times, with the extended duration size_t expected_num_waypoints = 1 + desired_duration_ / timestep_; waypoints_.elapsed_times.setLinSpaced(expected_num_waypoints, 0., desired_duration_); // Retrieve new positions at the new times - Eigen::RowVectorXd new_positions; waypoints_.positions.resize(expected_num_waypoints); for (size_t idx = 0; idx < waypoints_.elapsed_times.size(); ++idx) waypoints_.positions[idx] = spline(waypoints_.elapsed_times(idx)).coeff(0); - -/* - // Clear previous results - waypoints_ = JointTrajectory(); - expected_num_waypoints = 1 + desired_duration_ / timestep_; - waypoints_.elapsed_times.setLinSpaced(expected_num_waypoints, 0., desired_duration_); - waypoints_.positions = interpolate(waypoints_.elapsed_times); - calculateDerivativesFromPosition(); - ErrorCodeEnum error_code = forwardLimitCompensation(&index_last_successful_); -*/ } JointTrajectory SingleJointGenerator::getTrajectory() From 929a75f92e1fc7d802c9e27512d7e9fa3e004833 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 6 May 2020 15:00:57 -0500 Subject: [PATCH 08/38] Set up if-condition to skip splines, if necessary --- src/single_joint_generator.cpp | 46 +++++++++++++++++++++++----------- 1 file changed, 32 insertions(+), 14 deletions(-) diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index 5a0a35ee..325c65fd 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -71,20 +71,38 @@ ErrorCodeEnum SingleJointGenerator::generateTrajectory() void SingleJointGenerator::extendTrajectoryDuration() { - // Fit and generate a spline function to the original (time, position) - Eigen::RowVectorXd time(waypoints_.elapsed_times); - Eigen::RowVectorXd position(waypoints_.positions); - - const auto fit = SplineFitting1D::Interpolate(position, 2, time); - Spline1D spline(fit); - - // New times, with the extended duration - size_t expected_num_waypoints = 1 + desired_duration_ / timestep_; - waypoints_.elapsed_times.setLinSpaced(expected_num_waypoints, 0., desired_duration_); - // Retrieve new positions at the new times - waypoints_.positions.resize(expected_num_waypoints); - for (size_t idx = 0; idx < waypoints_.elapsed_times.size(); ++idx) - waypoints_.positions[idx] = spline(waypoints_.elapsed_times(idx)).coeff(0); + size_t new_num_waypoints = 1 + desired_duration_ / timestep_; + + // If waypoints were successfully generated for this dimension previously, just stretch the trajectory with splines. + // This is the best way because it reduces overshoot. + // Otherwise, re-generate a new trajectory from scratch. + if (false /*index_last_successful_ == static_cast(waypoints_.positions.size() - 1)*/) + { + // Fit and generate a spline function to the original (time, position) + Eigen::RowVectorXd time(waypoints_.elapsed_times); + Eigen::RowVectorXd position(waypoints_.positions); + + const auto fit = SplineFitting1D::Interpolate(position, 2, time); + Spline1D spline(fit); + + // New times, with the extended duration + waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., desired_duration_); + // Retrieve new positions at the new times + waypoints_.positions.resize(new_num_waypoints); + + for (size_t idx = 0; idx < waypoints_.elapsed_times.size(); ++idx) + waypoints_.positions[idx] = spline(waypoints_.elapsed_times(idx)).coeff(0); + + return; + } + + // Clear previous results + waypoints_ = JointTrajectory(); + waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., desired_duration_); + waypoints_.positions = interpolate(waypoints_.elapsed_times); + calculateDerivativesFromPosition(); + ErrorCodeEnum error_code = forwardLimitCompensation(&index_last_successful_); + return; } JointTrajectory SingleJointGenerator::getTrajectory() From 3037ac2276046834ef500c322abd0b4d770ba78a Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 6 May 2020 17:16:31 -0500 Subject: [PATCH 09/38] Fix the crash in OscillatingUR5Test --- src/single_joint_generator.cpp | 6 ++++-- test/trajectory_generation_test.cpp | 7 ++++--- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index 325c65fd..d730fc48 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -74,9 +74,9 @@ void SingleJointGenerator::extendTrajectoryDuration() size_t new_num_waypoints = 1 + desired_duration_ / timestep_; // If waypoints were successfully generated for this dimension previously, just stretch the trajectory with splines. - // This is the best way because it reduces overshoot. + // ^This is the best way because it reduces overshoot. // Otherwise, re-generate a new trajectory from scratch. - if (false /*index_last_successful_ == static_cast(waypoints_.positions.size() - 1)*/) + if (index_last_successful_ == static_cast(waypoints_.elapsed_times.size() - 1)) { // Fit and generate a spline function to the original (time, position) Eigen::RowVectorXd time(waypoints_.elapsed_times); @@ -93,9 +93,11 @@ void SingleJointGenerator::extendTrajectoryDuration() for (size_t idx = 0; idx < waypoints_.elapsed_times.size(); ++idx) waypoints_.positions[idx] = spline(waypoints_.elapsed_times(idx)).coeff(0); + calculateDerivativesFromPosition(); return; } + // Plan a new trajectory from scratch: // Clear previous results waypoints_ = JointTrajectory(); waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., desired_duration_); diff --git a/test/trajectory_generation_test.cpp b/test/trajectory_generation_test.cpp index ff4b590b..a91f532e 100644 --- a/test/trajectory_generation_test.cpp +++ b/test/trajectory_generation_test.cpp @@ -197,7 +197,7 @@ class TrajectoryGenerationTest : public ::testing::Test } }; // class TrajectoryGenerationTest - +/* TEST_F(TrajectoryGenerationTest, EasyDefaultTrajectory) { // Use the class defaults. This trajectory is easy, does not require limit @@ -491,7 +491,7 @@ TEST_F(TrajectoryGenerationTest, NoisyStreamingCommand) uint expected_num_waypoints = num_waypoints; EXPECT_NEAR(uint(x_smoothed.size()), expected_num_waypoints, num_waypoint_tolerance); } - +*/ TEST_F(TrajectoryGenerationTest, OscillatingUR5TrackJointCase) { // This test comes from a MoveIt trajectory. @@ -597,7 +597,7 @@ TEST_F(TrajectoryGenerationTest, OscillatingUR5TrackJointCase) } EXPECT_EQ(trackjt_current_joint_states.at(0).size(), trackjt_goal_joint_states.at(0).size()); } - +/* TEST_F(TrajectoryGenerationTest, SuddenChangeOfDirection) { // Test a "corner" trajectory. @@ -989,6 +989,7 @@ TEST_F(TrajectoryGenerationTest, SingleJointOscillation) EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep_, timestep_tolerance); } +*/ } // namespace trackjoint int main(int argc, char** argv) From ed3af25acc7aa8a5e8b33eeba4c57ce9103ae605 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 6 May 2020 17:25:20 -0500 Subject: [PATCH 10/38] All tests pass except DurationExtension --- test/trajectory_generation_test.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/test/trajectory_generation_test.cpp b/test/trajectory_generation_test.cpp index a91f532e..ff4b590b 100644 --- a/test/trajectory_generation_test.cpp +++ b/test/trajectory_generation_test.cpp @@ -197,7 +197,7 @@ class TrajectoryGenerationTest : public ::testing::Test } }; // class TrajectoryGenerationTest -/* + TEST_F(TrajectoryGenerationTest, EasyDefaultTrajectory) { // Use the class defaults. This trajectory is easy, does not require limit @@ -491,7 +491,7 @@ TEST_F(TrajectoryGenerationTest, NoisyStreamingCommand) uint expected_num_waypoints = num_waypoints; EXPECT_NEAR(uint(x_smoothed.size()), expected_num_waypoints, num_waypoint_tolerance); } -*/ + TEST_F(TrajectoryGenerationTest, OscillatingUR5TrackJointCase) { // This test comes from a MoveIt trajectory. @@ -597,7 +597,7 @@ TEST_F(TrajectoryGenerationTest, OscillatingUR5TrackJointCase) } EXPECT_EQ(trackjt_current_joint_states.at(0).size(), trackjt_goal_joint_states.at(0).size()); } -/* + TEST_F(TrajectoryGenerationTest, SuddenChangeOfDirection) { // Test a "corner" trajectory. @@ -989,7 +989,6 @@ TEST_F(TrajectoryGenerationTest, SingleJointOscillation) EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep_, timestep_tolerance); } -*/ } // namespace trackjoint int main(int argc, char** argv) From 3eda4a31d0b80d26d894fe27c18668c99d559381 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 29 Jun 2020 16:42:42 -0500 Subject: [PATCH 11/38] Always use minimum possible duration for each joint --- include/trackjoint/single_joint_generator.h | 5 ++--- src/single_joint_generator.cpp | 11 +++++++---- src/trajectory_generator.cpp | 4 ++-- 3 files changed, 11 insertions(+), 9 deletions(-) diff --git a/include/trackjoint/single_joint_generator.h b/include/trackjoint/single_joint_generator.h index b7f92b34..a84d1d0a 100644 --- a/include/trackjoint/single_joint_generator.h +++ b/include/trackjoint/single_joint_generator.h @@ -31,7 +31,6 @@ class SingleJointGenerator /** \brief Constructor * * input timestep desired time between waypoints - * input desired_duration total desired duration of the trajectory * input max_duration allow the trajectory to be extended up to this limit. Error if that cannot be done. * input current_joint_states vector of the initial kinematic states for each degree of freedom * input goal_joint_states vector of the target kinematic states for each degree of freedom @@ -45,14 +44,14 @@ class SingleJointGenerator * input use_streaming_mode set to true for fast streaming applications. Returns a maximum of num_waypoints_threshold * waypoints. */ - SingleJointGenerator(double timestep, double desired_duration, double max_duration, + SingleJointGenerator(double timestep, double max_duration, const KinematicState& current_joint_state, const KinematicState& goal_joint_state, const Limits& limits, size_t desired_num_waypoints, size_t num_waypoints_threshold, size_t max_num_waypoints_trajectory_mode, const double position_tolerance, bool use_streaming_mode); /** \brief reset data members and prepare to generate a new trajectory */ - void reset(double timestep, double desired_duration, double max_duration, const KinematicState& current_joint_state, + void reset(double timestep, double max_duration, const KinematicState& current_joint_state, const KinematicState& goal_joint_state, const Limits& limits, size_t desired_num_waypoints, const double position_tolerance, bool use_streaming_mode); diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index d730fc48..670c5b19 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -10,7 +10,7 @@ namespace trackjoint { -SingleJointGenerator::SingleJointGenerator(double timestep, double desired_duration, double max_duration, +SingleJointGenerator::SingleJointGenerator(double timestep, double max_duration, const KinematicState& current_joint_state, const KinematicState& goal_joint_state, const Limits& limits, size_t desired_num_waypoints, size_t num_waypoints_threshold, @@ -19,7 +19,6 @@ SingleJointGenerator::SingleJointGenerator(double timestep, double desired_durat : kNumWaypointsThreshold(num_waypoints_threshold) , kMaxNumWaypointsFullTrajectory(max_num_waypoints_trajectory_mode) , timestep_(timestep) - , desired_duration_(desired_duration) , max_duration_(max_duration) , current_joint_state_(current_joint_state) , goal_joint_state_(goal_joint_state) @@ -27,17 +26,21 @@ SingleJointGenerator::SingleJointGenerator(double timestep, double desired_durat , position_tolerance_(position_tolerance) , use_streaming_mode_(use_streaming_mode) { + // Start with this estimate of the shortest possible duration + // The shortest possible duration avoids oscillation, as much as possible + // Desired duration cannot be less than one timestep + desired_duration_ = std::max(timestep_, fabs((goal_joint_state.position - current_joint_state.position) / limits_.velocity_limit)); + // Waypoint times nominal_times_ = Eigen::VectorXd::LinSpaced(desired_num_waypoints, 0, desired_duration_); } -void SingleJointGenerator::reset(double timestep, double desired_duration, double max_duration, +void SingleJointGenerator::reset(double timestep, double max_duration, const KinematicState& current_joint_state, const KinematicState& goal_joint_state, const Limits& limits, size_t desired_num_waypoints, const double position_tolerance, bool use_streaming_mode) { timestep_ = timestep; - desired_duration_ = desired_duration; max_duration_ = max_duration; current_joint_state_ = current_joint_state; goal_joint_state_ = goal_joint_state; diff --git a/src/trajectory_generator.cpp b/src/trajectory_generator.cpp index f0f47b25..da55969f 100644 --- a/src/trajectory_generator.cpp +++ b/src/trajectory_generator.cpp @@ -32,7 +32,7 @@ TrajectoryGenerator::TrajectoryGenerator(uint num_dof, double timestep, double d for (size_t joint = 0; joint < kNumDof; ++joint) { single_joint_generators_.push_back( - SingleJointGenerator(upsampled_timestep_, desired_duration_, max_duration_, current_joint_states[joint], + SingleJointGenerator(upsampled_timestep_, max_duration_, current_joint_states[joint], goal_joint_states[joint], limits[joint], upsampled_num_waypoints_, kNumWaypointsThreshold, kMaxNumWaypointsFullTrajectory, position_tolerance, use_streaming_mode_)); } @@ -59,7 +59,7 @@ void TrajectoryGenerator::reset(double timestep, double desired_duration, double // Initialize a trajectory generator for each joint for (size_t joint = 0; joint < kNumDof; ++joint) { - single_joint_generators_[joint].reset(upsampled_timestep_, desired_duration_, max_duration_, + single_joint_generators_[joint].reset(upsampled_timestep_, max_duration_, current_joint_states[joint], goal_joint_states[joint], limits[joint], upsampled_num_waypoints_, position_tolerance, use_streaming_mode_); } From 5b4b31f6b36821a36ac3d688de15a929de099e8c Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 29 Jun 2020 17:06:39 -0500 Subject: [PATCH 12/38] Add the if/else fix in backwardLimitComp (PR 57) --- src/single_joint_generator.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index 670c5b19..f59bc052 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -371,9 +371,8 @@ inline bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index break; } } - // else, can't make all of the correction in this timestep, so make as - // much of a change as possible - else + // Can't make all of the correction in this timestep, so make as much of a change as possible + if (!successful_compensation) { // This is what accel and jerk would be if we set velocity(index) to the // limit From 7946d38f8b6917cfde9e4391349b455b18876d22 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 29 Jun 2020 21:36:02 -0500 Subject: [PATCH 13/38] Add an example that should be able to execute faster --- src/simple_example.cpp | 22 +++++++++++----------- src/trajectory_generator.cpp | 5 +++++ 2 files changed, 16 insertions(+), 11 deletions(-) diff --git a/src/simple_example.cpp b/src/simple_example.cpp index 49e4ac54..94a81fa3 100644 --- a/src/simple_example.cpp +++ b/src/simple_example.cpp @@ -21,37 +21,37 @@ int main(int argc, char** argv) // This example is for just one degree of freedom constexpr int num_dof = 1; // Timestep. Units don't matter as long as they're consistent - constexpr double timestep = 0.0075; + constexpr double timestep = 0.0039; // Total desired trajectory duration - constexpr double desired_duration = 0.028322; + constexpr double desired_duration = timestep; // TrackJoint is allowed to extend the trajectory up to this duration, if a solution at kDesiredDuration can't be // found - constexpr double max_duration = 10; + constexpr double max_duration = 30; // streaming mode returns just a few waypoints but executes very quickly. We won't use it here -- we'll calculate // the whole trajectory at once. constexpr bool use_streaming_mode = false; // Optional logging of TrackJoint output - const std::string output_path_base = "/home/" + std::string(getenv("USER")) + "/trackjoint_data/output_joint"; + const std::string output_path_base = "/home/" + std::string(getenv("USER")) + "/Downloads/trackjoint_data/output_joint"; std::vector current_joint_states(1); trackjoint::KinematicState joint_state; - joint_state.position = 0.00596041; - joint_state.velocity = -0.176232; - joint_state.acceleration = -3.06289; + joint_state.position = 0.842514; + joint_state.velocity = 1.82451; + joint_state.acceleration = 0; // This is the initial state of the joint current_joint_states[0] = joint_state; std::vector goal_joint_states(1); - joint_state.position = -0.00121542; - joint_state.velocity = -0.289615; - joint_state.acceleration = -2.88021; + joint_state.position = 1.12338; + joint_state.velocity = 1.13232; + joint_state.acceleration = -2.97729; goal_joint_states[0] = joint_state; trackjoint::Limits single_joint_limits; // Typically, jerk limit >> acceleration limit > velocity limit single_joint_limits.velocity_limit = 3.15; single_joint_limits.acceleration_limit = 5; - single_joint_limits.jerk_limit = 5000; + single_joint_limits.jerk_limit = 1000; std::vector limits(1, single_joint_limits); // This descibes how far TrackJoint can deviate from a smooth, interpolated polynomial. diff --git a/src/trajectory_generator.cpp b/src/trajectory_generator.cpp index da55969f..0333dfbd 100644 --- a/src/trajectory_generator.cpp +++ b/src/trajectory_generator.cpp @@ -309,6 +309,11 @@ ErrorCodeEnum TrajectoryGenerator::synchronizeTrajComponents(std::vector Date: Mon, 29 Jun 2020 22:19:14 -0500 Subject: [PATCH 14/38] It works! --- src/simple_example.cpp | 8 ++++---- src/single_joint_generator.cpp | 28 +++++++++++++++++++++++++++- src/trajectory_generator.cpp | 5 ----- 3 files changed, 31 insertions(+), 10 deletions(-) diff --git a/src/simple_example.cpp b/src/simple_example.cpp index 94a81fa3..08ed6cc5 100644 --- a/src/simple_example.cpp +++ b/src/simple_example.cpp @@ -23,7 +23,7 @@ int main(int argc, char** argv) // Timestep. Units don't matter as long as they're consistent constexpr double timestep = 0.0039; // Total desired trajectory duration - constexpr double desired_duration = timestep; + constexpr double desired_duration = 30 * timestep; // TrackJoint is allowed to extend the trajectory up to this duration, if a solution at kDesiredDuration can't be // found constexpr double max_duration = 30; @@ -50,13 +50,13 @@ int main(int argc, char** argv) trackjoint::Limits single_joint_limits; // Typically, jerk limit >> acceleration limit > velocity limit single_joint_limits.velocity_limit = 3.15; - single_joint_limits.acceleration_limit = 5; - single_joint_limits.jerk_limit = 1000; + single_joint_limits.acceleration_limit = 100; + single_joint_limits.jerk_limit = 1e5; std::vector limits(1, single_joint_limits); // This descibes how far TrackJoint can deviate from a smooth, interpolated polynomial. // It is used for calculations internally. It should be set to a smaller number than your task requires. - const double position_tolerance = 1e-6; + const double position_tolerance = 1e-3; // Instantiate a trajectory generation object trackjoint::TrajectoryGenerator traj_gen(num_dof, timestep, desired_duration, max_duration, current_joint_states, diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index f59bc052..aa1b1dd9 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -61,6 +61,22 @@ ErrorCodeEnum SingleJointGenerator::generateTrajectory() waypoints_.elapsed_times.setLinSpaced(waypoints_.positions.size(), 0., desired_duration_); calculateDerivativesFromPosition(); + std::cout << "Position i " << waypoints_.positions[0] << std::endl; + std::cout << "Position i " << waypoints_.positions[1] << std::endl; + std::cout << "Position i " << waypoints_.positions[2] << std::endl; + + std::cout << "Velocity i " << waypoints_.velocities[0] << std::endl; + std::cout << "Velocity i " << waypoints_.velocities[1] << std::endl; + std::cout << "Velocity i " << waypoints_.velocities[2] << std::endl; + + std::cout << "Acceleration i " << waypoints_.accelerations[0] << std::endl; + std::cout << "Acceleration i " << waypoints_.accelerations[1] << std::endl; + std::cout << "Acceleration i " << waypoints_.accelerations[2] << std::endl; + + std::cout << "Jerk i " << waypoints_.jerks[0] << std::endl; + std::cout << "Jerk i " << waypoints_.jerks[1] << std::endl; + std::cout << "Jerk i " << waypoints_.jerks[2] << std::endl; + ErrorCodeEnum error_code = positionVectorLimitLookAhead(&index_last_successful_); if (error_code) { @@ -203,6 +219,7 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde } if (fabs(position_error) > position_tolerance_) { + std::cout << "Failed in jerk comp!" << std::endl; recordFailureTime(index, index_last_successful); // Only break, do not return, because we are looking for the FIRST failure. May find an earlier failure in // subsequent code @@ -254,6 +271,7 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde } if (fabs(position_error) > position_tolerance_) { + std::cout << "Failed in accel. comp!" << std::endl; recordFailureTime(index, index_last_successful); // Only break, do not return, because we are looking for the FIRST failure. May find an earlier failure in // subsequent code @@ -294,6 +312,7 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde fabs(waypoints_.accelerations(index + 1)) > acceleration_limit || fabs(waypoints_.jerks(index + 1)) > jerk_limit) { + std::cout << "Failed in vel. comp!" << std::endl; recordFailureTime(index, index_last_successful); // Only break, do not return, because we are looking for the FIRST failure. May find an earlier failure in // subsequent code @@ -436,7 +455,13 @@ inline ErrorCodeEnum SingleJointGenerator::predictTimeToReach() { // Try increasing the duration, based on fraction of states that weren't reached successfully desired_duration_ = - 1. + 0.5 * (1. - index_last_successful_ / (waypoints_.positions.size() - 1)) * desired_duration_; + (1. + 0.1 * (1. - index_last_successful_ / (waypoints_.positions.size() - 1))) * desired_duration_; + + std::cout << "---" << std::endl; + std::cout << "index_last_successful_: " << index_last_successful_ << std::endl; + std::cout << "total indices: " << waypoints_.positions.size() << std::endl; + std::cout << "new duration: " << desired_duration_ << std::endl; + std::cout << "---" << std::endl; // // Round to nearest timestep if (std::fmod(desired_duration_, timestep_) > 0.5 * timestep_) @@ -539,6 +564,7 @@ inline ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* inline void SingleJointGenerator::calculateDerivativesFromPosition() { // From position vector, approximate vel/accel/jerk. + std::cout << "TIMESTEP: " << timestep_ << std::endl; waypoints_.velocities = DiscreteDifferentiation(waypoints_.positions, timestep_, current_joint_state_.velocity); waypoints_.accelerations = DiscreteDifferentiation(waypoints_.velocities, timestep_, current_joint_state_.acceleration); diff --git a/src/trajectory_generator.cpp b/src/trajectory_generator.cpp index 0333dfbd..da55969f 100644 --- a/src/trajectory_generator.cpp +++ b/src/trajectory_generator.cpp @@ -309,11 +309,6 @@ ErrorCodeEnum TrajectoryGenerator::synchronizeTrajComponents(std::vector Date: Mon, 29 Jun 2020 22:23:55 -0500 Subject: [PATCH 15/38] Restore original kinematic limits. DownSample actually worked fine --- src/simple_example.cpp | 6 +++--- src/single_joint_generator.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/simple_example.cpp b/src/simple_example.cpp index 08ed6cc5..615b9378 100644 --- a/src/simple_example.cpp +++ b/src/simple_example.cpp @@ -23,7 +23,7 @@ int main(int argc, char** argv) // Timestep. Units don't matter as long as they're consistent constexpr double timestep = 0.0039; // Total desired trajectory duration - constexpr double desired_duration = 30 * timestep; + constexpr double desired_duration = timestep; // TrackJoint is allowed to extend the trajectory up to this duration, if a solution at kDesiredDuration can't be // found constexpr double max_duration = 30; @@ -50,8 +50,8 @@ int main(int argc, char** argv) trackjoint::Limits single_joint_limits; // Typically, jerk limit >> acceleration limit > velocity limit single_joint_limits.velocity_limit = 3.15; - single_joint_limits.acceleration_limit = 100; - single_joint_limits.jerk_limit = 1e5; + single_joint_limits.acceleration_limit = 5; + single_joint_limits.jerk_limit = 1000; std::vector limits(1, single_joint_limits); // This descibes how far TrackJoint can deviate from a smooth, interpolated polynomial. diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index aa1b1dd9..81f92b2a 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -455,7 +455,7 @@ inline ErrorCodeEnum SingleJointGenerator::predictTimeToReach() { // Try increasing the duration, based on fraction of states that weren't reached successfully desired_duration_ = - (1. + 0.1 * (1. - index_last_successful_ / (waypoints_.positions.size() - 1))) * desired_duration_; + (1. + 0.2 * (1. - index_last_successful_ / (waypoints_.positions.size() - 1))) * desired_duration_; std::cout << "---" << std::endl; std::cout << "index_last_successful_: " << index_last_successful_ << std::endl; From 75ba568a810242a8ae29ab8db480ce26cf9dcda5 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 29 Jun 2020 22:26:22 -0500 Subject: [PATCH 16/38] position_tolerance can be a bit bigger --- src/simple_example.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/simple_example.cpp b/src/simple_example.cpp index 615b9378..1c56ae8b 100644 --- a/src/simple_example.cpp +++ b/src/simple_example.cpp @@ -56,7 +56,7 @@ int main(int argc, char** argv) // This descibes how far TrackJoint can deviate from a smooth, interpolated polynomial. // It is used for calculations internally. It should be set to a smaller number than your task requires. - const double position_tolerance = 1e-3; + const double position_tolerance = 1e-4; // Instantiate a trajectory generation object trackjoint::TrajectoryGenerator traj_gen(num_dof, timestep, desired_duration, max_duration, current_joint_states, From 7c937adc50abdd0c20abff94e53d425dc97d3346 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 30 Jun 2020 09:37:48 -0500 Subject: [PATCH 17/38] Remove debug statements, add hints if failure on first waypoint --- src/single_joint_generator.cpp | 33 +++++++-------------------------- 1 file changed, 7 insertions(+), 26 deletions(-) diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index 81f92b2a..c47191d3 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -61,22 +61,6 @@ ErrorCodeEnum SingleJointGenerator::generateTrajectory() waypoints_.elapsed_times.setLinSpaced(waypoints_.positions.size(), 0., desired_duration_); calculateDerivativesFromPosition(); - std::cout << "Position i " << waypoints_.positions[0] << std::endl; - std::cout << "Position i " << waypoints_.positions[1] << std::endl; - std::cout << "Position i " << waypoints_.positions[2] << std::endl; - - std::cout << "Velocity i " << waypoints_.velocities[0] << std::endl; - std::cout << "Velocity i " << waypoints_.velocities[1] << std::endl; - std::cout << "Velocity i " << waypoints_.velocities[2] << std::endl; - - std::cout << "Acceleration i " << waypoints_.accelerations[0] << std::endl; - std::cout << "Acceleration i " << waypoints_.accelerations[1] << std::endl; - std::cout << "Acceleration i " << waypoints_.accelerations[2] << std::endl; - - std::cout << "Jerk i " << waypoints_.jerks[0] << std::endl; - std::cout << "Jerk i " << waypoints_.jerks[1] << std::endl; - std::cout << "Jerk i " << waypoints_.jerks[2] << std::endl; - ErrorCodeEnum error_code = positionVectorLimitLookAhead(&index_last_successful_); if (error_code) { @@ -219,7 +203,6 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde } if (fabs(position_error) > position_tolerance_) { - std::cout << "Failed in jerk comp!" << std::endl; recordFailureTime(index, index_last_successful); // Only break, do not return, because we are looking for the FIRST failure. May find an earlier failure in // subsequent code @@ -271,7 +254,6 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde } if (fabs(position_error) > position_tolerance_) { - std::cout << "Failed in accel. comp!" << std::endl; recordFailureTime(index, index_last_successful); // Only break, do not return, because we are looking for the FIRST failure. May find an earlier failure in // subsequent code @@ -312,7 +294,6 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde fabs(waypoints_.accelerations(index + 1)) > acceleration_limit || fabs(waypoints_.jerks(index + 1)) > jerk_limit) { - std::cout << "Failed in vel. comp!" << std::endl; recordFailureTime(index, index_last_successful); // Only break, do not return, because we are looking for the FIRST failure. May find an earlier failure in // subsequent code @@ -457,12 +438,6 @@ inline ErrorCodeEnum SingleJointGenerator::predictTimeToReach() desired_duration_ = (1. + 0.2 * (1. - index_last_successful_ / (waypoints_.positions.size() - 1))) * desired_duration_; - std::cout << "---" << std::endl; - std::cout << "index_last_successful_: " << index_last_successful_ << std::endl; - std::cout << "total indices: " << waypoints_.positions.size() << std::endl; - std::cout << "new duration: " << desired_duration_ << std::endl; - std::cout << "---" << std::endl; - // // Round to nearest timestep if (std::fmod(desired_duration_, timestep_) > 0.5 * timestep_) desired_duration_ = desired_duration_ + timestep_; @@ -543,6 +518,13 @@ inline ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* if (error_code) return error_code; + // Helpful hint if limit comp fails on the first waypoint + if (index_last_successful_ == 1) + { + std::cout << "Limit compensation failed at the first waypoint. " << + "Try a larger position_tolerance parameter or smaller timestep." << std::endl; + } + // Re-compile the position with these modifications. // Ensure the first and last elements are a perfect match with initial/final // conditions @@ -564,7 +546,6 @@ inline ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* inline void SingleJointGenerator::calculateDerivativesFromPosition() { // From position vector, approximate vel/accel/jerk. - std::cout << "TIMESTEP: " << timestep_ << std::endl; waypoints_.velocities = DiscreteDifferentiation(waypoints_.positions, timestep_, current_joint_state_.velocity); waypoints_.accelerations = DiscreteDifferentiation(waypoints_.velocities, timestep_, current_joint_state_.acceleration); From 5424cdb2c6d4e0b5b79f872e96608e7bab94d2a2 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 30 Jun 2020 10:39:05 -0500 Subject: [PATCH 18/38] Revert changes to three_dof_examples.cpp --- src/three_dof_examples.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/three_dof_examples.cpp b/src/three_dof_examples.cpp index 1e825013..2d5a2d53 100644 --- a/src/three_dof_examples.cpp +++ b/src/three_dof_examples.cpp @@ -124,7 +124,7 @@ int main(int argc, char** argv) } } std::cout << "============================================" << std::endl; -/* + ///////////////////////////////// // Second example - large motions ///////////////////////////////// @@ -217,6 +217,6 @@ int main(int argc, char** argv) } } std::cout << "============================================" << std::endl; -*/ + return 0; } From 0bce555bb46788915d209a96ad1894773dc48a59 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 30 Jun 2020 10:44:25 -0500 Subject: [PATCH 19/38] Small fixup to streaming_example.cpp for readability --- src/streaming_example.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/streaming_example.cpp b/src/streaming_example.cpp index 34574314..fe41820e 100644 --- a/src/streaming_example.cpp +++ b/src/streaming_example.cpp @@ -39,7 +39,7 @@ int main(int argc, char** argv) constexpr double final_acceleration_tolerance = 1e-1; // For streaming mode, it is important to keep the desired duration >=10 timesteps. // Otherwise, an error will be thrown. This helps with accuracy - constexpr double min_desired_duration = 1 * timestep; + constexpr double min_desired_duration = timestep; // Between TrackJoint iterations, move ahead this many waypoints along the trajectory. constexpr std::size_t next_waypoint = 1; From cef283e3dc129e001986b0498bc4637ae5c7ab05 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 5 Jul 2020 11:55:31 -0500 Subject: [PATCH 20/38] Reset desired_duration_ in reset() function, too. --- src/single_joint_generator.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index c47191d3..0e616c71 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -48,6 +48,11 @@ void SingleJointGenerator::reset(double timestep, double max_duration, position_tolerance_ = position_tolerance; use_streaming_mode_ = use_streaming_mode; + // Start with this estimate of the shortest possible duration + // The shortest possible duration avoids oscillation, as much as possible + // Desired duration cannot be less than one timestep + desired_duration_ = std::max(timestep_, fabs((goal_joint_state.position - current_joint_state.position) / limits_.velocity_limit)); + // Waypoint times nominal_times_ = Eigen::VectorXd::LinSpaced(desired_num_waypoints, 0, desired_duration_); } From 8a8a7a2083e33aafb83f7f7ab4e5e21d6c94793c Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 5 Jul 2020 14:00:55 -0500 Subject: [PATCH 21/38] Delete extra loop in clipVector() --- src/trajectory_generator.cpp | 39 +++++++++++++++++------------------- 1 file changed, 18 insertions(+), 21 deletions(-) diff --git a/src/trajectory_generator.cpp b/src/trajectory_generator.cpp index da55969f..047abba5 100644 --- a/src/trajectory_generator.cpp +++ b/src/trajectory_generator.cpp @@ -369,28 +369,25 @@ void TrajectoryGenerator::clipVectorsForOutput(std::vector* tra { for (size_t joint = 0; joint < kNumDof; ++joint) { - for (size_t joint = 0; joint < trajectory->size(); ++joint) + for (auto waypt = 0; waypt < trajectory->at(joint).velocities.size(); ++waypt) { - for (auto waypt = 0; waypt < trajectory->at(joint).velocities.size(); ++waypt) - { - // Velocity - if (trajectory->at(joint).velocities[waypt] > limits_[joint].velocity_limit) - trajectory->at(joint).velocities[waypt] = limits_[joint].velocity_limit; - if (trajectory->at(joint).velocities[waypt] < -limits_[joint].velocity_limit) - trajectory->at(joint).velocities[waypt] = -limits_[joint].velocity_limit; - - // Acceleration - if (trajectory->at(joint).accelerations[waypt] > limits_[joint].acceleration_limit) - trajectory->at(joint).accelerations[waypt] = limits_[joint].acceleration_limit; - if (trajectory->at(joint).accelerations[waypt] < -limits_[joint].acceleration_limit) - trajectory->at(joint).accelerations[waypt] = -limits_[joint].acceleration_limit; - - // Jerk - if (trajectory->at(joint).jerks[waypt] > limits_[joint].jerk_limit) - trajectory->at(joint).jerks[waypt] = limits_[joint].jerk_limit; - if (trajectory->at(joint).jerks[waypt] < -limits_[joint].jerk_limit) - trajectory->at(joint).jerks[waypt] = -limits_[joint].jerk_limit; - } + // Velocity + if (trajectory->at(joint).velocities[waypt] > limits_[joint].velocity_limit) + trajectory->at(joint).velocities[waypt] = limits_[joint].velocity_limit; + if (trajectory->at(joint).velocities[waypt] < -limits_[joint].velocity_limit) + trajectory->at(joint).velocities[waypt] = -limits_[joint].velocity_limit; + + // Acceleration + if (trajectory->at(joint).accelerations[waypt] > limits_[joint].acceleration_limit) + trajectory->at(joint).accelerations[waypt] = limits_[joint].acceleration_limit; + if (trajectory->at(joint).accelerations[waypt] < -limits_[joint].acceleration_limit) + trajectory->at(joint).accelerations[waypt] = -limits_[joint].acceleration_limit; + + // Jerk + if (trajectory->at(joint).jerks[waypt] > limits_[joint].jerk_limit) + trajectory->at(joint).jerks[waypt] = limits_[joint].jerk_limit; + if (trajectory->at(joint).jerks[waypt] < -limits_[joint].jerk_limit) + trajectory->at(joint).jerks[waypt] = -limits_[joint].jerk_limit; } } } From 5863f42076177fe98bb08a309f3e40b5370c2f0a Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 5 Jul 2020 14:02:19 -0500 Subject: [PATCH 22/38] Change three_dof_examples to something that needs debugging Third joint acceleration definitely needs some work --- src/three_dof_examples.cpp | 141 +++++++------------------------------ 1 file changed, 24 insertions(+), 117 deletions(-) diff --git a/src/three_dof_examples.cpp b/src/three_dof_examples.cpp index 2d5a2d53..4161340b 100644 --- a/src/three_dof_examples.cpp +++ b/src/three_dof_examples.cpp @@ -19,48 +19,48 @@ int main(int argc, char** argv) { constexpr int num_dof = 3; - const double timestep = 0.001; - double desired_duration = 0.028322; - constexpr double max_duration = 10; + const double timestep = 0.0039; + double desired_duration = 0.162051; + constexpr double max_duration = 30; // Streaming mode returns just a few waypoints but executes very quickly. constexpr bool use_streaming_mode = false; // Position tolerance for each waypoint - constexpr double waypoint_position_tolerance = 1e-5; - const std::string output_path_base = "/home/" + std::string(getenv("USER")) + "/trackjoint_data/output_joint"; + constexpr double waypoint_position_tolerance = 1e-4; + const std::string output_path_base = "/home/" + std::string(getenv("USER")) + "/Downloads/trackjoint_data/output_joint"; - //////////////////////////////////////////////// - // First example - small motions, come to a halt - //////////////////////////////////////////////// - std::cout << "EXAMPLE 1" << std::endl; std::vector current_joint_states(3); trackjoint::KinematicState joint_state; - joint_state.position = 0.00596041; - joint_state.velocity = -0.176232; - joint_state.acceleration = -3.06289; + joint_state.position = 1.23984; + joint_state.velocity = 1.45704; + joint_state.acceleration = -0.0196446; current_joint_states[0] = joint_state; - joint_state.position = -0.596041; - joint_state.velocity = 0.2; - joint_state.acceleration = 2.39; + joint_state.position = -1.12637; + joint_state.velocity = -0.774015; + joint_state.acceleration = 0.0104357; current_joint_states[1] = joint_state; - joint_state.position = -0.1; - joint_state.velocity = 0.13; - joint_state.acceleration = 0.2; + joint_state.position = -0.014012; + joint_state.velocity = 0.583426; + joint_state.acceleration = -0.00786606; current_joint_states[2] = joint_state; std::vector goal_joint_states(3); - joint_state.position = -0.00121542; - joint_state.velocity = 0; - joint_state.acceleration = 0; + joint_state.position = 1.4757; + joint_state.velocity = 1.45675; + joint_state.acceleration = 0.126129; goal_joint_states[0] = joint_state; - joint_state.position = -0.57; + joint_state.position = -0.545844; + joint_state.velocity = 1.81361; + joint_state.acceleration = 1.93357; goal_joint_states[1] = joint_state; - joint_state.position = 0; + joint_state.position = -0.447873; + joint_state.velocity = -1.35293; + joint_state.acceleration = -1.43058; goal_joint_states[2] = joint_state; trackjoint::Limits single_joint_limits; single_joint_limits.velocity_limit = 3.15; single_joint_limits.acceleration_limit = 5; - single_joint_limits.jerk_limit = 5000; + single_joint_limits.jerk_limit = 10000; std::vector limits(3, single_joint_limits); // Initialize main class @@ -125,98 +125,5 @@ int main(int argc, char** argv) } std::cout << "============================================" << std::endl; - ///////////////////////////////// - // Second example - large motions - ///////////////////////////////// - std::cout << "EXAMPLE 2" << std::endl; - desired_duration = 1; - - joint_state.position = 0.74; - joint_state.velocity = 0.33; - joint_state.acceleration = 0.15; - current_joint_states[0] = joint_state; - joint_state.position = -0.59; - joint_state.velocity = 0.2; - joint_state.acceleration = 2.39; - current_joint_states[1] = joint_state; - joint_state.position = -1.1; - joint_state.velocity = 1.13; - joint_state.acceleration = 0.9; - current_joint_states[2] = joint_state; - - joint_state.position = 0.93; - joint_state.velocity = 0.1; - joint_state.acceleration = -0.2; - goal_joint_states[0] = joint_state; - joint_state.position = -0.86; - joint_state.velocity = 0.05; - joint_state.acceleration = -0.12; - goal_joint_states[1] = joint_state; - joint_state.position = -0.42; - joint_state.velocity = 0.59; - joint_state.acceleration = -0.23; - goal_joint_states[2] = joint_state; - - // Create a new traj gen object with new parameters - traj_gen.~TrajectoryGenerator(); - new (&traj_gen) - trackjoint::TrajectoryGenerator(num_dof, timestep, desired_duration, max_duration, current_joint_states, - goal_joint_states, limits, waypoint_position_tolerance, use_streaming_mode); - - error_code = traj_gen.inputChecking(current_joint_states, goal_joint_states, limits, timestep); - - // Input error handling - if an error is found, the trajectory is not - // generated. - if (error_code != trackjoint::ErrorCodeEnum::kNoError) - { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; - return -1; - } - - // Measure runtime - start = std::chrono::system_clock::now(); - error_code = traj_gen.generateTrajectories(&output_trajectories); - end = std::chrono::system_clock::now(); - - // Trajectory generation error handling - if (error_code != trackjoint::ErrorCodeEnum::kNoError) - { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; - return -1; - } - - elapsed_seconds = end - start; - - std::cout << "Runtime: " << elapsed_seconds.count() << std::endl; - std::cout << "Num waypoints: " << output_trajectories.at(0).positions.size() << std::endl; - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; - - // Save the synchronized trajectories to .csv files - traj_gen.saveTrajectoriesToFile(output_trajectories, output_path_base); - - // Print the synchronized trajectories - for (size_t joint = 0; joint < output_trajectories.size(); ++joint) - { - std::cout << "==========" << std::endl; - std::cout << std::endl; - std::cout << std::endl; - std::cout << "==========" << std::endl; - std::cout << "Joint " << joint << std::endl; - std::cout << "==========" << std::endl; - std::cout << std::endl; - std::cout << std::endl; - std::cout << "==========" << std::endl; - for (size_t waypoint = 0; waypoint < static_cast(output_trajectories.at(joint).positions.size()); - ++waypoint) - { - std::cout << "Elapsed time: " << output_trajectories.at(joint).elapsed_times(waypoint) - << " Position: " << output_trajectories.at(joint).positions(waypoint) - << " Velocity: " << output_trajectories.at(joint).velocities(waypoint) - << " Acceleration: " << output_trajectories.at(joint).accelerations(waypoint) - << " Jerk: " << output_trajectories.at(joint).jerks(waypoint) << std::endl; - } - } - std::cout << "============================================" << std::endl; - return 0; } From a516d4792cd6512b4a1b5211bc3bf30b0ab81531 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 6 Jul 2020 08:13:10 -0500 Subject: [PATCH 23/38] Fix spline interpolation --- src/single_joint_generator.cpp | 29 +++++++++++++++++++++-------- src/trajectory_generator.cpp | 3 +++ 2 files changed, 24 insertions(+), 8 deletions(-) diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index 0e616c71..9d5ef1fc 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -86,11 +86,18 @@ void SingleJointGenerator::extendTrajectoryDuration() // Otherwise, re-generate a new trajectory from scratch. if (index_last_successful_ == static_cast(waypoints_.elapsed_times.size() - 1)) { - // Fit and generate a spline function to the original (time, position) - Eigen::RowVectorXd time(waypoints_.elapsed_times); + std::cout << "Stretching trajectory!" << std::endl; + std::cout << "New desired duration: " << desired_duration_ << std::endl; + + // Fit and generate a spline function to the original positions, same number of waypoints, new (extended) duration + // This only decreases velocity/accel/jerk, so no worries re. limit violation + //Eigen::VectorXd new_times; + //new_times.setLinSpaced(waypoints_.elapsed_times.size(), 0, desired_duration_); + Eigen::RowVectorXd new_times; + new_times.setLinSpaced(waypoints_.elapsed_times.size(), 0, desired_duration_); Eigen::RowVectorXd position(waypoints_.positions); - const auto fit = SplineFitting1D::Interpolate(position, 2, time); + const auto fit = SplineFitting1D::Interpolate(position, 2, new_times); Spline1D spline(fit); // New times, with the extended duration @@ -101,17 +108,23 @@ void SingleJointGenerator::extendTrajectoryDuration() for (size_t idx = 0; idx < waypoints_.elapsed_times.size(); ++idx) waypoints_.positions[idx] = spline(waypoints_.elapsed_times(idx)).coeff(0); + std::cout << "Final waypoint after stretching: " << waypoints_.positions[waypoints_.positions.size() - 1] << std::endl; + calculateDerivativesFromPosition(); return; } // Plan a new trajectory from scratch: // Clear previous results - waypoints_ = JointTrajectory(); - waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., desired_duration_); - waypoints_.positions = interpolate(waypoints_.elapsed_times); - calculateDerivativesFromPosition(); - ErrorCodeEnum error_code = forwardLimitCompensation(&index_last_successful_); + else + { + waypoints_ = JointTrajectory(); + waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., desired_duration_); + waypoints_.positions = interpolate(waypoints_.elapsed_times); + calculateDerivativesFromPosition(); + ErrorCodeEnum error_code = forwardLimitCompensation(&index_last_successful_); + } + return; } diff --git a/src/trajectory_generator.cpp b/src/trajectory_generator.cpp index 047abba5..a2de92b0 100644 --- a/src/trajectory_generator.cpp +++ b/src/trajectory_generator.cpp @@ -331,6 +331,9 @@ ErrorCodeEnum TrajectoryGenerator::synchronizeTrajComponents(std::vectorat(joint) = single_joint_generators_[joint].getTrajectory(); + + std::cout << "End position for Joint " << joint << ": " << + single_joint_generators_[joint].getTrajectory().positions[single_joint_generators_[joint].getTrajectory().positions.size()-1] << std::endl; } // If this was the index of longest duration, don't need to re-generate a trajectory else From f3fb818cc241b4a4aa28d9aeeb47f2f48c4960f0 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 10 Jul 2020 12:27:51 -0500 Subject: [PATCH 24/38] Convert simple_example to a good failure case --- src/simple_example.cpp | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/src/simple_example.cpp b/src/simple_example.cpp index 1c56ae8b..c2dea80e 100644 --- a/src/simple_example.cpp +++ b/src/simple_example.cpp @@ -21,12 +21,10 @@ int main(int argc, char** argv) // This example is for just one degree of freedom constexpr int num_dof = 1; // Timestep. Units don't matter as long as they're consistent - constexpr double timestep = 0.0039; - // Total desired trajectory duration - constexpr double desired_duration = timestep; + constexpr double timestep = 0.004; // TrackJoint is allowed to extend the trajectory up to this duration, if a solution at kDesiredDuration can't be // found - constexpr double max_duration = 30; + constexpr double max_duration = 5; // streaming mode returns just a few waypoints but executes very quickly. We won't use it here -- we'll calculate // the whole trajectory at once. constexpr bool use_streaming_mode = false; @@ -35,28 +33,33 @@ int main(int argc, char** argv) std::vector current_joint_states(1); trackjoint::KinematicState joint_state; - joint_state.position = 0.842514; - joint_state.velocity = 1.82451; + joint_state.position = 1.16431; + joint_state.velocity = 2.66465; joint_state.acceleration = 0; // This is the initial state of the joint current_joint_states[0] = joint_state; std::vector goal_joint_states(1); - joint_state.position = 1.12338; - joint_state.velocity = 1.13232; - joint_state.acceleration = -2.97729; + joint_state.position = 1.40264; + joint_state.velocity = 2.88556; + joint_state.acceleration = 0.0615633; goal_joint_states[0] = joint_state; trackjoint::Limits single_joint_limits; // Typically, jerk limit >> acceleration limit > velocity limit single_joint_limits.velocity_limit = 3.15; single_joint_limits.acceleration_limit = 5; - single_joint_limits.jerk_limit = 1000; + single_joint_limits.jerk_limit = 20000; std::vector limits(1, single_joint_limits); + // Estimate trajectory duration + // This is the fastest possible trajectory execution time, + // assuming the robot starts at full velocity. + double desired_duration = fabs(goal_joint_states[0].position - current_joint_states[0].position) / single_joint_limits.velocity_limit; + // This descibes how far TrackJoint can deviate from a smooth, interpolated polynomial. // It is used for calculations internally. It should be set to a smaller number than your task requires. - const double position_tolerance = 1e-4; + const double position_tolerance = 0.02; // Instantiate a trajectory generation object trackjoint::TrajectoryGenerator traj_gen(num_dof, timestep, desired_duration, max_duration, current_joint_states, From c8b828c5c0b3291068ea61789103caf4c69a4d81 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 10 Jul 2020 13:45:37 -0500 Subject: [PATCH 25/38] Derivative calculations in the middle of LimitComp often caused limits to be exceeded --- src/simple_example.cpp | 1 + src/single_joint_generator.cpp | 15 +++------------ 2 files changed, 4 insertions(+), 12 deletions(-) diff --git a/src/simple_example.cpp b/src/simple_example.cpp index c2dea80e..59e8fdea 100644 --- a/src/simple_example.cpp +++ b/src/simple_example.cpp @@ -56,6 +56,7 @@ int main(int argc, char** argv) // This is the fastest possible trajectory execution time, // assuming the robot starts at full velocity. double desired_duration = fabs(goal_joint_states[0].position - current_joint_states[0].position) / single_joint_limits.velocity_limit; + std::cout << "Desired duration: " << desired_duration << std::endl; // This descibes how far TrackJoint can deviate from a smooth, interpolated polynomial. // It is used for calculations internally. It should be set to a smaller number than your task requires. diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index 9d5ef1fc..918b4f28 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -86,9 +86,6 @@ void SingleJointGenerator::extendTrajectoryDuration() // Otherwise, re-generate a new trajectory from scratch. if (index_last_successful_ == static_cast(waypoints_.elapsed_times.size() - 1)) { - std::cout << "Stretching trajectory!" << std::endl; - std::cout << "New desired duration: " << desired_duration_ << std::endl; - // Fit and generate a spline function to the original positions, same number of waypoints, new (extended) duration // This only decreases velocity/accel/jerk, so no worries re. limit violation //Eigen::VectorXd new_times; @@ -108,8 +105,6 @@ void SingleJointGenerator::extendTrajectoryDuration() for (size_t idx = 0; idx < waypoints_.elapsed_times.size(); ++idx) waypoints_.positions[idx] = spline(waypoints_.elapsed_times(idx)).coeff(0); - std::cout << "Final waypoint after stretching: " << waypoints_.positions[waypoints_.positions.size() - 1] << std::endl; - calculateDerivativesFromPosition(); return; } @@ -208,9 +203,6 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde waypoints_.accelerations(index - 1) * timestep_ + 0.5 * waypoints_.jerks(index) * timestep_ * timestep_; - // Re-calculate derivatives from the updated velocity vector - calculateDerivativesFromVelocity(); - delta_v = 0.5 * delta_j * timestep_ * timestep_; // Try adjusting the velocity in previous timesteps to compensate for this limit, if needed @@ -251,8 +243,6 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde waypoints_.velocities(index) = waypoints_.velocities(index - 1) + waypoints_.accelerations(index - 1) * timestep_ + 0.5 * waypoints_.jerks(index) * timestep_ * timestep_; - // Re-calculate derivatives from the updated velocity vector - calculateDerivativesFromVelocity(); } else { @@ -293,8 +283,6 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde { delta_v = std::copysign(velocity_limit, waypoints_.velocities(index)) - waypoints_.velocities(index); waypoints_.velocities(index) = std::copysign(velocity_limit, waypoints_.velocities(index)); - // Re-calculate derivatives from the updated velocity vector - calculateDerivativesFromVelocity(); // Try adjusting the velocity in previous timesteps to compensate for this limit. // Try to account for position error, too. @@ -320,6 +308,9 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde } } + // Re-calculate derivatives from the updated velocity vector + calculateDerivativesFromVelocity(); + return ErrorCodeEnum::kNoError; } From 63ca859270f611d9e262fde9484228989797b408 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 10 Jul 2020 17:22:19 -0500 Subject: [PATCH 26/38] Better estimates for desired durations --- src/simple_example.cpp | 3 +-- src/three_dof_examples.cpp | 6 +++++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/simple_example.cpp b/src/simple_example.cpp index 59e8fdea..e6b19096 100644 --- a/src/simple_example.cpp +++ b/src/simple_example.cpp @@ -53,8 +53,7 @@ int main(int argc, char** argv) std::vector limits(1, single_joint_limits); // Estimate trajectory duration - // This is the fastest possible trajectory execution time, - // assuming the robot starts at full velocity. + // This is the fastest possible trajectory execution time, assuming the robot starts at full velocity. double desired_duration = fabs(goal_joint_states[0].position - current_joint_states[0].position) / single_joint_limits.velocity_limit; std::cout << "Desired duration: " << desired_duration << std::endl; diff --git a/src/three_dof_examples.cpp b/src/three_dof_examples.cpp index 4161340b..48deb780 100644 --- a/src/three_dof_examples.cpp +++ b/src/three_dof_examples.cpp @@ -20,7 +20,6 @@ int main(int argc, char** argv) { constexpr int num_dof = 3; const double timestep = 0.0039; - double desired_duration = 0.162051; constexpr double max_duration = 30; // Streaming mode returns just a few waypoints but executes very quickly. constexpr bool use_streaming_mode = false; @@ -63,6 +62,11 @@ int main(int argc, char** argv) single_joint_limits.jerk_limit = 10000; std::vector limits(3, single_joint_limits); + // Estimate trajectory duration + // This is the fastest possible trajectory execution time, assuming the robot starts at full velocity. + double desired_duration = fabs(goal_joint_states[1].position - current_joint_states[1].position) / single_joint_limits.velocity_limit; + std::cout << "Desired duration: " << desired_duration << std::endl; + // Initialize main class trackjoint::TrajectoryGenerator traj_gen(num_dof, timestep, desired_duration, max_duration, current_joint_states, goal_joint_states, limits, waypoint_position_tolerance, use_streaming_mode); From 33251c844afe632cf30ce43bb3f8b8da79416e07 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 13 Jul 2020 11:59:35 -0500 Subject: [PATCH 27/38] Clang format --- include/trackjoint/single_joint_generator.h | 11 +++++------ src/simple_example.cpp | 6 ++++-- src/single_joint_generator.cpp | 21 +++++++++++---------- src/three_dof_examples.cpp | 6 ++++-- src/trajectory_generator.cpp | 17 ++++++++++------- 5 files changed, 34 insertions(+), 27 deletions(-) diff --git a/include/trackjoint/single_joint_generator.h b/include/trackjoint/single_joint_generator.h index a84d1d0a..d5b567a1 100644 --- a/include/trackjoint/single_joint_generator.h +++ b/include/trackjoint/single_joint_generator.h @@ -12,7 +12,7 @@ #pragma once -#include // copysign +#include // copysign #include // Spline-fitting is used to extend trajectory duration #include "trackjoint/error_codes.h" #include "trackjoint/joint_trajectory.h" @@ -44,11 +44,10 @@ class SingleJointGenerator * input use_streaming_mode set to true for fast streaming applications. Returns a maximum of num_waypoints_threshold * waypoints. */ - SingleJointGenerator(double timestep, double max_duration, - const KinematicState& current_joint_state, const KinematicState& goal_joint_state, - const Limits& limits, size_t desired_num_waypoints, size_t num_waypoints_threshold, - size_t max_num_waypoints_trajectory_mode, const double position_tolerance, - bool use_streaming_mode); + SingleJointGenerator(double timestep, double max_duration, const KinematicState& current_joint_state, + const KinematicState& goal_joint_state, const Limits& limits, size_t desired_num_waypoints, + size_t num_waypoints_threshold, size_t max_num_waypoints_trajectory_mode, + const double position_tolerance, bool use_streaming_mode); /** \brief reset data members and prepare to generate a new trajectory */ void reset(double timestep, double max_duration, const KinematicState& current_joint_state, diff --git a/src/simple_example.cpp b/src/simple_example.cpp index e6b19096..17c0a663 100644 --- a/src/simple_example.cpp +++ b/src/simple_example.cpp @@ -29,7 +29,8 @@ int main(int argc, char** argv) // the whole trajectory at once. constexpr bool use_streaming_mode = false; // Optional logging of TrackJoint output - const std::string output_path_base = "/home/" + std::string(getenv("USER")) + "/Downloads/trackjoint_data/output_joint"; + const std::string output_path_base = "/home/" + std::string(getenv("USER")) + "/Downloads/trackjoint_data/" + "output_joint"; std::vector current_joint_states(1); trackjoint::KinematicState joint_state; @@ -54,7 +55,8 @@ int main(int argc, char** argv) // Estimate trajectory duration // This is the fastest possible trajectory execution time, assuming the robot starts at full velocity. - double desired_duration = fabs(goal_joint_states[0].position - current_joint_states[0].position) / single_joint_limits.velocity_limit; + double desired_duration = + fabs(goal_joint_states[0].position - current_joint_states[0].position) / single_joint_limits.velocity_limit; std::cout << "Desired duration: " << desired_duration << std::endl; // This descibes how far TrackJoint can deviate from a smooth, interpolated polynomial. diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index 918b4f28..973c4239 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -29,16 +29,16 @@ SingleJointGenerator::SingleJointGenerator(double timestep, double max_duration, // Start with this estimate of the shortest possible duration // The shortest possible duration avoids oscillation, as much as possible // Desired duration cannot be less than one timestep - desired_duration_ = std::max(timestep_, fabs((goal_joint_state.position - current_joint_state.position) / limits_.velocity_limit)); + desired_duration_ = + std::max(timestep_, fabs((goal_joint_state.position - current_joint_state.position) / limits_.velocity_limit)); // Waypoint times nominal_times_ = Eigen::VectorXd::LinSpaced(desired_num_waypoints, 0, desired_duration_); } -void SingleJointGenerator::reset(double timestep, double max_duration, - const KinematicState& current_joint_state, const KinematicState& goal_joint_state, - const Limits& limits, size_t desired_num_waypoints, const double position_tolerance, - bool use_streaming_mode) +void SingleJointGenerator::reset(double timestep, double max_duration, const KinematicState& current_joint_state, + const KinematicState& goal_joint_state, const Limits& limits, + size_t desired_num_waypoints, const double position_tolerance, bool use_streaming_mode) { timestep_ = timestep; max_duration_ = max_duration; @@ -51,7 +51,8 @@ void SingleJointGenerator::reset(double timestep, double max_duration, // Start with this estimate of the shortest possible duration // The shortest possible duration avoids oscillation, as much as possible // Desired duration cannot be less than one timestep - desired_duration_ = std::max(timestep_, fabs((goal_joint_state.position - current_joint_state.position) / limits_.velocity_limit)); + desired_duration_ = + std::max(timestep_, fabs((goal_joint_state.position - current_joint_state.position) / limits_.velocity_limit)); // Waypoint times nominal_times_ = Eigen::VectorXd::LinSpaced(desired_num_waypoints, 0, desired_duration_); @@ -88,8 +89,8 @@ void SingleJointGenerator::extendTrajectoryDuration() { // Fit and generate a spline function to the original positions, same number of waypoints, new (extended) duration // This only decreases velocity/accel/jerk, so no worries re. limit violation - //Eigen::VectorXd new_times; - //new_times.setLinSpaced(waypoints_.elapsed_times.size(), 0, desired_duration_); + // Eigen::VectorXd new_times; + // new_times.setLinSpaced(waypoints_.elapsed_times.size(), 0, desired_duration_); Eigen::RowVectorXd new_times; new_times.setLinSpaced(waypoints_.elapsed_times.size(), 0, desired_duration_); Eigen::RowVectorXd position(waypoints_.positions); @@ -530,8 +531,8 @@ inline ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* // Helpful hint if limit comp fails on the first waypoint if (index_last_successful_ == 1) { - std::cout << "Limit compensation failed at the first waypoint. " << - "Try a larger position_tolerance parameter or smaller timestep." << std::endl; + std::cout << "Limit compensation failed at the first waypoint. " + << "Try a larger position_tolerance parameter or smaller timestep." << std::endl; } // Re-compile the position with these modifications. diff --git a/src/three_dof_examples.cpp b/src/three_dof_examples.cpp index 48deb780..796329ad 100644 --- a/src/three_dof_examples.cpp +++ b/src/three_dof_examples.cpp @@ -25,7 +25,8 @@ int main(int argc, char** argv) constexpr bool use_streaming_mode = false; // Position tolerance for each waypoint constexpr double waypoint_position_tolerance = 1e-4; - const std::string output_path_base = "/home/" + std::string(getenv("USER")) + "/Downloads/trackjoint_data/output_joint"; + const std::string output_path_base = "/home/" + std::string(getenv("USER")) + "/Downloads/trackjoint_data/" + "output_joint"; std::vector current_joint_states(3); trackjoint::KinematicState joint_state; @@ -64,7 +65,8 @@ int main(int argc, char** argv) // Estimate trajectory duration // This is the fastest possible trajectory execution time, assuming the robot starts at full velocity. - double desired_duration = fabs(goal_joint_states[1].position - current_joint_states[1].position) / single_joint_limits.velocity_limit; + double desired_duration = + fabs(goal_joint_states[1].position - current_joint_states[1].position) / single_joint_limits.velocity_limit; std::cout << "Desired duration: " << desired_duration << std::endl; // Initialize main class diff --git a/src/trajectory_generator.cpp b/src/trajectory_generator.cpp index a2de92b0..19b9d012 100644 --- a/src/trajectory_generator.cpp +++ b/src/trajectory_generator.cpp @@ -32,8 +32,8 @@ TrajectoryGenerator::TrajectoryGenerator(uint num_dof, double timestep, double d for (size_t joint = 0; joint < kNumDof; ++joint) { single_joint_generators_.push_back( - SingleJointGenerator(upsampled_timestep_, max_duration_, current_joint_states[joint], - goal_joint_states[joint], limits[joint], upsampled_num_waypoints_, kNumWaypointsThreshold, + SingleJointGenerator(upsampled_timestep_, max_duration_, current_joint_states[joint], goal_joint_states[joint], + limits[joint], upsampled_num_waypoints_, kNumWaypointsThreshold, kMaxNumWaypointsFullTrajectory, position_tolerance, use_streaming_mode_)); } } @@ -59,9 +59,9 @@ void TrajectoryGenerator::reset(double timestep, double desired_duration, double // Initialize a trajectory generator for each joint for (size_t joint = 0; joint < kNumDof; ++joint) { - single_joint_generators_[joint].reset(upsampled_timestep_, max_duration_, - current_joint_states[joint], goal_joint_states[joint], limits[joint], - upsampled_num_waypoints_, position_tolerance, use_streaming_mode_); + single_joint_generators_[joint].reset(upsampled_timestep_, max_duration_, current_joint_states[joint], + goal_joint_states[joint], limits[joint], upsampled_num_waypoints_, + position_tolerance, use_streaming_mode_); } } @@ -332,8 +332,11 @@ ErrorCodeEnum TrajectoryGenerator::synchronizeTrajComponents(std::vectorat(joint) = single_joint_generators_[joint].getTrajectory(); - std::cout << "End position for Joint " << joint << ": " << - single_joint_generators_[joint].getTrajectory().positions[single_joint_generators_[joint].getTrajectory().positions.size()-1] << std::endl; + std::cout << "End position for Joint " << joint << ": " + << single_joint_generators_[joint] + .getTrajectory() + .positions[single_joint_generators_[joint].getTrajectory().positions.size() - 1] + << std::endl; } // If this was the index of longest duration, don't need to re-generate a trajectory else From f6b8f9aebc1325cb915f1d5c9ba383ee26f7d0af Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 21 Aug 2020 22:58:04 -0500 Subject: [PATCH 28/38] Increase position_tolerance to help pass the first waypoint --- test/trajectory_generation_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/trajectory_generation_test.cpp b/test/trajectory_generation_test.cpp index ff4b590b..6a37bfb1 100644 --- a/test/trajectory_generation_test.cpp +++ b/test/trajectory_generation_test.cpp @@ -71,7 +71,7 @@ class TrajectoryGenerationTest : public ::testing::Test double max_duration_ = 10; std::vector current_joint_states_, goal_joint_states_; std::vector limits_; - double position_tolerance_ = 1e-6; + double position_tolerance_ = 1e-4; bool use_streaming_mode_ = false; bool write_output_ = true; std::vector output_trajectories_; From a9c3d16d1c3b7d97fc63803ae3dc77829d57e500 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sat, 22 Aug 2020 18:40:16 -0500 Subject: [PATCH 29/38] Add checks on Timestep to every relevant unit test --- test/trajectory_generation_test.cpp | 40 ++++++++++++++++++++++++++--- 1 file changed, 36 insertions(+), 4 deletions(-) diff --git a/test/trajectory_generation_test.cpp b/test/trajectory_generation_test.cpp index 6a37bfb1..f302971e 100644 --- a/test/trajectory_generation_test.cpp +++ b/test/trajectory_generation_test.cpp @@ -258,6 +258,10 @@ TEST_F(TrajectoryGenerationTest, OneTimestepDuration) double position_tolerance = 1e-4; double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); EXPECT_LT(position_error, position_tolerance); + // Timestep + double timestep_tolerance = 0.1 * timestep_; + EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep_, + timestep_tolerance); // Duration const double duration_tolerance = 5e-3; size_t vector_length = output_trajectories_[0].elapsed_times.size() - 1; @@ -288,16 +292,16 @@ TEST_F(TrajectoryGenerationTest, RoughlyTwoTimestepDuration) const double position_tolerance = 1e-4; const double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); EXPECT_LT(position_error, position_tolerance); + // Timestep + double timestep_tolerance = 0.003; + EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep, + timestep_tolerance); // Duration const double duration_tolerance = 5e-4; size_t vector_length = output_trajectories_[0].elapsed_times.size() - 1; EXPECT_NEAR(output_trajectories_[0].elapsed_times(vector_length), desired_duration, duration_tolerance); // Number of waypoints EXPECT_EQ(output_trajectories_[0].elapsed_times.size(), 3); - // Timestep - double timestep_tolerance = 0.003; - EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep, - timestep_tolerance); } TEST_F(TrajectoryGenerationTest, FourTimestepDuration) @@ -324,6 +328,10 @@ TEST_F(TrajectoryGenerationTest, FourTimestepDuration) const double position_tolerance = 1e-4; const double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); EXPECT_LT(position_error, position_tolerance); + // Timestep + double timestep_tolerance = 0.1 * timestep; + EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep, + timestep_tolerance); // Duration const double duration_tolerance = 5e-3; size_t vector_length = output_trajectories_[0].elapsed_times.size() - 1; @@ -371,6 +379,10 @@ TEST_F(TrajectoryGenerationTest, SixTimestepDuration) double position_tolerance = 1e-4; double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); EXPECT_LT(position_error, position_tolerance); + // Timestep + double timestep_tolerance = 0.1 * timestep_; + EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep_, + timestep_tolerance); // Duration const double duration_tolerance = 5e-3; size_t vector_length = output_trajectories_[0].elapsed_times.size() - 1; @@ -413,6 +425,10 @@ TEST_F(TrajectoryGenerationTest, VelAccelJerkLimit) double position_tolerance = 1e-4; double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); EXPECT_LT(position_error, position_tolerance); + // Timestep + double timestep_tolerance = 0.1 * timestep_; + EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep_, + timestep_tolerance); // Duration const double duration_tolerance = 5e-3; size_t vector_length = output_trajectories_[0].elapsed_times.size() - 1; @@ -643,6 +659,10 @@ TEST_F(TrajectoryGenerationTest, SuddenChangeOfDirection) double position_tolerance = 1e-4; double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); EXPECT_LT(position_error, position_tolerance); + // Timestep + double timestep_tolerance = 0.1 * timestep_; + EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep_, + timestep_tolerance); // Duration const double duration_tolerance = 5e-3; size_t vector_length = output_trajectories_[0].elapsed_times.size() - 1; @@ -689,6 +709,10 @@ TEST_F(TrajectoryGenerationTest, LimitCompensation) const double position_tolerance = 1e-4; const double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); EXPECT_LT(position_error, position_tolerance); + // Timestep + double timestep_tolerance = 0.1 * timestep_; + EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep_, + timestep_tolerance); // Duration uint num_waypoint_tolerance = 1; uint expected_num_waypoints = 1 + desired_duration / timestep; @@ -738,6 +762,10 @@ TEST_F(TrajectoryGenerationTest, DurationExtension) const double position_tolerance = 1e-4; const double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); EXPECT_LT(position_error, position_tolerance); + // Timestep + double timestep_tolerance = 0.1 * timestep; + EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep, + timestep_tolerance); // Duration const double expected_duration = 1.05; const double duration_tolerance = 5e-3; @@ -793,6 +821,10 @@ TEST_F(TrajectoryGenerationTest, PositiveAndNegativeLimits) const double position_tolerance = 1e-4; const double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); EXPECT_LT(position_error, position_tolerance); + // Timestep + double timestep_tolerance = 0.1 * timestep; + EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep, + timestep_tolerance); // Duration uint num_waypoint_tolerance = 1; uint expected_num_waypoints = 1 + desired_duration / timestep; From 4aa0177e360aa82f3ba3dc3097228fda2283071b Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 23 Aug 2020 11:12:55 -0500 Subject: [PATCH 30/38] Add arg for whether upsampling occurred --- include/trackjoint/single_joint_generator.h | 5 ++-- src/single_joint_generator.cpp | 29 ++++++++++++++++----- src/trajectory_generator.cpp | 7 +++-- 3 files changed, 31 insertions(+), 10 deletions(-) diff --git a/include/trackjoint/single_joint_generator.h b/include/trackjoint/single_joint_generator.h index d5b567a1..4b7553a6 100644 --- a/include/trackjoint/single_joint_generator.h +++ b/include/trackjoint/single_joint_generator.h @@ -43,16 +43,17 @@ class SingleJointGenerator * Should be set lower than the accuracy requirements for your task * input use_streaming_mode set to true for fast streaming applications. Returns a maximum of num_waypoints_threshold * waypoints. + * input timestep_was_upsampled If upsampling happened (we are working with very few waypoints), do not adjust timestep */ SingleJointGenerator(double timestep, double max_duration, const KinematicState& current_joint_state, const KinematicState& goal_joint_state, const Limits& limits, size_t desired_num_waypoints, size_t num_waypoints_threshold, size_t max_num_waypoints_trajectory_mode, - const double position_tolerance, bool use_streaming_mode); + const double position_tolerance, bool use_streaming_mode, bool timestep_was_upsampled); /** \brief reset data members and prepare to generate a new trajectory */ void reset(double timestep, double max_duration, const KinematicState& current_joint_state, const KinematicState& goal_joint_state, const Limits& limits, size_t desired_num_waypoints, - const double position_tolerance, bool use_streaming_mode); + const double position_tolerance, bool use_streaming_mode, bool timestep_was_upsampled); /** \brief Generate a jerk-limited trajectory for this joint * diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index 973c4239..bad03d20 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -15,7 +15,7 @@ SingleJointGenerator::SingleJointGenerator(double timestep, double max_duration, const KinematicState& goal_joint_state, const Limits& limits, size_t desired_num_waypoints, size_t num_waypoints_threshold, size_t max_num_waypoints_trajectory_mode, const double position_tolerance, - bool use_streaming_mode) + bool use_streaming_mode, bool timestep_was_upsampled) : kNumWaypointsThreshold(num_waypoints_threshold) , kMaxNumWaypointsFullTrajectory(max_num_waypoints_trajectory_mode) , timestep_(timestep) @@ -29,8 +29,16 @@ SingleJointGenerator::SingleJointGenerator(double timestep, double max_duration, // Start with this estimate of the shortest possible duration // The shortest possible duration avoids oscillation, as much as possible // Desired duration cannot be less than one timestep - desired_duration_ = - std::max(timestep_, fabs((goal_joint_state.position - current_joint_state.position) / limits_.velocity_limit)); + if (!timestep_was_upsampled) + { + desired_duration_ = + std::max(timestep_, fabs((goal_joint_state.position - current_joint_state.position) / limits_.velocity_limit)); + } + // If upsampling was used, we don't want to mess with the timestep or duration minimization + else + { + desired_duration_ = (desired_num_waypoints - 1) * timestep; + } // Waypoint times nominal_times_ = Eigen::VectorXd::LinSpaced(desired_num_waypoints, 0, desired_duration_); @@ -38,7 +46,8 @@ SingleJointGenerator::SingleJointGenerator(double timestep, double max_duration, void SingleJointGenerator::reset(double timestep, double max_duration, const KinematicState& current_joint_state, const KinematicState& goal_joint_state, const Limits& limits, - size_t desired_num_waypoints, const double position_tolerance, bool use_streaming_mode) + size_t desired_num_waypoints, const double position_tolerance, bool use_streaming_mode, + bool timestep_was_upsampled) { timestep_ = timestep; max_duration_ = max_duration; @@ -51,8 +60,16 @@ void SingleJointGenerator::reset(double timestep, double max_duration, const Kin // Start with this estimate of the shortest possible duration // The shortest possible duration avoids oscillation, as much as possible // Desired duration cannot be less than one timestep - desired_duration_ = - std::max(timestep_, fabs((goal_joint_state.position - current_joint_state.position) / limits_.velocity_limit)); + if (!timestep_was_upsampled) + { + desired_duration_ = + std::max(timestep_, fabs((goal_joint_state.position - current_joint_state.position) / limits_.velocity_limit)); + } + // If upsampling was used, we don't want to mess with the timestep or duration minimization + else + { + desired_duration_ = (desired_num_waypoints - 1) * timestep; + } // Waypoint times nominal_times_ = Eigen::VectorXd::LinSpaced(desired_num_waypoints, 0, desired_duration_); diff --git a/src/trajectory_generator.cpp b/src/trajectory_generator.cpp index 19b9d012..25466184 100644 --- a/src/trajectory_generator.cpp +++ b/src/trajectory_generator.cpp @@ -29,12 +29,14 @@ TrajectoryGenerator::TrajectoryGenerator(uint num_dof, double timestep, double d upsample(); // Initialize a trajectory generator for each joint + bool timestep_was_upsampled = (upsample_rounds_ > 0) ? true : false; for (size_t joint = 0; joint < kNumDof; ++joint) { single_joint_generators_.push_back( SingleJointGenerator(upsampled_timestep_, max_duration_, current_joint_states[joint], goal_joint_states[joint], limits[joint], upsampled_num_waypoints_, kNumWaypointsThreshold, - kMaxNumWaypointsFullTrajectory, position_tolerance, use_streaming_mode_)); + kMaxNumWaypointsFullTrajectory, position_tolerance, use_streaming_mode_, + timestep_was_upsampled)); } } @@ -57,11 +59,12 @@ void TrajectoryGenerator::reset(double timestep, double desired_duration, double upsample(); // Initialize a trajectory generator for each joint + bool timestep_was_upsampled = (upsample_rounds_ > 0) ? true : false; for (size_t joint = 0; joint < kNumDof; ++joint) { single_joint_generators_[joint].reset(upsampled_timestep_, max_duration_, current_joint_states[joint], goal_joint_states[joint], limits[joint], upsampled_num_waypoints_, - position_tolerance, use_streaming_mode_); + position_tolerance, use_streaming_mode_, timestep_was_upsampled); } } From 7e4b4b52a73791b71b88f7ada56a22f3a305ad04 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 23 Aug 2020 11:19:04 -0500 Subject: [PATCH 31/38] Slightly tweak test conditions for tests that seem to be working well --- test/trajectory_generation_test.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/test/trajectory_generation_test.cpp b/test/trajectory_generation_test.cpp index f302971e..29ded143 100644 --- a/test/trajectory_generation_test.cpp +++ b/test/trajectory_generation_test.cpp @@ -432,7 +432,7 @@ TEST_F(TrajectoryGenerationTest, VelAccelJerkLimit) // Duration const double duration_tolerance = 5e-3; size_t vector_length = output_trajectories_[0].elapsed_times.size() - 1; - EXPECT_NEAR(output_trajectories_[0].elapsed_times(vector_length), desired_duration_, duration_tolerance); + EXPECT_LE(output_trajectories_[0].elapsed_times(vector_length), desired_duration_); } TEST_F(TrajectoryGenerationTest, NoisyStreamingCommand) @@ -759,7 +759,7 @@ TEST_F(TrajectoryGenerationTest, DurationExtension) EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); // Position error - const double position_tolerance = 1e-4; + const double position_tolerance = 5e-4; const double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); EXPECT_LT(position_error, position_tolerance); // Timestep @@ -767,10 +767,9 @@ TEST_F(TrajectoryGenerationTest, DurationExtension) EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep, timestep_tolerance); // Duration - const double expected_duration = 1.05; - const double duration_tolerance = 5e-3; + const double expected_duration = 0.865; size_t vector_length = output_trajectories_[0].elapsed_times.size() - 1; - EXPECT_NEAR(output_trajectories_[0].elapsed_times(vector_length), expected_duration, duration_tolerance); + EXPECT_LE(output_trajectories_[0].elapsed_times(vector_length), expected_duration); } TEST_F(TrajectoryGenerationTest, PositiveAndNegativeLimits) From 0dc5fe9627a2470dafec530b8ac7c6ca74d349de Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 23 Aug 2020 11:47:46 -0500 Subject: [PATCH 32/38] Fix the allocation of waypoints.elapsed_times, only 2 tests fail now --- src/single_joint_generator.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index bad03d20..9fd38032 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -81,7 +81,7 @@ ErrorCodeEnum SingleJointGenerator::generateTrajectory() waypoints_ = JointTrajectory(); waypoints_.positions = interpolate(nominal_times_); - waypoints_.elapsed_times.setLinSpaced(waypoints_.positions.size(), 0., desired_duration_); + waypoints_.elapsed_times.setLinSpaced(waypoints_.positions.size(), 0., (waypoints_.positions.size() - 1) * timestep_); calculateDerivativesFromPosition(); ErrorCodeEnum error_code = positionVectorLimitLookAhead(&index_last_successful_); @@ -116,7 +116,7 @@ void SingleJointGenerator::extendTrajectoryDuration() Spline1D spline(fit); // New times, with the extended duration - waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., desired_duration_); + waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., (new_num_waypoints - 1) * timestep_); // Retrieve new positions at the new times waypoints_.positions.resize(new_num_waypoints); @@ -132,7 +132,7 @@ void SingleJointGenerator::extendTrajectoryDuration() else { waypoints_ = JointTrajectory(); - waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., desired_duration_); + waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., (new_num_waypoints - 1) * timestep_); waypoints_.positions = interpolate(waypoints_.elapsed_times); calculateDerivativesFromPosition(); ErrorCodeEnum error_code = forwardLimitCompensation(&index_last_successful_); @@ -475,7 +475,7 @@ inline ErrorCodeEnum SingleJointGenerator::predictTimeToReach() if (new_num_waypoints > kMaxNumWaypointsFullTrajectory) new_num_waypoints = kMaxNumWaypointsFullTrajectory; - waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., desired_duration_); + waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., (new_num_waypoints - 1) * timestep_); waypoints_.positions.resize(waypoints_.elapsed_times.size()); waypoints_.velocities.resize(waypoints_.elapsed_times.size()); waypoints_.accelerations.resize(waypoints_.elapsed_times.size()); From a6f5a0c2d9bef732d5a7bf3fd5f1c50bbfb65cac Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 23 Aug 2020 11:49:55 -0500 Subject: [PATCH 33/38] Remove a few commented lines --- src/single_joint_generator.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index 9fd38032..17014c21 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -106,8 +106,6 @@ void SingleJointGenerator::extendTrajectoryDuration() { // Fit and generate a spline function to the original positions, same number of waypoints, new (extended) duration // This only decreases velocity/accel/jerk, so no worries re. limit violation - // Eigen::VectorXd new_times; - // new_times.setLinSpaced(waypoints_.elapsed_times.size(), 0, desired_duration_); Eigen::RowVectorXd new_times; new_times.setLinSpaced(waypoints_.elapsed_times.size(), 0, desired_duration_); Eigen::RowVectorXd position(waypoints_.positions); From bb0d42bf5c2a879e59d42f8823caccd4072a525f Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 23 Aug 2020 11:55:22 -0500 Subject: [PATCH 34/38] Fix a typo in LimitCompensation test conditions --- test/trajectory_generation_test.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/trajectory_generation_test.cpp b/test/trajectory_generation_test.cpp index 29ded143..2075b44a 100644 --- a/test/trajectory_generation_test.cpp +++ b/test/trajectory_generation_test.cpp @@ -710,8 +710,8 @@ TEST_F(TrajectoryGenerationTest, LimitCompensation) const double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); EXPECT_LT(position_error, position_tolerance); // Timestep - double timestep_tolerance = 0.1 * timestep_; - EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep_, + double timestep_tolerance = 0.1 * timestep; + EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep, timestep_tolerance); // Duration uint num_waypoint_tolerance = 1; From 875844a6256f8264abdd7599f3de529060e2b4e3 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 13 Sep 2020 12:40:16 -0500 Subject: [PATCH 35/38] Initialize index_last_successful_ --- src/single_joint_generator.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index 17014c21..703366f8 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -24,6 +24,7 @@ SingleJointGenerator::SingleJointGenerator(double timestep, double max_duration, , goal_joint_state_(goal_joint_state) , limits_(limits) , position_tolerance_(position_tolerance) + , index_last_successful_(0) , use_streaming_mode_(use_streaming_mode) { // Start with this estimate of the shortest possible duration @@ -55,6 +56,7 @@ void SingleJointGenerator::reset(double timestep, double max_duration, const Kin goal_joint_state_ = goal_joint_state; limits_ = limits; position_tolerance_ = position_tolerance; + index_last_successful_ = 0; use_streaming_mode_ = use_streaming_mode; // Start with this estimate of the shortest possible duration From a3236c6072203058157001350313fde3dbf41f87 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 13 Sep 2020 12:49:20 -0500 Subject: [PATCH 36/38] Tweak simple_example.cpp for better accuracy --- src/simple_example.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/simple_example.cpp b/src/simple_example.cpp index 17c0a663..237ce1fe 100644 --- a/src/simple_example.cpp +++ b/src/simple_example.cpp @@ -21,7 +21,7 @@ int main(int argc, char** argv) // This example is for just one degree of freedom constexpr int num_dof = 1; // Timestep. Units don't matter as long as they're consistent - constexpr double timestep = 0.004; + constexpr double timestep = 0.001; // TrackJoint is allowed to extend the trajectory up to this duration, if a solution at kDesiredDuration can't be // found constexpr double max_duration = 5; @@ -50,7 +50,7 @@ int main(int argc, char** argv) // Typically, jerk limit >> acceleration limit > velocity limit single_joint_limits.velocity_limit = 3.15; single_joint_limits.acceleration_limit = 5; - single_joint_limits.jerk_limit = 20000; + single_joint_limits.jerk_limit = 100; std::vector limits(1, single_joint_limits); // Estimate trajectory duration @@ -61,7 +61,7 @@ int main(int argc, char** argv) // This descibes how far TrackJoint can deviate from a smooth, interpolated polynomial. // It is used for calculations internally. It should be set to a smaller number than your task requires. - const double position_tolerance = 0.02; + const double position_tolerance = 0.0001; // Instantiate a trajectory generation object trackjoint::TrajectoryGenerator traj_gen(num_dof, timestep, desired_duration, max_duration, current_joint_states, From b006cbbd67a5863f222747c38cb5f8b00921332f Mon Sep 17 00:00:00 2001 From: John Morris Date: Sun, 20 Sep 2020 16:57:13 -0500 Subject: [PATCH 37/38] Rebase, clang format, clang tidy MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Clang tidy fixes (#53) * Clean up inline functions (#54) - Remove unnecessary inline tags - Move remaining inline function implementations into header * Update executable names in Readme (#56) * Run `clang-format` Command copied from `moveit_ci`: find . -name '*.h' -or -name '*.hpp' -or -name '*.cpp' | \ xargs /usr/bin/clang-format -i -style=file * Adjust clang-format rules and re-run Fix hideous string breaks pointed out by @AndyZe * single_joint_generator: Silence compiler warning [...]/src/single_joint_generator.cpp: In member function ‘void trackjoint::SingleJointGenerator::extendTrajectoryDuration()’: [...]/src/single_joint_generator.cpp:123:30: warning: comparison between signed and unsigned integer expressions [-Wsign-compare] for (size_t idx = 0; idx < waypoints_.elapsed_times.size(); ++idx) ~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ [...]/src/single_joint_generator.cpp:138:19: warning: unused variable ‘error_code’ [-Wunused-variable] ErrorCodeEnum error_code = forwardLimitCompensation(&index_last_successful_); ^~~~~~~~~~ * single_joint_generator: Silence compiler warning [...]/src/single_joint_generator.cpp: In member function ‘void trackjoint::SingleJointGenerator::extendTrajectoryDuration()’: [...]/src/single_joint_generator.cpp:123:30: error: comparison between signed and unsigned integer expressions [-Werror=sign-compare] for (size_t idx = 0; idx < waypoints_.elapsed_times.size(); ++idx) ~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ * test/trajectory_generation_test: Silence compiler warning [...]/test/trajectory_generation_test.cpp: In member function ‘virtual void trackjoint::TrajectoryGenerationTest_VelAccelJerkLimit_Test::TestBody()’: [...]/test/trajectory_generation_test.cpp:433:16: error: unused variable ‘duration_tolerance’ [-Werror=unused-variable] const double duration_tolerance = 5e-3; ^~~~~~~~~~~~~~~~~~ * Remove junk files * .gitignore generated `.catkin-tools` directory These files were removed in previous "Remove junk files" commit; @AndyZe suggested we keep them out permanently by `.gitignore`ing them. * Change `int` to `size_t` for vector indices in tests Remove unneeded `static_cast`; prepare for upcoming changes. * trajectory_generator.cpp: Fix `clang-tidy` errors [...]/src/trajectory_generator.cpp:32:58: warning: redundant boolean literal in ternary expression result [readability-simplify-boolean-expr] bool timestep_was_upsampled = (upsample_rounds_ > 0) ? true : false; ~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~ upsample_rounds_ > 0 [...]/src/trajectory_generator.cpp:61:58: warning: redundant boolean literal in ternary expression result [readability-simplify-boolean-expr] bool timestep_was_upsampled = (upsample_rounds_ > 0) ? true : false; ~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~ upsample_rounds_ > 0 * single_joint_generator.cpp: Fix clang-tidy errors [...]/src/single_joint_generator.cpp:116:14: warning: local copy 'spline' of the variable 'fit' is never modified; consider avoiding the copy [performance-unnecessary-copy-initialization] Spline1D spline(fit); ~~~~~~~~ ^ const & * Slence clang-tidy warnings [...]/src/single_joint_generator.cpp:279:5: warning: Value stored to 'delta_v' is never read [clang-analyzer-deadcode.DeadStores] delta_v = 0; ^ [...]/src/single_joint_generator.cpp:279:5: note: Value stored to 'delta_v' is never read 1 warning generated. * Silence clang-tidy warnings /usr/include/clang/6.0.0/include/emmintrin.h:1593:10: warning: Dereference of null pointer [clang-analyzer-core.NullDereference] return *(__m128d*)__dp; ^ [...]/test/trajectory_generation_test.cpp:1093:33: note: Calling 'TrajectoryGenerationTest::calculatePositionAccuracy' const double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); ^ [...]/test/trajectory_generation_test.cpp:146:28: note: Assuming the condition is false for (size_t joint = 0; joint < trajectory.size(); ++joint) ^ [...]/test/trajectory_generation_test.cpp:146:5: note: Loop condition is false. Execution continues on line 152 for (size_t joint = 0; joint < trajectory.size(); ++joint) ^ [...]/test/trajectory_generation_test.cpp:152:20: note: Calling 'MatrixBase::norm' double error = (final_positions - goal_positions).norm(); ^ /usr/include/eigen3/Eigen/src/Core/Dot.h:107:23: note: Calling 'MatrixBase::squaredNorm' return numext::sqrt(squaredNorm()); ^ /usr/include/eigen3/Eigen/src/Core/Dot.h:95:23: note: Calling 'DenseBase::sum' return numext::real((*this).cwiseAbs2().sum()); ^ /usr/include/eigen3/Eigen/src/Core/Redux.h:451:6: note: Left side of '||' is false if(SizeAtCompileTime==0 || (SizeAtCompileTime==Dynamic && size()==0)) ^ /usr/include/eigen3/Eigen/src/Core/Redux.h:451:31: note: Left side of '&&' is true if(SizeAtCompileTime==0 || (SizeAtCompileTime==Dynamic && size()==0)) ^ /usr/include/eigen3/Eigen/src/Core/Redux.h:451:3: note: Taking false branch if(SizeAtCompileTime==0 || (SizeAtCompileTime==Dynamic && size()==0)) ^ /usr/include/eigen3/Eigen/src/Core/Redux.h:453:10: note: Calling 'DenseBase::redux' return derived().redux(Eigen::internal::scalar_sum_op()); ^ /usr/include/eigen3/Eigen/src/Core/Redux.h:418:10: note: Calling 'redux_impl::run' return internal::redux_impl::run(thisEval, func); ^ /usr/include/eigen3/Eigen/src/Core/Redux.h:231:8: note: Assuming 'alignedSize' is not equal to 0 if(alignedSize) ^ /usr/include/eigen3/Eigen/src/Core/Redux.h:231:5: note: Taking true branch if(alignedSize) ^ /usr/include/eigen3/Eigen/src/Core/Redux.h:233:34: note: Calling 'redux_evaluator::packet' PacketScalar packet_res0 = mat.template packet(alignedStart); ^ /usr/include/eigen3/Eigen/src/Core/Redux.h:377:12: note: Calling 'unary_evaluator::packet' { return m_evaluator.template packet(index); } ^ /usr/include/eigen3/Eigen/src/Core/CoreEvaluators.h:563:31: note: Calling 'binary_evaluator::packet' return m_functor.packetOp(m_argImpl.template packet(index)); ^ /usr/include/eigen3/Eigen/src/Core/CoreEvaluators.h:734:31: note: Calling 'evaluator::packet' return m_functor.packetOp(m_lhsImpl.template packet(index), ^ /usr/include/eigen3/Eigen/src/Core/CoreEvaluators.h:204:41: note: Passing null pointer value via 1st parameter 'from' return ploadt(m_data + index); ^ /usr/include/eigen3/Eigen/src/Core/CoreEvaluators.h:204:12: note: Calling 'ploadt' return ploadt(m_data + index); ^ /usr/include/eigen3/Eigen/src/Core/GenericPacketMath.h:462:3: note: Taking true branch if(Alignment >= unpacket_traits::alignment) ^ /usr/include/eigen3/Eigen/src/Core/GenericPacketMath.h:463:26: note: Passing null pointer value via 1st parameter 'from' return pload(from); ^ /usr/include/eigen3/Eigen/src/Core/GenericPacketMath.h:463:12: note: Calling 'pload' return pload(from); ^ /usr/include/eigen3/Eigen/src/Core/arch/SSE/PacketMath.h:307:124: note: Passing null pointer value via 1st parameter '__dp' template<> EIGEN_STRONG_INLINE Packet2d pload(const double* from) { EIGEN_DEBUG_ALIGNED_LOAD return _mm_load_pd(from); } ^ /usr/include/eigen3/Eigen/src/Core/arch/SSE/PacketMath.h:307:112: note: Calling '_mm_load_pd' template<> EIGEN_STRONG_INLINE Packet2d pload(const double* from) { EIGEN_DEBUG_ALIGNED_LOAD return _mm_load_pd(from); } ^ /usr/include/clang/6.0.0/include/emmintrin.h:1593:10: note: Dereference of null pointer return *(__m128d*)__dp; ^ Co-authored-by: Nathan Brooks Co-authored-by: AndyZe --- .catkin_tools/CATKIN_IGNORE | 0 .catkin_tools/README | 13 ----- .catkin_tools/VERSION | 1 - .../profiles/default/devel_collisions.txt | 0 .clang-format | 2 +- .gitignore | 6 +- README.md | 2 +- include/trackjoint/error_codes.h | 47 +++++++-------- include/trackjoint/single_joint_generator.h | 16 +++-- include/trackjoint/trajectory_generator.h | 8 +-- include/trackjoint/utilities.h | 2 +- src/simple_example.cpp | 14 ++--- src/single_joint_generator.cpp | 40 +++++-------- src/streaming_example.cpp | 10 ++-- src/three_dof_examples.cpp | 14 ++--- src/trajectory_generator.cpp | 37 ++++++------ test/trajectory_generation_test.cpp | 58 ++++++++++--------- 17 files changed, 131 insertions(+), 139 deletions(-) delete mode 100644 .catkin_tools/CATKIN_IGNORE delete mode 100644 .catkin_tools/README delete mode 100644 .catkin_tools/VERSION delete mode 100644 .catkin_tools/profiles/default/devel_collisions.txt diff --git a/.catkin_tools/CATKIN_IGNORE b/.catkin_tools/CATKIN_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/.catkin_tools/README b/.catkin_tools/README deleted file mode 100644 index 4706f475..00000000 --- a/.catkin_tools/README +++ /dev/null @@ -1,13 +0,0 @@ -# Catkin Tools Metadata - -This directory was generated by catkin_tools and it contains persistent -configuration information used by the `catkin` command and its sub-commands. - -Each subdirectory of the `profiles` directory contains a set of persistent -configuration options for separate profiles. The default profile is called -`default`. If another profile is desired, it can be described in the -`profiles.yaml` file in this directory. - -Please see the catkin_tools documentation before editing any files in this -directory. Most actions can be performed with the `catkin` command-line -program. diff --git a/.catkin_tools/VERSION b/.catkin_tools/VERSION deleted file mode 100644 index c8a5397f..00000000 --- a/.catkin_tools/VERSION +++ /dev/null @@ -1 +0,0 @@ -0.4.5 \ No newline at end of file diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt deleted file mode 100644 index e69de29b..00000000 diff --git a/.clang-format b/.clang-format index eca6051a..c3c41084 100644 --- a/.clang-format +++ b/.clang-format @@ -10,7 +10,7 @@ AllowShortLoopsOnASingleLine: false AllowShortFunctionsOnASingleLine: None AllowShortLoopsOnASingleLine: false AlwaysBreakTemplateDeclarations: true -AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakBeforeMultilineStrings: true BreakBeforeBinaryOperators: false BreakBeforeTernaryOperators: false BreakConstructorInitializersBeforeComma: true diff --git a/.gitignore b/.gitignore index 967678e8..c9a672a7 100644 --- a/.gitignore +++ b/.gitignore @@ -18,4 +18,8 @@ .DS_Store # SSH Keys (used for Docker) -id_rsa \ No newline at end of file +id_rsa + +# Catkin tools +/.catkin-tools/ + diff --git a/README.md b/README.md index ed3493cb..c9f6c8f9 100644 --- a/README.md +++ b/README.md @@ -19,7 +19,7 @@ TODO(andyze): fix Travis badge: ## Run -1. rosrun trackjoint run_example +1. rosrun trackjoint simple_example (or streaming_example) (You don't need to start `roscore` because the executable doesn't use ROS) diff --git a/include/trackjoint/error_codes.h b/include/trackjoint/error_codes.h index 92e6227d..631fbfee 100644 --- a/include/trackjoint/error_codes.h +++ b/include/trackjoint/error_codes.h @@ -22,32 +22,33 @@ namespace trackjoint */ enum ErrorCodeEnum { - kNoError = 0, - kDesiredDurationTooShort = 1, - kMaxDurationExceeded = 2, - kVelocityExceedsLimit = 3, - kAccelExceedsLimit = 4, - kMaxDurationLessThanDesiredDuration = 5, - kLimitNotPositive = 6, - kGoalPositionMismatch = 7, - kFailureToGenerateSingleWaypoint = 8, - kLessThanTenTimestepsForStreamingMode = 9 + NO_ERROR = 0, + DESIRED_DURATION_TOO_SHORT = 1, + MAX_DURATION_EXCEEDED = 2, + VELOCITY_EXCEEDS_LIMIT = 3, + ACCEL_EXCEEDS_LIMIT = 4, + MAX_DURATION_LESS_THAN_DESIRED_DURATION = 5, + LIMIT_NOT_POSITIVE = 6, + GOAL_POSITION_MISMATCH = 7, + FAILURE_TO_GENERATE_SINGLE_WAYPOINT = 8, + LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE = 9 }; /** * \brief Use this map to look up human-readable strings for each error code */ -const std::unordered_map kErrorCodeMap( - { { kNoError, "No error, trajectory generation was successful" }, - { kDesiredDurationTooShort, "Desired duration is too short, cannot have less than one timestep in a " - "trajectory" }, - { kMaxDurationExceeded, "Max duration was exceeded" }, - { kVelocityExceedsLimit, "A velocity input exceeds the velocity limit" }, - { kAccelExceedsLimit, "An acceleration input exceeds the acceleration limit" }, - { kMaxDurationLessThanDesiredDuration, "max_duration should not be less than desired_duration" }, - { kLimitNotPositive, "Vel/accel/jerk limits should be greater than zero" }, - { kGoalPositionMismatch, "Mismatch between the final position and the goal position" }, - { kFailureToGenerateSingleWaypoint, "Failed to generate even a single new waypoint" }, - { kLessThanTenTimestepsForStreamingMode, "In streaming mode, desired duration should be at least 10 " - "timesteps" } }); +const std::unordered_map ERROR_CODE_MAP( + { { NO_ERROR, "No error, trajectory generation was successful" }, + { DESIRED_DURATION_TOO_SHORT, + "Desired duration is too short, cannot have less than one timestep in a " + "trajectory" }, + { MAX_DURATION_EXCEEDED, "Max duration was exceeded" }, + { VELOCITY_EXCEEDS_LIMIT, "A velocity input exceeds the velocity limit" }, + { ACCEL_EXCEEDS_LIMIT, "An acceleration input exceeds the acceleration limit" }, + { MAX_DURATION_LESS_THAN_DESIRED_DURATION, "max_duration should not be less than desired_duration" }, + { LIMIT_NOT_POSITIVE, "Vel/accel/jerk limits should be greater than zero" }, + { GOAL_POSITION_MISMATCH, "Mismatch between the final position and the goal position" }, + { FAILURE_TO_GENERATE_SINGLE_WAYPOINT, "Failed to generate even a single new waypoint" }, + { LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE, + "In streaming mode, desired duration should be at least 10 timesteps" } }); } // end namespace trackjoint diff --git a/include/trackjoint/single_joint_generator.h b/include/trackjoint/single_joint_generator.h index 4b7553a6..382484aa 100644 --- a/include/trackjoint/single_joint_generator.h +++ b/include/trackjoint/single_joint_generator.h @@ -43,7 +43,8 @@ class SingleJointGenerator * Should be set lower than the accuracy requirements for your task * input use_streaming_mode set to true for fast streaming applications. Returns a maximum of num_waypoints_threshold * waypoints. - * input timestep_was_upsampled If upsampling happened (we are working with very few waypoints), do not adjust timestep + * input timestep_was_upsampled If upsampling happened (we are working with very few waypoints), do not adjust + * timestep */ SingleJointGenerator(double timestep, double max_duration, const KinematicState& current_joint_state, const KinematicState& goal_joint_state, const Limits& limits, size_t desired_num_waypoints, @@ -83,6 +84,16 @@ class SingleJointGenerator void updateTrajectoryDuration(double new_trajectory_duration); private: + /** \brief Record the index when compensation first failed */ + inline void recordFailureTime(size_t current_index, size_t* index_last_successful) + { + // Record the index when compensation first failed + if (current_index < *index_last_successful) + { + *index_last_successful = current_index; + } + }; + /** \brief interpolate from start to end state with a polynomial * * input times a vector of waypoint times. @@ -101,9 +112,6 @@ class SingleJointGenerator * vector */ ErrorCodeEnum positionVectorLimitLookAhead(size_t* index_last_successful); - /** \brief Record the index when compensation first failed */ - inline void recordFailureTime(size_t current_index, size_t* index_last_successful); - /** \brief Check whether the duration needs to be extended, and do it */ ErrorCodeEnum predictTimeToReach(); diff --git a/include/trackjoint/trajectory_generator.h b/include/trackjoint/trajectory_generator.h index 4e4dc14a..f2a97781 100644 --- a/include/trackjoint/trajectory_generator.h +++ b/include/trackjoint/trajectory_generator.h @@ -78,7 +78,7 @@ class TrajectoryGenerator * input limits vector of kinematic limits for each degree of freedom * input nominal_timestep the user-requested time between waypoints * returna TrackJoint status code - */ + */ ErrorCodeEnum inputChecking(const std::vector& current_joint_states, const std::vector& goal_joint_states, const std::vector& limits, double nominal_timestep); @@ -87,7 +87,7 @@ class TrajectoryGenerator /** \brief Ensure limits are obeyed before outputting. * * input trajectory the calculated trajectories for n joints - */ + */ void clipVectorsForOutput(std::vector* trajectory); /** \brief upsample if num. waypoints would be short. Helps with accuracy. */ @@ -100,7 +100,7 @@ class TrajectoryGenerator * input velocity_vector a vector of velocities * input acceleration_vector a vector of accelerations * input jerk_vector a vector of jerks - */ + */ void downSample(Eigen::VectorXd* time_vector, Eigen::VectorXd* position_vector, Eigen::VectorXd* velocity_vector, Eigen::VectorXd* acceleration_vector, Eigen::VectorXd* jerk_vector); @@ -108,7 +108,7 @@ class TrajectoryGenerator * * input output_trajectories the calculated trajectories for n joints * returna TrackJoint status code - */ + */ ErrorCodeEnum synchronizeTrajComponents(std::vector* output_trajectories); // TODO(andyz): set this back to a small number when done testing diff --git a/include/trackjoint/utilities.h b/include/trackjoint/utilities.h index 0357c42e..dd7dfaac 100644 --- a/include/trackjoint/utilities.h +++ b/include/trackjoint/utilities.h @@ -35,7 +35,7 @@ Eigen::VectorXd DiscreteDifferentiation(const Eigen::VectorXd& input_vector, dou * input joint the index of a joint * input output_trajectories the calculated trajectories for n joints * input desired_duration the user-requested duration of the trajectory -*/ + */ void PrintJointTrajectory(const std::size_t joint, const std::vector& output_trajectories, const double desired_duration); diff --git a/src/simple_example.cpp b/src/simple_example.cpp index 237ce1fe..76eaa556 100644 --- a/src/simple_example.cpp +++ b/src/simple_example.cpp @@ -29,8 +29,8 @@ int main(int argc, char** argv) // the whole trajectory at once. constexpr bool use_streaming_mode = false; // Optional logging of TrackJoint output - const std::string output_path_base = "/home/" + std::string(getenv("USER")) + "/Downloads/trackjoint_data/" - "output_joint"; + const std::string output_path_base = + "/home/" + std::string(getenv("USER")) + "/Downloads/trackjoint_data/output_joint"; std::vector current_joint_states(1); trackjoint::KinematicState joint_state; @@ -73,9 +73,9 @@ int main(int argc, char** argv) traj_gen.inputChecking(current_joint_states, goal_joint_states, limits, timestep); // Input error handling - if an error is found, the trajectory is not generated. - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } @@ -86,9 +86,9 @@ int main(int argc, char** argv) auto end = std::chrono::system_clock::now(); // Trajectory generation error handling - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } @@ -96,7 +96,7 @@ int main(int argc, char** argv) std::cout << "Runtime: " << elapsed_seconds.count() << std::endl; std::cout << "Num waypoints: " << output_trajectories.at(0).positions.size() << std::endl; - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; // Save the synchronized trajectories to .csv files traj_gen.saveTrajectoriesToFile(output_trajectories, output_path_base); diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index 703366f8..87f4876c 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -113,15 +113,14 @@ void SingleJointGenerator::extendTrajectoryDuration() Eigen::RowVectorXd position(waypoints_.positions); const auto fit = SplineFitting1D::Interpolate(position, 2, new_times); - Spline1D spline(fit); // New times, with the extended duration waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., (new_num_waypoints - 1) * timestep_); // Retrieve new positions at the new times waypoints_.positions.resize(new_num_waypoints); - for (size_t idx = 0; idx < waypoints_.elapsed_times.size(); ++idx) - waypoints_.positions[idx] = spline(waypoints_.elapsed_times(idx)).coeff(0); + for (Eigen::Index idx = 0; idx < waypoints_.elapsed_times.size(); ++idx) + waypoints_.positions[idx] = fit(waypoints_.elapsed_times(idx)).coeff(0); calculateDerivativesFromPosition(); return; @@ -135,7 +134,7 @@ void SingleJointGenerator::extendTrajectoryDuration() waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., (new_num_waypoints - 1) * timestep_); waypoints_.positions = interpolate(waypoints_.elapsed_times); calculateDerivativesFromPosition(); - ErrorCodeEnum error_code = forwardLimitCompensation(&index_last_successful_); + forwardLimitCompensation(&index_last_successful_); } return; @@ -151,7 +150,7 @@ size_t SingleJointGenerator::getLastSuccessfulIndex() return index_last_successful_; } -inline Eigen::VectorXd SingleJointGenerator::interpolate(Eigen::VectorXd& times) +Eigen::VectorXd SingleJointGenerator::interpolate(Eigen::VectorXd& times) { // See De Luca, "Trajectory Planning" pdf, slide 19 // Interpolate a smooth trajectory from initial to final state while matching @@ -182,7 +181,7 @@ inline Eigen::VectorXd SingleJointGenerator::interpolate(Eigen::VectorXd& times) return interpolated_position; } -inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_successful) +ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_successful) { // This is the indexing convention. // 1. accel(i) = accel(i-1) + jerk(i) * dt @@ -294,8 +293,6 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde position_error = 0; for (size_t index = 1; index < *index_last_successful; ++index) { - delta_v = 0; - // If the velocity limit would be exceeded if (fabs(waypoints_.velocities(index)) > velocity_limit) { @@ -329,19 +326,10 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde // Re-calculate derivatives from the updated velocity vector calculateDerivativesFromVelocity(); - return ErrorCodeEnum::kNoError; -} - -inline void SingleJointGenerator::recordFailureTime(size_t current_index, size_t* index_last_successful) -{ - // Record the index when compensation first failed - if (current_index < *index_last_successful) - { - *index_last_successful = current_index; - } + return ErrorCodeEnum::NO_ERROR; } -inline bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, double excess_velocity) +bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, double excess_velocity) { // The algorithm: // 1) check jerk limits, from beginning to end of trajectory. Don't bother @@ -445,13 +433,13 @@ inline bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index return successful_compensation; } -inline ErrorCodeEnum SingleJointGenerator::predictTimeToReach() +ErrorCodeEnum SingleJointGenerator::predictTimeToReach() { // Take a trajectory that could not reach the desired position in time. // Try increasing the duration until it is interpolated without violating limits. // This gives a new duration estimate. - ErrorCodeEnum error_code = ErrorCodeEnum::kNoError; + ErrorCodeEnum error_code = ErrorCodeEnum::NO_ERROR; // If in normal mode, we can extend trajectories if (!use_streaming_mode_) @@ -528,18 +516,18 @@ inline ErrorCodeEnum SingleJointGenerator::predictTimeToReach() // Normal mode: Error if we extended the duration to the maximum and it still wasn't successful if (!use_streaming_mode_ && index_last_successful_ < static_cast(waypoints_.elapsed_times.size() - 1)) { - error_code = ErrorCodeEnum::kMaxDurationExceeded; + error_code = ErrorCodeEnum::MAX_DURATION_EXCEEDED; } // Error if not even a single waypoint could be generated if (waypoints_.positions.size() < 2) { - error_code = ErrorCodeEnum::kFailureToGenerateSingleWaypoint; + error_code = ErrorCodeEnum::FAILURE_TO_GENERATE_SINGLE_WAYPOINT; } return error_code; } -inline ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* index_last_successful) +ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* index_last_successful) { ErrorCodeEnum error_code = forwardLimitCompensation(index_last_successful); if (error_code) @@ -570,7 +558,7 @@ inline ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* return error_code; } -inline void SingleJointGenerator::calculateDerivativesFromPosition() +void SingleJointGenerator::calculateDerivativesFromPosition() { // From position vector, approximate vel/accel/jerk. waypoints_.velocities = DiscreteDifferentiation(waypoints_.positions, timestep_, current_joint_state_.velocity); @@ -579,7 +567,7 @@ inline void SingleJointGenerator::calculateDerivativesFromPosition() waypoints_.jerks = DiscreteDifferentiation(waypoints_.accelerations, timestep_, 0); } -inline void SingleJointGenerator::calculateDerivativesFromVelocity() +void SingleJointGenerator::calculateDerivativesFromVelocity() { // From velocity vector, approximate accel/jerk. waypoints_.accelerations = diff --git a/src/streaming_example.cpp b/src/streaming_example.cpp index fe41820e..c8d72642 100644 --- a/src/streaming_example.cpp +++ b/src/streaming_example.cpp @@ -70,15 +70,15 @@ int main(int argc, char** argv) trackjoint::ErrorCodeEnum error_code = traj_gen.inputChecking(start_state, goal_joint_states, limits, timestep); if (error_code) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } // Generate the initial trajectory error_code = traj_gen.generateTrajectories(&output_trajectories); - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } std::cout << "Initial trajectory calculation:" << std::endl; @@ -110,9 +110,9 @@ int main(int argc, char** argv) std::cout << "Run time (microseconds): " << std::chrono::duration_cast(end_time - start_time).count() << std::endl; - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } diff --git a/src/three_dof_examples.cpp b/src/three_dof_examples.cpp index 796329ad..7e8ef65e 100644 --- a/src/three_dof_examples.cpp +++ b/src/three_dof_examples.cpp @@ -25,8 +25,8 @@ int main(int argc, char** argv) constexpr bool use_streaming_mode = false; // Position tolerance for each waypoint constexpr double waypoint_position_tolerance = 1e-4; - const std::string output_path_base = "/home/" + std::string(getenv("USER")) + "/Downloads/trackjoint_data/" - "output_joint"; + const std::string output_path_base = + "/home/" + std::string(getenv("USER")) + "/Downloads/trackjoint_data/output_joint"; std::vector current_joint_states(3); trackjoint::KinematicState joint_state; @@ -80,9 +80,9 @@ int main(int argc, char** argv) // Input error handling - if an error is found, the trajectory is not // generated. - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } @@ -92,9 +92,9 @@ int main(int argc, char** argv) auto end = std::chrono::system_clock::now(); // Trajectory generation error handling - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } @@ -102,7 +102,7 @@ int main(int argc, char** argv) std::cout << "Runtime: " << elapsed_seconds.count() << std::endl; std::cout << "Num waypoints: " << output_trajectories.at(0).positions.size() << std::endl; - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; // Save the synchronized trajectories to .csv files traj_gen.saveTrajectoriesToFile(output_trajectories, output_path_base); diff --git a/src/trajectory_generator.cpp b/src/trajectory_generator.cpp index 25466184..cd5a7b69 100644 --- a/src/trajectory_generator.cpp +++ b/src/trajectory_generator.cpp @@ -29,14 +29,13 @@ TrajectoryGenerator::TrajectoryGenerator(uint num_dof, double timestep, double d upsample(); // Initialize a trajectory generator for each joint - bool timestep_was_upsampled = (upsample_rounds_ > 0) ? true : false; + bool timestep_was_upsampled = upsample_rounds_ > 0; for (size_t joint = 0; joint < kNumDof; ++joint) { - single_joint_generators_.push_back( - SingleJointGenerator(upsampled_timestep_, max_duration_, current_joint_states[joint], goal_joint_states[joint], - limits[joint], upsampled_num_waypoints_, kNumWaypointsThreshold, - kMaxNumWaypointsFullTrajectory, position_tolerance, use_streaming_mode_, - timestep_was_upsampled)); + single_joint_generators_.push_back(SingleJointGenerator( + upsampled_timestep_, max_duration_, current_joint_states[joint], goal_joint_states[joint], limits[joint], + upsampled_num_waypoints_, kNumWaypointsThreshold, kMaxNumWaypointsFullTrajectory, position_tolerance, + use_streaming_mode_, timestep_was_upsampled)); } } @@ -59,7 +58,7 @@ void TrajectoryGenerator::reset(double timestep, double desired_duration, double upsample(); // Initialize a trajectory generator for each joint - bool timestep_was_upsampled = (upsample_rounds_ > 0) ? true : false; + bool timestep_was_upsampled = upsample_rounds_ > 0; for (size_t joint = 0; joint < kNumDof; ++joint) { single_joint_generators_[joint].reset(upsampled_timestep_, max_duration_, current_joint_states[joint], @@ -203,13 +202,13 @@ ErrorCodeEnum TrajectoryGenerator::inputChecking(const std::vector limits[joint].velocity_limit) { - return ErrorCodeEnum::kVelocityExceedsLimit; + return ErrorCodeEnum::VELOCITY_EXCEEDS_LIMIT; } // Check that goal vels. are less than the limits. if (abs(goal_joint_states[joint].velocity) > limits[joint].velocity_limit) { - return ErrorCodeEnum::kVelocityExceedsLimit; + return ErrorCodeEnum::VELOCITY_EXCEEDS_LIMIT; } // Check that current accels. are less than the limits. if (abs(current_joint_states[joint].acceleration) > limits[joint].acceleration_limit) { - return ErrorCodeEnum::kAccelExceedsLimit; + return ErrorCodeEnum::ACCEL_EXCEEDS_LIMIT; } // Check that goal accels. are less than the limits. if (abs(goal_joint_states[joint].acceleration) > limits[joint].acceleration_limit) { - return ErrorCodeEnum::kAccelExceedsLimit; + return ErrorCodeEnum::ACCEL_EXCEEDS_LIMIT; } // Check for positive limits. if (limits[joint].velocity_limit <= 0 || limits[joint].acceleration_limit <= 0 || limits[joint].jerk_limit <= 0) { - return ErrorCodeEnum::kLimitNotPositive; + return ErrorCodeEnum::LIMIT_NOT_POSITIVE; } // In streaming mode, the user-requested duration should be >= kNumWaypointsThreshold * timestep. // upsample and downSample aren't used in streaming mode. if (rounded_duration < kNumWaypointsThreshold * nominal_timestep && use_streaming_mode_) { - return ErrorCodeEnum::kLessThanTenTimestepsForStreamingMode; + return ErrorCodeEnum::LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE; } } - return ErrorCodeEnum::kNoError; + return ErrorCodeEnum::NO_ERROR; } void TrajectoryGenerator::saveTrajectoriesToFile(const std::vector& output_trajectories, @@ -318,7 +317,7 @@ ErrorCodeEnum TrajectoryGenerator::synchronizeTrajComponents(std::vector* trajectory) @@ -403,7 +402,7 @@ void TrajectoryGenerator::clipVectorsForOutput(std::vector* tra ErrorCodeEnum TrajectoryGenerator::generateTrajectories(std::vector* output_trajectories) { - ErrorCodeEnum error_code = ErrorCodeEnum::kNoError; + ErrorCodeEnum error_code = ErrorCodeEnum::NO_ERROR; // Generate individual joint trajectories for (size_t joint = 0; joint < kNumDof; ++joint) { diff --git a/test/trajectory_generation_test.cpp b/test/trajectory_generation_test.cpp index 2075b44a..95fb0d8c 100644 --- a/test/trajectory_generation_test.cpp +++ b/test/trajectory_generation_test.cpp @@ -67,7 +67,7 @@ class TrajectoryGenerationTest : public ::testing::Test // Default test parameters for 3 joints double timestep_ = 0.01; double desired_duration_ = 1; - int num_dof_ = 3; + size_t num_dof_ = 3; double max_duration_ = 10; std::vector current_joint_states_, goal_joint_states_; std::vector limits_; @@ -78,7 +78,7 @@ class TrajectoryGenerationTest : public ::testing::Test void checkBounds() { - for (int i = 0; i < static_cast(output_trajectories_.size()); ++i) + for (size_t i = 0; i < output_trajectories_.size(); ++i) { if (output_trajectories_[i].elapsed_times.size() == 0) { @@ -148,7 +148,13 @@ class TrajectoryGenerationTest : public ::testing::Test final_positions(joint) = trajectory.at(joint).positions((trajectory.at(joint).positions.size() - 1)); } - double error = (final_positions - goal_positions).norm(); + // Make clang-tidy happy about taking the norm() of a zero-length + // vector + Eigen::VectorXd errors = final_positions - goal_positions; + if (errors.size() < 1) + return 0; + + double error = (errors).norm(); return error; } @@ -205,7 +211,7 @@ TEST_F(TrajectoryGenerationTest, EasyDefaultTrajectory) TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration_, max_duration_, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 1e-4; @@ -252,7 +258,7 @@ TEST_F(TrajectoryGenerationTest, OneTimestepDuration) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error double position_tolerance = 1e-4; @@ -286,7 +292,7 @@ TEST_F(TrajectoryGenerationTest, RoughlyTwoTimestepDuration) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 1e-4; @@ -322,7 +328,7 @@ TEST_F(TrajectoryGenerationTest, FourTimestepDuration) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 1e-4; @@ -373,7 +379,7 @@ TEST_F(TrajectoryGenerationTest, SixTimestepDuration) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error double position_tolerance = 1e-4; @@ -417,7 +423,7 @@ TEST_F(TrajectoryGenerationTest, VelAccelJerkLimit) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); verifyVelAccelJerkLimits(output_trajectories_, limits_); @@ -430,7 +436,6 @@ TEST_F(TrajectoryGenerationTest, VelAccelJerkLimit) EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep_, timestep_tolerance); // Duration - const double duration_tolerance = 5e-3; size_t vector_length = output_trajectories_[0].elapsed_times.size() - 1; EXPECT_LE(output_trajectories_[0].elapsed_times(vector_length), desired_duration_); } @@ -552,8 +557,9 @@ TEST_F(TrajectoryGenerationTest, OscillatingUR5TrackJointCase) // Reading MoveIt experimental data from .txt files moveit_des_positions = loadWaypointsFromFile(REF_PATH + "/test/data/30_percent_speed_oscillation/moveit_des_pos.txt"); - moveit_des_velocities = - loadWaypointsFromFile(REF_PATH + "/test/data/30_percent_speed_oscillation/moveit_des_vel.txt"); + moveit_des_velocities = loadWaypointsFromFile(REF_PATH + + "/test/data/30_percent_speed_oscillation/" + "moveit_des_vel.txt"); moveit_des_accelerations = loadWaypointsFromFile(REF_PATH + "/test/data/30_percent_speed_oscillation/moveit_des_acc.txt"); moveit_times_from_start = @@ -566,7 +572,7 @@ TEST_F(TrajectoryGenerationTest, OscillatingUR5TrackJointCase) goal_joint_states_.clear(); // for each joint - for (int joint = 0; joint < num_dof_; ++joint) + for (size_t joint = 0; joint < num_dof_; ++joint) { // Save the start state of the robot joint_state.position = moveit_des_positions[point][joint]; @@ -605,7 +611,7 @@ TEST_F(TrajectoryGenerationTest, OscillatingUR5TrackJointCase) // Saving Trackjoint output to .csv files for plotting traj_gen.saveTrajectoriesToFile(output_trajectories_, BASE_FILEPATH + "_joint"); - EXPECT_EQ(ErrorCodeEnum::kNoError, error_code); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, error_code); // Timestep const double timestep_tolerance = 0.25 * timestep_; EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep_, @@ -653,7 +659,7 @@ TEST_F(TrajectoryGenerationTest, SuddenChangeOfDirection) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error double position_tolerance = 1e-4; @@ -703,7 +709,7 @@ TEST_F(TrajectoryGenerationTest, LimitCompensation) TrajectoryGenerator traj_gen(num_dof_, timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 1e-4; @@ -756,7 +762,7 @@ TEST_F(TrajectoryGenerationTest, DurationExtension) TrajectoryGenerator traj_gen(num_dof_, timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 5e-4; @@ -814,7 +820,7 @@ TEST_F(TrajectoryGenerationTest, PositiveAndNegativeLimits) TrajectoryGenerator traj_gen(num_dof_, timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 1e-4; @@ -862,9 +868,9 @@ TEST_F(TrajectoryGenerationTest, TimestepDidNotMatch) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); output_trajectories_.resize(num_dof_); - EXPECT_EQ(ErrorCodeEnum::kNoError, + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.inputChecking(current_joint_states_, goal_joint_states_, limits_, timestep_)); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 2e-3; @@ -923,7 +929,7 @@ TEST_F(TrajectoryGenerationTest, CustomerStreaming) TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration, max_duration_, current_joint_states_, goal_joint_states_, limits_, waypoint_position_tolerance, use_streaming_mode_); ErrorCodeEnum error_code = traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(error_code, ErrorCodeEnum::kNoError); + EXPECT_EQ(error_code, ErrorCodeEnum::NO_ERROR); double position_error = std::numeric_limits::max(); double velocity_error = std::numeric_limits::max(); @@ -935,7 +941,7 @@ TEST_F(TrajectoryGenerationTest, CustomerStreaming) traj_gen.reset(timestep_, desired_duration, max_duration_, current_joint_states_, goal_joint_states_, limits_, waypoint_position_tolerance, use_streaming_mode_); error_code = traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(error_code, ErrorCodeEnum::kNoError); + EXPECT_EQ(error_code, ErrorCodeEnum::NO_ERROR); // Get a new seed state for next trajectory generation if ((std::size_t)output_trajectories_.at(joint_to_update).positions.size() > next_waypoint) { @@ -970,7 +976,7 @@ TEST_F(TrajectoryGenerationTest, StreamingTooFewTimesteps) TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration_, max_duration_, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); - EXPECT_EQ(ErrorCodeEnum::kLessThanTenTimestepsForStreamingMode, + EXPECT_EQ(ErrorCodeEnum::LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE, traj_gen.inputChecking(current_joint_states_, goal_joint_states_, limits_, timestep_)); } @@ -1007,9 +1013,9 @@ TEST_F(TrajectoryGenerationTest, SingleJointOscillation) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); output_trajectories_.resize(num_dof_); - EXPECT_EQ(ErrorCodeEnum::kNoError, + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.inputChecking(current_joint_states_, goal_joint_states_, limits_, timestep_)); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 2e-3; @@ -1029,4 +1035,4 @@ int main(int argc, char** argv) int result = RUN_ALL_TESTS(); return result; -} \ No newline at end of file +} From ff00dbc9eccc90a3178a7850f412f9eb5ef40eb8 Mon Sep 17 00:00:00 2001 From: John Morris Date: Mon, 21 Sep 2020 12:36:40 -0500 Subject: [PATCH 38/38] Add example programs to tests In PR #62, I broke the examples, which luckily @AndyZe found by manually testing. Broken examples should be tested automatically. --- CMakeLists.txt | 1 + test/test_examples.py | 32 ++++++++++++++++++++++++++++++++ test/test_examples.test | 5 +++++ 3 files changed, 38 insertions(+) create mode 100755 test/test_examples.py create mode 100644 test/test_examples.test diff --git a/CMakeLists.txt b/CMakeLists.txt index 042200cf..fa80d4ba 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -155,6 +155,7 @@ if(CATKIN_ENABLE_TESTING) ${catkin_LIBRARIES} ) + add_rostest(test/test_examples.test) endif() ## Test for correct C++ source code diff --git a/test/test_examples.py b/test/test_examples.py new file mode 100755 index 00000000..a2db5fa7 --- /dev/null +++ b/test/test_examples.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python +PKG = 'trackjoint' +import subprocess +import unittest +import roslib + +roslib.load_manifest(PKG) # This line is not needed with Catkin. + +class TestExamples(unittest.TestCase): + + def run_cmd(self, *cmd): + rosrun_cmd = ["rosrun", "trackjoint"] + list(cmd) + print("Command: %s" % ' '.join(rosrun_cmd)) + try: + p = subprocess.Popen(rosrun_cmd) + outs, errs = p.communicate() + except Exception as e: + self.fail("Command failed: %s" % str(e)) + self.assertEqual(p.returncode, 0) + + def test_simple_example(self): + self.run_cmd("simple_example") + + def test_streaming_example(self): + self.run_cmd("streaming_example") + + def test_three_dof_examples(self): + self.run_cmd("three_dof_examples") + +if __name__ == '__main__': + import rostest + rostest.rosrun(PKG, 'test_examples', TestExamples) diff --git a/test/test_examples.test b/test/test_examples.test new file mode 100644 index 00000000..844efd15 --- /dev/null +++ b/test/test_examples.test @@ -0,0 +1,5 @@ + + + +