From 42f87ed0b8625beff460a410730ae27352f1a63a Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 22 Apr 2020 11:55:38 -0500 Subject: [PATCH 01/44] 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 f0f1f79a..b76b2ed5 100644 --- a/include/trackjoint/single_joint_generator.h +++ b/include/trackjoint/single_joint_generator.h @@ -115,7 +115,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 4be76d52..3fa0a374 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 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ 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 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ 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 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ 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. @@ -384,8 +384,7 @@ bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, doubl 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::NO_ERROR; @@ -422,7 +421,7 @@ 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_); } } @@ -500,17 +499,23 @@ ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* index_l return error_code; } -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 5bde2e57..8cf1e1d3 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 911102fbe8c6568f83fa35b57c7ea2e79d5a6568 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 22 Apr 2020 15:38:23 -0500 Subject: [PATCH 02/44] Fix sign of delta_v to backwardLimitComp() --- include/trackjoint/single_joint_generator.h | 2 +- src/single_joint_generator.cpp | 27 ++++++++++++++------- src/trajectory_generator.cpp | 2 +- 3 files changed, 20 insertions(+), 11 deletions(-) diff --git a/include/trackjoint/single_joint_generator.h b/include/trackjoint/single_joint_generator.h index b76b2ed5..6156bf67 100644 --- a/include/trackjoint/single_joint_generator.h +++ b/include/trackjoint/single_joint_generator.h @@ -105,7 +105,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 3fa0a374..20da9297 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -167,7 +167,7 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ 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 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ // 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 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ // 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_; @@ -276,7 +276,16 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ return ErrorCodeEnum::NO_ERROR; } -bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, double* excess_velocity) +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; + } +} + +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 @@ -297,10 +306,10 @@ bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, doubl 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 = @@ -339,7 +348,7 @@ bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, doubl { // 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 = @@ -369,7 +378,7 @@ bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, doubl 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 728fd8f8..9202e76e 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/44] 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 5b85f7d9..8417a181 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 8cf1e1d3..fb70eab9 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 0cfe5576f2c17074b13d352010ef6159be2adcf4 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 22 Apr 2020 13:27:22 -0500 Subject: [PATCH 04/44] 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 9202e76e..6a9358e4 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 c974284d670020ef04ce7c965d02448ceaa3fe54 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 4 May 2020 11:24:51 -0500 Subject: [PATCH 05/44] 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 fb70eab9..12603749 100644 --- a/test/trajectory_generation_test.cpp +++ b/test/trajectory_generation_test.cpp @@ -942,6 +942,53 @@ TEST_F(TrajectoryGenerationTest, StreamingTooFewTimesteps) EXPECT_EQ(ErrorCodeEnum::LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE, 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 1e7c9f0af8135aa0b433e9bd5debfee94d4821d2 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 4 May 2020 17:17:40 -0500 Subject: [PATCH 06/44] 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 6156bf67..d119dd08 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 20da9297..14e4dc5f 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 56cb26c2..0f2a58bc 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 a6e8b58434e4f170be6430a25b9242d7529ef7d0 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 5 May 2020 15:59:43 -0500 Subject: [PATCH 07/44] 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 14e4dc5f..b26c1c44 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 93a75668bbf7e2f3da37d09a120f9a75921ad449 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 6 May 2020 15:00:57 -0500 Subject: [PATCH 08/44] 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 b26c1c44..503d2d31 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 bd03348541c73dfb5cef098e4e8ed8b131f20bb3 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 6 May 2020 17:16:31 -0500 Subject: [PATCH 09/44] 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 503d2d31..be17f709 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 12603749..eea5cb5c 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 b9ee3ddac4d64cf6aa5f4598ed20911eae746b23 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 6 May 2020 17:25:20 -0500 Subject: [PATCH 10/44] 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 eea5cb5c..12603749 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 6ba0ce19b60b89bf7bdf88f838ec9ab20bda8c6d Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 29 Jun 2020 16:42:42 -0500 Subject: [PATCH 11/44] 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 d119dd08..6012e5ba 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 be17f709..11701b69 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 6a9358e4..3a6f2ede 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 d2d06d8de1a84575172134029f73e1ca0f8aab3b Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 29 Jun 2020 17:06:39 -0500 Subject: [PATCH 12/44] 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 11701b69..4da928ff 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 c60e557bc303006de0f0a9e40bbc17d1172e6c5b Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 29 Jun 2020 21:36:02 -0500 Subject: [PATCH 13/44] 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 63795669..60aeeda5 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 3a6f2ede..681d2742 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/44] 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 60aeeda5..aa4b1ac3 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 4da928ff..e04db096 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 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ } 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 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ } 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 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ 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 @@ 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 @@ ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* index_l 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 681d2742..3a6f2ede 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/44] 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 aa4b1ac3..8977d22b 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 e04db096..fa9bad15 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -455,7 +455,7 @@ 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 2c916dd786bf4b1839a35ed16210ccdf832466c6 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 29 Jun 2020 22:26:22 -0500 Subject: [PATCH 16/44] 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 8977d22b..5052cf97 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 dd0d065c4ee3b39e6cfce9cf165d085dfd87c1c3 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 30 Jun 2020 09:37:48 -0500 Subject: [PATCH 17/44] 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 fa9bad15..e215ae5e 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 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ } 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 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ } 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 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ 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 @@ 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 @@ ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* index_l 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 @@ ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* index_l 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 d709306bb47351a8941385b87808c2c52412c767 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 30 Jun 2020 10:39:05 -0500 Subject: [PATCH 18/44] 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 0f2a58bc..56cb26c2 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 68d623ca876e1485033c7a7926ba42c1b9ad22ae Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 30 Jun 2020 10:44:25 -0500 Subject: [PATCH 19/44] 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 8417a181..c8d72642 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 b63e02cee0eb5cc6bbfcede986158809c4092a7b Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 5 Jul 2020 11:55:31 -0500 Subject: [PATCH 20/44] 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 e215ae5e..4accddd9 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 c8d4928568750102ba9b749c29c2fe76ccfc573a Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 5 Jul 2020 14:00:55 -0500 Subject: [PATCH 21/44] 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 3a6f2ede..b9ef1a9d 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 a7c0c5796ff2d2da0c1c1532ebfb31db1131a297 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 5 Jul 2020 14:02:19 -0500 Subject: [PATCH 22/44] Change three_dof_examples to something that needs debugging Third joint acceleration definitely needs some work --- src/three_dof_examples.cpp | 48 +++++++++++++++++++------------------- 1 file changed, 24 insertions(+), 24 deletions(-) diff --git a/src/three_dof_examples.cpp b/src/three_dof_examples.cpp index 56cb26c2..a0c53d81 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 From 440ee3f855597ebecb986969fa42a4ffde445c6f Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 6 Jul 2020 08:13:10 -0500 Subject: [PATCH 23/44] 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 4accddd9..f7ce1f84 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 b9ef1a9d..b174ba85 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 ca03c4e2a4eddd80f3de518618e635da5fdead33 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 10 Jul 2020 12:27:51 -0500 Subject: [PATCH 24/44] 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 5052cf97..2ad1eb8a 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 400656cf59015fb00572a4e17724b8b53930b389 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 10 Jul 2020 13:45:37 -0500 Subject: [PATCH 25/44] Derivative calculations in the middle of LimitComp often caused limits to be exceeded --- src/simple_example.cpp | 1 + src/single_joint_generator.cpp | 18 +++++------------- 2 files changed, 6 insertions(+), 13 deletions(-) diff --git a/src/simple_example.cpp b/src/simple_example.cpp index 2ad1eb8a..25e51e95 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 f7ce1f84..a3bb5940 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 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ 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 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ 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 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ { 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,7 +308,11 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ } } - return ErrorCodeEnum::NO_ERROR; + + // 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) From 921f62729fdc1aa7545595a953888b2d858dd79d Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 10 Jul 2020 17:22:19 -0500 Subject: [PATCH 26/44] 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 25e51e95..12c703e9 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 a0c53d81..d410184d 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 15f1dc2437d1ab2c0bd7e7f2b1d7998f49568d57 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 13 Jul 2020 11:59:35 -0500 Subject: [PATCH 27/44] 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 6012e5ba..4506ff55 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 12c703e9..d9997479 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 a3bb5940..afb30d9b 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); @@ -531,8 +532,8 @@ ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* index_l // 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 d410184d..b640ab2f 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 b174ba85..9a461362 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 afa724e53f41609a90c347fd56f3dd2030303cbe Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 21 Aug 2020 22:58:04 -0500 Subject: [PATCH 28/44] 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 12603749..fb23dc7e 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 f44e3bcc66f6f762b21f4e0dc6c1db41399fd98e Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sat, 22 Aug 2020 18:40:16 -0500 Subject: [PATCH 29/44] 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 fb23dc7e..88d27d5e 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 49399815d1bd225aa6df8866b7a7df6d3ae4a702 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 23 Aug 2020 11:12:55 -0500 Subject: [PATCH 30/44] 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 4506ff55..e0a732f1 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 afb30d9b..9aa26f41 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 9a461362..640ea1c5 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 f880a2f844785cf6b06d18c772f0649c29f0b82a Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 23 Aug 2020 11:19:04 -0500 Subject: [PATCH 31/44] 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 88d27d5e..015dcce0 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::NO_ERROR, 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 e967c3b80515893e70f18d053e7d18e4dbda9461 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 23 Aug 2020 11:47:46 -0500 Subject: [PATCH 32/44] 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 9aa26f41..0fecd4fb 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_); @@ -476,7 +476,7 @@ 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 26a6cc997490f16bbacf3421e04cb99d2be1cd21 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 23 Aug 2020 11:49:55 -0500 Subject: [PATCH 33/44] 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 0fecd4fb..f0243967 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 e01a9125d90698e2b8718239e6c565d6c7b1576f Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 23 Aug 2020 11:55:22 -0500 Subject: [PATCH 34/44] 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 015dcce0..b8bf55c6 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 19791d1c8e307e16cc5bc297d499a6553188e121 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 13 Sep 2020 12:40:16 -0500 Subject: [PATCH 35/44] 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 f0243967..06c392cb 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 9929cc21800cf67aa808fa06c2d8c1d6d6426914 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 13 Sep 2020 12:49:20 -0500 Subject: [PATCH 36/44] 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 d9997479..fa3260d1 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 a0ad0a9b7251d7dbcc36644cc84c9814cd912cc2 Mon Sep 17 00:00:00 2001 From: John Morris Date: Sun, 20 Sep 2020 16:57:13 -0500 Subject: [PATCH 37/44] 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 ++++- include/trackjoint/error_codes.h | 9 ++++--- include/trackjoint/single_joint_generator.h | 3 ++- include/trackjoint/trajectory_generator.h | 8 +++--- include/trackjoint/utilities.h | 2 +- src/simple_example.cpp | 4 +-- src/single_joint_generator.cpp | 26 +++++-------------- src/three_dof_examples.cpp | 7 +++-- src/trajectory_generator.cpp | 13 +++++----- test/trajectory_generation_test.cpp | 24 ++++++++++------- 15 files changed, 53 insertions(+), 65 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/include/trackjoint/error_codes.h b/include/trackjoint/error_codes.h index f5156a06..631fbfee 100644 --- a/include/trackjoint/error_codes.h +++ b/include/trackjoint/error_codes.h @@ -39,8 +39,9 @@ enum ErrorCodeEnum */ 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" }, + { 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" }, @@ -48,6 +49,6 @@ const std::unordered_map ERROR_CODE_MAP( { 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" } }); + { 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 e0a732f1..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, 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 fa3260d1..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; diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index 06c392cb..1ef32307 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; @@ -294,8 +293,6 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ 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) { @@ -330,19 +327,10 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ // 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 @@ -571,7 +559,7 @@ ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* index_l 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); @@ -580,7 +568,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/three_dof_examples.cpp b/src/three_dof_examples.cpp index b640ab2f..3b440347 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; @@ -103,6 +103,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::ERROR_CODE_MAP.at(error_code) << std::endl; +<<<<<<< HEAD // Save the synchronized trajectories to .csv files traj_gen.saveTrajectoriesToFile(output_trajectories, output_path_base); @@ -196,6 +197,8 @@ 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::ERROR_CODE_MAP.at(error_code) << std::endl; +======= +>>>>>>> Rebase, clang format, clang tidy // 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 640ea1c5..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], diff --git a/test/trajectory_generation_test.cpp b/test/trajectory_generation_test.cpp index b8bf55c6..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; } @@ -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]; @@ -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; From 43724a1223aad8c029adaa40fcf2a5fa1a750a4a Mon Sep 17 00:00:00 2001 From: John Morris Date: Wed, 23 Sep 2020 11:03:03 -0500 Subject: [PATCH 38/44] single_joint_generator: Remove trailing `_` from function parameter (#65) --- src/single_joint_generator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index 1ef32307..9ba5b490 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -535,7 +535,7 @@ ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* index_l return error_code; // Helpful hint if limit comp fails on the first waypoint - if (index_last_successful_ == 1) + 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; From 0fc6930cbcd141b5a363bbc52178022f904c74d9 Mon Sep 17 00:00:00 2001 From: John Morris Date: Mon, 5 Oct 2020 13:37:23 -0500 Subject: [PATCH 39/44] Zultron/tracksuite refactor pt1 (#62) * Member variable refactor: Remove calculations from constructor SingleJointGenerator::reset does these same computations, refactor code to use this instead * SingleJointGenerator: Move configuration variables to struct Configuration variables are moved to a struct, written to by `reset()` and used by other methods. * NoisyStreamingCommand test: Record traveled trajectory Record traveled trajectory for inspection by `checkBounds()` in `TearDown()` function * NoisyStreamingCommand test: Add velocity/acceleration to next state To simulate velocity-, acceleration- and jerk-limited motion, the next state's velocity and acceleration must be updated along with position. --- include/trackjoint/configuration.h | 54 +++++ include/trackjoint/error_codes.h | 34 ++-- include/trackjoint/single_joint_generator.h | 49 +++-- src/simple_example.cpp | 2 + src/single_joint_generator.cpp | 211 ++++++++++---------- src/streaming_example.cpp | 2 + src/three_dof_examples.cpp | 2 + src/trajectory_generator.cpp | 8 +- test/trajectory_generation_test.cpp | 90 ++++++++- 9 files changed, 298 insertions(+), 154 deletions(-) create mode 100644 include/trackjoint/configuration.h diff --git a/include/trackjoint/configuration.h b/include/trackjoint/configuration.h new file mode 100644 index 00000000..0a15ba36 --- /dev/null +++ b/include/trackjoint/configuration.h @@ -0,0 +1,54 @@ +/********************************************************************* + * Copyright (c) 2019, PickNik Consulting + * All rights reserved. + * + * Unauthorized copying of this file, via any medium is strictly prohibited + * Proprietary and confidential + *********************************************************************/ + +/* Author: John Morris + Desc: Trajectory generator configuration. +*/ + +#pragma once + +#include "trackjoint/limits.h" + +namespace trackjoint +{ +/** + * \brief Trajectory generator configuration. + */ +struct Configuration +{ + Configuration(double timestep, double max_duration, const Limits& limits, const double position_tolerance, + bool use_streaming_mode) + : timestep(timestep) + , max_duration(max_duration) + , limits(limits) + , position_tolerance(position_tolerance) + , use_streaming_mode(use_streaming_mode) + { + } + + Configuration() + { + } + + double timestep; + double max_duration; + Limits limits; + double position_tolerance; + // If streaming mode is enabled, trajectories are clipped at + // kNumWaypointsThreshold so the algorithm runs very quickly. + // + // Streaming mode is intended for realtime streaming applications. + // + // There could be even fewer waypoints than that if the goal is very + // close or the algorithm only finds a few waypoints successfully. + // + // In streaming mode, trajectory duration is not extended until it + // successfully reaches the goal. + bool use_streaming_mode; +}; +} // namespace trackjoint diff --git a/include/trackjoint/error_codes.h b/include/trackjoint/error_codes.h index 631fbfee..6dd5529a 100644 --- a/include/trackjoint/error_codes.h +++ b/include/trackjoint/error_codes.h @@ -31,24 +31,28 @@ enum ErrorCodeEnum LIMIT_NOT_POSITIVE = 6, GOAL_POSITION_MISMATCH = 7, FAILURE_TO_GENERATE_SINGLE_WAYPOINT = 8, - LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE = 9 + LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE = 9, + OBJECT_NOT_RESET = 10, }; /** * \brief Use this map to look up human-readable strings for each error code */ -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" } }); +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" }, + { OBJECT_NOT_RESET, "Must call reset() before generating trajectory" }, + +}); } // end namespace trackjoint diff --git a/include/trackjoint/single_joint_generator.h b/include/trackjoint/single_joint_generator.h index 382484aa..355e994e 100644 --- a/include/trackjoint/single_joint_generator.h +++ b/include/trackjoint/single_joint_generator.h @@ -17,6 +17,7 @@ #include "trackjoint/error_codes.h" #include "trackjoint/joint_trajectory.h" #include "trackjoint/kinematic_state.h" +#include "trackjoint/configuration.h" #include "trackjoint/limits.h" #include "trackjoint/utilities.h" @@ -29,6 +30,27 @@ class SingleJointGenerator { public: /** \brief Constructor + * + * input num_waypoints_threshold minimum/maximum number of waypoints for full trajectory/streaming modes, respectively + * input max_num_waypoints_trajectory_mode to maintain determinism, return an error if more than this many waypoints + * is required + */ + SingleJointGenerator(size_t num_waypoints_threshold, size_t max_num_waypoints_trajectory_mode); + + /** \brief reset data members and prepare to generate a new trajectory + * + * input configuration A `Configuration` object + * 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 + * input desired_num_waypoints nominal number of waypoints, calculated from user-supplied duration and timestep + * input timestep_was_upsampled If upsampling happened (we are working with very few waypoints), do not adjust + * timestep + * + */ + void reset(const Configuration& configuration, const KinematicState& current_joint_state, + const KinematicState& goal_joint_state, size_t desired_num_waypoints, bool timestep_was_upsampled); + + /** \brief reset data members and prepare to generate a new trajectory * * input timestep desired time between waypoints * input max_duration allow the trajectory to be extended up to this limit. Error if that cannot be done. @@ -36,22 +58,14 @@ class SingleJointGenerator * input goal_joint_states vector of the target kinematic states for each degree of freedom * input limits vector of kinematic limits for each degree of freedom * input desired_num_waypoints nominal number of waypoints, calculated from user-supplied duration and timestep - * input num_waypoints_threshold minimum/maximum number of waypoints for full trajectory/streaming modes, respectively - * input max_num_waypoints_trajectory_mode to maintain determinism, return an error if more than this many waypoints - * is required * input position_tolerance tolerance for how close the final trajectory should follow a smooth interpolation. * 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. + * 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, 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, bool timestep_was_upsampled); @@ -123,22 +137,13 @@ class SingleJointGenerator const size_t kNumWaypointsThreshold, kMaxNumWaypointsFullTrajectory; - double timestep_; - double desired_duration_, max_duration_; + Configuration configuration_; + double desired_duration_; KinematicState current_joint_state_; KinematicState goal_joint_state_; - Limits limits_; - double position_tolerance_; Eigen::VectorXd nominal_times_; JointTrajectory waypoints_; size_t index_last_successful_; - - // If streaming mode is enabled, trajectories are clipped at kNumWaypointsThreshold so the algorithm runs very - // quickly. - // streaming mode is intended for realtime streaming applications. - // There could be even fewer waypoints than that if the goal is very close or the algorithm only finds a few waypoints - // successfully. - // In streaming mode, trajectory duration is not extended until it successfully reaches the goal. - bool use_streaming_mode_; + bool is_reset_; }; // end class SingleJointGenerator } // namespace trackjoint diff --git a/src/simple_example.cpp b/src/simple_example.cpp index 76eaa556..d3ec40d8 100644 --- a/src/simple_example.cpp +++ b/src/simple_example.cpp @@ -66,6 +66,8 @@ int main(int argc, char** argv) // Instantiate a trajectory generation object trackjoint::TrajectoryGenerator traj_gen(num_dof, timestep, desired_duration, max_duration, current_joint_states, goal_joint_states, limits, position_tolerance, use_streaming_mode); + traj_gen.reset(timestep, desired_duration, max_duration, current_joint_states, goal_joint_states, limits, + position_tolerance, use_streaming_mode); // This vector holds the trajectories for each DOF std::vector output_trajectories(num_dof); // Optionally, check user input for common errors, like current velocities being less than velocity limits diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index 9ba5b490..9e763883 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -10,54 +10,23 @@ namespace trackjoint { -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, - size_t max_num_waypoints_trajectory_mode, const double position_tolerance, - bool use_streaming_mode, bool timestep_was_upsampled) +SingleJointGenerator::SingleJointGenerator(size_t num_waypoints_threshold, size_t max_num_waypoints_trajectory_mode) : kNumWaypointsThreshold(num_waypoints_threshold) , kMaxNumWaypointsFullTrajectory(max_num_waypoints_trajectory_mode) - , timestep_(timestep) - , max_duration_(max_duration) - , current_joint_state_(current_joint_state) - , 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 - // The shortest possible duration avoids oscillation, as much as possible - // Desired duration cannot be less than one timestep - 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; - } + , is_reset_(false) - // 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(const Configuration& configuration, const KinematicState& current_joint_state, + const KinematicState& goal_joint_state, size_t desired_num_waypoints, bool timestep_was_upsampled) { - timestep_ = timestep; - max_duration_ = max_duration; + configuration_ = configuration; current_joint_state_ = current_joint_state; goal_joint_state_ = goal_joint_state; - limits_ = limits; - position_tolerance_ = position_tolerance; index_last_successful_ = 0; - use_streaming_mode_ = use_streaming_mode; + is_reset_ = true; // Start with this estimate of the shortest possible duration // The shortest possible duration avoids oscillation, as much as possible @@ -65,25 +34,39 @@ void SingleJointGenerator::reset(double timestep, double max_duration, const Kin if (!timestep_was_upsampled) { desired_duration_ = - std::max(timestep_, fabs((goal_joint_state.position - current_joint_state.position) / limits_.velocity_limit)); + std::max(configuration.timestep, fabs((goal_joint_state.position - current_joint_state.position) / + configuration_.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; + desired_duration_ = (desired_num_waypoints - 1) * configuration.timestep; } // 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, + bool timestep_was_upsampled) +{ + Configuration configuration(timestep, max_duration, limits, position_tolerance, use_streaming_mode); + this->reset(configuration, current_joint_state, goal_joint_state, desired_num_waypoints, timestep_was_upsampled); +} + ErrorCodeEnum SingleJointGenerator::generateTrajectory() { + if (!is_reset_) + return ErrorCodeEnum::OBJECT_NOT_RESET; + // Clear previous results waypoints_ = JointTrajectory(); waypoints_.positions = interpolate(nominal_times_); - waypoints_.elapsed_times.setLinSpaced(waypoints_.positions.size(), 0., (waypoints_.positions.size() - 1) * timestep_); + waypoints_.elapsed_times.setLinSpaced(waypoints_.positions.size(), 0., + (waypoints_.positions.size() - 1) * configuration_.timestep); calculateDerivativesFromPosition(); ErrorCodeEnum error_code = positionVectorLimitLookAhead(&index_last_successful_); @@ -99,7 +82,7 @@ ErrorCodeEnum SingleJointGenerator::generateTrajectory() void SingleJointGenerator::extendTrajectoryDuration() { - size_t new_num_waypoints = 1 + desired_duration_ / timestep_; + size_t new_num_waypoints = 1 + desired_duration_ / configuration_.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. @@ -115,7 +98,7 @@ void SingleJointGenerator::extendTrajectoryDuration() const auto fit = SplineFitting1D::Interpolate(position, 2, new_times); // New times, with the extended duration - waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., (new_num_waypoints - 1) * timestep_); + waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., (new_num_waypoints - 1) * configuration_.timestep); // Retrieve new positions at the new times waypoints_.positions.resize(new_num_waypoints); @@ -131,7 +114,7 @@ void SingleJointGenerator::extendTrajectoryDuration() else { waypoints_ = JointTrajectory(); - waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., (new_num_waypoints - 1) * timestep_); + waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., (new_num_waypoints - 1) * configuration_.timestep); waypoints_.positions = interpolate(waypoints_.elapsed_times); calculateDerivativesFromPosition(); forwardLimitCompensation(&index_last_successful_); @@ -190,7 +173,7 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ // Start with the assumption that the entire trajectory can be completed. // Streaming mode returns at the minimum number of waypoints. // Streaming mode is not necessary if the number of waypoints is already very short. - if (!use_streaming_mode_ || static_cast(waypoints_.positions.size()) <= kNumWaypointsThreshold) + if (!configuration_.use_streaming_mode || static_cast(waypoints_.positions.size()) <= kNumWaypointsThreshold) *index_last_successful = waypoints_.positions.size() - 1; else *index_last_successful = kNumWaypointsThreshold - 1; @@ -199,9 +182,9 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ // Discrete differentiation introduces small numerical errors, so allow a small tolerance const double limit_relative_tol = 0.999999; - const double jerk_limit = limit_relative_tol * limits_.jerk_limit; - const double acceleration_limit = limit_relative_tol * limits_.acceleration_limit; - const double velocity_limit = limit_relative_tol * limits_.velocity_limit; + const double jerk_limit = limit_relative_tol * configuration_.limits.jerk_limit; + const double acceleration_limit = limit_relative_tol * configuration_.limits.acceleration_limit; + const double velocity_limit = limit_relative_tol * configuration_.limits.velocity_limit; // Preallocate double delta_a(0), delta_v(0), position_error(0); @@ -215,20 +198,21 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ double delta_j = std::copysign(jerk_limit, waypoints_.jerks(index)) - waypoints_.jerks(index); waypoints_.jerks(index) = std::copysign(jerk_limit, waypoints_.jerks(index)); - waypoints_.accelerations(index) = waypoints_.accelerations(index - 1) + waypoints_.jerks(index) * timestep_; + waypoints_.accelerations(index) = + waypoints_.accelerations(index - 1) + waypoints_.jerks(index) * configuration_.timestep; waypoints_.velocities(index) = waypoints_.velocities(index - 1) + - waypoints_.accelerations(index - 1) * timestep_ + - 0.5 * waypoints_.jerks(index) * timestep_ * timestep_; + waypoints_.accelerations(index - 1) * configuration_.timestep + + 0.5 * waypoints_.jerks(index) * configuration_.timestep * configuration_.timestep; - delta_v = 0.5 * delta_j * timestep_ * timestep_; + delta_v = 0.5 * delta_j * configuration_.timestep * configuration_.timestep; // Try adjusting the velocity in previous timesteps to compensate for this limit, if needed successful_compensation = backwardLimitCompensation(index, -delta_v); if (!successful_compensation) { - position_error = position_error + delta_v * timestep_; + position_error = position_error + delta_v * configuration_.timestep; } - if (fabs(position_error) > position_tolerance_) + if (fabs(position_error) > configuration_.position_tolerance) { recordFailureTime(index, index_last_successful); // Only break, do not return, because we are looking for the FIRST failure. May find an earlier failure in @@ -252,14 +236,15 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ // Check jerk limit before applying the change. // The first condition checks if the new jerk(i) is going to exceed the limit. Pretty straightforward. // We also calculate a new jerk(i+1). The second condition checks if jerk(i+1) would exceed the limit. - if ((fabs((temp_accel - waypoints_.accelerations(index)) / timestep_) <= jerk_limit) && - (fabs(waypoints_.jerks(index) + delta_a / timestep_) <= jerk_limit)) + if ((fabs((temp_accel - waypoints_.accelerations(index)) / configuration_.timestep) <= jerk_limit) && + (fabs(waypoints_.jerks(index) + delta_a / configuration_.timestep) <= jerk_limit)) { waypoints_.accelerations(index) = temp_accel; - waypoints_.jerks(index) = (waypoints_.accelerations(index) - waypoints_.accelerations(index - 1)) / timestep_; - waypoints_.velocities(index) = waypoints_.velocities(index - 1) + - waypoints_.accelerations(index - 1) * timestep_ + - 0.5 * waypoints_.jerks(index) * timestep_ * timestep_; + waypoints_.jerks(index) = + (waypoints_.accelerations(index) - waypoints_.accelerations(index - 1)) / configuration_.timestep; + waypoints_.velocities(index) = + waypoints_.velocities(index - 1) + waypoints_.accelerations(index - 1) * configuration_.timestep + + 0.5 * waypoints_.jerks(index) * configuration_.timestep * configuration_.timestep; } else { @@ -271,13 +256,13 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ } // Try adjusting the velocity in previous timesteps to compensate for this limit, if needed - delta_v = delta_a * timestep_; + delta_v = delta_a * configuration_.timestep; successful_compensation = backwardLimitCompensation(index, -delta_v); if (!successful_compensation) { - position_error = position_error + delta_v * timestep_; + position_error = position_error + delta_v * configuration_.timestep; } - if (fabs(position_error) > position_tolerance_) + if (fabs(position_error) > configuration_.position_tolerance) { recordFailureTime(index, index_last_successful); // Only break, do not return, because we are looking for the FIRST failure. May find an earlier failure in @@ -301,17 +286,17 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ // Try adjusting the velocity in previous timesteps to compensate for this limit. // Try to account for position error, too. - delta_v += position_error / timestep_; + delta_v += position_error / configuration_.timestep; successful_compensation = backwardLimitCompensation(index, -delta_v); if (!successful_compensation) { - position_error = position_error + delta_v * timestep_; + position_error = position_error + delta_v * configuration_.timestep; } else position_error = 0; - if (fabs(position_error) > position_tolerance_ || fabs(waypoints_.accelerations(index)) > acceleration_limit || - fabs(waypoints_.jerks(index)) > jerk_limit || + if (fabs(position_error) > configuration_.position_tolerance || + fabs(waypoints_.accelerations(index)) > acceleration_limit || fabs(waypoints_.jerks(index)) > jerk_limit || fabs(waypoints_.accelerations(index + 1)) > acceleration_limit || fabs(waypoints_.jerks(index + 1)) > jerk_limit) { @@ -348,31 +333,36 @@ bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, doubl for (size_t index = limited_index; index > 2; --index) { // if there is some room to increase the velocity at timestep i - if (fabs(waypoints_.velocities(index)) < limits_.velocity_limit) + if (fabs(waypoints_.velocities(index)) < configuration_.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) <= configuration_.limits.velocity_limit - excess_velocity) || + (excess_velocity < 0 && + waypoints_.velocities(index) >= -configuration_.limits.velocity_limit - 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 = - (backward_accel - (waypoints_.velocities(index - 1) - waypoints_.velocities(index - 2)) / timestep_) / - timestep_; + double backward_accel = (new_velocity - waypoints_.velocities(index - 1)) / configuration_.timestep; + double backward_jerk = (backward_accel - (waypoints_.velocities(index - 1) - waypoints_.velocities(index - 2)) / + configuration_.timestep) / + configuration_.timestep; // Accel and jerk, calculated from upcoming waypoints - double forward_accel = (waypoints_.velocities(index + 1) - new_velocity) / timestep_; + double forward_accel = (waypoints_.velocities(index + 1) - new_velocity) / configuration_.timestep; double forward_jerk = - (forward_accel - (new_velocity - waypoints_.velocities(index - 1)) / timestep_) / timestep_; + (forward_accel - (new_velocity - waypoints_.velocities(index - 1)) / configuration_.timestep) / + configuration_.timestep; // Calculate this new velocity if it would not violate accel/jerk limits - if ((fabs(backward_jerk) < limits_.jerk_limit) && (fabs(backward_accel) < limits_.acceleration_limit) && - (fabs(forward_jerk) < limits_.jerk_limit) && (fabs(forward_accel) < limits_.acceleration_limit)) + if ((fabs(backward_jerk) < configuration_.limits.jerk_limit) && + (fabs(backward_accel) < configuration_.limits.acceleration_limit) && + (fabs(forward_jerk) < configuration_.limits.jerk_limit) && + (fabs(forward_accel) < configuration_.limits.acceleration_limit)) { waypoints_.velocities(index) = new_velocity; // if at the velocity limit, must stop accelerating - if (fabs(waypoints_.velocities(index)) >= limits_.velocity_limit) + if (fabs(waypoints_.velocities(index)) >= configuration_.limits.velocity_limit) { waypoints_.accelerations(index) = 0; waypoints_.jerks(index) = 0; @@ -392,27 +382,30 @@ bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, doubl { // 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) * configuration_.limits.velocity_limit; // Accel and jerk, calculated from the previous waypoints - double backward_accel = (new_velocity - waypoints_.velocities(index - 1)) / timestep_; - double backward_jerk = - (backward_accel - (waypoints_.velocities(index - 1) - waypoints_.velocities(index - 2)) / timestep_) / - timestep_; + double backward_accel = (new_velocity - waypoints_.velocities(index - 1)) / configuration_.timestep; + double backward_jerk = (backward_accel - (waypoints_.velocities(index - 1) - waypoints_.velocities(index - 2)) / + configuration_.timestep) / + configuration_.timestep; // Accel and jerk, calculated from upcoming waypoints - double forward_accel = (waypoints_.velocities(index + 1) - new_velocity) / timestep_; + double forward_accel = (waypoints_.velocities(index + 1) - new_velocity) / configuration_.timestep; double forward_jerk = - (forward_accel - (new_velocity - waypoints_.velocities(index - 1)) / timestep_) / timestep_; + (forward_accel - (new_velocity - waypoints_.velocities(index - 1)) / configuration_.timestep) / + configuration_.timestep; // Apply these changes if all limits are satisfied (for the prior // waypoint and the future waypoint) - if ((fabs(backward_accel) < limits_.acceleration_limit) && (fabs(backward_jerk) < limits_.jerk_limit) && - (fabs(forward_accel) < limits_.acceleration_limit) && (fabs(forward_jerk) < limits_.jerk_limit)) + if ((fabs(backward_accel) < configuration_.limits.acceleration_limit) && + (fabs(backward_jerk) < configuration_.limits.jerk_limit) && + (fabs(forward_accel) < configuration_.limits.acceleration_limit) && + (fabs(forward_jerk) < configuration_.limits.jerk_limit)) { double delta_v = new_velocity - waypoints_.velocities(index); waypoints_.velocities(index) = new_velocity; // if at the velocity limit, must stop accelerating - if (fabs(waypoints_.velocities(index)) >= limits_.velocity_limit) + if (fabs(waypoints_.velocities(index)) >= configuration_.limits.velocity_limit) { waypoints_.accelerations(index) = 0; waypoints_.jerks(index) = 0; @@ -428,7 +421,8 @@ bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, doubl } // Clip velocities at min/max due to small rounding errors waypoints_.velocities(index) = - std::min(std::max(waypoints_.velocities(index), -limits_.velocity_limit), limits_.velocity_limit); + std::min(std::max(waypoints_.velocities(index), -configuration_.limits.velocity_limit), + configuration_.limits.velocity_limit); } return successful_compensation; @@ -443,28 +437,28 @@ ErrorCodeEnum SingleJointGenerator::predictTimeToReach() ErrorCodeEnum error_code = ErrorCodeEnum::NO_ERROR; // If in normal mode, we can extend trajectories - if (!use_streaming_mode_) + if (!configuration_.use_streaming_mode) { size_t new_num_waypoints = 0; // Iterate over new durations until the position error is acceptable or the maximum duration is reached while ((index_last_successful_ < static_cast(waypoints_.positions.size() - 1)) && - (desired_duration_ < max_duration_) && (new_num_waypoints < kMaxNumWaypointsFullTrajectory)) + (desired_duration_ < configuration_.max_duration) && (new_num_waypoints < kMaxNumWaypointsFullTrajectory)) { // Try increasing the duration, based on fraction of states that weren't reached successfully desired_duration_ = (1. + 0.2 * (1. - index_last_successful_ / (waypoints_.positions.size() - 1))) * desired_duration_; // // Round to nearest timestep - if (std::fmod(desired_duration_, timestep_) > 0.5 * timestep_) - desired_duration_ = desired_duration_ + timestep_; + if (std::fmod(desired_duration_, configuration_.timestep) > 0.5 * configuration_.timestep) + desired_duration_ = desired_duration_ + configuration_.timestep; new_num_waypoints = std::max(static_cast(waypoints_.positions.size() + 1), - static_cast(floor(1 + desired_duration_ / timestep_))); + static_cast(floor(1 + desired_duration_ / configuration_.timestep))); // Cap the trajectory duration to maintain determinism if (new_num_waypoints > kMaxNumWaypointsFullTrajectory) new_num_waypoints = kMaxNumWaypointsFullTrajectory; - waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., (new_num_waypoints - 1) * timestep_); + waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., (new_num_waypoints - 1) * configuration_.timestep); waypoints_.positions.resize(waypoints_.elapsed_times.size()); waypoints_.velocities.resize(waypoints_.elapsed_times.size()); waypoints_.accelerations.resize(waypoints_.elapsed_times.size()); @@ -515,7 +509,8 @@ 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)) + if (!configuration_.use_streaming_mode && + index_last_successful_ < static_cast(waypoints_.elapsed_times.size() - 1)) { error_code = ErrorCodeEnum::MAX_DURATION_EXCEEDED; } @@ -548,12 +543,13 @@ ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* index_l // Initial waypoint waypoints_.positions(0) = current_joint_state_.position; for (size_t index = 1; index < static_cast(waypoints_.positions.size()) - 1; ++index) - waypoints_.positions(index) = waypoints_.positions(index - 1) + waypoints_.velocities(index - 1) * timestep_ + - 0.5 * waypoints_.accelerations(index - 1) * pow(timestep_, 2) + - one_sixth * waypoints_.jerks(index - 1) * pow(timestep_, 3); + waypoints_.positions(index) = waypoints_.positions(index - 1) + + waypoints_.velocities(index - 1) * configuration_.timestep + + 0.5 * waypoints_.accelerations(index - 1) * pow(configuration_.timestep, 2) + + one_sixth * waypoints_.jerks(index - 1) * pow(configuration_.timestep, 3); // Final waypoint should match the goal, unless trajectory was cut short for streaming mode - if (!use_streaming_mode_) + if (!configuration_.use_streaming_mode) waypoints_.positions(waypoints_.positions.size() - 1) = goal_joint_state_.position; return error_code; @@ -562,18 +558,19 @@ ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* index_l void SingleJointGenerator::calculateDerivativesFromPosition() { // From position vector, approximate vel/accel/jerk. - waypoints_.velocities = DiscreteDifferentiation(waypoints_.positions, timestep_, current_joint_state_.velocity); + waypoints_.velocities = + DiscreteDifferentiation(waypoints_.positions, configuration_.timestep, current_joint_state_.velocity); waypoints_.accelerations = - DiscreteDifferentiation(waypoints_.velocities, timestep_, current_joint_state_.acceleration); - waypoints_.jerks = DiscreteDifferentiation(waypoints_.accelerations, timestep_, 0); + DiscreteDifferentiation(waypoints_.velocities, configuration_.timestep, current_joint_state_.acceleration); + waypoints_.jerks = DiscreteDifferentiation(waypoints_.accelerations, configuration_.timestep, 0); } 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); + DiscreteDifferentiation(waypoints_.velocities, configuration_.timestep, current_joint_state_.acceleration); + waypoints_.jerks = DiscreteDifferentiation(waypoints_.accelerations, configuration_.timestep, 0); } void SingleJointGenerator::updateTrajectoryDuration(double new_trajectory_duration) @@ -581,6 +578,6 @@ void SingleJointGenerator::updateTrajectoryDuration(double new_trajectory_durati // The trajectory will be forced to have this duration (or fail) because // max_duration == desired_duration desired_duration_ = new_trajectory_duration; - max_duration_ = new_trajectory_duration; + configuration_.max_duration = new_trajectory_duration; } } // end namespace trackjoint diff --git a/src/streaming_example.cpp b/src/streaming_example.cpp index c8d72642..6a563da2 100644 --- a/src/streaming_example.cpp +++ b/src/streaming_example.cpp @@ -65,6 +65,8 @@ int main(int argc, char** argv) std::vector output_trajectories(num_dof); trackjoint::TrajectoryGenerator traj_gen(num_dof, timestep, desired_duration, max_duration, start_state, goal_joint_states, limits, waypoint_position_tolerance, use_streaming_mode); + traj_gen.reset(timestep, desired_duration, max_duration, start_state, goal_joint_states, limits, + waypoint_position_tolerance, use_streaming_mode); // An example of optional input validation trackjoint::ErrorCodeEnum error_code = traj_gen.inputChecking(start_state, goal_joint_states, limits, timestep); diff --git a/src/three_dof_examples.cpp b/src/three_dof_examples.cpp index 3b440347..1403a48b 100644 --- a/src/three_dof_examples.cpp +++ b/src/three_dof_examples.cpp @@ -72,6 +72,8 @@ int main(int argc, char** argv) // 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); + traj_gen.reset(timestep, desired_duration, max_duration, current_joint_states, goal_joint_states, limits, + waypoint_position_tolerance, use_streaming_mode); std::vector output_trajectories(num_dof); diff --git a/src/trajectory_generator.cpp b/src/trajectory_generator.cpp index cd5a7b69..91bf6074 100644 --- a/src/trajectory_generator.cpp +++ b/src/trajectory_generator.cpp @@ -29,13 +29,9 @@ TrajectoryGenerator::TrajectoryGenerator(uint num_dof, double timestep, double d upsample(); // Initialize a trajectory generator for each joint - 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(kNumWaypointsThreshold, kMaxNumWaypointsFullTrajectory)); } } @@ -436,4 +432,4 @@ ErrorCodeEnum TrajectoryGenerator::generateTrajectories(std::vector output_trajectories_; + bool skip_teardown_checks_; void checkBounds() { @@ -195,15 +196,35 @@ class TrajectoryGenerationTest : public ::testing::Test void TearDown() override { - checkBounds(); - if (write_output_) + if (!skip_teardown_checks_) { - writeOutputToFiles(); + checkBounds(); + if (write_output_) + { + writeOutputToFiles(); + } } } }; // class TrajectoryGenerationTest +TEST_F(TrajectoryGenerationTest, DetectNoReset) +{ + // Calling `generateTrajectories()` without an initial `reset()` + // should return an error code (and not segfault!) + + TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration_, max_duration_, current_joint_states_, + goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); + ErrorCodeEnum error_code = traj_gen.generateTrajectories(&output_trajectories_); + EXPECT_EQ(error_code, ErrorCodeEnum::OBJECT_NOT_RESET); + + // Ensure OBJECT_NOT_RESET can be converted to string + std::cout << "Error: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; + + // TearDown() should skip post-test checks + skip_teardown_checks_ = true; +} + TEST_F(TrajectoryGenerationTest, EasyDefaultTrajectory) { // Use the class defaults. This trajectory is easy, does not require limit @@ -211,6 +232,8 @@ 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_); + traj_gen.reset(timestep_, desired_duration_, max_duration_, current_joint_states_, goal_joint_states_, limits_, + position_tolerance_, use_streaming_mode_); EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error @@ -256,6 +279,8 @@ TEST_F(TrajectoryGenerationTest, OneTimestepDuration) TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); + traj_gen.reset(timestep_, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, + position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); @@ -290,6 +315,8 @@ TEST_F(TrajectoryGenerationTest, RoughlyTwoTimestepDuration) TrajectoryGenerator traj_gen(num_dof_, timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); + traj_gen.reset(timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, + position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); @@ -326,6 +353,8 @@ TEST_F(TrajectoryGenerationTest, FourTimestepDuration) TrajectoryGenerator traj_gen(num_dof_, timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); + traj_gen.reset(timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, + position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); @@ -377,6 +406,8 @@ TEST_F(TrajectoryGenerationTest, SixTimestepDuration) TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); + traj_gen.reset(timestep_, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, + position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); @@ -421,6 +452,8 @@ TEST_F(TrajectoryGenerationTest, VelAccelJerkLimit) TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration_, max_duration, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); + traj_gen.reset(timestep_, desired_duration_, max_duration, current_joint_states_, goal_joint_states_, limits_, + position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); @@ -471,6 +504,24 @@ TEST_F(TrajectoryGenerationTest, NoisyStreamingCommand) limits_[1] = single_joint_limits; limits_[2] = single_joint_limits; + // For recording actual followed trajectory + std::vector recorded_trajectories(num_dof_); + for (size_t joint = 0; joint < num_dof_; ++joint) + { + // Resize vector + recorded_trajectories[joint].positions.resize(num_waypoints); + recorded_trajectories[joint].velocities.resize(num_waypoints); + recorded_trajectories[joint].accelerations.resize(num_waypoints); + recorded_trajectories[joint].jerks.resize(num_waypoints); + recorded_trajectories[joint].elapsed_times.resize(num_waypoints); + // Set initial waypoint + recorded_trajectories[joint].positions(0) = current_joint_states_[joint].position; + recorded_trajectories[joint].velocities(0) = current_joint_states_[joint].velocity; + recorded_trajectories[joint].accelerations(0) = current_joint_states_[joint].acceleration; + recorded_trajectories[joint].jerks(0) = 0; + recorded_trajectories[joint].elapsed_times(0) = 0; + } + Eigen::VectorXd x_desired(num_waypoints); Eigen::VectorXd x_smoothed(num_waypoints); @@ -501,6 +552,18 @@ TEST_F(TrajectoryGenerationTest, NoisyStreamingCommand) x_smoothed(waypoint) = output_trajectories_.at(0).positions(1); // ... and setting the next current position as the updated x_smoothed joint_state.position = x_smoothed(waypoint); + joint_state.velocity = output_trajectories_.at(0).velocities(1); + joint_state.acceleration = output_trajectories_.at(0).accelerations(1); + + // Record next point + for (size_t joint = 0; joint < num_dof_; joint++) + { + recorded_trajectories[joint].positions(waypoint) = output_trajectories_[joint].positions(1); + recorded_trajectories[joint].velocities(waypoint) = output_trajectories_[joint].velocities(1); + recorded_trajectories[joint].accelerations(waypoint) = output_trajectories_[joint].accelerations(1); + recorded_trajectories[joint].jerks(waypoint) = output_trajectories_[joint].jerks(1); + recorded_trajectories[joint].elapsed_times(waypoint) = time; + } current_joint_states_[0] = joint_state; current_joint_states_[1] = joint_state; @@ -511,6 +574,9 @@ TEST_F(TrajectoryGenerationTest, NoisyStreamingCommand) uint num_waypoint_tolerance = 1; uint expected_num_waypoints = num_waypoints; EXPECT_NEAR(uint(x_smoothed.size()), expected_num_waypoints, num_waypoint_tolerance); + + // Put recorded trajectories where the tearDown() method will check them + output_trajectories_ = recorded_trajectories; } TEST_F(TrajectoryGenerationTest, OscillatingUR5TrackJointCase) @@ -657,6 +723,8 @@ TEST_F(TrajectoryGenerationTest, SuddenChangeOfDirection) TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); + traj_gen.reset(timestep_, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, + position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); @@ -709,6 +777,8 @@ 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_); + traj_gen.reset(timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, + position_tolerance_, use_streaming_mode_); EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error @@ -762,6 +832,8 @@ 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_); + traj_gen.reset(timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, + position_tolerance_, use_streaming_mode_); EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error @@ -820,6 +892,8 @@ 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_); + traj_gen.reset(timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, + position_tolerance_, use_streaming_mode_); EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error @@ -866,6 +940,8 @@ TEST_F(TrajectoryGenerationTest, TimestepDidNotMatch) TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration_, max_duration_, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); + traj_gen.reset(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::NO_ERROR, @@ -928,6 +1004,8 @@ TEST_F(TrajectoryGenerationTest, CustomerStreaming) // Generate initial trajectory TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration, max_duration_, current_joint_states_, goal_joint_states_, limits_, waypoint_position_tolerance, use_streaming_mode_); + traj_gen.reset(timestep_, desired_duration, max_duration_, current_joint_states_, goal_joint_states_, limits_, + position_tolerance_, use_streaming_mode_); ErrorCodeEnum error_code = traj_gen.generateTrajectories(&output_trajectories_); EXPECT_EQ(error_code, ErrorCodeEnum::NO_ERROR); @@ -976,6 +1054,8 @@ 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_); + traj_gen.reset(timestep_, desired_duration_, max_duration_, current_joint_states_, goal_joint_states_, limits_, + position_tolerance_, use_streaming_mode_); EXPECT_EQ(ErrorCodeEnum::LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE, traj_gen.inputChecking(current_joint_states_, goal_joint_states_, limits_, timestep_)); } @@ -1011,6 +1091,8 @@ TEST_F(TrajectoryGenerationTest, SingleJointOscillation) TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration_, max_duration_, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); + traj_gen.reset(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::NO_ERROR, From d3cf022d73ba7405b347c879f63ca6f73b0c3741 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 22 Oct 2020 14:52:17 -0500 Subject: [PATCH 40/44] Don't need a warning for failure to comp at first waypoint Duration extension can fix it --- src/single_joint_generator.cpp | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index 9e763883..411336b7 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -75,6 +75,7 @@ ErrorCodeEnum SingleJointGenerator::generateTrajectory() return error_code; } + // Extend the trajectory duration, if needed error_code = predictTimeToReach(); return error_code; @@ -529,13 +530,6 @@ ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* index_l 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 From 060dab4ff8f8182e200340af4fd52dee7c06ff16 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 22 Oct 2020 14:59:52 -0500 Subject: [PATCH 41/44] Print warning if results directory doesn't exist --- include/trackjoint/trajectory_generator.h | 1 + src/trajectory_generator.cpp | 7 +++++++ 2 files changed, 8 insertions(+) diff --git a/include/trackjoint/trajectory_generator.h b/include/trackjoint/trajectory_generator.h index f2a97781..a2a27183 100644 --- a/include/trackjoint/trajectory_generator.h +++ b/include/trackjoint/trajectory_generator.h @@ -21,6 +21,7 @@ #include "trackjoint/utilities.h" // C++ +#include #include #include // shared_ptr #include diff --git a/src/trajectory_generator.cpp b/src/trajectory_generator.cpp index 91bf6074..40f3a9e6 100644 --- a/src/trajectory_generator.cpp +++ b/src/trajectory_generator.cpp @@ -256,6 +256,13 @@ void TrajectoryGenerator::saveTrajectoriesToFile(const std::vector Date: Thu, 22 Oct 2020 15:25:28 -0500 Subject: [PATCH 42/44] Fix small rebase mistake --- src/three_dof_examples.cpp | 96 -------------------------------------- 1 file changed, 96 deletions(-) diff --git a/src/three_dof_examples.cpp b/src/three_dof_examples.cpp index 1403a48b..2172953a 100644 --- a/src/three_dof_examples.cpp +++ b/src/three_dof_examples.cpp @@ -105,102 +105,6 @@ 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::ERROR_CODE_MAP.at(error_code) << std::endl; -<<<<<<< HEAD - - // 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; - - ///////////////////////////////// - // 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::NO_ERROR) - { - std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.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::NO_ERROR) - { - std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.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::ERROR_CODE_MAP.at(error_code) << std::endl; -======= ->>>>>>> Rebase, clang format, clang tidy // Save the synchronized trajectories to .csv files traj_gen.saveTrajectoriesToFile(output_trajectories, output_path_base); From a28a7d46720ef6e4eb9d7d3c7eccc6a36dfae32e Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 22 Oct 2020 15:40:12 -0500 Subject: [PATCH 43/44] Clang format --- src/single_joint_generator.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index 411336b7..845d2bf9 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -309,7 +309,6 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ } } - // Re-calculate derivatives from the updated velocity vector calculateDerivativesFromVelocity(); @@ -345,9 +344,10 @@ bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, doubl 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)) / configuration_.timestep; - double backward_jerk = (backward_accel - (waypoints_.velocities(index - 1) - waypoints_.velocities(index - 2)) / - configuration_.timestep) / - configuration_.timestep; + double backward_jerk = + (backward_accel - + (waypoints_.velocities(index - 1) - waypoints_.velocities(index - 2)) / configuration_.timestep) / + configuration_.timestep; // Accel and jerk, calculated from upcoming waypoints double forward_accel = (waypoints_.velocities(index + 1) - new_velocity) / configuration_.timestep; double forward_jerk = @@ -386,9 +386,10 @@ bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, doubl double new_velocity = std::copysign(1.0, excess_velocity) * configuration_.limits.velocity_limit; // Accel and jerk, calculated from the previous waypoints double backward_accel = (new_velocity - waypoints_.velocities(index - 1)) / configuration_.timestep; - double backward_jerk = (backward_accel - (waypoints_.velocities(index - 1) - waypoints_.velocities(index - 2)) / - configuration_.timestep) / - configuration_.timestep; + double backward_jerk = + (backward_accel - + (waypoints_.velocities(index - 1) - waypoints_.velocities(index - 2)) / configuration_.timestep) / + configuration_.timestep; // Accel and jerk, calculated from upcoming waypoints double forward_accel = (waypoints_.velocities(index + 1) - new_velocity) / configuration_.timestep; double forward_jerk = From 4b4f3f3ea300018e94047a0faf923be54a565273 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 22 Oct 2020 15:50:02 -0500 Subject: [PATCH 44/44] Add comment about duration extension constant --- src/single_joint_generator.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index 845d2bf9..a1c8796a 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -447,6 +447,9 @@ ErrorCodeEnum SingleJointGenerator::predictTimeToReach() (desired_duration_ < configuration_.max_duration) && (new_num_waypoints < kMaxNumWaypointsFullTrajectory)) { // Try increasing the duration, based on fraction of states that weren't reached successfully + // Choice of 0.2 is subjective but it should be between 0-1. + // A smaller fraction will find a solution that's closer to time-optimal because it adds fewer new waypoints to + // the search. But, a smaller fraction likely increases runtime. desired_duration_ = (1. + 0.2 * (1. - index_last_successful_ / (waypoints_.positions.size() - 1))) * desired_duration_;