From 1be42250f0731af68e96d4b5113093b7c17aad6f Mon Sep 17 00:00:00 2001 From: John Morris Date: Tue, 15 Sep 2020 14:51:43 -0500 Subject: [PATCH 1/4] Member variable refactor: Remove calculations from constructor SingleJointGenerator::reset does these same computations, refactor code to use this instead --- include/trackjoint/error_codes.h | 34 ++++++------ include/trackjoint/single_joint_generator.h | 21 ++++---- src/simple_example.cpp | 2 + src/single_joint_generator.cpp | 37 +++---------- src/streaming_example.cpp | 2 + src/three_dof_examples.cpp | 2 + src/trajectory_generator.cpp | 8 +-- test/trajectory_generation_test.cpp | 57 +++++++++++++++++++-- 8 files changed, 98 insertions(+), 65 deletions(-) diff --git a/include/trackjoint/error_codes.h b/include/trackjoint/error_codes.h index 631fbfee..6dd5529a 100644 --- a/include/trackjoint/error_codes.h +++ b/include/trackjoint/error_codes.h @@ -31,24 +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 382484aa..31cdd43d 100644 --- a/include/trackjoint/single_joint_generator.h +++ b/include/trackjoint/single_joint_generator.h @@ -29,6 +29,14 @@ 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 timestep desired time between waypoints * input max_duration allow the trajectory to be extended up to this limit. Error if that cannot be done. @@ -36,22 +44,14 @@ class SingleJointGenerator * 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 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 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); @@ -140,5 +140,6 @@ class SingleJointGenerator // 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/src/simple_example.cpp b/src/simple_example.cpp index 76eaa556..d3ec40d8 100644 --- a/src/simple_example.cpp +++ b/src/simple_example.cpp @@ -66,6 +66,8 @@ int main(int argc, char** argv) // 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 87f4876c..70098a1a 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -10,39 +10,12 @@ namespace trackjoint { -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 timestep_was_upsampled) +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) - , 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; - } + , is_reset_(false) - // 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, @@ -58,6 +31,7 @@ void SingleJointGenerator::reset(double timestep, double max_duration, const Kin position_tolerance_ = position_tolerance; index_last_successful_ = 0; use_streaming_mode_ = use_streaming_mode; + is_reset_ = true; // Start with this estimate of the shortest possible duration // The shortest possible duration avoids oscillation, as much as possible @@ -79,6 +53,9 @@ void SingleJointGenerator::reset(double timestep, double max_duration, const Kin ErrorCodeEnum SingleJointGenerator::generateTrajectory() { + if (!is_reset_) + return ErrorCodeEnum::OBJECT_NOT_RESET; + // Clear previous results waypoints_ = JointTrajectory(); waypoints_.positions = interpolate(nominal_times_); diff --git a/src/streaming_example.cpp b/src/streaming_example.cpp index c8d72642..6a563da2 100644 --- a/src/streaming_example.cpp +++ b/src/streaming_example.cpp @@ -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 7e8ef65e..2172953a 100644 --- a/src/three_dof_examples.cpp +++ b/src/three_dof_examples.cpp @@ -72,6 +72,8 @@ int main(int argc, char** argv) // 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); diff --git a/src/trajectory_generator.cpp b/src/trajectory_generator.cpp index cd5a7b69..91bf6074 100644 --- a/src/trajectory_generator.cpp +++ b/src/trajectory_generator.cpp @@ -29,13 +29,9 @@ 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_, max_duration_, current_joint_states[joint], goal_joint_states[joint], limits[joint], - upsampled_num_waypoints_, kNumWaypointsThreshold, kMaxNumWaypointsFullTrajectory, position_tolerance, - use_streaming_mode_, timestep_was_upsampled)); + single_joint_generators_.push_back(SingleJointGenerator(kNumWaypointsThreshold, kMaxNumWaypointsFullTrajectory)); } } @@ -436,4 +432,4 @@ ErrorCodeEnum TrajectoryGenerator::generateTrajectories(std::vector output_trajectories_; + bool skip_teardown_checks_; void checkBounds() { @@ -195,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 @@ -211,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 @@ -256,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_)); @@ -290,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_)); @@ -326,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_)); @@ -377,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_)); @@ -421,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_)); @@ -657,6 +690,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_)); @@ -709,6 +744,8 @@ 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 @@ -762,6 +799,8 @@ 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 @@ -820,6 +859,8 @@ 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 @@ -866,6 +907,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, @@ -928,6 +971,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); @@ -976,6 +1021,8 @@ 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_)); } @@ -1011,6 +1058,8 @@ TEST_F(TrajectoryGenerationTest, SingleJointOscillation) 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, From 852544909b9e5466af804a07abf6fbdaded0b1e2 Mon Sep 17 00:00:00 2001 From: John Morris Date: Tue, 15 Sep 2020 14:55:05 -0500 Subject: [PATCH 2/4] SingleJointGenerator: Move configuration variables to struct Configuration variables are moved to a struct, written to by `reset()` and used by other methods. --- include/trackjoint/configuration.h | 54 ++++++ include/trackjoint/single_joint_generator.h | 28 ++-- src/single_joint_generator.cpp | 174 +++++++++++--------- 3 files changed, 167 insertions(+), 89 deletions(-) create mode 100644 include/trackjoint/configuration.h 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/single_joint_generator.h b/include/trackjoint/single_joint_generator.h index 31cdd43d..355e994e 100644 --- a/include/trackjoint/single_joint_generator.h +++ b/include/trackjoint/single_joint_generator.h @@ -17,6 +17,7 @@ #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" @@ -36,6 +37,19 @@ class SingleJointGenerator */ 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 @@ -123,23 +137,13 @@ class SingleJointGenerator 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/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index 70098a1a..ebd3896f 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -18,19 +18,14 @@ SingleJointGenerator::SingleJointGenerator(size_t num_waypoints_threshold, size_ { } -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, +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; - max_duration_ = max_duration; + configuration_ = configuration; 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; is_reset_ = true; // Start with this estimate of the shortest possible duration @@ -39,18 +34,28 @@ void SingleJointGenerator::reset(double timestep, double max_duration, const Kin if (!timestep_was_upsampled) { desired_duration_ = - std::max(timestep_, fabs((goal_joint_state.position - current_joint_state.position) / limits_.velocity_limit)); + 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) * timestep; + 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_) @@ -60,7 +65,8 @@ ErrorCodeEnum SingleJointGenerator::generateTrajectory() waypoints_ = JointTrajectory(); waypoints_.positions = interpolate(nominal_times_); - waypoints_.elapsed_times.setLinSpaced(waypoints_.positions.size(), 0., (waypoints_.positions.size() - 1) * timestep_); + waypoints_.elapsed_times.setLinSpaced(waypoints_.positions.size(), 0., + (waypoints_.positions.size() - 1) * configuration_.timestep); calculateDerivativesFromPosition(); ErrorCodeEnum error_code = positionVectorLimitLookAhead(&index_last_successful_); @@ -76,7 +82,7 @@ ErrorCodeEnum SingleJointGenerator::generateTrajectory() void SingleJointGenerator::extendTrajectoryDuration() { - size_t new_num_waypoints = 1 + desired_duration_ / timestep_; + 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. @@ -92,7 +98,7 @@ void SingleJointGenerator::extendTrajectoryDuration() 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_); + 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); @@ -108,7 +114,7 @@ void SingleJointGenerator::extendTrajectoryDuration() else { waypoints_ = JointTrajectory(); - waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., (new_num_waypoints - 1) * timestep_); + 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_); @@ -167,7 +173,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; @@ -176,9 +182,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); @@ -192,20 +198,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; - 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); 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 @@ -229,14 +236,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_; + 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 { @@ -248,13 +256,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_; + 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 @@ -278,17 +286,17 @@ ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_ // Try adjusting the velocity in previous timesteps to compensate for this limit. // Try to account for position error, too. - delta_v += position_error / timestep_; + 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) { @@ -324,31 +332,36 @@ 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; // Accel and jerk, calculated from the previous waypoints - double backward_accel = (new_velocity - waypoints_.velocities(index - 1)) / timestep_; - double backward_jerk = - (backward_accel - (waypoints_.velocities(index - 1) - waypoints_.velocities(index - 2)) / timestep_) / - 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)) / + 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; @@ -368,27 +381,30 @@ bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, doubl { // 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_jerk = - (backward_accel - (waypoints_.velocities(index - 1) - waypoints_.velocities(index - 2)) / timestep_) / - 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)) / + 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; @@ -404,7 +420,8 @@ bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, doubl } // 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; @@ -419,28 +436,28 @@ ErrorCodeEnum SingleJointGenerator::predictTimeToReach() 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 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., (new_num_waypoints - 1) * timestep_); + 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()); @@ -491,7 +508,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; } @@ -524,12 +542,13 @@ 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; @@ -538,18 +557,19 @@ ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* index_l void SingleJointGenerator::calculateDerivativesFromPosition() { // From position vector, approximate vel/accel/jerk. - waypoints_.velocities = DiscreteDifferentiation(waypoints_.positions, timestep_, current_joint_state_.velocity); + waypoints_.velocities = + DiscreteDifferentiation(waypoints_.positions, configuration_.timestep, current_joint_state_.velocity); 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::calculateDerivativesFromVelocity() { // 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) @@ -557,6 +577,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 From b737ec30cc94fd44252e8b18de4b63e8351e16b8 Mon Sep 17 00:00:00 2001 From: John Morris Date: Thu, 17 Sep 2020 00:01:21 -0500 Subject: [PATCH 3/4] NoisyStreamingCommand test: Record traveled trajectory Record traveled trajectory for inspection by `checkBounds()` in `TearDown()` function --- test/trajectory_generation_test.cpp | 31 +++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/test/trajectory_generation_test.cpp b/test/trajectory_generation_test.cpp index 5e97baa4..c3cf0e80 100644 --- a/test/trajectory_generation_test.cpp +++ b/test/trajectory_generation_test.cpp @@ -504,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); @@ -535,6 +553,16 @@ TEST_F(TrajectoryGenerationTest, NoisyStreamingCommand) // ... and setting the next current position as the updated x_smoothed joint_state.position = x_smoothed(waypoint); + // 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; current_joint_states_[2] = joint_state; @@ -544,6 +572,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) From 883c5b15bb5523b2d7017a7348d8c3ff2a3a76a4 Mon Sep 17 00:00:00 2001 From: John Morris Date: Thu, 17 Sep 2020 00:43:43 -0500 Subject: [PATCH 4/4] NoisyStreamingCommand test: Add velocity/acceleration to next state To simulate velocity-, acceleration- and jerk-limited motion, the next state's velocity and acceleration must be updated along with position. --- test/trajectory_generation_test.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/test/trajectory_generation_test.cpp b/test/trajectory_generation_test.cpp index c3cf0e80..bc4117a9 100644 --- a/test/trajectory_generation_test.cpp +++ b/test/trajectory_generation_test.cpp @@ -552,6 +552,8 @@ 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++)