From 9b6ee5d8d3491555bc18fe8b4b465ad78ab1af84 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Fri, 3 Nov 2023 18:17:36 -0500 Subject: [PATCH 01/13] Add toleranced waypoints to TrajOpt Solver --- .../src/basic_cartesian_example.cpp | 8 +- .../src/glass_upright_example.cpp | 10 +- .../src/pick_and_place_example.cpp | 4 +- .../puzzle_piece_auxillary_axes_example.cpp | 10 +- .../src/puzzle_piece_example.cpp | 6 +- .../trajopt/CMakeLists.txt | 1 + .../profile/trajopt_default_plan_profile.h | 8 +- .../trajopt/trajopt_utils.h | 23 +- .../trajopt/trajopt_waypoint_config.h | 95 +++++++ .../profile/trajopt_default_plan_profile.cpp | 235 ++++++++++------ .../trajopt/src/trajopt_utils.cpp | 14 +- .../trajopt/src/trajopt_waypoint_config.cpp | 261 ++++++++++++++++++ 12 files changed, 556 insertions(+), 119 deletions(-) create mode 100644 tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_waypoint_config.h create mode 100644 tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp diff --git a/tesseract_examples/src/basic_cartesian_example.cpp b/tesseract_examples/src/basic_cartesian_example.cpp index 713ce6fd9ad..e2e8700f45d 100644 --- a/tesseract_examples/src/basic_cartesian_example.cpp +++ b/tesseract_examples/src/basic_cartesian_example.cpp @@ -233,8 +233,12 @@ bool BasicCartesianExample::run() profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "cartesian_program", composite_profile); auto plan_profile = std::make_shared(); - plan_profile->cartesian_coeff = Eigen::VectorXd::Ones(6); - plan_profile->joint_coeff = Eigen::VectorXd::Ones(7); + plan_profile->cartesian_cost_config.enabled = false; + plan_profile->cartesian_constraint_config.enabled = true; + plan_profile->cartesian_constraint_config.coeff = Eigen::VectorXd::Ones(6); + plan_profile->joint_cost_config.enabled = false; + plan_profile->joint_constraint_config.enabled = true; + plan_profile->joint_constraint_config.coeff = Eigen::VectorXd::Ones(7); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "RASTER", plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "freespace_profile", plan_profile); } diff --git a/tesseract_examples/src/glass_upright_example.cpp b/tesseract_examples/src/glass_upright_example.cpp index 213e80cfdd8..fd8a5dab073 100644 --- a/tesseract_examples/src/glass_upright_example.cpp +++ b/tesseract_examples/src/glass_upright_example.cpp @@ -223,10 +223,12 @@ bool GlassUprightExample::run() profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "UPRIGHT", composite_profile); auto plan_profile = std::make_shared(); - plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 5); - plan_profile->cartesian_coeff(0) = 0; - plan_profile->cartesian_coeff(1) = 0; - plan_profile->cartesian_coeff(2) = 0; + plan_profile->cartesian_cost_config.enabled = false; + plan_profile->cartesian_constraint_config.enabled = true; + plan_profile->cartesian_constraint_config.coeff = Eigen::VectorXd::Constant(6, 1, 5); + plan_profile->cartesian_constraint_config.coeff(0) = 0; + plan_profile->cartesian_constraint_config.coeff(1) = 0; + plan_profile->cartesian_constraint_config.coeff(2) = 0; // Add profile to Dictionary profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "UPRIGHT", plan_profile); diff --git a/tesseract_examples/src/pick_and_place_example.cpp b/tesseract_examples/src/pick_and_place_example.cpp index b052b86a25d..d0eae598498 100644 --- a/tesseract_examples/src/pick_and_place_example.cpp +++ b/tesseract_examples/src/pick_and_place_example.cpp @@ -250,7 +250,9 @@ bool PickAndPlaceExample::run() { // Create TrajOpt Profile auto trajopt_plan_profile = std::make_shared(); - trajopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 10); + trajopt_plan_profile->cartesian_cost_config.enabled = false; + trajopt_plan_profile->cartesian_constraint_config.enabled = true; + trajopt_plan_profile->cartesian_constraint_config.coeff = Eigen::VectorXd::Constant(6, 1, 10); auto trajopt_composite_profile = std::make_shared(); trajopt_composite_profile->longest_valid_segment_length = 0.05; diff --git a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp index ccb49fd73e8..bb9f2219b75 100644 --- a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp +++ b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp @@ -248,10 +248,12 @@ bool PuzzlePieceAuxillaryAxesExample::run() { // Create TrajOpt Profile auto trajopt_plan_profile = std::make_shared(); - trajopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 5); - trajopt_plan_profile->cartesian_coeff(3) = 2; - trajopt_plan_profile->cartesian_coeff(4) = 2; - trajopt_plan_profile->cartesian_coeff(5) = 0; + trajopt_plan_profile->cartesian_cost_config.enabled = false; + trajopt_plan_profile->cartesian_constraint_config.enabled = true; + trajopt_plan_profile->cartesian_constraint_config.coeff = Eigen::VectorXd::Constant(6, 1, 5); + trajopt_plan_profile->cartesian_constraint_config.coeff(3) = 2; + trajopt_plan_profile->cartesian_constraint_config.coeff(4) = 2; + trajopt_plan_profile->cartesian_constraint_config.coeff(5) = 0; auto trajopt_composite_profile = std::make_shared(); trajopt_composite_profile->collision_constraint_config.enabled = false; diff --git a/tesseract_examples/src/puzzle_piece_example.cpp b/tesseract_examples/src/puzzle_piece_example.cpp index b1020c0ef83..95c15fdcf2a 100644 --- a/tesseract_examples/src/puzzle_piece_example.cpp +++ b/tesseract_examples/src/puzzle_piece_example.cpp @@ -240,8 +240,10 @@ bool PuzzlePieceExample::run() { // Create TrajOpt Profile auto trajopt_plan_profile = std::make_shared(); - trajopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 10); - trajopt_plan_profile->cartesian_coeff(5) = 0; + trajopt_plan_profile->cartesian_cost_config.enabled = false; + trajopt_plan_profile->cartesian_constraint_config.enabled = true; + trajopt_plan_profile->cartesian_constraint_config.coeff = Eigen::VectorXd::Constant(6, 1, 10); + trajopt_plan_profile->cartesian_constraint_config.coeff(5) = 0; auto trajopt_composite_profile = std::make_shared(); trajopt_composite_profile->collision_constraint_config.enabled = false; diff --git a/tesseract_motion_planners/trajopt/CMakeLists.txt b/tesseract_motion_planners/trajopt/CMakeLists.txt index 6b7fb51bb16..5ac51385590 100644 --- a/tesseract_motion_planners/trajopt/CMakeLists.txt +++ b/tesseract_motion_planners/trajopt/CMakeLists.txt @@ -5,6 +5,7 @@ find_package(trajopt_sco REQUIRED) add_library( ${PROJECT_NAME}_trajopt src/trajopt_collision_config.cpp + src/trajopt_waypoint_config.cpp src/trajopt_motion_planner.cpp src/trajopt_utils.cpp src/profile/trajopt_default_plan_profile.cpp diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h index abe1e2bfeb1..83bcdf96710 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h @@ -33,6 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include namespace tesseract_planning @@ -51,9 +52,10 @@ class TrajOptDefaultPlanProfile : public TrajOptPlanProfile TrajOptDefaultPlanProfile(TrajOptDefaultPlanProfile&&) = default; TrajOptDefaultPlanProfile& operator=(TrajOptDefaultPlanProfile&&) = default; - Eigen::VectorXd cartesian_coeff{ Eigen::VectorXd::Constant(1, 1, 5) }; - Eigen::VectorXd joint_coeff{ Eigen::VectorXd::Constant(1, 1, 5) }; - trajopt::TermType term_type{ trajopt::TermType::TT_CNT }; + CartesianWaypointConfig cartesian_cost_config; + CartesianWaypointConfig cartesian_constraint_config; + JointWaypointConfig joint_cost_config; + JointWaypointConfig joint_constraint_config; /** @brief Error function that is set as a constraint for each timestep. * 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 875680e2aeb..7676698db0c 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 @@ -42,15 +42,20 @@ trajopt::TermInfo::Ptr createCartesianWaypointTermInfo(int index, const std::string& tcp_frame, const Eigen::Isometry3d& tcp_offset, const Eigen::VectorXd& coeffs, - trajopt::TermType type); - -trajopt::TermInfo::Ptr createDynamicCartesianWaypointTermInfo(int index, - const std::string& working_frame, - const Eigen::Isometry3d& c_wp, - const std::string& tcp_frame, - const Eigen::Isometry3d& tcp_offset, - const Eigen::VectorXd& coeffs, - trajopt::TermType type); + trajopt::TermType type, + Eigen::VectorXd lower_tolerance = Eigen::VectorXd::Zero(6), + Eigen::VectorXd upper_tolerance = Eigen::VectorXd::Zero(6)); + +trajopt::TermInfo::Ptr +createDynamicCartesianWaypointTermInfo(int index, + const std::string& working_frame, + const Eigen::Isometry3d& c_wp, + const std::string& tcp_frame, + const Eigen::Isometry3d& tcp_offset, + const Eigen::VectorXd& coeffs, + trajopt::TermType type, + Eigen::VectorXd lower_tolerance = Eigen::VectorXd::Zero(6), + Eigen::VectorXd upper_tolerance = Eigen::VectorXd::Zero(6)); 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 new file mode 100644 index 00000000000..cbb6a4c987d --- /dev/null +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_waypoint_config.h @@ -0,0 +1,95 @@ +/** + * @file trajopt_waypoint_config.h + * @brief TrajOpt waypoint configuration settings + * + * @author Tyler Marr + * @date November 2, 2023 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2023, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_MOTION_PLANNERS_TRAJOPT_CONFIG_TRAJOPT_WAYPOINT_CONFIG_H +#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 tesseract_planning +{ +/** + * @brief Config settings for cartesian waypoints + */ +struct CartesianWaypointConfig +{ + 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; + + /** @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; + + /** @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; + /** @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; + + /** @brief coefficients corresponsing to dx, dy, dz, rx, ry, rz*/ + Eigen::VectorXd coeff{ Eigen::VectorXd::Constant(1, 1, 5) }; + + tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const; +}; + +/** + * @brief Config settings for joint waypoints. + */ +struct JointWaypointConfig +{ + 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; + + /** @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; + + /** @brief Distance below waypoint that is allowed. Should be size of joints in a joint state*/ + Eigen::VectorXd lower_tolerance; + /** @brief Distance above waypoint that is allowed. Should be size of joints in a joint state*/ + Eigen::VectorXd upper_tolerance; + + /** @brief coefficients corresponsing to joint values*/ + Eigen::VectorXd coeff{ Eigen::VectorXd::Constant(1, 1, 5) }; + + tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const; +}; +} // namespace tesseract_planning + +#endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_CONFIG_TRAJOPT_WAYPOINT_CONFIG_H diff --git a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp index 363805cce1f..6acb89fb25a 100644 --- a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp +++ b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp @@ -38,57 +38,32 @@ namespace tesseract_planning { TrajOptDefaultPlanProfile::TrajOptDefaultPlanProfile(const tinyxml2::XMLElement& xml_element) { - const tinyxml2::XMLElement* cartesian_coeff_element = xml_element.FirstChildElement("CartesianCoefficients"); - const tinyxml2::XMLElement* joint_coeff_element = xml_element.FirstChildElement("JointCoefficients"); - const tinyxml2::XMLElement* term_type_element = xml_element.FirstChildElement("Term"); + const tinyxml2::XMLElement* cartesian_cost_element = xml_element.FirstChildElement("CartesianCostConfig"); + const tinyxml2::XMLElement* cartesian_constraint_element = xml_element.FirstChildElement("CartesianConstraintConfig"); + const tinyxml2::XMLElement* joint_cost_element = xml_element.FirstChildElement("JointCostConfig"); + const tinyxml2::XMLElement* joint_constraint_element = xml_element.FirstChildElement("JointConstraintConfig"); const tinyxml2::XMLElement* cnt_error_fn_element = xml_element.FirstChildElement("ConstraintErrorFunctions"); tinyxml2::XMLError status{ tinyxml2::XMLError::XML_SUCCESS }; - if (cartesian_coeff_element != nullptr) + if (cartesian_cost_element != nullptr) { - std::vector cart_coeff_tokens; - std::string cart_coeff_string; - status = tesseract_common::QueryStringText(cartesian_coeff_element, cart_coeff_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("TrajOptPlanProfile: Error parsing CartesianCoeff string"); - - boost::split(cart_coeff_tokens, cart_coeff_string, boost::is_any_of(" "), boost::token_compress_on); - - if (!tesseract_common::isNumeric(cart_coeff_tokens)) - throw std::runtime_error("TrajOptPlanProfile: CartesianCoeff are not all numeric values."); - - cartesian_coeff.resize(static_cast(cart_coeff_tokens.size())); - for (std::size_t i = 0; i < cart_coeff_tokens.size(); ++i) - tesseract_common::toNumeric(cart_coeff_tokens[i], cartesian_coeff[static_cast(i)]); + cartesian_cost_config = CartesianWaypointConfig(*cartesian_cost_element); } - if (joint_coeff_element != nullptr) + if (cartesian_constraint_element != nullptr) { - std::vector joint_coeff_tokens; - std::string joint_coeff_string; - status = tesseract_common::QueryStringText(joint_coeff_element, joint_coeff_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("TrajOptPlanProfile: Error parsing JointCoeff string"); - - boost::split(joint_coeff_tokens, joint_coeff_string, boost::is_any_of(" "), boost::token_compress_on); - - if (!tesseract_common::isNumeric(joint_coeff_tokens)) - throw std::runtime_error("TrajOptPlanProfile: JointCoeff are not all numeric values."); - - joint_coeff.resize(static_cast(joint_coeff_tokens.size())); - for (std::size_t i = 0; i < joint_coeff_tokens.size(); ++i) - tesseract_common::toNumeric(joint_coeff_tokens[i], joint_coeff[static_cast(i)]); + cartesian_constraint_config = CartesianWaypointConfig(*cartesian_constraint_element); } - if (term_type_element != nullptr) + if (joint_cost_element != nullptr) { - auto type = static_cast(trajopt::TermType::TT_CNT); - status = term_type_element->QueryIntAttribute("type", &type); - if (status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("TrajOptPlanProfile: Error parsing Term type attribute."); + joint_cost_config = JointWaypointConfig(*joint_cost_element); + } - term_type = static_cast(type); + if (joint_constraint_element != nullptr) + { + joint_constraint_config = JointWaypointConfig(*joint_constraint_element); } if (cnt_error_fn_element != nullptr) @@ -122,8 +97,6 @@ void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci, Eigen::Isometry3d tcp_offset = pci.env->findTCPOffset(mi); - trajopt::TermInfo::Ptr ti{ nullptr }; - /* Check if this cartesian waypoint is dynamic * (i.e. defined relative to a frame that will move with the kinematic chain) */ @@ -131,39 +104,85 @@ void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci, bool is_static_working_frame = (std::find(active_links.begin(), active_links.end(), mi.working_frame) == active_links.end()); - if (cartesian_waypoint.isToleranced()) - CONSOLE_BRIDGE_logWarn("Tolerance cartesian waypoints are not supported in this version of TrajOpt."); + // Override cost tolerances if the profile specifies that they should be overrided. + Eigen::VectorXd lower_tolerance_cost = cartesian_waypoint.getLowerTolerance(); + Eigen::VectorXd upper_tolerance_cost = cartesian_waypoint.getUpperTolerance(); + if (cartesian_cost_config.use_tolerance_override) + { + lower_tolerance_cost = cartesian_cost_config.lower_tolerance; + upper_tolerance_cost = cartesian_cost_config.upper_tolerance; + } + Eigen::VectorXd lower_tolerance_cnt = cartesian_waypoint.getLowerTolerance(); + Eigen::VectorXd upper_tolerance_cnt = cartesian_waypoint.getUpperTolerance(); + if (cartesian_constraint_config.use_tolerance_override) + { + lower_tolerance_cnt = cartesian_constraint_config.lower_tolerance; + upper_tolerance_cnt = cartesian_constraint_config.upper_tolerance; + } if ((is_static_working_frame && is_active_tcp_frame) || (!is_active_tcp_frame && !is_static_working_frame)) { - ti = createCartesianWaypointTermInfo(index, - mi.working_frame, - cartesian_waypoint.getTransform(), - mi.tcp_frame, - tcp_offset, - cartesian_coeff, - term_type); + if (cartesian_cost_config.enabled) + { + trajopt::TermInfo::Ptr ti = createCartesianWaypointTermInfo(index, + mi.working_frame, + cartesian_waypoint.getTransform(), + mi.tcp_frame, + tcp_offset, + cartesian_cost_config.coeff, + trajopt::TermType::TT_COST, + lower_tolerance_cost, + upper_tolerance_cost); + pci.cost_infos.push_back(ti); + } + if (cartesian_constraint_config.enabled) + { + trajopt::TermInfo::Ptr ti = createCartesianWaypointTermInfo(index, + mi.working_frame, + cartesian_waypoint.getTransform(), + mi.tcp_frame, + tcp_offset, + cartesian_constraint_config.coeff, + trajopt::TermType::TT_CNT, + lower_tolerance_cnt, + upper_tolerance_cnt); + pci.cnt_infos.push_back(ti); + } } else if (!is_static_working_frame && is_active_tcp_frame) { - ti = createDynamicCartesianWaypointTermInfo(index, - mi.working_frame, - cartesian_waypoint.getTransform(), - mi.tcp_frame, - tcp_offset, - cartesian_coeff, - term_type); + if (cartesian_cost_config.enabled) + { + trajopt::TermInfo::Ptr ti = createDynamicCartesianWaypointTermInfo(index, + mi.working_frame, + cartesian_waypoint.getTransform(), + mi.tcp_frame, + tcp_offset, + cartesian_cost_config.coeff, + trajopt::TermType::TT_COST, + lower_tolerance_cost, + upper_tolerance_cost); + pci.cost_infos.push_back(ti); + } + if (cartesian_constraint_config.enabled) + { + trajopt::TermInfo::Ptr ti = createDynamicCartesianWaypointTermInfo(index, + mi.working_frame, + cartesian_waypoint.getTransform(), + mi.tcp_frame, + tcp_offset, + cartesian_constraint_config.coeff, + trajopt::TermType::TT_CNT, + lower_tolerance_cnt, + upper_tolerance_cnt); + pci.cnt_infos.push_back(ti); + } } else { throw std::runtime_error("TrajOpt, both tcp_frame and working_frame are both static!"); } - if (term_type == trajopt::TermType::TT_CNT) - pci.cnt_infos.push_back(ti); - else - pci.cost_infos.push_back(ti); - // Add constraints from error functions if available. addConstraintErrorFunctions(pci, index); } @@ -175,21 +194,59 @@ void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci, const std::vector& /*active_links*/, int index) const { - trajopt::TermInfo::Ptr ti; + // Override cost tolerances if the profile specifies that they should be overrided. + Eigen::VectorXd lower_tolerance_cost = joint_waypoint.getLowerTolerance(); + Eigen::VectorXd upper_tolerance_cost = joint_waypoint.getUpperTolerance(); + if (joint_cost_config.use_tolerance_override) + { + lower_tolerance_cost = joint_cost_config.lower_tolerance; + upper_tolerance_cost = joint_cost_config.upper_tolerance; + } + Eigen::VectorXd lower_tolerance_cnt = joint_waypoint.getLowerTolerance(); + Eigen::VectorXd upper_tolerance_cnt = joint_waypoint.getUpperTolerance(); + if (joint_constraint_config.use_tolerance_override) + { + lower_tolerance_cnt = joint_constraint_config.lower_tolerance; + upper_tolerance_cnt = joint_constraint_config.upper_tolerance; + } if (joint_waypoint.isToleranced()) - ti = createTolerancedJointWaypointTermInfo(joint_waypoint.getPosition(), - joint_waypoint.getLowerTolerance(), - joint_waypoint.getUpperTolerance(), - index, - joint_coeff, - term_type); - else - ti = createJointWaypointTermInfo(joint_waypoint.getPosition(), index, joint_coeff, term_type); - - if (term_type == trajopt::TermType::TT_CNT) - pci.cnt_infos.push_back(ti); + { + if (joint_cost_config.enabled) + { + trajopt::TermInfo::Ptr ti = createTolerancedJointWaypointTermInfo(joint_waypoint.getPosition(), + lower_tolerance_cost, + upper_tolerance_cost, + index, + joint_cost_config.coeff, + trajopt::TermType::TT_COST); + pci.cost_infos.push_back(ti); + } + if (joint_constraint_config.enabled) + { + trajopt::TermInfo::Ptr ti = createTolerancedJointWaypointTermInfo(joint_waypoint.getPosition(), + lower_tolerance_cnt, + upper_tolerance_cnt, + index, + joint_constraint_config.coeff, + trajopt::TermType::TT_CNT); + pci.cnt_infos.push_back(ti); + } + } else - pci.cost_infos.push_back(ti); + { + if (joint_cost_config.enabled) + { + trajopt::TermInfo::Ptr ti = createJointWaypointTermInfo( + joint_waypoint.getPosition(), index, joint_cost_config.coeff, trajopt::TermType::TT_COST); + pci.cost_infos.push_back(ti); + } + if (joint_constraint_config.enabled) + { + trajopt::TermInfo::Ptr ti = createJointWaypointTermInfo( + joint_waypoint.getPosition(), index, joint_constraint_config.coeff, trajopt::TermType::TT_CNT); + pci.cnt_infos.push_back(ti); + } + } // Add constraints from error functions if available. addConstraintErrorFunctions(pci, index); @@ -213,28 +270,22 @@ void TrajOptDefaultPlanProfile::addConstraintErrorFunctions(trajopt::ProblemCons tinyxml2::XMLElement* TrajOptDefaultPlanProfile::toXML(tinyxml2::XMLDocument& doc) const { - Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " "); - tinyxml2::XMLElement* xml_planner = doc.NewElement("Planner"); xml_planner->SetAttribute("type", std::to_string(1).c_str()); tinyxml2::XMLElement* xml_trajopt = doc.NewElement("TrajOptDefaultPlanProfile"); - tinyxml2::XMLElement* xml_cart_coeff = doc.NewElement("CartesianCoefficients"); - std::stringstream cart_coeff; - cart_coeff << cartesian_coeff.format(eigen_format); - xml_cart_coeff->SetText(cart_coeff.str().c_str()); - xml_trajopt->InsertEndChild(xml_cart_coeff); - - tinyxml2::XMLElement* xml_joint_coeff = doc.NewElement("JointCoefficients"); - std::stringstream jnt_coeff; - jnt_coeff << joint_coeff.format(eigen_format); - xml_joint_coeff->SetText(jnt_coeff.str().c_str()); - xml_trajopt->InsertEndChild(xml_joint_coeff); - - tinyxml2::XMLElement* xml_term_type = doc.NewElement("Term"); - xml_term_type->SetAttribute("type", static_cast(term_type)); - xml_trajopt->InsertEndChild(xml_term_type); + tinyxml2::XMLElement* xml_cart_cost = cartesian_cost_config.toXML(doc); + xml_trajopt->InsertEndChild(xml_cart_cost); + + tinyxml2::XMLElement* xml_cart_cnt = cartesian_constraint_config.toXML(doc); + xml_trajopt->InsertEndChild(xml_cart_cnt); + + tinyxml2::XMLElement* xml_joint_cost = joint_cost_config.toXML(doc); + xml_trajopt->InsertEndChild(xml_joint_cost); + + tinyxml2::XMLElement* xml_joint_cnt = joint_constraint_config.toXML(doc); + xml_trajopt->InsertEndChild(xml_joint_cnt); xml_planner->InsertEndChild(xml_trajopt); diff --git a/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp b/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp index 0c218964096..ac14b55c40f 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp @@ -33,7 +33,9 @@ trajopt::TermInfo::Ptr createCartesianWaypointTermInfo(int index, const std::string& tcp_frame, const Eigen::Isometry3d& tcp_offset, const Eigen::VectorXd& coeffs, - trajopt::TermType type) + trajopt::TermType type, + Eigen::VectorXd lower_tolerance, + Eigen::VectorXd upper_tolerance) { auto pose_info = std::make_shared(); pose_info->term_type = type; @@ -57,6 +59,9 @@ trajopt::TermInfo::Ptr createCartesianWaypointTermInfo(int index, pose_info->rot_coeffs = coeffs.tail<3>(); } + pose_info->lower_tolerance = lower_tolerance; + pose_info->upper_tolerance = upper_tolerance; + return pose_info; } @@ -66,7 +71,9 @@ trajopt::TermInfo::Ptr createDynamicCartesianWaypointTermInfo(int index, const std::string& tcp_frame, const Eigen::Isometry3d& tcp_offset, const Eigen::VectorXd& coeffs, - trajopt::TermType type) + trajopt::TermType type, + Eigen::VectorXd lower_tolerance, + Eigen::VectorXd upper_tolerance) { std::shared_ptr pose = std::make_shared(); pose->term_type = type; @@ -90,6 +97,9 @@ trajopt::TermInfo::Ptr createDynamicCartesianWaypointTermInfo(int index, pose->rot_coeffs = coeffs.tail<3>(); } + pose->lower_tolerance = lower_tolerance; + pose->upper_tolerance = upper_tolerance; + return pose; } diff --git a/tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp b/tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp new file mode 100644 index 00000000000..2c5cfd376b1 --- /dev/null +++ b/tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp @@ -0,0 +1,261 @@ +/** + * @file trajopt_collision_config.cpp + * @brief TrajOpt collision configuration settings + * + * @author Tyler Marr + * @date November 2, 2023 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include + +namespace tesseract_planning +{ +CartesianWaypointConfig::CartesianWaypointConfig(const tinyxml2::XMLElement& xml_element) +{ + const tinyxml2::XMLElement* enabled_element = xml_element.FirstChildElement("Enabled"); + const tinyxml2::XMLElement* use_tolerance_override_element = xml_element.FirstChildElement("UseToleranceOverride"); + const tinyxml2::XMLElement* lower_tolerance_element = xml_element.FirstChildElement("LowerTolerance"); + const tinyxml2::XMLElement* upper_tolerance_element = xml_element.FirstChildElement("UpperTolerance"); + const tinyxml2::XMLElement* coefficients_element = xml_element.FirstChildElement("Coefficients"); + + if (enabled_element == nullptr) + throw std::runtime_error("CartesianWaypointConfig: Must have Enabled element."); + + tinyxml2::XMLError status = enabled_element->QueryBoolText(&enabled); + if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) + throw std::runtime_error("CartesianWaypointConfig: Error parsing Enabled string"); + + if (use_tolerance_override_element != nullptr) + { + status = use_tolerance_override_element->QueryBoolText(&use_tolerance_override); + if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) + throw std::runtime_error("CartesianWaypointConfig: Error parsing UseToleranceOverride string"); + } + + if (lower_tolerance_element != nullptr) + { + std::vector lower_tolerance_tokens; + std::string lower_tolerance_string; + status = tesseract_common::QueryStringText(lower_tolerance_element, lower_tolerance_string); + if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) + throw std::runtime_error("CartesianWaypointConfig: Error parsing LowerTolerance string"); + + boost::split(lower_tolerance_tokens, lower_tolerance_string, boost::is_any_of(" "), boost::token_compress_on); + + if (!tesseract_common::isNumeric(lower_tolerance_tokens)) + throw std::runtime_error("CartesianWaypointConfig: LowerTolerance are not all numeric values."); + + lower_tolerance.resize(static_cast(lower_tolerance_tokens.size())); + for (std::size_t i = 0; i < lower_tolerance_tokens.size(); ++i) + tesseract_common::toNumeric(lower_tolerance_tokens[i], lower_tolerance[static_cast(i)]); + } + + if (upper_tolerance_element != nullptr) + { + std::vector upper_tolerance_tokens; + std::string upper_tolerance_string; + status = tesseract_common::QueryStringText(upper_tolerance_element, upper_tolerance_string); + if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) + throw std::runtime_error("CartesianWaypointConfig: Error parsing UpperTolerance string"); + + boost::split(upper_tolerance_tokens, upper_tolerance_string, boost::is_any_of(" "), boost::token_compress_on); + + if (!tesseract_common::isNumeric(upper_tolerance_tokens)) + throw std::runtime_error("CartesianWaypointConfig: UpperTolerance are not all numeric values."); + + upper_tolerance.resize(static_cast(upper_tolerance_tokens.size())); + for (std::size_t i = 0; i < upper_tolerance_tokens.size(); ++i) + tesseract_common::toNumeric(upper_tolerance_tokens[i], upper_tolerance[static_cast(i)]); + } + + if (coefficients_element != nullptr) + { + std::vector coefficients_tokens; + std::string coefficients_string; + status = tesseract_common::QueryStringText(coefficients_element, coefficients_string); + if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) + throw std::runtime_error("CartesianWaypointConfig: Error parsing Coefficients string"); + + boost::split(coefficients_tokens, coefficients_string, boost::is_any_of(" "), boost::token_compress_on); + + if (!tesseract_common::isNumeric(coefficients_tokens)) + throw std::runtime_error("CartesianWaypointConfig: Coefficients are not all numeric values."); + + coeff.resize(static_cast(coefficients_tokens.size())); + for (std::size_t i = 0; i < coefficients_tokens.size(); ++i) + tesseract_common::toNumeric(coefficients_tokens[i], coeff[static_cast(i)]); + } +} + +tinyxml2::XMLElement* CartesianWaypointConfig::toXML(tinyxml2::XMLDocument& doc) const +{ + Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " "); + + tinyxml2::XMLElement* xml_cartesian_waypoint_config = doc.NewElement("CartesianWaypointConfig"); + + tinyxml2::XMLElement* xml_enabled = doc.NewElement("Enabled"); + xml_enabled->SetText(enabled); + xml_cartesian_waypoint_config->InsertEndChild(xml_enabled); + + tinyxml2::XMLElement* xml_use_tolerance_override = doc.NewElement("UseToleranceOverride"); + xml_use_tolerance_override->SetText(use_tolerance_override); + xml_cartesian_waypoint_config->InsertEndChild(xml_use_tolerance_override); + + tinyxml2::XMLElement* xml_lower_tolerance = doc.NewElement("LowerTolerance"); + std::stringstream lower_tolerance_ss; + lower_tolerance_ss << lower_tolerance.format(eigen_format); + xml_lower_tolerance->SetText(lower_tolerance_ss.str().c_str()); + xml_cartesian_waypoint_config->InsertEndChild(xml_lower_tolerance); + + tinyxml2::XMLElement* xml_upper_tolerance = doc.NewElement("UpperTolerance"); + std::stringstream upper_tolerance_ss; + upper_tolerance_ss << upper_tolerance.format(eigen_format); + xml_upper_tolerance->SetText(upper_tolerance_ss.str().c_str()); + xml_cartesian_waypoint_config->InsertEndChild(xml_upper_tolerance); + + tinyxml2::XMLElement* xml_coeff = doc.NewElement("Coefficients"); + std::stringstream coeff_ss; + coeff_ss << coeff.format(eigen_format); + xml_coeff->SetText(coeff_ss.str().c_str()); + xml_cartesian_waypoint_config->InsertEndChild(xml_coeff); + + return xml_cartesian_waypoint_config; +} + +JointWaypointConfig::JointWaypointConfig(const tinyxml2::XMLElement& xml_element) +{ + const tinyxml2::XMLElement* enabled_element = xml_element.FirstChildElement("Enabled"); + const tinyxml2::XMLElement* use_tolerance_override_element = xml_element.FirstChildElement("UseToleranceOverride"); + const tinyxml2::XMLElement* lower_tolerance_element = xml_element.FirstChildElement("LowerTolerance"); + const tinyxml2::XMLElement* upper_tolerance_element = xml_element.FirstChildElement("UpperTolerance"); + const tinyxml2::XMLElement* coefficients_element = xml_element.FirstChildElement("Coefficients"); + + if (enabled_element == nullptr) + throw std::runtime_error("JointWaypointConfig: Must have Enabled element."); + + tinyxml2::XMLError status = enabled_element->QueryBoolText(&enabled); + if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) + throw std::runtime_error("JointWaypointConfig: Error parsing Enabled string"); + + if (use_tolerance_override_element != nullptr) + { + status = use_tolerance_override_element->QueryBoolText(&use_tolerance_override); + if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) + throw std::runtime_error("JointWaypointConfig: Error parsing UseToleranceOverride string"); + } + + if (lower_tolerance_element != nullptr) + { + std::vector lower_tolerance_tokens; + std::string lower_tolerance_string; + status = tesseract_common::QueryStringText(lower_tolerance_element, lower_tolerance_string); + if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) + throw std::runtime_error("JointWaypointConfig: Error parsing LowerTolerance string"); + + boost::split(lower_tolerance_tokens, lower_tolerance_string, boost::is_any_of(" "), boost::token_compress_on); + + if (!tesseract_common::isNumeric(lower_tolerance_tokens)) + throw std::runtime_error("JointWaypointConfig: LowerTolerance are not all numeric values."); + + lower_tolerance.resize(static_cast(lower_tolerance_tokens.size())); + for (std::size_t i = 0; i < lower_tolerance_tokens.size(); ++i) + tesseract_common::toNumeric(lower_tolerance_tokens[i], lower_tolerance[static_cast(i)]); + } + + if (upper_tolerance_element != nullptr) + { + std::vector upper_tolerance_tokens; + std::string upper_tolerance_string; + status = tesseract_common::QueryStringText(upper_tolerance_element, upper_tolerance_string); + if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) + throw std::runtime_error("JointWaypointConfig: Error parsing UpperTolerance string"); + + boost::split(upper_tolerance_tokens, upper_tolerance_string, boost::is_any_of(" "), boost::token_compress_on); + + if (!tesseract_common::isNumeric(upper_tolerance_tokens)) + throw std::runtime_error("JointWaypointConfig: UpperTolerance are not all numeric values."); + + upper_tolerance.resize(static_cast(upper_tolerance_tokens.size())); + for (std::size_t i = 0; i < upper_tolerance_tokens.size(); ++i) + tesseract_common::toNumeric(upper_tolerance_tokens[i], upper_tolerance[static_cast(i)]); + } + + if (coefficients_element != nullptr) + { + std::vector coefficients_tokens; + std::string coefficients_string; + status = tesseract_common::QueryStringText(coefficients_element, coefficients_string); + if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) + throw std::runtime_error("JointWaypointConfig: Error parsing Coefficients string"); + + boost::split(coefficients_tokens, coefficients_string, boost::is_any_of(" "), boost::token_compress_on); + + if (!tesseract_common::isNumeric(coefficients_tokens)) + throw std::runtime_error("JointWaypointConfig: Coefficients are not all numeric values."); + + coeff.resize(static_cast(coefficients_tokens.size())); + for (std::size_t i = 0; i < coefficients_tokens.size(); ++i) + tesseract_common::toNumeric(coefficients_tokens[i], coeff[static_cast(i)]); + } +} + +tinyxml2::XMLElement* JointWaypointConfig::toXML(tinyxml2::XMLDocument& doc) const +{ + Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " "); + + tinyxml2::XMLElement* xml_cartesian_waypoint_config = doc.NewElement("CartesianWaypointConfig"); + + tinyxml2::XMLElement* xml_enabled = doc.NewElement("Enabled"); + xml_enabled->SetText(enabled); + xml_cartesian_waypoint_config->InsertEndChild(xml_enabled); + + tinyxml2::XMLElement* xml_use_tolerance_override = doc.NewElement("UseToleranceOverride"); + xml_use_tolerance_override->SetText(use_tolerance_override); + xml_cartesian_waypoint_config->InsertEndChild(xml_use_tolerance_override); + + tinyxml2::XMLElement* xml_lower_tolerance = doc.NewElement("LowerTolerance"); + std::stringstream lower_tolerance_ss; + lower_tolerance_ss << lower_tolerance.format(eigen_format); + xml_lower_tolerance->SetText(lower_tolerance_ss.str().c_str()); + xml_cartesian_waypoint_config->InsertEndChild(xml_lower_tolerance); + + tinyxml2::XMLElement* xml_upper_tolerance = doc.NewElement("UpperTolerance"); + std::stringstream upper_tolerance_ss; + upper_tolerance_ss << upper_tolerance.format(eigen_format); + xml_upper_tolerance->SetText(upper_tolerance_ss.str().c_str()); + xml_cartesian_waypoint_config->InsertEndChild(xml_upper_tolerance); + + tinyxml2::XMLElement* xml_coeff = doc.NewElement("Coefficients"); + std::stringstream coeff_ss; + coeff_ss << coeff.format(eigen_format); + xml_coeff->SetText(coeff_ss.str().c_str()); + xml_cartesian_waypoint_config->InsertEndChild(xml_coeff); + + return xml_cartesian_waypoint_config; +} +} // namespace tesseract_planning From 3725d3886e7448b51add5d3892a0796ee6b211e0 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Mon, 26 Feb 2024 13:56:24 -0600 Subject: [PATCH 02/13] Fixed failing plan from improper xml reading/writing --- .../profile/trajopt_default_plan_profile.cpp | 28 +++++-- .../trajopt/src/trajopt_waypoint_config.cpp | 55 ++++++++------ .../trajopt/test/trajopt_planner_tests.cpp | 74 ++++++++++++++++--- 3 files changed, 118 insertions(+), 39 deletions(-) diff --git a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp index 6acb89fb25a..51ae2bbbfb2 100644 --- a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp +++ b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp @@ -48,22 +48,26 @@ TrajOptDefaultPlanProfile::TrajOptDefaultPlanProfile(const tinyxml2::XMLElement& if (cartesian_cost_element != nullptr) { - cartesian_cost_config = CartesianWaypointConfig(*cartesian_cost_element); + const tinyxml2::XMLElement* cartesian_waypoint_config = cartesian_cost_element->FirstChildElement("CartesianWaypointConfig"); + cartesian_cost_config = CartesianWaypointConfig(*cartesian_waypoint_config); } if (cartesian_constraint_element != nullptr) { - cartesian_constraint_config = CartesianWaypointConfig(*cartesian_constraint_element); + const tinyxml2::XMLElement* cartesian_waypoint_config = cartesian_constraint_element->FirstChildElement("CartesianWaypointConfig"); + cartesian_constraint_config = CartesianWaypointConfig(*cartesian_waypoint_config); } if (joint_cost_element != nullptr) { - joint_cost_config = JointWaypointConfig(*joint_cost_element); + const tinyxml2::XMLElement* cartesian_waypoint_config = joint_cost_element->FirstChildElement("CartesianWaypointConfig"); + joint_cost_config = JointWaypointConfig(*cartesian_waypoint_config); } if (joint_constraint_element != nullptr) { - joint_constraint_config = JointWaypointConfig(*joint_constraint_element); + const tinyxml2::XMLElement* cartesian_waypoint_config = joint_constraint_element->FirstChildElement("CartesianWaypointConfig"); + joint_constraint_config = JointWaypointConfig(*cartesian_waypoint_config); } if (cnt_error_fn_element != nullptr) @@ -275,17 +279,25 @@ tinyxml2::XMLElement* TrajOptDefaultPlanProfile::toXML(tinyxml2::XMLDocument& do tinyxml2::XMLElement* xml_trajopt = doc.NewElement("TrajOptDefaultPlanProfile"); + tinyxml2::XMLElement* xml_cart_cost_parent = doc.NewElement("CartesianCostConfig"); tinyxml2::XMLElement* xml_cart_cost = cartesian_cost_config.toXML(doc); - xml_trajopt->InsertEndChild(xml_cart_cost); + xml_cart_cost_parent->InsertEndChild(xml_cart_cost); + xml_trajopt->InsertEndChild(xml_cart_cost_parent); + tinyxml2::XMLElement* xml_cart_cnt_parent = doc.NewElement("CartesianConstraintConfig"); tinyxml2::XMLElement* xml_cart_cnt = cartesian_constraint_config.toXML(doc); - xml_trajopt->InsertEndChild(xml_cart_cnt); + xml_cart_cnt_parent->InsertEndChild(xml_cart_cnt); + xml_trajopt->InsertEndChild(xml_cart_cnt_parent); + tinyxml2::XMLElement* xml_joint_cost_parent = doc.NewElement("JointCostConfig"); tinyxml2::XMLElement* xml_joint_cost = joint_cost_config.toXML(doc); - xml_trajopt->InsertEndChild(xml_joint_cost); + xml_joint_cost_parent->InsertEndChild(xml_joint_cost); + xml_trajopt->InsertEndChild(xml_joint_cost_parent); + tinyxml2::XMLElement* xml_joint_cnt_parent = doc.NewElement("JointConstraintConfig"); tinyxml2::XMLElement* xml_joint_cnt = joint_constraint_config.toXML(doc); - xml_trajopt->InsertEndChild(xml_joint_cnt); + xml_joint_cnt_parent->InsertEndChild(xml_joint_cnt); + xml_trajopt->InsertEndChild(xml_joint_cnt_parent); xml_planner->InsertEndChild(xml_trajopt); diff --git a/tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp b/tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp index 2c5cfd376b1..06fa0d615ed 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp @@ -45,7 +45,6 @@ CartesianWaypointConfig::CartesianWaypointConfig(const tinyxml2::XMLElement& xml if (enabled_element == nullptr) throw std::runtime_error("CartesianWaypointConfig: Must have Enabled element."); - tinyxml2::XMLError status = enabled_element->QueryBoolText(&enabled); if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) throw std::runtime_error("CartesianWaypointConfig: Error parsing Enabled string"); @@ -126,17 +125,23 @@ tinyxml2::XMLElement* CartesianWaypointConfig::toXML(tinyxml2::XMLDocument& doc) xml_use_tolerance_override->SetText(use_tolerance_override); xml_cartesian_waypoint_config->InsertEndChild(xml_use_tolerance_override); - tinyxml2::XMLElement* xml_lower_tolerance = doc.NewElement("LowerTolerance"); - std::stringstream lower_tolerance_ss; - lower_tolerance_ss << lower_tolerance.format(eigen_format); - xml_lower_tolerance->SetText(lower_tolerance_ss.str().c_str()); - xml_cartesian_waypoint_config->InsertEndChild(xml_lower_tolerance); + if (lower_tolerance.size() > 0) + { + tinyxml2::XMLElement* xml_lower_tolerance = doc.NewElement("LowerTolerance"); + std::stringstream lower_tolerance_ss; + lower_tolerance_ss << lower_tolerance.format(eigen_format); + xml_lower_tolerance->SetText(lower_tolerance_ss.str().c_str()); + xml_cartesian_waypoint_config->InsertEndChild(xml_lower_tolerance); + } - tinyxml2::XMLElement* xml_upper_tolerance = doc.NewElement("UpperTolerance"); - std::stringstream upper_tolerance_ss; - upper_tolerance_ss << upper_tolerance.format(eigen_format); - xml_upper_tolerance->SetText(upper_tolerance_ss.str().c_str()); - xml_cartesian_waypoint_config->InsertEndChild(xml_upper_tolerance); + if (upper_tolerance.size() > 0) + { + tinyxml2::XMLElement* xml_upper_tolerance = doc.NewElement("UpperTolerance"); + std::stringstream upper_tolerance_ss; + upper_tolerance_ss << upper_tolerance.format(eigen_format); + xml_upper_tolerance->SetText(upper_tolerance_ss.str().c_str()); + xml_cartesian_waypoint_config->InsertEndChild(xml_upper_tolerance); + } tinyxml2::XMLElement* xml_coeff = doc.NewElement("Coefficients"); std::stringstream coeff_ss; @@ -238,17 +243,23 @@ tinyxml2::XMLElement* JointWaypointConfig::toXML(tinyxml2::XMLDocument& doc) con xml_use_tolerance_override->SetText(use_tolerance_override); xml_cartesian_waypoint_config->InsertEndChild(xml_use_tolerance_override); - tinyxml2::XMLElement* xml_lower_tolerance = doc.NewElement("LowerTolerance"); - std::stringstream lower_tolerance_ss; - lower_tolerance_ss << lower_tolerance.format(eigen_format); - xml_lower_tolerance->SetText(lower_tolerance_ss.str().c_str()); - xml_cartesian_waypoint_config->InsertEndChild(xml_lower_tolerance); - - tinyxml2::XMLElement* xml_upper_tolerance = doc.NewElement("UpperTolerance"); - std::stringstream upper_tolerance_ss; - upper_tolerance_ss << upper_tolerance.format(eigen_format); - xml_upper_tolerance->SetText(upper_tolerance_ss.str().c_str()); - xml_cartesian_waypoint_config->InsertEndChild(xml_upper_tolerance); + if (lower_tolerance.size() > 0) + { + tinyxml2::XMLElement* xml_lower_tolerance = doc.NewElement("LowerTolerance"); + std::stringstream lower_tolerance_ss; + lower_tolerance_ss << lower_tolerance.format(eigen_format); + xml_lower_tolerance->SetText(lower_tolerance_ss.str().c_str()); + xml_cartesian_waypoint_config->InsertEndChild(xml_lower_tolerance); + } + + if (upper_tolerance.size() > 0) + { + tinyxml2::XMLElement* xml_upper_tolerance = doc.NewElement("UpperTolerance"); + std::stringstream upper_tolerance_ss; + upper_tolerance_ss << upper_tolerance.format(eigen_format); + xml_upper_tolerance->SetText(upper_tolerance_ss.str().c_str()); + xml_cartesian_waypoint_config->InsertEndChild(xml_upper_tolerance); + } tinyxml2::XMLElement* xml_coeff = doc.NewElement("Coefficients"); std::stringstream coeff_ss; diff --git a/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp b/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp index a1a2892d086..8b5df6f4fde 100644 --- a/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp +++ b/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp @@ -231,6 +231,10 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointJoint) // NOLINT request.profiles = profiles; { + plan_profile->cartesian_cost_config.enabled = false; + plan_profile->cartesian_constraint_config.enabled = true; + plan_profile->joint_cost_config.enabled = false; + plan_profile->joint_constraint_config.enabled = true; std::shared_ptr pci = test_planner.createProblem(request); trajopt::TrajOptProb::Ptr problem = trajopt::ConstructProblem(*pci); @@ -244,7 +248,10 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointJoint) // NOLINT (tesseract_tests::vectorContainsType(problem->getCosts()))); } { - plan_profile->term_type = trajopt::TermType::TT_COST; + plan_profile->cartesian_cost_config.enabled = true; + plan_profile->cartesian_constraint_config.enabled = false; + plan_profile->joint_cost_config.enabled = true; + plan_profile->joint_constraint_config.enabled = false; std::shared_ptr pci = test_planner.createProblem(request); @@ -311,6 +318,10 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointCart) // NOLINT request.profiles = profiles; { + plan_profile->cartesian_cost_config.enabled = false; + plan_profile->cartesian_constraint_config.enabled = true; + plan_profile->joint_cost_config.enabled = false; + plan_profile->joint_constraint_config.enabled = true; std::shared_ptr pci = test_planner.createProblem(request); trajopt::TrajOptProb::Ptr problem = trajopt::ConstructProblem(*pci); @@ -325,7 +336,10 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointCart) // NOLINT } { - plan_profile->term_type = trajopt::TermType::TT_COST; + plan_profile->cartesian_cost_config.enabled = true; + plan_profile->cartesian_constraint_config.enabled = false; + plan_profile->joint_cost_config.enabled = true; + plan_profile->joint_constraint_config.enabled = false; std::shared_ptr pci = test_planner.createProblem(request); trajopt::TrajOptProb::Ptr problem = trajopt::ConstructProblem(*pci); @@ -395,6 +409,10 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartJoint) // NOLINT request.profiles = profiles; { + plan_profile->cartesian_cost_config.enabled = false; + plan_profile->cartesian_constraint_config.enabled = true; + plan_profile->joint_cost_config.enabled = false; + plan_profile->joint_constraint_config.enabled = true; std::shared_ptr pci = test_planner.createProblem(request); trajopt::TrajOptProb::Ptr problem = trajopt::ConstructProblem(*pci); @@ -409,7 +427,10 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartJoint) // NOLINT } { - plan_profile->term_type = trajopt::TermType::TT_COST; + plan_profile->cartesian_cost_config.enabled = true; + plan_profile->cartesian_constraint_config.enabled = false; + plan_profile->joint_cost_config.enabled = true; + plan_profile->joint_constraint_config.enabled = false; std::shared_ptr pci = test_planner.createProblem(request); trajopt::TrajOptProb::Ptr problem = trajopt::ConstructProblem(*pci); @@ -479,6 +500,10 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartCart) // NOLINT request.profiles = profiles; { + plan_profile->cartesian_cost_config.enabled = false; + plan_profile->cartesian_constraint_config.enabled = true; + plan_profile->joint_cost_config.enabled = false; + plan_profile->joint_constraint_config.enabled = true; std::shared_ptr pci = test_planner.createProblem(request); trajopt::TrajOptProb::Ptr problem = trajopt::ConstructProblem(*pci); @@ -492,7 +517,10 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartCart) // NOLINT (tesseract_tests::vectorContainsType(problem->getCosts()))); } { - plan_profile->term_type = trajopt::TermType::TT_COST; + plan_profile->cartesian_cost_config.enabled = true; + plan_profile->cartesian_constraint_config.enabled = false; + plan_profile->joint_cost_config.enabled = true; + plan_profile->joint_constraint_config.enabled = false; std::shared_ptr pci = test_planner.createProblem(request); trajopt::TrajOptProb::Ptr problem = trajopt::ConstructProblem(*pci); @@ -579,6 +607,11 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsCartCart) // NOL composite_profile->collision_constraint_config.enabled = t4; composite_profile->collision_cost_config.enabled = t4; + plan_profile->cartesian_cost_config.enabled = false; + plan_profile->cartesian_constraint_config.enabled = true; + plan_profile->joint_cost_config.enabled = false; + plan_profile->joint_constraint_config.enabled = true; + pci = test_planner.createProblem(request); problem = trajopt::ConstructProblem(*pci); @@ -627,6 +660,10 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointConstraint) // NOLINT // Create Profiles auto plan_profile = std::make_shared(); + plan_profile->cartesian_cost_config.enabled = false; + plan_profile->cartesian_constraint_config.enabled = true; + plan_profile->joint_cost_config.enabled = false; + plan_profile->joint_constraint_config.enabled = true; auto composite_profile = std::make_shared(); // Profile Dictionary @@ -687,7 +724,10 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointCost) // NOLINT // Create Profiles auto plan_profile = std::make_shared(); - plan_profile->term_type = trajopt::TermType::TT_COST; // Everything associated with profile is now added as a cost + plan_profile->cartesian_cost_config.enabled = true; + plan_profile->cartesian_constraint_config.enabled = false; + plan_profile->joint_cost_config.enabled = true; + plan_profile->joint_constraint_config.enabled = false; auto composite_profile = std::make_shared(); @@ -751,9 +791,16 @@ TEST(TesseractPlanningTrajoptSerializeUnit, SerializeTrajoptDefaultPlanToXml) / { // Write program to file TrajOptDefaultPlanProfile plan_profile; - plan_profile.cartesian_coeff = Eigen::VectorXd::Ones(6) * 10; - plan_profile.joint_coeff = Eigen::VectorXd::Ones(6) * 9; - plan_profile.term_type = trajopt::TermType::TT_COST; + plan_profile.cartesian_cost_config.coeff = Eigen::VectorXd::Ones(6) * 10; + plan_profile.joint_cost_config.coeff = Eigen::VectorXd::Ones(6) * 9; + plan_profile.cartesian_cost_config.enabled = true; + plan_profile.cartesian_constraint_config.enabled = false; + plan_profile.joint_cost_config.enabled = true; + plan_profile.joint_constraint_config.enabled = false; + plan_profile.cartesian_constraint_config.lower_tolerance = Eigen::VectorXd::Ones(6) * -0.01; + plan_profile.cartesian_constraint_config.upper_tolerance = Eigen::VectorXd::Ones(6) * 0.02; + plan_profile.joint_constraint_config.lower_tolerance = Eigen::VectorXd::Ones(6) * -0.03; + plan_profile.joint_constraint_config.upper_tolerance = Eigen::VectorXd::Ones(6) * 0.04; EXPECT_TRUE(toXMLFile(plan_profile, tesseract_common::getTempPath() + "trajopt_default_plan_example_input.xml")); @@ -768,7 +815,16 @@ TEST(TesseractPlanningTrajoptSerializeUnit, SerializeTrajoptDefaultPlanToXml) / // Re-write file and compare changed from default term EXPECT_TRUE( toXMLFile(imported_plan_profile, tesseract_common::getTempPath() + "trajopt_default_plan_example_input2.xml")); - EXPECT_TRUE(plan_profile.term_type == imported_plan_profile.term_type); + EXPECT_TRUE(plan_profile.cartesian_cost_config.enabled == imported_plan_profile.cartesian_cost_config.enabled); + EXPECT_TRUE(plan_profile.cartesian_constraint_config.enabled == imported_plan_profile.cartesian_constraint_config.enabled); + EXPECT_TRUE(plan_profile.joint_cost_config.enabled == imported_plan_profile.joint_cost_config.enabled); + EXPECT_TRUE(plan_profile.joint_constraint_config.enabled == imported_plan_profile.joint_constraint_config.enabled); + EXPECT_TRUE(plan_profile.cartesian_cost_config.coeff == imported_plan_profile.cartesian_cost_config.coeff); + EXPECT_TRUE(plan_profile.joint_cost_config.coeff == imported_plan_profile.joint_cost_config.coeff); + EXPECT_TRUE(plan_profile.cartesian_constraint_config.lower_tolerance == imported_plan_profile.cartesian_constraint_config.lower_tolerance); + EXPECT_TRUE(plan_profile.cartesian_constraint_config.upper_tolerance == imported_plan_profile.cartesian_constraint_config.upper_tolerance); + EXPECT_TRUE(plan_profile.joint_constraint_config.lower_tolerance == imported_plan_profile.joint_constraint_config.lower_tolerance); + EXPECT_TRUE(plan_profile.joint_constraint_config.upper_tolerance == imported_plan_profile.joint_constraint_config.upper_tolerance); } int main(int argc, char** argv) From 57432490c7fa94466e3eef707d8f5da30097f019 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Mon, 26 Feb 2024 14:03:52 -0600 Subject: [PATCH 03/13] Clang formatting --- .../src/profile/trajopt_default_plan_profile.cpp | 13 +++++++++---- .../trajopt/test/trajopt_planner_tests.cpp | 15 ++++++++++----- 2 files changed, 19 insertions(+), 9 deletions(-) diff --git a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp index 51ae2bbbfb2..64d4031eaea 100644 --- a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp +++ b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp @@ -48,25 +48,30 @@ TrajOptDefaultPlanProfile::TrajOptDefaultPlanProfile(const tinyxml2::XMLElement& if (cartesian_cost_element != nullptr) { - const tinyxml2::XMLElement* cartesian_waypoint_config = cartesian_cost_element->FirstChildElement("CartesianWaypointConfig"); + const tinyxml2::XMLElement* cartesian_waypoint_config = cartesian_cost_element->FirstChildElement("CartesianWaypoin" + "tConfig"); cartesian_cost_config = CartesianWaypointConfig(*cartesian_waypoint_config); } if (cartesian_constraint_element != nullptr) { - const tinyxml2::XMLElement* cartesian_waypoint_config = cartesian_constraint_element->FirstChildElement("CartesianWaypointConfig"); + const tinyxml2::XMLElement* cartesian_waypoint_config = cartesian_constraint_element->FirstChildElement("CartesianW" + "aypointCon" + "fig"); cartesian_constraint_config = CartesianWaypointConfig(*cartesian_waypoint_config); } if (joint_cost_element != nullptr) { - const tinyxml2::XMLElement* cartesian_waypoint_config = joint_cost_element->FirstChildElement("CartesianWaypointConfig"); + const tinyxml2::XMLElement* cartesian_waypoint_config = joint_cost_element->FirstChildElement("CartesianWaypointCon" + "fig"); joint_cost_config = JointWaypointConfig(*cartesian_waypoint_config); } if (joint_constraint_element != nullptr) { - const tinyxml2::XMLElement* cartesian_waypoint_config = joint_constraint_element->FirstChildElement("CartesianWaypointConfig"); + const tinyxml2::XMLElement* cartesian_waypoint_config = joint_constraint_element->FirstChildElement("CartesianWaypo" + "intConfig"); joint_constraint_config = JointWaypointConfig(*cartesian_waypoint_config); } diff --git a/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp b/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp index 8b5df6f4fde..3c9ca83cd53 100644 --- a/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp +++ b/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp @@ -816,15 +816,20 @@ TEST(TesseractPlanningTrajoptSerializeUnit, SerializeTrajoptDefaultPlanToXml) / EXPECT_TRUE( toXMLFile(imported_plan_profile, tesseract_common::getTempPath() + "trajopt_default_plan_example_input2.xml")); EXPECT_TRUE(plan_profile.cartesian_cost_config.enabled == imported_plan_profile.cartesian_cost_config.enabled); - EXPECT_TRUE(plan_profile.cartesian_constraint_config.enabled == imported_plan_profile.cartesian_constraint_config.enabled); + EXPECT_TRUE(plan_profile.cartesian_constraint_config.enabled == + imported_plan_profile.cartesian_constraint_config.enabled); EXPECT_TRUE(plan_profile.joint_cost_config.enabled == imported_plan_profile.joint_cost_config.enabled); EXPECT_TRUE(plan_profile.joint_constraint_config.enabled == imported_plan_profile.joint_constraint_config.enabled); EXPECT_TRUE(plan_profile.cartesian_cost_config.coeff == imported_plan_profile.cartesian_cost_config.coeff); EXPECT_TRUE(plan_profile.joint_cost_config.coeff == imported_plan_profile.joint_cost_config.coeff); - EXPECT_TRUE(plan_profile.cartesian_constraint_config.lower_tolerance == imported_plan_profile.cartesian_constraint_config.lower_tolerance); - EXPECT_TRUE(plan_profile.cartesian_constraint_config.upper_tolerance == imported_plan_profile.cartesian_constraint_config.upper_tolerance); - EXPECT_TRUE(plan_profile.joint_constraint_config.lower_tolerance == imported_plan_profile.joint_constraint_config.lower_tolerance); - EXPECT_TRUE(plan_profile.joint_constraint_config.upper_tolerance == imported_plan_profile.joint_constraint_config.upper_tolerance); + EXPECT_TRUE(plan_profile.cartesian_constraint_config.lower_tolerance == + imported_plan_profile.cartesian_constraint_config.lower_tolerance); + EXPECT_TRUE(plan_profile.cartesian_constraint_config.upper_tolerance == + imported_plan_profile.cartesian_constraint_config.upper_tolerance); + EXPECT_TRUE(plan_profile.joint_constraint_config.lower_tolerance == + imported_plan_profile.joint_constraint_config.lower_tolerance); + EXPECT_TRUE(plan_profile.joint_constraint_config.upper_tolerance == + imported_plan_profile.joint_constraint_config.upper_tolerance); } int main(int argc, char** argv) From 67980986dcf84f8c34822dd518043552346675b1 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Mon, 26 Feb 2024 14:46:59 -0600 Subject: [PATCH 04/13] Pass tolerances by reference per clang tidy --- .../tesseract_motion_planners/trajopt/trajopt_utils.h | 8 ++++---- tesseract_motion_planners/trajopt/src/trajopt_utils.cpp | 8 ++++---- 2 files changed, 8 insertions(+), 8 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 7676698db0c..78729fbf8a8 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 @@ -43,8 +43,8 @@ trajopt::TermInfo::Ptr createCartesianWaypointTermInfo(int index, const Eigen::Isometry3d& tcp_offset, const Eigen::VectorXd& coeffs, trajopt::TermType type, - Eigen::VectorXd lower_tolerance = Eigen::VectorXd::Zero(6), - Eigen::VectorXd upper_tolerance = Eigen::VectorXd::Zero(6)); + const Eigen::VectorXd& lower_tolerance = Eigen::VectorXd::Zero(6), + const Eigen::VectorXd& upper_tolerance = Eigen::VectorXd::Zero(6)); trajopt::TermInfo::Ptr createDynamicCartesianWaypointTermInfo(int index, @@ -54,8 +54,8 @@ createDynamicCartesianWaypointTermInfo(int index, const Eigen::Isometry3d& tcp_offset, const Eigen::VectorXd& coeffs, trajopt::TermType type, - Eigen::VectorXd lower_tolerance = Eigen::VectorXd::Zero(6), - Eigen::VectorXd upper_tolerance = Eigen::VectorXd::Zero(6)); + const Eigen::VectorXd& lower_tolerance = Eigen::VectorXd::Zero(6), + const Eigen::VectorXd& upper_tolerance = Eigen::VectorXd::Zero(6)); trajopt::TermInfo::Ptr createNearJointStateTermInfo(const Eigen::VectorXd& target, const std::vector& joint_names, diff --git a/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp b/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp index ac14b55c40f..160d843480e 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp @@ -34,8 +34,8 @@ trajopt::TermInfo::Ptr createCartesianWaypointTermInfo(int index, const Eigen::Isometry3d& tcp_offset, const Eigen::VectorXd& coeffs, trajopt::TermType type, - Eigen::VectorXd lower_tolerance, - Eigen::VectorXd upper_tolerance) + const Eigen::VectorXd& lower_tolerance, + const Eigen::VectorXd& upper_tolerance) { auto pose_info = std::make_shared(); pose_info->term_type = type; @@ -72,8 +72,8 @@ trajopt::TermInfo::Ptr createDynamicCartesianWaypointTermInfo(int index, const Eigen::Isometry3d& tcp_offset, const Eigen::VectorXd& coeffs, trajopt::TermType type, - Eigen::VectorXd lower_tolerance, - Eigen::VectorXd upper_tolerance) + const Eigen::VectorXd& lower_tolerance, + const Eigen::VectorXd& upper_tolerance) { std::shared_ptr pose = std::make_shared(); pose->term_type = type; From 224a4d5f6de654a68c57cc1834cc3fe6f1b035d2 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Mon, 26 Feb 2024 14:49:24 -0600 Subject: [PATCH 05/13] Clang formatting --- .../trajopt/trajopt_utils.h | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 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 78729fbf8a8..ad7d22ac9e9 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 @@ -36,15 +36,16 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { -trajopt::TermInfo::Ptr createCartesianWaypointTermInfo(int index, - const std::string& working_frame, - const Eigen::Isometry3d& c_wp, - const std::string& tcp_frame, - 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)); +trajopt::TermInfo::Ptr +createCartesianWaypointTermInfo(int index, + const std::string& working_frame, + const Eigen::Isometry3d& c_wp, + const std::string& tcp_frame, + 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)); trajopt::TermInfo::Ptr createDynamicCartesianWaypointTermInfo(int index, From b6e680fefc5b1b81ab364fb0a4be2f2c1987ba1d Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Tue, 5 Mar 2024 09:20:27 -0600 Subject: [PATCH 06/13] 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 ad7d22ac9e9..a2e38648705 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 cbb6a4c987d..ac6c0e681ad 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 06fa0d615ed..0c02bd3d865 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 From 43de83cf5fe8573f74899fbeb8540600bdc13115 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Tue, 5 Mar 2024 09:54:13 -0600 Subject: [PATCH 07/13] Clang formatting --- .../trajopt/trajopt_utils.h | 19 +++++++++---------- .../trajopt/trajopt_waypoint_config.h | 6 +++--- 2 files changed, 12 insertions(+), 13 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 a2e38648705..7db812259c1 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 @@ -36,16 +36,15 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { -trajopt::TermInfo::Ptr -createCartesianWaypointTermInfo(int index, - const std::string& working_frame, - const Eigen::Isometry3d& c_wp, - const std::string& tcp_frame, - const Eigen::Isometry3d& tcp_offset, - const Eigen::VectorXd& coeffs, - trajopt::TermType type, - const Eigen::VectorXd& lower_tolerance = Eigen::VectorXd(), - const Eigen::VectorXd& upper_tolerance = Eigen::VectorXd()); +trajopt::TermInfo::Ptr createCartesianWaypointTermInfo(int index, + const std::string& working_frame, + const Eigen::Isometry3d& c_wp, + const std::string& tcp_frame, + const Eigen::Isometry3d& tcp_offset, + const Eigen::VectorXd& coeffs, + trajopt::TermType type, + const Eigen::VectorXd& lower_tolerance = Eigen::VectorXd(), + const Eigen::VectorXd& upper_tolerance = Eigen::VectorXd()); trajopt::TermInfo::Ptr createDynamicCartesianWaypointTermInfo(int index, 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 ac6c0e681ad..9d8dab39bf2 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 @@ -31,9 +31,9 @@ namespace tinyxml2 { - class XMLElement; - class XMLDocument; -} +class XMLElement; +class XMLDocument; +} // namespace tinyxml2 namespace tesseract_planning { From 8e363bb7dead0c97fbfb386f6e1e11e951760bb2 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Tue, 5 Mar 2024 10:10:41 -0600 Subject: [PATCH 08/13] Fix clang-tidy errors --- .../src/cartesian_waypoint.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/tesseract_command_language/src/cartesian_waypoint.cpp b/tesseract_command_language/src/cartesian_waypoint.cpp index 88e0c35fed4..e1e6d2c5475 100644 --- a/tesseract_command_language/src/cartesian_waypoint.cpp +++ b/tesseract_command_language/src/cartesian_waypoint.cpp @@ -8,12 +8,16 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -tesseract_planning::CartesianWaypoint::CartesianWaypoint(const Eigen::Isometry3d& transform) : transform_(transform) {} +tesseract_planning::CartesianWaypoint::CartesianWaypoint( + const Eigen::Isometry3d& transform) // NOLINT(modernize-pass-by-value) + : transform_(transform) +{ +} tesseract_planning::CartesianWaypoint::CartesianWaypoint( - const Eigen::Isometry3d& transform, - const Eigen::VectorXd& lower_tol, // NOLINT(modernize-pass-by-value) - const Eigen::VectorXd& upper_tol) // NOLINT(modernize-pass-by-value) + const Eigen::Isometry3d& transform, // NOLINT(modernize-pass-by-value) + const Eigen::VectorXd& lower_tol, // NOLINT(modernize-pass-by-value) + const Eigen::VectorXd& upper_tol) // NOLINT(modernize-pass-by-value) : transform_(transform), lower_tolerance_(lower_tol), upper_tolerance_(upper_tol) { } From 92c395fd3c1b2e2900bdd62d5dd988a66f931073 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Tue, 5 Mar 2024 10:54:08 -0600 Subject: [PATCH 09/13] Ignore clang-tidy warning about copying Eigen objects --- .../descartes/descartes_robot_sampler.h | 4 ++-- .../descartes/impl/descartes_robot_sampler.hpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_robot_sampler.h b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_robot_sampler.h index 0af002ef980..dcfdb00169e 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_robot_sampler.h +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_robot_sampler.h @@ -54,12 +54,12 @@ class DescartesRobotSampler : public descartes_light::WaypointSampler * @param is_valid This is a user defined function to filter out solution */ DescartesRobotSampler(std::string target_working_frame, - const Eigen::Isometry3d& target_pose, + const Eigen::Isometry3d& target_pose, // NOLINT(modernize-pass-by-value) PoseSamplerFn target_pose_sampler, tesseract_kinematics::KinematicGroup::ConstPtr manip, DescartesCollision::Ptr collision, std::string tcp_frame, - const Eigen::Isometry3d& tcp_offset, + const Eigen::Isometry3d& tcp_offset, // NOLINT(modernize-pass-by-value) bool allow_collision, DescartesVertexEvaluator::Ptr is_valid, bool use_redundant_joint_solutions); diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_robot_sampler.hpp b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_robot_sampler.hpp index d541174966a..27e079b560d 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_robot_sampler.hpp +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_robot_sampler.hpp @@ -40,12 +40,12 @@ namespace tesseract_planning { template DescartesRobotSampler::DescartesRobotSampler(std::string target_working_frame, - const Eigen::Isometry3d& target_pose, + const Eigen::Isometry3d& target_pose, // NOLINT(modernize-pass-by-value) PoseSamplerFn target_pose_sampler, tesseract_kinematics::KinematicGroup::ConstPtr manip, DescartesCollision::Ptr collision, std::string tcp_frame, - const Eigen::Isometry3d& tcp_offset, + const Eigen::Isometry3d& tcp_offset, // NOLINT(modernize-pass-by-value) bool allow_collision, DescartesVertexEvaluator::Ptr is_valid, bool use_redundant_joint_solutions) From 4122f3a490b8afdced83731153390bb04835b12e Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Tue, 5 Mar 2024 10:54:35 -0600 Subject: [PATCH 10/13] Fix issue of reference config object which had already been moved (clang-tidy) --- .../descartes/impl/descartes_collision_edge_evaluator.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_collision_edge_evaluator.hpp b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_collision_edge_evaluator.hpp index 81e061a0f5f..e048811b25a 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_collision_edge_evaluator.hpp +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_collision_edge_evaluator.hpp @@ -54,7 +54,7 @@ DescartesCollisionEdgeEvaluator::DescartesCollisionEdgeEvaluator( if (discrete_contact_manager_ != nullptr) { discrete_contact_manager_->setActiveCollisionObjects(active_link_names_); - discrete_contact_manager_->applyContactManagerConfig(config.contact_manager_config); + discrete_contact_manager_->applyContactManagerConfig(collision_check_config_.contact_manager_config); } else if (collision_check_config_.type == tesseract_collision::CollisionEvaluatorType::DISCRETE || collision_check_config_.type == tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE) @@ -66,7 +66,7 @@ DescartesCollisionEdgeEvaluator::DescartesCollisionEdgeEvaluator( if (continuous_contact_manager_ != nullptr) { continuous_contact_manager_->setActiveCollisionObjects(active_link_names_); - continuous_contact_manager_->applyContactManagerConfig(config.contact_manager_config); + continuous_contact_manager_->applyContactManagerConfig(collision_check_config_.contact_manager_config); } else if (collision_check_config_.type == tesseract_collision::CollisionEvaluatorType::CONTINUOUS || collision_check_config_.type == tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS) From e84ca11de7afa4c49858e542cdc3f0472857e74e Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Tue, 5 Mar 2024 10:57:15 -0600 Subject: [PATCH 11/13] Clang formatting --- .../descartes/descartes_robot_sampler.h | 4 ++-- .../impl/descartes_robot_sampler.hpp | 21 ++++++++++--------- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_robot_sampler.h b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_robot_sampler.h index dcfdb00169e..4166c4a1978 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_robot_sampler.h +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_robot_sampler.h @@ -54,12 +54,12 @@ class DescartesRobotSampler : public descartes_light::WaypointSampler * @param is_valid This is a user defined function to filter out solution */ DescartesRobotSampler(std::string target_working_frame, - const Eigen::Isometry3d& target_pose, // NOLINT(modernize-pass-by-value) + const Eigen::Isometry3d& target_pose, // NOLINT(modernize-pass-by-value) PoseSamplerFn target_pose_sampler, tesseract_kinematics::KinematicGroup::ConstPtr manip, DescartesCollision::Ptr collision, std::string tcp_frame, - const Eigen::Isometry3d& tcp_offset, // NOLINT(modernize-pass-by-value) + const Eigen::Isometry3d& tcp_offset, // NOLINT(modernize-pass-by-value) bool allow_collision, DescartesVertexEvaluator::Ptr is_valid, bool use_redundant_joint_solutions); diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_robot_sampler.hpp b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_robot_sampler.hpp index 27e079b560d..972da390e20 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_robot_sampler.hpp +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_robot_sampler.hpp @@ -39,16 +39,17 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { template -DescartesRobotSampler::DescartesRobotSampler(std::string target_working_frame, - const Eigen::Isometry3d& target_pose, // NOLINT(modernize-pass-by-value) - PoseSamplerFn target_pose_sampler, - tesseract_kinematics::KinematicGroup::ConstPtr manip, - DescartesCollision::Ptr collision, - std::string tcp_frame, - const Eigen::Isometry3d& tcp_offset, // NOLINT(modernize-pass-by-value) - bool allow_collision, - DescartesVertexEvaluator::Ptr is_valid, - bool use_redundant_joint_solutions) +DescartesRobotSampler::DescartesRobotSampler( + std::string target_working_frame, + const Eigen::Isometry3d& target_pose, // NOLINT(modernize-pass-by-value) + PoseSamplerFn target_pose_sampler, + tesseract_kinematics::KinematicGroup::ConstPtr manip, + DescartesCollision::Ptr collision, + std::string tcp_frame, + const Eigen::Isometry3d& tcp_offset, // NOLINT(modernize-pass-by-value) + bool allow_collision, + DescartesVertexEvaluator::Ptr is_valid, + bool use_redundant_joint_solutions) : target_working_frame_(std::move(target_working_frame)) , target_pose_(target_pose) , target_pose_sampler_(std::move(target_pose_sampler)) From 267052355748cc8aa79aa41b577f421a69fb7ad9 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Tue, 5 Mar 2024 11:58:13 -0600 Subject: [PATCH 12/13] Fix more clang-tidy errors --- tesseract_motion_planners/descartes/src/deserialize.cpp | 2 +- tesseract_motion_planners/descartes/src/serialize.cpp | 2 +- .../tesseract_motion_planners/trajopt/trajopt_waypoint_config.h | 1 + 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/tesseract_motion_planners/descartes/src/deserialize.cpp b/tesseract_motion_planners/descartes/src/deserialize.cpp index c74e56a1586..8d692e2194e 100644 --- a/tesseract_motion_planners/descartes/src/deserialize.cpp +++ b/tesseract_motion_planners/descartes/src/deserialize.cpp @@ -42,7 +42,7 @@ namespace tesseract_planning DescartesDefaultPlanProfile descartesPlanParser(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* descartes_plan_element = xml_element.FirstChildElement("DescartesPlanProfile"); - return DescartesDefaultPlanProfile(*descartes_plan_element); + return { *descartes_plan_element }; } DescartesDefaultPlanProfile descartesPlanFromXMLElement(const tinyxml2::XMLElement* profile_xml) diff --git a/tesseract_motion_planners/descartes/src/serialize.cpp b/tesseract_motion_planners/descartes/src/serialize.cpp index ec6efc76332..f6dca27b92e 100644 --- a/tesseract_motion_planners/descartes/src/serialize.cpp +++ b/tesseract_motion_planners/descartes/src/serialize.cpp @@ -74,7 +74,7 @@ std::string toXMLString(const DescartesPlanProfile& plan_profile) std::shared_ptr doc = toXMLDocument(plan_profile); tinyxml2::XMLPrinter printer; doc->Print(&printer); - return std::string(printer.CStr()); + return { printer.CStr() }; } } // namespace tesseract_planning 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 9d8dab39bf2..1b25da8b31c 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 @@ -31,6 +31,7 @@ namespace tinyxml2 { +// NOLINTNEXTLINE(cppcoreguidelines-virtual-class-destructor) class XMLElement; class XMLDocument; } // namespace tinyxml2 From 3f9ae08e72e0aae782f0ff25019c19ecca9d81e2 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Tue, 5 Mar 2024 13:39:34 -0600 Subject: [PATCH 13/13] Fixed failing serialization test --- .../trajopt/trajopt_waypoint_config.h | 4 ++-- .../trajopt/src/profile/trajopt_default_plan_profile.cpp | 8 ++++---- .../trajopt/src/trajopt_waypoint_config.cpp | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) 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 1b25da8b31c..2bae33e7cd2 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 @@ -59,10 +59,10 @@ struct CartesianWaypointConfig /** @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::Matrix lower_tolerance; + Eigen::Matrix lower_tolerance{ Eigen::VectorXd::Zero(6) }; /** @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::Matrix upper_tolerance; + Eigen::Matrix upper_tolerance{ Eigen::VectorXd::Zero(6) }; /** @brief coefficients corresponsing to dx, dy, dz, rx, ry, rz*/ Eigen::Matrix coeff{ Eigen::VectorXd::Constant(6, 5) }; diff --git a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp index 64d4031eaea..20b91ac4245 100644 --- a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp +++ b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp @@ -63,15 +63,15 @@ TrajOptDefaultPlanProfile::TrajOptDefaultPlanProfile(const tinyxml2::XMLElement& if (joint_cost_element != nullptr) { - const tinyxml2::XMLElement* cartesian_waypoint_config = joint_cost_element->FirstChildElement("CartesianWaypointCon" - "fig"); + const tinyxml2::XMLElement* cartesian_waypoint_config = joint_cost_element->FirstChildElement("JointWaypointConfi" + "g"); joint_cost_config = JointWaypointConfig(*cartesian_waypoint_config); } if (joint_constraint_element != nullptr) { - const tinyxml2::XMLElement* cartesian_waypoint_config = joint_constraint_element->FirstChildElement("CartesianWaypo" - "intConfig"); + const tinyxml2::XMLElement* cartesian_waypoint_config = joint_constraint_element->FirstChildElement("JointWaypointC" + "onfig"); joint_constraint_config = JointWaypointConfig(*cartesian_waypoint_config); } diff --git a/tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp b/tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp index 0c02bd3d865..4252dee2ede 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp @@ -235,7 +235,7 @@ tinyxml2::XMLElement* JointWaypointConfig::toXML(tinyxml2::XMLDocument& doc) con { Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " "); - tinyxml2::XMLElement* xml_cartesian_waypoint_config = doc.NewElement("CartesianWaypointConfig"); + tinyxml2::XMLElement* xml_cartesian_waypoint_config = doc.NewElement("JointWaypointConfig"); tinyxml2::XMLElement* xml_enabled = doc.NewElement("Enabled"); xml_enabled->SetText(enabled);