From f9ba6236f7a729ca642595e00a6c0b1a40f2155c Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Mon, 2 Dec 2024 07:40:08 +0100 Subject: [PATCH 1/2] Fix issue with tolerance size --- .../trajopt/trajopt_waypoint_config.h | 21 +++++++++++-------- .../trajopt/src/trajopt_utils.cpp | 19 +++++++++++++++-- 2 files changed, 29 insertions(+), 11 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 eb57605eb94..42e77cf830b 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,15 +59,18 @@ struct CartesianWaypointConfig * 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::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::VectorXd::Zero(6) }; - - /** @brief coefficients corresponsing to dx, dy, dz, rx, ry, rz*/ - Eigen::Matrix coeff{ Eigen::VectorXd::Constant(6, 5) }; + /** @brief Distance below waypoint that is allowed. Should be size = 6 or 1, if size = 1 the value is replicated. + * 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::VectorXd::Zero(6) }; + /** @brief Distance above waypoint that is allowed. Should be size = 6 or 1, if size = 1 the value is replicated. + * 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::VectorXd::Zero(6) }; + + /** @brief coefficients corresponsing to dx, dy, dz, rx, ry, rz. + * Should be size = 6 or 1, if size = 1 the value is replicated. */ + Eigen::VectorXd coeff{ Eigen::VectorXd::Constant(6, 5) }; tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const; }; diff --git a/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp b/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp index acdca0f7015..a064ddd103f 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp @@ -60,8 +60,23 @@ std::shared_ptr createCartesianWaypointTermInfo(int index, pose_info->rot_coeffs = coeffs.tail<3>(); } - pose_info->lower_tolerance = lower_tolerance; - pose_info->upper_tolerance = upper_tolerance; + if (lower_tolerance.size() == 1) + { + pose_info->lower_tolerance = Eigen::Vector3d::Constant(lower_tolerance(0)); + } + else if (lower_tolerance.size() == 6) + { + pose_info->lower_tolerance = lower_tolerance; + } + + if (upper_tolerance.size() == 1) + { + pose_info->upper_tolerance = Eigen::Vector3d::Constant(upper_tolerance(0)); + } + else if (upper_tolerance.size() == 6) + { + pose_info->upper_tolerance = upper_tolerance; + } return pose_info; } From 2297b9e98da95e521b6e59fb0cddb640ad4a2123 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 3 Dec 2024 08:18:41 +0100 Subject: [PATCH 2/2] Fix tolerance size --- tesseract_motion_planners/trajopt/src/trajopt_utils.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp b/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp index a064ddd103f..648667c2249 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp @@ -62,7 +62,7 @@ std::shared_ptr createCartesianWaypointTermInfo(int index, if (lower_tolerance.size() == 1) { - pose_info->lower_tolerance = Eigen::Vector3d::Constant(lower_tolerance(0)); + pose_info->lower_tolerance = Eigen::VectorXd::Constant(6, lower_tolerance(0)); } else if (lower_tolerance.size() == 6) { @@ -71,7 +71,7 @@ std::shared_ptr createCartesianWaypointTermInfo(int index, if (upper_tolerance.size() == 1) { - pose_info->upper_tolerance = Eigen::Vector3d::Constant(upper_tolerance(0)); + pose_info->upper_tolerance = Eigen::VectorXd::Constant(6, upper_tolerance(0)); } else if (upper_tolerance.size() == 6) {