From 343adacdf0cc604f4f9aec9d512e5085fcf94e70 Mon Sep 17 00:00:00 2001 From: "David V. Lu" Date: Thu, 30 Sep 2021 12:59:02 -0400 Subject: [PATCH 1/4] Add toString(trajectory) to trajectory_tools.h Signed-off-by: David V. Lu --- .../trajectory_processing/trajectory_tools.h | 2 + .../src/trajectory_tools.cpp | 51 +++++++++++++++++++ .../test/test_time_parameterization.cpp | 35 ++----------- 3 files changed, 58 insertions(+), 30 deletions(-) diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h index 9f77ca81e0..24d340df45 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h @@ -37,9 +37,11 @@ #pragma once #include +#include namespace trajectory_processing { bool isTrajectoryEmpty(const moveit_msgs::msg::RobotTrajectory& trajectory); std::size_t trajectoryWaypointCount(const moveit_msgs::msg::RobotTrajectory& trajectory); +std::string toString(const robot_trajectory::RobotTrajectory& trajectory, bool print_jerk = true); } // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/src/trajectory_tools.cpp b/moveit_core/trajectory_processing/src/trajectory_tools.cpp index 04b8f8b594..282e4cedd5 100644 --- a/moveit_core/trajectory_processing/src/trajectory_tools.cpp +++ b/moveit_core/trajectory_processing/src/trajectory_tools.cpp @@ -35,6 +35,7 @@ /* Author: Ioan Sucan */ #include +#include namespace trajectory_processing { @@ -47,4 +48,54 @@ std::size_t trajectoryWaypointCount(const moveit_msgs::msg::RobotTrajectory& tra { return std::max(trajectory.joint_trajectory.points.size(), trajectory.multi_dof_joint_trajectory.points.size()); } + +std::string toString(const robot_trajectory::RobotTrajectory& trajectory, bool print_jerk) +{ + const moveit::core::JointModelGroup* group = trajectory.getGroup(); + const std::vector& idx = group->getVariableIndexList(); + unsigned int count = trajectory.getWayPointCount(); + + if (count == 0) + { + return "Empty trajectory."; + } + + std::stringstream ss; + ss << "Trajectory has " << count << " points over " << trajectory.getWayPointDurationFromStart(count - 1) + << " seconds.\n"; + for (unsigned i = 0; i < count; i++) + { + const moveit::core::RobotState& point = trajectory.getWayPoint(i); + ss << " waypoint " << i << "\t"; + ss << "time " << trajectory.getWayPointDurationFromStart(i); + ss << " pos "; + for (int index : idx) + { + ss << point.getVariablePosition(index) << " "; + } + ss << "vel "; + for (int index : idx) + { + ss << point.getVariableVelocity(index) << " "; + } + ss << "acc "; + for (int index : idx) + { + ss << point.getVariableAcceleration(index) << " "; + } + if (print_jerk && i > 0) + { + const moveit::core::RobotState& prev = trajectory.getWayPoint(i - 1); + ss << "jrk "; + double dt = trajectory.getWayPointDurationFromStart(i) - trajectory.getWayPointDurationFromStart(i - 1); + for (int index : idx) + { + ss << (point.getVariableAcceleration(index) - prev.getVariableAcceleration(index)) / dt << " "; + } + } + ss << "\n"; + } + return ss.str(); +} + } // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/test/test_time_parameterization.cpp b/moveit_core/trajectory_processing/test/test_time_parameterization.cpp index 643a4e82d4..ebaa429cb7 100644 --- a/moveit_core/trajectory_processing/test/test_time_parameterization.cpp +++ b/moveit_core/trajectory_processing/test/test_time_parameterization.cpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include "rclcpp/rclcpp.hpp" @@ -111,32 +112,6 @@ int initStraightTrajectory(robot_trajectory::RobotTrajectory& trajectory, double return 0; } -void printTrajectory(robot_trajectory::RobotTrajectory& trajectory) -{ - const moveit::core::JointModelGroup* group = trajectory.getGroup(); - const std::vector& idx = group->getVariableIndexList(); - unsigned int count = trajectory.getWayPointCount(); - - std::cout << "trajectory length is " << trajectory.getWayPointDurationFromStart(count - 1) << " seconds." - << std::endl; - std::cout << " Trajectory Points" << std::endl; - for (unsigned i = 0; i < count; i++) - { - moveit::core::RobotStatePtr point = trajectory.getWayPointPtr(i); - printf(" waypoint %2d time %6.2f pos %6.2f vel %6.2f acc %6.2f ", i, trajectory.getWayPointDurationFromStart(i), - point->getVariablePosition(idx[0]), point->getVariableVelocity(idx[0]), - point->getVariableAcceleration(idx[0])); - if (i > 0) - { - moveit::core::RobotStatePtr prev = trajectory.getWayPointPtr(i - 1); - printf("jrk %6.2f", - (point->getVariableAcceleration(idx[0]) - prev->getVariableAcceleration(idx[0])) / - (trajectory.getWayPointDurationFromStart(i) - trajectory.getWayPointDurationFromStart(i - 1))); - } - printf("\n"); - } -} - TEST(TestTimeParameterization, TestIterativeParabolic) { trajectory_processing::IterativeParabolicTimeParameterization time_parameterization; @@ -147,7 +122,7 @@ TEST(TestTimeParameterization, TestIterativeParabolic) std::cout << "IterativeParabolicTimeParameterization took " << (std::chrono::system_clock::now() - wt).count() << std::endl; - printTrajectory(TRAJECTORY); + std::cout << trajectory_processing::toString(TRAJECTORY); ASSERT_LT(TRAJECTORY.getWayPointDurationFromStart(TRAJECTORY.getWayPointCount() - 1), 3.0); } @@ -159,7 +134,7 @@ TEST(TestTimeParameterization, TestIterativeSpline) auto wt = std::chrono::system_clock::now(); EXPECT_TRUE(time_parameterization.computeTimeStamps(TRAJECTORY)); std::cout << "IterativeSplineParameterization took " << (std::chrono::system_clock::now() - wt).count() << std::endl; - printTrajectory(TRAJECTORY); + std::cout << trajectory_processing::toString(TRAJECTORY); ASSERT_LT(TRAJECTORY.getWayPointDurationFromStart(TRAJECTORY.getWayPointCount() - 1), 5.0); } @@ -172,7 +147,7 @@ TEST(TestTimeParameterization, TestIterativeSplineAddPoints) EXPECT_TRUE(time_parameterization.computeTimeStamps(TRAJECTORY)); std::cout << "IterativeSplineParameterization with added points took " << (std::chrono::system_clock::now() - wt).count() << std::endl; - printTrajectory(TRAJECTORY); + std::cout << trajectory_processing::toString(TRAJECTORY); ASSERT_LT(TRAJECTORY.getWayPointDurationFromStart(TRAJECTORY.getWayPointCount() - 1), 5.0); } @@ -182,7 +157,7 @@ TEST(TestTimeParameterization, TestRepeatedPoint) EXPECT_EQ(initRepeatedPointTrajectory(TRAJECTORY), 0); EXPECT_TRUE(time_parameterization.computeTimeStamps(TRAJECTORY)); - printTrajectory(TRAJECTORY); + std::cout << trajectory_processing::toString(TRAJECTORY); ASSERT_LT(TRAJECTORY.getWayPointDurationFromStart(TRAJECTORY.getWayPointCount() - 1), 0.001); } From 05852006294ac15b70f3456b4e7ba7d6c019cee2 Mon Sep 17 00:00:00 2001 From: "David V. Lu" Date: Thu, 30 Sep 2021 16:34:05 -0400 Subject: [PATCH 2/4] Incorporate PR Feedback --- .../robot_trajectory/robot_trajectory.h | 12 ++++ .../robot_trajectory/src/robot_trajectory.cpp | 71 +++++++++++++++++++ .../trajectory_processing/trajectory_tools.h | 2 - .../src/trajectory_tools.cpp | 51 ------------- .../test/test_time_parameterization.cpp | 16 +++-- 5 files changed, 94 insertions(+), 58 deletions(-) diff --git a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h index e1f3b9f75c..f6ef472d86 100644 --- a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h +++ b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h @@ -273,6 +273,14 @@ class RobotTrajectory */ bool getStateAtDurationFromStart(const double request_duration, moveit::core::RobotStatePtr& output_state) const; + /** @brief Print information about the trajectory + * @param out Stream to print to + * @param variable_indexes The indexes of the variables to print. + * If empty/not specified and the group is defined, then uses the indexes for the group + * If empty and the group is not defined, uses ALL variables in robot_model + */ + void print(std::ostream& out, std::vector variable_indexes = std::vector()) const; + private: moveit::core::RobotModelConstPtr robot_model_; const moveit::core::JointModelGroup* group_; @@ -280,4 +288,8 @@ class RobotTrajectory std::deque duration_from_previous_; rclcpp::Clock clock_ros_; }; + +/** \brief Operator overload for printing trajectory to a stream */ +std::ostream& operator<<(std::ostream& out, const RobotTrajectory& trajectory); + } // namespace robot_trajectory diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp index bf312f94a8..6f8516a906 100644 --- a/moveit_core/robot_trajectory/src/robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/src/robot_trajectory.cpp @@ -504,4 +504,75 @@ bool RobotTrajectory::getStateAtDurationFromStart(const double request_duration, return true; } +void RobotTrajectory::print(std::ostream& out, std::vector variable_indexes) const +{ + size_t num_points = getWayPointCount(); + if (num_points == 0) + { + out << "Empty trajectory."; + return; + } + + std::ios::fmtflags old_settings = out.flags(); + int old_precision = out.precision(); + out << std::fixed << std::setprecision(2); + + out << "Trajectory has " << num_points << " points over " << getDuration() << " seconds\n"; + + if (variable_indexes.empty()) + { + if (group_) + { + variable_indexes = group_->getVariableIndexList(); + } + else + { + size_t variable_count = robot_model_->getVariableCount(); + variable_indexes.reserve(variable_count); + for (size_t i = 0; i < variable_count; i++) + { + variable_indexes.push_back(i); + } + } + } + + for (size_t p_i = 0; p_i < num_points; p_i++) + { + const moveit::core::RobotState& point = getWayPoint(p_i); + out << " waypoint " << std::setw(3) << p_i; + out << " time " << std::setw(5) << getWayPointDurationFromStart(p_i); + out << " pos "; + for (int index : variable_indexes) + { + out << point.getVariablePosition(index) << " "; + } + if (point.hasVelocities()) + { + out << "vel "; + for (int index : variable_indexes) + { + out << point.getVariableVelocity(index) << " "; + } + } + if (point.hasAccelerations()) + { + out << "acc "; + for (int index : variable_indexes) + { + out << point.getVariableAcceleration(index) << " "; + } + } + out << "\n"; + } + + out.flags(old_settings); + out.precision(old_precision); +} + +std::ostream& operator<<(std::ostream& out, const RobotTrajectory& trajectory) +{ + trajectory.print(out); + return out; +} + } // end of namespace robot_trajectory diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h index 24d340df45..9f77ca81e0 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h @@ -37,11 +37,9 @@ #pragma once #include -#include namespace trajectory_processing { bool isTrajectoryEmpty(const moveit_msgs::msg::RobotTrajectory& trajectory); std::size_t trajectoryWaypointCount(const moveit_msgs::msg::RobotTrajectory& trajectory); -std::string toString(const robot_trajectory::RobotTrajectory& trajectory, bool print_jerk = true); } // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/src/trajectory_tools.cpp b/moveit_core/trajectory_processing/src/trajectory_tools.cpp index 282e4cedd5..04b8f8b594 100644 --- a/moveit_core/trajectory_processing/src/trajectory_tools.cpp +++ b/moveit_core/trajectory_processing/src/trajectory_tools.cpp @@ -35,7 +35,6 @@ /* Author: Ioan Sucan */ #include -#include namespace trajectory_processing { @@ -48,54 +47,4 @@ std::size_t trajectoryWaypointCount(const moveit_msgs::msg::RobotTrajectory& tra { return std::max(trajectory.joint_trajectory.points.size(), trajectory.multi_dof_joint_trajectory.points.size()); } - -std::string toString(const robot_trajectory::RobotTrajectory& trajectory, bool print_jerk) -{ - const moveit::core::JointModelGroup* group = trajectory.getGroup(); - const std::vector& idx = group->getVariableIndexList(); - unsigned int count = trajectory.getWayPointCount(); - - if (count == 0) - { - return "Empty trajectory."; - } - - std::stringstream ss; - ss << "Trajectory has " << count << " points over " << trajectory.getWayPointDurationFromStart(count - 1) - << " seconds.\n"; - for (unsigned i = 0; i < count; i++) - { - const moveit::core::RobotState& point = trajectory.getWayPoint(i); - ss << " waypoint " << i << "\t"; - ss << "time " << trajectory.getWayPointDurationFromStart(i); - ss << " pos "; - for (int index : idx) - { - ss << point.getVariablePosition(index) << " "; - } - ss << "vel "; - for (int index : idx) - { - ss << point.getVariableVelocity(index) << " "; - } - ss << "acc "; - for (int index : idx) - { - ss << point.getVariableAcceleration(index) << " "; - } - if (print_jerk && i > 0) - { - const moveit::core::RobotState& prev = trajectory.getWayPoint(i - 1); - ss << "jrk "; - double dt = trajectory.getWayPointDurationFromStart(i) - trajectory.getWayPointDurationFromStart(i - 1); - for (int index : idx) - { - ss << (point.getVariableAcceleration(index) - prev.getVariableAcceleration(index)) / dt << " "; - } - } - ss << "\n"; - } - return ss.str(); -} - } // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/test/test_time_parameterization.cpp b/moveit_core/trajectory_processing/test/test_time_parameterization.cpp index ebaa429cb7..34d8b7a655 100644 --- a/moveit_core/trajectory_processing/test/test_time_parameterization.cpp +++ b/moveit_core/trajectory_processing/test/test_time_parameterization.cpp @@ -41,7 +41,6 @@ #include #include #include -#include #include #include "rclcpp/rclcpp.hpp" @@ -112,6 +111,13 @@ int initStraightTrajectory(robot_trajectory::RobotTrajectory& trajectory, double return 0; } +void printTrajectory(robot_trajectory::RobotTrajectory& trajectory) +{ + const moveit::core::JointModelGroup* group = trajectory.getGroup(); + const std::vector& idx = group->getVariableIndexList(); + trajectory.print(std::cout, { idx[0] }); +} + TEST(TestTimeParameterization, TestIterativeParabolic) { trajectory_processing::IterativeParabolicTimeParameterization time_parameterization; @@ -122,7 +128,7 @@ TEST(TestTimeParameterization, TestIterativeParabolic) std::cout << "IterativeParabolicTimeParameterization took " << (std::chrono::system_clock::now() - wt).count() << std::endl; - std::cout << trajectory_processing::toString(TRAJECTORY); + printTrajectory(TRAJECTORY); ASSERT_LT(TRAJECTORY.getWayPointDurationFromStart(TRAJECTORY.getWayPointCount() - 1), 3.0); } @@ -134,7 +140,7 @@ TEST(TestTimeParameterization, TestIterativeSpline) auto wt = std::chrono::system_clock::now(); EXPECT_TRUE(time_parameterization.computeTimeStamps(TRAJECTORY)); std::cout << "IterativeSplineParameterization took " << (std::chrono::system_clock::now() - wt).count() << std::endl; - std::cout << trajectory_processing::toString(TRAJECTORY); + printTrajectory(TRAJECTORY); ASSERT_LT(TRAJECTORY.getWayPointDurationFromStart(TRAJECTORY.getWayPointCount() - 1), 5.0); } @@ -147,7 +153,7 @@ TEST(TestTimeParameterization, TestIterativeSplineAddPoints) EXPECT_TRUE(time_parameterization.computeTimeStamps(TRAJECTORY)); std::cout << "IterativeSplineParameterization with added points took " << (std::chrono::system_clock::now() - wt).count() << std::endl; - std::cout << trajectory_processing::toString(TRAJECTORY); + printTrajectory(TRAJECTORY); ASSERT_LT(TRAJECTORY.getWayPointDurationFromStart(TRAJECTORY.getWayPointCount() - 1), 5.0); } @@ -157,7 +163,7 @@ TEST(TestTimeParameterization, TestRepeatedPoint) EXPECT_EQ(initRepeatedPointTrajectory(TRAJECTORY), 0); EXPECT_TRUE(time_parameterization.computeTimeStamps(TRAJECTORY)); - std::cout << trajectory_processing::toString(TRAJECTORY); + printTrajectory(TRAJECTORY); ASSERT_LT(TRAJECTORY.getWayPointDurationFromStart(TRAJECTORY.getWayPointCount() - 1), 0.001); } From a6d56a5d4de72ac14b6a1e91955bb319d548dd69 Mon Sep 17 00:00:00 2001 From: "David V. Lu" Date: Fri, 1 Oct 2021 11:17:36 -0400 Subject: [PATCH 3/4] More output formatting, plus example in comments. --- .../include/moveit/robot_trajectory/robot_trajectory.h | 7 +++++++ moveit_core/robot_trajectory/src/robot_trajectory.cpp | 9 +++++---- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h index f6ef472d86..0881c7ff04 100644 --- a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h +++ b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h @@ -278,6 +278,13 @@ class RobotTrajectory * @param variable_indexes The indexes of the variables to print. * If empty/not specified and the group is defined, then uses the indexes for the group * If empty and the group is not defined, uses ALL variables in robot_model + * + * e.g. + * Trajectory has 13 points over 2.965 seconds + * waypoint 0 time 0.000 pos 0.000 vel 0.000 acc 0.000 + * waypoint 1 time 0.067 pos 0.001 vel 0.033 acc 1.000 + * waypoint 2 time 0.665 pos 0.200 vel 0.632 acc 1.000 + * ... */ void print(std::ostream& out, std::vector variable_indexes = std::vector()) const; diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp index 6f8516a906..9fb31b376c 100644 --- a/moveit_core/robot_trajectory/src/robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/src/robot_trajectory.cpp @@ -515,7 +515,7 @@ void RobotTrajectory::print(std::ostream& out, std::vector variable_indexes std::ios::fmtflags old_settings = out.flags(); int old_precision = out.precision(); - out << std::fixed << std::setprecision(2); + out << std::fixed << std::setprecision(3); out << "Trajectory has " << num_points << " points over " << getDuration() << " seconds\n"; @@ -544,14 +544,14 @@ void RobotTrajectory::print(std::ostream& out, std::vector variable_indexes out << " pos "; for (int index : variable_indexes) { - out << point.getVariablePosition(index) << " "; + out << std::setw(6) << point.getVariablePosition(index) << " "; } if (point.hasVelocities()) { out << "vel "; for (int index : variable_indexes) { - out << point.getVariableVelocity(index) << " "; + out << std::setw(6) << point.getVariableVelocity(index) << " "; } } if (point.hasAccelerations()) @@ -559,7 +559,7 @@ void RobotTrajectory::print(std::ostream& out, std::vector variable_indexes out << "acc "; for (int index : variable_indexes) { - out << point.getVariableAcceleration(index) << " "; + out << std::setw(6) << point.getVariableAcceleration(index) << " "; } } out << "\n"; @@ -567,6 +567,7 @@ void RobotTrajectory::print(std::ostream& out, std::vector variable_indexes out.flags(old_settings); out.precision(old_precision); + out.flush(); } std::ostream& operator<<(std::ostream& out, const RobotTrajectory& trajectory) From 7cd1d7ac22316e07960b8d12cd304f2ade98f9d9 Mon Sep 17 00:00:00 2001 From: "David V. Lu!!" Date: Mon, 4 Oct 2021 10:41:50 -0400 Subject: [PATCH 4/4] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Tyler Weaver Co-authored-by: Michael Görner --- .../robot_trajectory/robot_trajectory.h | 2 +- .../robot_trajectory/src/robot_trajectory.cpp | 19 ++++++++++++------- 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h index 0881c7ff04..28d8d61e33 100644 --- a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h +++ b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h @@ -296,7 +296,7 @@ class RobotTrajectory rclcpp::Clock clock_ros_; }; -/** \brief Operator overload for printing trajectory to a stream */ +/** @brief Operator overload for printing trajectory to a stream */ std::ostream& operator<<(std::ostream& out, const RobotTrajectory& trajectory); } // namespace robot_trajectory diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp index 9fb31b376c..0a3e6eadfe 100644 --- a/moveit_core/robot_trajectory/src/robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/src/robot_trajectory.cpp @@ -527,16 +527,13 @@ void RobotTrajectory::print(std::ostream& out, std::vector variable_indexes } else { - size_t variable_count = robot_model_->getVariableCount(); - variable_indexes.reserve(variable_count); - for (size_t i = 0; i < variable_count; i++) - { - variable_indexes.push_back(i); - } + // use all variables + variable_indexes.resize(robot_model_->getVariableCount()); + std::iota(variable_indexes.begin(), variable_indexes.end(), 0); } } - for (size_t p_i = 0; p_i < num_points; p_i++) + for (size_t p_i = 0; p_i < num_points; ++p_i) { const moveit::core::RobotState& point = getWayPoint(p_i); out << " waypoint " << std::setw(3) << p_i; @@ -562,6 +559,14 @@ void RobotTrajectory::print(std::ostream& out, std::vector variable_indexes out << std::setw(6) << point.getVariableAcceleration(index) << " "; } } + if (point.hasEffort()) + { + out << "eff "; + for (int index : variable_indexes) + { + out << std::setw(6) << point.getVariableEffort(index) << " "; + } + } out << "\n"; }