From c24cb982427788b72357765e10f52eae1db1a0c5 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sat, 7 Dec 2024 15:35:31 -0600 Subject: [PATCH] Add simple ompl profile interface --- tesseract_motion_planners/ompl/CMakeLists.txt | 4 +- .../ompl/deserialize.h | 55 -- .../ompl/ompl_motion_planner.h | 8 - .../ompl/ompl_problem.h | 12 - .../ompl/profile/ompl_default_plan_profile.h | 99 +- .../ompl/profile/ompl_profile.h | 56 +- .../ompl/serialize.h | 51 - .../ompl/src/deserialize.cpp | 127 --- .../ompl/src/ompl_motion_planner.cpp | 153 +-- .../src/profile/ompl_default_plan_profile.cpp | 870 +++++++----------- .../ompl/src/serialize.cpp | 83 -- .../ompl/test/ompl_planner_tests.cpp | 35 - 12 files changed, 401 insertions(+), 1152 deletions(-) delete mode 100644 tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/deserialize.h delete mode 100644 tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/serialize.h delete mode 100644 tesseract_motion_planners/ompl/src/deserialize.cpp delete mode 100644 tesseract_motion_planners/ompl/src/serialize.cpp diff --git a/tesseract_motion_planners/ompl/CMakeLists.txt b/tesseract_motion_planners/ompl/CMakeLists.txt index ae481465b3..ee1c8f32d0 100644 --- a/tesseract_motion_planners/ompl/CMakeLists.txt +++ b/tesseract_motion_planners/ompl/CMakeLists.txt @@ -14,9 +14,7 @@ set(OMPL_SRC src/profile/ompl_default_plan_profile.cpp src/utils.cpp src/state_collision_validator.cpp - src/compound_state_validator.cpp - src/serialize.cpp - src/deserialize.cpp) + src/compound_state_validator.cpp) # if(NOT OMPL_VERSION VERSION_LESS "1.4.0") list(APPEND OMPL_SRC src/config/ompl_planner_constrained_config.cpp) endif() diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/deserialize.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/deserialize.h deleted file mode 100644 index cc6019a933..0000000000 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/deserialize.h +++ /dev/null @@ -1,55 +0,0 @@ -/** - * @file deserialize.h - * @brief Provide methods for deserialize ompl plans to xml - * - * @author Tyler Marr - * @date August 24, 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_OMPL_DESERIALIZE_H -#define TESSERACT_MOTION_PLANNERS_OMPL_DESERIALIZE_H -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -namespace tinyxml2 -{ -class XMLElement; // NOLINT -class XMLDocument; -} // namespace tinyxml2 - -namespace tesseract_planning -{ -class OMPLDefaultPlanProfile; - -OMPLDefaultPlanProfile omplPlanParser(const tinyxml2::XMLElement& xml_element); - -OMPLDefaultPlanProfile omplPlanFromXMLElement(const tinyxml2::XMLElement* profile_xml); - -OMPLDefaultPlanProfile omplPlanFromXMLDocument(const tinyxml2::XMLDocument& xml_doc); - -OMPLDefaultPlanProfile omplPlanFromXMLFile(const std::string& file_path); - -OMPLDefaultPlanProfile omplPlanFromXMLString(const std::string& xml_string); - -} // namespace tesseract_planning - -#endif // TESSERACT_MOTION_PLANNERS_OMPL_DESERIALIZE_H diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h index b8f57fb04a..6a9ce33902 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h @@ -94,14 +94,6 @@ class OMPLMotionPlanner : public MotionPlanner protected: /** @brief OMPL Parallel planner */ std::shared_ptr parallel_plan_; - - OMPLProblemConfig createSubProblem(const PlannerRequest& request, - const tesseract_common::ManipulatorInfo& composite_mi, - const std::shared_ptr& manip, - const MoveInstructionPoly& start_instruction, - const MoveInstructionPoly& end_instruction, - int n_output_states, - int index) const; }; } // namespace tesseract_planning diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_problem.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_problem.h index 5717095400..ec65310b28 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_problem.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_problem.h @@ -68,18 +68,6 @@ namespace tesseract_planning struct OMPLProblem; struct OMPLPlannerConfigurator; -using StateSamplerAllocator = - std::function; - -using OptimizationObjectiveAllocator = - std::function; - -using StateValidityCheckerAllocator = - std::function; - -using MotionValidatorAllocator = - std::function; - enum class OMPLProblemStateSpace { REAL_STATE_SPACE, diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h index 54d3469cf8..1e99339b56 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h @@ -41,12 +41,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -namespace ompl::base -{ -class StateValidityChecker; -using StateValidityCheckerPtr = std::shared_ptr; -} // namespace ompl::base - namespace tesseract_planning { struct OMPLPlannerConfigurator; @@ -62,10 +56,6 @@ class OMPLDefaultPlanProfile : public OMPLPlanProfile using ConstPtr = std::shared_ptr; OMPLDefaultPlanProfile(); - OMPLDefaultPlanProfile(const tinyxml2::XMLElement& xml_element); - - /** @brief The OMPL state space to use when planning */ - OMPLProblemStateSpace state_space{ OMPLProblemStateSpace::REAL_STATE_SPACE }; /** @brief Max planning time allowed in seconds */ double planning_time = 5.0; @@ -104,57 +94,64 @@ class OMPLDefaultPlanProfile : public OMPLPlanProfile /** @brief The collision check configuration */ tesseract_collision::CollisionCheckConfig collision_check_config; - /** @brief The state sampler allocator. This can be null and it will use Tesseract default state sampler allocator. */ - StateSamplerAllocator state_sampler_allocator; + std::unique_ptr create(const PlannerRequest& request, + const MoveInstructionPoly& start_instruction, + const MoveInstructionPoly& end_instruction, + int n_output_states) const override; - /** @brief Set the optimization objective function allocator. Default is to minimize path length */ - OptimizationObjectiveAllocator optimization_objective_allocator; +protected: + void setup(OMPLProblem& prob) const; - /** @brief The ompl state validity checker. If nullptr and collision checking enabled it uses - * StateCollisionValidator */ - StateValidityCheckerAllocator svc_allocator; + static void applyGoalStates(OMPLProblem& prob, + const Eigen::Isometry3d& cartesian_waypoint, + const MoveInstructionPoly& parent_instruction, + const tesseract_common::ManipulatorInfo& manip_info); - /** @brief The ompl motion validator. If nullptr and continuous collision checking enabled it used - * ContinuousMotionValidator */ - MotionValidatorAllocator mv_allocator; + static void applyGoalStates(OMPLProblem& prob, const Eigen::VectorXd& joint_waypoint); - void setup(OMPLProblem& prob) const override; + static void applyStartStates(OMPLProblem& prob, + const Eigen::Isometry3d& cartesian_waypoint, + const MoveInstructionPoly& parent_instruction, + const tesseract_common::ManipulatorInfo& manip_info); - void applyGoalStates(OMPLProblem& prob, - const Eigen::Isometry3d& cartesian_waypoint, - const MoveInstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const override; + static void applyStartStates(OMPLProblem& prob, const Eigen::VectorXd& joint_waypoint); - void applyGoalStates(OMPLProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const MoveInstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const override; + /** + * @brief Create state sampler allocator + * @param prob The OMPL problem + * @return OMPL state sampler allocator + */ + virtual std::function + createStateSamplerAllocator(OMPLProblem& prob) const; - void applyStartStates(OMPLProblem& prob, - const Eigen::Isometry3d& cartesian_waypoint, - const MoveInstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const override; + /** + * @brief Create state validator which should not be the collision state validator + * @param prob The OMPL problem + * @return OMPL state validator + */ + virtual std::unique_ptr createStateValidator(OMPLProblem& prob) const; - void applyStartStates(OMPLProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const MoveInstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const override; + /** + * @brief Create collision state validator + * @param prob The OMPL problem + * @return OMPL state collision validator + */ + virtual std::unique_ptr createCollisionStateValidator(OMPLProblem& prob) const; - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + /** + * @brief Create motion validator + * @param prob The OMPL problem + * @return OMPL motion validator + */ + virtual std::unique_ptr + createMotionValidator(OMPLProblem& prob, const ompl::base::StateValidityCheckerPtr& svc_without_collision) const; -protected: - ompl::base::StateValidityCheckerPtr processStateValidator(OMPLProblem& prob) const; - void processMotionValidator(OMPLProblem& prob, - const ompl::base::StateValidityCheckerPtr& svc_without_collision) const; - void processOptimizationObjective(OMPLProblem& prob) const; + /** + * @brief Create OMPL optimization object + * @param prob The OMPL problem + * @return OMPL optimization objective + */ + virtual std::unique_ptr createOptimizationObjective(OMPLProblem& prob) const; friend class boost::serialization::access; template diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h index 1d5822238a..9f579f5c05 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h @@ -34,18 +34,17 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include +#include #include -#include +#include -namespace tinyxml2 -{ -class XMLElement; // NOLINT -class XMLDocument; -} // namespace tinyxml2 +#include namespace tesseract_planning { struct OMPLProblem; + class OMPLPlanProfile : public Profile { public: @@ -60,37 +59,18 @@ class OMPLPlanProfile : public Profile */ static std::size_t getStaticKey(); - virtual void setup(OMPLProblem& prob) const = 0; - - virtual void applyGoalStates(OMPLProblem& prob, - const Eigen::Isometry3d& cartesian_waypoint, - const MoveInstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const = 0; - - virtual void applyGoalStates(OMPLProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const MoveInstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const = 0; - - virtual void applyStartStates(OMPLProblem& prob, - const Eigen::Isometry3d& cartesian_waypoint, - const MoveInstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const = 0; - - virtual void applyStartStates(OMPLProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const MoveInstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const = 0; - - virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; + /** + * @brief Create OMPL Planning Problem + * @param request The planning request + * @param start_instruction The start instruction + * @param end_instruction The goal instruction + * @param n_output_states The number of interpolated output states + * @return A OMPL Problem + */ + virtual std::unique_ptr create(const PlannerRequest& request, + const MoveInstructionPoly& start_instruction, + const MoveInstructionPoly& end_instruction, + int n_output_states) const = 0; protected: friend class boost::serialization::access; @@ -98,8 +78,6 @@ class OMPLPlanProfile : public Profile void serialize(Archive&, const unsigned int); // NOLINT }; -/** @todo Currently OMPL does not have support of composite profile everything is handled by the plan profile */ - } // namespace tesseract_planning BOOST_CLASS_EXPORT_KEY(tesseract_planning::OMPLPlanProfile) diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/serialize.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/serialize.h deleted file mode 100644 index b3b633583a..0000000000 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/serialize.h +++ /dev/null @@ -1,51 +0,0 @@ -/** - * @file serialize.h - * @brief Provide methods for serializing ompl plans to xml - * - * @author Tyler Marr - * @date August 24, 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_OMPL_SERIALIZE_H -#define TESSERACT_MOTION_PLANNERS_OMPL_SERIALIZE_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -namespace tinyxml2 -{ -class XMLElement; // NOLINT -class XMLDocument; -} // namespace tinyxml2 - -namespace tesseract_planning -{ -class OMPLPlanProfile; - -std::shared_ptr toXMLDocument(const OMPLPlanProfile& plan_profile); - -bool toXMLFile(const OMPLPlanProfile& plan_profile, const std::string& file_path); - -std::string toXMLString(const OMPLPlanProfile& plan_profile); - -} // namespace tesseract_planning -#endif // TESSERACT_MOTION_PLANNERS_OMPL_SERIALIZE_H diff --git a/tesseract_motion_planners/ompl/src/deserialize.cpp b/tesseract_motion_planners/ompl/src/deserialize.cpp deleted file mode 100644 index 744c63a9af..0000000000 --- a/tesseract_motion_planners/ompl/src/deserialize.cpp +++ /dev/null @@ -1,127 +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 -#include - -namespace tesseract_planning -{ -OMPLDefaultPlanProfile omplPlanParser(const tinyxml2::XMLElement& xml_element) -{ - const tinyxml2::XMLElement* ompl_plan_element = xml_element.FirstChildElement("OMPLPlanProfile"); - return OMPLDefaultPlanProfile{ *ompl_plan_element }; -} - -OMPLDefaultPlanProfile omplPlanFromXMLElement(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 omplPlanParser(*planner_xml); -} - -OMPLDefaultPlanProfile omplPlanFromXMLDocument(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 omplPlanFromXMLElement(planner_xml); -} - -OMPLDefaultPlanProfile omplPlanFromXMLString(const std::string& xml_string) -{ - tinyxml2::XMLDocument xml_doc; - tinyxml2::XMLError 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 omplPlanFromXMLDocument(xml_doc); -} - -OMPLDefaultPlanProfile omplPlanFromXMLFile(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 omplPlanFromXMLString(xml_string); - } - - throw std::runtime_error("Could not open file " + file_path + "for parsing."); -} - -} // namespace tesseract_planning diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index 2532d3fdd4..6516b9a463 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -320,105 +320,6 @@ void OMPLMotionPlanner::clear() { parallel_plan_ = nullptr; } std::unique_ptr OMPLMotionPlanner::clone() const { return std::make_unique(name_); } -OMPLProblemConfig -OMPLMotionPlanner::createSubProblem(const PlannerRequest& request, - const tesseract_common::ManipulatorInfo& composite_mi, - const std::shared_ptr& manip, - const MoveInstructionPoly& start_instruction, - const MoveInstructionPoly& end_instruction, - int n_output_states, - int index) const -{ - std::vector joint_names = manip->getJointNames(); - std::vector active_link_names = manip->getActiveLinkNames(); - - // Get Plan Profile - auto cur_plan_profile = getProfile( - name_, end_instruction.getProfile(name_), *request.profiles, std::make_shared()); - - if (!cur_plan_profile) - throw std::runtime_error("OMPLMotionPlanner: Invalid profile"); - - /** @todo Should check that the joint names match the order of the manipulator */ - OMPLProblemConfig config; - config.start_uuid = start_instruction.getUUID(); - config.end_uuid = end_instruction.getUUID(); - config.problem = std::make_shared(); - config.problem->env = request.env; - config.problem->env_state = request.env_state; - config.problem->manip = manip; - config.problem->contact_checker = request.env->getDiscreteContactManager(); - config.problem->contact_checker->setCollisionObjectsTransform(request.env_state.link_transforms); - config.problem->contact_checker->setActiveCollisionObjects(active_link_names); - - cur_plan_profile->setup(*config.problem); - config.problem->n_output_states = n_output_states; - - if (end_instruction.getWaypoint().isJointWaypoint() || end_instruction.getWaypoint().isStateWaypoint()) - { - assert(checkJointPositionFormat(joint_names, end_instruction.getWaypoint())); - const Eigen::VectorXd& cur_position = getJointPosition(end_instruction.getWaypoint()); - cur_plan_profile->applyGoalStates( - *config.problem, cur_position, end_instruction, composite_mi, active_link_names, index); - - if (start_instruction.getWaypoint().isJointWaypoint() || start_instruction.getWaypoint().isStateWaypoint()) - { - assert(checkJointPositionFormat(joint_names, start_instruction.getWaypoint())); - const Eigen::VectorXd& prev_position = getJointPosition(start_instruction.getWaypoint()); - cur_plan_profile->applyStartStates( - *config.problem, prev_position, start_instruction, composite_mi, active_link_names, index); - } - else if (start_instruction.getWaypoint().isCartesianWaypoint()) - { - const auto& prev_wp = start_instruction.getWaypoint().as(); - cur_plan_profile->applyStartStates( - *config.problem, prev_wp.getTransform(), start_instruction, composite_mi, active_link_names, index); - } - else - { - throw std::runtime_error("OMPLMotionPlanner: unknown waypoint type"); - } - - return config; - } - - if (end_instruction.getWaypoint().isCartesianWaypoint()) - { - const auto& cur_wp = end_instruction.getWaypoint().as(); - cur_plan_profile->applyGoalStates( - *config.problem, cur_wp.getTransform(), end_instruction, composite_mi, active_link_names, index); - - if (index == 0) - { - if (start_instruction.getWaypoint().isJointWaypoint() || start_instruction.getWaypoint().isStateWaypoint()) - { - assert(checkJointPositionFormat(joint_names, start_instruction.getWaypoint())); - const Eigen::VectorXd& prev_position = getJointPosition(start_instruction.getWaypoint()); - cur_plan_profile->applyStartStates( - *config.problem, prev_position, start_instruction, composite_mi, active_link_names, index); - } - else if (start_instruction.getWaypoint().isCartesianWaypoint()) - { - const auto& prev_wp = start_instruction.getWaypoint().as(); - cur_plan_profile->applyStartStates( - *config.problem, prev_wp.getTransform(), start_instruction, composite_mi, active_link_names, index); - } - else - { - throw std::runtime_error("OMPLMotionPlanner: unknown waypoint type"); - } - } - else - { - /** @todo Update. Extract the solution for the previous plan and set as the start */ - assert(false); - } - - return config; - } - - throw std::runtime_error("OMPLMotionPlanner: unknown waypoint type"); -} std::vector OMPLMotionPlanner::createProblems(const PlannerRequest& request) const { std::vector problems; @@ -426,49 +327,10 @@ std::vector OMPLMotionPlanner::createProblems(const PlannerRe // 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(); - - tesseract_kinematics::JointGroup::Ptr manip; - if (composite_mi.manipulator.empty()) - throw std::runtime_error("OMPL, manipulator is empty!"); - - try - { - tesseract_kinematics::KinematicGroup::Ptr kin_group; - std::string error_msg; - if (composite_mi.manipulator_ik_solver.empty()) - { - kin_group = request.env->getKinematicGroup(composite_mi.manipulator); - error_msg = "Failed to find kinematic group for manipulator '" + composite_mi.manipulator + "'"; - } - else - { - kin_group = request.env->getKinematicGroup(composite_mi.manipulator, composite_mi.manipulator_ik_solver); - error_msg = "Failed to find kinematic group for manipulator '" + composite_mi.manipulator + "' with solver '" + - composite_mi.manipulator_ik_solver + "'"; - } - - if (kin_group == nullptr) - { - CONSOLE_BRIDGE_logError("%s", error_msg.c_str()); - throw std::runtime_error(error_msg); - } - - manip = kin_group; - } - catch (...) - { - manip = request.env->getJointGroup(composite_mi.manipulator); - } - - if (!manip) - throw std::runtime_error("Failed to get joint/kinematic group: " + composite_mi.manipulator); - // Flatten the input for planning auto move_instructions = request.instructions.flatten(&moveFilter); // Transform plan instructions into ompl problem - int index = 0; int num_output_states = 1; MoveInstructionPoly start_instruction = move_instructions.front().get().as(); @@ -482,8 +344,19 @@ std::vector OMPLMotionPlanner::createProblems(const PlannerRe if (waypoint.isCartesianWaypoint() || waypoint.isStateWaypoint() || (waypoint.isJointWaypoint() && waypoint.as().isConstrained())) { - problems.push_back(createSubProblem( - request, composite_mi, manip, start_instruction, move_instruction, num_output_states, index++)); + // 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("OMPLMotionPlanner: Invalid profile"); + + OMPLProblemConfig config; + config.start_uuid = start_instruction.getUUID(); + config.end_uuid = move_instruction.getUUID(); + config.problem = cur_plan_profile->create(request, start_instruction, move_instruction, num_output_states); + + problems.push_back(config); start_instruction = move_instruction; num_output_states = 1; } diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_default_plan_profile.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_default_plan_profile.cpp index a3f164a3b0..512ce543c8 100644 --- a/tesseract_motion_planners/ompl/src/profile/ompl_default_plan_profile.cpp +++ b/tesseract_motion_planners/ompl/src/profile/ompl_default_plan_profile.cpp @@ -31,7 +31,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include -#include #include #include #include @@ -40,7 +39,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include +#include #include #include @@ -64,240 +65,122 @@ OMPLDefaultPlanProfile::OMPLDefaultPlanProfile() { } -OMPLDefaultPlanProfile::OMPLDefaultPlanProfile(const tinyxml2::XMLElement& xml_element) - : planners({ std::make_shared(), std::make_shared() }) +std::unique_ptr OMPLDefaultPlanProfile::create(const PlannerRequest& request, + const MoveInstructionPoly& start_instruction, + const MoveInstructionPoly& end_instruction, + int n_output_states) const { - const tinyxml2::XMLElement* state_space_element = xml_element.FirstChildElement("StateSpace"); - const tinyxml2::XMLElement* planning_time_element = xml_element.FirstChildElement("PlanningTime"); - const tinyxml2::XMLElement* max_solutions_element = xml_element.FirstChildElement("MaxSolutions"); - const tinyxml2::XMLElement* simplify_element = xml_element.FirstChildElement("Simplify"); - const tinyxml2::XMLElement* optimize_element = xml_element.FirstChildElement("Optimize"); - const tinyxml2::XMLElement* planners_element = xml_element.FirstChildElement("Planners"); - // const tinyxml2::XMLElement* collision_check_element = xml_element.FirstChildElement("CollisionCheck"); - // const tinyxml2::XMLElement* collision_continuous_element = xml_element.FirstChildElement("CollisionContinuous"); - // const tinyxml2::XMLElement* collision_safety_margin_element = - // xml_element.FirstChildElement("CollisionSafetyMargin"); - // const tinyxml2::XMLElement* longest_valid_segment_fraction_element = - // xml_element.FirstChildElement("LongestValidSegme" - // "ntFraction"); - // const tinyxml2::XMLElement* longest_valid_segment_length_element = - // xml_element.FirstChildElement("LongestValidSegment" - // "Length"); - - int status{ tinyxml2::XMLError::XML_SUCCESS }; - - if (state_space_element != nullptr) - { - auto type = static_cast(OMPLProblemStateSpace::REAL_STATE_SPACE); - status = state_space_element->QueryIntAttribute("type", &type); - if (status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLPlanProfile: Error parsing StateSpace type attribute."); + const tesseract_common::ManipulatorInfo& composite_mi = request.instructions.getManipulatorInfo(); - state_space = static_cast(type); - } + tesseract_kinematics::JointGroup::Ptr manip; + if (composite_mi.manipulator.empty()) + throw std::runtime_error("OMPL, manipulator is empty!"); - if (planning_time_element != nullptr) + try { - std::string planning_time_string; - status = tesseract_common::QueryStringText(planning_time_element, planning_time_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLPlanProfile: Error parsing PlanningTime string"); + tesseract_kinematics::KinematicGroup::Ptr kin_group; + std::string error_msg; + if (composite_mi.manipulator_ik_solver.empty()) + { + kin_group = request.env->getKinematicGroup(composite_mi.manipulator); + error_msg = "Failed to find kinematic group for manipulator '" + composite_mi.manipulator + "'"; + } + else + { + kin_group = request.env->getKinematicGroup(composite_mi.manipulator, composite_mi.manipulator_ik_solver); + error_msg = "Failed to find kinematic group for manipulator '" + composite_mi.manipulator + "' with solver '" + + composite_mi.manipulator_ik_solver + "'"; + } - if (!tesseract_common::isNumeric(planning_time_string)) - throw std::runtime_error("OMPLPlanProfile: PlanningTime is not a numeric values."); + if (kin_group == nullptr) + { + CONSOLE_BRIDGE_logError("%s", error_msg.c_str()); + throw std::runtime_error(error_msg); + } - tesseract_common::toNumeric(planning_time_string, planning_time); + manip = kin_group; } - - if (max_solutions_element != nullptr) + catch (...) { - std::string max_solutions_string; - status = tesseract_common::QueryStringText(max_solutions_element, max_solutions_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLPlanProfile: Error parsing MaxSolutions string"); + manip = request.env->getJointGroup(composite_mi.manipulator); + } - if (!tesseract_common::isNumeric(max_solutions_string)) - throw std::runtime_error("OMPLPlanProfile: MaxSolutions is not a numeric values."); + if (!manip) + throw std::runtime_error("Failed to get joint/kinematic group: " + composite_mi.manipulator); - tesseract_common::toNumeric(max_solutions_string, max_solutions); - } + std::vector joint_names = manip->getJointNames(); + std::vector active_link_names = manip->getActiveLinkNames(); - if (simplify_element != nullptr) - { - status = simplify_element->QueryBoolText(&simplify); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLPlanProfile: Error parsing Simplify string"); - } + /** @todo Should check that the joint names match the order of the manipulator */ + auto problem = std::make_unique(); + problem->env = request.env; + problem->env_state = request.env_state; + problem->manip = manip; + problem->contact_checker = request.env->getDiscreteContactManager(); + problem->contact_checker->setCollisionObjectsTransform(request.env_state.link_transforms); + problem->contact_checker->setActiveCollisionObjects(active_link_names); + + setup(*problem); + problem->n_output_states = n_output_states; - if (optimize_element != nullptr) + if (end_instruction.getWaypoint().isJointWaypoint() || end_instruction.getWaypoint().isStateWaypoint()) { - status = optimize_element->QueryBoolText(&optimize); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLPlanProfile: Error parsing Optimize string"); + assert(checkJointPositionFormat(joint_names, end_instruction.getWaypoint())); + const Eigen::VectorXd& cur_position = getJointPosition(end_instruction.getWaypoint()); + applyGoalStates(*problem, cur_position); + + if (start_instruction.getWaypoint().isJointWaypoint() || start_instruction.getWaypoint().isStateWaypoint()) + { + assert(checkJointPositionFormat(joint_names, start_instruction.getWaypoint())); + const Eigen::VectorXd& prev_position = getJointPosition(start_instruction.getWaypoint()); + applyStartStates(*problem, prev_position); + } + else if (start_instruction.getWaypoint().isCartesianWaypoint()) + { + const auto& prev_wp = start_instruction.getWaypoint().as(); + applyStartStates(*problem, prev_wp.getTransform(), start_instruction, composite_mi); + } + else + { + throw std::runtime_error("OMPLMotionPlanner: unknown waypoint type"); + } + + return problem; } - if (planners_element != nullptr) + if (end_instruction.getWaypoint().isCartesianWaypoint()) { - planners.clear(); - for (const tinyxml2::XMLElement* e = planners_element->FirstChildElement("Planner"); e != nullptr; - e = e->NextSiblingElement("Planner")) - { - int type{ 0 }; - status = e->QueryIntAttribute("type", &type); - if (status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLPlanProfile: Error parsing Planner type attribute."); + const auto& cur_wp = end_instruction.getWaypoint().as(); + applyGoalStates(*problem, cur_wp.getTransform(), end_instruction, composite_mi); - switch (type) + if (request.instructions.getFirstMoveInstruction()->getUUID() == start_instruction.getUUID()) + { + if (start_instruction.getWaypoint().isJointWaypoint() || start_instruction.getWaypoint().isStateWaypoint()) { - case static_cast(OMPLPlannerType::SBL): - { - SBLConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::EST): - { - ESTConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::LBKPIECE1): - { - LBKPIECE1Configurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::BKPIECE1): - { - BKPIECE1Configurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::KPIECE1): - { - KPIECE1Configurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::BiTRRT): - { - BiTRRTConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::RRT): - { - RRTConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::RRTConnect): - { - RRTConnectConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::RRTstar): - { - RRTstarConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::TRRT): - { - TRRTConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::PRM): - { - PRMConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::PRMstar): - { - PRMstarConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::LazyPRMstar): - { - LazyPRMstarConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::SPARS): - { - SPARSConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - default: - { - throw std::runtime_error("Unsupported OMPL Planner type"); - } + assert(checkJointPositionFormat(joint_names, start_instruction.getWaypoint())); + const Eigen::VectorXd& prev_position = getJointPosition(start_instruction.getWaypoint()); + applyStartStates(*problem, prev_position); } + else if (start_instruction.getWaypoint().isCartesianWaypoint()) + { + const auto& prev_wp = start_instruction.getWaypoint().as(); + applyStartStates(*problem, prev_wp.getTransform(), start_instruction, composite_mi); + } + else + { + throw std::runtime_error("OMPLMotionPlanner: unknown waypoint type"); + } + } + else + { + /** @todo Update. Extract the solution for the previous plan and set as the start */ + assert(false); } + + return problem; } - /// @todo Update XML - // if (collision_check_element) - // { - // status = collision_check_element->QueryBoolText(&collision_check); - // if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - // throw std::runtime_error("OMPLPlanProfile: Error parsing CollisionCheck string"); - // } - - // if (collision_continuous_element) - // { - // status = collision_continuous_element->QueryBoolText(&collision_continuous); - // if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - // throw std::runtime_error("OMPLPlanProfile: Error parsing CollisionContinuous string"); - // } - - /// @todo Update XML - // if (collision_safety_margin_element) - // { - // std::string collision_safety_margin_string; - // status = tesseract_common::QueryStringText(collision_safety_margin_element, collision_safety_margin_string); - // if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - // throw std::runtime_error("OMPLPlanProfile: Error parsing CollisionSafetyMargin string"); - - // if (!tesseract_common::isNumeric(collision_safety_margin_string)) - // throw std::runtime_error("OMPLPlanProfile: CollisionSafetyMargin is not a numeric values."); - - // tesseract_common::toNumeric(collision_safety_margin_string, collision_safety_margin); - // } - - // if (longest_valid_segment_fraction_element) - // { - // std::string longest_valid_segment_fraction_string; - // status = tesseract_common::QueryStringText(longest_valid_segment_fraction_element, - // longest_valid_segment_fraction_string); - // if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - // throw std::runtime_error("OMPLPlanProfile: Error parsing LongestValidSegmentFraction string"); - - // if (!tesseract_common::isNumeric(longest_valid_segment_fraction_string)) - // throw std::runtime_error("OMPLPlanProfile: LongestValidSegmentFraction is not a numeric values."); - - // tesseract_common::toNumeric(longest_valid_segment_fraction_string, longest_valid_segment_fraction); - // } - - // if (longest_valid_segment_length_element) - // { - // std::string longest_valid_segment_length_string; - // status = - // tesseract_common::QueryStringText(longest_valid_segment_length_element, - // longest_valid_segment_length_string); - // if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - // throw std::runtime_error("OMPLPlanProfile: Error parsing LongestValidSegmentLength string"); - - // if (!tesseract_common::isNumeric(longest_valid_segment_length_string)) - // throw std::runtime_error("OMPLPlanProfile: LongestValidSegmentLength is not a numeric values."); - - // tesseract_common::toNumeric(longest_valid_segment_length_string, longest_valid_segment_length); - // } + throw std::runtime_error("OMPLMotionPlanner: unknown waypoint type"); } void OMPLDefaultPlanProfile::setup(OMPLProblem& prob) const @@ -307,74 +190,68 @@ void OMPLDefaultPlanProfile::setup(OMPLProblem& prob) const prob.max_solutions = max_solutions; prob.simplify = simplify; prob.optimize = optimize; - + prob.state_space = OMPLProblemStateSpace::REAL_STATE_SPACE; prob.contact_checker->applyContactManagerConfig(collision_check_config.contact_manager_config); std::vector joint_names = prob.manip->getJointNames(); auto dof = static_cast(prob.manip->numJoints()); auto limits = prob.manip->getLimits().joint_limits; - if (state_space == OMPLProblemStateSpace::REAL_STATE_SPACE) - prob.extractor = [dof](const ompl::base::State* state) -> Eigen::Map { - return tesseract_planning::RealVectorStateSpaceExtractor(state, dof); - }; + prob.extractor = [dof](const ompl::base::State* state) -> Eigen::Map { + return tesseract_planning::RealVectorStateSpaceExtractor(state, dof); + }; -#ifndef OMPL_LESS_1_4_0 - else if (state_space == OMPLProblemStateSpace::REAL_CONSTRAINTED_STATE_SPACE) - prob.extractor = tesseract_planning::ConstrainedStateSpaceExtractor; -#endif - else - throw std::runtime_error("OMPLMotionPlannerDefaultConfig: Unsupported configuration!"); + // Construct the OMPL state space for this manipulator + ompl::base::StateSpacePtr state_space_ptr; - if (prob.state_space == OMPLProblemStateSpace::REAL_STATE_SPACE) - { - // Construct the OMPL state space for this manipulator - ompl::base::StateSpacePtr state_space_ptr; + auto rss = std::make_shared(); + for (unsigned i = 0; i < dof; ++i) + rss->addDimension(joint_names[i], limits(i, 0), limits(i, 1)); - auto rss = std::make_shared(); - for (unsigned i = 0; i < dof; ++i) - rss->addDimension(joint_names[i], limits(i, 0), limits(i, 1)); + rss->setStateSamplerAllocator(createStateSamplerAllocator(prob)); - if (state_sampler_allocator) - { - rss->setStateSamplerAllocator( - [=](const ompl::base::StateSpace* space) { return state_sampler_allocator(space, prob); }); - } - else - { - Eigen::VectorXd weights = Eigen::VectorXd::Ones(dof); - rss->setStateSamplerAllocator( - [weights, limits](const ompl::base::StateSpace* state_space) -> ompl::base::StateSamplerPtr { - return allocWeightedRealVectorStateSampler(state_space, weights, limits); - }); - } + state_space_ptr = rss; - state_space_ptr = rss; + // Setup Longest Valid Segment + processLongestValidSegment(state_space_ptr, collision_check_config); - // Setup Longest Valid Segment - processLongestValidSegment(state_space_ptr, collision_check_config); + // Create Simple Setup from state space + prob.simple_setup = std::make_shared(state_space_ptr); - // Create Simple Setup from state space - prob.simple_setup = std::make_shared(state_space_ptr); + // Setup state validators + auto csvc = std::make_shared(); + ompl::base::StateValidityCheckerPtr svc_without_collision = createStateValidator(prob); + if (svc_without_collision != nullptr) + csvc->addStateValidator(svc_without_collision); + + auto svc_collision = createCollisionStateValidator(prob); + if (svc_collision != nullptr) + csvc->addStateValidator(std::move(svc_collision)); - // Setup state checking functionality - ompl::base::StateValidityCheckerPtr svc_without_collision = processStateValidator(prob); + prob.simple_setup->setStateValidityChecker(csvc); - // Setup motion validation (i.e. collision checking) - processMotionValidator(prob, svc_without_collision); + // Setup motion validation (i.e. collision checking) + auto mv = createMotionValidator(prob, svc_without_collision); + if (mv != nullptr) + prob.simple_setup->getSpaceInformation()->setMotionValidator(std::move(mv)); - // make sure the planners run until the time limit, and get the best possible solution - processOptimizationObjective(prob); + // make sure the planners run until the time limit, and get the best possible solution + if (prob.optimize) + { + auto obj = createOptimizationObjective(prob); + if (obj != nullptr) + prob.simple_setup->getProblemDefinition()->setOptimizationObjective(std::move(obj)); } } void OMPLDefaultPlanProfile::applyGoalStates(OMPLProblem& prob, const Eigen::Isometry3d& cartesian_waypoint, const MoveInstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - const std::vector& /*active_links*/, - int /*index*/) const + const tesseract_common::ManipulatorInfo& manip_info) { + if (prob.state_space != OMPLProblemStateSpace::REAL_STATE_SPACE) + throw std::runtime_error("OMPL profile only supports real state space"); + const auto dof = prob.manip->numJoints(); tesseract_common::KinematicLimits limits = prob.manip->getLimits(); @@ -394,84 +271,19 @@ void OMPLDefaultPlanProfile::applyGoalStates(OMPLProblem& prob, Eigen::Isometry3d tcp_frame_cwp = cartesian_waypoint * tcp_offset.inverse(); - if (prob.state_space == OMPLProblemStateSpace::REAL_STATE_SPACE) - { - /** @todo Need to add Descartes pose sample to ompl profile */ - tesseract_kinematics::KinGroupIKInput ik_input(tcp_frame_cwp, mi.working_frame, mi.tcp_frame); - tesseract_kinematics::IKSolutions joint_solutions = - std::dynamic_pointer_cast(prob.manip) - ->calcInvKin({ ik_input }, Eigen::VectorXd::Zero(dof)); - auto goal_states = std::make_shared(prob.simple_setup->getSpaceInformation()); - std::vector contact_map_vec( - static_cast(joint_solutions.size())); - - for (std::size_t i = 0; i < joint_solutions.size(); ++i) - { - Eigen::VectorXd& solution = joint_solutions[i]; - - // Check limits - if (tesseract_common::satisfiesLimits(solution, limits.joint_limits)) - { - tesseract_common::enforceLimits(solution, limits.joint_limits); - } - else - { - CONSOLE_BRIDGE_logDebug("In OMPLDefaultPlanProfile: Goal state has invalid bounds"); - } - - // Get discrete contact manager for testing provided start and end position - // This is required because collision checking happens in motion validators now - // instead of the isValid function to avoid unnecessary collision checks. - if (!checkStateInCollision(prob, solution, contact_map_vec[i])) - { - { - ompl::base::ScopedState<> goal_state(prob.simple_setup->getStateSpace()); - for (unsigned j = 0; j < dof; ++j) - goal_state[j] = solution[static_cast(j)]; - - goal_states->addState(goal_state); - } - - auto redundant_solutions = tesseract_kinematics::getRedundantSolutions( - solution, limits.joint_limits, prob.manip->getRedundancyCapableJointIndices()); - for (const auto& rs : redundant_solutions) - { - ompl::base::ScopedState<> goal_state(prob.simple_setup->getStateSpace()); - for (unsigned j = 0; j < dof; ++j) - goal_state[j] = rs[static_cast(j)]; - - goal_states->addState(goal_state); - } - } - } - - if (!goal_states->hasStates()) - { - for (std::size_t i = 0; i < contact_map_vec.size(); i++) - for (const auto& contact_vec : contact_map_vec[i]) - for (const auto& contact : contact_vec.second) - CONSOLE_BRIDGE_logError(("Solution: " + std::to_string(i) + " Links: " + contact.link_names[0] + ", " + - contact.link_names[1] + " Distance: " + std::to_string(contact.distance)) - .c_str()); - throw std::runtime_error("In OMPLDefaultPlanProfile: All goal states are either in collision or outside limits"); - } - prob.simple_setup->setGoal(goal_states); - } -} + /** @todo Need to add Descartes pose sample to ompl profile */ + tesseract_kinematics::KinGroupIKInput ik_input(tcp_frame_cwp, mi.working_frame, mi.tcp_frame); + tesseract_kinematics::IKSolutions joint_solutions = + std::dynamic_pointer_cast(prob.manip) + ->calcInvKin({ ik_input }, Eigen::VectorXd::Zero(dof)); + auto goal_states = std::make_shared(prob.simple_setup->getSpaceInformation()); + std::vector contact_map_vec(static_cast(joint_solutions.size())); -void OMPLDefaultPlanProfile::applyGoalStates(OMPLProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const MoveInstructionPoly& /*parent_instruction*/, - const tesseract_common::ManipulatorInfo& /*manip_info*/, - const std::vector& /*active_links*/, - int /*index*/) const -{ - const auto dof = prob.manip->numJoints(); - tesseract_common::KinematicLimits limits = prob.manip->getLimits(); - if (prob.state_space == OMPLProblemStateSpace::REAL_STATE_SPACE) + for (std::size_t i = 0; i < joint_solutions.size(); ++i) { + Eigen::VectorXd& solution = joint_solutions[i]; + // Check limits - Eigen::VectorXd solution = joint_waypoint; if (tesseract_common::satisfiesLimits(solution, limits.joint_limits)) { tesseract_common::enforceLimits(solution, limits.joint_limits); @@ -484,32 +296,90 @@ void OMPLDefaultPlanProfile::applyGoalStates(OMPLProblem& prob, // Get discrete contact manager for testing provided start and end position // This is required because collision checking happens in motion validators now // instead of the isValid function to avoid unnecessary collision checks. - tesseract_collision::ContactResultMap contact_map; - if (checkStateInCollision(prob, solution, contact_map)) + if (!checkStateInCollision(prob, solution, contact_map_vec[i])) { - CONSOLE_BRIDGE_logError("In OMPLDefaultPlanProfile: Goal state is in collision"); - for (const auto& contact_vec : contact_map) + { + ompl::base::ScopedState<> goal_state(prob.simple_setup->getStateSpace()); + for (unsigned j = 0; j < dof; ++j) + goal_state[j] = solution[static_cast(j)]; + + goal_states->addState(goal_state); + } + + auto redundant_solutions = tesseract_kinematics::getRedundantSolutions( + solution, limits.joint_limits, prob.manip->getRedundancyCapableJointIndices()); + for (const auto& rs : redundant_solutions) + { + ompl::base::ScopedState<> goal_state(prob.simple_setup->getStateSpace()); + for (unsigned j = 0; j < dof; ++j) + goal_state[j] = rs[static_cast(j)]; + + goal_states->addState(goal_state); + } + } + } + + if (!goal_states->hasStates()) + { + for (std::size_t i = 0; i < contact_map_vec.size(); i++) + for (const auto& contact_vec : contact_map_vec[i]) for (const auto& contact : contact_vec.second) - CONSOLE_BRIDGE_logError(("Links: " + contact.link_names[0] + ", " + contact.link_names[1] + - " Distance: " + std::to_string(contact.distance)) + CONSOLE_BRIDGE_logError(("Solution: " + std::to_string(i) + " Links: " + contact.link_names[0] + ", " + + contact.link_names[1] + " Distance: " + std::to_string(contact.distance)) .c_str()); - } + throw std::runtime_error("In OMPLDefaultPlanProfile: All goal states are either in collision or outside limits"); + } + prob.simple_setup->setGoal(goal_states); +} - ompl::base::ScopedState<> goal_state(prob.simple_setup->getStateSpace()); - for (unsigned i = 0; i < dof; ++i) - goal_state[i] = joint_waypoint[i]; +void OMPLDefaultPlanProfile::applyGoalStates(OMPLProblem& prob, const Eigen::VectorXd& joint_waypoint) +{ + if (prob.state_space != OMPLProblemStateSpace::REAL_STATE_SPACE) + throw std::runtime_error("OMPL profile only supports real state space"); - prob.simple_setup->setGoalState(goal_state); + const auto dof = prob.manip->numJoints(); + tesseract_common::KinematicLimits limits = prob.manip->getLimits(); + + // Check limits + Eigen::VectorXd solution = joint_waypoint; + if (tesseract_common::satisfiesLimits(solution, limits.joint_limits)) + { + tesseract_common::enforceLimits(solution, limits.joint_limits); + } + else + { + CONSOLE_BRIDGE_logDebug("In OMPLDefaultPlanProfile: Goal state has invalid bounds"); + } + + // Get discrete contact manager for testing provided start and end position + // This is required because collision checking happens in motion validators now + // instead of the isValid function to avoid unnecessary collision checks. + tesseract_collision::ContactResultMap contact_map; + if (checkStateInCollision(prob, solution, contact_map)) + { + CONSOLE_BRIDGE_logError("In OMPLDefaultPlanProfile: Goal state is in collision"); + for (const auto& contact_vec : contact_map) + for (const auto& contact : contact_vec.second) + CONSOLE_BRIDGE_logError(("Links: " + contact.link_names[0] + ", " + contact.link_names[1] + + " Distance: " + std::to_string(contact.distance)) + .c_str()); } + + ompl::base::ScopedState<> goal_state(prob.simple_setup->getStateSpace()); + for (unsigned i = 0; i < dof; ++i) + goal_state[i] = joint_waypoint[i]; + + prob.simple_setup->setGoalState(goal_state); } void OMPLDefaultPlanProfile::applyStartStates(OMPLProblem& prob, const Eigen::Isometry3d& cartesian_waypoint, const MoveInstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - const std::vector& /*active_links*/, - int /*index*/) const + const tesseract_common::ManipulatorInfo& manip_info) { + if (prob.state_space != OMPLProblemStateSpace::REAL_STATE_SPACE) + throw std::runtime_error("OMPL profile only supports real state space"); + const auto dof = prob.manip->numJoints(); tesseract_common::KinematicLimits limits = prob.manip->getLimits(); @@ -529,274 +399,178 @@ void OMPLDefaultPlanProfile::applyStartStates(OMPLProblem& prob, Eigen::Isometry3d tcp_frame_cwp = cartesian_waypoint * tcp.inverse(); - if (prob.state_space == OMPLProblemStateSpace::REAL_STATE_SPACE) - { - /** @todo Need to add Descartes pose sampler to ompl profile */ - /** @todo Need to also provide the seed instruction to use here */ - tesseract_kinematics::KinGroupIKInput ik_input(tcp_frame_cwp, mi.working_frame, mi.tcp_frame); - tesseract_kinematics::IKSolutions joint_solutions = - std::dynamic_pointer_cast(prob.manip) - ->calcInvKin({ ik_input }, Eigen::VectorXd::Zero(dof)); - bool found_start_state = false; - std::vector contact_map_vec(joint_solutions.size()); - - for (std::size_t i = 0; i < joint_solutions.size(); ++i) - { - Eigen::VectorXd& solution = joint_solutions[i]; - - // Check limits - if (tesseract_common::satisfiesLimits(solution, limits.joint_limits)) - { - tesseract_common::enforceLimits(solution, limits.joint_limits); - } - else - { - CONSOLE_BRIDGE_logDebug("In OMPLDefaultPlanProfile: Start state has invalid bounds"); - } - - // Get discrete contact manager for testing provided start and end position - // This is required because collision checking happens in motion validators now - // instead of the isValid function to avoid unnecessary collision checks. - if (!checkStateInCollision(prob, solution, contact_map_vec[i])) - { - found_start_state = true; - { - ompl::base::ScopedState<> start_state(prob.simple_setup->getStateSpace()); - for (unsigned j = 0; j < dof; ++j) - start_state[j] = solution[static_cast(j)]; - - prob.simple_setup->addStartState(start_state); - } - - auto redundant_solutions = tesseract_kinematics::getRedundantSolutions( - solution, limits.joint_limits, prob.manip->getRedundancyCapableJointIndices()); - for (const auto& rs : redundant_solutions) - { - ompl::base::ScopedState<> start_state(prob.simple_setup->getStateSpace()); - for (unsigned j = 0; j < dof; ++j) - start_state[j] = rs[static_cast(j)]; - - prob.simple_setup->addStartState(start_state); - } - } - } + /** @todo Need to add Descartes pose sampler to ompl profile */ + /** @todo Need to also provide the seed instruction to use here */ + tesseract_kinematics::KinGroupIKInput ik_input(tcp_frame_cwp, mi.working_frame, mi.tcp_frame); + tesseract_kinematics::IKSolutions joint_solutions = + std::dynamic_pointer_cast(prob.manip) + ->calcInvKin({ ik_input }, Eigen::VectorXd::Zero(dof)); + bool found_start_state = false; + std::vector contact_map_vec(joint_solutions.size()); - if (!found_start_state) - { - for (std::size_t i = 0; i < contact_map_vec.size(); i++) - for (const auto& contact_vec : contact_map_vec[i]) - for (const auto& contact : contact_vec.second) - CONSOLE_BRIDGE_logError(("Solution: " + std::to_string(i) + " Links: " + contact.link_names[0] + ", " + - contact.link_names[1] + " Distance: " + std::to_string(contact.distance)) - .c_str()); - throw std::runtime_error("In OMPLPlannerFreespaceConfig: All start states are either in collision or outside " - "limits"); - } - } -} - -void OMPLDefaultPlanProfile::applyStartStates(OMPLProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const MoveInstructionPoly& /*parent_instruction*/, - const tesseract_common::ManipulatorInfo& /*manip_info*/, - const std::vector& /*active_links*/, - int /*index*/) const -{ - const auto dof = prob.manip->numJoints(); - tesseract_common::KinematicLimits limits = prob.manip->getLimits(); - - if (prob.state_space == OMPLProblemStateSpace::REAL_STATE_SPACE) // NOLINT + for (std::size_t i = 0; i < joint_solutions.size(); ++i) { - Eigen::VectorXd solution = joint_waypoint; + Eigen::VectorXd& solution = joint_solutions[i]; + // Check limits if (tesseract_common::satisfiesLimits(solution, limits.joint_limits)) { tesseract_common::enforceLimits(solution, limits.joint_limits); } else { - CONSOLE_BRIDGE_logDebug("In OMPLDefaultPlanProfile: Start state is outside limits"); + CONSOLE_BRIDGE_logDebug("In OMPLDefaultPlanProfile: Start state has invalid bounds"); } // Get discrete contact manager for testing provided start and end position // This is required because collision checking happens in motion validators now // instead of the isValid function to avoid unnecessary collision checks. - tesseract_collision::ContactResultMap contact_map; - if (checkStateInCollision(prob, joint_waypoint, contact_map)) + if (!checkStateInCollision(prob, solution, contact_map_vec[i])) { - CONSOLE_BRIDGE_logError("In OMPLPlannerFreespaceConfig: Start state is in collision"); - for (const auto& contact_vec : contact_map) - for (const auto& contact : contact_vec.second) - CONSOLE_BRIDGE_logError(("Links: " + contact.link_names[0] + ", " + contact.link_names[1] + - " Distance: " + std::to_string(contact.distance)) - .c_str()); - } + found_start_state = true; + { + ompl::base::ScopedState<> start_state(prob.simple_setup->getStateSpace()); + for (unsigned j = 0; j < dof; ++j) + start_state[j] = solution[static_cast(j)]; - ompl::base::ScopedState<> start_state(prob.simple_setup->getStateSpace()); - for (unsigned i = 0; i < dof; ++i) - start_state[i] = joint_waypoint[i]; + prob.simple_setup->addStartState(start_state); + } - prob.simple_setup->addStartState(start_state); + auto redundant_solutions = tesseract_kinematics::getRedundantSolutions( + solution, limits.joint_limits, prob.manip->getRedundancyCapableJointIndices()); + for (const auto& rs : redundant_solutions) + { + ompl::base::ScopedState<> start_state(prob.simple_setup->getStateSpace()); + for (unsigned j = 0; j < dof; ++j) + start_state[j] = rs[static_cast(j)]; + + prob.simple_setup->addStartState(start_state); + } + } + } + + if (!found_start_state) + { + for (std::size_t i = 0; i < contact_map_vec.size(); i++) + for (const auto& contact_vec : contact_map_vec[i]) + for (const auto& contact : contact_vec.second) + CONSOLE_BRIDGE_logError(("Solution: " + std::to_string(i) + " Links: " + contact.link_names[0] + ", " + + contact.link_names[1] + " Distance: " + std::to_string(contact.distance)) + .c_str()); + throw std::runtime_error("In OMPLPlannerFreespaceConfig: All start states are either in collision or outside " + "limits"); } } -tinyxml2::XMLElement* OMPLDefaultPlanProfile::toXML(tinyxml2::XMLDocument& doc) const +void OMPLDefaultPlanProfile::applyStartStates(OMPLProblem& prob, const Eigen::VectorXd& joint_waypoint) { - Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " "); + if (prob.state_space != OMPLProblemStateSpace::REAL_STATE_SPACE) + throw std::runtime_error("OMPL profile only supports real state space"); - tinyxml2::XMLElement* xml_planner = doc.NewElement("Planner"); - xml_planner->SetAttribute("type", std::to_string(2).c_str()); - - tinyxml2::XMLElement* xml_ompl = doc.NewElement("OMPLPlanProfile"); + const auto dof = prob.manip->numJoints(); + tesseract_common::KinematicLimits limits = prob.manip->getLimits(); - tinyxml2::XMLElement* xml_ompl_planners = doc.NewElement("Planners"); + Eigen::VectorXd solution = joint_waypoint; - for (const auto& planner : planners) + if (tesseract_common::satisfiesLimits(solution, limits.joint_limits)) { - tinyxml2::XMLElement* xml_ompl_planner = doc.NewElement("Planner"); - xml_ompl_planner->SetAttribute("type", std::to_string(static_cast(planner->getType())).c_str()); - tinyxml2::XMLElement* xml_planner = planner->toXML(doc); - xml_ompl_planner->InsertEndChild(xml_planner); - xml_ompl_planners->InsertEndChild(xml_ompl_planner); + tesseract_common::enforceLimits(solution, limits.joint_limits); + } + else + { + CONSOLE_BRIDGE_logDebug("In OMPLDefaultPlanProfile: Start state is outside limits"); } - xml_ompl->InsertEndChild(xml_ompl_planners); - - tinyxml2::XMLElement* xml_state_space = doc.NewElement("StateSpace"); - xml_state_space->SetAttribute("type", std::to_string(static_cast(state_space)).c_str()); - xml_ompl->InsertEndChild(xml_state_space); - - tinyxml2::XMLElement* xml_planning_time = doc.NewElement("PlanningTime"); - xml_planning_time->SetText(planning_time); - xml_ompl->InsertEndChild(xml_planning_time); - - tinyxml2::XMLElement* xml_max_solutions = doc.NewElement("MaxSolutions"); - xml_max_solutions->SetText(max_solutions); - xml_ompl->InsertEndChild(xml_max_solutions); - - tinyxml2::XMLElement* xml_simplify = doc.NewElement("Simplify"); - xml_simplify->SetText(simplify); - xml_ompl->InsertEndChild(xml_simplify); - - tinyxml2::XMLElement* xml_optimize = doc.NewElement("Optimize"); - xml_optimize->SetText(optimize); - xml_ompl->InsertEndChild(xml_optimize); - - /// @todo Update XML - // tinyxml2::XMLElement* xml_collision_check = doc.NewElement("CollisionCheck"); - // xml_collision_check->SetText(collision_check); - // xml_ompl->InsertEndChild(xml_collision_check); - - // tinyxml2::XMLElement* xml_collision_continuous = doc.NewElement("CollisionContinuous"); - // xml_collision_continuous->SetText(collision_continuous); - // xml_ompl->InsertEndChild(xml_collision_continuous); - - // tinyxml2::XMLElement* xml_collision_safety_margin = doc.NewElement("CollisionSafetyMargin"); - // xml_collision_safety_margin->SetText(collision_safety_margin); - // xml_ompl->InsertEndChild(xml_collision_safety_margin); - - // tinyxml2::XMLElement* xml_long_valid_seg_frac = doc.NewElement("LongestValidSegmentFraction"); - // xml_long_valid_seg_frac->SetText(longest_valid_segment_fraction); - // xml_ompl->InsertEndChild(xml_long_valid_seg_frac); - - // tinyxml2::XMLElement* xml_long_valid_seg_len = doc.NewElement("LongestValidSegmentLength"); - // xml_long_valid_seg_len->SetText(longest_valid_segment_length); - // xml_ompl->InsertEndChild(xml_long_valid_seg_len); + // Get discrete contact manager for testing provided start and end position + // This is required because collision checking happens in motion validators now + // instead of the isValid function to avoid unnecessary collision checks. + tesseract_collision::ContactResultMap contact_map; + if (checkStateInCollision(prob, joint_waypoint, contact_map)) + { + CONSOLE_BRIDGE_logError("In OMPLPlannerFreespaceConfig: Start state is in collision"); + for (const auto& contact_vec : contact_map) + for (const auto& contact : contact_vec.second) + CONSOLE_BRIDGE_logError(("Links: " + contact.link_names[0] + ", " + contact.link_names[1] + + " Distance: " + std::to_string(contact.distance)) + .c_str()); + } - // TODO: Add plugins for state_sampler_allocator, optimization_objective_allocator, svc_allocator, - // mv_allocator + ompl::base::ScopedState<> start_state(prob.simple_setup->getStateSpace()); + for (unsigned i = 0; i < dof; ++i) + start_state[i] = joint_waypoint[i]; - xml_planner->InsertEndChild(xml_ompl); + prob.simple_setup->addStartState(start_state); +} - return xml_planner; +std::function +OMPLDefaultPlanProfile::createStateSamplerAllocator(OMPLProblem& prob) const +{ + Eigen::MatrixX2d limits = prob.manip->getLimits().joint_limits; + Eigen::VectorXd weights = Eigen::VectorXd::Ones(prob.manip->numJoints()); + return [weights, limits](const ompl::base::StateSpace* state_space) -> ompl::base::StateSamplerPtr { + return allocWeightedRealVectorStateSampler(state_space, weights, limits); + }; } -ompl::base::StateValidityCheckerPtr OMPLDefaultPlanProfile::processStateValidator(OMPLProblem& prob) const +std::unique_ptr +OMPLDefaultPlanProfile::createStateValidator(OMPLProblem& /*prob*/) const { - ompl::base::StateValidityCheckerPtr svc_without_collision; - auto csvc = std::make_shared(); - if (svc_allocator != nullptr) - { - svc_without_collision = svc_allocator(prob.simple_setup->getSpaceInformation(), prob); - csvc->addStateValidator(svc_without_collision); - } + return nullptr; +} +std::unique_ptr +OMPLDefaultPlanProfile::createCollisionStateValidator(OMPLProblem& prob) const +{ if (collision_check_config.type == tesseract_collision::CollisionEvaluatorType::DISCRETE || collision_check_config.type == tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE) { - auto svc = std::make_shared( + return std::make_unique( prob.simple_setup->getSpaceInformation(), *prob.env, prob.manip, collision_check_config, prob.extractor); - csvc->addStateValidator(svc); } - prob.simple_setup->setStateValidityChecker(csvc); - return svc_without_collision; + return nullptr; } -void OMPLDefaultPlanProfile::processMotionValidator( - OMPLProblem& prob, - const ompl::base::StateValidityCheckerPtr& svc_without_collision) const +std::unique_ptr +OMPLDefaultPlanProfile::createMotionValidator(OMPLProblem& prob, + const ompl::base::StateValidityCheckerPtr& svc_without_collision) const { - if (mv_allocator != nullptr) - { - auto mv = mv_allocator(prob.simple_setup->getSpaceInformation(), prob); - prob.simple_setup->getSpaceInformation()->setMotionValidator(mv); - } - else + if (collision_check_config.type != tesseract_collision::CollisionEvaluatorType::NONE) { - if (collision_check_config.type != tesseract_collision::CollisionEvaluatorType::NONE) + if (collision_check_config.type == tesseract_collision::CollisionEvaluatorType::CONTINUOUS || + collision_check_config.type == tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS) { - ompl::base::MotionValidatorPtr mv; - if (collision_check_config.type == tesseract_collision::CollisionEvaluatorType::CONTINUOUS || - collision_check_config.type == tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS) - { - mv = std::make_shared(prob.simple_setup->getSpaceInformation(), + return std::make_unique(prob.simple_setup->getSpaceInformation(), svc_without_collision, *prob.env, prob.manip, collision_check_config, prob.extractor); - } - else - { - // Collision checking is preformed using the state validator which this calls. - mv = std::make_shared(prob.simple_setup->getSpaceInformation()); - } - prob.simple_setup->getSpaceInformation()->setMotionValidator(mv); } + + // Collision checking is preformed using the state validator which this calls. + return std::make_unique(prob.simple_setup->getSpaceInformation()); } + + return nullptr; } -void OMPLDefaultPlanProfile::processOptimizationObjective(OMPLProblem& prob) const +std::unique_ptr +OMPLDefaultPlanProfile::createOptimizationObjective(OMPLProblem& prob) const { - if (optimization_objective_allocator) - { - prob.simple_setup->getProblemDefinition()->setOptimizationObjective( - optimization_objective_allocator(prob.simple_setup->getSpaceInformation(), prob)); - } - else if (prob.optimize) - { - // Add default optimization function to minimize path length - prob.simple_setup->getProblemDefinition()->setOptimizationObjective( - std::make_shared(prob.simple_setup->getSpaceInformation())); - } + return std::make_unique(prob.simple_setup->getSpaceInformation()); } template void OMPLDefaultPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlanProfile); - ar& BOOST_SERIALIZATION_NVP(state_space); ar& BOOST_SERIALIZATION_NVP(planning_time); ar& BOOST_SERIALIZATION_NVP(max_solutions); ar& BOOST_SERIALIZATION_NVP(simplify); ar& BOOST_SERIALIZATION_NVP(optimize); ar& BOOST_SERIALIZATION_NVP(planners); ar& BOOST_SERIALIZATION_NVP(collision_check_config); - // ar& BOOST_SERIALIZATION_NVP(state_sampler_allocator); - // ar& BOOST_SERIALIZATION_NVP(optimization_objective_allocator); - // ar& BOOST_SERIALIZATION_NVP(svc_allocator); - // ar& BOOST_SERIALIZATION_NVP(mv_allocator); } } // namespace tesseract_planning diff --git a/tesseract_motion_planners/ompl/src/serialize.cpp b/tesseract_motion_planners/ompl/src/serialize.cpp deleted file mode 100644 index bcfe772eaa..0000000000 --- a/tesseract_motion_planners/ompl/src/serialize.cpp +++ /dev/null @@ -1,83 +0,0 @@ -/** - * @file serialize.cpp - * @brief - * - * @author Tyler Marr - * @date August 20, 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 -#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 OMPLPlanProfile& plan_profile) -{ - auto doc = std::make_shared(); - tinyxml2::XMLElement* xml_root = doc->NewElement("Profile"); - xml_root->SetAttribute("name", "OMPLDefaultProfile"); - 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 OMPLPlanProfile& 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 OMPLPlanProfile& plan_profile) -{ - std::shared_ptr doc = toXMLDocument(plan_profile); - tinyxml2::XMLPrinter printer; - doc->Print(&printer); - return std::string{ printer.CStr() }; -} - -} // namespace tesseract_planning diff --git a/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp b/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp index 45052b5b2b..2c304a7e7d 100644 --- a/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp +++ b/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp @@ -63,8 +63,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include -#include #include #include @@ -115,39 +113,6 @@ static void addBox(tesseract_environment::Environment& env) env.applyCommand(std::make_shared(link_1, joint_1)); } -TEST(TesseractPlanningOMPLSerializeUnit, SerializeOMPLDefaultPlanToXml) // NOLINT -{ - // Write program to file - OMPLDefaultPlanProfile plan_profile; - plan_profile.simplify = true; - plan_profile.planners.push_back(std::make_shared()); - plan_profile.planners.push_back(std::make_shared()); - plan_profile.planners.push_back(std::make_shared()); - plan_profile.planners.push_back(std::make_shared()); - plan_profile.planners.push_back(std::make_shared()); - plan_profile.planners.push_back(std::make_shared()); - plan_profile.planners.push_back(std::make_shared()); - plan_profile.planners.push_back(std::make_shared()); - plan_profile.planners.push_back(std::make_shared()); - plan_profile.planners.push_back(std::make_shared()); - plan_profile.planners.push_back(std::make_shared()); - plan_profile.planners.push_back(std::make_shared()); - plan_profile.planners.push_back(std::make_shared()); - plan_profile.planners.push_back(std::make_shared()); - - EXPECT_TRUE(toXMLFile(plan_profile, tesseract_common::getTempPath() + "ompl_default_plan_example_input.xml")); - - // Import file - OMPLDefaultPlanProfile imported_plan_profile = omplPlanFromXMLFile(tesseract_common::getTempPath() + "ompl_default_" - "plan_example_" - "input.xml"); - - // Re-write file and compare changed from default term - EXPECT_TRUE( - toXMLFile(imported_plan_profile, tesseract_common::getTempPath() + "ompl_default_plan_example_input2.xml")); - EXPECT_TRUE(plan_profile.simplify == imported_plan_profile.simplify); -} - template class OMPLTestFixture : public ::testing::Test {