From 8e3bf3cca32b04580bae5c0c2d00e11c38223a2e Mon Sep 17 00:00:00 2001 From: John Morris Date: Mon, 9 Nov 2020 14:01:20 -0600 Subject: [PATCH 1/5] .travis.yml: Add code-coverage check --- .travis.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.travis.yml b/.travis.yml index 7aacd59c..83cc6a8d 100644 --- a/.travis.yml +++ b/.travis.yml @@ -22,6 +22,7 @@ env: - TEST=clang-format # check code formatting for compliance to .clang-format rules - TEST=clang-tidy-fix # perform static code analysis and compliance check against .clang-tidy rules - TEST=catkin_lint # perform catkin_lint checks + - TEST=code-coverage # check test coverage # pull in packages from a local .rosinstall file - UPSTREAM_WORKSPACE=trackjoint.rosinstall From a933b5e9c5c54627b2d1006b83cdcfcb3a296b3a Mon Sep 17 00:00:00 2001 From: John Morris Date: Tue, 10 Nov 2020 16:38:37 -0600 Subject: [PATCH 2/5] Add single_joint_generator_test Copied from `trajectory_generation_test` and stripped down for the single joint --- CMakeLists.txt | 9 + test/single_joint_generator_test.cpp | 275 ++++++++++++++++++++++++++ test/single_joint_generator_test.test | 13 ++ 3 files changed, 297 insertions(+) create mode 100644 test/single_joint_generator_test.cpp create mode 100644 test/single_joint_generator_test.test diff --git a/CMakeLists.txt b/CMakeLists.txt index 042200cf..98965a2f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -155,6 +155,15 @@ if(CATKIN_ENABLE_TESTING) ${catkin_LIBRARIES} ) + add_rostest_gtest(single_joint_generator_test + test/single_joint_generator_test.test + test/single_joint_generator_test.cpp + ) + target_link_libraries(single_joint_generator_test + ${LIBRARY_NAME} + ${catkin_LIBRARIES} + ) + endif() ## Test for correct C++ source code diff --git a/test/single_joint_generator_test.cpp b/test/single_joint_generator_test.cpp new file mode 100644 index 00000000..4f582a26 --- /dev/null +++ b/test/single_joint_generator_test.cpp @@ -0,0 +1,275 @@ +/********************************************************************* + * Software License Agreement + * + * Copyright (c) 2020, PickNik Consulting + * All rights reserved. + * + *********************************************************************/ + +/* Author: Dave Coleman, John Morris + Desc: Run TrackJoint single joint trajectory algorithm test cases +*/ + +// C++ +#include +#include +#include +#include +#include + +// Testing +#include + +// Target testing library +#include +#include +#include "ros/package.h" +#include "ros/ros.h" + +// Preparing data file handling +#include +std::string REF_PATH = ros::package::getPath("trackjoint"); +std::string BASE_FILEPATH = REF_PATH + "/test/data/single_joint_output"; + +namespace trackjoint +{ +class SingleJointGeneratorTest : public ::testing::Test +{ +public: + SingleJointGeneratorTest() + { + // Default test parameters + current_joint_state_.position = 0; + current_joint_state_.velocity = 0; + current_joint_state_.acceleration = 0; + goal_joint_state_.position = 1; + goal_joint_state_.velocity = 0; + goal_joint_state_.acceleration = 0; + joint_limits_.velocity_limit = 20; + joint_limits_.acceleration_limit = 200; + joint_limits_.jerk_limit = 20000; + } + +protected: + // Default test parameters + double timestep_ = 0.01; + double desired_duration_ = 1; + Eigen::Index num_waypoints_ = 0; + double max_duration_ = 0; + KinematicState current_joint_state_, goal_joint_state_; + Limits joint_limits_; + double position_error_; + double position_tolerance_ = 1e-4; + bool use_streaming_mode_ = false; + bool write_output_ = true; + JointTrajectory output_trajectory_; + // From trajectory_generator.h + const size_t kNumWaypointsThreshold_ = 10; + const size_t kMaxNumWaypointsFullTrajectory_ = 10000; + + Eigen::Index size() + { + assert (num_waypoints_ > 0); + return num_waypoints_; + } + + std::string name() + { + return ::testing::UnitTest::GetInstance()->current_test_info()->name(); + } + + void genTrajectory() + { + if (num_waypoints_ == 0) + num_waypoints_ = 1 + desired_duration_ / timestep_; + if (max_duration_ == 0) + max_duration_ = desired_duration_; + + SingleJointGenerator gen(kNumWaypointsThreshold_, + kMaxNumWaypointsFullTrajectory_); + gen.reset(timestep_, max_duration_, current_joint_state_, + goal_joint_state_, joint_limits_, num_waypoints_, + position_tolerance_, use_streaming_mode_, true); + int err = gen.generateTrajectory(); + std::cerr << name() << " Error: " + << trackjoint::ERROR_CODE_MAP.at(err) << std::endl; + EXPECT_EQ(ErrorCodeEnum::NO_ERROR, err); + output_trajectory_ = gen.getTrajectory(); + } + + void checkBounds() + { + if (output_trajectory_.elapsed_times.size() == 0) + return; + + // Sanity check vector lengths + EXPECT_EQ(output_trajectory_.elapsed_times.size(), size()); + EXPECT_EQ(output_trajectory_.positions.size(), size()); + EXPECT_EQ(output_trajectory_.velocities.size(), size()); + EXPECT_EQ(output_trajectory_.accelerations.size(), size()); + EXPECT_EQ(output_trajectory_.jerks.size(), size()); + + double elapsed_time = output_trajectory_.elapsed_times[size() - 1]; + + // Get estimate of min/max position and velocity using start and + // end states + double min_pos = std::min(current_joint_state_.position, + goal_joint_state_.position); + double max_pos = std::max(current_joint_state_.position, + goal_joint_state_.position); + double max_vel_mag = + std::max(std::fabs(current_joint_state_.velocity), + std::fabs(goal_joint_state_.velocity)); + + // Here, we consider two cases to estimate worst case min/max + // + // Case 1: We move at the maximum start/end velocity for half of + // the trajectory duration + // Needed for cases where we do a S curve + double potential_min = min_pos - max_vel_mag * elapsed_time / 2.0; + double potential_max = max_pos + max_vel_mag * elapsed_time / 2.0; + + // Case 2: We move at the velocity needed to move from start to + // end for half of the trajectory duration + // Needed for cases with start and end velocity of 0 + double dist_vel_mag = + std::fabs((goal_joint_state_.position - current_joint_state_.position) + / elapsed_time); + double potential_min_2 = min_pos - dist_vel_mag * elapsed_time / 2.0; + double potential_max_2 = max_pos + dist_vel_mag * elapsed_time / 2.0; + + min_pos = std::min(potential_min, potential_min_2); + max_pos = std::max(potential_max, potential_max_2); + + EXPECT_TRUE(VerifyVectorWithinBounds(min_pos, max_pos, + output_trajectory_.positions)); + } + + double calculatePositionAccuracy(KinematicState goal_joint_state, + JointTrajectory& trajectory) + { + double goal_position = goal_joint_state_.position; + double final_position = trajectory.positions((size() - 1)); + + double error = final_position - goal_position; + return error; + } + + void checkPositionError() + { + position_error_ = calculatePositionAccuracy( + goal_joint_state_, output_trajectory_); + EXPECT_LT(position_error_, position_tolerance_); + } + + void checkTimestep() + { + double timestep_tolerance = 0.1 * timestep_; + EXPECT_NEAR(output_trajectory_.elapsed_times[1] + - output_trajectory_.elapsed_times[0], + timestep_, timestep_tolerance); + } + + void checkDuration() + { + uint num_waypoint_tolerance = 1; + uint expected_num_waypoints = 1 + desired_duration_ / timestep_; + EXPECT_NEAR(uint(size()), expected_num_waypoints, num_waypoint_tolerance); + } + + void verifyVelAccelJerkLimits() + { + double maxVelocityMagnitude = \ + output_trajectory_.velocities.cwiseAbs().maxCoeff(); + EXPECT_LE(maxVelocityMagnitude, joint_limits_.velocity_limit); + double maxAccelerationMagnitude = \ + output_trajectory_.accelerations.cwiseAbs().maxCoeff(); + EXPECT_LE(maxAccelerationMagnitude, joint_limits_.acceleration_limit); + double maxJerkMagnitude = \ + output_trajectory_.jerks.cwiseAbs().maxCoeff(); + EXPECT_LE(maxJerkMagnitude, joint_limits_.jerk_limit); + } + + void writeOutputToFiles() + { + std::ofstream output_file; + std::string file = BASE_FILEPATH + "_" + name() + ".csv"; + std::cerr << "Writing values to file " << file << std::endl; + + output_file.open(file, std::ofstream::out); + for (Eigen::Index waypoint = 0; waypoint < output_trajectory_.positions.size(); ++waypoint) + { + output_file << output_trajectory_.elapsed_times(waypoint) << " " + << output_trajectory_.positions(waypoint) << " " + << output_trajectory_.velocities(waypoint) << " " + << output_trajectory_.accelerations(waypoint) << " " + << output_trajectory_.jerks(waypoint) << std::endl; + } + output_file.clear(); + output_file.close(); + } + + void runTest() + { + genTrajectory(); + checkPositionError(); + checkTimestep(); + checkDuration(); + verifyVelAccelJerkLimits(); + checkBounds(); + } + + void TearDown() override + { + if (write_output_) + writeOutputToFiles(); + } + +}; // class SingleJointGeneratorTest + +TEST_F(SingleJointGeneratorTest, LimitAcceleration) +{ + // Limit only acceleration + // Derived from LinuxCNC "limit3/limit-accel-and-max" test + + goal_joint_state_.position = 160; + + joint_limits_.velocity_limit = 1e99; + joint_limits_.acceleration_limit = 1000; + joint_limits_.jerk_limit = 1e99; + + desired_duration_ = 0.800; + max_duration_ = 10.0; + timestep_ = 0.001; + + runTest(); +} + +TEST_F(SingleJointGeneratorTest, LimitVelocity) +{ + // Limit only velocity + // Derived from LinuxCNC "limit3/limit-velocity" test + + goal_joint_state_.position = 400; + + joint_limits_.velocity_limit = 500; + joint_limits_.acceleration_limit = 1e99; + joint_limits_.jerk_limit = 1e99; + + desired_duration_ = 0.800; + max_duration_ = 10.0; + timestep_ = 0.001; + + runTest(); +} + +} // namespace trackjoint + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + + int result = RUN_ALL_TESTS(); + + return result; +} diff --git a/test/single_joint_generator_test.test b/test/single_joint_generator_test.test new file mode 100644 index 00000000..c975cf92 --- /dev/null +++ b/test/single_joint_generator_test.test @@ -0,0 +1,13 @@ + + + + + From 043e5fd5a142d8b05feb4a8f2c821e9957652d0b Mon Sep 17 00:00:00 2001 From: John Morris Date: Fri, 13 Nov 2020 11:44:54 -0600 Subject: [PATCH 3/5] Add `plot` utility for debugging test output --- scripts/plot | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) create mode 100755 scripts/plot diff --git a/scripts/plot b/scripts/plot new file mode 100755 index 00000000..382066df --- /dev/null +++ b/scripts/plot @@ -0,0 +1,19 @@ +#!/usr/bin/env python3 +# +# sudo apt-get install python3-pandas +import pandas +import matplotlib.pyplot as plt +import sys + +# Read command-line args +if len(sys.argv) < 2: + sys.stderr.write(f"Usage: {sys.argv[0]} FILE.csv [YLIM_MIN YLIM_MAX]\n") + sys.exit(1) +fname = sys.argv[1] +ylim = tuple([int(i) for i in (sys.argv[2:] + [-2000, 2000])[:2]]) + +# Generate and show plot +data = pandas.read_csv(fname, sep=' ') +data.columns=['t', 'd', 'v', 'a', 'j'] +ax = data.plot(x='t', ylim=ylim) +plt.show() From 242f8770838480600d2ab19fe4619a1eea3bd9bf Mon Sep 17 00:00:00 2001 From: John Morris Date: Tue, 17 Nov 2020 13:20:14 -0600 Subject: [PATCH 4/5] Updates after review: test variable names; indentation @andyze suggested these changes in PR #68. --- test/single_joint_generator_test.cpp | 12 ++++++------ test/single_joint_generator_test.test | 12 ++---------- test/trajectory_generation_test.cpp | 12 ++++++------ 3 files changed, 14 insertions(+), 22 deletions(-) diff --git a/test/single_joint_generator_test.cpp b/test/single_joint_generator_test.cpp index 4f582a26..809d0677 100644 --- a/test/single_joint_generator_test.cpp +++ b/test/single_joint_generator_test.cpp @@ -126,8 +126,8 @@ class SingleJointGeneratorTest : public ::testing::Test // Case 1: We move at the maximum start/end velocity for half of // the trajectory duration // Needed for cases where we do a S curve - double potential_min = min_pos - max_vel_mag * elapsed_time / 2.0; - double potential_max = max_pos + max_vel_mag * elapsed_time / 2.0; + double potential_min_duration = min_pos - max_vel_mag * elapsed_time / 2.0; + double potential_max_duration = max_pos + max_vel_mag * elapsed_time / 2.0; // Case 2: We move at the velocity needed to move from start to // end for half of the trajectory duration @@ -135,11 +135,11 @@ class SingleJointGeneratorTest : public ::testing::Test double dist_vel_mag = std::fabs((goal_joint_state_.position - current_joint_state_.position) / elapsed_time); - double potential_min_2 = min_pos - dist_vel_mag * elapsed_time / 2.0; - double potential_max_2 = max_pos + dist_vel_mag * elapsed_time / 2.0; + double potential_min_duration_2 = min_pos - dist_vel_mag * elapsed_time / 2.0; + double potential_max_duration_2 = max_pos + dist_vel_mag * elapsed_time / 2.0; - min_pos = std::min(potential_min, potential_min_2); - max_pos = std::max(potential_max, potential_max_2); + min_pos = std::min(potential_min_duration, potential_min_duration_2); + max_pos = std::max(potential_max_duration, potential_max_duration_2); EXPECT_TRUE(VerifyVectorWithinBounds(min_pos, max_pos, output_trajectory_.positions)); diff --git a/test/single_joint_generator_test.test b/test/single_joint_generator_test.test index c975cf92..bafeba0e 100644 --- a/test/single_joint_generator_test.test +++ b/test/single_joint_generator_test.test @@ -1,13 +1,5 @@ - - + + diff --git a/test/trajectory_generation_test.cpp b/test/trajectory_generation_test.cpp index bc4117a9..08d8c30a 100644 --- a/test/trajectory_generation_test.cpp +++ b/test/trajectory_generation_test.cpp @@ -96,18 +96,18 @@ class TrajectoryGenerationTest : public ::testing::Test // Here, we consider two cases to estimate worst case min/max // Case 1: We move at the maximum start/end velocity for half of the trajectory duration // Needed for cases where we do a S curve - double potential_min = min_pos - max_vel_mag * elapsed_time / 2.0; - double potential_max = max_pos + max_vel_mag * elapsed_time / 2.0; + double potential_min_duration = min_pos - max_vel_mag * elapsed_time / 2.0; + double potential_max_duration = max_pos + max_vel_mag * elapsed_time / 2.0; // Case 2: We move at the velocity needed to move from start to end for half of the trajectory duration // Needed for cases with start and end velocity of 0 double dist_vel_mag = std::fabs((goal_joint_states_[i].position - current_joint_states_[i].position) / elapsed_time); - double potential_min_2 = min_pos - dist_vel_mag * elapsed_time / 2.0; - double potential_max_2 = max_pos + dist_vel_mag * elapsed_time / 2.0; + double potential_min_duration_2 = min_pos - dist_vel_mag * elapsed_time / 2.0; + double potential_max_duration_2 = max_pos + dist_vel_mag * elapsed_time / 2.0; - min_pos = std::min(potential_min, potential_min_2); - max_pos = std::max(potential_max, potential_max_2); + min_pos = std::min(potential_min_duration, potential_min_duration_2); + max_pos = std::max(potential_max_duration, potential_max_duration_2); EXPECT_TRUE(VerifyVectorWithinBounds(min_pos, max_pos, output_trajectories_[i].positions)); } From f4b038e567f4ff532c94784c62fbc81264752c08 Mon Sep 17 00:00:00 2001 From: John Morris Date: Tue, 17 Nov 2020 13:41:16 -0600 Subject: [PATCH 5/5] Clang formatting --- src/single_joint_generator.cpp | 14 ++-- test/single_joint_generator_test.cpp | 118 ++++++++++++--------------- 2 files changed, 56 insertions(+), 76 deletions(-) diff --git a/src/single_joint_generator.cpp b/src/single_joint_generator.cpp index a1c8796a..3def77b0 100644 --- a/src/single_joint_generator.cpp +++ b/src/single_joint_generator.cpp @@ -344,10 +344,9 @@ bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, doubl 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)) / configuration_.timestep; - double backward_jerk = - (backward_accel - - (waypoints_.velocities(index - 1) - waypoints_.velocities(index - 2)) / configuration_.timestep) / - 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) / configuration_.timestep; double forward_jerk = @@ -386,10 +385,9 @@ bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, doubl 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)) / configuration_.timestep; - double backward_jerk = - (backward_accel - - (waypoints_.velocities(index - 1) - waypoints_.velocities(index - 2)) / configuration_.timestep) / - 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) / configuration_.timestep; double forward_jerk = diff --git a/test/single_joint_generator_test.cpp b/test/single_joint_generator_test.cpp index 809d0677..1557cd00 100644 --- a/test/single_joint_generator_test.cpp +++ b/test/single_joint_generator_test.cpp @@ -69,7 +69,7 @@ class SingleJointGeneratorTest : public ::testing::Test Eigen::Index size() { - assert (num_waypoints_ > 0); + assert(num_waypoints_ > 0); return num_waypoints_; } @@ -85,68 +85,57 @@ class SingleJointGeneratorTest : public ::testing::Test if (max_duration_ == 0) max_duration_ = desired_duration_; - SingleJointGenerator gen(kNumWaypointsThreshold_, - kMaxNumWaypointsFullTrajectory_); - gen.reset(timestep_, max_duration_, current_joint_state_, - goal_joint_state_, joint_limits_, num_waypoints_, + SingleJointGenerator gen(kNumWaypointsThreshold_, kMaxNumWaypointsFullTrajectory_); + gen.reset(timestep_, max_duration_, current_joint_state_, goal_joint_state_, joint_limits_, num_waypoints_, position_tolerance_, use_streaming_mode_, true); int err = gen.generateTrajectory(); - std::cerr << name() << " Error: " - << trackjoint::ERROR_CODE_MAP.at(err) << std::endl; + std::cerr << name() << " Error: " << trackjoint::ERROR_CODE_MAP.at(err) << std::endl; EXPECT_EQ(ErrorCodeEnum::NO_ERROR, err); output_trajectory_ = gen.getTrajectory(); } void checkBounds() { - if (output_trajectory_.elapsed_times.size() == 0) - return; - - // Sanity check vector lengths - EXPECT_EQ(output_trajectory_.elapsed_times.size(), size()); - EXPECT_EQ(output_trajectory_.positions.size(), size()); - EXPECT_EQ(output_trajectory_.velocities.size(), size()); - EXPECT_EQ(output_trajectory_.accelerations.size(), size()); - EXPECT_EQ(output_trajectory_.jerks.size(), size()); - - double elapsed_time = output_trajectory_.elapsed_times[size() - 1]; - - // Get estimate of min/max position and velocity using start and - // end states - double min_pos = std::min(current_joint_state_.position, - goal_joint_state_.position); - double max_pos = std::max(current_joint_state_.position, - goal_joint_state_.position); - double max_vel_mag = - std::max(std::fabs(current_joint_state_.velocity), - std::fabs(goal_joint_state_.velocity)); - - // Here, we consider two cases to estimate worst case min/max - // - // Case 1: We move at the maximum start/end velocity for half of - // the trajectory duration - // Needed for cases where we do a S curve - double potential_min_duration = min_pos - max_vel_mag * elapsed_time / 2.0; - double potential_max_duration = max_pos + max_vel_mag * elapsed_time / 2.0; - - // Case 2: We move at the velocity needed to move from start to - // end for half of the trajectory duration - // Needed for cases with start and end velocity of 0 - double dist_vel_mag = - std::fabs((goal_joint_state_.position - current_joint_state_.position) - / elapsed_time); - double potential_min_duration_2 = min_pos - dist_vel_mag * elapsed_time / 2.0; - double potential_max_duration_2 = max_pos + dist_vel_mag * elapsed_time / 2.0; - - min_pos = std::min(potential_min_duration, potential_min_duration_2); - max_pos = std::max(potential_max_duration, potential_max_duration_2); - - EXPECT_TRUE(VerifyVectorWithinBounds(min_pos, max_pos, - output_trajectory_.positions)); + if (output_trajectory_.elapsed_times.size() == 0) + return; + + // Sanity check vector lengths + EXPECT_EQ(output_trajectory_.elapsed_times.size(), size()); + EXPECT_EQ(output_trajectory_.positions.size(), size()); + EXPECT_EQ(output_trajectory_.velocities.size(), size()); + EXPECT_EQ(output_trajectory_.accelerations.size(), size()); + EXPECT_EQ(output_trajectory_.jerks.size(), size()); + + double elapsed_time = output_trajectory_.elapsed_times[size() - 1]; + + // Get estimate of min/max position and velocity using start and + // end states + double min_pos = std::min(current_joint_state_.position, goal_joint_state_.position); + double max_pos = std::max(current_joint_state_.position, goal_joint_state_.position); + double max_vel_mag = std::max(std::fabs(current_joint_state_.velocity), std::fabs(goal_joint_state_.velocity)); + + // Here, we consider two cases to estimate worst case min/max + // + // Case 1: We move at the maximum start/end velocity for half of + // the trajectory duration + // Needed for cases where we do a S curve + double potential_min_duration = min_pos - max_vel_mag * elapsed_time / 2.0; + double potential_max_duration = max_pos + max_vel_mag * elapsed_time / 2.0; + + // Case 2: We move at the velocity needed to move from start to + // end for half of the trajectory duration + // Needed for cases with start and end velocity of 0 + double dist_vel_mag = std::fabs((goal_joint_state_.position - current_joint_state_.position) / elapsed_time); + double potential_min_duration_2 = min_pos - dist_vel_mag * elapsed_time / 2.0; + double potential_max_duration_2 = max_pos + dist_vel_mag * elapsed_time / 2.0; + + min_pos = std::min(potential_min_duration, potential_min_duration_2); + max_pos = std::max(potential_max_duration, potential_max_duration_2); + + EXPECT_TRUE(VerifyVectorWithinBounds(min_pos, max_pos, output_trajectory_.positions)); } - double calculatePositionAccuracy(KinematicState goal_joint_state, - JointTrajectory& trajectory) + double calculatePositionAccuracy(KinematicState goal_joint_state, JointTrajectory& trajectory) { double goal_position = goal_joint_state_.position; double final_position = trajectory.positions((size() - 1)); @@ -157,17 +146,15 @@ class SingleJointGeneratorTest : public ::testing::Test void checkPositionError() { - position_error_ = calculatePositionAccuracy( - goal_joint_state_, output_trajectory_); + position_error_ = calculatePositionAccuracy(goal_joint_state_, output_trajectory_); EXPECT_LT(position_error_, position_tolerance_); } void checkTimestep() { double timestep_tolerance = 0.1 * timestep_; - EXPECT_NEAR(output_trajectory_.elapsed_times[1] - - output_trajectory_.elapsed_times[0], - timestep_, timestep_tolerance); + EXPECT_NEAR(output_trajectory_.elapsed_times[1] - output_trajectory_.elapsed_times[0], timestep_, + timestep_tolerance); } void checkDuration() @@ -179,14 +166,11 @@ class SingleJointGeneratorTest : public ::testing::Test void verifyVelAccelJerkLimits() { - double maxVelocityMagnitude = \ - output_trajectory_.velocities.cwiseAbs().maxCoeff(); + double maxVelocityMagnitude = output_trajectory_.velocities.cwiseAbs().maxCoeff(); EXPECT_LE(maxVelocityMagnitude, joint_limits_.velocity_limit); - double maxAccelerationMagnitude = \ - output_trajectory_.accelerations.cwiseAbs().maxCoeff(); + double maxAccelerationMagnitude = output_trajectory_.accelerations.cwiseAbs().maxCoeff(); EXPECT_LE(maxAccelerationMagnitude, joint_limits_.acceleration_limit); - double maxJerkMagnitude = \ - output_trajectory_.jerks.cwiseAbs().maxCoeff(); + double maxJerkMagnitude = output_trajectory_.jerks.cwiseAbs().maxCoeff(); EXPECT_LE(maxJerkMagnitude, joint_limits_.jerk_limit); } @@ -199,10 +183,8 @@ class SingleJointGeneratorTest : public ::testing::Test output_file.open(file, std::ofstream::out); for (Eigen::Index waypoint = 0; waypoint < output_trajectory_.positions.size(); ++waypoint) { - output_file << output_trajectory_.elapsed_times(waypoint) << " " - << output_trajectory_.positions(waypoint) << " " - << output_trajectory_.velocities(waypoint) << " " - << output_trajectory_.accelerations(waypoint) << " " + output_file << output_trajectory_.elapsed_times(waypoint) << " " << output_trajectory_.positions(waypoint) << " " + << output_trajectory_.velocities(waypoint) << " " << output_trajectory_.accelerations(waypoint) << " " << output_trajectory_.jerks(waypoint) << std::endl; } output_file.clear();