From e5244fa85b24235c338086ef674bcc90a9ccb9dc Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Tue, 5 Mar 2024 09:20:27 -0600 Subject: [PATCH] Cleanup PR feedback --- .../trajopt/trajopt_utils.h | 8 ++--- .../trajopt/trajopt_waypoint_config.h | 34 ++++++++++++------- .../trajopt/src/trajopt_waypoint_config.cpp | 2 ++ 3 files changed, 27 insertions(+), 17 deletions(-) diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_utils.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_utils.h index ad7d22ac9e..a2e3864870 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_utils.h +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_utils.h @@ -44,8 +44,8 @@ createCartesianWaypointTermInfo(int index, const Eigen::Isometry3d& tcp_offset, const Eigen::VectorXd& coeffs, trajopt::TermType type, - const Eigen::VectorXd& lower_tolerance = Eigen::VectorXd::Zero(6), - const Eigen::VectorXd& upper_tolerance = Eigen::VectorXd::Zero(6)); + const Eigen::VectorXd& lower_tolerance = Eigen::VectorXd(), + const Eigen::VectorXd& upper_tolerance = Eigen::VectorXd()); trajopt::TermInfo::Ptr createDynamicCartesianWaypointTermInfo(int index, @@ -55,8 +55,8 @@ createDynamicCartesianWaypointTermInfo(int index, const Eigen::Isometry3d& tcp_offset, const Eigen::VectorXd& coeffs, trajopt::TermType type, - const Eigen::VectorXd& lower_tolerance = Eigen::VectorXd::Zero(6), - const Eigen::VectorXd& upper_tolerance = Eigen::VectorXd::Zero(6)); + const Eigen::VectorXd& lower_tolerance = Eigen::VectorXd(), + const Eigen::VectorXd& upper_tolerance = Eigen::VectorXd()); trajopt::TermInfo::Ptr createNearJointStateTermInfo(const Eigen::VectorXd& target, const std::vector& joint_names, diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_waypoint_config.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_waypoint_config.h index cbb6a4c987..ac6c0e681a 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_waypoint_config.h +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_waypoint_config.h @@ -27,14 +27,14 @@ #define TESSERACT_MOTION_PLANNERS_TRAJOPT_CONFIG_TRAJOPT_WAYPOINT_CONFIG_H #include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - #include +namespace tinyxml2 +{ + class XMLElement; + class XMLDocument; +} + namespace tesseract_planning { /** @@ -42,25 +42,29 @@ namespace tesseract_planning */ struct CartesianWaypointConfig { + // LCOV_EXCL_START + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + // LCOV_EXCL_STOP + CartesianWaypointConfig() = default; CartesianWaypointConfig(const tinyxml2::XMLElement& xml_element); /** @brief If true, a cost/constraint term will be added to the problem. Default: true*/ - bool enabled = true; + bool enabled{ true }; /** @brief If true, will override existing waypoint tolerance with described tolerance here. Default: false * This is useful if you want to have a smaller tolerance for the cost than the constraint.*/ - bool use_tolerance_override = false; + bool use_tolerance_override{ false }; /** @brief Distance below waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3 * elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle()) */ - Eigen::VectorXd lower_tolerance; + Eigen::Matrix lower_tolerance; /** @brief Distance above waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3 * elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle())*/ - Eigen::VectorXd upper_tolerance; + Eigen::Matrix upper_tolerance; /** @brief coefficients corresponsing to dx, dy, dz, rx, ry, rz*/ - Eigen::VectorXd coeff{ Eigen::VectorXd::Constant(1, 1, 5) }; + Eigen::Matrix coeff{ Eigen::VectorXd::Constant(6, 5) }; tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const; }; @@ -70,15 +74,19 @@ struct CartesianWaypointConfig */ struct JointWaypointConfig { + // LCOV_EXCL_START + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + // LCOV_EXCL_STOP + JointWaypointConfig() = default; JointWaypointConfig(const tinyxml2::XMLElement& xml_element); /** @brief If true, a cost/constraint term will be added to the problem. Default: true*/ - bool enabled = true; + bool enabled{ true }; /** @brief If true, will override existing waypoint tolerance with described tolerance here. Default: false * This is useful if you want to have a smaller tolerance for the cost than the constraint.*/ - bool use_tolerance_override = false; + bool use_tolerance_override{ false }; /** @brief Distance below waypoint that is allowed. Should be size of joints in a joint state*/ Eigen::VectorXd lower_tolerance; diff --git a/tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp b/tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp index 06fa0d615e..0c02bd3d86 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp @@ -27,7 +27,9 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include