diff --git a/tesseract_examples/src/freespace_hybrid_example.cpp b/tesseract_examples/src/freespace_hybrid_example.cpp index c674c79d83..c89589adf3 100644 --- a/tesseract_examples/src/freespace_hybrid_example.cpp +++ b/tesseract_examples/src/freespace_hybrid_example.cpp @@ -49,7 +49,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include +#include #include #include #include @@ -197,11 +197,11 @@ bool FreespaceHybridExample::run() auto profiles = std::make_shared(); // Create OMPL Profile - auto ompl_profile = std::make_shared(); + auto ompl_profile = std::make_shared(); auto ompl_planner_config = std::make_shared(); ompl_planner_config->range = range_; - ompl_profile->planning_time = planning_time_; - ompl_profile->planners = { ompl_planner_config, ompl_planner_config }; + ompl_profile->solver_config.planning_time = planning_time_; + ompl_profile->solver_config.planners = { ompl_planner_config, ompl_planner_config }; profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "FREESPACE", ompl_profile); diff --git a/tesseract_examples/src/freespace_ompl_example.cpp b/tesseract_examples/src/freespace_ompl_example.cpp index 199407c2ab..f843a581cb 100644 --- a/tesseract_examples/src/freespace_ompl_example.cpp +++ b/tesseract_examples/src/freespace_ompl_example.cpp @@ -48,7 +48,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include +#include #include #include @@ -180,11 +180,11 @@ bool FreespaceOMPLExample::run() auto executor = factory.createTaskComposerExecutor("TaskflowExecutor"); // Create OMPL Profile - auto ompl_profile = std::make_shared(); + auto ompl_profile = std::make_shared(); auto ompl_planner_config = std::make_shared(); ompl_planner_config->range = range_; - ompl_profile->planning_time = planning_time_; - ompl_profile->planners = { ompl_planner_config, ompl_planner_config }; + ompl_profile->solver_config.planning_time = planning_time_; + ompl_profile->solver_config.planners = { ompl_planner_config, ompl_planner_config }; // Create profile dictionary auto profiles = std::make_shared(); diff --git a/tesseract_motion_planners/examples/freespace_example.cpp b/tesseract_motion_planners/examples/freespace_example.cpp index d8f9993dc3..373e91e4ff 100644 --- a/tesseract_motion_planners/examples/freespace_example.cpp +++ b/tesseract_motion_planners/examples/freespace_example.cpp @@ -34,7 +34,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include +#include #include #include @@ -114,7 +114,7 @@ int main(int /*argc*/, char** /*argv*/) } // Create Profiles - auto ompl_plan_profile = std::make_shared(); + auto ompl_plan_profile = std::make_shared(); auto trajopt_plan_profile = std::make_shared(); auto trajopt_composite_profile = std::make_shared(); auto trajopt_solver_profile = std::make_shared(); diff --git a/tesseract_motion_planners/ompl/CMakeLists.txt b/tesseract_motion_planners/ompl/CMakeLists.txt index ae481465b3..220864a8ee 100644 --- a/tesseract_motion_planners/ompl/CMakeLists.txt +++ b/tesseract_motion_planners/ompl/CMakeLists.txt @@ -4,19 +4,17 @@ link_directories(BEFORE ${OMPL_LIBRARY_DIRS}) # OMPL Freespace Planning Interface set(OMPL_SRC - src/ompl_motion_planner.cpp + src/profile/ompl_profile.cpp + src/profile/ompl_real_vector_plan_profile.cpp + src/compound_state_validator.cpp src/continuous_motion_validator.cpp src/discrete_motion_validator.cpp - src/weighted_real_vector_state_sampler.cpp + src/ompl_motion_planner.cpp src/ompl_planner_configurator.cpp - src/ompl_problem.cpp - src/profile/ompl_profile.cpp - src/profile/ompl_default_plan_profile.cpp - src/utils.cpp + src/ompl_solver_config.cpp src/state_collision_validator.cpp - src/compound_state_validator.cpp - src/serialize.cpp - src/deserialize.cpp) + src/utils.cpp + src/weighted_real_vector_state_sampler.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/fwd.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/fwd.h index 63075fe17c..e487e7448c 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/fwd.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/fwd.h @@ -33,9 +33,8 @@ struct PRMstarConfigurator; struct LazyPRMstarConfigurator; struct SPARSConfigurator; -// ompl_problem.h -enum class OMPLProblemStateSpace; -struct OMPLProblem; +// ompl_solver_config.h +struct OMPLSolverConfig; // state_collision_validator.h class StateCollisionValidator; @@ -45,7 +44,7 @@ class WeightedRealVectorStateSampler; // profile class OMPLPlanProfile; -class OMPLDefaultPlanProfile; +class OMPLRealVectorPlanProfile; } // namespace tesseract_planning #endif // TESSERACT_MOTION_PLANNERS_OMPL_FWD_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..1857bfe293 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 @@ -33,25 +33,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include - #include #include - -namespace ompl::tools -{ -class ParallelPlan; -} // namespace ompl::tools +#include namespace tesseract_planning { -struct OMPLProblem; -struct OMPLProblemConfig -{ - std::shared_ptr problem; - boost::uuids::uuid start_uuid{}; - boost::uuids::uuid end_uuid{}; -}; - /** * @brief This planner is intended to provide an easy to use interface to OMPL for freespace planning. It is made to * take a start and end point and automate the generation of the OMPL problem. @@ -89,19 +76,25 @@ class OMPLMotionPlanner : public MotionPlanner std::unique_ptr clone() const override; - virtual std::vector createProblems(const PlannerRequest& request) const; - 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; + /** + * @brief This a utility function for assigning the trajectory to the results data structure + * @param output The results data structure to update + * @param start_uuid The start uuid of the provided trajectory + * @param end_uuid The end uuid of the provided trajectory + * @param start_index The start index to begin search for start uuid + * @param joint_names The joint names + * @param traj The provided trajectory + * @param format_result_as_input Indicate if the result should be formated as input + * @return The start index for the next segment + */ + static long assignTrajectory(tesseract_planning::CompositeInstruction& output, + boost::uuids::uuid start_uuid, + boost::uuids::uuid end_uuid, + long start_index, + const std::vector& joint_names, + const tesseract_common::TrajArray& traj, + bool format_result_as_input); }; } // namespace tesseract_planning diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_planner_configurator.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_planner_configurator.h index c4f3dcfe87..9699ce6c42 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_planner_configurator.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_planner_configurator.h @@ -39,12 +39,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -namespace tinyxml2 -{ -class XMLElement; // NOLINT -class XMLDocument; -} // namespace tinyxml2 - namespace ompl::base { class Planner; @@ -88,8 +82,6 @@ struct OMPLPlannerConfigurator virtual OMPLPlannerType getType() const = 0; - virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; - protected: friend class boost::serialization::access; template @@ -104,7 +96,6 @@ struct SBLConfigurator : public OMPLPlannerConfigurator SBLConfigurator& operator=(const SBLConfigurator&) = default; SBLConfigurator(SBLConfigurator&&) = default; SBLConfigurator& operator=(SBLConfigurator&&) = default; - SBLConfigurator(const tinyxml2::XMLElement& xml_element); /** @brief Max motion added to tree */ double range = 0; @@ -114,9 +105,6 @@ struct SBLConfigurator : public OMPLPlannerConfigurator OMPLPlannerType getType() const override; - /** @brief Serialize planner to xml */ - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; - protected: friend class boost::serialization::access; template @@ -131,7 +119,6 @@ struct ESTConfigurator : public OMPLPlannerConfigurator ESTConfigurator& operator=(const ESTConfigurator&) = default; ESTConfigurator(ESTConfigurator&&) = default; ESTConfigurator& operator=(ESTConfigurator&&) = default; - ESTConfigurator(const tinyxml2::XMLElement& xml_element); /** @brief Max motion added to tree */ double range = 0; @@ -144,9 +131,6 @@ struct ESTConfigurator : public OMPLPlannerConfigurator OMPLPlannerType getType() const override; - /** @brief Serialize planner to xml */ - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; - protected: friend class boost::serialization::access; template @@ -161,7 +145,6 @@ struct LBKPIECE1Configurator : public OMPLPlannerConfigurator LBKPIECE1Configurator& operator=(const LBKPIECE1Configurator&) = default; LBKPIECE1Configurator(LBKPIECE1Configurator&&) = default; LBKPIECE1Configurator& operator=(LBKPIECE1Configurator&&) = default; - LBKPIECE1Configurator(const tinyxml2::XMLElement& xml_element); /** @brief Max motion added to tree */ double range = 0; @@ -177,9 +160,6 @@ struct LBKPIECE1Configurator : public OMPLPlannerConfigurator OMPLPlannerType getType() const override; - /** @brief Serialize planner to xml */ - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; - protected: friend class boost::serialization::access; template @@ -194,7 +174,6 @@ struct BKPIECE1Configurator : public OMPLPlannerConfigurator BKPIECE1Configurator& operator=(const BKPIECE1Configurator&) = default; BKPIECE1Configurator(BKPIECE1Configurator&&) = default; BKPIECE1Configurator& operator=(BKPIECE1Configurator&&) = default; - BKPIECE1Configurator(const tinyxml2::XMLElement& xml_element); /** @brief Max motion added to tree */ double range = 0; @@ -213,9 +192,6 @@ struct BKPIECE1Configurator : public OMPLPlannerConfigurator OMPLPlannerType getType() const override; - /** @brief Serialize planner to xml */ - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; - protected: friend class boost::serialization::access; template @@ -230,7 +206,6 @@ struct KPIECE1Configurator : public OMPLPlannerConfigurator KPIECE1Configurator& operator=(const KPIECE1Configurator&) = default; KPIECE1Configurator(KPIECE1Configurator&&) = default; KPIECE1Configurator& operator=(KPIECE1Configurator&&) = default; - KPIECE1Configurator(const tinyxml2::XMLElement& xml_element); /** @brief Max motion added to tree */ double range = 0; @@ -252,9 +227,6 @@ struct KPIECE1Configurator : public OMPLPlannerConfigurator OMPLPlannerType getType() const override; - /** @brief Serialize planner to xml */ - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; - protected: friend class boost::serialization::access; template @@ -269,7 +241,6 @@ struct BiTRRTConfigurator : public OMPLPlannerConfigurator BiTRRTConfigurator& operator=(const BiTRRTConfigurator&) = default; BiTRRTConfigurator(BiTRRTConfigurator&&) = default; BiTRRTConfigurator& operator=(BiTRRTConfigurator&&) = default; - BiTRRTConfigurator(const tinyxml2::XMLElement& xml_element); /** @brief Max motion added to tree */ double range = 0; @@ -295,9 +266,6 @@ struct BiTRRTConfigurator : public OMPLPlannerConfigurator OMPLPlannerType getType() const override; - /** @brief Serialize planner to xml */ - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; - protected: friend class boost::serialization::access; template @@ -312,7 +280,6 @@ struct RRTConfigurator : public OMPLPlannerConfigurator RRTConfigurator& operator=(const RRTConfigurator&) = default; RRTConfigurator(RRTConfigurator&&) = default; RRTConfigurator& operator=(RRTConfigurator&&) = default; - RRTConfigurator(const tinyxml2::XMLElement& xml_element); /** @brief Max motion added to tree */ double range = 0; @@ -325,9 +292,6 @@ struct RRTConfigurator : public OMPLPlannerConfigurator OMPLPlannerType getType() const override; - /** @brief Serialize planner to xml */ - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; - protected: friend class boost::serialization::access; template @@ -342,7 +306,6 @@ struct RRTConnectConfigurator : public OMPLPlannerConfigurator RRTConnectConfigurator& operator=(const RRTConnectConfigurator&) = default; RRTConnectConfigurator(RRTConnectConfigurator&&) = default; RRTConnectConfigurator& operator=(RRTConnectConfigurator&&) = default; - RRTConnectConfigurator(const tinyxml2::XMLElement& xml_element); /** @brief Max motion added to tree */ double range = 0; @@ -352,9 +315,6 @@ struct RRTConnectConfigurator : public OMPLPlannerConfigurator OMPLPlannerType getType() const override; - /** @brief Serialize planner to xml */ - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; - protected: friend class boost::serialization::access; template @@ -369,7 +329,6 @@ struct RRTstarConfigurator : public OMPLPlannerConfigurator RRTstarConfigurator& operator=(const RRTstarConfigurator&) = default; RRTstarConfigurator(RRTstarConfigurator&&) = default; RRTstarConfigurator& operator=(RRTstarConfigurator&&) = default; - RRTstarConfigurator(const tinyxml2::XMLElement& xml_element); /** @brief Max motion added to tree */ double range = 0; @@ -385,9 +344,6 @@ struct RRTstarConfigurator : public OMPLPlannerConfigurator OMPLPlannerType getType() const override; - /** @brief Serialize planner to xml */ - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; - protected: friend class boost::serialization::access; template @@ -402,7 +358,6 @@ struct TRRTConfigurator : public OMPLPlannerConfigurator TRRTConfigurator& operator=(const TRRTConfigurator&) = default; TRRTConfigurator(TRRTConfigurator&&) = default; TRRTConfigurator& operator=(TRRTConfigurator&&) = default; - TRRTConfigurator(const tinyxml2::XMLElement& xml_element); /** @brief Max motion added to tree */ double range = 0; @@ -427,9 +382,6 @@ struct TRRTConfigurator : public OMPLPlannerConfigurator OMPLPlannerType getType() const override; - /** @brief Serialize planner to xml */ - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; - protected: friend class boost::serialization::access; template @@ -444,7 +396,6 @@ struct PRMConfigurator : public OMPLPlannerConfigurator PRMConfigurator& operator=(const PRMConfigurator&) = default; PRMConfigurator(PRMConfigurator&&) = default; PRMConfigurator& operator=(PRMConfigurator&&) = default; - PRMConfigurator(const tinyxml2::XMLElement& xml_element); /** @brief Use k nearest neighbors. */ int max_nearest_neighbors = 10; @@ -454,9 +405,6 @@ struct PRMConfigurator : public OMPLPlannerConfigurator OMPLPlannerType getType() const override; - /** @brief Serialize planner to xml */ - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; - protected: friend class boost::serialization::access; template @@ -471,16 +419,12 @@ struct PRMstarConfigurator : public OMPLPlannerConfigurator PRMstarConfigurator& operator=(const PRMstarConfigurator&) = default; PRMstarConfigurator(PRMstarConfigurator&&) = default; PRMstarConfigurator& operator=(PRMstarConfigurator&&) = default; - PRMstarConfigurator(const tinyxml2::XMLElement& xml_element); /** @brief Create the planner */ ompl::base::PlannerPtr create(ompl::base::SpaceInformationPtr si) const override; OMPLPlannerType getType() const override; - /** @brief Serialize planner to xml */ - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; - protected: friend class boost::serialization::access; template @@ -495,16 +439,12 @@ struct LazyPRMstarConfigurator : public OMPLPlannerConfigurator LazyPRMstarConfigurator& operator=(const LazyPRMstarConfigurator&) = default; LazyPRMstarConfigurator(LazyPRMstarConfigurator&&) = default; LazyPRMstarConfigurator& operator=(LazyPRMstarConfigurator&&) = default; - LazyPRMstarConfigurator(const tinyxml2::XMLElement& xml_element); /** @brief Create the planner */ ompl::base::PlannerPtr create(ompl::base::SpaceInformationPtr si) const override; OMPLPlannerType getType() const override; - /** @brief Serialize planner to xml */ - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; - protected: friend class boost::serialization::access; template @@ -519,7 +459,6 @@ struct SPARSConfigurator : public OMPLPlannerConfigurator SPARSConfigurator& operator=(const SPARSConfigurator&) = default; SPARSConfigurator(SPARSConfigurator&&) = default; SPARSConfigurator& operator=(SPARSConfigurator&&) = default; - SPARSConfigurator(const tinyxml2::XMLElement& xml_element); /** @brief The maximum number of failures before terminating the algorithm */ int max_failures = 1000; @@ -538,9 +477,6 @@ struct SPARSConfigurator : public OMPLPlannerConfigurator OMPLPlannerType getType() const override; - /** @brief Serialize planner to xml */ - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; - protected: friend class boost::serialization::access; template 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 deleted file mode 100644 index 5717095400..0000000000 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_problem.h +++ /dev/null @@ -1,174 +0,0 @@ -/** - * @file ompl_problem.h - * @brief Tesseract OMPL problem definition - * - * @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_OMPL_OMPL_PROBLEM_H -#define TESSERACT_MOTION_PLANNERS_OMPL_OMPL_PROBLEM_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include -#include - -#include -#include -#include - -namespace ompl::base -{ -class SpaceInformation; -using SpaceInformationPtr = std::shared_ptr; -class StateValidityChecker; -using StateValidityCheckerPtr = std::shared_ptr; -class StateSpace; -using StateSpacePtr = std::shared_ptr; -class StateSampler; -using StateSamplerPtr = std::shared_ptr; -class OptimizationObjective; -using OptimizationObjectivePtr = std::shared_ptr; -class MotionValidator; -using MotionValidatorPtr = std::shared_ptr; -} // namespace ompl::base - -namespace ompl::geometric -{ -class SimpleSetup; -using SimpleSetupPtr = std::shared_ptr; -} // namespace ompl::geometric - -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, -#ifndef OMPL_LESS_1_4_0 - REAL_CONSTRAINTED_STATE_SPACE, -#endif - SE3_STATE_SPACE, -}; - -struct OMPLProblem -{ - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - using UPtr = std::unique_ptr; - using ConstUPtr = std::unique_ptr; - - // LCOV_EXCL_START - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - // LCOV_EXCL_STOP - - // These are required for Tesseract to configure ompl - std::shared_ptr env; - tesseract_scene_graph::SceneState env_state; - - // This is used to verify that start and goal states are not in collision - std::shared_ptr contact_checker; - - // Problem Configuration - OMPLProblemStateSpace state_space{ OMPLProblemStateSpace::REAL_STATE_SPACE }; - - // Kinematic Objects - std::shared_ptr manip; - - /** @brief Max planning time allowed in seconds */ - double planning_time = 5.0; - - /** @brief The max number of solutions. If max solutions are hit it will exit even if other threads are running. */ - int max_solutions = 10; - - /** - * @brief Simplify trajectory. - * - * Note: If set to true it ignores n_output_states and returns the simplest trajectory. - */ - bool simplify = false; - - /** - * @brief Number of states in the output trajectory - * Note: This is ignored if the simplify is set to true. - * Note: The trajectory can be longer if original trajectory is longer and reducing the number of states causes - * the solution to be invalid. - */ - int n_output_states = 20; - - /** - * @brief This uses all available planning time to create the most optimized trajectory given the objective function. - * - * This is required because not all OMPL planners are optimize graph planners. If the planner you choose is an - * optimize graph planner then setting this to true has no affect. In the case of non-optimize planners they still - * use the OptimizeObjective function but only when searching the graph to find the most optimize solution based - * on the provided optimize objective function. In the case of these type of planners like RRT and RRTConnect if set - * to true it will leverage all planning time to keep finding solutions up to your max solutions count to find the - * most optimal solution. - */ - bool optimize = true; - - /** @brief OMPL problem to be solved ***REQUIRED*** */ - ompl::geometric::SimpleSetupPtr simple_setup; - - /** - * @brief The planner configurators ***REQUIRED*** - * - * This will create a new thread for each planner configurator provided. T - */ - std::vector> planners{}; - - /** - * @brief This will extract an Eigen::VectorXd from the OMPL State ***REQUIRED*** - */ - OMPLStateExtractor extractor{}; - - /** - * @brief Convert the path stored in simple_setup to tesseract trajectory - * This is required because the motion planner is not aware of the state space type. - * @return Tesseract Trajectory - */ - tesseract_common::TrajArray getTrajectory() const; -}; - -} // namespace tesseract_planning - -#endif // TESSERACT_MOTION_PLANNERS_OMPL_OMPL_PROBLEM_H diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_solver_config.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_solver_config.h new file mode 100644 index 0000000000..31632f660b --- /dev/null +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_solver_config.h @@ -0,0 +1,90 @@ +/** + * @file ompl_solver_config.h + * @brief Tesseract OMPL solver config + * + * @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_OMPL_OMPL_SOLVER_CONFIG_H +#define TESSERACT_MOTION_PLANNERS_OMPL_OMPL_SOLVER_CONFIG_H + +#include +#include +#include +#include + +namespace tesseract_planning +{ +struct OMPLPlannerConfigurator; +struct OMPLSolverConfig +{ + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + using UPtr = std::unique_ptr; + using ConstUPtr = std::unique_ptr; + + virtual ~OMPLSolverConfig() = default; + + /** @brief Max planning time allowed in seconds */ + double planning_time = 5.0; + + /** @brief The max number of solutions. If max solutions are hit it will exit even if other threads are running. */ + int max_solutions = 10; + + /** + * @brief Simplify trajectory. + * + * Note: If set to true it ignores n_output_states and returns the simplest trajectory. + */ + bool simplify = false; + + /** + * @brief This uses all available planning time to create the most optimized trajectory given the objective function. + * + * This is required because not all OMPL planners are optimize graph planners. If the planner you choose is an + * optimize graph planner then setting this to true has no affect. In the case of non-optimize planners they still + * use the OptimizeObjective function but only when searching the graph to find the most optimize solution based + * on the provided optimize objective function. In the case of these type of planners like RRT and RRTConnect if set + * to true it will leverage all planning time to keep finding solutions up to your max solutions count to find the + * most optimal solution. + */ + bool optimize = true; + + /** + * @brief The planner configurators + * + * This defaults to two RRTConnectConfigurator + * + * This will create a new thread for each planner configurator provided. + */ + std::vector> planners; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT +}; + +} // namespace tesseract_planning + +BOOST_CLASS_EXPORT_KEY(tesseract_planning::OMPLSolverConfig) + +#endif // TESSERACT_MOTION_PLANNERS_OMPL_OMPL_SOLVER_CONFIG_H 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 deleted file mode 100644 index 54d3469cf8..0000000000 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h +++ /dev/null @@ -1,167 +0,0 @@ -/** - * @file ompl_default_plan_profile.h - * @brief Tesseract OMPL default plan profile - * - * @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_OMPL_OMPL_DEFAULT_PLAN_PROFILE_H -#define TESSERACT_MOTION_PLANNERS_OMPL_OMPL_DEFAULT_PLAN_PROFILE_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include - -#include - -#include -#include - -namespace ompl::base -{ -class StateValidityChecker; -using StateValidityCheckerPtr = std::shared_ptr; -} // namespace ompl::base - -namespace tesseract_planning -{ -struct OMPLPlannerConfigurator; -/** - * @brief OMPL does not support the concept of multi waypoint planning like descartes and trajopt. Because of this - * every plan instruction will be its a seperate ompl motion plan and therefore planning information is relevent - * for this motion planner in the profile. - */ -class OMPLDefaultPlanProfile : public OMPLPlanProfile -{ -public: - using Ptr = std::shared_ptr; - 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; - - /** @brief The max number of solutions. If max solutions are hit it will exit even if other threads are running. */ - int max_solutions = 10; - - /** - * @brief Simplify trajectory. - * - * Note: If set to true it ignores n_output_states and returns the simplest trajectory. - */ - bool simplify = false; - - /** - * @brief This uses all available planning time to create the most optimized trajectory given the objective function. - * - * This is required because not all OMPL planners are optimize graph planners. If the planner you choose is an - * optimize graph planner then setting this to true has no affect. In the case of non-optimize planners they still - * use the OptimizeObjective function but only when searching the graph to find the most optimize solution based - * on the provided optimize objective function. In the case of these type of planners like RRT and RRTConnect if set - * to true it will leverage all planning time to keep finding solutions up to your max solutions count to find the - * most optimal solution. - */ - bool optimize = true; - - /** - * @brief The planner configurators - * - * This defaults to two RRTConnectConfigurator - * - * This will create a new thread for each planner configurator provided. T - */ - std::vector> planners; - - /** @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; - - /** @brief Set the optimization objective function allocator. Default is to minimize path length */ - OptimizationObjectiveAllocator optimization_objective_allocator; - - /** @brief The ompl state validity checker. If nullptr and collision checking enabled it uses - * StateCollisionValidator */ - StateValidityCheckerAllocator svc_allocator; - - /** @brief The ompl motion validator. If nullptr and continuous collision checking enabled it used - * ContinuousMotionValidator */ - MotionValidatorAllocator mv_allocator; - - void setup(OMPLProblem& prob) const override; - - 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; - - 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; - - 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; - - 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; - - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; - -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; - - friend class boost::serialization::access; - template - void serialize(Archive&, const unsigned int); // NOLINT -}; -} // namespace tesseract_planning - -BOOST_CLASS_EXPORT_KEY(tesseract_planning::OMPLDefaultPlanProfile) - -#endif // TESSERACT_MOTION_PLANNERS_OMPL_OMPL_DEFAULT_PLAN_PROFILE_H 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..c4e337e330 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 @@ -33,19 +33,25 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + #include +#include +#include #include + #include -namespace tinyxml2 +namespace ompl::geometric { -class XMLElement; // NOLINT -class XMLDocument; -} // namespace tinyxml2 +class SimpleSetup; +} namespace tesseract_planning { -struct OMPLProblem; +struct OMPLSolverConfig; +struct OMPLPlannerConfigurator; + class OMPLPlanProfile : public Profile { public: @@ -60,37 +66,31 @@ 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 the OMPL Parallel Plan Solver Config + * @return The OMPL Parallel Plan Solver Config + */ + virtual std::unique_ptr createSolverConfig() const = 0; + + /** + * @brief Create the state extractor + * @return The OMPL state extractor + */ + virtual OMPLStateExtractor createStateExtractor(const tesseract_kinematics::JointGroup& manip) const = 0; + + /** + * @brief Create OMPL Simple Setup + * @param start_instruction The start instruction + * @param end_instruction The goal instruction + * @param composite_mi The parent composite manip info + * @param env The environment + * @return A OMPL Simple Setup + */ + virtual std::unique_ptr + createSimpleSetup(const MoveInstructionPoly& start_instruction, + const MoveInstructionPoly& end_instruction, + const tesseract_common::ManipulatorInfo& composite_mi, + const std::shared_ptr& env) const = 0; protected: friend class boost::serialization::access; @@ -98,8 +98,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/profile/ompl_real_vector_plan_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h new file mode 100644 index 0000000000..5115e6116e --- /dev/null +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h @@ -0,0 +1,183 @@ +/** + * @file ompl_real_vector_plan_profile.h + * @brief Tesseract OMPL real vector state space plan profile + * + * @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_OMPL_OMPL_REAL_VECTOR_PLAN_PROFILE_H +#define TESSERACT_MOTION_PLANNERS_OMPL_OMPL_REAL_VECTOR_PLAN_PROFILE_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include + +#include +#include + +#include +#include +#include + +namespace ompl::base +{ +class StateSampler; +class StateSpace; +class StateValidityChecker; +class MotionValidator; +class OptimizationObjective; +} // namespace ompl::base + +namespace tesseract_planning +{ +/** + * @brief OMPL does not support the concept of multi waypoint planning like descartes and trajopt. Because of this + * every plan instruction will be its a seperate ompl motion plan and therefore planning information is relevent + * for this motion planner in the profile. + */ +class OMPLRealVectorPlanProfile : public OMPLPlanProfile +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + OMPLRealVectorPlanProfile(); + + /** @brief The OMPL parallel planner solver config */ + OMPLSolverConfig solver_config; + + /** @brief The collision check configuration */ + tesseract_collision::CollisionCheckConfig collision_check_config; + + std::unique_ptr createSolverConfig() const override; + + OMPLStateExtractor createStateExtractor(const tesseract_kinematics::JointGroup& manip) const override; + + std::unique_ptr + createSimpleSetup(const MoveInstructionPoly& start_instruction, + const MoveInstructionPoly& end_instruction, + const tesseract_common::ManipulatorInfo& composite_mi, + const std::shared_ptr& env) const override; + +protected: + static void applyGoalStates(ompl::geometric::SimpleSetup& simple_setup, + const tesseract_kinematics::KinGroupIKInput& ik_input, + const tesseract_kinematics::KinematicGroup& manip, + tesseract_collision::DiscreteContactManager& contact_checker); + + static void applyStartStates(ompl::geometric::SimpleSetup& simple_setup, + const tesseract_kinematics::KinGroupIKInput& ik_input, + const tesseract_kinematics::KinematicGroup& manip, + tesseract_collision::DiscreteContactManager& contact_checker); + + static void applyGoalStates(ompl::geometric::SimpleSetup& simple_setup, + const Eigen::VectorXd& joint_waypoint, + const tesseract_kinematics::JointGroup& manip, + tesseract_collision::DiscreteContactManager& contact_checker); + + static void applyStartStates(ompl::geometric::SimpleSetup& simple_setup, + const Eigen::VectorXd& joint_waypoint, + const tesseract_kinematics::JointGroup& manip, + tesseract_collision::DiscreteContactManager& contact_checker); + + /** + * @brief Create state sampler allocator + * @param env The environment + * @param prob The OMPL problem + * @return OMPL state sampler allocator + */ + virtual std::function(const ompl::base::StateSpace*)> + createStateSamplerAllocator(const std::shared_ptr& /*env*/, + const std::shared_ptr& manip) const; + + /** + * @brief Create state validator which should not be the collision state validator + * @param simple_setup The OMPL Simple Setup + * @param env The environment + * @param manip The manipulator + * @param state_extractor The state extractor + * @return OMPL state validator + */ + virtual std::unique_ptr + createStateValidator(const ompl::geometric::SimpleSetup& simple_setup, + const std::shared_ptr& env, + const std::shared_ptr& manip, + const OMPLStateExtractor& state_extractor) const; + + /** + * @brief Create collision state validator + * @param simple_setup The OMPL Simple Setup + * @param env The environment + * @param manip The manipulator + * @param state_extractor The state extractor + * @return OMPL state collision validator + */ + virtual std::unique_ptr + createCollisionStateValidator(const ompl::geometric::SimpleSetup& simple_setup, + const std::shared_ptr& env, + const std::shared_ptr& manip, + const OMPLStateExtractor& state_extractor) const; + + /** + * @brief Create motion validator + * @param simple_setup The OMPL Simple Setup + * @param env The environment + * @param manip The manipulator + * @param state_extractor The state extractor + * @return OMPL motion validator + */ + virtual std::unique_ptr + createMotionValidator(const ompl::geometric::SimpleSetup& simple_setup, + const std::shared_ptr& env, + const std::shared_ptr& manip, + const OMPLStateExtractor& state_extractor, + const std::shared_ptr& svc_without_collision) const; + + /** + * @brief Create OMPL optimization object + * @param simple_setup The OMPL Simple Setup + * @param env The environment + * @param manip The manipulator + * @param state_extractor The state extractor + * @return OMPL optimization objective + */ + virtual std::unique_ptr + createOptimizationObjective(const ompl::geometric::SimpleSetup& simple_setup, + const std::shared_ptr& env, + const std::shared_ptr& manip, + const OMPLStateExtractor& state_extractor) const; + + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT +}; +} // namespace tesseract_planning + +BOOST_CLASS_EXPORT_KEY(tesseract_planning::OMPLRealVectorPlanProfile) + +#endif // TESSERACT_MOTION_PLANNERS_OMPL_OMPL_REAL_VECTOR_PLAN_PROFILE_H 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/include/tesseract_motion_planners/ompl/utils.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/utils.h index 13c2310c44..cf40b56186 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/utils.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/utils.h @@ -37,6 +37,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include namespace ompl::base { @@ -54,8 +55,6 @@ class PathGeometric; namespace tesseract_planning { -struct OMPLProblem; - Eigen::Map RealVectorStateSpaceExtractor(const ompl::base::State* s1, unsigned dimension); #ifndef OMPL_LESS_1_4_0 @@ -91,14 +90,16 @@ void processLongestValidSegment(const ompl::base::StateSpacePtr& state_space_ptr /** * @brief For the provided problem check if the state is in collision - * @param prob The OMPL Problem - * @param state The joint state * @param contact_map Map of contact results. Will be empty if return true + * @param contact_checker The contact checker to leverage + * @param manip The manipulator for calculating forward kinematics + * @param state The joint state * @return True if in collision otherwise false */ -bool checkStateInCollision(OMPLProblem& prob, - const Eigen::VectorXd& state, - tesseract_collision::ContactResultMap& contact_map); +bool checkStateInCollision(tesseract_collision::ContactResultMap& contact_map, + tesseract_collision::DiscreteContactManager& contact_checker, + const tesseract_kinematics::JointGroup& manip, + const Eigen::VectorXd& state); /** * @brief Default State sampler which uses the weights information to scale the sampled state. This is use full 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 c35aaa89b1..db6dc4c5c5 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -37,10 +37,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include +#include #include +#include #include -#include +#include #include #include @@ -54,6 +55,9 @@ constexpr auto SOLUTION_FOUND{ "Found valid solution" }; constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input: " }; constexpr auto ERROR_FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution: " }; +using CachedSimpleSetups = std::vector>; +using CachedSimpleSetupsPtr = std::shared_ptr; + namespace tesseract_planning { bool checkStartState(const ompl::base::ProblemDefinitionPtr& prob_def, @@ -102,394 +106,285 @@ bool OMPLMotionPlanner::terminate() return false; } -PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const +std::pair parallelPlan(ompl::geometric::SimpleSetup& simple_setup, + const OMPLSolverConfig& solver_config, + const unsigned num_output_states) { - PlannerResponse response; std::string reason; - if (!checkRequest(request, reason)) - { - response.successful = false; - response.message = std::string(ERROR_INVALID_INPUT) + reason; - return response; - } - std::vector problems; - if (request.data) + simple_setup.setup(); + auto parallel_plan = std::make_shared(simple_setup.getProblemDefinition()); + + for (const auto& planner : solver_config.planners) + parallel_plan->addPlanner(planner->create(simple_setup.getSpaceInformation())); + + ompl::base::PlannerStatus status; + if (!solver_config.optimize) { - problems = *std::static_pointer_cast>(request.data); + // Solve problem. Results are stored in the response + // Disabling hybridization because there is a bug which will return a trajectory that starts at the end state + // and finishes at the end state. + status = + parallel_plan->solve(solver_config.planning_time, 1, static_cast(solver_config.max_solutions), false); } else { - try - { - problems = createProblems(request); - } - catch (std::exception& e) - { - CONSOLE_BRIDGE_logError("OMPLPlanner failed to generate problem: %s.", e.what()); - response.successful = false; - response.message = std::string(ERROR_INVALID_INPUT) + e.what(); - return response; - } - - response.data = std::make_shared>(problems); - } - - // If the verbose set the log level to debug. - if (request.verbose) - console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG); - - /// @todo: Need to expand this to support multiple motion plans leveraging taskflow - for (auto& pc : problems) - { - auto& p = pc.problem; - p->simple_setup->setup(); - auto parallel_plan = std::make_shared(p->simple_setup->getProblemDefinition()); - - for (const auto& planner : p->planners) - parallel_plan->addPlanner(planner->create(p->simple_setup->getSpaceInformation())); - - ompl::base::PlannerStatus status; - if (!p->optimize) + ompl::time::point end = ompl::time::now() + ompl::time::seconds(solver_config.planning_time); + const ompl::base::ProblemDefinitionPtr& pdef = simple_setup.getProblemDefinition(); + while (ompl::time::now() < end) { // Solve problem. Results are stored in the response // Disabling hybridization because there is a bug which will return a trajectory that starts at the end state // and finishes at the end state. - status = parallel_plan->solve(p->planning_time, 1, static_cast(p->max_solutions), false); - } - else - { - ompl::time::point end = ompl::time::now() + ompl::time::seconds(p->planning_time); - const ompl::base::ProblemDefinitionPtr& pdef = p->simple_setup->getProblemDefinition(); - while (ompl::time::now() < end) + ompl::base::PlannerStatus localResult = + parallel_plan->solve(std::max(ompl::time::seconds(end - ompl::time::now()), 0.0), + 1, + static_cast(solver_config.max_solutions), + false); + if (localResult) { - // Solve problem. Results are stored in the response - // Disabling hybridization because there is a bug which will return a trajectory that starts at the end state - // and finishes at the end state. - ompl::base::PlannerStatus localResult = - parallel_plan->solve(std::max(ompl::time::seconds(end - ompl::time::now()), 0.0), - 1, - static_cast(p->max_solutions), - false); - if (localResult) - { - if (status != ompl::base::PlannerStatus::EXACT_SOLUTION) - status = localResult; + if (status != ompl::base::PlannerStatus::EXACT_SOLUTION) + status = localResult; - if (!pdef->hasOptimizationObjective()) - { - reason = "Terminating early since there is no optimization objective specified"; - CONSOLE_BRIDGE_logDebug(reason.c_str()); - break; - } + if (!pdef->hasOptimizationObjective()) + { + reason = "Terminating early since there is no optimization objective specified"; + CONSOLE_BRIDGE_logDebug(reason.c_str()); + break; + } - ompl::base::Cost obj_cost = pdef->getSolutionPath()->cost(pdef->getOptimizationObjective()); - CONSOLE_BRIDGE_logDebug("Motion Objective Cost: %f", obj_cost.value()); + ompl::base::Cost obj_cost = pdef->getSolutionPath()->cost(pdef->getOptimizationObjective()); + CONSOLE_BRIDGE_logDebug("Motion Objective Cost: %f", obj_cost.value()); - if (pdef->getOptimizationObjective()->isSatisfied(obj_cost)) - { - reason = "Terminating early since solution path satisfies the optimization objective"; - CONSOLE_BRIDGE_logDebug(reason.c_str()); - break; - } + if (pdef->getOptimizationObjective()->isSatisfied(obj_cost)) + { + reason = "Terminating early since solution path satisfies the optimization objective"; + CONSOLE_BRIDGE_logDebug(reason.c_str()); + break; + } - if (pdef->getSolutionCount() >= static_cast(p->max_solutions)) - { - reason = "Terminating early since " + std::to_string(p->max_solutions) + " solutions were generated"; - CONSOLE_BRIDGE_logDebug(reason.c_str()); - break; - } + if (pdef->getSolutionCount() >= static_cast(solver_config.max_solutions)) + { + reason = + "Terminating early since " + std::to_string(solver_config.max_solutions) + " solutions were generated"; + CONSOLE_BRIDGE_logDebug(reason.c_str()); + break; } } - if (ompl::time::now() >= end) - reason = "Exceeded allowed time"; } + if (ompl::time::now() >= end) + reason = "Exceeded allowed time"; + } - if (status != ompl::base::PlannerStatus::EXACT_SOLUTION) - { - response.successful = false; - response.message = std::string(ERROR_FAILED_TO_FIND_VALID_SOLUTION) + reason; - return response; - } + if (status != ompl::base::PlannerStatus::EXACT_SOLUTION) + return std::make_pair(false, std::string(ERROR_FAILED_TO_FIND_VALID_SOLUTION) + reason); - if (p->simplify) + if (solver_config.simplify) + { + simple_setup.simplifySolution(); + } + else + { + // Interpolate the path if it shouldn't be simplified and there are currently fewer states than requested + if (simple_setup.getSolutionPath().getStateCount() < num_output_states) { - p->simple_setup->simplifySolution(); + simple_setup.getSolutionPath().interpolate(num_output_states); } else { - // Interpolate the path if it shouldn't be simplified and there are currently fewer states than requested - auto num_output_states = static_cast(p->n_output_states); - if (p->simple_setup->getSolutionPath().getStateCount() < num_output_states) - { - p->simple_setup->getSolutionPath().interpolate(num_output_states); - } - else - { - // Now try to simplify the trajectory to get it under the requested number of output states - // The interpolate function only executes if the current number of states is less than the requested - p->simple_setup->simplifySolution(); - if (p->simple_setup->getSolutionPath().getStateCount() < num_output_states) - p->simple_setup->getSolutionPath().interpolate(num_output_states); - } + // Now try to simplify the trajectory to get it under the requested number of output states + // The interpolate function only executes if the current number of states is less than the requested + simple_setup.simplifySolution(); + if (simple_setup.getSolutionPath().getStateCount() < num_output_states) + simple_setup.getSolutionPath().interpolate(num_output_states); } } - // Flatten the results to make them easier to process - /** @todo Current does not handle if the returned solution is greater than the request */ - /** @todo Switch to processing the composite directly versus a flat list to solve the problem above */ - response.results = request.instructions; + return std::make_pair(true, "SUCCESS"); +} - std::size_t start_index{ 0 }; - for (auto& pc : problems) +long OMPLMotionPlanner::assignTrajectory(tesseract_planning::CompositeInstruction& output, + boost::uuids::uuid start_uuid, + boost::uuids::uuid end_uuid, + long start_index, + const std::vector& joint_names, + const tesseract_common::TrajArray& traj, + const bool format_result_as_input) +{ + bool found{ false }; + Eigen::Index row{ 0 }; + auto& ci = output.getInstructions(); + for (auto it = ci.begin() + static_cast(start_index); it != ci.end(); ++it) { - auto& p = pc.problem; - tesseract_common::TrajArray traj = p->getTrajectory(); - assert(checkStartState(p->simple_setup->getProblemDefinition(), traj.row(0), p->extractor)); - assert(checkGoalState(p->simple_setup->getProblemDefinition(), traj.bottomRows(1).transpose(), p->extractor)); - assert(traj.rows() >= p->n_output_states); - - const std::vector joint_names = p->manip->getJointNames(); - const Eigen::MatrixX2d joint_limits = p->manip->getLimits().joint_limits; - - // Enforce limits - for (Eigen::Index i = 0; i < traj.rows(); i++) + if (it->isMoveInstruction()) { - assert(tesseract_common::satisfiesLimits(traj.row(i), joint_limits, 1e-4)); - tesseract_common::enforceLimits(traj.row(i), joint_limits); - } + auto& mi = it->as(); + if (mi.getUUID() == start_uuid) + found = true; - bool found{ false }; - Eigen::Index row{ 0 }; - auto& ci = response.results.getInstructions(); - for (auto it = ci.begin() + static_cast(start_index); it != ci.end(); ++it) - { - if (it->isMoveInstruction()) + if (mi.getUUID() == end_uuid) { - auto& mi = it->as(); - if (mi.getUUID() == pc.start_uuid) - found = true; - - if (mi.getUUID() == pc.end_uuid) + std::vector extra; + for (; row < traj.rows() - 1; ++row) { - std::vector extra; - for (; row < traj.rows() - 1; ++row) + MoveInstructionPoly child = mi.createChild(); + if (format_result_as_input) { - MoveInstructionPoly child = mi.createChild(); - if (request.format_result_as_input) - { - JointWaypointPoly jwp = mi.createJointWaypoint(); - jwp.setIsConstrained(false); - jwp.setNames(joint_names); - jwp.setPosition(traj.row(row)); - child.assignJointWaypoint(jwp); - } - else - { - StateWaypointPoly swp = mi.createStateWaypoint(); - swp.setNames(joint_names); - swp.setPosition(traj.row(row)); - child.assignStateWaypoint(swp); - } - - extra.emplace_back(child); + JointWaypointPoly jwp = mi.createJointWaypoint(); + jwp.setIsConstrained(false); + jwp.setNames(joint_names); + jwp.setPosition(traj.row(row)); + child.assignJointWaypoint(jwp); + } + else + { + StateWaypointPoly swp = mi.createStateWaypoint(); + swp.setNames(joint_names); + swp.setPosition(traj.row(row)); + child.assignStateWaypoint(swp); } - assignSolution(mi, joint_names, traj.row(row), request.format_result_as_input); + extra.emplace_back(child); + } - if (!extra.empty()) - ci.insert(it, extra.begin(), extra.end()); + assignSolution(mi, joint_names, traj.row(row), format_result_as_input); - start_index += extra.size(); - break; - } + if (!extra.empty()) + ci.insert(it, extra.begin(), extra.end()); - if (found) - assignSolution(mi, joint_names, traj.row(row++), request.format_result_as_input); + start_index += static_cast(extra.size()); + break; } - ++start_index; + if (found) + assignSolution(mi, joint_names, traj.row(row++), format_result_as_input); } + + ++start_index; } - response.successful = true; - response.message = SOLUTION_FOUND; - return response; + return start_index; } -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 +PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) 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->getState(); - config.problem->manip = manip; - config.problem->contact_checker = request.env->getDiscreteContactManager(); - config.problem->contact_checker->setCollisionObjectsTransform(config.problem->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()) + PlannerResponse response; + response.results = request.instructions; + + std::string reason; + if (!checkRequest(request, reason)) { - 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); + response.successful = false; + response.message = std::string(ERROR_INVALID_INPUT) + reason; + return response; + } - 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"); - } + // Assume all the plan instructions have the same manipulator as the composite + tesseract_common::ManipulatorInfo composite_mi = request.instructions.getManipulatorInfo(); + assert(!composite_mi.empty()); - return config; - } + // Flatten the input for planning + auto move_instructions = request.instructions.flatten(&moveFilter); - 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); + // This is for replanning the same problem + CachedSimpleSetups cached_simple_setups; + if (request.data != nullptr) + cached_simple_setups = *std::static_pointer_cast(request.data); + else + cached_simple_setups.reserve(move_instructions.size()); - 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); - } + // Transform plan instructions into ompl problem + unsigned num_output_states = 1; + long start_index{ 0 }; + std::size_t segment{ 1 }; + std::reference_wrapper start_instruction = move_instructions.front(); + for (std::size_t i = 1; i < move_instructions.size(); ++i) + { + ++num_output_states; - return config; - } + std::reference_wrapper end_instruction = move_instructions[i]; + const auto& end_move_instruction = end_instruction.get().as(); + const auto& waypoint = end_move_instruction.getWaypoint(); + if (waypoint.isJointWaypoint() && !waypoint.as().isConstrained()) + continue; - throw std::runtime_error("OMPLMotionPlanner: unknown waypoint type"); -} -std::vector OMPLMotionPlanner::createProblems(const PlannerRequest& request) const -{ - std::vector problems; + // Get Plan Profile + auto cur_plan_profile = getProfile(name_, + end_move_instruction.getProfile(name_), + *request.profiles, + std::make_shared()); - // Assume all the plan instructions have the same manipulator as the composite - assert(!request.instructions.getManipulatorInfo().empty()); + if (!cur_plan_profile) + throw std::runtime_error("OMPLMotionPlanner: Invalid profile"); - const tesseract_common::ManipulatorInfo& composite_mi = request.instructions.getManipulatorInfo(); + // Get end state kinematics data + tesseract_common::ManipulatorInfo end_mi = composite_mi.getCombined(end_move_instruction.getManipulatorInfo()); + tesseract_kinematics::JointGroup::UPtr manip = request.env->getJointGroup(end_mi.manipulator); - tesseract_kinematics::JointGroup::Ptr manip; - if (composite_mi.manipulator.empty()) - throw std::runtime_error("OMPL, manipulator is empty!"); + // Create problem data + const auto& start_move_instruction = start_instruction.get().as(); + std::unique_ptr solver_config = cur_plan_profile->createSolverConfig(); + OMPLStateExtractor extractor = cur_plan_profile->createStateExtractor(*manip); + std::shared_ptr simple_setup; - try - { - tesseract_kinematics::KinematicGroup::Ptr kin_group; - std::string error_msg; - if (composite_mi.manipulator_ik_solver.empty()) + if (cached_simple_setups.empty() || segment > cached_simple_setups.size()) { - kin_group = request.env->getKinematicGroup(composite_mi.manipulator); - error_msg = "Failed to find kinematic group for manipulator '" + composite_mi.manipulator + "'"; + simple_setup = + cur_plan_profile->createSimpleSetup(start_move_instruction, end_move_instruction, composite_mi, request.env); + cached_simple_setups.push_back(simple_setup); } 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 + "'"; + simple_setup = cached_simple_setups.at(segment - 1); } - if (kin_group == nullptr) + // Parallel Plan problem + auto status = parallelPlan(*simple_setup, *solver_config, num_output_states); + if (!status.first) { - CONSOLE_BRIDGE_logError("%s", error_msg.c_str()); - throw std::runtime_error(error_msg); + response.successful = false; + response.message = status.second; + response.data = std::make_shared(cached_simple_setups); + return response; } - 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); + // Extract Solution + const std::vector joint_names = manip->getJointNames(); + const Eigen::MatrixX2d joint_limits = manip->getLimits().joint_limits; + tesseract_common::TrajArray traj = toTrajArray(simple_setup->getSolutionPath(), extractor); + assert(checkStartState(simple_setup->getProblemDefinition(), traj.row(0), extractor)); + assert(checkGoalState(simple_setup->getProblemDefinition(), traj.bottomRows(1).transpose(), extractor)); + assert(traj.rows() >= num_output_states); - // Transform plan instructions into ompl problem - int index = 0; - int num_output_states = 1; - MoveInstructionPoly start_instruction = move_instructions.front().get().as(); - - for (std::size_t i = 1; i < move_instructions.size(); ++i) - { - ++num_output_states; - const auto& instruction = move_instructions[i].get(); - assert(instruction.isMoveInstruction()); - const auto& move_instruction = instruction.as(); - const auto& waypoint = move_instruction.getWaypoint(); - if (waypoint.isCartesianWaypoint() || waypoint.isStateWaypoint() || - (waypoint.isJointWaypoint() && waypoint.as().isConstrained())) + // Enforce limits + for (Eigen::Index i = 0; i < traj.rows(); i++) { - problems.push_back(createSubProblem( - request, composite_mi, manip, start_instruction, move_instruction, num_output_states, index++)); - start_instruction = move_instruction; - num_output_states = 1; + assert(tesseract_common::satisfiesLimits(traj.row(i), joint_limits, 1e-4)); + tesseract_common::enforceLimits(traj.row(i), joint_limits); } + + // Assign trajectory to results + start_index = assignTrajectory(response.results, + start_move_instruction.getUUID(), + end_move_instruction.getUUID(), + start_index, + joint_names, + traj, + request.format_result_as_input); + + // Reset data for next segment + start_instruction = end_instruction; + num_output_states = 1; + ++segment; } - return problems; + response.successful = true; + response.message = SOLUTION_FOUND; + response.data = std::make_shared(cached_simple_setups); + return response; } +void OMPLMotionPlanner::clear() {} + +std::unique_ptr OMPLMotionPlanner::clone() const { return std::make_unique(name_); } + } // namespace tesseract_planning diff --git a/tesseract_motion_planners/ompl/src/ompl_planner_configurator.cpp b/tesseract_motion_planners/ompl/src/ompl_planner_configurator.cpp index 9488d20ce2..7bf4790bcd 100644 --- a/tesseract_motion_planners/ompl/src/ompl_planner_configurator.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_planner_configurator.cpp @@ -45,7 +45,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include -#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -58,25 +57,6 @@ void OMPLPlannerConfigurator::serialize(Archive& /*ar*/, const unsigned int /*ve { } -SBLConfigurator::SBLConfigurator(const tinyxml2::XMLElement& xml_element) -{ - const tinyxml2::XMLElement* sbl_element = xml_element.FirstChildElement("SBL"); - const tinyxml2::XMLElement* range_element = sbl_element->FirstChildElement("Range"); - - if (range_element != nullptr) - { - std::string range_string; - int status = tesseract_common::QueryStringText(range_element, range_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: SBL: Error parsing Range string"); - - if (!tesseract_common::isNumeric(range_string)) - throw std::runtime_error("OMPLConfigurator: SBL: Range is not a numeric values."); - - tesseract_common::toNumeric(range_string, range); - } -} - ompl::base::PlannerPtr SBLConfigurator::create(ompl::base::SpaceInformationPtr si) const { auto planner = std::make_shared(si); @@ -86,17 +66,6 @@ ompl::base::PlannerPtr SBLConfigurator::create(ompl::base::SpaceInformationPtr s OMPLPlannerType SBLConfigurator::getType() const { return OMPLPlannerType::SBL; } -tinyxml2::XMLElement* SBLConfigurator::toXML(tinyxml2::XMLDocument& doc) const -{ - tinyxml2::XMLElement* ompl_xml = doc.NewElement("SBL"); - - tinyxml2::XMLElement* range_xml = doc.NewElement("Range"); - range_xml->SetText(range); - ompl_xml->InsertEndChild(range_xml); - - return ompl_xml; -} - template void SBLConfigurator::serialize(Archive& ar, const unsigned int /*version*/) { @@ -104,41 +73,6 @@ void SBLConfigurator::serialize(Archive& ar, const unsigned int /*version*/) ar& BOOST_SERIALIZATION_NVP(range); } -ESTConfigurator::ESTConfigurator(const tinyxml2::XMLElement& xml_element) -{ - const tinyxml2::XMLElement* est_element = xml_element.FirstChildElement("EST"); - const tinyxml2::XMLElement* range_element = est_element->FirstChildElement("Range"); - const tinyxml2::XMLElement* goal_bias_element = est_element->FirstChildElement("GoalBias"); - - int status{ tinyxml2::XMLError::XML_SUCCESS }; - - if (range_element != nullptr) - { - std::string range_string; - status = tesseract_common::QueryStringText(range_element, range_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: EST: Error parsing Range string"); - - if (!tesseract_common::isNumeric(range_string)) - throw std::runtime_error("OMPLConfigurator: EST: Range is not a numeric values."); - - tesseract_common::toNumeric(range_string, range); - } - - if (goal_bias_element != nullptr) - { - std::string goal_bias_string; - status = tesseract_common::QueryStringText(goal_bias_element, goal_bias_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: EST: Error parsing GoalBias string"); - - if (!tesseract_common::isNumeric(goal_bias_string)) - throw std::runtime_error("OMPLConfigurator: EST: GoalBias is not a numeric values."); - - tesseract_common::toNumeric(goal_bias_string, goal_bias); - } -} - ompl::base::PlannerPtr ESTConfigurator::create(ompl::base::SpaceInformationPtr si) const { auto planner = std::make_shared(si); @@ -149,21 +83,6 @@ ompl::base::PlannerPtr ESTConfigurator::create(ompl::base::SpaceInformationPtr s OMPLPlannerType ESTConfigurator::getType() const { return OMPLPlannerType::EST; } -tinyxml2::XMLElement* ESTConfigurator::toXML(tinyxml2::XMLDocument& doc) const -{ - tinyxml2::XMLElement* ompl_xml = doc.NewElement("EST"); - - tinyxml2::XMLElement* range_xml = doc.NewElement("Range"); - range_xml->SetText(range); - ompl_xml->InsertEndChild(range_xml); - - tinyxml2::XMLElement* goal_bias_xml = doc.NewElement("GoalBias"); - goal_bias_xml->SetText(goal_bias); - ompl_xml->InsertEndChild(goal_bias_xml); - - return ompl_xml; -} - template void ESTConfigurator::serialize(Archive& ar, const unsigned int /*version*/) { @@ -172,56 +91,6 @@ void ESTConfigurator::serialize(Archive& ar, const unsigned int /*version*/) ar& BOOST_SERIALIZATION_NVP(goal_bias); } -LBKPIECE1Configurator::LBKPIECE1Configurator(const tinyxml2::XMLElement& xml_element) -{ - const tinyxml2::XMLElement* lbkpiece1_element = xml_element.FirstChildElement("LBKPIECE1"); - const tinyxml2::XMLElement* range_element = lbkpiece1_element->FirstChildElement("Range"); - const tinyxml2::XMLElement* border_fraction_element = lbkpiece1_element->FirstChildElement("BorderFraction"); - const tinyxml2::XMLElement* min_valid_path_fraction_element = lbkpiece1_element->FirstChildElement("MinValidPathFract" - "ion"); - - int status{ tinyxml2::XMLError::XML_SUCCESS }; - - if (range_element != nullptr) - { - std::string range_string; - status = tesseract_common::QueryStringText(range_element, range_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: LBKPIECE1: Error parsing Range string"); - - if (!tesseract_common::isNumeric(range_string)) - throw std::runtime_error("OMPLConfigurator: LBKPIECE1: Range is not a numeric values."); - - tesseract_common::toNumeric(range_string, range); - } - - if (border_fraction_element != nullptr) - { - std::string border_fraction_string; - status = tesseract_common::QueryStringText(border_fraction_element, border_fraction_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: LBKPIECE1: Error parsing BorderFraction string"); - - if (!tesseract_common::isNumeric(border_fraction_string)) - throw std::runtime_error("OMPLConfigurator: LBKPIECE1: BorderFraction is not a numeric values."); - - tesseract_common::toNumeric(border_fraction_string, border_fraction); - } - - if (min_valid_path_fraction_element != nullptr) - { - std::string min_valid_path_fraction_string; - status = tesseract_common::QueryStringText(min_valid_path_fraction_element, min_valid_path_fraction_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: LBKPIECE1: Error parsing MinValidPathFraction string"); - - if (!tesseract_common::isNumeric(min_valid_path_fraction_string)) - throw std::runtime_error("OMPLConfigurator: LBKPIECE1: MinValidPathFraction is not a numeric values."); - - tesseract_common::toNumeric(min_valid_path_fraction_string, min_valid_path_fraction); - } -} - ompl::base::PlannerPtr LBKPIECE1Configurator::create(ompl::base::SpaceInformationPtr si) const { auto planner = std::make_shared(si); @@ -233,25 +102,6 @@ ompl::base::PlannerPtr LBKPIECE1Configurator::create(ompl::base::SpaceInformatio OMPLPlannerType LBKPIECE1Configurator::getType() const { return OMPLPlannerType::LBKPIECE1; } -tinyxml2::XMLElement* LBKPIECE1Configurator::toXML(tinyxml2::XMLDocument& doc) const -{ - tinyxml2::XMLElement* ompl_xml = doc.NewElement("LBKPIECE1"); - - tinyxml2::XMLElement* range_xml = doc.NewElement("Range"); - range_xml->SetText(range); - ompl_xml->InsertEndChild(range_xml); - - tinyxml2::XMLElement* border_fraction_xml = doc.NewElement("BorderFraction"); - border_fraction_xml->SetText(border_fraction); - ompl_xml->InsertEndChild(border_fraction_xml); - - tinyxml2::XMLElement* min_valid_path_fraction_xml = doc.NewElement("MinValidPathFraction"); - min_valid_path_fraction_xml->SetText(min_valid_path_fraction); - ompl_xml->InsertEndChild(min_valid_path_fraction_xml); - - return ompl_xml; -} - template void LBKPIECE1Configurator::serialize(Archive& ar, const unsigned int /*version*/) { @@ -261,73 +111,6 @@ void LBKPIECE1Configurator::serialize(Archive& ar, const unsigned int /*version* ar& BOOST_SERIALIZATION_NVP(min_valid_path_fraction); } -BKPIECE1Configurator::BKPIECE1Configurator(const tinyxml2::XMLElement& xml_element) -{ - const tinyxml2::XMLElement* bkpiece1_element = xml_element.FirstChildElement("BKPIECE1"); - const tinyxml2::XMLElement* range_element = bkpiece1_element->FirstChildElement("Range"); - const tinyxml2::XMLElement* border_fraction_element = bkpiece1_element->FirstChildElement("BorderFraction"); - const tinyxml2::XMLElement* failed_expansion_score_factor_element = bkpiece1_element->FirstChildElement("FailedExpans" - "ionScoreFact" - "or"); - const tinyxml2::XMLElement* min_valid_path_fraction_element = bkpiece1_element->FirstChildElement("MinValidPathFracti" - "on"); - - int status{ tinyxml2::XMLError::XML_SUCCESS }; - - if (range_element != nullptr) - { - std::string range_string; - status = tesseract_common::QueryStringText(range_element, range_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: BKPIECE1: Error parsing Range string"); - - if (!tesseract_common::isNumeric(range_string)) - throw std::runtime_error("OMPLConfigurator: BKPIECE1: Range is not a numeric values."); - - tesseract_common::toNumeric(range_string, range); - } - - if (border_fraction_element != nullptr) - { - std::string border_fraction_string; - status = tesseract_common::QueryStringText(border_fraction_element, border_fraction_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: BKPIECE1: Error parsing BorderFraction string"); - - if (!tesseract_common::isNumeric(border_fraction_string)) - throw std::runtime_error("OMPLConfigurator: BKPIECE1: BorderFraction is not a numeric values."); - - tesseract_common::toNumeric(border_fraction_string, border_fraction); - } - - if (failed_expansion_score_factor_element != nullptr) - { - std::string failed_expansion_score_factor_string; - status = - tesseract_common::QueryStringText(failed_expansion_score_factor_element, failed_expansion_score_factor_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: BKPIECE1: Error parsing FailedExpansionScoreFactor string"); - - if (!tesseract_common::isNumeric(failed_expansion_score_factor_string)) - throw std::runtime_error("OMPLConfigurator: BKPIECE1: FailedExpansionScoreFactor is not a numeric values."); - - tesseract_common::toNumeric(failed_expansion_score_factor_string, failed_expansion_score_factor); - } - - if (min_valid_path_fraction_element != nullptr) - { - std::string min_valid_path_fraction_string; - status = tesseract_common::QueryStringText(min_valid_path_fraction_element, min_valid_path_fraction_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: BKPIECE1: Error parsing MinValidPathFraction string"); - - if (!tesseract_common::isNumeric(min_valid_path_fraction_string)) - throw std::runtime_error("OMPLConfigurator: BKPIECE1: MinValidPathFraction is not a numeric values."); - - tesseract_common::toNumeric(min_valid_path_fraction_string, min_valid_path_fraction); - } -} - ompl::base::PlannerPtr BKPIECE1Configurator::create(ompl::base::SpaceInformationPtr si) const { auto planner = std::make_shared(si); @@ -340,29 +123,6 @@ ompl::base::PlannerPtr BKPIECE1Configurator::create(ompl::base::SpaceInformation OMPLPlannerType BKPIECE1Configurator::getType() const { return OMPLPlannerType::BKPIECE1; } -tinyxml2::XMLElement* BKPIECE1Configurator::toXML(tinyxml2::XMLDocument& doc) const -{ - tinyxml2::XMLElement* ompl_xml = doc.NewElement("BKPIECE1"); - - tinyxml2::XMLElement* range_xml = doc.NewElement("Range"); - range_xml->SetText(range); - ompl_xml->InsertEndChild(range_xml); - - tinyxml2::XMLElement* border_fraction_xml = doc.NewElement("BorderFraction"); - border_fraction_xml->SetText(border_fraction); - ompl_xml->InsertEndChild(border_fraction_xml); - - tinyxml2::XMLElement* failed_expansion_score_factor_xml = doc.NewElement("FailedExpansionScoreFactor"); - failed_expansion_score_factor_xml->SetText(failed_expansion_score_factor); - ompl_xml->InsertEndChild(failed_expansion_score_factor_xml); - - tinyxml2::XMLElement* min_valid_path_fraction_xml = doc.NewElement("MinValidPathFraction"); - min_valid_path_fraction_xml->SetText(min_valid_path_fraction); - ompl_xml->InsertEndChild(min_valid_path_fraction_xml); - - return ompl_xml; -} - template void BKPIECE1Configurator::serialize(Archive& ar, const unsigned int /*version*/) { @@ -373,87 +133,6 @@ void BKPIECE1Configurator::serialize(Archive& ar, const unsigned int /*version*/ ar& BOOST_SERIALIZATION_NVP(min_valid_path_fraction); } -KPIECE1Configurator::KPIECE1Configurator(const tinyxml2::XMLElement& xml_element) -{ - const tinyxml2::XMLElement* kpiece1_element = xml_element.FirstChildElement("KPIECE1"); - const tinyxml2::XMLElement* range_element = kpiece1_element->FirstChildElement("Range"); - const tinyxml2::XMLElement* goal_bias_element = kpiece1_element->FirstChildElement("GoalBias"); - const tinyxml2::XMLElement* border_fraction_element = kpiece1_element->FirstChildElement("BorderFraction"); - const tinyxml2::XMLElement* failed_expansion_score_factor_element = kpiece1_element->FirstChildElement("FailedExpansi" - "onScoreFacto" - "r"); - const tinyxml2::XMLElement* min_valid_path_fraction_element = kpiece1_element->FirstChildElement("MinValidPathFractio" - "n"); - - int status{ tinyxml2::XMLError::XML_SUCCESS }; - - if (range_element != nullptr) - { - std::string range_string; - status = tesseract_common::QueryStringText(range_element, range_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: KPIECE1Configurator: Error parsing Range string"); - - if (!tesseract_common::isNumeric(range_string)) - throw std::runtime_error("OMPLConfigurator: KPIECE1Configurator: Range is not a numeric values."); - - tesseract_common::toNumeric(range_string, range); - } - - if (goal_bias_element != nullptr) - { - std::string goal_bias_string; - status = tesseract_common::QueryStringText(goal_bias_element, goal_bias_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: KPIECE1: Error parsing GoalBias string"); - - if (!tesseract_common::isNumeric(goal_bias_string)) - throw std::runtime_error("OMPLConfigurator: KPIECE1: GoalBias is not a numeric values."); - - tesseract_common::toNumeric(goal_bias_string, goal_bias); - } - - if (border_fraction_element != nullptr) - { - std::string border_fraction_string; - status = tesseract_common::QueryStringText(border_fraction_element, border_fraction_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: KPIECE1: Error parsing BorderFraction string"); - - if (!tesseract_common::isNumeric(border_fraction_string)) - throw std::runtime_error("OMPLConfigurator: KPIECE1: BorderFraction is not a numeric values."); - - tesseract_common::toNumeric(border_fraction_string, border_fraction); - } - - if (failed_expansion_score_factor_element != nullptr) - { - std::string failed_expansion_score_factor_string; - status = - tesseract_common::QueryStringText(failed_expansion_score_factor_element, failed_expansion_score_factor_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: KPIECE1: Error parsing FailedExpansionScoreFactor string"); - - if (!tesseract_common::isNumeric(failed_expansion_score_factor_string)) - throw std::runtime_error("OMPLConfigurator: KPIECE1: FailedExpansionScoreFactor is not a numeric values."); - - tesseract_common::toNumeric(failed_expansion_score_factor_string, failed_expansion_score_factor); - } - - if (min_valid_path_fraction_element != nullptr) - { - std::string min_valid_path_fraction_string; - status = tesseract_common::QueryStringText(min_valid_path_fraction_element, min_valid_path_fraction_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: KPIECE1: Error parsing MinValidPathFraction string"); - - if (!tesseract_common::isNumeric(min_valid_path_fraction_string)) - throw std::runtime_error("OMPLConfigurator: KPIECE1: MinValidPathFraction is not a numeric values."); - - tesseract_common::toNumeric(min_valid_path_fraction_string, min_valid_path_fraction); - } -} - ompl::base::PlannerPtr KPIECE1Configurator::create(ompl::base::SpaceInformationPtr si) const { auto planner = std::make_shared(si); @@ -467,33 +146,6 @@ ompl::base::PlannerPtr KPIECE1Configurator::create(ompl::base::SpaceInformationP OMPLPlannerType KPIECE1Configurator::getType() const { return OMPLPlannerType::KPIECE1; } -tinyxml2::XMLElement* KPIECE1Configurator::toXML(tinyxml2::XMLDocument& doc) const -{ - tinyxml2::XMLElement* ompl_xml = doc.NewElement("KPIECE1"); - - tinyxml2::XMLElement* range_xml = doc.NewElement("Range"); - range_xml->SetText(range); - ompl_xml->InsertEndChild(range_xml); - - tinyxml2::XMLElement* goal_bias_xml = doc.NewElement("GoalBias"); - goal_bias_xml->SetText(goal_bias); - ompl_xml->InsertEndChild(goal_bias_xml); - - tinyxml2::XMLElement* border_fraction_xml = doc.NewElement("BorderFraction"); - border_fraction_xml->SetText(border_fraction); - ompl_xml->InsertEndChild(border_fraction_xml); - - tinyxml2::XMLElement* failed_expansion_score_factor_xml = doc.NewElement("FailedExpansionScoreFactor"); - failed_expansion_score_factor_xml->SetText(failed_expansion_score_factor); - ompl_xml->InsertEndChild(failed_expansion_score_factor_xml); - - tinyxml2::XMLElement* min_valid_path_fraction_xml = doc.NewElement("MinValidPathFraction"); - min_valid_path_fraction_xml->SetText(min_valid_path_fraction); - ompl_xml->InsertEndChild(min_valid_path_fraction_xml); - - return ompl_xml; -} - template void KPIECE1Configurator::serialize(Archive& ar, const unsigned int /*version*/) { @@ -505,100 +157,6 @@ void KPIECE1Configurator::serialize(Archive& ar, const unsigned int /*version*/) ar& BOOST_SERIALIZATION_NVP(min_valid_path_fraction); } -BiTRRTConfigurator::BiTRRTConfigurator(const tinyxml2::XMLElement& xml_element) -{ - const tinyxml2::XMLElement* bitrrt_element = xml_element.FirstChildElement("BiTRRT"); - const tinyxml2::XMLElement* range_element = bitrrt_element->FirstChildElement("Range"); - const tinyxml2::XMLElement* temp_change_factor_element = bitrrt_element->FirstChildElement("TempChangeFactor"); - const tinyxml2::XMLElement* cost_threshold_element = bitrrt_element->FirstChildElement("CostThreshold"); - const tinyxml2::XMLElement* init_temperature_element = bitrrt_element->FirstChildElement("InitTemperature"); - const tinyxml2::XMLElement* frontier_threshold_element = bitrrt_element->FirstChildElement("FrontierThreshold"); - const tinyxml2::XMLElement* frontier_node_ratio_element = bitrrt_element->FirstChildElement("FrontierNodeRatio"); - - int status{ tinyxml2::XMLError::XML_SUCCESS }; - - if (range_element != nullptr) - { - std::string range_string; - status = tesseract_common::QueryStringText(range_element, range_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: BiTRRT: Error parsing Range string"); - - if (!tesseract_common::isNumeric(range_string)) - throw std::runtime_error("OMPLConfigurator: BiTRRT: Range is not a numeric values."); - - tesseract_common::toNumeric(range_string, range); - } - - if (temp_change_factor_element != nullptr) - { - std::string temp_change_factor_string; - status = tesseract_common::QueryStringText(temp_change_factor_element, temp_change_factor_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: BiTRRT: Error parsing TempChangeFactor string"); - - if (!tesseract_common::isNumeric(temp_change_factor_string)) - throw std::runtime_error("OMPLConfigurator: BiTRRT: TempChangeFactor is not a numeric values."); - - tesseract_common::toNumeric(temp_change_factor_string, temp_change_factor); - } - - if (cost_threshold_element != nullptr) - { - std::string cost_threshold_string; - status = tesseract_common::QueryStringText(cost_threshold_element, cost_threshold_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: BiTRRT: Error parsing CostThreshold string"); - - if (!tesseract_common::isNumeric(cost_threshold_string)) - { - if (cost_threshold_string != "inf") - throw std::runtime_error("OMPLConfigurator: BiTRRT: CostThreshold is not a numeric values."); - } - else - tesseract_common::toNumeric(cost_threshold_string, cost_threshold); - } - - if (init_temperature_element != nullptr) - { - std::string init_temperature_string; - status = tesseract_common::QueryStringText(init_temperature_element, init_temperature_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: BiTRRT: Error parsing InitTemperature string"); - - if (!tesseract_common::isNumeric(init_temperature_string)) - throw std::runtime_error("OMPLConfigurator: BiTRRT: InitTemperature is not a numeric values."); - - tesseract_common::toNumeric(init_temperature_string, init_temperature); - } - - if (frontier_threshold_element != nullptr) - { - std::string frontier_threshold_string; - status = tesseract_common::QueryStringText(frontier_threshold_element, frontier_threshold_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: BiTRRT: Error parsing FrontierThreshold string"); - - if (!tesseract_common::isNumeric(frontier_threshold_string)) - throw std::runtime_error("OMPLConfigurator: BiTRRT: FrontierThreshold is not a numeric values."); - - tesseract_common::toNumeric(frontier_threshold_string, frontier_threshold); - } - - if (frontier_node_ratio_element != nullptr) - { - std::string frontier_node_ratio_string; - status = tesseract_common::QueryStringText(frontier_node_ratio_element, frontier_node_ratio_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: BiTRRT: Error FrontierNodeRatio GoalBias string"); - - if (!tesseract_common::isNumeric(frontier_node_ratio_string)) - throw std::runtime_error("OMPLConfigurator: BiTRRT: FrontierNodeRatio is not a numeric values."); - - tesseract_common::toNumeric(frontier_node_ratio_string, frontier_node_ratio); - } -} - ompl::base::PlannerPtr BiTRRTConfigurator::create(ompl::base::SpaceInformationPtr si) const { auto planner = std::make_shared(si); @@ -613,37 +171,6 @@ ompl::base::PlannerPtr BiTRRTConfigurator::create(ompl::base::SpaceInformationPt OMPLPlannerType BiTRRTConfigurator::getType() const { return OMPLPlannerType::BiTRRT; } -tinyxml2::XMLElement* BiTRRTConfigurator::toXML(tinyxml2::XMLDocument& doc) const -{ - tinyxml2::XMLElement* ompl_xml = doc.NewElement("BiTRRT"); - - tinyxml2::XMLElement* range_xml = doc.NewElement("Range"); - range_xml->SetText(range); - ompl_xml->InsertEndChild(range_xml); - - tinyxml2::XMLElement* temp_change_factor_xml = doc.NewElement("TempChangeFactor"); - temp_change_factor_xml->SetText(temp_change_factor); - ompl_xml->InsertEndChild(temp_change_factor_xml); - - tinyxml2::XMLElement* cost_threshold_xml = doc.NewElement("CostThreshold"); - cost_threshold_xml->SetText(cost_threshold); - ompl_xml->InsertEndChild(cost_threshold_xml); - - tinyxml2::XMLElement* init_temperature_xml = doc.NewElement("InitTemperature"); - init_temperature_xml->SetText(init_temperature); - ompl_xml->InsertEndChild(init_temperature_xml); - - tinyxml2::XMLElement* frontier_threshold_xml = doc.NewElement("FrontierThreshold"); - frontier_threshold_xml->SetText(frontier_threshold); - ompl_xml->InsertEndChild(frontier_threshold_xml); - - tinyxml2::XMLElement* frontier_node_ratio_xml = doc.NewElement("FrontierNodeRatio"); - frontier_node_ratio_xml->SetText(frontier_node_ratio); - ompl_xml->InsertEndChild(frontier_node_ratio_xml); - - return ompl_xml; -} - template void BiTRRTConfigurator::serialize(Archive& ar, const unsigned int /*version*/) { @@ -656,41 +183,6 @@ void BiTRRTConfigurator::serialize(Archive& ar, const unsigned int /*version*/) ar& BOOST_SERIALIZATION_NVP(frontier_node_ratio); } -RRTConfigurator::RRTConfigurator(const tinyxml2::XMLElement& xml_element) -{ - const tinyxml2::XMLElement* rrt_element = xml_element.FirstChildElement("RRT"); - const tinyxml2::XMLElement* range_element = rrt_element->FirstChildElement("Range"); - const tinyxml2::XMLElement* goal_bias_element = rrt_element->FirstChildElement("GoalBias"); - - int status{ tinyxml2::XMLError::XML_SUCCESS }; - - if (range_element != nullptr) - { - std::string range_string; - status = tesseract_common::QueryStringText(range_element, range_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: RRT: Error parsing Range string"); - - if (!tesseract_common::isNumeric(range_string)) - throw std::runtime_error("OMPLConfigurator: RRT: Range is not a numeric values."); - - tesseract_common::toNumeric(range_string, range); - } - - if (goal_bias_element != nullptr) - { - std::string goal_bias_string; - status = tesseract_common::QueryStringText(goal_bias_element, goal_bias_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: RRT: Error parsing GoalBias string"); - - if (!tesseract_common::isNumeric(goal_bias_string)) - throw std::runtime_error("OMPLConfigurator: RRT: GoalBias is not a numeric values."); - - tesseract_common::toNumeric(goal_bias_string, goal_bias); - } -} - ompl::base::PlannerPtr RRTConfigurator::create(ompl::base::SpaceInformationPtr si) const { auto planner = std::make_shared(si); @@ -701,21 +193,6 @@ ompl::base::PlannerPtr RRTConfigurator::create(ompl::base::SpaceInformationPtr s OMPLPlannerType RRTConfigurator::getType() const { return OMPLPlannerType::RRT; } -tinyxml2::XMLElement* RRTConfigurator::toXML(tinyxml2::XMLDocument& doc) const -{ - tinyxml2::XMLElement* ompl_xml = doc.NewElement("RRT"); - - tinyxml2::XMLElement* range_xml = doc.NewElement("Range"); - range_xml->SetText(range); - ompl_xml->InsertEndChild(range_xml); - - tinyxml2::XMLElement* goal_bias_xml = doc.NewElement("GoalBias"); - goal_bias_xml->SetText(goal_bias); - ompl_xml->InsertEndChild(goal_bias_xml); - - return ompl_xml; -} - template void RRTConfigurator::serialize(Archive& ar, const unsigned int /*version*/) { @@ -724,27 +201,6 @@ void RRTConfigurator::serialize(Archive& ar, const unsigned int /*version*/) ar& BOOST_SERIALIZATION_NVP(goal_bias); } -RRTConnectConfigurator::RRTConnectConfigurator(const tinyxml2::XMLElement& xml_element) -{ - const tinyxml2::XMLElement* rrt_connect_element = xml_element.FirstChildElement("RRTConnect"); - const tinyxml2::XMLElement* range_element = rrt_connect_element->FirstChildElement("Range"); - - int status{ tinyxml2::XMLError::XML_SUCCESS }; - - if (range_element != nullptr) - { - std::string range_string; - status = tesseract_common::QueryStringText(range_element, range_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: RRTConnect: Error parsing Range string"); - - if (!tesseract_common::isNumeric(range_string)) - throw std::runtime_error("OMPLConfigurator: RRTConnect: Range is not a numeric values."); - - tesseract_common::toNumeric(range_string, range); - } -} - ompl::base::PlannerPtr RRTConnectConfigurator::create(ompl::base::SpaceInformationPtr si) const { auto planner = std::make_shared(si); @@ -754,17 +210,6 @@ ompl::base::PlannerPtr RRTConnectConfigurator::create(ompl::base::SpaceInformati OMPLPlannerType RRTConnectConfigurator::getType() const { return OMPLPlannerType::RRTConnect; } -tinyxml2::XMLElement* RRTConnectConfigurator::toXML(tinyxml2::XMLDocument& doc) const -{ - tinyxml2::XMLElement* ompl_xml = doc.NewElement("RRTConnect"); - - tinyxml2::XMLElement* range_xml = doc.NewElement("Range"); - range_xml->SetText(range); - ompl_xml->InsertEndChild(range_xml); - - return ompl_xml; -} - template void RRTConnectConfigurator::serialize(Archive& ar, const unsigned int /*version*/) { @@ -772,51 +217,6 @@ void RRTConnectConfigurator::serialize(Archive& ar, const unsigned int /*version ar& BOOST_SERIALIZATION_NVP(range); } -RRTstarConfigurator::RRTstarConfigurator(const tinyxml2::XMLElement& xml_element) -{ - const tinyxml2::XMLElement* rrt_star_element = xml_element.FirstChildElement("RRTstar"); - const tinyxml2::XMLElement* range_element = rrt_star_element->FirstChildElement("Range"); - const tinyxml2::XMLElement* goal_bias_element = rrt_star_element->FirstChildElement("GoalBias"); - const tinyxml2::XMLElement* delay_collision_checking_element = rrt_star_element->FirstChildElement("DelayCollisionChe" - "cking"); - - int status{ tinyxml2::XMLError::XML_SUCCESS }; - - if (range_element != nullptr) - { - std::string range_string; - status = tesseract_common::QueryStringText(range_element, range_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: RRTstar: Error parsing Range string"); - - if (!tesseract_common::isNumeric(range_string)) - throw std::runtime_error("OMPLConfigurator: RRTstar: Range is not a numeric values."); - - tesseract_common::toNumeric(range_string, range); - } - - if (goal_bias_element != nullptr) - { - std::string goal_bias_string; - status = tesseract_common::QueryStringText(goal_bias_element, goal_bias_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: RRTstar: Error parsing GoalBias string"); - - if (!tesseract_common::isNumeric(goal_bias_string)) - throw std::runtime_error("OMPLConfigurator: RRTstar: GoalBias is not a numeric values."); - - tesseract_common::toNumeric(goal_bias_string, goal_bias); - } - - if (delay_collision_checking_element != nullptr) - { - std::string delay_collision_checking_string; - status = delay_collision_checking_element->QueryBoolText(&delay_collision_checking); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: RRTstar: Error parsing DelayCollisionChecking string"); - } -} - ompl::base::PlannerPtr RRTstarConfigurator::create(ompl::base::SpaceInformationPtr si) const { auto planner = std::make_shared(si); @@ -828,25 +228,6 @@ ompl::base::PlannerPtr RRTstarConfigurator::create(ompl::base::SpaceInformationP OMPLPlannerType RRTstarConfigurator::getType() const { return OMPLPlannerType::RRTstar; } -tinyxml2::XMLElement* RRTstarConfigurator::toXML(tinyxml2::XMLDocument& doc) const -{ - tinyxml2::XMLElement* ompl_xml = doc.NewElement("RRTstar"); - - tinyxml2::XMLElement* range_xml = doc.NewElement("Range"); - range_xml->SetText(range); - ompl_xml->InsertEndChild(range_xml); - - tinyxml2::XMLElement* goal_bias_xml = doc.NewElement("GoalBias"); - goal_bias_xml->SetText(goal_bias); - ompl_xml->InsertEndChild(goal_bias_xml); - - tinyxml2::XMLElement* delay_collision_checking_xml = doc.NewElement("DelayCollisionChecking"); - delay_collision_checking_xml->SetText(delay_collision_checking); - ompl_xml->InsertEndChild(delay_collision_checking_xml); - - return ompl_xml; -} - template void RRTstarConfigurator::serialize(Archive& ar, const unsigned int /*version*/) { @@ -856,97 +237,6 @@ void RRTstarConfigurator::serialize(Archive& ar, const unsigned int /*version*/) ar& BOOST_SERIALIZATION_NVP(delay_collision_checking); } -TRRTConfigurator::TRRTConfigurator(const tinyxml2::XMLElement& xml_element) -{ - const tinyxml2::XMLElement* trrt_element = xml_element.FirstChildElement("TRRT"); - const tinyxml2::XMLElement* range_element = trrt_element->FirstChildElement("Range"); - const tinyxml2::XMLElement* goal_bias_element = trrt_element->FirstChildElement("GoalBias"); - const tinyxml2::XMLElement* temp_change_factor_element = trrt_element->FirstChildElement("TempChangeFactor"); - const tinyxml2::XMLElement* init_temperature_element = trrt_element->FirstChildElement("InitTemp"); - const tinyxml2::XMLElement* frontier_threshold_element = trrt_element->FirstChildElement("FrontierThreshold"); - const tinyxml2::XMLElement* frontier_node_ratio_element = trrt_element->FirstChildElement("FrontierNodeRatio"); - - int status{ tinyxml2::XMLError::XML_SUCCESS }; - - if (range_element != nullptr) - { - std::string range_string; - status = tesseract_common::QueryStringText(range_element, range_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: TRRT: Error parsing Range string"); - - if (!tesseract_common::isNumeric(range_string)) - throw std::runtime_error("OMPLConfigurator: TRRT: Range is not a numeric values."); - - tesseract_common::toNumeric(range_string, range); - } - - if (goal_bias_element != nullptr) - { - std::string goal_bias_string; - status = tesseract_common::QueryStringText(goal_bias_element, goal_bias_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: TRRT: Error parsing GoalBias string"); - - if (!tesseract_common::isNumeric(goal_bias_string)) - throw std::runtime_error("OMPLConfigurator: TRRT: GoalBias is not a numeric values."); - - tesseract_common::toNumeric(goal_bias_string, goal_bias); - } - - if (temp_change_factor_element != nullptr) - { - std::string temp_change_factor_string; - status = tesseract_common::QueryStringText(temp_change_factor_element, temp_change_factor_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: TRRT: Error parsing TempChangeFactor string"); - - if (!tesseract_common::isNumeric(temp_change_factor_string)) - throw std::runtime_error("OMPLConfigurator: TRRT: TempChangeFactor is not a numeric values."); - - tesseract_common::toNumeric(temp_change_factor_string, temp_change_factor); - } - - if (init_temperature_element != nullptr) - { - std::string init_temperature_string; - status = tesseract_common::QueryStringText(init_temperature_element, init_temperature_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: TRRT: Error parsing InitTemp string"); - - if (!tesseract_common::isNumeric(init_temperature_string)) - throw std::runtime_error("OMPLConfigurator: TRRT: InitTemp is not a numeric values."); - - tesseract_common::toNumeric(init_temperature_string, init_temperature); - } - - if (frontier_threshold_element != nullptr) - { - std::string frontier_threshold_string; - status = tesseract_common::QueryStringText(frontier_threshold_element, frontier_threshold_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: TRRT: Error parsing FrontierThreshold string"); - - if (!tesseract_common::isNumeric(frontier_threshold_string)) - throw std::runtime_error("OMPLConfigurator: TRRT: FrontierThreshold is not a numeric values."); - - tesseract_common::toNumeric(frontier_threshold_string, frontier_threshold); - } - - if (frontier_node_ratio_element != nullptr) - { - std::string frontier_node_ratio_string; - status = tesseract_common::QueryStringText(frontier_node_ratio_element, frontier_node_ratio_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: TRRT: Error parsing FrontierNodeRatio string"); - - if (!tesseract_common::isNumeric(frontier_node_ratio_string)) - throw std::runtime_error("OMPLConfigurator: TRRT: FrontierNodeRatio is not a numeric values."); - - tesseract_common::toNumeric(frontier_node_ratio_string, frontier_node_ratio); - } -} - ompl::base::PlannerPtr TRRTConfigurator::create(ompl::base::SpaceInformationPtr si) const { auto planner = std::make_shared(si); @@ -961,37 +251,6 @@ ompl::base::PlannerPtr TRRTConfigurator::create(ompl::base::SpaceInformationPtr OMPLPlannerType TRRTConfigurator::getType() const { return OMPLPlannerType::TRRT; } -tinyxml2::XMLElement* TRRTConfigurator::toXML(tinyxml2::XMLDocument& doc) const -{ - tinyxml2::XMLElement* ompl_xml = doc.NewElement("TRRT"); - - tinyxml2::XMLElement* range_xml = doc.NewElement("Range"); - range_xml->SetText(range); - ompl_xml->InsertEndChild(range_xml); - - tinyxml2::XMLElement* goal_bias_xml = doc.NewElement("GoalBias"); - goal_bias_xml->SetText(goal_bias); - ompl_xml->InsertEndChild(goal_bias_xml); - - tinyxml2::XMLElement* temp_change_factor_xml = doc.NewElement("TempChangeFactor"); - temp_change_factor_xml->SetText(temp_change_factor); - ompl_xml->InsertEndChild(temp_change_factor_xml); - - tinyxml2::XMLElement* init_temperature_xml = doc.NewElement("InitTemp"); - init_temperature_xml->SetText(init_temperature); - ompl_xml->InsertEndChild(init_temperature_xml); - - tinyxml2::XMLElement* frontier_threshold_xml = doc.NewElement("FrontierThreshold"); - frontier_threshold_xml->SetText(frontier_threshold); - ompl_xml->InsertEndChild(frontier_threshold_xml); - - tinyxml2::XMLElement* frontier_node_ratio_xml = doc.NewElement("FrontierNodeRatio"); - frontier_node_ratio_xml->SetText(frontier_node_ratio); - ompl_xml->InsertEndChild(frontier_node_ratio_xml); - - return ompl_xml; -} - template void TRRTConfigurator::serialize(Archive& ar, const unsigned int /*version*/) { @@ -1004,27 +263,6 @@ void TRRTConfigurator::serialize(Archive& ar, const unsigned int /*version*/) ar& BOOST_SERIALIZATION_NVP(frontier_node_ratio); } -PRMConfigurator::PRMConfigurator(const tinyxml2::XMLElement& xml_element) -{ - const tinyxml2::XMLElement* prm_element = xml_element.FirstChildElement("PRM"); - const tinyxml2::XMLElement* max_nearest_neighbors_element = prm_element->FirstChildElement("MaxNearestNeighbors"); - - int status{ tinyxml2::XMLError::XML_SUCCESS }; - - if (max_nearest_neighbors_element != nullptr) - { - std::string max_nearest_neighbors_string; - status = tesseract_common::QueryStringText(max_nearest_neighbors_element, max_nearest_neighbors_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: PRM: Error parsing MaxNearestNeighbors string"); - - if (!tesseract_common::isNumeric(max_nearest_neighbors_string)) - throw std::runtime_error("OMPLConfigurator: PRM: MaxNearestNeighbors is not a numeric values."); - - tesseract_common::toNumeric(max_nearest_neighbors_string, max_nearest_neighbors); - } -} - ompl::base::PlannerPtr PRMConfigurator::create(ompl::base::SpaceInformationPtr si) const { auto planner = std::make_shared(si); @@ -1034,17 +272,6 @@ ompl::base::PlannerPtr PRMConfigurator::create(ompl::base::SpaceInformationPtr s OMPLPlannerType PRMConfigurator::getType() const { return OMPLPlannerType::PRM; } -tinyxml2::XMLElement* PRMConfigurator::toXML(tinyxml2::XMLDocument& doc) const -{ - tinyxml2::XMLElement* ompl_xml = doc.NewElement("PRM"); - - tinyxml2::XMLElement* max_nearest_neighbors_xml = doc.NewElement("MaxNearestNeighbors"); - max_nearest_neighbors_xml->SetText(max_nearest_neighbors); - ompl_xml->InsertEndChild(max_nearest_neighbors_xml); - - return ompl_xml; -} - template void PRMConfigurator::serialize(Archive& ar, const unsigned int /*version*/) { @@ -1052,8 +279,6 @@ void PRMConfigurator::serialize(Archive& ar, const unsigned int /*version*/) ar& BOOST_SERIALIZATION_NVP(max_nearest_neighbors); } -PRMstarConfigurator::PRMstarConfigurator(const tinyxml2::XMLElement&) {} - ompl::base::PlannerPtr PRMstarConfigurator::create(ompl::base::SpaceInformationPtr si) const { return std::make_shared(si); @@ -1061,21 +286,12 @@ ompl::base::PlannerPtr PRMstarConfigurator::create(ompl::base::SpaceInformationP OMPLPlannerType PRMstarConfigurator::getType() const { return OMPLPlannerType::PRMstar; } -tinyxml2::XMLElement* PRMstarConfigurator::toXML(tinyxml2::XMLDocument& doc) const -{ - tinyxml2::XMLElement* ompl_xml = doc.NewElement("PRMstar"); - - return ompl_xml; -} - template void PRMstarConfigurator::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); } -LazyPRMstarConfigurator::LazyPRMstarConfigurator(const tinyxml2::XMLElement&) {} - ompl::base::PlannerPtr LazyPRMstarConfigurator::create(ompl::base::SpaceInformationPtr si) const { return std::make_shared(si); @@ -1083,82 +299,12 @@ ompl::base::PlannerPtr LazyPRMstarConfigurator::create(ompl::base::SpaceInformat OMPLPlannerType LazyPRMstarConfigurator::getType() const { return OMPLPlannerType::LazyPRMstar; } -tinyxml2::XMLElement* LazyPRMstarConfigurator::toXML(tinyxml2::XMLDocument& doc) const -{ - tinyxml2::XMLElement* ompl_xml = doc.NewElement("LazyPRMstar"); - - return ompl_xml; -} - template void LazyPRMstarConfigurator::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); } -SPARSConfigurator::SPARSConfigurator(const tinyxml2::XMLElement& xml_element) -{ - const tinyxml2::XMLElement* spars_element = xml_element.FirstChildElement("SPARS"); - const tinyxml2::XMLElement* max_failures_element = spars_element->FirstChildElement("MaxFailures"); - const tinyxml2::XMLElement* dense_delta_fraction_element = spars_element->FirstChildElement("DenseDataFraction"); - const tinyxml2::XMLElement* sparse_delta_fraction_element = spars_element->FirstChildElement("SparseDeltaFraction"); - const tinyxml2::XMLElement* stretch_factor_element = spars_element->FirstChildElement("StretchFactor"); - - int status{ tinyxml2::XMLError::XML_SUCCESS }; - - if (max_failures_element != nullptr) - { - std::string max_failures_string; - status = tesseract_common::QueryStringText(max_failures_element, max_failures_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: SPARS: Error parsing MaxFailures string"); - - if (!tesseract_common::isNumeric(max_failures_string)) - throw std::runtime_error("OMPLConfigurator: SPARS: MaxFailures is not a numeric values."); - - tesseract_common::toNumeric(max_failures_string, max_failures); - } - - if (dense_delta_fraction_element != nullptr) - { - std::string dense_delta_fraction_string; - status = tesseract_common::QueryStringText(dense_delta_fraction_element, dense_delta_fraction_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: SPARS: Error parsing DenseDataFraction string"); - - if (!tesseract_common::isNumeric(dense_delta_fraction_string)) - throw std::runtime_error("OMPLConfigurator: SPARS: DenseDataFraction is not a numeric values."); - - tesseract_common::toNumeric(dense_delta_fraction_string, dense_delta_fraction); - } - - if (sparse_delta_fraction_element != nullptr) - { - std::string sparse_delta_fraction_string; - status = tesseract_common::QueryStringText(sparse_delta_fraction_element, sparse_delta_fraction_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: SPARS: Error parsing SparseDeltaFraction string"); - - if (!tesseract_common::isNumeric(sparse_delta_fraction_string)) - throw std::runtime_error("OMPLConfigurator: SPARS: SparseDeltaFraction is not a numeric values."); - - tesseract_common::toNumeric(sparse_delta_fraction_string, sparse_delta_fraction); - } - - if (stretch_factor_element != nullptr) - { - std::string stretch_factor_string; - status = tesseract_common::QueryStringText(stretch_factor_element, stretch_factor_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLConfigurator: SPARS: Error parsing StretchFactor string"); - - if (!tesseract_common::isNumeric(stretch_factor_string)) - throw std::runtime_error("OMPLConfigurator: SPARS: StretchFactor is not a numeric values."); - - tesseract_common::toNumeric(stretch_factor_string, stretch_factor); - } -} - ompl::base::PlannerPtr SPARSConfigurator::create(ompl::base::SpaceInformationPtr si) const { auto planner = std::make_shared(si); @@ -1171,29 +317,6 @@ ompl::base::PlannerPtr SPARSConfigurator::create(ompl::base::SpaceInformationPtr OMPLPlannerType SPARSConfigurator::getType() const { return OMPLPlannerType::SPARS; } -tinyxml2::XMLElement* SPARSConfigurator::toXML(tinyxml2::XMLDocument& doc) const -{ - tinyxml2::XMLElement* ompl_xml = doc.NewElement("SPARS"); - - tinyxml2::XMLElement* max_failures_xml = doc.NewElement("MaxFailures"); - max_failures_xml->SetText(max_failures); - ompl_xml->InsertEndChild(max_failures_xml); - - tinyxml2::XMLElement* dense_delta_fraction_xml = doc.NewElement("DenseDataFraction"); - dense_delta_fraction_xml->SetText(dense_delta_fraction); - ompl_xml->InsertEndChild(dense_delta_fraction_xml); - - tinyxml2::XMLElement* sparse_delta_fraction_xml = doc.NewElement("SparseDeltaFraction"); - sparse_delta_fraction_xml->SetText(sparse_delta_fraction); - ompl_xml->InsertEndChild(sparse_delta_fraction_xml); - - tinyxml2::XMLElement* stretch_factor_xml = doc.NewElement("StretchFactor"); - stretch_factor_xml->SetText(stretch_factor); - ompl_xml->InsertEndChild(stretch_factor_xml); - - return ompl_xml; -} - template void SPARSConfigurator::serialize(Archive& ar, const unsigned int /*version*/) { diff --git a/tesseract_motion_planners/ompl/src/ompl_problem.cpp b/tesseract_motion_planners/ompl/src/ompl_solver_config.cpp similarity index 54% rename from tesseract_motion_planners/ompl/src/ompl_problem.cpp rename to tesseract_motion_planners/ompl/src/ompl_solver_config.cpp index b0d93e39ea..c00584fd02 100644 --- a/tesseract_motion_planners/ompl/src/ompl_problem.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_solver_config.cpp @@ -1,6 +1,6 @@ /** - * @file ompl_problem.h - * @brief Tesseract OMPL problem definition + * @file ompl_solver_config.cpp + * @brief Tesseract OMPL solver config * * @author Levi Armstrong * @date June 18, 2020 @@ -24,24 +24,25 @@ * limitations under the License. */ -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include +#include #include -#include +#include +#include +#include namespace tesseract_planning { -tesseract_common::TrajArray OMPLProblem::getTrajectory() const +template +void OMPLSolverConfig::serialize(Archive& ar, const unsigned int /*version*/) { - assert(extractor != nullptr); - return toTrajArray(this->simple_setup->getSolutionPath(), extractor); + 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); } - } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::OMPLSolverConfig) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::OMPLSolverConfig) 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 deleted file mode 100644 index a3f164a3b0..0000000000 --- a/tesseract_motion_planners/ompl/src/profile/ompl_default_plan_profile.cpp +++ /dev/null @@ -1,806 +0,0 @@ -/** - * @file ompl_default_plan_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 -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -#include -#include -#include -#include -#include -#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 -{ -OMPLDefaultPlanProfile::OMPLDefaultPlanProfile() - : planners({ std::make_shared(), std::make_shared() }) -{ -} - -OMPLDefaultPlanProfile::OMPLDefaultPlanProfile(const tinyxml2::XMLElement& xml_element) - : planners({ std::make_shared(), std::make_shared() }) -{ - 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."); - - state_space = static_cast(type); - } - - if (planning_time_element != nullptr) - { - 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"); - - if (!tesseract_common::isNumeric(planning_time_string)) - throw std::runtime_error("OMPLPlanProfile: PlanningTime is not a numeric values."); - - tesseract_common::toNumeric(planning_time_string, planning_time); - } - - if (max_solutions_element != nullptr) - { - 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"); - - if (!tesseract_common::isNumeric(max_solutions_string)) - throw std::runtime_error("OMPLPlanProfile: MaxSolutions is not a numeric values."); - - tesseract_common::toNumeric(max_solutions_string, max_solutions); - } - - 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"); - } - - if (optimize_element != nullptr) - { - status = optimize_element->QueryBoolText(&optimize); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLPlanProfile: Error parsing Optimize string"); - } - - if (planners_element != nullptr) - { - 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."); - - switch (type) - { - 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"); - } - } - } - } - - /// @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); - // } -} - -void OMPLDefaultPlanProfile::setup(OMPLProblem& prob) const -{ - prob.planners = planners; - prob.planning_time = planning_time; - prob.max_solutions = max_solutions; - prob.simplify = simplify; - prob.optimize = optimize; - - 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); - }; - -#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!"); - - 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)); - - 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; - - // 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); - - // Setup state checking functionality - ompl::base::StateValidityCheckerPtr svc_without_collision = processStateValidator(prob); - - // Setup motion validation (i.e. collision checking) - processMotionValidator(prob, svc_without_collision); - - // make sure the planners run until the time limit, and get the best possible solution - processOptimizationObjective(prob); - } -} - -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 auto dof = prob.manip->numJoints(); - tesseract_common::KinematicLimits limits = prob.manip->getLimits(); - - assert(!(manip_info.empty() && parent_instruction.getManipulatorInfo().empty())); - tesseract_common::ManipulatorInfo mi = manip_info.getCombined(parent_instruction.getManipulatorInfo()); - - if (mi.manipulator.empty()) - throw std::runtime_error("OMPL, manipulator is empty!"); - - if (mi.tcp_frame.empty()) - throw std::runtime_error("OMPL, tcp_frame is empty!"); - - if (mi.working_frame.empty()) - throw std::runtime_error("OMPL, working_frame is empty!"); - - Eigen::Isometry3d tcp_offset = prob.env->findTCPOffset(mi); - - 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); - } -} - -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) - { - // 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 auto dof = prob.manip->numJoints(); - tesseract_common::KinematicLimits limits = prob.manip->getLimits(); - - assert(!(manip_info.empty() && parent_instruction.getManipulatorInfo().empty())); - tesseract_common::ManipulatorInfo mi = manip_info.getCombined(parent_instruction.getManipulatorInfo()); - - if (mi.manipulator.empty()) - throw std::runtime_error("OMPL, manipulator is empty!"); - - if (mi.tcp_frame.empty()) - throw std::runtime_error("OMPL, tcp_frame is empty!"); - - if (mi.working_frame.empty()) - throw std::runtime_error("OMPL, working_frame is empty!"); - - Eigen::Isometry3d tcp = prob.env->findTCPOffset(mi); - - 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); - } - } - } - - 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 - { - 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: Start state is outside limits"); - } - - // 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()); - } - - 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); - } -} - -tinyxml2::XMLElement* OMPLDefaultPlanProfile::toXML(tinyxml2::XMLDocument& doc) const -{ - Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " "); - - tinyxml2::XMLElement* xml_planner = doc.NewElement("Planner"); - xml_planner->SetAttribute("type", std::to_string(2).c_str()); - - tinyxml2::XMLElement* xml_ompl = doc.NewElement("OMPLPlanProfile"); - - tinyxml2::XMLElement* xml_ompl_planners = doc.NewElement("Planners"); - - for (const auto& planner : planners) - { - 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); - } - - 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); - - // TODO: Add plugins for state_sampler_allocator, optimization_objective_allocator, svc_allocator, - // mv_allocator - - xml_planner->InsertEndChild(xml_ompl); - - return xml_planner; -} - -ompl::base::StateValidityCheckerPtr OMPLDefaultPlanProfile::processStateValidator(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); - } - - if (collision_check_config.type == tesseract_collision::CollisionEvaluatorType::DISCRETE || - collision_check_config.type == tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE) - { - auto svc = std::make_shared( - 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; -} - -void OMPLDefaultPlanProfile::processMotionValidator( - 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) - { - 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(), - 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); - } - } -} - -void OMPLDefaultPlanProfile::processOptimizationObjective(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())); - } -} - -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 - -#include -TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::OMPLDefaultPlanProfile) -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::OMPLDefaultPlanProfile) diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp new file mode 100644 index 0000000000..e77b80db7b --- /dev/null +++ b/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp @@ -0,0 +1,507 @@ +/** + * @file ompl_real_vector_plan_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 +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace tesseract_planning +{ +OMPLRealVectorPlanProfile::OMPLRealVectorPlanProfile() +{ + solver_config.planners = { std::make_shared(), + std::make_shared() }; +} + +std::unique_ptr OMPLRealVectorPlanProfile::createSolverConfig() const +{ + return std::make_unique(solver_config); +} + +OMPLStateExtractor OMPLRealVectorPlanProfile::createStateExtractor(const tesseract_kinematics::JointGroup& manip) const +{ + const auto dof = static_cast(manip.numJoints()); + return [dof](const ompl::base::State* state) -> Eigen::Map { + return tesseract_planning::RealVectorStateSpaceExtractor(state, dof); + }; +} + +std::unique_ptr +OMPLRealVectorPlanProfile::createSimpleSetup(const MoveInstructionPoly& start_instruction, + const MoveInstructionPoly& end_instruction, + const tesseract_common::ManipulatorInfo& composite_mi, + const std::shared_ptr& env) const +{ + // Start and End Manipulator Information + // These should have the same manipulator name but could have different ik sovler name + tesseract_common::ManipulatorInfo start_mi = composite_mi.getCombined(start_instruction.getManipulatorInfo()); + tesseract_common::ManipulatorInfo end_mi = composite_mi.getCombined(end_instruction.getManipulatorInfo()); + + // Get kinematics + tesseract_kinematics::JointGroup::Ptr manip = env->getJointGroup(end_mi.manipulator); + const auto dof = static_cast(manip->numJoints()); + const std::vector joint_names = manip->getJointNames(); + const Eigen::MatrixX2d limits = manip->getLimits().joint_limits; + + // 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)); + + rss->setStateSamplerAllocator(createStateSamplerAllocator(env, manip)); + + state_space_ptr = rss; + + // Setup Longest Valid Segment + processLongestValidSegment(state_space_ptr, collision_check_config); + + // Create Simple Setup from state space + auto simple_setup = std::make_unique(state_space_ptr); + + // Create state extractor + OMPLStateExtractor state_extractor = createStateExtractor(*manip); + + // Setup state validators + auto csvc = std::make_shared(); + ompl::base::StateValidityCheckerPtr svc_without_collision = + createStateValidator(*simple_setup, env, manip, state_extractor); + if (svc_without_collision != nullptr) + csvc->addStateValidator(svc_without_collision); + + auto svc_collision = createCollisionStateValidator(*simple_setup, env, manip, state_extractor); + if (svc_collision != nullptr) + csvc->addStateValidator(std::move(svc_collision)); + + simple_setup->setStateValidityChecker(csvc); + + // Setup motion validation (i.e. collision checking) + auto mv = createMotionValidator(*simple_setup, env, manip, state_extractor, svc_without_collision); + if (mv != nullptr) + simple_setup->getSpaceInformation()->setMotionValidator(std::move(mv)); + + // make sure the planners run until the time limit, and get the best possible solution + if (solver_config.optimize) + { + auto obj = createOptimizationObjective(*simple_setup, env, manip, state_extractor); + if (obj != nullptr) + simple_setup->getProblemDefinition()->setOptimizationObjective(std::move(obj)); + } + + // Collision checker for validating start and goal states + tesseract_collision::DiscreteContactManager::UPtr contact_checker = env->getDiscreteContactManager(); + contact_checker->applyContactManagerConfig(collision_check_config.contact_manager_config); + contact_checker->setCollisionObjectsTransform(env->getState().link_transforms); + + // Add start states + if (start_instruction.getWaypoint().isJointWaypoint() || start_instruction.getWaypoint().isStateWaypoint()) + { + tesseract_kinematics::JointGroup::UPtr joint_group = env->getJointGroup(start_mi.manipulator); + assert(checkJointPositionFormat(joint_group->getJointNames(), start_instruction.getWaypoint())); + contact_checker->setActiveCollisionObjects(joint_group->getActiveLinkNames()); + const Eigen::VectorXd& cur_position = getJointPosition(start_instruction.getWaypoint()); + applyStartStates(*simple_setup, cur_position, *joint_group, *contact_checker); + } + else if (start_instruction.getWaypoint().isCartesianWaypoint()) + { + const auto& cur_wp = start_instruction.getWaypoint().as(); + Eigen::Isometry3d tcp_offset = env->findTCPOffset(start_mi); + Eigen::Isometry3d tcp_frame_cwp = cur_wp.getTransform() * tcp_offset.inverse(); + tesseract_kinematics::KinematicGroup::UPtr kin_group; + if (start_mi.manipulator_ik_solver.empty()) + kin_group = env->getKinematicGroup(start_mi.manipulator); + else + kin_group = env->getKinematicGroup(start_mi.manipulator, start_mi.manipulator_ik_solver); + + contact_checker->setActiveCollisionObjects(kin_group->getActiveLinkNames()); + tesseract_kinematics::KinGroupIKInput ik_input(tcp_frame_cwp, start_mi.working_frame, start_mi.tcp_frame); + applyStartStates(*simple_setup, ik_input, *kin_group, *contact_checker); + } + else + { + throw std::runtime_error("OMPL, unsupported waypoint type"); + } + + // Add Goal states + if (end_instruction.getWaypoint().isJointWaypoint() || end_instruction.getWaypoint().isStateWaypoint()) + { + tesseract_kinematics::JointGroup::UPtr joint_group = env->getJointGroup(end_mi.manipulator); + assert(checkJointPositionFormat(joint_group->getJointNames(), end_instruction.getWaypoint())); + contact_checker->setActiveCollisionObjects(joint_group->getActiveLinkNames()); + const Eigen::VectorXd& cur_position = getJointPosition(end_instruction.getWaypoint()); + applyGoalStates(*simple_setup, cur_position, *joint_group, *contact_checker); + } + else if (end_instruction.getWaypoint().isCartesianWaypoint()) + { + const auto& cur_wp = end_instruction.getWaypoint().as(); + Eigen::Isometry3d tcp_offset = env->findTCPOffset(end_mi); + Eigen::Isometry3d tcp_frame_cwp = cur_wp.getTransform() * tcp_offset.inverse(); + tesseract_kinematics::KinematicGroup::UPtr kin_group; + if (end_mi.manipulator_ik_solver.empty()) + kin_group = env->getKinematicGroup(end_mi.manipulator); + else + kin_group = env->getKinematicGroup(end_mi.manipulator, end_mi.manipulator_ik_solver); + + contact_checker->setActiveCollisionObjects(kin_group->getActiveLinkNames()); + tesseract_kinematics::KinGroupIKInput ik_input(tcp_frame_cwp, end_mi.working_frame, end_mi.tcp_frame); + applyGoalStates(*simple_setup, ik_input, *kin_group, *contact_checker); + } + else + { + throw std::runtime_error("OMPL, unsupported waypoint type"); + } + + return simple_setup; +} + +void OMPLRealVectorPlanProfile::applyGoalStates(ompl::geometric::SimpleSetup& simple_setup, + const tesseract_kinematics::KinGroupIKInput& ik_input, + const tesseract_kinematics::KinematicGroup& manip, + tesseract_collision::DiscreteContactManager& contact_checker) +{ + /** @todo Need to add Descartes pose sample to ompl profile */ + const auto dof = manip.numJoints(); + tesseract_common::KinematicLimits limits = manip.getLimits(); + tesseract_kinematics::IKSolutions joint_solutions = manip.calcInvKin({ ik_input }, Eigen::VectorXd::Zero(dof)); + auto goal_states = std::make_shared(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 OMPLRealVectorPlanProfile: 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(contact_map_vec[i], contact_checker, manip, solution)) + { + { + ompl::base::ScopedState<> goal_state(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, manip.getRedundancyCapableJointIndices()); + for (const auto& rs : redundant_solutions) + { + ompl::base::ScopedState<> goal_state(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 OMPLRealVectorPlanProfile: All goal states are either in collision or outside limits"); + } + simple_setup.setGoal(goal_states); +} + +void OMPLRealVectorPlanProfile::applyGoalStates(ompl::geometric::SimpleSetup& simple_setup, + const Eigen::VectorXd& joint_waypoint, + const tesseract_kinematics::JointGroup& manip, + tesseract_collision::DiscreteContactManager& contact_checker) +{ + const auto dof = manip.numJoints(); + tesseract_common::KinematicLimits limits = 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 OMPLRealVectorPlanProfile: 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(contact_map, contact_checker, manip, solution)) + { + CONSOLE_BRIDGE_logError("In OMPLRealVectorPlanProfile: 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(simple_setup.getStateSpace()); + for (unsigned i = 0; i < dof; ++i) + goal_state[i] = joint_waypoint[i]; + + simple_setup.setGoalState(goal_state); +} + +void OMPLRealVectorPlanProfile::applyStartStates(ompl::geometric::SimpleSetup& simple_setup, + const tesseract_kinematics::KinGroupIKInput& ik_input, + const tesseract_kinematics::KinematicGroup& manip, + tesseract_collision::DiscreteContactManager& contact_checker) +{ + /** @todo Need to add Descartes pose sampler to ompl profile */ + /** @todo Need to also provide the seed instruction to use here */ + const auto dof = manip.numJoints(); + tesseract_common::KinematicLimits limits = manip.getLimits(); + tesseract_kinematics::IKSolutions joint_solutions = 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 OMPLRealVectorPlanProfile: 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(contact_map_vec[i], contact_checker, manip, solution)) + { + found_start_state = true; + { + ompl::base::ScopedState<> start_state(simple_setup.getStateSpace()); + for (unsigned j = 0; j < dof; ++j) + start_state[j] = solution[static_cast(j)]; + + simple_setup.addStartState(start_state); + } + + auto redundant_solutions = tesseract_kinematics::getRedundantSolutions( + solution, limits.joint_limits, manip.getRedundancyCapableJointIndices()); + for (const auto& rs : redundant_solutions) + { + ompl::base::ScopedState<> start_state(simple_setup.getStateSpace()); + for (unsigned j = 0; j < dof; ++j) + start_state[j] = rs[static_cast(j)]; + + 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"); + } +} + +void OMPLRealVectorPlanProfile::applyStartStates(ompl::geometric::SimpleSetup& simple_setup, + const Eigen::VectorXd& joint_waypoint, + const tesseract_kinematics::JointGroup& manip, + tesseract_collision::DiscreteContactManager& contact_checker) +{ + const auto dof = manip.numJoints(); + tesseract_common::KinematicLimits limits = manip.getLimits(); + + 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 OMPLRealVectorPlanProfile: Start state is outside limits"); + } + + // 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(contact_map, contact_checker, manip, solution)) + { + 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()); + } + + ompl::base::ScopedState<> start_state(simple_setup.getStateSpace()); + for (unsigned i = 0; i < dof; ++i) + start_state[i] = joint_waypoint[i]; + + simple_setup.addStartState(start_state); +} + +std::function(const ompl::base::StateSpace*)> +OMPLRealVectorPlanProfile::createStateSamplerAllocator( + const std::shared_ptr& /*env*/, + const std::shared_ptr& manip) const +{ + Eigen::MatrixX2d limits = manip->getLimits().joint_limits; + Eigen::VectorXd weights = Eigen::VectorXd::Ones(manip->numJoints()); + return [weights, limits](const ompl::base::StateSpace* state_space) -> ompl::base::StateSamplerPtr { + return allocWeightedRealVectorStateSampler(state_space, weights, limits); + }; +} + +std::unique_ptr OMPLRealVectorPlanProfile::createStateValidator( + const ompl::geometric::SimpleSetup& /*simple_setup*/, + const std::shared_ptr& /*env*/, + const std::shared_ptr& /*manip*/, + const OMPLStateExtractor& /*state_extractor*/) const +{ + return nullptr; +} + +std::unique_ptr OMPLRealVectorPlanProfile::createCollisionStateValidator( + const ompl::geometric::SimpleSetup& simple_setup, + const std::shared_ptr& env, + const std::shared_ptr& manip, + const OMPLStateExtractor& state_extractor) const +{ + if (collision_check_config.type == tesseract_collision::CollisionEvaluatorType::DISCRETE || + collision_check_config.type == tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE) + { + return std::make_unique( + simple_setup.getSpaceInformation(), *env, manip, collision_check_config, state_extractor); + } + + return nullptr; +} + +std::unique_ptr OMPLRealVectorPlanProfile::createMotionValidator( + const ompl::geometric::SimpleSetup& simple_setup, + const std::shared_ptr& env, + const std::shared_ptr& manip, + const OMPLStateExtractor& state_extractor, + const std::shared_ptr& svc_without_collision) const +{ + 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) + { + return std::make_unique(simple_setup.getSpaceInformation(), + svc_without_collision, + *env, + manip, + collision_check_config, + state_extractor); + } + + // Collision checking is preformed using the state validator which this calls. + return std::make_unique(simple_setup.getSpaceInformation()); + } + + return nullptr; +} + +std::unique_ptr OMPLRealVectorPlanProfile::createOptimizationObjective( + const ompl::geometric::SimpleSetup& simple_setup, + const std::shared_ptr& /*env*/, + const std::shared_ptr& /*manip*/, + const OMPLStateExtractor& /*state_extractor*/) const +{ + return std::make_unique(simple_setup.getSpaceInformation()); +} + +template +void OMPLRealVectorPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlanProfile); + ar& BOOST_SERIALIZATION_NVP(solver_config); + ar& BOOST_SERIALIZATION_NVP(collision_check_config); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::OMPLRealVectorPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::OMPLRealVectorPlanProfile) 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/src/utils.cpp b/tesseract_motion_planners/ompl/src/utils.cpp index 6157974418..36cbde39cf 100644 --- a/tesseract_motion_planners/ompl/src/utils.cpp +++ b/tesseract_motion_planners/ompl/src/utils.cpp @@ -37,7 +37,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include #include #include @@ -108,16 +107,17 @@ void processLongestValidSegment(const ompl::base::StateSpacePtr& state_space_ptr state_space_ptr->setLongestValidSegmentFraction(longest_valid_segment_fraction); } -bool checkStateInCollision(OMPLProblem& prob, - const Eigen::VectorXd& state, - tesseract_collision::ContactResultMap& contact_map) +bool checkStateInCollision(tesseract_collision::ContactResultMap& contact_map, + tesseract_collision::DiscreteContactManager& contact_checker, + const tesseract_kinematics::JointGroup& manip, + const Eigen::VectorXd& state) { - tesseract_common::TransformMap link_transforms = prob.manip->calcFwdKin(state); + tesseract_common::TransformMap link_transforms = manip.calcFwdKin(state); - for (const auto& link_name : prob.contact_checker->getActiveCollisionObjects()) - prob.contact_checker->setCollisionObjectsTransform(link_name, link_transforms[link_name]); + for (const auto& link_name : contact_checker.getActiveCollisionObjects()) + contact_checker.setCollisionObjectsTransform(link_name, link_transforms[link_name]); - prob.contact_checker->contactTest(contact_map, tesseract_collision::ContactTestType::FIRST); + contact_checker.contactTest(contact_map, tesseract_collision::ContactTestType::FIRST); return (!contact_map.empty()); } diff --git a/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp b/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp index 1fa5b17981..7b63b22431 100644 --- a/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp +++ b/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp @@ -62,9 +62,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #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 { @@ -227,17 +192,17 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit) // NOLINT CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env, 3.14, 1.0, 3.14, 10); // Create Profiles - auto plan_profile = std::make_shared(); + auto plan_profile = std::make_shared(); plan_profile->collision_check_config.contact_manager_config.margin_data_override_type = tesseract_collision::CollisionMarginOverrideType::OVERRIDE_DEFAULT_MARGIN; plan_profile->collision_check_config.contact_manager_config.margin_data.setDefaultCollisionMargin(0.025); plan_profile->collision_check_config.longest_valid_segment_length = 0.1; plan_profile->collision_check_config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; - plan_profile->planning_time = 10; - plan_profile->optimize = false; - plan_profile->max_solutions = 2; - plan_profile->simplify = false; - plan_profile->planners = { this->configurator, this->configurator }; + plan_profile->solver_config.planning_time = 10; + plan_profile->solver_config.optimize = false; + plan_profile->solver_config.max_solutions = 2; + plan_profile->solver_config.simplify = false; + plan_profile->solver_config.planners = { this->configurator, this->configurator }; // Profile Dictionary auto profiles = std::make_shared(); @@ -389,17 +354,17 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianGoalPlannerUnit) // NOLINT CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env, 3.14, 1.0, 3.14, 10); // Create Profiles - auto plan_profile = std::make_shared(); + auto plan_profile = std::make_shared(); plan_profile->collision_check_config.contact_manager_config.margin_data_override_type = tesseract_collision::CollisionMarginOverrideType::OVERRIDE_DEFAULT_MARGIN; plan_profile->collision_check_config.contact_manager_config.margin_data.setDefaultCollisionMargin(0.02); plan_profile->collision_check_config.longest_valid_segment_length = 0.1; plan_profile->collision_check_config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; - plan_profile->planning_time = 10; - plan_profile->optimize = false; - plan_profile->max_solutions = 2; - plan_profile->simplify = false; - plan_profile->planners = { this->configurator, this->configurator }; + plan_profile->solver_config.planning_time = 10; + plan_profile->solver_config.optimize = false; + plan_profile->solver_config.max_solutions = 2; + plan_profile->solver_config.simplify = false; + plan_profile->solver_config.planners = { this->configurator, this->configurator }; // Profile Dictionary auto profiles = std::make_shared(); @@ -482,17 +447,17 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianStartPlannerUnit) // NOLINT CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env, 3.14, 1.0, 3.14, 10); // Create Profiles - auto plan_profile = std::make_shared(); + auto plan_profile = std::make_shared(); plan_profile->collision_check_config.contact_manager_config.margin_data_override_type = tesseract_collision::CollisionMarginOverrideType::OVERRIDE_DEFAULT_MARGIN; plan_profile->collision_check_config.contact_manager_config.margin_data.setDefaultCollisionMargin(0.02); plan_profile->collision_check_config.longest_valid_segment_length = 0.1; plan_profile->collision_check_config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; - plan_profile->planning_time = 10; - plan_profile->optimize = false; - plan_profile->max_solutions = 2; - plan_profile->simplify = false; - plan_profile->planners = { this->configurator, this->configurator }; + plan_profile->solver_config.planning_time = 10; + plan_profile->solver_config.optimize = false; + plan_profile->solver_config.max_solutions = 2; + plan_profile->solver_config.simplify = false; + plan_profile->solver_config.planners = { this->configurator, this->configurator }; // Profile Dictionary auto profiles = std::make_shared();