diff --git a/CMakeLists.txt b/CMakeLists.txt index 042200cf..9c0c6483 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -155,6 +155,15 @@ if(CATKIN_ENABLE_TESTING) ${catkin_LIBRARIES} ) + add_rostest_gtest(algorithm_test + test/algorithm_test.test + test/algorithm_test.cpp + ) + target_link_libraries(algorithm_test + ${LIBRARY_NAME} + ${catkin_LIBRARIES} + ) + endif() ## Test for correct C++ source code 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/include/trackjoint/single_joint_generator.h b/include/trackjoint/single_joint_generator.h index 27b6bb6c..ae1092c3 100644 --- a/include/trackjoint/single_joint_generator.h +++ b/include/trackjoint/single_joint_generator.h @@ -95,7 +95,7 @@ class SingleJointGenerator /** \brief Start looking back through a velocity vector to calculate for an * excess velocity at limited_index. */ - bool backwardLimitCompensation(size_t limited_index, double* excess_velocity); + bool backwardLimitCompensation(size_t limited_index, double excess_velocity); /** \brief This uses backwardLimitCompensation() but it starts from a position * vector */ @@ -132,5 +132,7 @@ class SingleJointGenerator // successfully. // In streaming mode, trajectory duration is not extended until it successfully reaches the goal. bool use_streaming_mode_; + + friend class AlgorithmTest; }; // end class SingleJointGenerator } // namespace trackjoint diff --git a/include/trackjoint/trajectory_generator.h b/include/trackjoint/trajectory_generator.h index 4e4dc14a..9dc26ba8 100644 --- a/include/trackjoint/trajectory_generator.h +++ b/include/trackjoint/trajectory_generator.h @@ -127,5 +127,7 @@ class TrajectoryGenerator std::vector single_joint_generators_; size_t upsampled_num_waypoints_; size_t upsample_rounds_ = 0; // Every time we upsample, timestep is halved. Track this. -}; // end class TrajectoryGenerator + + friend class AlgorithmTest; +}; // end class TrajectoryGenerator } // 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 65db8bc4..2830dbdf 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 @@ -167,7 +167,7 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde delta_v = 0.5 * delta_j * timestep_ * timestep_; // Try adjusting the velocity in previous timesteps to compensate for this limit, if needed - successful_compensation = backwardLimitCompensation(index, &delta_v); + successful_compensation = backwardLimitCompensation(index, -delta_v); if (!successful_compensation) { position_error = position_error + delta_v * timestep_; @@ -218,7 +218,7 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde // Try adjusting the velocity in previous timesteps to compensate for this limit, if needed delta_v = delta_a * timestep_; - successful_compensation = backwardLimitCompensation(index, &delta_v); + successful_compensation = backwardLimitCompensation(index, -delta_v); if (!successful_compensation) { position_error = position_error + delta_v * timestep_; @@ -252,7 +252,7 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde // Try adjusting the velocity in previous timesteps to compensate for this limit. // Try to account for position error, too. delta_v += position_error / timestep_; - successful_compensation = backwardLimitCompensation(index, &delta_v); + successful_compensation = backwardLimitCompensation(index, -delta_v); if (!successful_compensation) { position_error = position_error + delta_v * timestep_; @@ -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) @@ -285,7 +285,7 @@ inline void SingleJointGenerator::recordFailureTime(size_t current_index, size_t } } -inline bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, double* excess_velocity) +bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, double excess_velocity) { // The algorithm: // 1) check jerk limits, from beginning to end of trajectory. Don't bother @@ -306,10 +306,10 @@ inline bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index if (fabs(waypoints_.velocities(index)) < limits_.velocity_limit) { // If the full change can be made in this timestep - if ((*excess_velocity > 0 && waypoints_.velocities(index) <= limits_.velocity_limit - *excess_velocity) || - (*excess_velocity < 0 && waypoints_.velocities(index) >= -limits_.velocity_limit - *excess_velocity)) + if ((excess_velocity > 0 && waypoints_.velocities(index) <= limits_.velocity_limit - excess_velocity) || + (excess_velocity < 0 && waypoints_.velocities(index) >= -limits_.velocity_limit - excess_velocity)) { - double new_velocity = waypoints_.velocities(index) + *excess_velocity; + double new_velocity = waypoints_.velocities(index) + excess_velocity; // Accel and jerk, calculated from the previous waypoints double backward_accel = (new_velocity - waypoints_.velocities(index - 1)) / timestep_; double backward_jerk = @@ -348,7 +348,7 @@ inline bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index { // This is what accel and jerk would be if we set velocity(index) to the // limit - double new_velocity = std::copysign(1.0, *excess_velocity) * limits_.velocity_limit; + double new_velocity = std::copysign(1.0, excess_velocity) * limits_.velocity_limit; // Accel and jerk, calculated from the previous waypoints double backward_accel = (new_velocity - waypoints_.velocities(index - 1)) / timestep_; double backward_jerk = @@ -378,7 +378,7 @@ inline bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index waypoints_.accelerations(index) = backward_accel; waypoints_.jerks(index) = backward_jerk; } - *excess_velocity = *excess_velocity - delta_v; + excess_velocity -= delta_v; } } } @@ -390,13 +390,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_) @@ -473,18 +473,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) @@ -508,7 +508,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); @@ -517,7 +517,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 04d095da..8417a181 100644 --- a/src/streaming_example.cpp +++ b/src/streaming_example.cpp @@ -39,7 +39,7 @@ int main(int argc, char** argv) constexpr double final_acceleration_tolerance = 1e-1; // For streaming mode, it is important to keep the desired duration >=10 timesteps. // Otherwise, an error will be thrown. This helps with accuracy - constexpr double min_desired_duration = 10 * timestep; + constexpr double min_desired_duration = 1 * timestep; // Between TrackJoint iterations, move ahead this many waypoints along the trajectory. constexpr std::size_t next_waypoint = 1; @@ -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/algorithm_test.cpp b/test/algorithm_test.cpp new file mode 100644 index 00000000..b2566df8 --- /dev/null +++ b/test/algorithm_test.cpp @@ -0,0 +1,147 @@ +/********************************************************************* + * Software License Agreement + * + * Copyright (c) 2020, PickNik Consulting + * All rights reserved. + * + *********************************************************************/ + +/* Author: Andy Zelenak + Desc: Unit testing of TrackJoint functions +*/ + +// C++ +#include +#include +#include +#include +#include + +// Testing +#include + +// Target testing library +#include +#include +#include "ros/package.h" +#include "ros/ros.h" + +namespace trackjoint +{ +class AlgorithmTest : public ::testing::Test +{ +public: + AlgorithmTest() : output_trajectories_(num_dof_) + { + KinematicState joint_state; + joint_state.position = -1; + joint_state.velocity = 0; + joint_state.acceleration = 0; + current_joint_states_.push_back(joint_state); + + joint_state.position = -0.995; + joint_state.velocity = 0; + joint_state.acceleration = 0; + goal_joint_states_.push_back(joint_state); + + Limits single_joint_limits; + single_joint_limits.velocity_limit = 1; + single_joint_limits.acceleration_limit = 100; + single_joint_limits.jerk_limit = 1000; + limits_.push_back(single_joint_limits); + } + +protected: + // Default test parameters for 3 joints + double timestep_ = 0.01; + double desired_duration_ = 10 * timestep_; + int num_dof_ = 1; + double max_duration_ = desired_duration_; + std::vector current_joint_states_, goal_joint_states_; + std::vector limits_; + double position_tolerance_ = 1e-8; + bool use_streaming_mode_ = false; + bool write_output_ = true; + std::vector output_trajectories_; + + void runBackwardLimitCompensationTest(Eigen::VectorXd original_velocities, size_t limited_index, + Eigen::VectorXd input_velocities) + { + EXPECT_EQ(original_velocities.size(), input_velocities.size()); + + // TODO(1035): Ideally this TrajectoryGenerator would be a local fixture variable, but its constructor would then + // access uninitialized vectors + TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration_, max_duration_, current_joint_states_, + goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); + + // Set up input velocities as generator's trajectory + size_t num_steps = input_velocities.size(); + traj_gen.single_joint_generators_[0].waypoints_.velocities.resize(num_steps); + traj_gen.single_joint_generators_[0].waypoints_.velocities = input_velocities; + traj_gen.single_joint_generators_[0].waypoints_.positions.resize(num_steps); + traj_gen.single_joint_generators_[0].waypoints_.accelerations.resize(num_steps); + traj_gen.single_joint_generators_[0].waypoints_.jerks.resize(num_steps); + + // Have generator compensate for velocity limit applied at limited_index + double delta_v = input_velocities[limited_index] - original_velocities[limited_index]; + double success = traj_gen.single_joint_generators_[0].backwardLimitCompensation(limited_index, -delta_v); + EXPECT_TRUE(success); + + // Check that sum(original_velocities) == sum(compensated_velocities) + // This is equivalent to testing that the correct distance was traveled + EXPECT_NEAR(original_velocities.sum(), traj_gen.single_joint_generators_[0].waypoints_.velocities.sum(), 1e-6); + } +}; // class AlgorithmTest + +TEST_F(AlgorithmTest, PositiveVelocityLimitBackwardLimitCompensationTest) +{ + // Test that backwardLimitCompensation compensates for delta_velocity after a velocity > limit has been limited + + size_t num_steps = 21; + // The intended velocity: + Eigen::VectorXd original_velocities(num_steps); + original_velocities << 0.997, 0.998, 0.999, 0.99987, 0.99988, 0.99989, 0.99990, 0.99991, 0.99992, 0.99993, 0.99994, + 0.99995, 0.99996, 0.99997, 0.99998, 0.99999, 1.0, 1.00001, 1.00001, 1.00001, 1.00001; + + // Limit element 17, which is over the velocity limit + size_t limited_index = 17; + + // Element 17 has been set to the limit (1.0), backwardLimitCompensation should make up for this + Eigen::VectorXd limited_velocities(num_steps); + limited_velocities << 0.997, 0.998, 0.999, 0.99987, 0.99988, 0.99989, 0.99990, 0.99991, 0.99992, 0.99993, 0.99994, + 0.99995, 0.99996, 0.99997, 0.99998, 0.99999, 1.0, 1.0 /* changed */, 1.00001, 1.00001, 1.00001; + + runBackwardLimitCompensationTest(original_velocities, limited_index, limited_velocities); +} + +TEST_F(AlgorithmTest, NegativeVelocityLimitBackwardLimitCompensationTest) +{ + // Test that backwardLimitCompensation compensates for delta_velocity after a velocity < -limit has been limited + + size_t num_steps = 21; + // The intended velocity: + Eigen::VectorXd original_velocities(num_steps); + original_velocities << -0.997, -0.998, -0.999, -0.99987, -0.99988, -0.99989, -0.99990, -0.99991, -0.99992, -0.99993, + -0.99994, -0.99995, -0.99996, -0.99997, -0.99998, -0.99999, -1.0, -1.00001, -1.00001, -1.00001, -1.00001; + + // Limit element 17, which is under the negative velocity limit + size_t limited_index = 17; + + // Element 17 has been set to the limit (-1.0), backwardLimitCompensation should make up for this + Eigen::VectorXd limited_velocities(num_steps); + limited_velocities << -0.997, -0.998, -0.999, -0.99987, -0.99988, -0.99989, -0.99990, -0.99991, -0.99992, -0.99993, + -0.99994, -0.99995, -0.99996, -0.99997, -0.99998, -0.99999, -1.0, -1.0 /* changed */, -1.00001, -1.00001, + -1.00001; + + runBackwardLimitCompensationTest(original_velocities, limited_index, limited_velocities); +} +} // namespace trackjoint + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + + int result = RUN_ALL_TESTS(); + + return result; +} diff --git a/test/algorithm_test.test b/test/algorithm_test.test new file mode 100644 index 00000000..93a7c12b --- /dev/null +++ b/test/algorithm_test.test @@ -0,0 +1,5 @@ + + + + + diff --git a/test/trajectory_generation_test.cpp b/test/trajectory_generation_test.cpp index c040806f..868d4609 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_); @@ -586,10 +586,7 @@ TEST_F(TrajectoryGenerationTest, OscillatingUR5TrackJointCase) ErrorCodeEnum error_code = traj_gen.generateTrajectories(&output_trajectories_); - // 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 +634,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 +680,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 +729,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 +784,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 +828,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; @@ -844,7 +841,7 @@ TEST_F(TrajectoryGenerationTest, TimestepDidNotMatch) EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep_, timestep_tolerance); } -/* + TEST_F(TrajectoryGenerationTest, CustomerStreaming) { // A customer-requested streaming test. @@ -862,7 +859,7 @@ TEST_F(TrajectoryGenerationTest, CustomerStreaming) constexpr double final_position_tolerance = 1e-5; constexpr double final_velocity_tolerance = 1e-3; constexpr double final_acceleration_tolerance = 1e-2; - const double min_desired_duration = 10 * timestep_; + const double min_desired_duration = timestep_; // Between iterations, skip this many waypoints. // Take next_waypoint from the previous trajectory to start the new trajectory. // Minimum is 1. @@ -892,7 +889,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 +901,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) { @@ -929,7 +926,7 @@ TEST_F(TrajectoryGenerationTest, CustomerStreaming) // If the test gets here, it passed. } -*/ + TEST_F(TrajectoryGenerationTest, StreamingTooFewTimesteps) { // An error should be thrown if streaming mode is enabled with a desired duration < kMinNumTimesteps @@ -939,7 +936,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 +948,4 @@ int main(int argc, char** argv) int result = RUN_ALL_TESTS(); return result; -} \ No newline at end of file +}