From 468fac19ff01dfa8c6dbbdd207896053a4787786 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Mon, 2 Dec 2024 07:40:08 +0100 Subject: [PATCH 1/4] 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 4b2e6567fe1..06b25a8c2cf 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 @@ -55,15 +55,18 @@ struct TrajOptCartesianWaypointConfig * 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) }; private: friend class boost::serialization::access; diff --git a/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp b/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp index eb88a1801e4..0c5beedf6c1 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp @@ -98,8 +98,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 bb35eb858c04185d037bbfaa16a8fa1ef5786792 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 3 Dec 2024 08:18:41 +0100 Subject: [PATCH 2/4] 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 0c5beedf6c1..867516bbe1e 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp @@ -100,7 +100,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) { @@ -109,7 +109,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) { From dab3b9c39e0f91ec707bb457949ca7a7cb661554 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Fri, 25 Apr 2025 15:33:57 +0200 Subject: [PATCH 3/4] Throw an exception when number of coeffs or tolerances is incorrect --- .../trajopt/src/trajopt_utils.cpp | 38 +++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp b/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp index 867516bbe1e..8ab9a61fbf7 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp @@ -97,6 +97,11 @@ std::shared_ptr createCartesianWaypointTermInfo(int index, pose_info->pos_coeffs = coeffs.head<3>(); pose_info->rot_coeffs = coeffs.tail<3>(); } + else + { + throw std::runtime_error("Invalid coeffs size for Cartesian waypoint term info. Expected 1 or 6, got " + + std::to_string(coeffs.size())); + } if (lower_tolerance.size() == 1) { @@ -106,6 +111,11 @@ std::shared_ptr createCartesianWaypointTermInfo(int index, { pose_info->lower_tolerance = lower_tolerance; } + else + { + throw std::runtime_error("Invalid lower tolerance size for Cartesian waypoint term info. Expected 1 or 6, got " + + std::to_string(lower_tolerance.size())); + } if (upper_tolerance.size() == 1) { @@ -115,6 +125,11 @@ std::shared_ptr createCartesianWaypointTermInfo(int index, { pose_info->upper_tolerance = upper_tolerance; } + else + { + throw std::runtime_error("Invalid upper tolerance size for Cartesian waypoint term info. Expected 1 or 6, got " + + std::to_string(upper_tolerance.size())); + } return pose_info; } @@ -150,6 +165,11 @@ std::shared_ptr createDynamicCartesianWaypointTermInfo(int in pose->pos_coeffs = coeffs.head<3>(); pose->rot_coeffs = coeffs.tail<3>(); } + else + { + throw std::runtime_error("Invalid coeffs size for Cartesian waypoint term info. Expected 1 or 6, got " + + std::to_string(coeffs.size())); + } pose->lower_tolerance = lower_tolerance; pose->upper_tolerance = upper_tolerance; @@ -187,9 +207,18 @@ std::shared_ptr createJointWaypointTermInfo(const Eigen::Vect { auto joint_info = std::make_shared(); if (coeffs.size() == 1) + { joint_info->coeffs = std::vector(static_cast(j_wp.size()), coeffs(0)); + } else if (coeffs.size() == j_wp.size()) + { joint_info->coeffs = std::vector(coeffs.data(), coeffs.data() + coeffs.rows() * coeffs.cols()); + } + else + { + throw std::runtime_error("Invalid coeffs size for joint waypoint term info. Expected 1 or " + + std::to_string(j_wp.size()) + ", got " + std::to_string(coeffs.size())); + } joint_info->targets = std::vector(j_wp.data(), j_wp.data() + j_wp.rows() * j_wp.cols()); joint_info->first_step = index; @@ -209,9 +238,18 @@ std::shared_ptr createTolerancedJointWaypointTermInfo(const E { auto joint_info = std::make_shared(); if (coeffs.size() == 1) + { joint_info->coeffs = std::vector(static_cast(j_wp.size()), coeffs(0)); + } else if (coeffs.size() == j_wp.size()) + { joint_info->coeffs = std::vector(coeffs.data(), coeffs.data() + coeffs.rows() * coeffs.cols()); + } + else + { + throw std::runtime_error("Invalid coeffs size for joint waypoint term info. Expected 1 or " + + std::to_string(j_wp.size()) + ", got " + std::to_string(coeffs.size())); + } joint_info->targets = std::vector(j_wp.data(), j_wp.data() + j_wp.rows() * j_wp.cols()); joint_info->lower_tols = From 03c699db9142f7f5a60ab5f184653ac2dd314830 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Fri, 25 Apr 2025 19:42:02 +0200 Subject: [PATCH 4/4] Allow zero size tolerances --- .../trajopt/src/trajopt_utils.cpp | 50 +++++++++++-------- 1 file changed, 28 insertions(+), 22 deletions(-) diff --git a/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp b/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp index 8ab9a61fbf7..0f1606f8c6f 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp @@ -103,32 +103,38 @@ std::shared_ptr createCartesianWaypointTermInfo(int index, std::to_string(coeffs.size())); } - if (lower_tolerance.size() == 1) + if (lower_tolerance.size() > 0) { - pose_info->lower_tolerance = Eigen::VectorXd::Constant(6, lower_tolerance(0)); - } - else if (lower_tolerance.size() == 6) - { - pose_info->lower_tolerance = lower_tolerance; - } - else - { - throw std::runtime_error("Invalid lower tolerance size for Cartesian waypoint term info. Expected 1 or 6, got " + - std::to_string(lower_tolerance.size())); + if (lower_tolerance.size() == 1) + { + pose_info->lower_tolerance = Eigen::VectorXd::Constant(6, lower_tolerance(0)); + } + else if (lower_tolerance.size() == 6) + { + pose_info->lower_tolerance = lower_tolerance; + } + else + { + throw std::runtime_error("Invalid lower tolerance size for Cartesian waypoint term info. Expected 1 or 6, got " + + std::to_string(lower_tolerance.size())); + } } - if (upper_tolerance.size() == 1) + if (upper_tolerance.size() > 0) { - pose_info->upper_tolerance = Eigen::VectorXd::Constant(6, upper_tolerance(0)); - } - else if (upper_tolerance.size() == 6) - { - pose_info->upper_tolerance = upper_tolerance; - } - else - { - throw std::runtime_error("Invalid upper tolerance size for Cartesian waypoint term info. Expected 1 or 6, got " + - std::to_string(upper_tolerance.size())); + if (upper_tolerance.size() == 1) + { + pose_info->upper_tolerance = Eigen::VectorXd::Constant(6, upper_tolerance(0)); + } + else if (upper_tolerance.size() == 6) + { + pose_info->upper_tolerance = upper_tolerance; + } + else + { + throw std::runtime_error("Invalid upper tolerance size for Cartesian waypoint term info. Expected 1 or 6, got " + + std::to_string(upper_tolerance.size())); + } } return pose_info;