From 3f9ae08e72e0aae782f0ff25019c19ecca9d81e2 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Tue, 5 Mar 2024 13:39:34 -0600 Subject: [PATCH] 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);