diff --git a/tesseract_examples/src/basic_cartesian_example.cpp b/tesseract_examples/src/basic_cartesian_example.cpp index 5fe07d2b457..d8ba5d27b11 100644 --- a/tesseract_examples/src/basic_cartesian_example.cpp +++ b/tesseract_examples/src/basic_cartesian_example.cpp @@ -231,8 +231,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 83be6b007fb..113e61026d6 100644 --- a/tesseract_examples/src/glass_upright_example.cpp +++ b/tesseract_examples/src/glass_upright_example.cpp @@ -221,10 +221,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 c37d8683597..91b4034f763 100644 --- a/tesseract_examples/src/pick_and_place_example.cpp +++ b/tesseract_examples/src/pick_and_place_example.cpp @@ -193,7 +193,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 2126b982d95..757df7c407f 100644 --- a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp +++ b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp @@ -197,10 +197,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 9d591edc81a..a63c6931f50 100644 --- a/tesseract_examples/src/puzzle_piece_example.cpp +++ b/tesseract_examples/src/puzzle_piece_example.cpp @@ -193,8 +193,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 bed11643809..5d5440e2ec1 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