From 518206d501322a6ad853afad5c2918c3c087c171 Mon Sep 17 00:00:00 2001 From: Nathan Brooks Date: Mon, 18 May 2020 09:05:00 -0600 Subject: [PATCH 01/15] Clang tidy fixes (#53) --- include/trackjoint/error_codes.h | 46 ++++++++++++++--------------- src/simple_example.cpp | 10 +++---- src/single_joint_generator.cpp | 8 ++--- src/streaming_example.cpp | 10 +++---- src/three_dof_examples.cpp | 20 ++++++------- src/trajectory_generator.cpp | 24 +++++++-------- test/trajectory_generation_test.cpp | 34 ++++++++++----------- 7 files changed, 76 insertions(+), 76 deletions(-) diff --git a/include/trackjoint/error_codes.h b/include/trackjoint/error_codes.h index 92e6227d..f5156a06 100644 --- a/include/trackjoint/error_codes.h +++ b/include/trackjoint/error_codes.h @@ -22,32 +22,32 @@ namespace trackjoint */ enum ErrorCodeEnum { - kNoError = 0, - kDesiredDurationTooShort = 1, - kMaxDurationExceeded = 2, - kVelocityExceedsLimit = 3, - kAccelExceedsLimit = 4, - kMaxDurationLessThanDesiredDuration = 5, - kLimitNotPositive = 6, - kGoalPositionMismatch = 7, - kFailureToGenerateSingleWaypoint = 8, - kLessThanTenTimestepsForStreamingMode = 9 + NO_ERROR = 0, + DESIRED_DURATION_TOO_SHORT = 1, + MAX_DURATION_EXCEEDED = 2, + VELOCITY_EXCEEDS_LIMIT = 3, + ACCEL_EXCEEDS_LIMIT = 4, + MAX_DURATION_LESS_THAN_DESIRED_DURATION = 5, + LIMIT_NOT_POSITIVE = 6, + GOAL_POSITION_MISMATCH = 7, + FAILURE_TO_GENERATE_SINGLE_WAYPOINT = 8, + LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE = 9 }; /** * \brief Use this map to look up human-readable strings for each error code */ -const std::unordered_map kErrorCodeMap( - { { kNoError, "No error, trajectory generation was successful" }, - { kDesiredDurationTooShort, "Desired duration is too short, cannot have less than one timestep in a " - "trajectory" }, - { kMaxDurationExceeded, "Max duration was exceeded" }, - { kVelocityExceedsLimit, "A velocity input exceeds the velocity limit" }, - { kAccelExceedsLimit, "An acceleration input exceeds the acceleration limit" }, - { kMaxDurationLessThanDesiredDuration, "max_duration should not be less than desired_duration" }, - { kLimitNotPositive, "Vel/accel/jerk limits should be greater than zero" }, - { kGoalPositionMismatch, "Mismatch between the final position and the goal position" }, - { kFailureToGenerateSingleWaypoint, "Failed to generate even a single new waypoint" }, - { kLessThanTenTimestepsForStreamingMode, "In streaming mode, desired duration should be at least 10 " - "timesteps" } }); +const std::unordered_map ERROR_CODE_MAP( + { { NO_ERROR, "No error, trajectory generation was successful" }, + { DESIRED_DURATION_TOO_SHORT, "Desired duration is too short, cannot have less than one timestep in a " + "trajectory" }, + { MAX_DURATION_EXCEEDED, "Max duration was exceeded" }, + { VELOCITY_EXCEEDS_LIMIT, "A velocity input exceeds the velocity limit" }, + { ACCEL_EXCEEDS_LIMIT, "An acceleration input exceeds the acceleration limit" }, + { MAX_DURATION_LESS_THAN_DESIRED_DURATION, "max_duration should not be less than desired_duration" }, + { LIMIT_NOT_POSITIVE, "Vel/accel/jerk limits should be greater than zero" }, + { GOAL_POSITION_MISMATCH, "Mismatch between the final position and the goal position" }, + { FAILURE_TO_GENERATE_SINGLE_WAYPOINT, "Failed to generate even a single new waypoint" }, + { LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE, "In streaming mode, desired duration should be at least 10 " + "timesteps" } }); } // end namespace trackjoint diff --git a/src/simple_example.cpp b/src/simple_example.cpp index 49e4ac54..63795669 100644 --- a/src/simple_example.cpp +++ b/src/simple_example.cpp @@ -68,9 +68,9 @@ int main(int argc, char** argv) traj_gen.inputChecking(current_joint_states, goal_joint_states, limits, timestep); // Input error handling - if an error is found, the trajectory is not generated. - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } @@ -81,9 +81,9 @@ int main(int argc, char** argv) auto end = std::chrono::system_clock::now(); // Trajectory generation error handling - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } @@ -91,7 +91,7 @@ int main(int argc, char** argv) std::cout << "Runtime: " << elapsed_seconds.count() << std::endl; std::cout << "Num waypoints: " << output_trajectories.at(0).positions.size() << std::endl; - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; // Save the synchronized trajectories to .csv files traj_gen.saveTrajectoriesToFile(output_trajectories, output_path_base); diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index 1d0e729f..05f4a2a4 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -273,7 +273,7 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde } } - return ErrorCodeEnum::kNoError; + return ErrorCodeEnum::NO_ERROR; } inline void SingleJointGenerator::recordFailureTime(size_t current_index, size_t* index_last_successful) @@ -397,7 +397,7 @@ inline ErrorCodeEnum SingleJointGenerator::predictTimeToReach() // limits. // This gives a new duration estimate. - ErrorCodeEnum error_code = ErrorCodeEnum::kNoError; + ErrorCodeEnum error_code = ErrorCodeEnum::NO_ERROR; // If in normal mode, we can extend trajectories if (!use_streaming_mode_) @@ -474,12 +474,12 @@ inline ErrorCodeEnum SingleJointGenerator::predictTimeToReach() // Normal mode: Error if we extended the duration to the maximum and it still wasn't successful if (!use_streaming_mode_ && index_last_successful_ < static_cast(waypoints_.elapsed_times.size() - 1)) { - error_code = ErrorCodeEnum::kMaxDurationExceeded; + error_code = ErrorCodeEnum::MAX_DURATION_EXCEEDED; } // Error if not even a single waypoint could be generated if (waypoints_.positions.size() < 2) { - error_code = ErrorCodeEnum::kFailureToGenerateSingleWaypoint; + error_code = ErrorCodeEnum::FAILURE_TO_GENERATE_SINGLE_WAYPOINT; } return error_code; diff --git a/src/streaming_example.cpp b/src/streaming_example.cpp index 04d095da..5b85f7d9 100644 --- a/src/streaming_example.cpp +++ b/src/streaming_example.cpp @@ -70,15 +70,15 @@ int main(int argc, char** argv) trackjoint::ErrorCodeEnum error_code = traj_gen.inputChecking(start_state, goal_joint_states, limits, timestep); if (error_code) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } // Generate the initial trajectory error_code = traj_gen.generateTrajectories(&output_trajectories); - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } std::cout << "Initial trajectory calculation:" << std::endl; @@ -110,9 +110,9 @@ int main(int argc, char** argv) std::cout << "Run time (microseconds): " << std::chrono::duration_cast(end_time - start_time).count() << std::endl; - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } diff --git a/src/three_dof_examples.cpp b/src/three_dof_examples.cpp index 2d5a2d53..56cb26c2 100644 --- a/src/three_dof_examples.cpp +++ b/src/three_dof_examples.cpp @@ -74,9 +74,9 @@ int main(int argc, char** argv) // Input error handling - if an error is found, the trajectory is not // generated. - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } @@ -86,9 +86,9 @@ int main(int argc, char** argv) auto end = std::chrono::system_clock::now(); // Trajectory generation error handling - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } @@ -96,7 +96,7 @@ int main(int argc, char** argv) std::cout << "Runtime: " << elapsed_seconds.count() << std::endl; std::cout << "Num waypoints: " << output_trajectories.at(0).positions.size() << std::endl; - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; // Save the synchronized trajectories to .csv files traj_gen.saveTrajectoriesToFile(output_trajectories, output_path_base); @@ -167,9 +167,9 @@ int main(int argc, char** argv) // Input error handling - if an error is found, the trajectory is not // generated. - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } @@ -179,9 +179,9 @@ int main(int argc, char** argv) end = std::chrono::system_clock::now(); // Trajectory generation error handling - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } @@ -189,7 +189,7 @@ int main(int argc, char** argv) std::cout << "Runtime: " << elapsed_seconds.count() << std::endl; std::cout << "Num waypoints: " << output_trajectories.at(0).positions.size() << std::endl; - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; // Save the synchronized trajectories to .csv files traj_gen.saveTrajectoriesToFile(output_trajectories, output_path_base); diff --git a/src/trajectory_generator.cpp b/src/trajectory_generator.cpp index a3779cd3..728fd8f8 100644 --- a/src/trajectory_generator.cpp +++ b/src/trajectory_generator.cpp @@ -194,13 +194,13 @@ ErrorCodeEnum TrajectoryGenerator::inputChecking(const std::vector limits[joint].velocity_limit) { - return ErrorCodeEnum::kVelocityExceedsLimit; + return ErrorCodeEnum::VELOCITY_EXCEEDS_LIMIT; } // Check that goal vels. are less than the limits. if (abs(goal_joint_states[joint].velocity) > limits[joint].velocity_limit) { - return ErrorCodeEnum::kVelocityExceedsLimit; + return ErrorCodeEnum::VELOCITY_EXCEEDS_LIMIT; } // Check that current accels. are less than the limits. if (abs(current_joint_states[joint].acceleration) > limits[joint].acceleration_limit) { - return ErrorCodeEnum::kAccelExceedsLimit; + return ErrorCodeEnum::ACCEL_EXCEEDS_LIMIT; } // Check that goal accels. are less than the limits. if (abs(goal_joint_states[joint].acceleration) > limits[joint].acceleration_limit) { - return ErrorCodeEnum::kAccelExceedsLimit; + return ErrorCodeEnum::ACCEL_EXCEEDS_LIMIT; } // Check for positive limits. if (limits[joint].velocity_limit <= 0 || limits[joint].acceleration_limit <= 0 || limits[joint].jerk_limit <= 0) { - return ErrorCodeEnum::kLimitNotPositive; + return ErrorCodeEnum::LIMIT_NOT_POSITIVE; } // In streaming mode, the user-requested duration should be >= kNumWaypointsThreshold * timestep. // upsample and downSample aren't used in streaming mode. if (rounded_duration < kNumWaypointsThreshold * nominal_timestep && use_streaming_mode_) { - return ErrorCodeEnum::kLessThanTenTimestepsForStreamingMode; + return ErrorCodeEnum::LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE; } } - return ErrorCodeEnum::kNoError; + return ErrorCodeEnum::NO_ERROR; } void TrajectoryGenerator::saveTrajectoriesToFile(const std::vector& output_trajectories, @@ -309,7 +309,7 @@ ErrorCodeEnum TrajectoryGenerator::synchronizeTrajComponents(std::vector* trajectory) @@ -391,7 +391,7 @@ void TrajectoryGenerator::clipVectorsForOutput(std::vector* tra ErrorCodeEnum TrajectoryGenerator::generateTrajectories(std::vector* output_trajectories) { - ErrorCodeEnum error_code = ErrorCodeEnum::kNoError; + ErrorCodeEnum error_code = ErrorCodeEnum::NO_ERROR; // Generate individual joint trajectories for (size_t joint = 0; joint < kNumDof; ++joint) { diff --git a/test/trajectory_generation_test.cpp b/test/trajectory_generation_test.cpp index 535a8375..5bde2e57 100644 --- a/test/trajectory_generation_test.cpp +++ b/test/trajectory_generation_test.cpp @@ -205,7 +205,7 @@ TEST_F(TrajectoryGenerationTest, EasyDefaultTrajectory) TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration_, max_duration_, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 1e-4; @@ -252,7 +252,7 @@ TEST_F(TrajectoryGenerationTest, OneTimestepDuration) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error double position_tolerance = 1e-4; @@ -282,7 +282,7 @@ TEST_F(TrajectoryGenerationTest, RoughlyTwoTimestepDuration) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 1e-4; @@ -318,7 +318,7 @@ TEST_F(TrajectoryGenerationTest, FourTimestepDuration) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 1e-4; @@ -365,7 +365,7 @@ TEST_F(TrajectoryGenerationTest, SixTimestepDuration) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error double position_tolerance = 1e-4; @@ -405,7 +405,7 @@ TEST_F(TrajectoryGenerationTest, VelAccelJerkLimit) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); verifyVelAccelJerkLimits(output_trajectories_, limits_); @@ -589,7 +589,7 @@ TEST_F(TrajectoryGenerationTest, OscillatingUR5TrackJointCase) // Saving Trackjoint output to .csv files for plotting traj_gen.saveTrajectoriesToFile(output_trajectories_, BASE_FILEPATH + "_joint"); - EXPECT_EQ(ErrorCodeEnum::kNoError, error_code); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, error_code); // Timestep const double timestep_tolerance = 0.25 * timestep_; EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep_, @@ -637,7 +637,7 @@ TEST_F(TrajectoryGenerationTest, SuddenChangeOfDirection) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error double position_tolerance = 1e-4; @@ -683,7 +683,7 @@ TEST_F(TrajectoryGenerationTest, LimitCompensation) TrajectoryGenerator traj_gen(num_dof_, timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 1e-4; @@ -732,7 +732,7 @@ TEST_F(TrajectoryGenerationTest, DurationExtension) TrajectoryGenerator traj_gen(num_dof_, timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 1e-4; @@ -787,7 +787,7 @@ TEST_F(TrajectoryGenerationTest, PositiveAndNegativeLimits) TrajectoryGenerator traj_gen(num_dof_, timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 1e-4; @@ -831,9 +831,9 @@ TEST_F(TrajectoryGenerationTest, TimestepDidNotMatch) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); output_trajectories_.resize(num_dof_); - EXPECT_EQ(ErrorCodeEnum::kNoError, + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.inputChecking(current_joint_states_, goal_joint_states_, limits_, timestep_)); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 2e-3; @@ -892,7 +892,7 @@ TEST_F(TrajectoryGenerationTest, CustomerStreaming) TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration, max_duration_, current_joint_states_, goal_joint_states_, limits_, waypoint_position_tolerance, use_streaming_mode_); ErrorCodeEnum error_code = traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(error_code, ErrorCodeEnum::kNoError); + EXPECT_EQ(error_code, ErrorCodeEnum::NO_ERROR); double position_error = std::numeric_limits::max(); double velocity_error = std::numeric_limits::max(); @@ -904,7 +904,7 @@ TEST_F(TrajectoryGenerationTest, CustomerStreaming) traj_gen.reset(timestep_, desired_duration, max_duration_, current_joint_states_, goal_joint_states_, limits_, waypoint_position_tolerance, use_streaming_mode_); error_code = traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(error_code, ErrorCodeEnum::kNoError); + EXPECT_EQ(error_code, ErrorCodeEnum::NO_ERROR); // Get a new seed state for next trajectory generation if ((std::size_t)output_trajectories_.at(joint_to_update).positions.size() > next_waypoint) { @@ -939,7 +939,7 @@ TEST_F(TrajectoryGenerationTest, StreamingTooFewTimesteps) TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration_, max_duration_, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); - EXPECT_EQ(ErrorCodeEnum::kLessThanTenTimestepsForStreamingMode, + EXPECT_EQ(ErrorCodeEnum::LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE, traj_gen.inputChecking(current_joint_states_, goal_joint_states_, limits_, timestep_)); } } // namespace trackjoint @@ -951,4 +951,4 @@ int main(int argc, char** argv) int result = RUN_ALL_TESTS(); return result; -} \ No newline at end of file +} From 29b975b2b0507409e5b08478e0307cca928df03f Mon Sep 17 00:00:00 2001 From: Nathan Brooks Date: Wed, 20 May 2020 11:32:12 -0600 Subject: [PATCH 02/15] Clean up inline functions (#54) - Remove unnecessary inline tags - Move remaining inline function implementations into header --- include/trackjoint/single_joint_generator.h | 13 ++++++++++--- src/single_joint_generator.cpp | 21 ++++++--------------- 2 files changed, 16 insertions(+), 18 deletions(-) diff --git a/include/trackjoint/single_joint_generator.h b/include/trackjoint/single_joint_generator.h index 315b8ea5..f0f1f79a 100644 --- a/include/trackjoint/single_joint_generator.h +++ b/include/trackjoint/single_joint_generator.h @@ -83,6 +83,16 @@ class SingleJointGenerator void updateTrajectoryDuration(double new_trajectory_duration); private: + /** \brief Record the index when compensation first failed */ + inline void recordFailureTime(size_t current_index, size_t* index_last_successful) + { + // Record the index when compensation first failed + if (current_index < *index_last_successful) + { + *index_last_successful = current_index; + } + }; + /** \brief interpolate from start to end state with a polynomial * * input times a vector of waypoint times. @@ -101,9 +111,6 @@ class SingleJointGenerator * vector */ ErrorCodeEnum positionVectorLimitLookAhead(size_t* index_last_successful); - /** \brief Record the index when compensation first failed */ - inline void recordFailureTime(size_t current_index, size_t* index_last_successful); - /** \brief Check whether the duration needs to be extended, and do it */ ErrorCodeEnum predictTimeToReach(); diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index 05f4a2a4..4be76d52 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -91,7 +91,7 @@ size_t SingleJointGenerator::getLastSuccessfulIndex() return index_last_successful_; } -inline Eigen::VectorXd SingleJointGenerator::interpolate(Eigen::VectorXd& times) +Eigen::VectorXd SingleJointGenerator::interpolate(Eigen::VectorXd& times) { // See De Luca, "Trajectory Planning" pdf, slide 19 // Interpolate a smooth trajectory from initial to final state while matching @@ -122,7 +122,7 @@ inline Eigen::VectorXd SingleJointGenerator::interpolate(Eigen::VectorXd& times) return interpolated_position; } -inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_successful) +ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_successful) { // This is the indexing convention. // 1. accel(i) = accel(i-1) + jerk(i) * dt @@ -276,16 +276,7 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde return ErrorCodeEnum::NO_ERROR; } -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) +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 @@ -390,7 +381,7 @@ inline bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index return successful_compensation; } -inline ErrorCodeEnum SingleJointGenerator::predictTimeToReach() +ErrorCodeEnum SingleJointGenerator::predictTimeToReach() { // Take a trajectory that could not reach the desired position in time. // Try increasing the duration until it is interpolated without violating @@ -485,7 +476,7 @@ inline ErrorCodeEnum SingleJointGenerator::predictTimeToReach() return error_code; } -inline ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* index_last_successful) +ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* index_last_successful) { ErrorCodeEnum error_code = forwardLimitCompensation(index_last_successful); if (error_code) @@ -509,7 +500,7 @@ inline ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* return error_code; } -inline void SingleJointGenerator::calculateDerivatives() +void SingleJointGenerator::calculateDerivatives() { // From position vector, approximate velocity and acceleration. // velocity = (difference between adjacent position elements) / delta_t From 250aa4b9c37c113a587521d72425faf6f403e664 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 18 Jun 2020 18:44:00 -0500 Subject: [PATCH 03/15] Update executable names in Readme (#56) --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index ed3493cb..c9f6c8f9 100644 --- a/README.md +++ b/README.md @@ -19,7 +19,7 @@ TODO(andyze): fix Travis badge: ## Run -1. rosrun trackjoint run_example +1. rosrun trackjoint simple_example (or streaming_example) (You don't need to start `roscore` because the executable doesn't use ROS) From 962197df9f5ee79658e8f3436ab77a2a9abfdba5 Mon Sep 17 00:00:00 2001 From: John Morris Date: Thu, 17 Sep 2020 11:22:51 -0500 Subject: [PATCH 04/15] 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 --- include/trackjoint/error_codes.h | 48 +++++++++++++++------ include/trackjoint/single_joint_generator.h | 3 +- include/trackjoint/trajectory_generator.h | 8 ++-- include/trackjoint/utilities.h | 2 +- src/simple_example.cpp | 5 ++- src/three_dof_examples.cpp | 5 ++- src/trajectory_generator.cpp | 9 ++-- test/trajectory_generation_test.cpp | 12 +++--- 8 files changed, 58 insertions(+), 34 deletions(-) diff --git a/include/trackjoint/error_codes.h b/include/trackjoint/error_codes.h index f5156a06..3dde5959 100644 --- a/include/trackjoint/error_codes.h +++ b/include/trackjoint/error_codes.h @@ -37,17 +37,39 @@ enum ErrorCodeEnum /** * \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 " + "timestep" + "s" } }); } // 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..8413b2ab 100644 --- a/src/simple_example.cpp +++ b/src/simple_example.cpp @@ -29,8 +29,9 @@ 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/three_dof_examples.cpp b/src/three_dof_examples.cpp index 5ea6604b..0d1485f2 100644 --- a/src/three_dof_examples.cpp +++ b/src/three_dof_examples.cpp @@ -25,8 +25,9 @@ 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; diff --git a/src/trajectory_generator.cpp b/src/trajectory_generator.cpp index 640ea1c5..e687d20f 100644 --- a/src/trajectory_generator.cpp +++ b/src/trajectory_generator.cpp @@ -32,11 +32,10 @@ TrajectoryGenerator::TrajectoryGenerator(uint num_dof, double timestep, double d 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_, - 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)); } } diff --git a/test/trajectory_generation_test.cpp b/test/trajectory_generation_test.cpp index f247256c..2dc06d0f 100644 --- a/test/trajectory_generation_test.cpp +++ b/test/trajectory_generation_test.cpp @@ -552,12 +552,12 @@ 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_accelerations = - loadWaypointsFromFile(REF_PATH + "/test/data/30_percent_speed_oscillation/moveit_des_acc.txt"); - moveit_times_from_start = - loadWaypointsFromFile(REF_PATH + "/test/data/30_percent_speed_oscillation/moveit_time_from_start.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 = loadWaypointsFromFile(REF_PATH + "/test/data/30_percent_speed_oscillation/" + "moveit_time_from_start.txt"); // For each MoveIt waypoint for (std::size_t point = 0; point < moveit_times_from_start.size() - 1; ++point) From e12fb232ed83fc02f0b7ac6c4e5f3aefab9ae960 Mon Sep 17 00:00:00 2001 From: John Morris Date: Sun, 20 Sep 2020 01:39:42 -0500 Subject: [PATCH 05/15] Adjust clang-format rules and re-run Fix hideous string breaks pointed out by @AndyZe --- .clang-format | 2 +- include/trackjoint/error_codes.h | 49 +++++++++-------------------- src/simple_example.cpp | 5 ++- src/three_dof_examples.cpp | 5 ++- test/trajectory_generation_test.cpp | 13 ++++---- 5 files changed, 26 insertions(+), 48 deletions(-) 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/include/trackjoint/error_codes.h b/include/trackjoint/error_codes.h index 3dde5959..631fbfee 100644 --- a/include/trackjoint/error_codes.h +++ b/include/trackjoint/error_codes.h @@ -37,39 +37,18 @@ enum ErrorCodeEnum /** * \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 " - "timestep" - "s" } }); +const std::unordered_map ERROR_CODE_MAP( + { { NO_ERROR, "No error, trajectory generation was successful" }, + { DESIRED_DURATION_TOO_SHORT, + "Desired duration is too short, cannot have less than one timestep in a " + "trajectory" }, + { MAX_DURATION_EXCEEDED, "Max duration was exceeded" }, + { VELOCITY_EXCEEDS_LIMIT, "A velocity input exceeds the velocity limit" }, + { ACCEL_EXCEEDS_LIMIT, "An acceleration input exceeds the acceleration limit" }, + { MAX_DURATION_LESS_THAN_DESIRED_DURATION, "max_duration should not be less than desired_duration" }, + { LIMIT_NOT_POSITIVE, "Vel/accel/jerk limits should be greater than zero" }, + { GOAL_POSITION_MISMATCH, "Mismatch between the final position and the goal position" }, + { FAILURE_TO_GENERATE_SINGLE_WAYPOINT, "Failed to generate even a single new waypoint" }, + { LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE, + "In streaming mode, desired duration should be at least 10 timesteps" } }); } // end namespace trackjoint diff --git a/src/simple_example.cpp b/src/simple_example.cpp index 8413b2ab..76eaa556 100644 --- a/src/simple_example.cpp +++ b/src/simple_example.cpp @@ -29,9 +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/three_dof_examples.cpp b/src/three_dof_examples.cpp index 0d1485f2..7e8ef65e 100644 --- a/src/three_dof_examples.cpp +++ b/src/three_dof_examples.cpp @@ -25,9 +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; diff --git a/test/trajectory_generation_test.cpp b/test/trajectory_generation_test.cpp index 2dc06d0f..ed623c75 100644 --- a/test/trajectory_generation_test.cpp +++ b/test/trajectory_generation_test.cpp @@ -552,12 +552,13 @@ 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_accelerations = loadWaypointsFromFile(REF_PATH + "/test/data/30_percent_speed_oscillation/" - "moveit_des_acc.txt"); - moveit_times_from_start = loadWaypointsFromFile(REF_PATH + "/test/data/30_percent_speed_oscillation/" - "moveit_time_from_start.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 = + loadWaypointsFromFile(REF_PATH + "/test/data/30_percent_speed_oscillation/moveit_time_from_start.txt"); // For each MoveIt waypoint for (std::size_t point = 0; point < moveit_times_from_start.size() - 1; ++point) From daad03d055a7bc40b8d8a8adce229d7312fbd53d Mon Sep 17 00:00:00 2001 From: John Morris Date: Tue, 15 Sep 2020 12:32:49 -0500 Subject: [PATCH 06/15] single_joint_generator: Silence compiler warning MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit [...]/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_); ^~~~~~~~~~ --- 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 4739ce7e..a9c33ba0 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -135,7 +135,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; From 9bdf11c8fbc63c43a53c5d32c29919e8cd3bba06 Mon Sep 17 00:00:00 2001 From: John Morris Date: Tue, 15 Sep 2020 12:45:46 -0500 Subject: [PATCH 07/15] single_joint_generator: Silence compiler warning MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit [...]/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) ~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ --- 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 a9c33ba0..cfc45639 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -120,7 +120,7 @@ void SingleJointGenerator::extendTrajectoryDuration() // Retrieve new positions at the new times waypoints_.positions.resize(new_num_waypoints); - for (size_t idx = 0; idx < waypoints_.elapsed_times.size(); ++idx) + for (Eigen::Index idx = 0; idx < waypoints_.elapsed_times.size(); ++idx) waypoints_.positions[idx] = spline(waypoints_.elapsed_times(idx)).coeff(0); calculateDerivativesFromPosition(); From e92bb92d1b7bcf708819c5d5629cb29547dfad2c Mon Sep 17 00:00:00 2001 From: John Morris Date: Tue, 15 Sep 2020 13:06:48 -0500 Subject: [PATCH 08/15] test/trajectory_generation_test: Silence compiler warning MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit [...]/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; ^~~~~~~~~~~~~~~~~~ --- test/trajectory_generation_test.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/test/trajectory_generation_test.cpp b/test/trajectory_generation_test.cpp index ed623c75..eeb93eb5 100644 --- a/test/trajectory_generation_test.cpp +++ b/test/trajectory_generation_test.cpp @@ -430,7 +430,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_); } From 9bbf67df1a7f7f87a2dd308ac5409be902ad6166 Mon Sep 17 00:00:00 2001 From: John Morris Date: Tue, 15 Sep 2020 17:03:37 -0500 Subject: [PATCH 09/15] Remove junk files --- .catkin_tools/CATKIN_IGNORE | 0 .catkin_tools/README | 13 ------------- .catkin_tools/VERSION | 1 - .catkin_tools/profiles/default/devel_collisions.txt | 0 4 files changed, 14 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 From b3cfcbf07759f6d9a7df627bc4cadca7f46bb2ff Mon Sep 17 00:00:00 2001 From: John Morris Date: Fri, 18 Sep 2020 16:42:37 -0500 Subject: [PATCH 10/15] .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. --- .gitignore | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) 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/ + From f48cf321db82bcc46db972ab75f254474792dfbe Mon Sep 17 00:00:00 2001 From: John Morris Date: Wed, 16 Sep 2020 23:37:01 -0500 Subject: [PATCH 11/15] Change `int` to `size_t` for vector indices in tests Remove unneeded `static_cast`; prepare for upcoming changes. --- test/trajectory_generation_test.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/test/trajectory_generation_test.cpp b/test/trajectory_generation_test.cpp index eeb93eb5..b4574672 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) { @@ -566,7 +566,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]; From 159318fd381c6dd0c4740af5fd3907cbeb8ef6ed Mon Sep 17 00:00:00 2001 From: John Morris Date: Thu, 17 Sep 2020 13:18:45 -0500 Subject: [PATCH 12/15] 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 --- src/trajectory_generator.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/trajectory_generator.cpp b/src/trajectory_generator.cpp index e687d20f..cd5a7b69 100644 --- a/src/trajectory_generator.cpp +++ b/src/trajectory_generator.cpp @@ -29,7 +29,7 @@ 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( @@ -58,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], From f9fd2208ed270da80134c6389a727df6238e7ff8 Mon Sep 17 00:00:00 2001 From: John Morris Date: Thu, 17 Sep 2020 13:20:53 -0500 Subject: [PATCH 13/15] 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 & --- src/single_joint_generator.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index cfc45639..af6997e0 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -113,7 +113,6 @@ 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_); @@ -121,7 +120,7 @@ void SingleJointGenerator::extendTrajectoryDuration() waypoints_.positions.resize(new_num_waypoints); for (Eigen::Index idx = 0; idx < waypoints_.elapsed_times.size(); ++idx) - waypoints_.positions[idx] = spline(waypoints_.elapsed_times(idx)).coeff(0); + waypoints_.positions[idx] = fit(waypoints_.elapsed_times(idx)).coeff(0); calculateDerivativesFromPosition(); return; From 58fc12a674702c4b91b6510dcf2262a6ecfa3d07 Mon Sep 17 00:00:00 2001 From: John Morris Date: Fri, 18 Sep 2020 15:08:08 -0500 Subject: [PATCH 14/15] 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. --- 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 af6997e0..87f4876c 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -293,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) { From a40551fb42c6bb2c0707b0fca1536ab5e8a86a9c Mon Sep 17 00:00:00 2001 From: John Morris Date: Fri, 18 Sep 2020 15:20:14 -0500 Subject: [PATCH 15/15] 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; ^ --- test/trajectory_generation_test.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/test/trajectory_generation_test.cpp b/test/trajectory_generation_test.cpp index b4574672..95fb0d8c 100644 --- a/test/trajectory_generation_test.cpp +++ b/test/trajectory_generation_test.cpp @@ -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; }