diff --git a/.catkin_tools/CATKIN_IGNORE b/.catkin_tools/CATKIN_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/.catkin_tools/README b/.catkin_tools/README deleted file mode 100644 index 4706f475..00000000 --- a/.catkin_tools/README +++ /dev/null @@ -1,13 +0,0 @@ -# Catkin Tools Metadata - -This directory was generated by catkin_tools and it contains persistent -configuration information used by the `catkin` command and its sub-commands. - -Each subdirectory of the `profiles` directory contains a set of persistent -configuration options for separate profiles. The default profile is called -`default`. If another profile is desired, it can be described in the -`profiles.yaml` file in this directory. - -Please see the catkin_tools documentation before editing any files in this -directory. Most actions can be performed with the `catkin` command-line -program. diff --git a/.catkin_tools/VERSION b/.catkin_tools/VERSION deleted file mode 100644 index c8a5397f..00000000 --- a/.catkin_tools/VERSION +++ /dev/null @@ -1 +0,0 @@ -0.4.5 \ No newline at end of file diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt deleted file mode 100644 index e69de29b..00000000 diff --git a/.clang-format b/.clang-format index eca6051a..c3c41084 100644 --- a/.clang-format +++ b/.clang-format @@ -10,7 +10,7 @@ AllowShortLoopsOnASingleLine: false AllowShortFunctionsOnASingleLine: None AllowShortLoopsOnASingleLine: false AlwaysBreakTemplateDeclarations: true -AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakBeforeMultilineStrings: true BreakBeforeBinaryOperators: false BreakBeforeTernaryOperators: false BreakConstructorInitializersBeforeComma: true diff --git a/.gitignore b/.gitignore index 967678e8..c9a672a7 100644 --- a/.gitignore +++ b/.gitignore @@ -18,4 +18,8 @@ .DS_Store # SSH Keys (used for Docker) -id_rsa \ No newline at end of file +id_rsa + +# Catkin tools +/.catkin-tools/ + diff --git a/README.md b/README.md index ed3493cb..c9f6c8f9 100644 --- a/README.md +++ b/README.md @@ -19,7 +19,7 @@ TODO(andyze): fix Travis badge: ## Run -1. rosrun trackjoint run_example +1. rosrun trackjoint simple_example (or streaming_example) (You don't need to start `roscore` because the executable doesn't use ROS) diff --git a/include/trackjoint/error_codes.h b/include/trackjoint/error_codes.h index 92e6227d..631fbfee 100644 --- a/include/trackjoint/error_codes.h +++ b/include/trackjoint/error_codes.h @@ -22,32 +22,33 @@ namespace trackjoint */ enum ErrorCodeEnum { - kNoError = 0, - kDesiredDurationTooShort = 1, - kMaxDurationExceeded = 2, - kVelocityExceedsLimit = 3, - kAccelExceedsLimit = 4, - kMaxDurationLessThanDesiredDuration = 5, - kLimitNotPositive = 6, - kGoalPositionMismatch = 7, - kFailureToGenerateSingleWaypoint = 8, - kLessThanTenTimestepsForStreamingMode = 9 + NO_ERROR = 0, + DESIRED_DURATION_TOO_SHORT = 1, + MAX_DURATION_EXCEEDED = 2, + VELOCITY_EXCEEDS_LIMIT = 3, + ACCEL_EXCEEDS_LIMIT = 4, + MAX_DURATION_LESS_THAN_DESIRED_DURATION = 5, + LIMIT_NOT_POSITIVE = 6, + GOAL_POSITION_MISMATCH = 7, + FAILURE_TO_GENERATE_SINGLE_WAYPOINT = 8, + LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE = 9 }; /** * \brief Use this map to look up human-readable strings for each error code */ -const std::unordered_map kErrorCodeMap( - { { kNoError, "No error, trajectory generation was successful" }, - { kDesiredDurationTooShort, "Desired duration is too short, cannot have less than one timestep in a " - "trajectory" }, - { kMaxDurationExceeded, "Max duration was exceeded" }, - { kVelocityExceedsLimit, "A velocity input exceeds the velocity limit" }, - { kAccelExceedsLimit, "An acceleration input exceeds the acceleration limit" }, - { kMaxDurationLessThanDesiredDuration, "max_duration should not be less than desired_duration" }, - { kLimitNotPositive, "Vel/accel/jerk limits should be greater than zero" }, - { kGoalPositionMismatch, "Mismatch between the final position and the goal position" }, - { kFailureToGenerateSingleWaypoint, "Failed to generate even a single new waypoint" }, - { kLessThanTenTimestepsForStreamingMode, "In streaming mode, desired duration should be at least 10 " - "timesteps" } }); +const std::unordered_map ERROR_CODE_MAP( + { { NO_ERROR, "No error, trajectory generation was successful" }, + { DESIRED_DURATION_TOO_SHORT, + "Desired duration is too short, cannot have less than one timestep in a " + "trajectory" }, + { MAX_DURATION_EXCEEDED, "Max duration was exceeded" }, + { VELOCITY_EXCEEDS_LIMIT, "A velocity input exceeds the velocity limit" }, + { ACCEL_EXCEEDS_LIMIT, "An acceleration input exceeds the acceleration limit" }, + { MAX_DURATION_LESS_THAN_DESIRED_DURATION, "max_duration should not be less than desired_duration" }, + { LIMIT_NOT_POSITIVE, "Vel/accel/jerk limits should be greater than zero" }, + { GOAL_POSITION_MISMATCH, "Mismatch between the final position and the goal position" }, + { FAILURE_TO_GENERATE_SINGLE_WAYPOINT, "Failed to generate even a single new waypoint" }, + { LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE, + "In streaming mode, desired duration should be at least 10 timesteps" } }); } // end namespace trackjoint diff --git a/include/trackjoint/single_joint_generator.h b/include/trackjoint/single_joint_generator.h index 4b7553a6..382484aa 100644 --- a/include/trackjoint/single_joint_generator.h +++ b/include/trackjoint/single_joint_generator.h @@ -43,7 +43,8 @@ class SingleJointGenerator * Should be set lower than the accuracy requirements for your task * input use_streaming_mode set to true for fast streaming applications. Returns a maximum of num_waypoints_threshold * waypoints. - * input timestep_was_upsampled If upsampling happened (we are working with very few waypoints), do not adjust timestep + * input timestep_was_upsampled If upsampling happened (we are working with very few waypoints), do not adjust + * timestep */ SingleJointGenerator(double timestep, double max_duration, const KinematicState& current_joint_state, const KinematicState& goal_joint_state, const Limits& limits, size_t desired_num_waypoints, @@ -83,6 +84,16 @@ class SingleJointGenerator void updateTrajectoryDuration(double new_trajectory_duration); private: + /** \brief Record the index when compensation first failed */ + inline void recordFailureTime(size_t current_index, size_t* index_last_successful) + { + // Record the index when compensation first failed + if (current_index < *index_last_successful) + { + *index_last_successful = current_index; + } + }; + /** \brief interpolate from start to end state with a polynomial * * input times a vector of waypoint times. @@ -101,9 +112,6 @@ class SingleJointGenerator * vector */ ErrorCodeEnum positionVectorLimitLookAhead(size_t* index_last_successful); - /** \brief Record the index when compensation first failed */ - inline void recordFailureTime(size_t current_index, size_t* index_last_successful); - /** \brief Check whether the duration needs to be extended, and do it */ ErrorCodeEnum predictTimeToReach(); diff --git a/include/trackjoint/trajectory_generator.h b/include/trackjoint/trajectory_generator.h index 4e4dc14a..f2a97781 100644 --- a/include/trackjoint/trajectory_generator.h +++ b/include/trackjoint/trajectory_generator.h @@ -78,7 +78,7 @@ class TrajectoryGenerator * input limits vector of kinematic limits for each degree of freedom * input nominal_timestep the user-requested time between waypoints * returna TrackJoint status code - */ + */ ErrorCodeEnum inputChecking(const std::vector& current_joint_states, const std::vector& goal_joint_states, const std::vector& limits, double nominal_timestep); @@ -87,7 +87,7 @@ class TrajectoryGenerator /** \brief Ensure limits are obeyed before outputting. * * input trajectory the calculated trajectories for n joints - */ + */ void clipVectorsForOutput(std::vector* trajectory); /** \brief upsample if num. waypoints would be short. Helps with accuracy. */ @@ -100,7 +100,7 @@ class TrajectoryGenerator * input velocity_vector a vector of velocities * input acceleration_vector a vector of accelerations * input jerk_vector a vector of jerks - */ + */ void downSample(Eigen::VectorXd* time_vector, Eigen::VectorXd* position_vector, Eigen::VectorXd* velocity_vector, Eigen::VectorXd* acceleration_vector, Eigen::VectorXd* jerk_vector); @@ -108,7 +108,7 @@ class TrajectoryGenerator * * input output_trajectories the calculated trajectories for n joints * returna TrackJoint status code - */ + */ ErrorCodeEnum synchronizeTrajComponents(std::vector* output_trajectories); // TODO(andyz): set this back to a small number when done testing diff --git a/include/trackjoint/utilities.h b/include/trackjoint/utilities.h index 0357c42e..dd7dfaac 100644 --- a/include/trackjoint/utilities.h +++ b/include/trackjoint/utilities.h @@ -35,7 +35,7 @@ Eigen::VectorXd DiscreteDifferentiation(const Eigen::VectorXd& input_vector, dou * input joint the index of a joint * input output_trajectories the calculated trajectories for n joints * input desired_duration the user-requested duration of the trajectory -*/ + */ void PrintJointTrajectory(const std::size_t joint, const std::vector& output_trajectories, const double desired_duration); diff --git a/src/simple_example.cpp b/src/simple_example.cpp index 237ce1fe..76eaa556 100644 --- a/src/simple_example.cpp +++ b/src/simple_example.cpp @@ -29,8 +29,8 @@ int main(int argc, char** argv) // the whole trajectory at once. constexpr bool use_streaming_mode = false; // Optional logging of TrackJoint output - const std::string output_path_base = "/home/" + std::string(getenv("USER")) + "/Downloads/trackjoint_data/" - "output_joint"; + const std::string output_path_base = + "/home/" + std::string(getenv("USER")) + "/Downloads/trackjoint_data/output_joint"; std::vector current_joint_states(1); trackjoint::KinematicState joint_state; @@ -73,9 +73,9 @@ int main(int argc, char** argv) traj_gen.inputChecking(current_joint_states, goal_joint_states, limits, timestep); // Input error handling - if an error is found, the trajectory is not generated. - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } @@ -86,9 +86,9 @@ int main(int argc, char** argv) auto end = std::chrono::system_clock::now(); // Trajectory generation error handling - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } @@ -96,7 +96,7 @@ int main(int argc, char** argv) std::cout << "Runtime: " << elapsed_seconds.count() << std::endl; std::cout << "Num waypoints: " << output_trajectories.at(0).positions.size() << std::endl; - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; // Save the synchronized trajectories to .csv files traj_gen.saveTrajectoriesToFile(output_trajectories, output_path_base); diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index 703366f8..87f4876c 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -113,15 +113,14 @@ void SingleJointGenerator::extendTrajectoryDuration() Eigen::RowVectorXd position(waypoints_.positions); const auto fit = SplineFitting1D::Interpolate(position, 2, new_times); - Spline1D spline(fit); // New times, with the extended duration waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., (new_num_waypoints - 1) * timestep_); // Retrieve new positions at the new times waypoints_.positions.resize(new_num_waypoints); - for (size_t idx = 0; idx < waypoints_.elapsed_times.size(); ++idx) - waypoints_.positions[idx] = spline(waypoints_.elapsed_times(idx)).coeff(0); + for (Eigen::Index idx = 0; idx < waypoints_.elapsed_times.size(); ++idx) + waypoints_.positions[idx] = fit(waypoints_.elapsed_times(idx)).coeff(0); calculateDerivativesFromPosition(); return; @@ -135,7 +134,7 @@ void SingleJointGenerator::extendTrajectoryDuration() waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., (new_num_waypoints - 1) * timestep_); waypoints_.positions = interpolate(waypoints_.elapsed_times); calculateDerivativesFromPosition(); - ErrorCodeEnum error_code = forwardLimitCompensation(&index_last_successful_); + forwardLimitCompensation(&index_last_successful_); } return; @@ -151,7 +150,7 @@ size_t SingleJointGenerator::getLastSuccessfulIndex() return index_last_successful_; } -inline Eigen::VectorXd SingleJointGenerator::interpolate(Eigen::VectorXd& times) +Eigen::VectorXd SingleJointGenerator::interpolate(Eigen::VectorXd& times) { // See De Luca, "Trajectory Planning" pdf, slide 19 // Interpolate a smooth trajectory from initial to final state while matching @@ -182,7 +181,7 @@ inline Eigen::VectorXd SingleJointGenerator::interpolate(Eigen::VectorXd& times) return interpolated_position; } -inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_successful) +ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_successful) { // This is the indexing convention. // 1. accel(i) = accel(i-1) + jerk(i) * dt @@ -294,8 +293,6 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde position_error = 0; for (size_t index = 1; index < *index_last_successful; ++index) { - delta_v = 0; - // If the velocity limit would be exceeded if (fabs(waypoints_.velocities(index)) > velocity_limit) { @@ -329,19 +326,10 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde // Re-calculate derivatives from the updated velocity vector calculateDerivativesFromVelocity(); - return ErrorCodeEnum::kNoError; -} - -inline void SingleJointGenerator::recordFailureTime(size_t current_index, size_t* index_last_successful) -{ - // Record the index when compensation first failed - if (current_index < *index_last_successful) - { - *index_last_successful = current_index; - } + return ErrorCodeEnum::NO_ERROR; } -inline bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, double excess_velocity) +bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, double excess_velocity) { // The algorithm: // 1) check jerk limits, from beginning to end of trajectory. Don't bother @@ -445,13 +433,13 @@ inline bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index return successful_compensation; } -inline ErrorCodeEnum SingleJointGenerator::predictTimeToReach() +ErrorCodeEnum SingleJointGenerator::predictTimeToReach() { // Take a trajectory that could not reach the desired position in time. // Try increasing the duration until it is interpolated without violating limits. // This gives a new duration estimate. - ErrorCodeEnum error_code = ErrorCodeEnum::kNoError; + ErrorCodeEnum error_code = ErrorCodeEnum::NO_ERROR; // If in normal mode, we can extend trajectories if (!use_streaming_mode_) @@ -528,18 +516,18 @@ inline ErrorCodeEnum SingleJointGenerator::predictTimeToReach() // Normal mode: Error if we extended the duration to the maximum and it still wasn't successful if (!use_streaming_mode_ && index_last_successful_ < static_cast(waypoints_.elapsed_times.size() - 1)) { - error_code = ErrorCodeEnum::kMaxDurationExceeded; + error_code = ErrorCodeEnum::MAX_DURATION_EXCEEDED; } // Error if not even a single waypoint could be generated if (waypoints_.positions.size() < 2) { - error_code = ErrorCodeEnum::kFailureToGenerateSingleWaypoint; + error_code = ErrorCodeEnum::FAILURE_TO_GENERATE_SINGLE_WAYPOINT; } return error_code; } -inline ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* index_last_successful) +ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* index_last_successful) { ErrorCodeEnum error_code = forwardLimitCompensation(index_last_successful); if (error_code) @@ -570,7 +558,7 @@ inline ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* return error_code; } -inline void SingleJointGenerator::calculateDerivativesFromPosition() +void SingleJointGenerator::calculateDerivativesFromPosition() { // From position vector, approximate vel/accel/jerk. waypoints_.velocities = DiscreteDifferentiation(waypoints_.positions, timestep_, current_joint_state_.velocity); @@ -579,7 +567,7 @@ inline void SingleJointGenerator::calculateDerivativesFromPosition() waypoints_.jerks = DiscreteDifferentiation(waypoints_.accelerations, timestep_, 0); } -inline void SingleJointGenerator::calculateDerivativesFromVelocity() +void SingleJointGenerator::calculateDerivativesFromVelocity() { // From velocity vector, approximate accel/jerk. waypoints_.accelerations = diff --git a/src/streaming_example.cpp b/src/streaming_example.cpp index fe41820e..c8d72642 100644 --- a/src/streaming_example.cpp +++ b/src/streaming_example.cpp @@ -70,15 +70,15 @@ int main(int argc, char** argv) trackjoint::ErrorCodeEnum error_code = traj_gen.inputChecking(start_state, goal_joint_states, limits, timestep); if (error_code) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } // Generate the initial trajectory error_code = traj_gen.generateTrajectories(&output_trajectories); - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } std::cout << "Initial trajectory calculation:" << std::endl; @@ -110,9 +110,9 @@ int main(int argc, char** argv) std::cout << "Run time (microseconds): " << std::chrono::duration_cast(end_time - start_time).count() << std::endl; - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } diff --git a/src/three_dof_examples.cpp b/src/three_dof_examples.cpp index 796329ad..7e8ef65e 100644 --- a/src/three_dof_examples.cpp +++ b/src/three_dof_examples.cpp @@ -25,8 +25,8 @@ int main(int argc, char** argv) constexpr bool use_streaming_mode = false; // Position tolerance for each waypoint constexpr double waypoint_position_tolerance = 1e-4; - const std::string output_path_base = "/home/" + std::string(getenv("USER")) + "/Downloads/trackjoint_data/" - "output_joint"; + const std::string output_path_base = + "/home/" + std::string(getenv("USER")) + "/Downloads/trackjoint_data/output_joint"; std::vector current_joint_states(3); trackjoint::KinematicState joint_state; @@ -80,9 +80,9 @@ int main(int argc, char** argv) // Input error handling - if an error is found, the trajectory is not // generated. - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } @@ -92,9 +92,9 @@ int main(int argc, char** argv) auto end = std::chrono::system_clock::now(); // Trajectory generation error handling - if (error_code != trackjoint::ErrorCodeEnum::kNoError) + if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; return -1; } @@ -102,7 +102,7 @@ int main(int argc, char** argv) std::cout << "Runtime: " << elapsed_seconds.count() << std::endl; std::cout << "Num waypoints: " << output_trajectories.at(0).positions.size() << std::endl; - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; // Save the synchronized trajectories to .csv files traj_gen.saveTrajectoriesToFile(output_trajectories, output_path_base); diff --git a/src/trajectory_generator.cpp b/src/trajectory_generator.cpp index 25466184..cd5a7b69 100644 --- a/src/trajectory_generator.cpp +++ b/src/trajectory_generator.cpp @@ -29,14 +29,13 @@ TrajectoryGenerator::TrajectoryGenerator(uint num_dof, double timestep, double d upsample(); // Initialize a trajectory generator for each joint - bool timestep_was_upsampled = (upsample_rounds_ > 0) ? true : false; + bool timestep_was_upsampled = upsample_rounds_ > 0; for (size_t joint = 0; joint < kNumDof; ++joint) { - single_joint_generators_.push_back( - SingleJointGenerator(upsampled_timestep_, max_duration_, current_joint_states[joint], goal_joint_states[joint], - limits[joint], upsampled_num_waypoints_, kNumWaypointsThreshold, - kMaxNumWaypointsFullTrajectory, position_tolerance, use_streaming_mode_, - timestep_was_upsampled)); + single_joint_generators_.push_back(SingleJointGenerator( + upsampled_timestep_, max_duration_, current_joint_states[joint], goal_joint_states[joint], limits[joint], + upsampled_num_waypoints_, kNumWaypointsThreshold, kMaxNumWaypointsFullTrajectory, position_tolerance, + use_streaming_mode_, timestep_was_upsampled)); } } @@ -59,7 +58,7 @@ void TrajectoryGenerator::reset(double timestep, double desired_duration, double upsample(); // Initialize a trajectory generator for each joint - bool timestep_was_upsampled = (upsample_rounds_ > 0) ? true : false; + bool timestep_was_upsampled = upsample_rounds_ > 0; for (size_t joint = 0; joint < kNumDof; ++joint) { single_joint_generators_[joint].reset(upsampled_timestep_, max_duration_, current_joint_states[joint], @@ -203,13 +202,13 @@ ErrorCodeEnum TrajectoryGenerator::inputChecking(const std::vector limits[joint].velocity_limit) { - return ErrorCodeEnum::kVelocityExceedsLimit; + return ErrorCodeEnum::VELOCITY_EXCEEDS_LIMIT; } // Check that goal vels. are less than the limits. if (abs(goal_joint_states[joint].velocity) > limits[joint].velocity_limit) { - return ErrorCodeEnum::kVelocityExceedsLimit; + return ErrorCodeEnum::VELOCITY_EXCEEDS_LIMIT; } // Check that current accels. are less than the limits. if (abs(current_joint_states[joint].acceleration) > limits[joint].acceleration_limit) { - return ErrorCodeEnum::kAccelExceedsLimit; + return ErrorCodeEnum::ACCEL_EXCEEDS_LIMIT; } // Check that goal accels. are less than the limits. if (abs(goal_joint_states[joint].acceleration) > limits[joint].acceleration_limit) { - return ErrorCodeEnum::kAccelExceedsLimit; + return ErrorCodeEnum::ACCEL_EXCEEDS_LIMIT; } // Check for positive limits. if (limits[joint].velocity_limit <= 0 || limits[joint].acceleration_limit <= 0 || limits[joint].jerk_limit <= 0) { - return ErrorCodeEnum::kLimitNotPositive; + return ErrorCodeEnum::LIMIT_NOT_POSITIVE; } // In streaming mode, the user-requested duration should be >= kNumWaypointsThreshold * timestep. // upsample and downSample aren't used in streaming mode. if (rounded_duration < kNumWaypointsThreshold * nominal_timestep && use_streaming_mode_) { - return ErrorCodeEnum::kLessThanTenTimestepsForStreamingMode; + return ErrorCodeEnum::LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE; } } - return ErrorCodeEnum::kNoError; + return ErrorCodeEnum::NO_ERROR; } void TrajectoryGenerator::saveTrajectoriesToFile(const std::vector& output_trajectories, @@ -318,7 +317,7 @@ ErrorCodeEnum TrajectoryGenerator::synchronizeTrajComponents(std::vector* trajectory) @@ -403,7 +402,7 @@ void TrajectoryGenerator::clipVectorsForOutput(std::vector* tra ErrorCodeEnum TrajectoryGenerator::generateTrajectories(std::vector* output_trajectories) { - ErrorCodeEnum error_code = ErrorCodeEnum::kNoError; + ErrorCodeEnum error_code = ErrorCodeEnum::NO_ERROR; // Generate individual joint trajectories for (size_t joint = 0; joint < kNumDof; ++joint) { diff --git a/test/trajectory_generation_test.cpp b/test/trajectory_generation_test.cpp index 2075b44a..95fb0d8c 100644 --- a/test/trajectory_generation_test.cpp +++ b/test/trajectory_generation_test.cpp @@ -67,7 +67,7 @@ class TrajectoryGenerationTest : public ::testing::Test // Default test parameters for 3 joints double timestep_ = 0.01; double desired_duration_ = 1; - int num_dof_ = 3; + size_t num_dof_ = 3; double max_duration_ = 10; std::vector current_joint_states_, goal_joint_states_; std::vector limits_; @@ -78,7 +78,7 @@ class TrajectoryGenerationTest : public ::testing::Test void checkBounds() { - for (int i = 0; i < static_cast(output_trajectories_.size()); ++i) + for (size_t i = 0; i < output_trajectories_.size(); ++i) { if (output_trajectories_[i].elapsed_times.size() == 0) { @@ -148,7 +148,13 @@ class TrajectoryGenerationTest : public ::testing::Test final_positions(joint) = trajectory.at(joint).positions((trajectory.at(joint).positions.size() - 1)); } - double error = (final_positions - goal_positions).norm(); + // Make clang-tidy happy about taking the norm() of a zero-length + // vector + Eigen::VectorXd errors = final_positions - goal_positions; + if (errors.size() < 1) + return 0; + + double error = (errors).norm(); return error; } @@ -205,7 +211,7 @@ TEST_F(TrajectoryGenerationTest, EasyDefaultTrajectory) TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration_, max_duration_, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 1e-4; @@ -252,7 +258,7 @@ TEST_F(TrajectoryGenerationTest, OneTimestepDuration) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error double position_tolerance = 1e-4; @@ -286,7 +292,7 @@ TEST_F(TrajectoryGenerationTest, RoughlyTwoTimestepDuration) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 1e-4; @@ -322,7 +328,7 @@ TEST_F(TrajectoryGenerationTest, FourTimestepDuration) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 1e-4; @@ -373,7 +379,7 @@ TEST_F(TrajectoryGenerationTest, SixTimestepDuration) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error double position_tolerance = 1e-4; @@ -417,7 +423,7 @@ TEST_F(TrajectoryGenerationTest, VelAccelJerkLimit) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); verifyVelAccelJerkLimits(output_trajectories_, limits_); @@ -430,7 +436,6 @@ TEST_F(TrajectoryGenerationTest, VelAccelJerkLimit) EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep_, timestep_tolerance); // Duration - const double duration_tolerance = 5e-3; size_t vector_length = output_trajectories_[0].elapsed_times.size() - 1; EXPECT_LE(output_trajectories_[0].elapsed_times(vector_length), desired_duration_); } @@ -552,8 +557,9 @@ TEST_F(TrajectoryGenerationTest, OscillatingUR5TrackJointCase) // Reading MoveIt experimental data from .txt files moveit_des_positions = loadWaypointsFromFile(REF_PATH + "/test/data/30_percent_speed_oscillation/moveit_des_pos.txt"); - moveit_des_velocities = - loadWaypointsFromFile(REF_PATH + "/test/data/30_percent_speed_oscillation/moveit_des_vel.txt"); + moveit_des_velocities = loadWaypointsFromFile(REF_PATH + + "/test/data/30_percent_speed_oscillation/" + "moveit_des_vel.txt"); moveit_des_accelerations = loadWaypointsFromFile(REF_PATH + "/test/data/30_percent_speed_oscillation/moveit_des_acc.txt"); moveit_times_from_start = @@ -566,7 +572,7 @@ TEST_F(TrajectoryGenerationTest, OscillatingUR5TrackJointCase) goal_joint_states_.clear(); // for each joint - for (int joint = 0; joint < num_dof_; ++joint) + for (size_t joint = 0; joint < num_dof_; ++joint) { // Save the start state of the robot joint_state.position = moveit_des_positions[point][joint]; @@ -605,7 +611,7 @@ TEST_F(TrajectoryGenerationTest, OscillatingUR5TrackJointCase) // Saving Trackjoint output to .csv files for plotting traj_gen.saveTrajectoriesToFile(output_trajectories_, BASE_FILEPATH + "_joint"); - EXPECT_EQ(ErrorCodeEnum::kNoError, error_code); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, error_code); // Timestep const double timestep_tolerance = 0.25 * timestep_; EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep_, @@ -653,7 +659,7 @@ TEST_F(TrajectoryGenerationTest, SuddenChangeOfDirection) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error double position_tolerance = 1e-4; @@ -703,7 +709,7 @@ TEST_F(TrajectoryGenerationTest, LimitCompensation) TrajectoryGenerator traj_gen(num_dof_, timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 1e-4; @@ -756,7 +762,7 @@ TEST_F(TrajectoryGenerationTest, DurationExtension) TrajectoryGenerator traj_gen(num_dof_, timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 5e-4; @@ -814,7 +820,7 @@ TEST_F(TrajectoryGenerationTest, PositiveAndNegativeLimits) TrajectoryGenerator traj_gen(num_dof_, timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 1e-4; @@ -862,9 +868,9 @@ TEST_F(TrajectoryGenerationTest, TimestepDidNotMatch) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); output_trajectories_.resize(num_dof_); - EXPECT_EQ(ErrorCodeEnum::kNoError, + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.inputChecking(current_joint_states_, goal_joint_states_, limits_, timestep_)); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 2e-3; @@ -923,7 +929,7 @@ TEST_F(TrajectoryGenerationTest, CustomerStreaming) TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration, max_duration_, current_joint_states_, goal_joint_states_, limits_, waypoint_position_tolerance, use_streaming_mode_); ErrorCodeEnum error_code = traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(error_code, ErrorCodeEnum::kNoError); + EXPECT_EQ(error_code, ErrorCodeEnum::NO_ERROR); double position_error = std::numeric_limits::max(); double velocity_error = std::numeric_limits::max(); @@ -935,7 +941,7 @@ TEST_F(TrajectoryGenerationTest, CustomerStreaming) traj_gen.reset(timestep_, desired_duration, max_duration_, current_joint_states_, goal_joint_states_, limits_, waypoint_position_tolerance, use_streaming_mode_); error_code = traj_gen.generateTrajectories(&output_trajectories_); - EXPECT_EQ(error_code, ErrorCodeEnum::kNoError); + EXPECT_EQ(error_code, ErrorCodeEnum::NO_ERROR); // Get a new seed state for next trajectory generation if ((std::size_t)output_trajectories_.at(joint_to_update).positions.size() > next_waypoint) { @@ -970,7 +976,7 @@ TEST_F(TrajectoryGenerationTest, StreamingTooFewTimesteps) TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration_, max_duration_, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); - EXPECT_EQ(ErrorCodeEnum::kLessThanTenTimestepsForStreamingMode, + EXPECT_EQ(ErrorCodeEnum::LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE, traj_gen.inputChecking(current_joint_states_, goal_joint_states_, limits_, timestep_)); } @@ -1007,9 +1013,9 @@ TEST_F(TrajectoryGenerationTest, SingleJointOscillation) goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); output_trajectories_.resize(num_dof_); - EXPECT_EQ(ErrorCodeEnum::kNoError, + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.inputChecking(current_joint_states_, goal_joint_states_, limits_, timestep_)); - EXPECT_EQ(ErrorCodeEnum::kNoError, traj_gen.generateTrajectories(&output_trajectories_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error const double position_tolerance = 2e-3; @@ -1029,4 +1035,4 @@ int main(int argc, char** argv) int result = RUN_ALL_TESTS(); return result; -} \ No newline at end of file +}