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/CMakeLists.txt b/CMakeLists.txt index 042200cf..fa80d4ba 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -155,6 +155,7 @@ if(CATKIN_ENABLE_TESTING) ${catkin_LIBRARIES} ) + add_rostest(test/test_examples.test) endif() ## Test for correct C++ source code diff --git a/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 315b8ea5..382484aa 100644 --- a/include/trackjoint/single_joint_generator.h +++ b/include/trackjoint/single_joint_generator.h @@ -12,7 +12,8 @@ #pragma once -#include // copysign +#include // copysign +#include // Spline-fitting is used to extend trajectory duration #include "trackjoint/error_codes.h" #include "trackjoint/joint_trajectory.h" #include "trackjoint/kinematic_state.h" @@ -21,13 +22,15 @@ namespace trackjoint { +typedef Eigen::Spline Spline1D; +typedef Eigen::SplineFitting SplineFitting1D; + class SingleJointGenerator { public: /** \brief Constructor * * input timestep desired time between waypoints - * input desired_duration total desired duration of the trajectory * input max_duration allow the trajectory to be extended up to this limit. Error if that cannot be done. * input current_joint_states vector of the initial kinematic states for each degree of freedom * input goal_joint_states vector of the target kinematic states for each degree of freedom @@ -40,17 +43,18 @@ class SingleJointGenerator * Should be set lower than the accuracy requirements for your task * input use_streaming_mode set to true for fast streaming applications. Returns a maximum of num_waypoints_threshold * waypoints. + * input timestep_was_upsampled If upsampling happened (we are working with very few waypoints), do not adjust + * timestep */ - SingleJointGenerator(double timestep, double desired_duration, double max_duration, - const KinematicState& current_joint_state, const KinematicState& goal_joint_state, - const Limits& limits, size_t desired_num_waypoints, size_t num_waypoints_threshold, - size_t max_num_waypoints_trajectory_mode, const double position_tolerance, - bool use_streaming_mode); + SingleJointGenerator(double timestep, double max_duration, const KinematicState& current_joint_state, + const KinematicState& goal_joint_state, const Limits& limits, size_t desired_num_waypoints, + size_t num_waypoints_threshold, size_t max_num_waypoints_trajectory_mode, + const double position_tolerance, bool use_streaming_mode, bool timestep_was_upsampled); /** \brief reset data members and prepare to generate a new trajectory */ - void reset(double timestep, double desired_duration, double max_duration, const KinematicState& current_joint_state, + void reset(double timestep, double max_duration, const KinematicState& current_joint_state, const KinematicState& goal_joint_state, const Limits& limits, size_t desired_num_waypoints, - const double position_tolerance, bool use_streaming_mode); + const double position_tolerance, bool use_streaming_mode, bool timestep_was_upsampled); /** \brief Generate a jerk-limited trajectory for this joint * @@ -58,11 +62,8 @@ class SingleJointGenerator */ ErrorCodeEnum generateTrajectory(); - /** \brief Calculate a trajectory once duration is known. Similar to generateTrajectory minus predictTimeToReach(). - * - * return a TrackJoint status code - */ - ErrorCodeEnum extendTrajectoryDuration(); + /** \brief Extend a trajectory to a new duration. Magnitudes of vel/accel/jerk will be decreased. */ + void extendTrajectoryDuration(); /** \brief Get the generated trajectory * @@ -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. @@ -95,20 +106,20 @@ 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 */ 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(); /** \brief Calculate vel/accel/jerk from position */ - void calculateDerivatives(); + void calculateDerivativesFromPosition(); + + /** \brief Calculate accel/jerk from velocity */ + void calculateDerivativesFromVelocity(); const size_t kNumWaypointsThreshold, kMaxNumWaypointsFullTrajectory; diff --git a/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 49e4ac54..76eaa556 100644 --- a/src/simple_example.cpp +++ b/src/simple_example.cpp @@ -21,42 +21,47 @@ int main(int argc, char** argv) // This example is for just one degree of freedom constexpr int num_dof = 1; // Timestep. Units don't matter as long as they're consistent - constexpr double timestep = 0.0075; - // Total desired trajectory duration - constexpr double desired_duration = 0.028322; + constexpr double timestep = 0.001; // TrackJoint is allowed to extend the trajectory up to this duration, if a solution at kDesiredDuration can't be // found - constexpr double max_duration = 10; + constexpr double max_duration = 5; // streaming mode returns just a few waypoints but executes very quickly. We won't use it here -- we'll calculate // the whole trajectory at once. constexpr bool use_streaming_mode = false; // Optional logging of TrackJoint output - const std::string output_path_base = "/home/" + std::string(getenv("USER")) + "/trackjoint_data/output_joint"; + const std::string output_path_base = + "/home/" + std::string(getenv("USER")) + "/Downloads/trackjoint_data/output_joint"; std::vector current_joint_states(1); trackjoint::KinematicState joint_state; - joint_state.position = 0.00596041; - joint_state.velocity = -0.176232; - joint_state.acceleration = -3.06289; + joint_state.position = 1.16431; + joint_state.velocity = 2.66465; + joint_state.acceleration = 0; // This is the initial state of the joint current_joint_states[0] = joint_state; std::vector goal_joint_states(1); - joint_state.position = -0.00121542; - joint_state.velocity = -0.289615; - joint_state.acceleration = -2.88021; + joint_state.position = 1.40264; + joint_state.velocity = 2.88556; + joint_state.acceleration = 0.0615633; goal_joint_states[0] = joint_state; trackjoint::Limits single_joint_limits; // Typically, jerk limit >> acceleration limit > velocity limit single_joint_limits.velocity_limit = 3.15; single_joint_limits.acceleration_limit = 5; - single_joint_limits.jerk_limit = 5000; + single_joint_limits.jerk_limit = 100; std::vector limits(1, single_joint_limits); + // Estimate trajectory duration + // This is the fastest possible trajectory execution time, assuming the robot starts at full velocity. + double desired_duration = + fabs(goal_joint_states[0].position - current_joint_states[0].position) / single_joint_limits.velocity_limit; + std::cout << "Desired duration: " << desired_duration << std::endl; + // This descibes how far TrackJoint can deviate from a smooth, interpolated polynomial. // It is used for calculations internally. It should be set to a smaller number than your task requires. - const double position_tolerance = 1e-6; + const double position_tolerance = 0.0001; // Instantiate a trajectory generation object trackjoint::TrajectoryGenerator traj_gen(num_dof, timestep, desired_duration, max_duration, current_joint_states, @@ -68,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; } @@ -81,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; } @@ -91,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 1d0e729f..87f4876c 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -10,41 +10,69 @@ namespace trackjoint { -SingleJointGenerator::SingleJointGenerator(double timestep, double desired_duration, double max_duration, +SingleJointGenerator::SingleJointGenerator(double timestep, double max_duration, const KinematicState& current_joint_state, const KinematicState& goal_joint_state, const Limits& limits, size_t desired_num_waypoints, size_t num_waypoints_threshold, size_t max_num_waypoints_trajectory_mode, const double position_tolerance, - bool use_streaming_mode) + bool use_streaming_mode, bool timestep_was_upsampled) : kNumWaypointsThreshold(num_waypoints_threshold) , kMaxNumWaypointsFullTrajectory(max_num_waypoints_trajectory_mode) , timestep_(timestep) - , desired_duration_(desired_duration) , max_duration_(max_duration) , current_joint_state_(current_joint_state) , goal_joint_state_(goal_joint_state) , limits_(limits) , position_tolerance_(position_tolerance) + , index_last_successful_(0) , use_streaming_mode_(use_streaming_mode) { + // Start with this estimate of the shortest possible duration + // The shortest possible duration avoids oscillation, as much as possible + // Desired duration cannot be less than one timestep + if (!timestep_was_upsampled) + { + desired_duration_ = + std::max(timestep_, fabs((goal_joint_state.position - current_joint_state.position) / limits_.velocity_limit)); + } + // If upsampling was used, we don't want to mess with the timestep or duration minimization + else + { + desired_duration_ = (desired_num_waypoints - 1) * timestep; + } + // Waypoint times nominal_times_ = Eigen::VectorXd::LinSpaced(desired_num_waypoints, 0, desired_duration_); } -void SingleJointGenerator::reset(double timestep, double desired_duration, double max_duration, - const KinematicState& current_joint_state, const KinematicState& goal_joint_state, - const Limits& limits, size_t desired_num_waypoints, const double position_tolerance, - bool use_streaming_mode) +void SingleJointGenerator::reset(double timestep, double max_duration, const KinematicState& current_joint_state, + const KinematicState& goal_joint_state, const Limits& limits, + size_t desired_num_waypoints, const double position_tolerance, bool use_streaming_mode, + bool timestep_was_upsampled) { timestep_ = timestep; - desired_duration_ = desired_duration; max_duration_ = max_duration; current_joint_state_ = current_joint_state; goal_joint_state_ = goal_joint_state; limits_ = limits; position_tolerance_ = position_tolerance; + index_last_successful_ = 0; use_streaming_mode_ = use_streaming_mode; + // Start with this estimate of the shortest possible duration + // The shortest possible duration avoids oscillation, as much as possible + // Desired duration cannot be less than one timestep + if (!timestep_was_upsampled) + { + desired_duration_ = + std::max(timestep_, fabs((goal_joint_state.position - current_joint_state.position) / limits_.velocity_limit)); + } + // If upsampling was used, we don't want to mess with the timestep or duration minimization + else + { + desired_duration_ = (desired_num_waypoints - 1) * timestep; + } + // Waypoint times nominal_times_ = Eigen::VectorXd::LinSpaced(desired_num_waypoints, 0, desired_duration_); } @@ -55,8 +83,8 @@ ErrorCodeEnum SingleJointGenerator::generateTrajectory() waypoints_ = JointTrajectory(); waypoints_.positions = interpolate(nominal_times_); - waypoints_.elapsed_times.setLinSpaced(waypoints_.positions.size(), 0., desired_duration_); - calculateDerivatives(); + waypoints_.elapsed_times.setLinSpaced(waypoints_.positions.size(), 0., (waypoints_.positions.size() - 1) * timestep_); + calculateDerivativesFromPosition(); ErrorCodeEnum error_code = positionVectorLimitLookAhead(&index_last_successful_); if (error_code) @@ -69,16 +97,47 @@ ErrorCodeEnum SingleJointGenerator::generateTrajectory() return error_code; } -ErrorCodeEnum SingleJointGenerator::extendTrajectoryDuration() +void SingleJointGenerator::extendTrajectoryDuration() { + size_t new_num_waypoints = 1 + desired_duration_ / timestep_; + + // If waypoints were successfully generated for this dimension previously, just stretch the trajectory with splines. + // ^This is the best way because it reduces overshoot. + // Otherwise, re-generate a new trajectory from scratch. + if (index_last_successful_ == static_cast(waypoints_.elapsed_times.size() - 1)) + { + // Fit and generate a spline function to the original positions, same number of waypoints, new (extended) duration + // This only decreases velocity/accel/jerk, so no worries re. limit violation + Eigen::RowVectorXd new_times; + new_times.setLinSpaced(waypoints_.elapsed_times.size(), 0, desired_duration_); + Eigen::RowVectorXd position(waypoints_.positions); + + const auto fit = SplineFitting1D::Interpolate(position, 2, new_times); + + // 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 (Eigen::Index idx = 0; idx < waypoints_.elapsed_times.size(); ++idx) + waypoints_.positions[idx] = fit(waypoints_.elapsed_times(idx)).coeff(0); + + calculateDerivativesFromPosition(); + return; + } + + // Plan a new trajectory from scratch: // Clear previous results - waypoints_ = JointTrajectory(); - size_t expected_num_waypoints = 1 + desired_duration_ / timestep_; - waypoints_.elapsed_times.setLinSpaced(expected_num_waypoints, 0., desired_duration_); - waypoints_.positions = interpolate(waypoints_.elapsed_times); - calculateDerivatives(); - ErrorCodeEnum error_code = forwardLimitCompensation(&index_last_successful_); - return error_code; + else + { + waypoints_ = JointTrajectory(); + waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., (new_num_waypoints - 1) * timestep_); + waypoints_.positions = interpolate(waypoints_.elapsed_times); + calculateDerivativesFromPosition(); + forwardLimitCompensation(&index_last_successful_); + } + + return; } JointTrajectory SingleJointGenerator::getTrajectory() @@ -91,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 @@ -122,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 @@ -161,13 +220,10 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde waypoints_.accelerations(index - 1) * timestep_ + 0.5 * waypoints_.jerks(index) * timestep_ * timestep_; - // Re-calculate derivatives from the updated velocity vector - calculateDerivatives(); - 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_; @@ -204,8 +260,6 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde waypoints_.velocities(index) = waypoints_.velocities(index - 1) + waypoints_.accelerations(index - 1) * timestep_ + 0.5 * waypoints_.jerks(index) * timestep_ * timestep_; - // Re-calculate derivatives from the updated velocity vector - calculateDerivatives(); } else { @@ -218,7 +272,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_; @@ -239,20 +293,16 @@ 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) { delta_v = std::copysign(velocity_limit, waypoints_.velocities(index)) - waypoints_.velocities(index); waypoints_.velocities(index) = std::copysign(velocity_limit, waypoints_.velocities(index)); - // Re-calculate derivatives from the updated velocity vector - calculateDerivatives(); // 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,19 +323,13 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde } } - return ErrorCodeEnum::kNoError; -} + // Re-calculate derivatives from the updated velocity vector + calculateDerivativesFromVelocity(); -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 @@ -306,10 +350,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 = @@ -342,13 +386,12 @@ inline bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index break; } } - // else, can't make all of the correction in this timestep, so make as - // much of a change as possible - else + // Can't make all of the correction in this timestep, so make as much of a change as possible + if (!successful_compensation) { // This is what accel and jerk would be if we set velocity(index) to the // limit - 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 +421,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,14 +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. + // 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_) @@ -409,7 +451,7 @@ inline ErrorCodeEnum SingleJointGenerator::predictTimeToReach() { // Try increasing the duration, based on fraction of states that weren't reached successfully desired_duration_ = - 1. + 0.5 * (1. - index_last_successful_ / (waypoints_.positions.size() - 1)) * desired_duration_; + (1. + 0.2 * (1. - index_last_successful_ / (waypoints_.positions.size() - 1))) * desired_duration_; // // Round to nearest timestep if (std::fmod(desired_duration_, timestep_) > 0.5 * timestep_) @@ -421,7 +463,7 @@ inline ErrorCodeEnum SingleJointGenerator::predictTimeToReach() if (new_num_waypoints > kMaxNumWaypointsFullTrajectory) new_num_waypoints = kMaxNumWaypointsFullTrajectory; - waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., desired_duration_); + waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., (new_num_waypoints - 1) * timestep_); waypoints_.positions.resize(waypoints_.elapsed_times.size()); waypoints_.velocities.resize(waypoints_.elapsed_times.size()); waypoints_.accelerations.resize(waypoints_.elapsed_times.size()); @@ -431,7 +473,7 @@ inline ErrorCodeEnum SingleJointGenerator::predictTimeToReach() // Try to create the trajectory again, with the new duration //////////////////////////////////////////////////////////// waypoints_.positions = interpolate(waypoints_.elapsed_times); - calculateDerivatives(); + calculateDerivativesFromPosition(); positionVectorLimitLookAhead(&index_last_successful_); } } @@ -474,23 +516,30 @@ 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) return error_code; + // Helpful hint if limit comp fails on the first waypoint + if (index_last_successful_ == 1) + { + std::cout << "Limit compensation failed at the first waypoint. " + << "Try a larger position_tolerance parameter or smaller timestep." << std::endl; + } + // Re-compile the position with these modifications. // Ensure the first and last elements are a perfect match with initial/final // conditions @@ -509,17 +558,23 @@ inline ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* return error_code; } -inline void SingleJointGenerator::calculateDerivatives() +void SingleJointGenerator::calculateDerivativesFromPosition() { - // From position vector, approximate velocity and acceleration. - // velocity = (difference between adjacent position elements) / delta_t - // acceleration = (difference between adjacent velocity elements) / delta_t + // From position vector, approximate vel/accel/jerk. waypoints_.velocities = DiscreteDifferentiation(waypoints_.positions, timestep_, current_joint_state_.velocity); waypoints_.accelerations = DiscreteDifferentiation(waypoints_.velocities, timestep_, current_joint_state_.acceleration); waypoints_.jerks = DiscreteDifferentiation(waypoints_.accelerations, timestep_, 0); } +void SingleJointGenerator::calculateDerivativesFromVelocity() +{ + // From velocity vector, approximate accel/jerk. + waypoints_.accelerations = + DiscreteDifferentiation(waypoints_.velocities, timestep_, current_joint_state_.acceleration); + waypoints_.jerks = DiscreteDifferentiation(waypoints_.accelerations, timestep_, 0); +} + void SingleJointGenerator::updateTrajectoryDuration(double new_trajectory_duration) { // The trajectory will be forced to have this duration (or fail) because diff --git a/src/streaming_example.cpp b/src/streaming_example.cpp index 04d095da..c8d72642 100644 --- a/src/streaming_example.cpp +++ b/src/streaming_example.cpp @@ -39,7 +39,7 @@ int main(int argc, char** argv) constexpr double final_acceleration_tolerance = 1e-1; // For streaming mode, it is important to keep the desired duration >=10 timesteps. // Otherwise, an error will be thrown. This helps with accuracy - constexpr double min_desired_duration = 10 * timestep; + constexpr double min_desired_duration = timestep; // Between TrackJoint iterations, move ahead this many waypoints along the trajectory. constexpr std::size_t next_waypoint = 1; @@ -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..7e8ef65e 100644 --- a/src/three_dof_examples.cpp +++ b/src/three_dof_examples.cpp @@ -19,50 +19,56 @@ int main(int argc, char** argv) { constexpr int num_dof = 3; - const double timestep = 0.001; - double desired_duration = 0.028322; - constexpr double max_duration = 10; + const double timestep = 0.0039; + constexpr double max_duration = 30; // Streaming mode returns just a few waypoints but executes very quickly. constexpr bool use_streaming_mode = false; // Position tolerance for each waypoint - constexpr double waypoint_position_tolerance = 1e-5; - const std::string output_path_base = "/home/" + std::string(getenv("USER")) + "/trackjoint_data/output_joint"; + constexpr double waypoint_position_tolerance = 1e-4; + const std::string output_path_base = + "/home/" + std::string(getenv("USER")) + "/Downloads/trackjoint_data/output_joint"; - //////////////////////////////////////////////// - // First example - small motions, come to a halt - //////////////////////////////////////////////// - std::cout << "EXAMPLE 1" << std::endl; std::vector current_joint_states(3); trackjoint::KinematicState joint_state; - joint_state.position = 0.00596041; - joint_state.velocity = -0.176232; - joint_state.acceleration = -3.06289; + joint_state.position = 1.23984; + joint_state.velocity = 1.45704; + joint_state.acceleration = -0.0196446; current_joint_states[0] = joint_state; - joint_state.position = -0.596041; - joint_state.velocity = 0.2; - joint_state.acceleration = 2.39; + joint_state.position = -1.12637; + joint_state.velocity = -0.774015; + joint_state.acceleration = 0.0104357; current_joint_states[1] = joint_state; - joint_state.position = -0.1; - joint_state.velocity = 0.13; - joint_state.acceleration = 0.2; + joint_state.position = -0.014012; + joint_state.velocity = 0.583426; + joint_state.acceleration = -0.00786606; current_joint_states[2] = joint_state; std::vector goal_joint_states(3); - joint_state.position = -0.00121542; - joint_state.velocity = 0; - joint_state.acceleration = 0; + joint_state.position = 1.4757; + joint_state.velocity = 1.45675; + joint_state.acceleration = 0.126129; goal_joint_states[0] = joint_state; - joint_state.position = -0.57; + joint_state.position = -0.545844; + joint_state.velocity = 1.81361; + joint_state.acceleration = 1.93357; goal_joint_states[1] = joint_state; - joint_state.position = 0; + joint_state.position = -0.447873; + joint_state.velocity = -1.35293; + joint_state.acceleration = -1.43058; goal_joint_states[2] = joint_state; trackjoint::Limits single_joint_limits; single_joint_limits.velocity_limit = 3.15; single_joint_limits.acceleration_limit = 5; - single_joint_limits.jerk_limit = 5000; + single_joint_limits.jerk_limit = 10000; std::vector limits(3, single_joint_limits); + // Estimate trajectory duration + // This is the fastest possible trajectory execution time, assuming the robot starts at full velocity. + double desired_duration = + fabs(goal_joint_states[1].position - current_joint_states[1].position) / single_joint_limits.velocity_limit; + std::cout << "Desired duration: " << desired_duration << std::endl; + // Initialize main class trackjoint::TrajectoryGenerator traj_gen(num_dof, timestep, desired_duration, max_duration, current_joint_states, goal_joint_states, limits, waypoint_position_tolerance, use_streaming_mode); @@ -74,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; } @@ -86,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; } @@ -96,100 +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; - - // Save the synchronized trajectories to .csv files - traj_gen.saveTrajectoriesToFile(output_trajectories, output_path_base); - - // Print the synchronized trajectories - for (size_t joint = 0; joint < output_trajectories.size(); ++joint) - { - std::cout << "==========" << std::endl; - std::cout << std::endl; - std::cout << std::endl; - std::cout << "==========" << std::endl; - std::cout << "Joint " << joint << std::endl; - std::cout << "==========" << std::endl; - std::cout << std::endl; - std::cout << std::endl; - std::cout << "==========" << std::endl; - for (size_t waypoint = 0; waypoint < static_cast(output_trajectories.at(joint).positions.size()); - ++waypoint) - { - std::cout << "Elapsed time: " << output_trajectories.at(joint).elapsed_times(waypoint) - << " Position: " << output_trajectories.at(joint).positions(waypoint) - << " Velocity: " << output_trajectories.at(joint).velocities(waypoint) - << " Acceleration: " << output_trajectories.at(joint).accelerations(waypoint) - << " Jerk: " << output_trajectories.at(joint).jerks(waypoint) << std::endl; - } - } - std::cout << "============================================" << std::endl; - - ///////////////////////////////// - // Second example - large motions - ///////////////////////////////// - std::cout << "EXAMPLE 2" << std::endl; - desired_duration = 1; - - joint_state.position = 0.74; - joint_state.velocity = 0.33; - joint_state.acceleration = 0.15; - current_joint_states[0] = joint_state; - joint_state.position = -0.59; - joint_state.velocity = 0.2; - joint_state.acceleration = 2.39; - current_joint_states[1] = joint_state; - joint_state.position = -1.1; - joint_state.velocity = 1.13; - joint_state.acceleration = 0.9; - current_joint_states[2] = joint_state; - - joint_state.position = 0.93; - joint_state.velocity = 0.1; - joint_state.acceleration = -0.2; - goal_joint_states[0] = joint_state; - joint_state.position = -0.86; - joint_state.velocity = 0.05; - joint_state.acceleration = -0.12; - goal_joint_states[1] = joint_state; - joint_state.position = -0.42; - joint_state.velocity = 0.59; - joint_state.acceleration = -0.23; - goal_joint_states[2] = joint_state; - - // Create a new traj gen object with new parameters - traj_gen.~TrajectoryGenerator(); - new (&traj_gen) - trackjoint::TrajectoryGenerator(num_dof, timestep, desired_duration, max_duration, current_joint_states, - goal_joint_states, limits, waypoint_position_tolerance, use_streaming_mode); - - error_code = traj_gen.inputChecking(current_joint_states, goal_joint_states, limits, timestep); - - // Input error handling - if an error is found, the trajectory is not - // generated. - if (error_code != trackjoint::ErrorCodeEnum::kNoError) - { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; - return -1; - } - - // Measure runtime - start = std::chrono::system_clock::now(); - error_code = traj_gen.generateTrajectories(&output_trajectories); - end = std::chrono::system_clock::now(); - - // Trajectory generation error handling - if (error_code != trackjoint::ErrorCodeEnum::kNoError) - { - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; - return -1; - } - - elapsed_seconds = end - start; - - std::cout << "Runtime: " << elapsed_seconds.count() << std::endl; - std::cout << "Num waypoints: " << output_trajectories.at(0).positions.size() << std::endl; - std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl; + 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..cd5a7b69 100644 --- a/src/trajectory_generator.cpp +++ b/src/trajectory_generator.cpp @@ -29,12 +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; for (size_t joint = 0; joint < kNumDof; ++joint) { - single_joint_generators_.push_back( - SingleJointGenerator(upsampled_timestep_, desired_duration_, max_duration_, current_joint_states[joint], - goal_joint_states[joint], limits[joint], upsampled_num_waypoints_, kNumWaypointsThreshold, - kMaxNumWaypointsFullTrajectory, position_tolerance, use_streaming_mode_)); + 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)); } } @@ -57,11 +58,12 @@ void TrajectoryGenerator::reset(double timestep, double desired_duration, double upsample(); // Initialize a trajectory generator for each joint + bool timestep_was_upsampled = upsample_rounds_ > 0; for (size_t joint = 0; joint < kNumDof; ++joint) { - single_joint_generators_[joint].reset(upsampled_timestep_, desired_duration_, max_duration_, - current_joint_states[joint], goal_joint_states[joint], limits[joint], - upsampled_num_waypoints_, position_tolerance, use_streaming_mode_); + single_joint_generators_[joint].reset(upsampled_timestep_, max_duration_, current_joint_states[joint], + goal_joint_states[joint], limits[joint], upsampled_num_waypoints_, + position_tolerance, use_streaming_mode_, timestep_was_upsampled); } } @@ -134,32 +136,38 @@ void TrajectoryGenerator::downSample(Eigen::VectorXd* time_vector, Eigen::Vector size_t num_elements_filled_in_new_vector = 0; // Number of elements traversed in the upsampled vector size_t num_up_sampled_elements_traversed = 0; + // The last index taken from the beginning of the upsampled vector + size_t last_element_pulled_from_beginning = 0; + // The last index taken from the end of the upsampled vector + size_t last_element_pulled_from_end = position_vector->size() - 1; + for (size_t count = 1; num_elements_filled_in_new_vector < new_vector_size - 2; ++count) { // Update num_elements_to_skip based on: - // (num_elements_remaining_in_upsampled_vector) / - // (num_elements_remaining_in_new_vector) + // (num_elements_remaining_in_upsampled_vector) / (num_elements_remaining_in_new_vector) num_elements_to_skip = (position_vector->size() - 2 - num_up_sampled_elements_traversed) / - (new_vector_size - 1 - num_elements_filled_in_new_vector); + (new_vector_size - 2 - num_elements_filled_in_new_vector); - new_positions[count] = (*position_vector)[count * num_elements_to_skip]; - new_velocities[count] = (*velocity_vector)[count * num_elements_to_skip]; - new_accelerations[count] = (*acceleration_vector)[count * num_elements_to_skip]; + new_positions[count] = (*position_vector)[last_element_pulled_from_beginning + num_elements_to_skip]; + new_velocities[count] = (*velocity_vector)[last_element_pulled_from_beginning + num_elements_to_skip]; + new_accelerations[count] = (*acceleration_vector)[last_element_pulled_from_beginning + num_elements_to_skip]; ++num_elements_filled_in_new_vector; num_up_sampled_elements_traversed = num_up_sampled_elements_traversed + num_elements_to_skip; + last_element_pulled_from_beginning = last_element_pulled_from_beginning + num_elements_to_skip; // Count down if we need to fill more elements. Subtract two because first and last element are already filled. if (num_elements_filled_in_new_vector < new_vector_size - 2) { // Start filling at (end-1) because the last element is already filled new_positions[new_positions.size() - 1 - count] = - (*position_vector)[position_vector->size() - 1 - count * num_elements_to_skip]; + (*position_vector)[last_element_pulled_from_end - num_elements_to_skip]; new_velocities[new_velocities.size() - 1 - count] = - (*velocity_vector)[velocity_vector->size() - 1 - count * num_elements_to_skip]; + (*velocity_vector)[last_element_pulled_from_end - num_elements_to_skip]; new_accelerations[new_accelerations.size() - 1 - count] = - (*acceleration_vector)[acceleration_vector->size() - 1 - count * num_elements_to_skip]; + (*acceleration_vector)[last_element_pulled_from_end - num_elements_to_skip]; ++num_elements_filled_in_new_vector; num_up_sampled_elements_traversed = num_up_sampled_elements_traversed + num_elements_to_skip; + last_element_pulled_from_end = last_element_pulled_from_end - num_elements_to_skip; } } @@ -194,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, @@ -309,7 +317,7 @@ ErrorCodeEnum TrajectoryGenerator::synchronizeTrajComponents(std::vectorat(joint) = single_joint_generators_[joint].getTrajectory(); + + std::cout << "End position for Joint " << joint << ": " + << single_joint_generators_[joint] + .getTrajectory() + .positions[single_joint_generators_[joint].getTrajectory().positions.size() - 1] + << std::endl; } // If this was the index of longest duration, don't need to re-generate a trajectory else @@ -356,42 +370,39 @@ ErrorCodeEnum TrajectoryGenerator::synchronizeTrajComponents(std::vector* trajectory) { for (size_t joint = 0; joint < kNumDof; ++joint) { - for (size_t joint = 0; joint < trajectory->size(); ++joint) + for (auto waypt = 0; waypt < trajectory->at(joint).velocities.size(); ++waypt) { - for (auto waypt = 0; waypt < trajectory->at(joint).velocities.size(); ++waypt) - { - // Velocity - if (trajectory->at(joint).velocities[waypt] > limits_[joint].velocity_limit) - trajectory->at(joint).velocities[waypt] = limits_[joint].velocity_limit; - if (trajectory->at(joint).velocities[waypt] < -limits_[joint].velocity_limit) - trajectory->at(joint).velocities[waypt] = -limits_[joint].velocity_limit; - - // Acceleration - if (trajectory->at(joint).accelerations[waypt] > limits_[joint].acceleration_limit) - trajectory->at(joint).accelerations[waypt] = limits_[joint].acceleration_limit; - if (trajectory->at(joint).accelerations[waypt] < -limits_[joint].acceleration_limit) - trajectory->at(joint).accelerations[waypt] = -limits_[joint].acceleration_limit; - - // Jerk - if (trajectory->at(joint).jerks[waypt] > limits_[joint].jerk_limit) - trajectory->at(joint).jerks[waypt] = limits_[joint].jerk_limit; - if (trajectory->at(joint).jerks[waypt] < -limits_[joint].jerk_limit) - trajectory->at(joint).jerks[waypt] = -limits_[joint].jerk_limit; - } + // Velocity + if (trajectory->at(joint).velocities[waypt] > limits_[joint].velocity_limit) + trajectory->at(joint).velocities[waypt] = limits_[joint].velocity_limit; + if (trajectory->at(joint).velocities[waypt] < -limits_[joint].velocity_limit) + trajectory->at(joint).velocities[waypt] = -limits_[joint].velocity_limit; + + // Acceleration + if (trajectory->at(joint).accelerations[waypt] > limits_[joint].acceleration_limit) + trajectory->at(joint).accelerations[waypt] = limits_[joint].acceleration_limit; + if (trajectory->at(joint).accelerations[waypt] < -limits_[joint].acceleration_limit) + trajectory->at(joint).accelerations[waypt] = -limits_[joint].acceleration_limit; + + // Jerk + if (trajectory->at(joint).jerks[waypt] > limits_[joint].jerk_limit) + trajectory->at(joint).jerks[waypt] = limits_[joint].jerk_limit; + if (trajectory->at(joint).jerks[waypt] < -limits_[joint].jerk_limit) + trajectory->at(joint).jerks[waypt] = -limits_[joint].jerk_limit; } } } 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) { @@ -425,4 +436,4 @@ ErrorCodeEnum TrajectoryGenerator::generateTrajectories(std::vector + + + diff --git a/test/trajectory_generation_test.cpp b/test/trajectory_generation_test.cpp index 535a8375..95fb0d8c 100644 --- a/test/trajectory_generation_test.cpp +++ b/test/trajectory_generation_test.cpp @@ -67,18 +67,18 @@ 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_; - double position_tolerance_ = 1e-6; + double position_tolerance_ = 1e-4; bool use_streaming_mode_ = false; bool write_output_ = true; std::vector output_trajectories_; 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,12 +258,16 @@ 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; double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); EXPECT_LT(position_error, position_tolerance); + // Timestep + double timestep_tolerance = 0.1 * timestep_; + EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep_, + timestep_tolerance); // Duration const double duration_tolerance = 5e-3; size_t vector_length = output_trajectories_[0].elapsed_times.size() - 1; @@ -282,22 +292,22 @@ 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; const double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); EXPECT_LT(position_error, position_tolerance); + // Timestep + double timestep_tolerance = 0.003; + EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep, + timestep_tolerance); // Duration const double duration_tolerance = 5e-4; size_t vector_length = output_trajectories_[0].elapsed_times.size() - 1; EXPECT_NEAR(output_trajectories_[0].elapsed_times(vector_length), desired_duration, duration_tolerance); // Number of waypoints EXPECT_EQ(output_trajectories_[0].elapsed_times.size(), 3); - // Timestep - double timestep_tolerance = 0.003; - EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep, - timestep_tolerance); } TEST_F(TrajectoryGenerationTest, FourTimestepDuration) @@ -318,12 +328,16 @@ 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; const double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); EXPECT_LT(position_error, position_tolerance); + // Timestep + double timestep_tolerance = 0.1 * timestep; + EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep, + timestep_tolerance); // Duration const double duration_tolerance = 5e-3; size_t vector_length = output_trajectories_[0].elapsed_times.size() - 1; @@ -365,12 +379,16 @@ 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; double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); EXPECT_LT(position_error, position_tolerance); + // Timestep + double timestep_tolerance = 0.1 * timestep_; + EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep_, + timestep_tolerance); // Duration const double duration_tolerance = 5e-3; size_t vector_length = output_trajectories_[0].elapsed_times.size() - 1; @@ -405,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_); @@ -413,10 +431,13 @@ TEST_F(TrajectoryGenerationTest, VelAccelJerkLimit) double position_tolerance = 1e-4; double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); EXPECT_LT(position_error, position_tolerance); + // Timestep + double timestep_tolerance = 0.1 * timestep_; + EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep_, + timestep_tolerance); // Duration - const double duration_tolerance = 5e-3; size_t vector_length = output_trajectories_[0].elapsed_times.size() - 1; - EXPECT_NEAR(output_trajectories_[0].elapsed_times(vector_length), desired_duration_, duration_tolerance); + EXPECT_LE(output_trajectories_[0].elapsed_times(vector_length), desired_duration_); } TEST_F(TrajectoryGenerationTest, NoisyStreamingCommand) @@ -536,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 = @@ -550,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]; @@ -589,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_, @@ -637,12 +659,16 @@ 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; double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); EXPECT_LT(position_error, position_tolerance); + // Timestep + double timestep_tolerance = 0.1 * timestep_; + EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep_, + timestep_tolerance); // Duration const double duration_tolerance = 5e-3; size_t vector_length = output_trajectories_[0].elapsed_times.size() - 1; @@ -683,12 +709,16 @@ 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; const double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); EXPECT_LT(position_error, position_tolerance); + // Timestep + double timestep_tolerance = 0.1 * timestep; + EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep, + timestep_tolerance); // Duration uint num_waypoint_tolerance = 1; uint expected_num_waypoints = 1 + desired_duration / timestep; @@ -732,17 +762,20 @@ 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; + const double position_tolerance = 5e-4; const double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); EXPECT_LT(position_error, position_tolerance); + // Timestep + double timestep_tolerance = 0.1 * timestep; + EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep, + timestep_tolerance); // Duration - const double expected_duration = 1.05; - const double duration_tolerance = 5e-3; + const double expected_duration = 0.865; size_t vector_length = output_trajectories_[0].elapsed_times.size() - 1; - EXPECT_NEAR(output_trajectories_[0].elapsed_times(vector_length), expected_duration, duration_tolerance); + EXPECT_LE(output_trajectories_[0].elapsed_times(vector_length), expected_duration); } TEST_F(TrajectoryGenerationTest, PositiveAndNegativeLimits) @@ -787,12 +820,16 @@ 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; const double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); EXPECT_LT(position_error, position_tolerance); + // Timestep + double timestep_tolerance = 0.1 * timestep; + EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep, + timestep_tolerance); // Duration uint num_waypoint_tolerance = 1; uint expected_num_waypoints = 1 + desired_duration / timestep; @@ -831,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; @@ -862,7 +899,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 +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(); @@ -904,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) { @@ -939,9 +976,56 @@ 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_)); } + +TEST_F(TrajectoryGenerationTest, SingleJointOscillation) +{ + // This test comes from MoveIt. Originally, this joint's trajectory oscillated + // Fixed by setting a shorter desired_duration_ + + timestep_ = 0.0075; + desired_duration_ = 0.1; + num_dof_ = 1; + + current_joint_states_.resize(num_dof_); + KinematicState joint_state; + joint_state.position = -0.00397532; + joint_state.velocity = -0.169886; + joint_state.acceleration = -0.84943; + current_joint_states_[0] = joint_state; + + goal_joint_states_.resize(num_dof_); + joint_state.position = -0.025211; + joint_state.velocity = -0.253157; + joint_state.acceleration = -0.87268; + goal_joint_states_[0] = joint_state; + + limits_.resize(num_dof_); + Limits single_joint_limits; + single_joint_limits.velocity_limit = 3.15; + single_joint_limits.acceleration_limit = 5; + single_joint_limits.jerk_limit = 5000; + limits_[0] = single_joint_limits; + + TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration_, max_duration_, current_joint_states_, + goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); + output_trajectories_.resize(num_dof_); + + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, + traj_gen.inputChecking(current_joint_states_, goal_joint_states_, limits_, timestep_)); + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); + + // Position error + const double position_tolerance = 2e-3; + const double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); + EXPECT_LT(position_error, position_tolerance); + // Timestep + const double timestep_tolerance = 0.0005; + EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep_, + timestep_tolerance); +} } // namespace trackjoint int main(int argc, char** argv) @@ -951,4 +1035,4 @@ int main(int argc, char** argv) int result = RUN_ALL_TESTS(); return result; -} \ No newline at end of file +}