diff --git a/.catkin_tools/CATKIN_IGNORE b/.catkin_tools/CATKIN_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/.catkin_tools/README b/.catkin_tools/README deleted file mode 100644 index 4706f475..00000000 --- a/.catkin_tools/README +++ /dev/null @@ -1,13 +0,0 @@ -# Catkin Tools Metadata - -This directory was generated by catkin_tools and it contains persistent -configuration information used by the `catkin` command and its sub-commands. - -Each subdirectory of the `profiles` directory contains a set of persistent -configuration options for separate profiles. The default profile is called -`default`. If another profile is desired, it can be described in the -`profiles.yaml` file in this directory. - -Please see the catkin_tools documentation before editing any files in this -directory. Most actions can be performed with the `catkin` command-line -program. diff --git a/.catkin_tools/VERSION b/.catkin_tools/VERSION deleted file mode 100644 index c8a5397f..00000000 --- a/.catkin_tools/VERSION +++ /dev/null @@ -1 +0,0 @@ -0.4.5 \ No newline at end of file diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt deleted file mode 100644 index e69de29b..00000000 diff --git a/.clang-format b/.clang-format index eca6051a..c3c41084 100644 --- a/.clang-format +++ b/.clang-format @@ -10,7 +10,7 @@ AllowShortLoopsOnASingleLine: false AllowShortFunctionsOnASingleLine: None AllowShortLoopsOnASingleLine: false AlwaysBreakTemplateDeclarations: true -AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakBeforeMultilineStrings: true BreakBeforeBinaryOperators: false BreakBeforeTernaryOperators: false BreakConstructorInitializersBeforeComma: true diff --git a/.gitignore b/.gitignore index 967678e8..c9a672a7 100644 --- a/.gitignore +++ b/.gitignore @@ -18,4 +18,8 @@ .DS_Store # SSH Keys (used for Docker) -id_rsa \ No newline at end of file +id_rsa + +# Catkin tools +/.catkin-tools/ + diff --git a/include/trackjoint/configuration.h b/include/trackjoint/configuration.h new file mode 100644 index 00000000..0a15ba36 --- /dev/null +++ b/include/trackjoint/configuration.h @@ -0,0 +1,54 @@ +/********************************************************************* + * Copyright (c) 2019, PickNik Consulting + * All rights reserved. + * + * Unauthorized copying of this file, via any medium is strictly prohibited + * Proprietary and confidential + *********************************************************************/ + +/* Author: John Morris + Desc: Trajectory generator configuration. +*/ + +#pragma once + +#include "trackjoint/limits.h" + +namespace trackjoint +{ +/** + * \brief Trajectory generator configuration. + */ +struct Configuration +{ + Configuration(double timestep, double max_duration, const Limits& limits, const double position_tolerance, + bool use_streaming_mode) + : timestep(timestep) + , max_duration(max_duration) + , limits(limits) + , position_tolerance(position_tolerance) + , use_streaming_mode(use_streaming_mode) + { + } + + Configuration() + { + } + + double timestep; + double max_duration; + Limits limits; + double position_tolerance; + // If streaming mode is enabled, trajectories are clipped at + // kNumWaypointsThreshold so the algorithm runs very quickly. + // + // Streaming mode is intended for realtime streaming applications. + // + // There could be even fewer waypoints than that if the goal is very + // close or the algorithm only finds a few waypoints successfully. + // + // In streaming mode, trajectory duration is not extended until it + // successfully reaches the goal. + bool use_streaming_mode; +}; +} // namespace trackjoint diff --git a/include/trackjoint/error_codes.h b/include/trackjoint/error_codes.h index f5156a06..6dd5529a 100644 --- a/include/trackjoint/error_codes.h +++ b/include/trackjoint/error_codes.h @@ -31,23 +31,28 @@ enum ErrorCodeEnum LIMIT_NOT_POSITIVE = 6, GOAL_POSITION_MISMATCH = 7, FAILURE_TO_GENERATE_SINGLE_WAYPOINT = 8, - LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE = 9 + LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE = 9, + OBJECT_NOT_RESET = 10, }; /** * \brief Use this map to look up human-readable strings for each error code */ -const std::unordered_map ERROR_CODE_MAP( - { { NO_ERROR, "No error, trajectory generation was successful" }, - { DESIRED_DURATION_TOO_SHORT, "Desired duration is too short, cannot have less than one timestep in a " - "trajectory" }, - { MAX_DURATION_EXCEEDED, "Max duration was exceeded" }, - { VELOCITY_EXCEEDS_LIMIT, "A velocity input exceeds the velocity limit" }, - { ACCEL_EXCEEDS_LIMIT, "An acceleration input exceeds the acceleration limit" }, - { MAX_DURATION_LESS_THAN_DESIRED_DURATION, "max_duration should not be less than desired_duration" }, - { LIMIT_NOT_POSITIVE, "Vel/accel/jerk limits should be greater than zero" }, - { GOAL_POSITION_MISMATCH, "Mismatch between the final position and the goal position" }, - { FAILURE_TO_GENERATE_SINGLE_WAYPOINT, "Failed to generate even a single new waypoint" }, - { LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE, "In streaming mode, desired duration should be at least 10 " - "timesteps" } }); +const std::unordered_map ERROR_CODE_MAP({ + { NO_ERROR, "No error, trajectory generation was successful" }, + { DESIRED_DURATION_TOO_SHORT, + "Desired duration is too short, cannot have less than one timestep in a " + "trajectory" }, + { MAX_DURATION_EXCEEDED, "Max duration was exceeded" }, + { VELOCITY_EXCEEDS_LIMIT, "A velocity input exceeds the velocity limit" }, + { ACCEL_EXCEEDS_LIMIT, "An acceleration input exceeds the acceleration limit" }, + { MAX_DURATION_LESS_THAN_DESIRED_DURATION, "max_duration should not be less than desired_duration" }, + { LIMIT_NOT_POSITIVE, "Vel/accel/jerk limits should be greater than zero" }, + { GOAL_POSITION_MISMATCH, "Mismatch between the final position and the goal position" }, + { FAILURE_TO_GENERATE_SINGLE_WAYPOINT, "Failed to generate even a single new waypoint" }, + { LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE, + "In streaming mode, desired duration should be at least 10 timesteps" }, + { OBJECT_NOT_RESET, "Must call reset() before generating trajectory" }, + +}); } // end namespace trackjoint diff --git a/include/trackjoint/single_joint_generator.h b/include/trackjoint/single_joint_generator.h index f0f1f79a..355e994e 100644 --- a/include/trackjoint/single_joint_generator.h +++ b/include/trackjoint/single_joint_generator.h @@ -12,45 +12,63 @@ #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" +#include "trackjoint/configuration.h" #include "trackjoint/limits.h" #include "trackjoint/utilities.h" namespace trackjoint { +typedef Eigen::Spline Spline1D; +typedef Eigen::SplineFitting SplineFitting1D; + class SingleJointGenerator { public: /** \brief Constructor + * + * input num_waypoints_threshold minimum/maximum number of waypoints for full trajectory/streaming modes, respectively + * input max_num_waypoints_trajectory_mode to maintain determinism, return an error if more than this many waypoints + * is required + */ + SingleJointGenerator(size_t num_waypoints_threshold, size_t max_num_waypoints_trajectory_mode); + + /** \brief reset data members and prepare to generate a new trajectory + * + * input configuration A `Configuration` object + * input current_joint_states vector of the initial kinematic states for each degree of freedom + * input goal_joint_states vector of the target kinematic states for each degree of freedom + * input desired_num_waypoints nominal number of waypoints, calculated from user-supplied duration and timestep + * input timestep_was_upsampled If upsampling happened (we are working with very few waypoints), do not adjust + * timestep + * + */ + void reset(const Configuration& configuration, const KinematicState& current_joint_state, + const KinematicState& goal_joint_state, size_t desired_num_waypoints, bool timestep_was_upsampled); + + /** \brief reset data members and prepare to generate a new trajectory * * input timestep desired time between waypoints - * input 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 * input limits vector of kinematic limits for each degree of freedom * input desired_num_waypoints nominal number of waypoints, calculated from user-supplied duration and timestep - * input num_waypoints_threshold minimum/maximum number of waypoints for full trajectory/streaming modes, respectively - * input max_num_waypoints_trajectory_mode to maintain determinism, return an error if more than this many waypoints - * is required * input position_tolerance tolerance for how close the final trajectory should follow a smooth interpolation. * Should be set lower than the accuracy requirements for your task * input use_streaming_mode set to true for fast streaming applications. Returns a maximum of num_waypoints_threshold - * waypoints. + * waypoints. + * input timestep_was_upsampled If upsampling happened (we are working with very few waypoints), do not adjust + * timestep + * */ - SingleJointGenerator(double timestep, double 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); - - /** \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 +76,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 * @@ -105,7 +120,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 */ @@ -115,26 +130,20 @@ class SingleJointGenerator ErrorCodeEnum predictTimeToReach(); /** \brief Calculate vel/accel/jerk from position */ - void calculateDerivatives(); + void calculateDerivativesFromPosition(); + + /** \brief Calculate accel/jerk from velocity */ + void calculateDerivativesFromVelocity(); const size_t kNumWaypointsThreshold, kMaxNumWaypointsFullTrajectory; - double timestep_; - double desired_duration_, max_duration_; + Configuration configuration_; + double desired_duration_; KinematicState current_joint_state_; KinematicState goal_joint_state_; - Limits limits_; - double position_tolerance_; Eigen::VectorXd nominal_times_; JointTrajectory waypoints_; size_t index_last_successful_; - - // If streaming mode is enabled, trajectories are clipped at kNumWaypointsThreshold so the algorithm runs very - // quickly. - // streaming mode is intended for realtime streaming applications. - // There could be even fewer waypoints than that if the goal is very close or the algorithm only finds a few waypoints - // successfully. - // In streaming mode, trajectory duration is not extended until it successfully reaches the goal. - bool use_streaming_mode_; + bool is_reset_; }; // end class SingleJointGenerator } // namespace trackjoint diff --git a/include/trackjoint/trajectory_generator.h b/include/trackjoint/trajectory_generator.h index 4e4dc14a..a2a27183 100644 --- a/include/trackjoint/trajectory_generator.h +++ b/include/trackjoint/trajectory_generator.h @@ -21,6 +21,7 @@ #include "trackjoint/utilities.h" // C++ +#include #include #include // shared_ptr #include @@ -78,7 +79,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 +88,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 +101,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 +109,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 63795669..d3ec40d8 100644 --- a/src/simple_example.cpp +++ b/src/simple_example.cpp @@ -21,46 +21,53 @@ 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, goal_joint_states, limits, position_tolerance, use_streaming_mode); + traj_gen.reset(timestep, desired_duration, max_duration, current_joint_states, goal_joint_states, limits, + position_tolerance, use_streaming_mode); // This vector holds the trajectories for each DOF std::vector output_trajectories(num_dof); // Optionally, check user input for common errors, like current velocities being less than velocity limits diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index 4be76d52..a1c8796a 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -10,53 +10,64 @@ namespace trackjoint { -SingleJointGenerator::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::SingleJointGenerator(size_t num_waypoints_threshold, size_t max_num_waypoints_trajectory_mode) : kNumWaypointsThreshold(num_waypoints_threshold) , kMaxNumWaypointsFullTrajectory(max_num_waypoints_trajectory_mode) - , timestep_(timestep) - , 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) - , use_streaming_mode_(use_streaming_mode) + , is_reset_(false) + { - // 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(const Configuration& configuration, const KinematicState& current_joint_state, + const KinematicState& goal_joint_state, size_t desired_num_waypoints, + bool timestep_was_upsampled) { - timestep_ = timestep; - desired_duration_ = desired_duration; - max_duration_ = max_duration; + configuration_ = configuration; current_joint_state_ = current_joint_state; goal_joint_state_ = goal_joint_state; - limits_ = limits; - position_tolerance_ = position_tolerance; - use_streaming_mode_ = use_streaming_mode; + index_last_successful_ = 0; + is_reset_ = true; + + // 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(configuration.timestep, fabs((goal_joint_state.position - current_joint_state.position) / + configuration_.limits.velocity_limit)); + } + // If upsampling was used, we don't want to mess with the timestep or duration minimization + else + { + desired_duration_ = (desired_num_waypoints - 1) * configuration.timestep; + } // Waypoint times nominal_times_ = Eigen::VectorXd::LinSpaced(desired_num_waypoints, 0, desired_duration_); } +void SingleJointGenerator::reset(double timestep, double max_duration, const KinematicState& current_joint_state, + const KinematicState& goal_joint_state, const Limits& limits, + size_t desired_num_waypoints, const double position_tolerance, bool use_streaming_mode, + bool timestep_was_upsampled) +{ + Configuration configuration(timestep, max_duration, limits, position_tolerance, use_streaming_mode); + this->reset(configuration, current_joint_state, goal_joint_state, desired_num_waypoints, timestep_was_upsampled); +} + ErrorCodeEnum SingleJointGenerator::generateTrajectory() { + if (!is_reset_) + return ErrorCodeEnum::OBJECT_NOT_RESET; + // Clear previous results waypoints_ = JointTrajectory(); waypoints_.positions = interpolate(nominal_times_); - waypoints_.elapsed_times.setLinSpaced(waypoints_.positions.size(), 0., desired_duration_); - calculateDerivatives(); + waypoints_.elapsed_times.setLinSpaced(waypoints_.positions.size(), 0., + (waypoints_.positions.size() - 1) * configuration_.timestep); + calculateDerivativesFromPosition(); ErrorCodeEnum error_code = positionVectorLimitLookAhead(&index_last_successful_); if (error_code) @@ -64,21 +75,53 @@ ErrorCodeEnum SingleJointGenerator::generateTrajectory() return error_code; } + // Extend the trajectory duration, if needed error_code = predictTimeToReach(); return error_code; } -ErrorCodeEnum SingleJointGenerator::extendTrajectoryDuration() +void SingleJointGenerator::extendTrajectoryDuration() { + size_t new_num_waypoints = 1 + desired_duration_ / configuration_.timestep; + + // If waypoints were successfully generated for this dimension previously, just stretch the trajectory with splines. + // ^This is the best way because it reduces overshoot. + // 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) * configuration_.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) * configuration_.timestep); + waypoints_.positions = interpolate(waypoints_.elapsed_times); + calculateDerivativesFromPosition(); + forwardLimitCompensation(&index_last_successful_); + } + + return; } JointTrajectory SingleJointGenerator::getTrajectory() @@ -131,7 +174,7 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ // Start with the assumption that the entire trajectory can be completed. // Streaming mode returns at the minimum number of waypoints. // Streaming mode is not necessary if the number of waypoints is already very short. - if (!use_streaming_mode_ || static_cast(waypoints_.positions.size()) <= kNumWaypointsThreshold) + if (!configuration_.use_streaming_mode || static_cast(waypoints_.positions.size()) <= kNumWaypointsThreshold) *index_last_successful = waypoints_.positions.size() - 1; else *index_last_successful = kNumWaypointsThreshold - 1; @@ -140,9 +183,9 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ // Discrete differentiation introduces small numerical errors, so allow a small tolerance const double limit_relative_tol = 0.999999; - const double jerk_limit = limit_relative_tol * limits_.jerk_limit; - const double acceleration_limit = limit_relative_tol * limits_.acceleration_limit; - const double velocity_limit = limit_relative_tol * limits_.velocity_limit; + const double jerk_limit = limit_relative_tol * configuration_.limits.jerk_limit; + const double acceleration_limit = limit_relative_tol * configuration_.limits.acceleration_limit; + const double velocity_limit = limit_relative_tol * configuration_.limits.velocity_limit; // Preallocate double delta_a(0), delta_v(0), position_error(0); @@ -156,23 +199,21 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ double delta_j = std::copysign(jerk_limit, waypoints_.jerks(index)) - waypoints_.jerks(index); waypoints_.jerks(index) = std::copysign(jerk_limit, waypoints_.jerks(index)); - waypoints_.accelerations(index) = waypoints_.accelerations(index - 1) + waypoints_.jerks(index) * timestep_; + waypoints_.accelerations(index) = + waypoints_.accelerations(index - 1) + waypoints_.jerks(index) * configuration_.timestep; waypoints_.velocities(index) = waypoints_.velocities(index - 1) + - waypoints_.accelerations(index - 1) * timestep_ + - 0.5 * waypoints_.jerks(index) * timestep_ * timestep_; + waypoints_.accelerations(index - 1) * configuration_.timestep + + 0.5 * waypoints_.jerks(index) * configuration_.timestep * configuration_.timestep; - // Re-calculate derivatives from the updated velocity vector - calculateDerivatives(); - - delta_v = 0.5 * delta_j * timestep_ * timestep_; + delta_v = 0.5 * delta_j * configuration_.timestep * configuration_.timestep; // Try adjusting the velocity in previous timesteps to compensate for this limit, if needed - successful_compensation = backwardLimitCompensation(index, &delta_v); + successful_compensation = backwardLimitCompensation(index, -delta_v); if (!successful_compensation) { - position_error = position_error + delta_v * timestep_; + position_error = position_error + delta_v * configuration_.timestep; } - if (fabs(position_error) > position_tolerance_) + if (fabs(position_error) > configuration_.position_tolerance) { recordFailureTime(index, index_last_successful); // Only break, do not return, because we are looking for the FIRST failure. May find an earlier failure in @@ -196,16 +237,15 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ // Check jerk limit before applying the change. // The first condition checks if the new jerk(i) is going to exceed the limit. Pretty straightforward. // We also calculate a new jerk(i+1). The second condition checks if jerk(i+1) would exceed the limit. - if ((fabs((temp_accel - waypoints_.accelerations(index)) / timestep_) <= jerk_limit) && - (fabs(waypoints_.jerks(index) + delta_a / timestep_) <= jerk_limit)) + if ((fabs((temp_accel - waypoints_.accelerations(index)) / configuration_.timestep) <= jerk_limit) && + (fabs(waypoints_.jerks(index) + delta_a / configuration_.timestep) <= jerk_limit)) { waypoints_.accelerations(index) = temp_accel; - waypoints_.jerks(index) = (waypoints_.accelerations(index) - waypoints_.accelerations(index - 1)) / timestep_; - waypoints_.velocities(index) = waypoints_.velocities(index - 1) + - waypoints_.accelerations(index - 1) * timestep_ + - 0.5 * waypoints_.jerks(index) * timestep_ * timestep_; - // Re-calculate derivatives from the updated velocity vector - calculateDerivatives(); + waypoints_.jerks(index) = + (waypoints_.accelerations(index) - waypoints_.accelerations(index - 1)) / configuration_.timestep; + waypoints_.velocities(index) = + waypoints_.velocities(index - 1) + waypoints_.accelerations(index - 1) * configuration_.timestep + + 0.5 * waypoints_.jerks(index) * configuration_.timestep * configuration_.timestep; } else { @@ -217,13 +257,13 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ } // Try adjusting the velocity in previous timesteps to compensate for this limit, if needed - delta_v = delta_a * timestep_; - successful_compensation = backwardLimitCompensation(index, &delta_v); + delta_v = delta_a * configuration_.timestep; + successful_compensation = backwardLimitCompensation(index, -delta_v); if (!successful_compensation) { - position_error = position_error + delta_v * timestep_; + position_error = position_error + delta_v * configuration_.timestep; } - if (fabs(position_error) > position_tolerance_) + if (fabs(position_error) > configuration_.position_tolerance) { recordFailureTime(index, index_last_successful); // Only break, do not return, because we are looking for the FIRST failure. May find an earlier failure in @@ -239,29 +279,25 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ position_error = 0; for (size_t index = 1; index < *index_last_successful; ++index) { - delta_v = 0; - // If the velocity limit would be exceeded if (fabs(waypoints_.velocities(index)) > velocity_limit) { 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); + delta_v += position_error / configuration_.timestep; + successful_compensation = backwardLimitCompensation(index, -delta_v); if (!successful_compensation) { - position_error = position_error + delta_v * timestep_; + position_error = position_error + delta_v * configuration_.timestep; } else position_error = 0; - if (fabs(position_error) > position_tolerance_ || fabs(waypoints_.accelerations(index)) > acceleration_limit || - fabs(waypoints_.jerks(index)) > jerk_limit || + if (fabs(position_error) > configuration_.position_tolerance || + fabs(waypoints_.accelerations(index)) > acceleration_limit || fabs(waypoints_.jerks(index)) > jerk_limit || fabs(waypoints_.accelerations(index + 1)) > acceleration_limit || fabs(waypoints_.jerks(index + 1)) > jerk_limit) { @@ -273,10 +309,13 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ } } + // Re-calculate derivatives from the updated velocity vector + calculateDerivativesFromVelocity(); + return ErrorCodeEnum::NO_ERROR; } -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 @@ -294,31 +333,37 @@ bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, doubl for (size_t index = limited_index; index > 2; --index) { // if there is some room to increase the velocity at timestep i - if (fabs(waypoints_.velocities(index)) < limits_.velocity_limit) + if (fabs(waypoints_.velocities(index)) < configuration_.limits.velocity_limit) { // If the full change can be made in this timestep - if ((*excess_velocity > 0 && waypoints_.velocities(index) <= limits_.velocity_limit - *excess_velocity) || - (*excess_velocity < 0 && waypoints_.velocities(index) >= -limits_.velocity_limit - *excess_velocity)) + if ((excess_velocity > 0 && + waypoints_.velocities(index) <= configuration_.limits.velocity_limit - excess_velocity) || + (excess_velocity < 0 && + waypoints_.velocities(index) >= -configuration_.limits.velocity_limit - excess_velocity)) { - double new_velocity = waypoints_.velocities(index) + *excess_velocity; + 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_accel = (new_velocity - waypoints_.velocities(index - 1)) / configuration_.timestep; double backward_jerk = - (backward_accel - (waypoints_.velocities(index - 1) - waypoints_.velocities(index - 2)) / timestep_) / - timestep_; + (backward_accel - + (waypoints_.velocities(index - 1) - waypoints_.velocities(index - 2)) / configuration_.timestep) / + configuration_.timestep; // Accel and jerk, calculated from upcoming waypoints - double forward_accel = (waypoints_.velocities(index + 1) - new_velocity) / timestep_; + double forward_accel = (waypoints_.velocities(index + 1) - new_velocity) / configuration_.timestep; double forward_jerk = - (forward_accel - (new_velocity - waypoints_.velocities(index - 1)) / timestep_) / timestep_; + (forward_accel - (new_velocity - waypoints_.velocities(index - 1)) / configuration_.timestep) / + configuration_.timestep; // Calculate this new velocity if it would not violate accel/jerk limits - if ((fabs(backward_jerk) < limits_.jerk_limit) && (fabs(backward_accel) < limits_.acceleration_limit) && - (fabs(forward_jerk) < limits_.jerk_limit) && (fabs(forward_accel) < limits_.acceleration_limit)) + if ((fabs(backward_jerk) < configuration_.limits.jerk_limit) && + (fabs(backward_accel) < configuration_.limits.acceleration_limit) && + (fabs(forward_jerk) < configuration_.limits.jerk_limit) && + (fabs(forward_accel) < configuration_.limits.acceleration_limit)) { waypoints_.velocities(index) = new_velocity; // if at the velocity limit, must stop accelerating - if (fabs(waypoints_.velocities(index)) >= limits_.velocity_limit) + if (fabs(waypoints_.velocities(index)) >= configuration_.limits.velocity_limit) { waypoints_.accelerations(index) = 0; waypoints_.jerks(index) = 0; @@ -333,33 +378,36 @@ bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, doubl 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) * configuration_.limits.velocity_limit; // Accel and jerk, calculated from the previous waypoints - double backward_accel = (new_velocity - waypoints_.velocities(index - 1)) / timestep_; + double backward_accel = (new_velocity - waypoints_.velocities(index - 1)) / configuration_.timestep; double backward_jerk = - (backward_accel - (waypoints_.velocities(index - 1) - waypoints_.velocities(index - 2)) / timestep_) / - timestep_; + (backward_accel - + (waypoints_.velocities(index - 1) - waypoints_.velocities(index - 2)) / configuration_.timestep) / + configuration_.timestep; // Accel and jerk, calculated from upcoming waypoints - double forward_accel = (waypoints_.velocities(index + 1) - new_velocity) / timestep_; + double forward_accel = (waypoints_.velocities(index + 1) - new_velocity) / configuration_.timestep; double forward_jerk = - (forward_accel - (new_velocity - waypoints_.velocities(index - 1)) / timestep_) / timestep_; + (forward_accel - (new_velocity - waypoints_.velocities(index - 1)) / configuration_.timestep) / + configuration_.timestep; // Apply these changes if all limits are satisfied (for the prior // waypoint and the future waypoint) - if ((fabs(backward_accel) < limits_.acceleration_limit) && (fabs(backward_jerk) < limits_.jerk_limit) && - (fabs(forward_accel) < limits_.acceleration_limit) && (fabs(forward_jerk) < limits_.jerk_limit)) + if ((fabs(backward_accel) < configuration_.limits.acceleration_limit) && + (fabs(backward_jerk) < configuration_.limits.jerk_limit) && + (fabs(forward_accel) < configuration_.limits.acceleration_limit) && + (fabs(forward_jerk) < configuration_.limits.jerk_limit)) { double delta_v = new_velocity - waypoints_.velocities(index); waypoints_.velocities(index) = new_velocity; // if at the velocity limit, must stop accelerating - if (fabs(waypoints_.velocities(index)) >= limits_.velocity_limit) + if (fabs(waypoints_.velocities(index)) >= configuration_.limits.velocity_limit) { waypoints_.accelerations(index) = 0; waypoints_.jerks(index) = 0; @@ -369,13 +417,14 @@ bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, doubl waypoints_.accelerations(index) = backward_accel; waypoints_.jerks(index) = backward_jerk; } - *excess_velocity = *excess_velocity - delta_v; + excess_velocity -= delta_v; } } } // Clip velocities at min/max due to small rounding errors waypoints_.velocities(index) = - std::min(std::max(waypoints_.velocities(index), -limits_.velocity_limit), limits_.velocity_limit); + std::min(std::max(waypoints_.velocities(index), -configuration_.limits.velocity_limit), + configuration_.limits.velocity_limit); } return successful_compensation; @@ -384,35 +433,37 @@ bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, doubl ErrorCodeEnum SingleJointGenerator::predictTimeToReach() { // Take a trajectory that could not reach the desired position in time. - // Try increasing the duration until it is interpolated without violating - // limits. + // Try increasing the duration until it is interpolated without violating limits. // This gives a new duration estimate. ErrorCodeEnum error_code = ErrorCodeEnum::NO_ERROR; // If in normal mode, we can extend trajectories - if (!use_streaming_mode_) + if (!configuration_.use_streaming_mode) { size_t new_num_waypoints = 0; // Iterate over new durations until the position error is acceptable or the maximum duration is reached while ((index_last_successful_ < static_cast(waypoints_.positions.size() - 1)) && - (desired_duration_ < max_duration_) && (new_num_waypoints < kMaxNumWaypointsFullTrajectory)) + (desired_duration_ < configuration_.max_duration) && (new_num_waypoints < kMaxNumWaypointsFullTrajectory)) { // Try increasing the duration, based on fraction of states that weren't reached successfully + // Choice of 0.2 is subjective but it should be between 0-1. + // A smaller fraction will find a solution that's closer to time-optimal because it adds fewer new waypoints to + // the search. But, a smaller fraction likely increases runtime. desired_duration_ = - 1. + 0.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_) - desired_duration_ = desired_duration_ + timestep_; + if (std::fmod(desired_duration_, configuration_.timestep) > 0.5 * configuration_.timestep) + desired_duration_ = desired_duration_ + configuration_.timestep; new_num_waypoints = std::max(static_cast(waypoints_.positions.size() + 1), - static_cast(floor(1 + desired_duration_ / timestep_))); + static_cast(floor(1 + desired_duration_ / configuration_.timestep))); // Cap the trajectory duration to maintain determinism if (new_num_waypoints > kMaxNumWaypointsFullTrajectory) new_num_waypoints = kMaxNumWaypointsFullTrajectory; - waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., desired_duration_); + waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., (new_num_waypoints - 1) * configuration_.timestep); waypoints_.positions.resize(waypoints_.elapsed_times.size()); waypoints_.velocities.resize(waypoints_.elapsed_times.size()); waypoints_.accelerations.resize(waypoints_.elapsed_times.size()); @@ -422,7 +473,7 @@ ErrorCodeEnum SingleJointGenerator::predictTimeToReach() // Try to create the trajectory again, with the new duration //////////////////////////////////////////////////////////// waypoints_.positions = interpolate(waypoints_.elapsed_times); - calculateDerivatives(); + calculateDerivativesFromPosition(); positionVectorLimitLookAhead(&index_last_successful_); } } @@ -463,7 +514,8 @@ ErrorCodeEnum SingleJointGenerator::predictTimeToReach() } // Normal mode: Error if we extended the duration to the maximum and it still wasn't successful - if (!use_streaming_mode_ && index_last_successful_ < static_cast(waypoints_.elapsed_times.size() - 1)) + if (!configuration_.use_streaming_mode && + index_last_successful_ < static_cast(waypoints_.elapsed_times.size() - 1)) { error_code = ErrorCodeEnum::MAX_DURATION_EXCEEDED; } @@ -489,26 +541,34 @@ ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* index_l // Initial waypoint waypoints_.positions(0) = current_joint_state_.position; for (size_t index = 1; index < static_cast(waypoints_.positions.size()) - 1; ++index) - waypoints_.positions(index) = waypoints_.positions(index - 1) + waypoints_.velocities(index - 1) * timestep_ + - 0.5 * waypoints_.accelerations(index - 1) * pow(timestep_, 2) + - one_sixth * waypoints_.jerks(index - 1) * pow(timestep_, 3); + waypoints_.positions(index) = waypoints_.positions(index - 1) + + waypoints_.velocities(index - 1) * configuration_.timestep + + 0.5 * waypoints_.accelerations(index - 1) * pow(configuration_.timestep, 2) + + one_sixth * waypoints_.jerks(index - 1) * pow(configuration_.timestep, 3); // Final waypoint should match the goal, unless trajectory was cut short for streaming mode - if (!use_streaming_mode_) + if (!configuration_.use_streaming_mode) waypoints_.positions(waypoints_.positions.size() - 1) = goal_joint_state_.position; return error_code; } -void SingleJointGenerator::calculateDerivatives() +void SingleJointGenerator::calculateDerivativesFromPosition() +{ + // From position vector, approximate vel/accel/jerk. + waypoints_.velocities = + DiscreteDifferentiation(waypoints_.positions, configuration_.timestep, current_joint_state_.velocity); + waypoints_.accelerations = + DiscreteDifferentiation(waypoints_.velocities, configuration_.timestep, current_joint_state_.acceleration); + waypoints_.jerks = DiscreteDifferentiation(waypoints_.accelerations, configuration_.timestep, 0); +} + +void SingleJointGenerator::calculateDerivativesFromVelocity() { - // From position vector, approximate velocity and acceleration. - // velocity = (difference between adjacent position elements) / delta_t - // acceleration = (difference between adjacent velocity elements) / delta_t - waypoints_.velocities = DiscreteDifferentiation(waypoints_.positions, timestep_, current_joint_state_.velocity); + // From velocity vector, approximate accel/jerk. waypoints_.accelerations = - DiscreteDifferentiation(waypoints_.velocities, timestep_, current_joint_state_.acceleration); - waypoints_.jerks = DiscreteDifferentiation(waypoints_.accelerations, timestep_, 0); + DiscreteDifferentiation(waypoints_.velocities, configuration_.timestep, current_joint_state_.acceleration); + waypoints_.jerks = DiscreteDifferentiation(waypoints_.accelerations, configuration_.timestep, 0); } void SingleJointGenerator::updateTrajectoryDuration(double new_trajectory_duration) @@ -516,6 +576,6 @@ void SingleJointGenerator::updateTrajectoryDuration(double new_trajectory_durati // The trajectory will be forced to have this duration (or fail) because // max_duration == desired_duration desired_duration_ = new_trajectory_duration; - max_duration_ = new_trajectory_duration; + configuration_.max_duration = new_trajectory_duration; } } // end namespace trackjoint diff --git a/src/streaming_example.cpp b/src/streaming_example.cpp index 5b85f7d9..6a563da2 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; @@ -65,6 +65,8 @@ int main(int argc, char** argv) std::vector output_trajectories(num_dof); trackjoint::TrajectoryGenerator traj_gen(num_dof, timestep, desired_duration, max_duration, start_state, goal_joint_states, limits, waypoint_position_tolerance, use_streaming_mode); + traj_gen.reset(timestep, desired_duration, max_duration, start_state, goal_joint_states, limits, + waypoint_position_tolerance, use_streaming_mode); // An example of optional input validation trackjoint::ErrorCodeEnum error_code = traj_gen.inputChecking(start_state, goal_joint_states, limits, timestep); diff --git a/src/three_dof_examples.cpp b/src/three_dof_examples.cpp index 56cb26c2..2172953a 100644 --- a/src/three_dof_examples.cpp +++ b/src/three_dof_examples.cpp @@ -19,53 +19,61 @@ 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); + traj_gen.reset(timestep, desired_duration, max_duration, current_joint_states, goal_joint_states, limits, + waypoint_position_tolerance, use_streaming_mode); std::vector output_trajectories(num_dof); @@ -125,98 +133,5 @@ int main(int argc, char** argv) } std::cout << "============================================" << std::endl; - ///////////////////////////////// - // Second example - large motions - ///////////////////////////////// - std::cout << "EXAMPLE 2" << std::endl; - desired_duration = 1; - - joint_state.position = 0.74; - joint_state.velocity = 0.33; - joint_state.acceleration = 0.15; - current_joint_states[0] = joint_state; - joint_state.position = -0.59; - joint_state.velocity = 0.2; - joint_state.acceleration = 2.39; - current_joint_states[1] = joint_state; - joint_state.position = -1.1; - joint_state.velocity = 1.13; - joint_state.acceleration = 0.9; - current_joint_states[2] = joint_state; - - joint_state.position = 0.93; - joint_state.velocity = 0.1; - joint_state.acceleration = -0.2; - goal_joint_states[0] = joint_state; - joint_state.position = -0.86; - joint_state.velocity = 0.05; - joint_state.acceleration = -0.12; - goal_joint_states[1] = joint_state; - joint_state.position = -0.42; - joint_state.velocity = 0.59; - joint_state.acceleration = -0.23; - goal_joint_states[2] = joint_state; - - // Create a new traj gen object with new parameters - traj_gen.~TrajectoryGenerator(); - new (&traj_gen) - trackjoint::TrajectoryGenerator(num_dof, timestep, desired_duration, max_duration, current_joint_states, - goal_joint_states, limits, waypoint_position_tolerance, use_streaming_mode); - - error_code = traj_gen.inputChecking(current_joint_states, goal_joint_states, limits, timestep); - - // Input error handling - if an error is found, the trajectory is not - // generated. - if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) - { - std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; - return -1; - } - - // Measure runtime - start = std::chrono::system_clock::now(); - error_code = traj_gen.generateTrajectories(&output_trajectories); - end = std::chrono::system_clock::now(); - - // Trajectory generation error handling - if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR) - { - std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; - return -1; - } - - elapsed_seconds = end - start; - - std::cout << "Runtime: " << elapsed_seconds.count() << std::endl; - std::cout << "Num waypoints: " << output_trajectories.at(0).positions.size() << std::endl; - std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; - - // 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; - return 0; } diff --git a/src/trajectory_generator.cpp b/src/trajectory_generator.cpp index 728fd8f8..40f3a9e6 100644 --- a/src/trajectory_generator.cpp +++ b/src/trajectory_generator.cpp @@ -31,10 +31,7 @@ TrajectoryGenerator::TrajectoryGenerator(uint num_dof, double timestep, double d // Initialize a trajectory generator for each joint 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(kNumWaypointsThreshold, kMaxNumWaypointsFullTrajectory)); } } @@ -57,11 +54,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 +132,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; } } @@ -252,6 +256,13 @@ void TrajectoryGenerator::saveTrajectoriesToFile(const 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 @@ -363,28 +380,25 @@ void TrajectoryGenerator::clipVectorsForOutput(std::vector* tra { for (size_t joint = 0; joint < kNumDof; ++joint) { - for (size_t joint = 0; joint < trajectory->size(); ++joint) + for (auto waypt = 0; waypt < trajectory->at(joint).velocities.size(); ++waypt) { - for (auto waypt = 0; waypt < trajectory->at(joint).velocities.size(); ++waypt) - { - // Velocity - if (trajectory->at(joint).velocities[waypt] > limits_[joint].velocity_limit) - trajectory->at(joint).velocities[waypt] = limits_[joint].velocity_limit; - if (trajectory->at(joint).velocities[waypt] < -limits_[joint].velocity_limit) - trajectory->at(joint).velocities[waypt] = -limits_[joint].velocity_limit; - - // Acceleration - if (trajectory->at(joint).accelerations[waypt] > limits_[joint].acceleration_limit) - trajectory->at(joint).accelerations[waypt] = limits_[joint].acceleration_limit; - if (trajectory->at(joint).accelerations[waypt] < -limits_[joint].acceleration_limit) - trajectory->at(joint).accelerations[waypt] = -limits_[joint].acceleration_limit; - - // Jerk - if (trajectory->at(joint).jerks[waypt] > limits_[joint].jerk_limit) - trajectory->at(joint).jerks[waypt] = limits_[joint].jerk_limit; - if (trajectory->at(joint).jerks[waypt] < -limits_[joint].jerk_limit) - trajectory->at(joint).jerks[waypt] = -limits_[joint].jerk_limit; - } + // Velocity + if (trajectory->at(joint).velocities[waypt] > limits_[joint].velocity_limit) + trajectory->at(joint).velocities[waypt] = limits_[joint].velocity_limit; + if (trajectory->at(joint).velocities[waypt] < -limits_[joint].velocity_limit) + trajectory->at(joint).velocities[waypt] = -limits_[joint].velocity_limit; + + // Acceleration + if (trajectory->at(joint).accelerations[waypt] > limits_[joint].acceleration_limit) + trajectory->at(joint).accelerations[waypt] = limits_[joint].acceleration_limit; + if (trajectory->at(joint).accelerations[waypt] < -limits_[joint].acceleration_limit) + trajectory->at(joint).accelerations[waypt] = -limits_[joint].acceleration_limit; + + // Jerk + if (trajectory->at(joint).jerks[waypt] > limits_[joint].jerk_limit) + trajectory->at(joint).jerks[waypt] = limits_[joint].jerk_limit; + if (trajectory->at(joint).jerks[waypt] < -limits_[joint].jerk_limit) + trajectory->at(joint).jerks[waypt] = -limits_[joint].jerk_limit; } } } diff --git a/test/trajectory_generation_test.cpp b/test/trajectory_generation_test.cpp index 5bde2e57..bc4117a9 100644 --- a/test/trajectory_generation_test.cpp +++ b/test/trajectory_generation_test.cpp @@ -36,7 +36,7 @@ namespace trackjoint class TrajectoryGenerationTest : public ::testing::Test { public: - TrajectoryGenerationTest() : output_trajectories_(num_dof_) + TrajectoryGenerationTest() : output_trajectories_(num_dof_), skip_teardown_checks_(false) { // Default test parameters for 3 joints KinematicState joint_state; @@ -67,18 +67,19 @@ 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_; + bool skip_teardown_checks_; 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 +149,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; } @@ -189,15 +196,35 @@ class TrajectoryGenerationTest : public ::testing::Test void TearDown() override { - checkBounds(); - if (write_output_) + if (!skip_teardown_checks_) { - writeOutputToFiles(); + checkBounds(); + if (write_output_) + { + writeOutputToFiles(); + } } } }; // class TrajectoryGenerationTest +TEST_F(TrajectoryGenerationTest, DetectNoReset) +{ + // Calling `generateTrajectories()` without an initial `reset()` + // should return an error code (and not segfault!) + + TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration_, max_duration_, current_joint_states_, + goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); + ErrorCodeEnum error_code = traj_gen.generateTrajectories(&output_trajectories_); + EXPECT_EQ(error_code, ErrorCodeEnum::OBJECT_NOT_RESET); + + // Ensure OBJECT_NOT_RESET can be converted to string + std::cout << "Error: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl; + + // TearDown() should skip post-test checks + skip_teardown_checks_ = true; +} + TEST_F(TrajectoryGenerationTest, EasyDefaultTrajectory) { // Use the class defaults. This trajectory is easy, does not require limit @@ -205,6 +232,8 @@ TEST_F(TrajectoryGenerationTest, EasyDefaultTrajectory) TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration_, max_duration_, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); + traj_gen.reset(timestep_, desired_duration_, max_duration_, current_joint_states_, goal_joint_states_, limits_, + position_tolerance_, use_streaming_mode_); EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error @@ -250,6 +279,8 @@ TEST_F(TrajectoryGenerationTest, OneTimestepDuration) TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); + traj_gen.reset(timestep_, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, + position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); @@ -258,6 +289,10 @@ TEST_F(TrajectoryGenerationTest, OneTimestepDuration) double position_tolerance = 1e-4; double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); EXPECT_LT(position_error, position_tolerance); + // Timestep + double timestep_tolerance = 0.1 * timestep_; + EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep_, + timestep_tolerance); // Duration const double duration_tolerance = 5e-3; size_t vector_length = output_trajectories_[0].elapsed_times.size() - 1; @@ -280,6 +315,8 @@ TEST_F(TrajectoryGenerationTest, RoughlyTwoTimestepDuration) TrajectoryGenerator traj_gen(num_dof_, timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); + traj_gen.reset(timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, + position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); @@ -288,16 +325,16 @@ TEST_F(TrajectoryGenerationTest, RoughlyTwoTimestepDuration) const double position_tolerance = 1e-4; const double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); EXPECT_LT(position_error, position_tolerance); + // Timestep + double timestep_tolerance = 0.003; + EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep, + timestep_tolerance); // Duration const double duration_tolerance = 5e-4; size_t vector_length = output_trajectories_[0].elapsed_times.size() - 1; EXPECT_NEAR(output_trajectories_[0].elapsed_times(vector_length), desired_duration, duration_tolerance); // Number of waypoints EXPECT_EQ(output_trajectories_[0].elapsed_times.size(), 3); - // Timestep - double timestep_tolerance = 0.003; - EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep, - timestep_tolerance); } TEST_F(TrajectoryGenerationTest, FourTimestepDuration) @@ -316,6 +353,8 @@ TEST_F(TrajectoryGenerationTest, FourTimestepDuration) TrajectoryGenerator traj_gen(num_dof_, timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); + traj_gen.reset(timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, + position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); @@ -324,6 +363,10 @@ TEST_F(TrajectoryGenerationTest, FourTimestepDuration) const double position_tolerance = 1e-4; const double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); EXPECT_LT(position_error, position_tolerance); + // Timestep + double timestep_tolerance = 0.1 * timestep; + EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep, + timestep_tolerance); // Duration const double duration_tolerance = 5e-3; size_t vector_length = output_trajectories_[0].elapsed_times.size() - 1; @@ -363,6 +406,8 @@ TEST_F(TrajectoryGenerationTest, SixTimestepDuration) TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); + traj_gen.reset(timestep_, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, + position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); @@ -371,6 +416,10 @@ TEST_F(TrajectoryGenerationTest, SixTimestepDuration) double position_tolerance = 1e-4; double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); EXPECT_LT(position_error, position_tolerance); + // Timestep + double timestep_tolerance = 0.1 * timestep_; + EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep_, + timestep_tolerance); // Duration const double duration_tolerance = 5e-3; size_t vector_length = output_trajectories_[0].elapsed_times.size() - 1; @@ -403,6 +452,8 @@ TEST_F(TrajectoryGenerationTest, VelAccelJerkLimit) TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration_, max_duration, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); + traj_gen.reset(timestep_, desired_duration_, max_duration, current_joint_states_, goal_joint_states_, limits_, + position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); @@ -413,10 +464,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) @@ -450,6 +504,24 @@ TEST_F(TrajectoryGenerationTest, NoisyStreamingCommand) limits_[1] = single_joint_limits; limits_[2] = single_joint_limits; + // For recording actual followed trajectory + std::vector recorded_trajectories(num_dof_); + for (size_t joint = 0; joint < num_dof_; ++joint) + { + // Resize vector + recorded_trajectories[joint].positions.resize(num_waypoints); + recorded_trajectories[joint].velocities.resize(num_waypoints); + recorded_trajectories[joint].accelerations.resize(num_waypoints); + recorded_trajectories[joint].jerks.resize(num_waypoints); + recorded_trajectories[joint].elapsed_times.resize(num_waypoints); + // Set initial waypoint + recorded_trajectories[joint].positions(0) = current_joint_states_[joint].position; + recorded_trajectories[joint].velocities(0) = current_joint_states_[joint].velocity; + recorded_trajectories[joint].accelerations(0) = current_joint_states_[joint].acceleration; + recorded_trajectories[joint].jerks(0) = 0; + recorded_trajectories[joint].elapsed_times(0) = 0; + } + Eigen::VectorXd x_desired(num_waypoints); Eigen::VectorXd x_smoothed(num_waypoints); @@ -480,6 +552,18 @@ TEST_F(TrajectoryGenerationTest, NoisyStreamingCommand) x_smoothed(waypoint) = output_trajectories_.at(0).positions(1); // ... and setting the next current position as the updated x_smoothed joint_state.position = x_smoothed(waypoint); + joint_state.velocity = output_trajectories_.at(0).velocities(1); + joint_state.acceleration = output_trajectories_.at(0).accelerations(1); + + // Record next point + for (size_t joint = 0; joint < num_dof_; joint++) + { + recorded_trajectories[joint].positions(waypoint) = output_trajectories_[joint].positions(1); + recorded_trajectories[joint].velocities(waypoint) = output_trajectories_[joint].velocities(1); + recorded_trajectories[joint].accelerations(waypoint) = output_trajectories_[joint].accelerations(1); + recorded_trajectories[joint].jerks(waypoint) = output_trajectories_[joint].jerks(1); + recorded_trajectories[joint].elapsed_times(waypoint) = time; + } current_joint_states_[0] = joint_state; current_joint_states_[1] = joint_state; @@ -490,6 +574,9 @@ TEST_F(TrajectoryGenerationTest, NoisyStreamingCommand) uint num_waypoint_tolerance = 1; uint expected_num_waypoints = num_waypoints; EXPECT_NEAR(uint(x_smoothed.size()), expected_num_waypoints, num_waypoint_tolerance); + + // Put recorded trajectories where the tearDown() method will check them + output_trajectories_ = recorded_trajectories; } TEST_F(TrajectoryGenerationTest, OscillatingUR5TrackJointCase) @@ -536,8 +623,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 +638,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]; @@ -635,6 +723,8 @@ TEST_F(TrajectoryGenerationTest, SuddenChangeOfDirection) TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); + traj_gen.reset(timestep_, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, + position_tolerance_, use_streaming_mode_); traj_gen.generateTrajectories(&output_trajectories_); EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); @@ -643,6 +733,10 @@ TEST_F(TrajectoryGenerationTest, SuddenChangeOfDirection) double position_tolerance = 1e-4; double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); EXPECT_LT(position_error, position_tolerance); + // Timestep + double timestep_tolerance = 0.1 * timestep_; + EXPECT_NEAR(output_trajectories_[0].elapsed_times[1] - output_trajectories_[0].elapsed_times[0], timestep_, + timestep_tolerance); // Duration const double duration_tolerance = 5e-3; size_t vector_length = output_trajectories_[0].elapsed_times.size() - 1; @@ -683,12 +777,18 @@ TEST_F(TrajectoryGenerationTest, LimitCompensation) TrajectoryGenerator traj_gen(num_dof_, timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); + traj_gen.reset(timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, + position_tolerance_, use_streaming_mode_); EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error 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 +832,22 @@ TEST_F(TrajectoryGenerationTest, DurationExtension) TrajectoryGenerator traj_gen(num_dof_, timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); + traj_gen.reset(timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, + position_tolerance_, use_streaming_mode_); EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error - 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 +892,18 @@ TEST_F(TrajectoryGenerationTest, PositiveAndNegativeLimits) TrajectoryGenerator traj_gen(num_dof_, timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); + traj_gen.reset(timestep, desired_duration, max_duration, current_joint_states_, goal_joint_states_, limits_, + position_tolerance_, use_streaming_mode_); EXPECT_EQ(ErrorCodeEnum::NO_ERROR, traj_gen.generateTrajectories(&output_trajectories_)); // Position error 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; @@ -829,6 +940,8 @@ TEST_F(TrajectoryGenerationTest, TimestepDidNotMatch) TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration_, max_duration_, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); + traj_gen.reset(timestep_, desired_duration_, max_duration_, current_joint_states_, goal_joint_states_, limits_, + position_tolerance_, use_streaming_mode_); output_trajectories_.resize(num_dof_); EXPECT_EQ(ErrorCodeEnum::NO_ERROR, @@ -862,7 +975,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. @@ -891,6 +1004,8 @@ TEST_F(TrajectoryGenerationTest, CustomerStreaming) // Generate initial trajectory TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration, max_duration_, current_joint_states_, goal_joint_states_, limits_, waypoint_position_tolerance, use_streaming_mode_); + traj_gen.reset(timestep_, desired_duration, max_duration_, current_joint_states_, goal_joint_states_, limits_, + position_tolerance_, use_streaming_mode_); ErrorCodeEnum error_code = traj_gen.generateTrajectories(&output_trajectories_); EXPECT_EQ(error_code, ErrorCodeEnum::NO_ERROR); @@ -939,9 +1054,60 @@ TEST_F(TrajectoryGenerationTest, StreamingTooFewTimesteps) TrajectoryGenerator traj_gen(num_dof_, timestep_, desired_duration_, max_duration_, current_joint_states_, goal_joint_states_, limits_, position_tolerance_, use_streaming_mode_); + traj_gen.reset(timestep_, desired_duration_, max_duration_, current_joint_states_, goal_joint_states_, limits_, + position_tolerance_, use_streaming_mode_); EXPECT_EQ(ErrorCodeEnum::LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE, traj_gen.inputChecking(current_joint_states_, goal_joint_states_, limits_, timestep_)); } + +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_); + traj_gen.reset(timestep_, desired_duration_, max_duration_, current_joint_states_, goal_joint_states_, limits_, + position_tolerance_, use_streaming_mode_); + output_trajectories_.resize(num_dof_); + + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, + 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)