From a69dffe0cb3dcb05be0c6e2480d4fbe7eee4cb05 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Fri, 13 Dec 2024 11:11:06 -0600 Subject: [PATCH] Update descartes profiles --- .../descartes/CMakeLists.txt | 5 +- .../descartes/descartes_motion_planner.h | 3 - .../descartes/descartes_problem.h | 70 ---- .../descartes/deserialize.h | 56 --- .../impl/descartes_motion_planner.hpp | 207 +++------- .../descartes_default_plan_profile.hpp | 376 ++++-------------- ...descartes_ladder_graph_solver_profile.hpp} | 39 +- .../impl/profile/descartes_profile.hpp | 55 +++ .../profile/descartes_default_plan_profile.h | 49 +-- .../descartes_ladder_graph_solver_profile.h | 60 +++ .../descartes/profile/descartes_profile.h | 65 ++- .../descartes/types.h | 66 --- .../descartes/src/deserialize.cpp | 126 ------ .../descartes_default_plan_profile.cpp | 11 +- .../descartes_ladder_graph_solver_profile.cpp | 50 +++ .../src/profile/descartes_profile.cpp | 12 + .../descartes/src/serialize.cpp | 80 ---- .../test/descartes_planner_tests.cpp | 77 ++-- 18 files changed, 438 insertions(+), 969 deletions(-) delete mode 100644 tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_problem.h delete mode 100644 tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/deserialize.h rename tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/{serialize.h => impl/profile/descartes_ladder_graph_solver_profile.hpp} (51%) create mode 100644 tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/profile/descartes_profile.hpp create mode 100644 tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_ladder_graph_solver_profile.h delete mode 100644 tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/types.h delete mode 100644 tesseract_motion_planners/descartes/src/deserialize.cpp create mode 100644 tesseract_motion_planners/descartes/src/profile/descartes_ladder_graph_solver_profile.cpp delete mode 100644 tesseract_motion_planners/descartes/src/serialize.cpp diff --git a/tesseract_motion_planners/descartes/CMakeLists.txt b/tesseract_motion_planners/descartes/CMakeLists.txt index 5eafd22e6a..9f94ee7b32 100644 --- a/tesseract_motion_planners/descartes/CMakeLists.txt +++ b/tesseract_motion_planners/descartes/CMakeLists.txt @@ -8,11 +8,10 @@ add_library( src/descartes_collision.cpp src/descartes_collision_edge_evaluator.cpp src/descartes_robot_sampler.cpp - src/serialize.cpp - src/deserialize.cpp src/descartes_utils.cpp src/profile/descartes_profile.cpp - src/profile/descartes_default_plan_profile.cpp) + src/profile/descartes_default_plan_profile.cpp + src/profile/descartes_ladder_graph_solver_profile.cpp) target_link_libraries( ${PROJECT_NAME}_descartes PUBLIC ${PROJECT_NAME}_core diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_motion_planner.h b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_motion_planner.h index fc5522ac30..63518795f1 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_motion_planner.h +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_motion_planner.h @@ -2,7 +2,6 @@ #define TESSERACT_MOTION_PLANNERS_DECARTES_MOTION_PLANNER_H #include -#include namespace tesseract_planning { @@ -25,8 +24,6 @@ class DescartesMotionPlanner : public MotionPlanner void clear() override; std::unique_ptr clone() const override; - - virtual std::shared_ptr> createProblem(const PlannerRequest& request) const; }; using DescartesMotionPlannerD = DescartesMotionPlanner; diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_problem.h b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_problem.h deleted file mode 100644 index aaa6c4d489..0000000000 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_problem.h +++ /dev/null @@ -1,70 +0,0 @@ -/** - * @file descartes_problem.h - * @brief - * - * @author Levi Armstrong - * @date June 18, 2020 - * @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. - */ -#ifndef TESSERACT_MOTION_PLANNERS_DESCARTES_PROBLEM_H -#define TESSERACT_MOTION_PLANNERS_DESCARTES_PROBLEM_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include - -#include - -namespace tesseract_planning -{ -template -struct DescartesProblem -{ - // LCOV_EXCL_START - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - // LCOV_EXCL_STOP - - // These are required for Tesseract to configure Descartes - std::shared_ptr env; - tesseract_scene_graph::SceneState env_state; - - // Kinematic Objects - std::shared_ptr manip; - - // These are required for descartes - std::vector::ConstPtr> edge_evaluators{}; - std::vector::ConstPtr> samplers{}; - std::vector::ConstPtr> state_evaluators{}; - int num_threads = static_cast(std::thread::hardware_concurrency()); -}; -using DescartesProblemF = DescartesProblem; -using DescartesProblemD = DescartesProblem; - -} // namespace tesseract_planning - -#endif // TESSERACT_MOTION_PLANNERS_DESCARTES_PROBLEM_H diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/deserialize.h b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/deserialize.h deleted file mode 100644 index d3d6325c77..0000000000 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/deserialize.h +++ /dev/null @@ -1,56 +0,0 @@ -/** - * @file deserialize.h - * @brief Provide methods for deserialize descartes plans to xml - * - * @author Tyler Marr - * @date August 25, 2020 - * @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. - */ -#ifndef TESSERACT_MOTION_PLANNERS_DESCARTES_DESERIALIZE_H -#define TESSERACT_MOTION_PLANNERS_DESCARTES_DESERIALIZE_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include - -namespace tinyxml2 -{ -class XMLElement; // NOLINT -class XMLDocument; -} // namespace tinyxml2 - -namespace tesseract_planning -{ -DescartesDefaultPlanProfile descartesPlanParser(const tinyxml2::XMLElement& xml_element); - -DescartesDefaultPlanProfile descartesPlanFromXMLElement(const tinyxml2::XMLElement* profile_xml); - -DescartesDefaultPlanProfile descartesPlanFromXMLDocument(const tinyxml2::XMLDocument& xml_doc); - -DescartesDefaultPlanProfile descartesPlanFromXMLFile(const std::string& file_path); - -DescartesDefaultPlanProfile descartesPlanFromXMLString(const std::string& xml_string); - -} // namespace tesseract_planning - -#endif // TESSERACT_MOTION_PLANNERS_DESCARTES_DESERIALIZE_H diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp index ade84d6c14..b18f31cc5b 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp @@ -40,6 +40,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include #include #include @@ -60,35 +61,67 @@ template PlannerResponse DescartesMotionPlanner::solve(const PlannerRequest& request) const { PlannerResponse response; - std::shared_ptr> problem; - if (request.data) - { - problem = std::static_pointer_cast>(request.data); - } - else + + // Get solver config + auto solver_profile = + getProfile>(name_, + request.instructions.getProfile(name_), + *request.profiles, + std::make_shared>()); + + auto solver = solver_profile->create(); + + std::vector::ConstPtr> edge_evaluators; + std::vector::ConstPtr> waypoint_samplers; + std::vector::ConstPtr> state_evaluators; + + const tesseract_common::ManipulatorInfo& composite_mi = request.instructions.getManipulatorInfo(); + if (composite_mi.empty()) + throw std::runtime_error("Descartes, manipulator info is empty!"); + + // Flatten the input for planning + auto move_instructions = request.instructions.flatten(&moveFilter); + + // Transform plan instructions into descartes samplers + int index = 0; + for (const auto& instruction : move_instructions) { - try - { - problem = createProblem(request); - } - catch (std::exception& e) - { - CONSOLE_BRIDGE_logError("DescartesMotionPlanner failed to generate problem: %s.", e.what()); - response.successful = false; - response.message = ERROR_INVALID_INPUT; - return response; - } + assert(instruction.get().isMoveInstruction()); + const auto& move_instruction = instruction.get().template as(); + + // If plan instruction has manipulator information then use it over the one provided by the composite. + tesseract_common::ManipulatorInfo manip_info = composite_mi.getCombined(move_instruction.getManipulatorInfo()); + + if (manip_info.empty()) + throw std::runtime_error("Descartes, manipulator info is empty!"); - response.data = problem; + // Get Plan Profile + auto cur_plan_profile = + getProfile>(name_, + move_instruction.getProfile(name_), + *request.profiles, + std::make_shared>()); + + if (!cur_plan_profile) + throw std::runtime_error("DescartesMotionPlanner: Invalid profile"); + + if (move_instruction.getWaypoint().isJointWaypoint() && + !move_instruction.getWaypoint().as().isConstrained()) + continue; + + waypoint_samplers.push_back(cur_plan_profile->createWaypointSampler(move_instruction, manip_info, request.env)); + state_evaluators.push_back(cur_plan_profile->createStateEvaluator(move_instruction, manip_info, request.env)); + if (index != 0) + edge_evaluators.push_back(cur_plan_profile->createEdgeEvaluator(move_instruction, manip_info, request.env)); + + ++index; } descartes_light::SearchResult descartes_result; try { - descartes_light::LadderGraphSolver solver(problem->num_threads); - // Build Graph - if (!solver.build(problem->samplers, problem->edge_evaluators, problem->state_evaluators)) + if (!solver->build(waypoint_samplers, edge_evaluators, state_evaluators)) { response.successful = false; response.message = ERROR_FAILED_TO_BUILD_GRAPH; @@ -96,7 +129,7 @@ PlannerResponse DescartesMotionPlanner::solve(const PlannerRequest& r } // Search Graph - descartes_result = solver.search(); + descartes_result = solver->search(); if (descartes_result.trajectory.empty()) { CONSOLE_BRIDGE_logError("Search for graph completion failed"); @@ -107,26 +140,17 @@ PlannerResponse DescartesMotionPlanner::solve(const PlannerRequest& r } catch (...) { - // CONSOLE_BRIDGE_logError("Failed to build vertices"); - // for (const auto& i : graph_builder.getFailedVertices()) - // response.failed_waypoints.push_back(config_->waypoints[i]); - - // // Copy the waypoint if it is not already in the failed waypoints list - // std::copy_if(config_->waypoints.begin(), - // config_->waypoints.end(), - // std::back_inserter(response.succeeded_waypoints), - // [&response](const Waypoint::ConstPtr wp) { - // return std::find(response.failed_waypoints.begin(), response.failed_waypoints.end(), wp) == - // response.failed_waypoints.end(); - // }); - response.successful = false; response.message = ERROR_FAILED_TO_BUILD_GRAPH; return response; } - const std::vector joint_names = problem->manip->getJointNames(); - const Eigen::MatrixX2d joint_limits = problem->manip->getLimits().joint_limits; + response.data = std::move(solver); + + // Get Manipulator Information + tesseract_kinematics::JointGroup::UPtr manip = request.env->getJointGroup(composite_mi.manipulator); + const std::vector joint_names = manip->getJointNames(); + const Eigen::MatrixX2d joint_limits = manip->getLimits().joint_limits; // Enforce limits std::vector solution{}; @@ -222,114 +246,5 @@ std::unique_ptr DescartesMotionPlanner::clone() const return std::make_unique>(name_); } -template -std::shared_ptr> -DescartesMotionPlanner::createProblem(const PlannerRequest& request) const -{ - auto prob = std::make_shared>(); - - // Clear descartes data - prob->edge_evaluators.clear(); - prob->samplers.clear(); - prob->state_evaluators.clear(); - - // Assume all the plan instructions have the same manipulator as the composite - assert(!request.instructions.getManipulatorInfo().empty()); - const tesseract_common::ManipulatorInfo& composite_mi = request.instructions.getManipulatorInfo(); - - if (composite_mi.manipulator.empty()) - throw std::runtime_error("Descartes, manipulator is empty!"); - - // Get Manipulator Information - try - { - if (composite_mi.manipulator_ik_solver.empty()) - prob->manip = request.env->getKinematicGroup(composite_mi.manipulator); - else - prob->manip = request.env->getKinematicGroup(composite_mi.manipulator, composite_mi.manipulator_ik_solver); - } - catch (...) - { - throw std::runtime_error("Descartes problem generator failed to create kinematic group!"); - } - - if (!prob->manip) - { - CONSOLE_BRIDGE_logError("No Kinematics Group found"); - return prob; - } - - prob->env = request.env; - prob->env_state = request.env->getState(); - - std::vector joint_names = prob->manip->getJointNames(); - - // Flatten the input for planning - auto move_instructions = request.instructions.flatten(&moveFilter); - - // Transform plan instructions into descartes samplers - int index = 0; - for (const auto& move_instruction : move_instructions) - { - const auto& instruction = move_instruction.get(); - - assert(instruction.isMoveInstruction()); - const auto& plan_instruction = instruction.template as(); - - // If plan instruction has manipulator information then use it over the one provided by the composite. - tesseract_common::ManipulatorInfo mi = composite_mi.getCombined(plan_instruction.getManipulatorInfo()); - - if (mi.manipulator.empty()) - throw std::runtime_error("Descartes, manipulator is empty!"); - - if (mi.tcp_frame.empty()) - throw std::runtime_error("Descartes, tcp_frame is empty!"); - - if (mi.working_frame.empty()) - throw std::runtime_error("Descartes, working_frame is empty!"); - - // Get Plan Profile - auto cur_plan_profile = - getProfile>(name_, - plan_instruction.getProfile(name_), - *request.profiles, - std::make_shared>()); - - if (!cur_plan_profile) - throw std::runtime_error("DescartesMotionPlanner: Invalid profile"); - - if (plan_instruction.getWaypoint().isCartesianWaypoint()) - { - const auto& cur_wp = plan_instruction.getWaypoint().template as(); - cur_plan_profile->apply(*prob, cur_wp.getTransform(), plan_instruction, composite_mi, index); - ++index; - } - else if (plan_instruction.getWaypoint().isJointWaypoint()) - { - if (plan_instruction.getWaypoint().as().isConstrained()) - { - assert(checkJointPositionFormat(joint_names, plan_instruction.getWaypoint())); - const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction.getWaypoint()); - cur_plan_profile->apply(*prob, cur_position, plan_instruction, composite_mi, index); - ++index; - } - } - else if (plan_instruction.getWaypoint().isStateWaypoint()) - { - assert(checkJointPositionFormat(joint_names, plan_instruction.getWaypoint())); - const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction.getWaypoint()); - cur_plan_profile->apply(*prob, cur_position, plan_instruction, composite_mi, index); - ++index; - } - else - { - throw std::runtime_error("DescartesMotionPlanner: unknown waypoint type"); - } - } - - // Call the base class generate which checks the problem to make sure everything is in order - return prob; -} - } // namespace tesseract_planning #endif // TESSERACT_MOTION_PLANNERS_IMPL_DESCARTES_DECARTES_MOTION_PLANNER_HPP diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/profile/descartes_default_plan_profile.hpp b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/profile/descartes_default_plan_profile.hpp index ba298bee38..2b83f4af78 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/profile/descartes_default_plan_profile.hpp +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/profile/descartes_default_plan_profile.hpp @@ -28,166 +28,68 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include #include #include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include +#include #include #include #include +#include #include #include #include #include #include +#include +#include namespace tesseract_planning { template -DescartesDefaultPlanProfile::DescartesDefaultPlanProfile(const tinyxml2::XMLElement& xml_element) +std::unique_ptr DescartesDefaultPlanProfile::createVertexEvaluator( + const MoveInstructionPoly& /*move_instruction*/, + const std::shared_ptr& manip, + const std::shared_ptr& /*env*/) const { - const tinyxml2::XMLElement* vertex_collisions_element = xml_element.FirstChildElement("VertexCollisions"); - const tinyxml2::XMLElement* edge_collisions_element = xml_element.FirstChildElement("EdgeCollisions"); - const tinyxml2::XMLElement* num_threads_element = xml_element.FirstChildElement("NumberThreads"); - const tinyxml2::XMLElement* allow_collisions_element = xml_element.FirstChildElement("AllowCollisions"); - const tinyxml2::XMLElement* debug_element = xml_element.FirstChildElement("Debug"); - - int status{ tinyxml2::XMLError::XML_SUCCESS }; - - if (vertex_collisions_element != nullptr) - { - const tinyxml2::XMLElement* enabled_element = vertex_collisions_element->FirstChildElement("Enabled"); - // const tinyxml2::XMLElement* coll_safety_margin_element = - // vertex_collisions_element->FirstChildElement("CollisionSaf" - // "etyMargin"); - - if (enabled_element != nullptr) - { - status = enabled_element->QueryBoolText(&enable_collision); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("DescartesPlanProfile: VertexCollisions: Error parsing Enabled string"); - } - - /** @todo Update XML */ - // if (coll_safety_margin_element) - // { - // std::string coll_safety_margin_string; - // status = tesseract_common::QueryStringText(coll_safety_margin_element, coll_safety_margin_string); - // if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - // throw std::runtime_error("DescartesPlanProfile: VertexCollisions:: Error parsing CollisionSafetyMargin - // string"); - - // if (!tesseract_common::isNumeric(coll_safety_margin_string)) - // throw std::runtime_error("DescartesPlanProfile: VertexCollisions:: CollisionSafetyMargin is not a numeric - // " - // "values."); - - // tesseract_common::toNumeric(coll_safety_margin_string, collision_safety_margin); - // } - } - - if (edge_collisions_element != nullptr) - { - const tinyxml2::XMLElement* enabled_element = edge_collisions_element->FirstChildElement("Enabled"); - const tinyxml2::XMLElement* coll_safety_margin_element = edge_collisions_element->FirstChildElement("CollisionSafet" - "yMargin"); - const tinyxml2::XMLElement* long_valid_seg_len_element = edge_collisions_element->FirstChildElement("LongestValidSe" - "gmentLength"); - - if (enabled_element != nullptr) - { - status = enabled_element->QueryBoolText(&enable_edge_collision); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("DescartesPlanProfile: EdgeCollisions: Error parsing Enabled string"); - } - - if (coll_safety_margin_element != nullptr) - { - std::string coll_safety_margin_string; - status = tesseract_common::QueryStringText(coll_safety_margin_element, coll_safety_margin_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("DescartesPlanProfile: EdgeCollisions: Error parsing CollisionSafetyMargin string"); - - if (!tesseract_common::isNumeric(coll_safety_margin_string)) - throw std::runtime_error("DescartesPlanProfile: EdgeCollisions: CollisionSafetyMargin is not a numeric " - "values."); - - /** @todo Update XML */ - // tesseract_common::toNumeric(coll_safety_margin_string, edge_collision_saftey_margin); - } - - if (long_valid_seg_len_element != nullptr) - { - std::string long_valid_seg_len_string; - status = tesseract_common::QueryStringText(long_valid_seg_len_element, long_valid_seg_len_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("DescartesPlanProfile: EdgeCollisions: Error parsing LongestValidSegmentLength " - "string"); - - if (!tesseract_common::isNumeric(long_valid_seg_len_string)) - throw std::runtime_error("DescartesPlanProfile: EdgeCollisions: LongestValidSegmentLength is not a numeric " - "values."); - - /** @todo Update XML */ - // tesseract_common::toNumeric(long_valid_seg_len_string, edge_longest_valid_segment_length); - } - } - - if (num_threads_element != nullptr) - { - std::string num_threads_string; - status = tesseract_common::QueryStringText(num_threads_element, num_threads_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("DescartesPlanProfile: Error parsing NumberThreads string"); - - if (!tesseract_common::isNumeric(num_threads_string)) - throw std::runtime_error("DescartesPlanProfile: NumberThreads is not a numeric values."); - - tesseract_common::toNumeric(num_threads_string, num_threads); - } - - if (allow_collisions_element != nullptr) - { - status = allow_collisions_element->QueryBoolText(&allow_collision); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("DescartesPlanProfile: Error parsing AllowCollisions string"); - } - - if (debug_element != nullptr) - { - status = debug_element->QueryBoolText(&debug); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("DescartesPlanProfile: Error parsing Debug string"); - } + return std::make_unique(manip->getLimits().joint_limits); } template -void DescartesDefaultPlanProfile::apply(DescartesProblem& prob, - const Eigen::Isometry3d& cartesian_waypoint, - const InstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - int index) const +PoseSamplerFn DescartesDefaultPlanProfile::createPoseSampler( + const MoveInstructionPoly& /*move_instruction*/, + const std::shared_ptr& /*manip*/, + const std::shared_ptr& /*env*/) const { - assert(parent_instruction.isMoveInstruction()); - const auto& base_instruction = parent_instruction.as(); - assert(!(manip_info.empty() && base_instruction.getManipulatorInfo().empty())); - tesseract_common::ManipulatorInfo mi = manip_info.getCombined(base_instruction.getManipulatorInfo()); - - if (mi.manipulator.empty()) - throw std::runtime_error("Descartes, manipulator is empty!"); + if (target_pose_fixed) + return sampleFixed; - if (mi.tcp_frame.empty()) - throw std::runtime_error("Descartes, tcp_frame is empty!"); + return [axis = target_pose_sample_axis, resolution = target_pose_sample_resolution]( + const Eigen::Isometry3d& tool_pose) { return sampleToolAxis(tool_pose, resolution, axis); }; +} - if (mi.working_frame.empty()) - throw std::runtime_error("Descartes, working_frame is empty!"); +template +std::unique_ptr> +DescartesDefaultPlanProfile::createWaypointSampler( + const MoveInstructionPoly& move_instruction, + const tesseract_common::ManipulatorInfo& manip_info, + const std::shared_ptr& env) const +{ + auto manip = DescartesPlanProfile::createKinematicGroup(manip_info, *env); + if (!move_instruction.getWaypoint().isCartesianWaypoint()) + { + assert(checkJointPositionFormat(manip->getJointNames(), move_instruction.getWaypoint())); + const Eigen::VectorXd& joint_waypoint = getJointPosition(move_instruction.getWaypoint()); + auto state = std::make_shared>(joint_waypoint.cast()); + return std::make_unique>(state); + } - Eigen::Isometry3d tcp_offset = prob.env->findTCPOffset(mi); - std::vector joint_names = prob.manip->getJointNames(); + Eigen::Isometry3d tcp_offset = env->findTCPOffset(manip_info); /* Check if this cartesian waypoint is dynamic * (i.e. defined relative to a frame that will move with the kinematic chain) */ @@ -199,182 +101,64 @@ void DescartesDefaultPlanProfile::apply(DescartesProblem& DescartesCollision::Ptr ci = nullptr; if (enable_collision) - ci = std::make_shared(*prob.env, prob.manip, vertex_collision_check_config, debug); - - // Add vertex evaluator - std::shared_ptr> sampler; - if (vertex_evaluator == nullptr) - { - auto ve = std::make_shared(prob.manip->getLimits().joint_limits); - sampler = std::make_shared>(mi.working_frame, - cartesian_waypoint, - target_pose_sampler, - prob.manip, - ci, - mi.tcp_frame, - tcp_offset, - allow_collision, - ve, - use_redundant_joint_solutions); - } - else - { - sampler = std::make_shared>(mi.working_frame, - cartesian_waypoint, - target_pose_sampler, - prob.manip, - ci, - mi.tcp_frame, - tcp_offset, - allow_collision, - vertex_evaluator(prob), - use_redundant_joint_solutions); - } - prob.samplers.push_back(std::move(sampler)); - - if (index != 0) - { - // Add edge Evaluator - if (edge_evaluator == nullptr) - { - if (enable_edge_collision) - { - auto compound_evaluator = std::make_shared>(); - compound_evaluator->evaluators.push_back( - std::make_shared>()); - compound_evaluator->evaluators.push_back(std::make_shared>( - *prob.env, prob.manip, edge_collision_check_config, allow_collision, debug)); - prob.edge_evaluators.push_back(compound_evaluator); - } - else - { - prob.edge_evaluators.push_back(std::make_shared>()); - } - } - else - { - if (enable_edge_collision) - { - auto compound_evaluator = std::make_shared>(); - compound_evaluator->evaluators.push_back(edge_evaluator(prob)); - compound_evaluator->evaluators.push_back(std::make_shared>( - *prob.env, prob.manip, edge_collision_check_config, allow_collision, debug)); - prob.edge_evaluators.push_back(compound_evaluator); - } - else - { - prob.edge_evaluators.push_back(edge_evaluator(prob)); - } - } - } - - // Add state evaluator - if (state_evaluator != nullptr) - prob.state_evaluators.push_back(state_evaluator(prob)); - else - prob.state_evaluators.push_back(std::make_shared>()); - - prob.num_threads = num_threads; + ci = std::make_shared(*env, manip, vertex_collision_check_config, debug); + + auto ve = createVertexEvaluator(move_instruction, manip, env); + auto pose_sampler = createPoseSampler(move_instruction, manip, env); + return std::make_unique>( + manip_info.working_frame, + move_instruction.getWaypoint().as().getTransform(), + pose_sampler, + manip, + ci, + manip_info.tcp_frame, + tcp_offset, + allow_collision, + std::move(ve), + use_redundant_joint_solutions); } template -void DescartesDefaultPlanProfile::apply(DescartesProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const InstructionPoly& /*parent_instruction*/, - const tesseract_common::ManipulatorInfo& /*manip_info*/, - int index) const +std::unique_ptr> DescartesDefaultPlanProfile::createEdgeEvaluator( + const MoveInstructionPoly& move_instruction, + const tesseract_common::ManipulatorInfo& manip_info, + const std::shared_ptr& env) const { - auto state = std::make_shared>(joint_waypoint.cast()); - auto sampler = std::make_shared>(state); - prob.samplers.push_back(std::move(sampler)); + auto manip = DescartesPlanProfile::createKinematicGroup(manip_info, *env); + if (move_instruction.getWaypoint().isCartesianWaypoint()) + { + if (!enable_edge_collision) + return std::make_unique>(); - std::vector joint_names = prob.manip->getJointNames(); + auto compound_evaluator = std::make_unique>(); + compound_evaluator->evaluators.push_back( + std::make_shared>()); + compound_evaluator->evaluators.push_back(std::make_shared>( + *env, manip, edge_collision_check_config, allow_collision, debug)); - if (index != 0) - { - // Add edge Evaluator - if (edge_evaluator == nullptr) - { - if (enable_edge_collision) - { - auto compound_evaluator = std::make_shared>(); - compound_evaluator->evaluators.push_back( - std::make_shared>()); - compound_evaluator->evaluators.push_back(std::make_shared>( - *prob.env, prob.manip, edge_collision_check_config, allow_collision, debug)); - prob.edge_evaluators.push_back(compound_evaluator); - } - else - { - prob.edge_evaluators.push_back(std::make_shared>()); - } - } - else - { - prob.edge_evaluators.push_back(edge_evaluator(prob)); - } + return compound_evaluator; } - if (state_evaluator != nullptr) - prob.state_evaluators.push_back(state_evaluator(prob)); - else - prob.state_evaluators.push_back(std::make_shared>()); + if (!enable_edge_collision) + return std::make_unique>(); - prob.num_threads = num_threads; + auto compound_evaluator = std::make_unique>(); + compound_evaluator->evaluators.push_back( + std::make_shared>()); + compound_evaluator->evaluators.push_back(std::make_shared>( + *env, manip, edge_collision_check_config, allow_collision, debug)); + + return compound_evaluator; } template -tinyxml2::XMLElement* DescartesDefaultPlanProfile::toXML(tinyxml2::XMLDocument& doc) const +std::unique_ptr> +DescartesDefaultPlanProfile::createStateEvaluator( + const MoveInstructionPoly& /*move_instruction*/, + const tesseract_common::ManipulatorInfo& /*manip_info*/, + const std::shared_ptr& /*env*/) const { - tinyxml2::XMLElement* xml_planner = doc.NewElement("Planner"); - xml_planner->SetAttribute("type", std::to_string(3).c_str()); - - tinyxml2::XMLElement* xml_descartes = doc.NewElement("DescartesPlanProfile"); - - tinyxml2::XMLElement* vertex_collisions = doc.NewElement("VertexCollisions"); - tinyxml2::XMLElement* vertex_collisions_enabled = doc.NewElement("Enabled"); - vertex_collisions_enabled->SetText(enable_collision); - vertex_collisions->InsertEndChild(vertex_collisions_enabled); - - // tinyxml2::XMLElement* vertex_collisions_safety_margin = doc.NewElement("CollisionSafetyMargin"); - // vertex_collisions_safety_margin->SetText(collision_safety_margin); - // vertex_collisions->InsertEndChild(vertex_collisions_safety_margin); - - xml_descartes->InsertEndChild(vertex_collisions); - - tinyxml2::XMLElement* edge_collisions = doc.NewElement("EdgeCollisions"); - tinyxml2::XMLElement* edge_collisions_enabled = doc.NewElement("Enabled"); - edge_collisions_enabled->SetText(enable_edge_collision); - edge_collisions->InsertEndChild(edge_collisions_enabled); - - /** @todo Update XML */ - // tinyxml2::XMLElement* edge_collisions_safety_margin = doc.NewElement("CollisionSafetyMargin"); - // edge_collisions_safety_margin->SetText(edge_collision_saftey_margin); - // edge_collisions->InsertEndChild(edge_collisions_safety_margin); - - // tinyxml2::XMLElement* edge_collisions_long_valid_seg_len = doc.NewElement("LongestValidSegmentLength"); - // edge_collisions_long_valid_seg_len->SetText(edge_longest_valid_segment_length); - // edge_collisions->InsertEndChild(edge_collisions_long_valid_seg_len); - - xml_descartes->InsertEndChild(edge_collisions); - - tinyxml2::XMLElement* number_threads = doc.NewElement("NumberThreads"); - number_threads->SetText(num_threads); - xml_descartes->InsertEndChild(number_threads); - - tinyxml2::XMLElement* allow_collision_element = doc.NewElement("AllowCollisions"); - allow_collision_element->SetText(allow_collision); - xml_descartes->InsertEndChild(allow_collision_element); - - tinyxml2::XMLElement* debug_element = doc.NewElement("Debug"); - debug_element->SetText(debug); - xml_descartes->InsertEndChild(debug_element); - - xml_planner->InsertEndChild(xml_descartes); - - // TODO: Add Edge Evaluator and IsValidFn? - - return xml_planner; + return std::make_unique>(); } } // namespace tesseract_planning diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/serialize.h b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/profile/descartes_ladder_graph_solver_profile.hpp similarity index 51% rename from tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/serialize.h rename to tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/profile/descartes_ladder_graph_solver_profile.hpp index 44e485ed44..35e2602000 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/serialize.h +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/profile/descartes_ladder_graph_solver_profile.hpp @@ -1,9 +1,9 @@ /** - * @file serialize.h - * @brief Provide methods for serializing descartes plans to xml + * @file descartes_ladder_graph_solver_profile.hpp + * @brief * - * @author Tyler Marr - * @date August 25, 2020 + * @author Levi Armstrong + * @date June 18, 2020 * @version TODO * @bug No known bugs * @@ -23,29 +23,20 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TESSERACT_MOTION_PLANNERS_DESCARTES_SERIALIZE_H -#define TESSERACT_MOTION_PLANNERS_DESCARTES_SERIALIZE_H +#ifndef TESSERACT_MOTION_PLANNERS_DESCARTES_IMPL_DESCARTES_DESCARTES_LADDER_GRAPH_SOLVER_PROFILE_HPP +#define TESSERACT_MOTION_PLANNERS_DESCARTES_IMPL_DESCARTES_DESCARTES_LADDER_GRAPH_SOLVER_PROFILE_HPP -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include - -namespace tinyxml2 -{ -class XMLElement; // NOLINT -class XMLDocument; -} // namespace tinyxml2 +#include +#include namespace tesseract_planning { -std::shared_ptr toXMLDocument(const DescartesPlanProfile& plan_profile); - -bool toXMLFile(const DescartesPlanProfile& plan_profile, const std::string& file_path); +template +std::unique_ptr> DescartesLadderGraphSolverProfile::create() const +{ + return std::make_unique>(num_threads); +} -std::string toXMLString(const DescartesPlanProfile& plan_profile); } // namespace tesseract_planning -#endif // TESSERACT_MOTION_PLANNERS_DESCARTES_SERIALIZE_H + +#endif // TESSERACT_MOTION_PLANNERS_DESCARTES_IMPL_DESCARTES_DESCARTES_LADDER_GRAPH_SOLVER_PROFILE_HPP diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/profile/descartes_profile.hpp b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/profile/descartes_profile.hpp new file mode 100644 index 0000000000..c76f932f00 --- /dev/null +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/profile/descartes_profile.hpp @@ -0,0 +1,55 @@ +/** + * @file descartes_profile.h + * @brief + * + * @author Levi Armstrong + * @date June 18, 2020 + * @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. + */ +#ifndef TESSERACT_MOTION_PLANNERS_DESCARTES_IMPL_DESCARTES_PROFILE_HPP +#define TESSERACT_MOTION_PLANNERS_DESCARTES_IMPL_DESCARTES_PROFILE_HPP +#include +#include +#include +#include + +namespace tesseract_planning +{ +template +std::shared_ptr +DescartesPlanProfile::createKinematicGroup(const tesseract_common::ManipulatorInfo& manip_info, + const tesseract_environment::Environment& env) const +{ + // Get Manipulator Information + try + { + if (manip_info.manipulator_ik_solver.empty()) + return env.getKinematicGroup(manip_info.manipulator); + + return env.getKinematicGroup(manip_info.manipulator, manip_info.manipulator_ik_solver); + } + catch (...) + { + throw std::runtime_error("Descartes problem generator failed to create kinematic group!"); + } +} + +} // namespace tesseract_planning +#endif // TESSERACT_MOTION_PLANNERS_DESCARTES_IMPL_DESCARTES_PROFILE_HPP diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_default_plan_profile.h b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_default_plan_profile.h index 1bdde8ee7d..db8e2ec1db 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_default_plan_profile.h +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_default_plan_profile.h @@ -33,12 +33,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include #include namespace tesseract_planning { +class DescartesVertexEvaluator; + template class DescartesDefaultPlanProfile : public DescartesPlanProfile { @@ -47,15 +48,10 @@ class DescartesDefaultPlanProfile : public DescartesPlanProfile using ConstPtr = std::shared_ptr>; DescartesDefaultPlanProfile() = default; - DescartesDefaultPlanProfile(const tinyxml2::XMLElement& xml_element); - - PoseSamplerFn target_pose_sampler = sampleFixed; - - DescartesEdgeEvaluatorAllocatorFn edge_evaluator{ nullptr }; - DescartesStateEvaluatorAllocatorFn state_evaluator{ nullptr }; - // If not provided it adds a joint limit is valid function - DescartesVertexEvaluatorAllocatorFn vertex_evaluator{ nullptr }; + bool target_pose_fixed{ true }; + Eigen::Vector3d target_pose_sample_axis{ 0, 0, 1 }; + double target_pose_sample_resolution{ M_PI_2 }; /** * @brief Flag to indicate that collisions should not cause failures during state/edge evaluation @@ -79,30 +75,37 @@ class DescartesDefaultPlanProfile : public DescartesPlanProfile */ bool use_redundant_joint_solutions{ false }; - /** @brief Number of threads to use during planning */ - int num_threads{ 1 }; - /** @brief Flag to produce debug information during planning */ bool debug{ false }; - void apply(DescartesProblem& prob, - const Eigen::Isometry3d& cartesian_waypoint, - const InstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - int index) const override; + std::unique_ptr> + createWaypointSampler(const MoveInstructionPoly& move_instruction, + const tesseract_common::ManipulatorInfo& manip_info, + const std::shared_ptr& env) const override; - void apply(DescartesProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const InstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - int index) const override; + std::unique_ptr> + createEdgeEvaluator(const MoveInstructionPoly& move_instruction, + const tesseract_common::ManipulatorInfo& manip_info, + const std::shared_ptr& env) const override; - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + std::unique_ptr> + createStateEvaluator(const MoveInstructionPoly& move_instruction, + const tesseract_common::ManipulatorInfo& manip_info, + const std::shared_ptr& env) const override; protected: friend class boost::serialization::access; template void serialize(Archive&, const unsigned int); // NOLINT + + virtual std::unique_ptr + createVertexEvaluator(const MoveInstructionPoly& move_instruction, + const std::shared_ptr& manip, + const std::shared_ptr& env) const; + + virtual PoseSamplerFn createPoseSampler(const MoveInstructionPoly& move_instruction, + const std::shared_ptr& manip, + const std::shared_ptr& env) const; }; using DescartesDefaultPlanProfileF = DescartesDefaultPlanProfile; diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_ladder_graph_solver_profile.h b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_ladder_graph_solver_profile.h new file mode 100644 index 0000000000..6f7da3e218 --- /dev/null +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_ladder_graph_solver_profile.h @@ -0,0 +1,60 @@ +/** + * @file descartes_ladder_graph_solver_profile.h + * @brief + * + * @author Levi Armstrong + * @date June 18, 2020 + * @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. + */ +#ifndef TESSERACT_MOTION_PLANNERS_DESCARTES_DESCARTES_LADDER_GRAPH_SOLVER_PROFILE_H +#define TESSERACT_MOTION_PLANNERS_DESCARTES_DESCARTES_LADDER_GRAPH_SOLVER_PROFILE_H + +#include + +namespace tesseract_planning +{ +template +class DescartesLadderGraphSolverProfile : public DescartesSolverProfile +{ +public: + using Ptr = std::shared_ptr>; + using ConstPtr = std::shared_ptr>; + + DescartesLadderGraphSolverProfile() = default; + + /** @brief Number of threads to use during planning */ + int num_threads{ 1 }; + + std::unique_ptr> create() const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT +}; + +using DescartesLadderGraphSolverProfileF = DescartesLadderGraphSolverProfile; +using DescartesLadderGraphSolverProfileD = DescartesLadderGraphSolverProfile; +} // namespace tesseract_planning + +BOOST_CLASS_EXPORT_KEY(tesseract_planning::DescartesLadderGraphSolverProfile) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::DescartesLadderGraphSolverProfile) + +#endif // TESSERACT_MOTION_PLANNERS_DESCARTES_DESCARTES_LADDER_GRAPH_SOLVER_PROFILE_H diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_profile.h b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_profile.h index 8f90e755ff..81d3b12fdd 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_profile.h +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_profile.h @@ -35,17 +35,40 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include -#include +#include +#include -namespace tinyxml2 -{ -class XMLElement; // NOLINT -class XMLDocument; -} // namespace tinyxml2 +#include +#include +#include +#include +#include namespace tesseract_planning { +template +class DescartesSolverProfile : public Profile +{ +public: + using Ptr = std::shared_ptr>; + using ConstPtr = std::shared_ptr>; + + DescartesSolverProfile() : Profile(DescartesSolverProfile::getStaticKey()) {} + + /** + * @brief A utility function for getting profile ID + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey() { return std::type_index(typeid(DescartesSolverProfile)).hash_code(); } + + virtual std::unique_ptr> create() const = 0; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int); // NOLINT +}; + template class DescartesPlanProfile : public Profile { @@ -61,19 +84,24 @@ class DescartesPlanProfile : public Profile */ static std::size_t getStaticKey() { return std::type_index(typeid(DescartesPlanProfile)).hash_code(); } - virtual void apply(DescartesProblem& prob, - const Eigen::Isometry3d& cartesian_waypoint, - const InstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - int index) const = 0; + std::shared_ptr + createKinematicGroup(const tesseract_common::ManipulatorInfo& manip_info, + const tesseract_environment::Environment& env) const; + + virtual std::unique_ptr> + createWaypointSampler(const MoveInstructionPoly& move_instruction, + const tesseract_common::ManipulatorInfo& manip_info, + const std::shared_ptr& env) const = 0; - virtual void apply(DescartesProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const InstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - int index) const = 0; + virtual std::unique_ptr> + createEdgeEvaluator(const MoveInstructionPoly& move_instruction, + const tesseract_common::ManipulatorInfo& manip_info, + const std::shared_ptr& env) const = 0; - virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; + virtual std::unique_ptr> + createStateEvaluator(const MoveInstructionPoly& move_instruction, + const tesseract_common::ManipulatorInfo& manip_info, + const std::shared_ptr& env) const = 0; protected: friend class boost::serialization::access; @@ -81,7 +109,6 @@ class DescartesPlanProfile : public Profile void serialize(Archive& ar, const unsigned int); // NOLINT }; -/** @todo Currently descartes does not have support of composite profile everything is handled by the plan profile */ } // namespace tesseract_planning #endif // TESSERACT_MOTION_PLANNERS_DESCARTES_DESCARTES_PROFILE_H diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/types.h b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/types.h deleted file mode 100644 index e9cc809c1f..0000000000 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/types.h +++ /dev/null @@ -1,66 +0,0 @@ -/** - * @file types.h - * @brief Tesseract descartes types - * - * @author Levi Armstrong - * @date April 18, 2018 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2017, 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_DESCARTES_TYPES_H -#define TESSERACT_MOTION_PLANNERS_DESCARTES_TYPES_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include - -namespace tesseract_planning -{ -/** - * @brief This is used for tesseract descartes samplers that filters out invalid solutions. - * - * Example: This would be used to filter out solution outside of custom joint limits. - * - */ -template -using DescartesVertexEvaluatorAllocatorFn = - std::function&)>; - -/** - * @brief This is used to create edge evaluator within tesseract, to allow thread safe creation of descartes edge - * evaluators - */ -template -using DescartesEdgeEvaluatorAllocatorFn = - std::function::Ptr(const DescartesProblem&)>; - -/** - * @brief Creates a state evaluator - */ -template -using DescartesStateEvaluatorAllocatorFn = - std::function::Ptr(const DescartesProblem&)>; - -} // namespace tesseract_planning -#endif // TESSERACT_MOTION_PLANNERS_DESCARTES_TYPES_H diff --git a/tesseract_motion_planners/descartes/src/deserialize.cpp b/tesseract_motion_planners/descartes/src/deserialize.cpp deleted file mode 100644 index 0214f2f3ca..0000000000 --- a/tesseract_motion_planners/descartes/src/deserialize.cpp +++ /dev/null @@ -1,126 +0,0 @@ -/** - * @file deserialize.cpp - * @brief Provide methods for deserialize instructions to xml and deserialization - * - * @author Tyler Marr - * @date August 21, 2020 - * @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 -#include -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include - -namespace tesseract_planning -{ -DescartesDefaultPlanProfile descartesPlanParser(const tinyxml2::XMLElement& xml_element) -{ - const tinyxml2::XMLElement* descartes_plan_element = xml_element.FirstChildElement("DescartesPlanProfile"); - return { *descartes_plan_element }; -} - -DescartesDefaultPlanProfile descartesPlanFromXMLElement(const tinyxml2::XMLElement* profile_xml) -{ - std::array version{ 0, 0, 0 }; - std::string version_string; - int status = tesseract_common::QueryStringAttribute(profile_xml, "version", version_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("fromXML: Error parsing robot attribute 'version'"); - - if (status != tinyxml2::XML_NO_ATTRIBUTE) - { - std::vector tokens; - boost::split(tokens, version_string, boost::is_any_of("."), boost::token_compress_on); - if (tokens.size() < 2 || tokens.size() > 3 || !tesseract_common::isNumeric(tokens)) - throw std::runtime_error("fromXML: Error parsing robot attribute 'version'"); - - tesseract_common::toNumeric(tokens[0], version[0]); - tesseract_common::toNumeric(tokens[1], version[1]); - if (tokens.size() == 3) - tesseract_common::toNumeric(tokens[2], version[2]); - else - version[2] = 0; - } - else - { - CONSOLE_BRIDGE_logWarn("No version number was provided so latest parser will be used."); - } - - const tinyxml2::XMLElement* planner_xml = profile_xml->FirstChildElement("Planner"); - if (planner_xml == nullptr) - throw std::runtime_error("fromXML: Could not find the 'Planner' element in the xml file."); - - int type{ 0 }; - status = planner_xml->QueryIntAttribute("type", &type); - if (status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("fromXML: Failed to parse instruction type attribute."); - - return descartesPlanParser(*planner_xml); -} - -DescartesDefaultPlanProfile descartesPlanFromXMLDocument(const tinyxml2::XMLDocument& xml_doc) -{ - const tinyxml2::XMLElement* planner_xml = xml_doc.FirstChildElement("Profile"); - if (planner_xml == nullptr) - throw std::runtime_error("Could not find the 'Profile' element in the xml file"); - - return descartesPlanFromXMLElement(planner_xml); -} - -DescartesDefaultPlanProfile descartesPlanFromXMLString(const std::string& xml_string) -{ - tinyxml2::XMLDocument xml_doc; - int status = xml_doc.Parse(xml_string.c_str()); - if (status != tinyxml2::XMLError::XML_SUCCESS) - throw std::runtime_error("Could not parse the Planner Profile XML File."); - - return descartesPlanFromXMLDocument(xml_doc); -} - -DescartesDefaultPlanProfile descartesPlanFromXMLFile(const std::string& file_path) -{ - // get the entire file - std::string xml_string; - std::fstream xml_file(file_path.c_str(), std::fstream::in); - if (xml_file.is_open()) - { - while (xml_file.good()) - { - std::string line; - std::getline(xml_file, line); - xml_string += (line + "\n"); - } - xml_file.close(); - return descartesPlanFromXMLString(xml_string); - } - - throw std::runtime_error("Could not open file " + file_path + "for parsing."); -} - -} // namespace tesseract_planning diff --git a/tesseract_motion_planners/descartes/src/profile/descartes_default_plan_profile.cpp b/tesseract_motion_planners/descartes/src/profile/descartes_default_plan_profile.cpp index 70d073c23b..63fbe27734 100644 --- a/tesseract_motion_planners/descartes/src/profile/descartes_default_plan_profile.cpp +++ b/tesseract_motion_planners/descartes/src/profile/descartes_default_plan_profile.cpp @@ -24,7 +24,9 @@ * limitations under the License. */ #include +#include #include +#include #include #include #include @@ -40,18 +42,15 @@ template void DescartesDefaultPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(DescartesPlanProfile); - /** @todo FIX */ - // ar& BOOST_SERIALIZATION_NVP(target_pose_sampler); - // ar& BOOST_SERIALIZATION_NVP(edge_evaluator); - // ar& BOOST_SERIALIZATION_NVP(state_evaluator); - // ar& BOOST_SERIALIZATION_NVP(vertex_evaluator); + ar& BOOST_SERIALIZATION_NVP(target_pose_fixed); + ar& BOOST_SERIALIZATION_NVP(target_pose_sample_axis); + ar& BOOST_SERIALIZATION_NVP(target_pose_sample_resolution); ar& BOOST_SERIALIZATION_NVP(allow_collision); ar& BOOST_SERIALIZATION_NVP(enable_collision); ar& BOOST_SERIALIZATION_NVP(vertex_collision_check_config); ar& BOOST_SERIALIZATION_NVP(enable_edge_collision); ar& BOOST_SERIALIZATION_NVP(edge_collision_check_config); ar& BOOST_SERIALIZATION_NVP(use_redundant_joint_solutions); - ar& BOOST_SERIALIZATION_NVP(num_threads); ar& BOOST_SERIALIZATION_NVP(debug); } diff --git a/tesseract_motion_planners/descartes/src/profile/descartes_ladder_graph_solver_profile.cpp b/tesseract_motion_planners/descartes/src/profile/descartes_ladder_graph_solver_profile.cpp new file mode 100644 index 0000000000..1d0a101267 --- /dev/null +++ b/tesseract_motion_planners/descartes/src/profile/descartes_ladder_graph_solver_profile.cpp @@ -0,0 +1,50 @@ +/** + * @file descartes_ladder_graph_solver_profile.cpp + * @brief + * + * @author Levi Armstrong + * @date June 18, 2020 + * @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 +#include +#include + +namespace tesseract_planning +{ +// Explicit template instantiation +template class DescartesLadderGraphSolverProfile; +template class DescartesLadderGraphSolverProfile; + +template +template +void DescartesLadderGraphSolverProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(DescartesSolverProfile); + ar& BOOST_SERIALIZATION_NVP(num_threads); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::DescartesLadderGraphSolverProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::DescartesLadderGraphSolverProfile) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::DescartesLadderGraphSolverProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::DescartesLadderGraphSolverProfile) diff --git a/tesseract_motion_planners/descartes/src/profile/descartes_profile.cpp b/tesseract_motion_planners/descartes/src/profile/descartes_profile.cpp index 0eeffa0a12..b3a6e6ff37 100644 --- a/tesseract_motion_planners/descartes/src/profile/descartes_profile.cpp +++ b/tesseract_motion_planners/descartes/src/profile/descartes_profile.cpp @@ -29,6 +29,13 @@ namespace tesseract_planning { +template +template +void DescartesSolverProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); +} + template template void DescartesPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) @@ -43,3 +50,8 @@ TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::DescartesPlanProfil BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::DescartesPlanProfile) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::DescartesPlanProfile) BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::DescartesPlanProfile) + +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::DescartesSolverProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::DescartesSolverProfile) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::DescartesSolverProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::DescartesSolverProfile) diff --git a/tesseract_motion_planners/descartes/src/serialize.cpp b/tesseract_motion_planners/descartes/src/serialize.cpp deleted file mode 100644 index 38be671f73..0000000000 --- a/tesseract_motion_planners/descartes/src/serialize.cpp +++ /dev/null @@ -1,80 +0,0 @@ -/** - * @file serialize.cpp - * @brief - * - * @author Tyler Marr - * @date August 25, 2020 - * @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 -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include - -static const std::array VERSION{ { 1, 0, 0 } }; -static const Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " "); - -namespace tesseract_planning -{ -std::shared_ptr toXMLDocument(const DescartesPlanProfile& plan_profile) -{ - auto doc = std::make_shared(); - tinyxml2::XMLElement* xml_root = doc->NewElement("Profile"); - xml_root->SetAttribute("name", "DescartesDefaultProfile"); - xml_root->SetAttribute( - "version", - (std::to_string(VERSION[0]) + "." + std::to_string(VERSION[1]) + "." + std::to_string(VERSION[2])).c_str()); - - tinyxml2::XMLElement* xml_plan_profile = plan_profile.toXML(*doc); - xml_root->InsertEndChild(xml_plan_profile); - doc->InsertFirstChild(xml_root); - - return doc; -} - -bool toXMLFile(const DescartesPlanProfile& plan_profile, const std::string& file_path) -{ - std::shared_ptr doc = toXMLDocument(plan_profile); - tinyxml2::XMLError status = doc->SaveFile(file_path.c_str()); - if (status != tinyxml2::XMLError::XML_SUCCESS) - { - CONSOLE_BRIDGE_logError("Failed to save Plan Profile XML File: %s", file_path.c_str()); - return false; - } - - return true; -} - -std::string toXMLString(const DescartesPlanProfile& plan_profile) -{ - std::shared_ptr doc = toXMLDocument(plan_profile); - tinyxml2::XMLPrinter printer; - doc->Print(&printer); - return { printer.CStr() }; -} - -} // namespace tesseract_planning diff --git a/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp b/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp index 9debf0705e..16299b953c 100644 --- a/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp +++ b/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp @@ -47,8 +47,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include -#include +#include #include #include #include @@ -89,24 +88,6 @@ class TesseractPlanningDescartesUnit : public ::testing::Test } }; -TEST(TesseractPlanningDescartesSerializeUnit, SerializeDescartesDefaultPlanToXml) // NOLINT -{ - // Write program to file - DescartesDefaultPlanProfile plan_profile; - plan_profile.enable_edge_collision = true; - - EXPECT_TRUE(toXMLFile(plan_profile, tesseract_common::getTempPath() + "descartes_default_plan_example_input.xml")); - - // Import file - DescartesDefaultPlanProfile imported_plan_profile = - descartesPlanFromXMLFile(tesseract_common::getTempPath() + "descartes_default_plan_example_input.xml"); - - // Re-write file and compare changed from default term - EXPECT_TRUE( - toXMLFile(imported_plan_profile, tesseract_common::getTempPath() + "descartes_default_plan_example_input2.xml")); - EXPECT_TRUE(plan_profile.enable_edge_collision == imported_plan_profile.enable_edge_collision); -} - TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerFixedPoses) // NOLINT { // Specify a start waypoint @@ -133,15 +114,18 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerFixedPoses) // NOLINT CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 10); // Create Profiles + auto solver_profile = std::make_shared(); + solver_profile->num_threads = 1; + auto plan_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); profiles->addProfile(DESCARTES_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); + profiles->addProfile(DESCARTES_DEFAULT_NAMESPACE, "TEST_PROFILE", solver_profile); // Create Planner DescartesMotionPlannerD single_descartes_planner(DESCARTES_DEFAULT_NAMESPACE); - plan_profile->num_threads = 1; // Create Planning Request PlannerRequest request; @@ -156,15 +140,8 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerFixedPoses) // NOLINT for (int i = 0; i < 10; ++i) { - // Test the problem generator - { - auto problem = single_descartes_planner.createProblem(request); - EXPECT_EQ(problem->samplers.size(), 11); - EXPECT_EQ(problem->edge_evaluators.size(), 10); - } - DescartesMotionPlannerD descartes_planner(DESCARTES_DEFAULT_NAMESPACE); - plan_profile->num_threads = 4; + solver_profile->num_threads = 4; PlannerResponse planner_response = descartes_planner.solve(request); @@ -236,19 +213,23 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerAxialSymetric) // NOLINT CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 10); // Create Profiles + auto solver_profile = std::make_shared(); + solver_profile->num_threads = 1; + auto plan_profile = std::make_shared(); + // Make this a tool z-axis free sampler - plan_profile->target_pose_sampler = [](const Eigen::Isometry3d& tool_pose) { - return tesseract_planning::sampleToolAxis(tool_pose, M_PI_4, Eigen::Vector3d(0, 0, 1)); - }; + plan_profile->target_pose_fixed = false; + plan_profile->target_pose_sample_axis = Eigen::Vector3d(0, 0, 1); + plan_profile->target_pose_sample_resolution = M_PI_4; // Profile Dictionary auto profiles = std::make_shared(); profiles->addProfile(DESCARTES_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); + profiles->addProfile(DESCARTES_DEFAULT_NAMESPACE, "TEST_PROFILE", solver_profile); // Create Planner DescartesMotionPlannerD single_descartes_planner(DESCARTES_DEFAULT_NAMESPACE); - plan_profile->num_threads = 1; // Create Planning Request PlannerRequest request; @@ -256,11 +237,6 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerAxialSymetric) // NOLINT request.env = env_; request.profiles = profiles; - auto problem = single_descartes_planner.createProblem(request); - problem->num_threads = 1; - EXPECT_EQ(problem->samplers.size(), 11); - EXPECT_EQ(problem->edge_evaluators.size(), 10); - PlannerResponse single_planner_response = single_descartes_planner.solve(request); EXPECT_TRUE(&single_planner_response); @@ -269,7 +245,7 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerAxialSymetric) // NOLINT for (int i = 0; i < 10; ++i) { DescartesMotionPlannerD descartes_planner(DESCARTES_DEFAULT_NAMESPACE); - plan_profile->num_threads = 4; + solver_profile->num_threads = 4; PlannerResponse planner_response = descartes_planner.solve(request); EXPECT_TRUE(&planner_response); @@ -330,12 +306,18 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerCollisionEdgeEvaluator) CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 2); // Create Profiles + auto solver_profile = std::make_shared(); + solver_profile->num_threads = 1; + auto plan_profile = std::make_shared(); + // Make this a tool z-axis free sampler - plan_profile->target_pose_sampler = [](const Eigen::Isometry3d& tool_pose) { - return tesseract_planning::sampleToolAxis(tool_pose, 60 * M_PI * 180.0, Eigen::Vector3d(0, 0, 1)); - }; - plan_profile->enable_edge_collision = true; // Add collision edge evaluator + plan_profile->target_pose_fixed = false; + plan_profile->target_pose_sample_axis = Eigen::Vector3d(0, 0, 1); + plan_profile->target_pose_sample_resolution = 60 * M_PI * 180.0; + + // Add collision edge evaluator + plan_profile->enable_edge_collision = true; // Profile Dictionary auto profiles = std::make_shared(); @@ -349,13 +331,6 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerCollisionEdgeEvaluator) // Create Planner DescartesMotionPlannerD single_descartes_planner(DESCARTES_DEFAULT_NAMESPACE); - plan_profile->num_threads = 1; - - // Test Problem size - TODO: Make dedicated unit test for DefaultDescartesProblemGenerator - auto problem = single_descartes_planner.createProblem(request); - EXPECT_EQ(problem->samplers.size(), 3); - EXPECT_EQ(problem->edge_evaluators.size(), 2); - EXPECT_EQ(problem->num_threads, 1); PlannerResponse single_planner_response = single_descartes_planner.solve(request); EXPECT_TRUE(&single_planner_response); @@ -365,7 +340,7 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerCollisionEdgeEvaluator) for (int i = 0; i < 10; ++i) { DescartesMotionPlannerD descartes_planner(DESCARTES_DEFAULT_NAMESPACE); - plan_profile->num_threads = 4; + solver_profile->num_threads = 4; PlannerResponse planner_response = descartes_planner.solve(request); EXPECT_TRUE(&planner_response);