diff --git a/tesseract_command_language/include/tesseract_command_language/composite_instruction.h b/tesseract_command_language/include/tesseract_command_language/composite_instruction.h index 81c8f21418a..9815d79d70d 100644 --- a/tesseract_command_language/include/tesseract_command_language/composite_instruction.h +++ b/tesseract_command_language/include/tesseract_command_language/composite_instruction.h @@ -201,6 +201,7 @@ class CompositeInstruction iterator insert(const_iterator p, const value_type& x); iterator insert(const_iterator p, value_type&& x); iterator insert(const_iterator p, std::initializer_list l); + iterator insert(const_iterator p, std::size_t n, const value_type& x); template void insert(const_iterator pos, InputIt first, InputIt last) { diff --git a/tesseract_command_language/include/tesseract_command_language/joint_waypoint.h b/tesseract_command_language/include/tesseract_command_language/joint_waypoint.h index 9f8e3b25a81..704fe2341c8 100644 --- a/tesseract_command_language/include/tesseract_command_language/joint_waypoint.h +++ b/tesseract_command_language/include/tesseract_command_language/joint_waypoint.h @@ -89,7 +89,10 @@ class JointWaypoint /** @returns norm of vector */ inline double norm() const { return waypoint.norm(); } /** @returns true if two are approximate */ - inline bool isApprox(const Eigen::VectorXd& other, double prec = 1e-12) { return waypoint.isApprox(other, prec); } + inline bool isApprox(const Eigen::VectorXd& other, double prec = 1e-12) const + { + return waypoint.isApprox(other, prec); + } /** @returns the transpose of the joint positions */ inline ConstTransposeReturnType transpose() const { return waypoint.transpose(); } // NOLINT diff --git a/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h b/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h index 4973bd039fc..a5aa1ca5d03 100644 --- a/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h +++ b/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h @@ -29,26 +29,121 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include +#include #include #include #include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #ifdef SWIG %shared_ptr(tesseract_planning::ProfileDictionary) #endif // SWIG +namespace tesseract_environment +{ +class Environment; +} + namespace tesseract_planning { +class Instruction; +class CompositeInstruction; + +/** + * @brief Struct to produce a planner-specific planning profile to apply to a single waypoint. + * @details Examples of waypoint profiles might include costs/constraints for a waypoint or a waypoint sampler + */ +class WaypointProfile +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + WaypointProfile() = default; + WaypointProfile(const WaypointProfile&) = delete; + WaypointProfile& operator=(const WaypointProfile&) = delete; + WaypointProfile(WaypointProfile&&) = delete; + WaypointProfile&& operator=(WaypointProfile&&) = delete; + + virtual ~WaypointProfile() = default; + + virtual std::any create(const Instruction& instruction, const tesseract_environment::Environment& env) const = 0; + +private: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int) // NOLINT + { + } +}; + +/** + * @brief Struct to produce a planner-specific planning profile to apply to a collection of waypoints defined in a + * composite instruction. + * @details Examples of composite profiles include costs/constraints that apply collectively to a group of waypoints + */ +class CompositeProfile +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + CompositeProfile() = default; + CompositeProfile(const CompositeProfile&) = delete; + CompositeProfile& operator=(const CompositeProfile&) = delete; + CompositeProfile(CompositeProfile&&) = delete; + CompositeProfile&& operator=(CompositeProfile&&) = delete; + + virtual ~CompositeProfile() = default; + virtual std::any create(const CompositeInstruction& instruction, + const tesseract_environment::Environment& env) const = 0; + +private: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int) // NOLINT + { + } +}; + +/** + * @brief Struct to produce configuration parameters for the motion planner + */ +struct PlannerProfile +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + PlannerProfile() = default; + PlannerProfile(const PlannerProfile&) = delete; + PlannerProfile& operator=(const PlannerProfile&) = delete; + PlannerProfile(PlannerProfile&&) = delete; + PlannerProfile&& operator=(PlannerProfile&&) = delete; + + virtual ~PlannerProfile() = default; + + virtual std::any create() const = 0; + +private: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int) // NOLINT + { + } +}; + /** * @brief This class is used to store profiles for motion planning and process planning * @details This is a thread safe class * A ProfileEntry is a std::unordered_map> * - The key is the profile name * - Where std::shared_ptr is the profile - * The ProfleEntry is also stored in std::unordered_map where the key here is the std::type_index(typeid(T)) + * The ProfileEntry is also stored in std::unordered_map where the key here is the std::type_index(typeid(T)) * @note When adding a profile entry the T should be the base class type. */ class ProfileDictionary @@ -210,10 +305,65 @@ class ProfileDictionary .erase(profile_name); } + std::unordered_map> waypoint_profiles; + std::unordered_map> composite_profiles; + std::unordered_map> planner_profiles; + + WaypointProfile::ConstPtr getWaypointProfile(const std::string& ns, const std::string& profile_name) const + { + return getProfile(ns, profile_name, waypoint_profiles); + } + + CompositeProfile::ConstPtr getCompositeProfile(const std::string& ns, const std::string& profile_name) const + { + return getProfile(ns, profile_name, composite_profiles); + } + + PlannerProfile::ConstPtr getPlannerProfile(const std::string& ns, const std::string& profile_name) const + { + return getProfile(ns, profile_name, planner_profiles); + } + protected: + template + typename ProfileT::ConstPtr getProfile( + const std::string& ns, + const std::string& profile_name, + const std::unordered_map>& map) const + { + try + { + return map.at(ns).at(profile_name); + } + catch (const std::out_of_range&) + { + std::stringstream ss; + ss << "Failed to get " << boost::core::demangle(typeid(ProfileT).name()) << " for '" << ns << "/" << profile_name + << "'\n"; + if (map.find(ns) == map.end()) + { + ss << "Profile namespace '" << ns << "' does not exist"; + } + else + { + ss << "Entries in profile namespace '" << ns << "' are:"; + for (auto it = map.at(ns).begin(); it != map.at(ns).end(); ++it) + { + ss << "\n\t" << it->first; + } + } + + throw std::out_of_range(ss.str()); + } + } + std::unordered_map> profiles_; mutable std::shared_mutex mutex_; }; } // namespace tesseract_planning +BOOST_SERIALIZATION_ASSUME_ABSTRACT(tesseract_planning::WaypointProfile); +BOOST_SERIALIZATION_ASSUME_ABSTRACT(tesseract_planning::CompositeProfile); +BOOST_SERIALIZATION_ASSUME_ABSTRACT(tesseract_planning::PlannerProfile); + #endif // TESSERACT_MOTION_PLANNERS_PROFILE_DICTIONARY_H diff --git a/tesseract_command_language/include/tesseract_command_language/utils/get_instruction_utils.h b/tesseract_command_language/include/tesseract_command_language/utils/get_instruction_utils.h index 505add5f9c7..27347e31b9b 100644 --- a/tesseract_command_language/include/tesseract_command_language/utils/get_instruction_utils.h +++ b/tesseract_command_language/include/tesseract_command_language/utils/get_instruction_utils.h @@ -35,6 +35,7 @@ namespace tesseract_planning { /** * @brief Get the first Instruction in a Composite Instruction that is identified by the filter + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction Composite Instruction to search * @param locate_filter The filter to indicate if an instruction should be considered * @param process_child_composites Indicate if child Composite Instructions should be searched @@ -46,6 +47,7 @@ const Instruction* getFirstInstruction(const CompositeInstruction& composite_ins /** * @brief Get the first Instruction in a Composite Instruction that is identified by the filter + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction Composite Instruction to search * @param locate_filter The filter to indicate if an instruction should be considered * @param process_child_composites Indicate if child Composite Instructions should be searched @@ -57,6 +59,7 @@ Instruction* getFirstInstruction(CompositeInstruction& composite_instruction, /** * @brief Get the last Instruction in a Composite Instruction that is identified by the filter + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction Composite Instruction to search * @param locate_filter The filter to indicate if an instruction should be considered * @param process_child_composites Indicate if child Composite Instructions should be searched @@ -68,6 +71,7 @@ const Instruction* getLastInstruction(const CompositeInstruction& composite_inst /** * @brief Get the last Instruction in a Composite Instruction that is identified by the filter + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction Composite Instruction to search * @param locate_filter The filter to indicate if an instruction should be considered * @param process_child_composites Indicate if child Composite Instructions should be searched @@ -79,7 +83,7 @@ Instruction* getLastInstruction(CompositeInstruction& composite_instruction, /** * @brief Get the first Move Instruction in a Composite Instruction - * This does not consider the start instruction in child composite instruction + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction Composite Instruction to search * @return The first Move Instruction (Non-Const) */ @@ -94,7 +98,7 @@ inline MoveInstruction* getFirstMoveInstruction(CompositeInstruction& composite_ /** * @brief Get the first Move Instruction in a Composite Instruction - * This does not consider the start instruction in child composite instruction + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction Composite Instruction to search * @return The first Move Instruction (Const) */ @@ -109,7 +113,7 @@ inline const MoveInstruction* getFirstMoveInstruction(const CompositeInstruction /** * @brief Get the first Plan Instruction in a Composite Instruction - * This does not consider the start instruction in child composite instruction + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction Composite Instruction to search * @return The first Plan Instruction (Non-Const) */ @@ -124,7 +128,7 @@ inline PlanInstruction* getFirstPlanInstruction(CompositeInstruction& composite_ /** * @brief Get the first Plan Instruction in a Composite Instruction - * This does not consider the start instruction in child composite instruction + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction Composite Instruction to search * @return The first Plan Instruction (Const) */ @@ -139,7 +143,7 @@ inline const PlanInstruction* getFirstPlanInstruction(const CompositeInstruction /** * @brief Get the last Move Instruction in a Composite Instruction - * This does not consider the start instruction in child composite instruction + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction Composite Instruction to search * @return The last Move Instruction (Non-Const) */ @@ -154,7 +158,7 @@ inline MoveInstruction* getLastMoveInstruction(CompositeInstruction& composite_i /** * @brief Get the last Move Instruction in a Composite Instruction - * This does not consider the start instruction in child composite instruction + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction Composite Instruction to search * @return The last Move Instruction (Const) */ @@ -169,7 +173,7 @@ inline const MoveInstruction* getLastMoveInstruction(const CompositeInstruction& /** * @brief Get the last Plan Instruction in a Composite Instruction - * This does not consider the start instruction in child composite instruction + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction Composite Instruction to search * @return The last Plan Instruction (Non-Const) */ @@ -184,7 +188,7 @@ inline PlanInstruction* getLastPlanInstruction(CompositeInstruction& composite_i /** * @brief Get the last Plan Instruction in a Composite Instruction - * This does not consider the start instruction in child composite instruction + * @detials This does not consider the start instruction of nested composite instructions * @param composite_instruction Composite Instruction to search * @return The last Plan Instruction (Const) */ @@ -199,6 +203,7 @@ inline const PlanInstruction* getLastPlanInstruction(const CompositeInstruction& /** * @brief Get number of Instruction in a Composite Instruction + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction The Composite Instruction to process * @param locate_filter The filter to indicate if an instruction should be considered * @param process_child_composites Indicate if child Composite Instructions should be searched @@ -210,7 +215,7 @@ long getInstructionCount(const CompositeInstruction& composite_instruction, /** * @brief Get number of Move Instruction in a Composite Instruction - * This does not consider the start instruction in the child composite instruction + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction The Composite Instruction to process * @return The number of Move Instructions */ @@ -221,7 +226,7 @@ inline long getMoveInstructionCount(const CompositeInstruction& composite_instru /** * @brief Get number of Plan Instruction in a Composite Instruction - * This does not consider the start instruction in the child composite instruction + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction The Composite Instruction to process * @return The number of Plan Instructions */ diff --git a/tesseract_command_language/src/composite_instruction.cpp b/tesseract_command_language/src/composite_instruction.cpp index 0dc9b05233a..58de78011a1 100644 --- a/tesseract_command_language/src/composite_instruction.cpp +++ b/tesseract_command_language/src/composite_instruction.cpp @@ -166,6 +166,10 @@ CompositeInstruction::iterator CompositeInstruction::insert(const_iterator p, st { return container_.insert(p, l); } +CompositeInstruction::iterator CompositeInstruction::insert(const_iterator p, std::size_t n, const value_type& x) +{ + return container_.insert(p, n, x); +} template CompositeInstruction::iterator CompositeInstruction::emplace(const_iterator pos, Args&&... args) diff --git a/tesseract_command_language/src/utils/flatten_utils.cpp b/tesseract_command_language/src/utils/flatten_utils.cpp index 80e91c796e7..35a81652ab8 100644 --- a/tesseract_command_language/src/utils/flatten_utils.cpp +++ b/tesseract_command_language/src/utils/flatten_utils.cpp @@ -49,7 +49,7 @@ void flattenHelper(std::vector>& flattened, const flattenFilterFn& filter, bool first_composite) { - if (composite.hasStartInstruction()) + if (composite.hasStartInstruction() && first_composite) if (!filter || filter(composite.getStartInstruction(), composite, first_composite)) flattened.emplace_back(composite.getStartInstruction()); @@ -92,7 +92,7 @@ void flattenHelper(std::vector>& flatt const flattenFilterFn& filter, bool first_composite) { - if (composite.hasStartInstruction()) + if (composite.hasStartInstruction() && first_composite) if (!filter || filter(composite.getStartInstruction(), composite, first_composite)) flattened.emplace_back(composite.getStartInstruction()); @@ -143,7 +143,7 @@ void flattenToPatternHelper(std::vector>& fl return; } - if (composite.hasStartInstruction()) + if (composite.hasStartInstruction() && first_composite) if (!filter || filter(composite.getStartInstruction(), composite, first_composite)) flattened.emplace_back(composite.getStartInstruction()); @@ -196,7 +196,7 @@ void flattenToPatternHelper(std::vector names(static_cast(dof), "joint"); + + CompositeInstruction composite; + for (Eigen::Index i = 0; i < 3; ++i) + { + CompositeInstruction child; + for (Eigen::Index j = 0; j < 10; ++j) + { + if (j == 0) + { + child.setStartInstruction( + MoveInstruction(JointWaypoint(names, Eigen::VectorXd::Zero(dof)), MoveInstructionType::START)); + } + else + { + child.push_back(MoveInstruction(JointWaypoint(names, Eigen::VectorXd::Zero(dof)), MoveInstructionType::LINEAR)); + } + } + composite.push_back(child); + } + + // Add an empty composite with a start instruction to the end of the top-level composite + { + CompositeInstruction child; + child.setStartInstruction( + MoveInstruction(JointWaypoint(names, Eigen::VectorXd::Zero(dof)), MoveInstructionType::START)); + composite.push_back(child); + } + + // The first move instruction should be the first instruction of the first nested composite (the top level composite + // instruction does not have a start instruction yet) + ASSERT_EQ(getFirstInstruction(composite, &moveFilter), &composite.front().as().front()); + + // Add a move instruction to the top-level composite + composite.setStartInstruction( + MoveInstruction(JointWaypoint(names, Eigen::VectorXd::Zero(dof)), MoveInstructionType::START)); + // The first move instruction should now be the start instruction of the top-level composite + ASSERT_EQ(getFirstInstruction(composite, &moveFilter), &composite.getStartInstruction()); + + // The last move instruction should be the last instruction of the second to last composite instruction since the last + // nested composite is empty except for a start instruction + ASSERT_EQ(getLastInstruction(composite, &moveFilter), &composite.at(2).as().back()); + + // Add an instruction to the last nested composite instruction + composite.back().as().push_back( + MoveInstruction(JointWaypoint(names, Eigen::VectorXd::Zero(dof)), MoveInstructionType::LINEAR)); + // The last move instruction should now be the last instruction of the last nested composite + ASSERT_EQ(getLastInstruction(composite, &moveFilter), &composite.back().as().back()); + + // The total count of move instructions, excluding start instructions of nested composite instructions, should be: + // - the number of original nested composites times the number of waypoints per composite (minus one for the one + // used in the start + // instruction) + // - plus one for an initial start instruction in the top-level composite + // - plus one instruction added in the initially empty last nested composite + ASSERT_EQ(getInstructionCount(composite, &moveFilter), 3 * (10 - 1) + 2); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/tesseract_motion_planners/core/src/simple/profile/simple_planner_utils.cpp b/tesseract_motion_planners/core/src/simple/profile/simple_planner_utils.cpp index 77fce613e3d..d4b0b951961 100644 --- a/tesseract_motion_planners/core/src/simple/profile/simple_planner_utils.cpp +++ b/tesseract_motion_planners/core/src/simple/profile/simple_planner_utils.cpp @@ -163,15 +163,20 @@ CompositeInstruction getInterpolatedComposite(const std::vector& jo // Get move type base on base instruction type MoveInstructionType move_type = getMoveInstructionType(base_instruction); + auto create_move_inst = [&](const Eigen::Index idx) -> MoveInstruction { + MoveInstruction inst(StateWaypoint(joint_names, states.col(idx)), move_type); + inst.setManipulatorInfo(base_instruction.getManipulatorInfo()); + inst.setDescription(base_instruction.getDescription()); + inst.setProfile(base_instruction.getProfile()); + inst.profile_overrides = base_instruction.profile_overrides; + return inst; + }; + // Convert to MoveInstructions + composite.setStartInstruction(create_move_inst(0)); for (long i = 1; i < states.cols(); ++i) { - MoveInstruction move_instruction(StateWaypoint(joint_names, states.col(i)), move_type); - move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo()); - move_instruction.setDescription(base_instruction.getDescription()); - move_instruction.setProfile(base_instruction.getProfile()); - move_instruction.profile_overrides = base_instruction.profile_overrides; - composite.push_back(move_instruction); + composite.push_back(create_move_inst(i)); } return composite; diff --git a/tesseract_motion_planners/examples/freespace_example.cpp b/tesseract_motion_planners/examples/freespace_example.cpp index 7490c45c6ba..7a0ee6208d2 100644 --- a/tesseract_motion_planners/examples/freespace_example.cpp +++ b/tesseract_motion_planners/examples/freespace_example.cpp @@ -29,28 +29,28 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include - #include - #include - -#include +#include +#include +#include +#include +#include +// OMPL #include - +#include +#include +#include +// TrajOpt #include #include #include -#include -#include -#include - -#include -#include - using namespace tesseract_planning; using namespace tesseract_planning::profile_ns; +static const std::string PROFILE_NAME = "DEFAULT"; + std::string locateResource(const std::string& url) { std::string mod_url = url; @@ -78,7 +78,7 @@ std::string locateResource(const std::string& url) return mod_url; } -int main(int /*argc*/, char** /*argv*/) +int main(int argc, char** argv) { try { @@ -89,9 +89,10 @@ int main(int /*argc*/, char** /*argv*/) tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/abb_irb2400.srdf"); env->init(urdf_path, srdf_path, locator); - // Dynamically load ignition visualizer if exist + // Dynamically load ignition visualizer if it exists tesseract_visualization::VisualizationLoader loader; - auto plotter = loader.get(); + const std::string plugin_name = argc < 2 ? "" : argv[1]; + auto plotter = loader.get(plugin_name); if (plotter != nullptr) { @@ -117,34 +118,26 @@ int main(int /*argc*/, char** /*argv*/) Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -.20, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0); // Define Plan Instructions - PlanInstruction start_instruction(wp0, PlanInstructionType::START); - PlanInstruction plan_f1(wp1, PlanInstructionType::FREESPACE, "DEFAULT"); + PlanInstruction start_instruction(wp0, PlanInstructionType::START, PROFILE_NAME, manip); + PlanInstruction plan_f1(wp1, PlanInstructionType::FREESPACE, PROFILE_NAME, manip); + PlanInstruction plan_f2(wp0, PlanInstructionType::FREESPACE, PROFILE_NAME, manip); // Create program CompositeInstruction program; program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); program.push_back(plan_f1); + program.push_back(plan_f2); - // Plot Program - if (plotter) - { - } - - // Create Profiles - auto ompl_plan_profile = std::make_shared(); - auto trajopt_plan_profile = std::make_shared(); - auto trajopt_composite_profile = std::make_shared(); + // Profile Dictionary + auto profiles = std::make_shared(); + profiles->planner_profiles[OMPL_DEFAULT_NAMESPACE][PROFILE_NAME] = std::make_shared(); + profiles->composite_profiles[OMPL_DEFAULT_NAMESPACE][PROFILE_NAME] = std::make_shared(); + profiles->waypoint_profiles[OMPL_DEFAULT_NAMESPACE][PROFILE_NAME] = std::make_shared(); // Create a seed CompositeInstruction seed = generateSeed(program, cur_state, env); - // Profile Dictionary - auto profiles = std::make_shared(); - profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "DEFAULT", ompl_plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); - // Create Planning Request PlannerRequest request; request.seed = seed; @@ -160,19 +153,27 @@ int main(int /*argc*/, char** /*argv*/) assert(ompl_status); // Plot OMPL Trajectory - if (plotter) + if (plotter != nullptr) { plotter->waitForInput(); plotter->plotTrajectory(toJointTrajectory(ompl_response.results), *state_solver); } - // Update Seed + // Create the TrajOpt profiles + auto trajopt_plan_profile = std::make_shared(); + auto trajopt_composite_profile = std::make_shared(); + + // Add the TrajOpt profiles to the dictionary + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, PROFILE_NAME, trajopt_plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, PROFILE_NAME, trajopt_composite_profile); + + // Update the seed to be the OMPL trajecory request.seed = ompl_response.results; // Solve TrajOpt Plan PlannerResponse trajopt_response; TrajOptMotionPlanner trajopt_planner; - auto trajopt_status = trajopt_planner.solve(request, trajopt_response); + auto trajopt_status = trajopt_planner.solve(request, trajopt_response, true); assert(trajopt_status); if (plotter) diff --git a/tesseract_motion_planners/ompl/CMakeLists.txt b/tesseract_motion_planners/ompl/CMakeLists.txt index 4dd3dbd52f8..81e011d48e1 100644 --- a/tesseract_motion_planners/ompl/CMakeLists.txt +++ b/tesseract_motion_planners/ompl/CMakeLists.txt @@ -4,19 +4,20 @@ link_directories(BEFORE ${OMPL_LIBRARY_DIRS}) # OMPL Freespace Planning Interface set(OMPL_SRC - src/ompl_motion_planner.cpp + # OMPL Interfaces src/continuous_motion_validator.cpp src/discrete_motion_validator.cpp + src/state_collision_validator.cpp + src/compound_state_validator.cpp src/weighted_real_vector_state_sampler.cpp - src/ompl_motion_planner_status_category.cpp + # Motion planner src/ompl_planner_configurator.cpp - src/ompl_problem.cpp - src/profile/ompl_default_plan_profile.cpp + src/ompl_motion_planner_status_category.cpp + src/ompl_motion_planner.cpp src/utils.cpp - src/state_collision_validator.cpp - src/compound_state_validator.cpp - src/serialize.cpp - src/deserialize.cpp) + # Profiles + src/profile/ompl_composite_profile_rvss.cpp + src/profile/ompl_waypoint_profile.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 cf7540c3468..00000000000 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/deserialize.h +++ /dev/null @@ -1,48 +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 -#include - -namespace tesseract_planning -{ -OMPLDefaultPlanProfile omplPlanParser(const tinyxml2::XMLElement& xml_element); - -OMPLDefaultPlanProfile omplPlanFromXMLElement(const tinyxml2::XMLElement* profile_xml); - -OMPLDefaultPlanProfile omplPlanFromXMLDocument(const tinyxml2::XMLDocument& xml_doc); - -OMPLDefaultPlanProfile omplPlanFromXMLFile(const std::string& file_path); - -OMPLDefaultPlanProfile omplPlanFromXMLString(const std::string& xml_string); - -} // namespace tesseract_planning - -#endif // TESSERACT_MOTION_PLANNERS_OMPL_DESERIALIZE_H diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h index 148da795c09..8efed26a6c6 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 @@ -29,27 +29,58 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include #include -#include +#include #ifdef SWIG %shared_ptr(tesseract_planning::OMPLMotionPlanner) #endif // SWIG -namespace ompl +namespace tesseract_planning { -namespace tools +/** @brief Configuration parameters for the Tesseract OMPL planner */ +struct OMPLPlannerParameters { -class ParallelPlan; -} -} // namespace ompl + /** @brief Max planning time allowed in seconds */ + double planning_time = 5.0; -namespace tesseract_planning -{ -class OMPLMotionPlannerStatusCategory; + /** @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 to the minimum required number of states. + * @details Note: trajectory simplification can require a non-trivial amount of time to perform + */ + bool simplify = true; + + /** + * @brief Use all available planning time to create the most optimized trajectory given the objective function. + * @details 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 = false; + + /** @brief OMPL planning factories for creating OMPL planners to solve the planning problem */ + std::vector planners = { std::make_shared() }; +}; + +/** @brief Function signature for extracting a Tesseract state representations from an OMPL state representation */ +using OMPLStateExtractor = std::function(const ompl::base::State*)>; + +/** + * Structure for the output data of an OMPL composite instruction + */ +using OMPLCompositeProfileData = std::tuple; /** * @brief This planner is intended to provide an easy to use interface to OMPL for freespace planning. It is made to @@ -94,17 +125,9 @@ class OMPLMotionPlanner : public MotionPlanner static bool checkUserInput(const PlannerRequest& request); - virtual std::vector createProblems(const PlannerRequest& request) const; - protected: /** @brief Name of planner */ std::string name_; - - /** @brief The planners status codes */ - std::shared_ptr status_category_; - - /** @brief OMPL Parallel planner */ - std::shared_ptr parallel_plan_; }; } // namespace tesseract_planning diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_problem.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_problem.h deleted file mode 100644 index 714a509d33f..00000000000 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_problem.h +++ /dev/null @@ -1,154 +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 -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include -#include -#include - -#ifdef SWIG -%shared_ptr(tesseract_planning::OMPLProblem) -%ignore tesseract_planning::OMPLProblem::extractor; -#endif // SWIG - -namespace tesseract_planning -{ -struct OMPLProblem; - -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; - - // These are required for Tesseract to configure ompl - tesseract_environment::Environment::ConstPtr env; - tesseract_scene_graph::SceneState env_state; - - // This is used to verify that start and goal states are not in collision - tesseract_collision::DiscreteContactManager::Ptr contact_checker; - - // Problem Configuration - OMPLProblemStateSpace state_space{ OMPLProblemStateSpace::REAL_STATE_SPACE }; - - // Kinematic Objects - tesseract_kinematics::JointGroup::ConstPtr 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 - -#ifdef SWIG -%template(OMPLProblems) std::vector>; -#endif - -#endif // TESSERACT_MOTION_PLANNERS_OMPL_OMPL_PROBLEM_H diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_composite_profile_rvss.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_composite_profile_rvss.h new file mode 100644 index 00000000000..021a7066373 --- /dev/null +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_composite_profile_rvss.h @@ -0,0 +1,92 @@ +#ifndef TESSERACT_MOTION_PLANNERS_OMPL_PROFILE_OMPL_COMPOSITE_PROFILE_RVSS_H +#define TESSERACT_MOTION_PLANNERS_OMPL_PROFILE_OMPL_COMPOSITE_PROFILE_RVSS_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include + +namespace tesseract_planning +{ +using StateValidityCheckerAllocator = + std::function; + +using MotionValidatorAllocator = std::function; + +using OptimizationObjectiveAllocator = + std::function; + +/** + * @brief OMPL composite profile for real-vector state spaces + */ +class OMPLCompositeProfileRVSS : public CompositeProfile +{ +public: + /** + * @brief The collision check configuration + * @details Different validators will be created depending on the type of collision checking requested + * - Discrete: + * - Discrete collision state validator, discrete motion validator + * - Continuous: + * - Continuous motion validator + * - None: + * - No additional validators will be created + */ + tesseract_collision::CollisionCheckConfig collision_check_config; + + /** @brief The state sampler allocator. This can be null and it will use Tesseract default state sampler allocator. */ + ompl::base::StateSamplerAllocator state_sampler_allocator{ nullptr }; + + /** + * @brief The ompl state validity checker + * @details If nullptr and collision checking enabled it uses StateCollisionValidator + */ + StateValidityCheckerAllocator state_validator_allocator{ nullptr }; + + /** + * @brief The ompl motion validator. + * @details If nullptr and continuous collision checking enabled it used ContinuousMotionValidator + */ + MotionValidatorAllocator motion_validator_allocator{ nullptr }; + + /** + * @brief Function for allocating the optimization objective + * @details Default is the minimization of path length + */ + OptimizationObjectiveAllocator optimization_objective_allocator = [](const ompl::base::SpaceInformationPtr& si) { + return std::make_shared(si); + }; + + /** + * @brief Creates the OMPL composite profile data given a composite instruction and environment + */ + std::any create(const CompositeInstruction& instruction, + const tesseract_environment::Environment& env) const override; + + /** + * @brief Seed for OMPL random number generator + * @details When this value is greater than or equal to zero, the OMPL random number generator will produce a + * determinstic sequence of random samples + */ + long rng_seed = -1; + +private: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int) + { + } +}; + +} // namespace tesseract_planning + +#endif // TESSERACT_MOTION_PLANNERS_OMPL_PROFILE_OMPL_COMPOSITE_PROFILE_RVSS_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 7250905a943..00000000000 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h +++ /dev/null @@ -1,161 +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 - -#ifdef SWIG -%shared_ptr(tesseract_planning::OMPLDefaultPlanProfile) -%ignore tesseract_planning::OMPLDefaultPlanProfile::allocWeightedRealVectorStateSampler; -#endif // SWIG - -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 OMPLDefaultPlanProfile : public OMPLPlanProfile -{ -public: - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - - OMPLDefaultPlanProfile() = default; - ~OMPLDefaultPlanProfile() override = default; - OMPLDefaultPlanProfile(const OMPLDefaultPlanProfile&) = default; - OMPLDefaultPlanProfile& operator=(const OMPLDefaultPlanProfile&) = default; - OMPLDefaultPlanProfile(OMPLDefaultPlanProfile&&) noexcept = default; - OMPLDefaultPlanProfile& operator=(OMPLDefaultPlanProfile&&) noexcept = default; - 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 = { std::make_shared(), - std::make_shared() }; - - /** @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 Instruction& parent_instruction, - const ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const override; - - void applyGoalStates(OMPLProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const Instruction& parent_instruction, - const ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const override; - - void applyStartStates(OMPLProblem& prob, - const Eigen::Isometry3d& cartesian_waypoint, - const Instruction& parent_instruction, - const ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const override; - - void applyStartStates(OMPLProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const Instruction& parent_instruction, - const 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; -}; -} // namespace tesseract_planning -#endif // TESSERACT_MOTION_PLANNERS_OMPL_OMPL_DEFAULT_PLAN_PROFILE_H diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_planner_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_planner_profile.h new file mode 100644 index 00000000000..5cbd6b8ec06 --- /dev/null +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_planner_profile.h @@ -0,0 +1,33 @@ +#ifndef TESSERACT_MOTION_PLANNERS_OMPL_PROFILE_OMPL_PLANNER_PROFILE_H +#define TESSERACT_MOTION_PLANNERS_OMPL_PROFILE_OMPL_PLANNER_PROFILE_H + +#include +#include + +namespace tesseract_planning +{ +/** + * @brief Planner profile that produces the high level parameters for the OMPL planner + */ +class OMPLPlannerProfile : public PlannerProfile +{ +public: + OMPLPlannerParameters params; + inline std::any create() const override + { + if (params.planners.empty()) + throw std::runtime_error("No OMPL planner factories defined"); + return params; + }; + +private: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int) + { + } +}; + +} // namespace tesseract_planning + +#endif // TESSERACT_MOTION_PLANNERS_OMPL_PROFILE_OMPL_PLANNER_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 deleted file mode 100644 index 277bd0fe10d..00000000000 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h +++ /dev/null @@ -1,104 +0,0 @@ -/** - * @file ompl_profile.h - * @brief Tesseract OMPL 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_PROFILE_H -#define TESSERACT_MOTION_PLANNERS_OMPL_OMPL_PROFILE_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include -#include - -#ifdef SWIG -%shared_ptr(tesseract_planning::OMPLPlanProfile) -#endif // SWIG - -namespace tesseract_planning -{ -class OMPLPlanProfile -{ -public: - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - - OMPLPlanProfile() = default; - virtual ~OMPLPlanProfile() = default; - OMPLPlanProfile(const OMPLPlanProfile&) = default; - OMPLPlanProfile& operator=(const OMPLPlanProfile&) = default; - OMPLPlanProfile(OMPLPlanProfile&&) noexcept = default; - OMPLPlanProfile& operator=(OMPLPlanProfile&&) noexcept = default; - - OMPLPlanProfile(const tinyxml2::XMLElement& xml_element); - - virtual void setup(OMPLProblem& prob) const = 0; - - virtual void applyGoalStates(OMPLProblem& prob, - const Eigen::Isometry3d& cartesian_waypoint, - const Instruction& parent_instruction, - const ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const = 0; - - virtual void applyGoalStates(OMPLProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const Instruction& parent_instruction, - const ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const = 0; - - virtual void applyStartStates(OMPLProblem& prob, - const Eigen::Isometry3d& cartesian_waypoint, - const Instruction& parent_instruction, - const ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const = 0; - - virtual void applyStartStates(OMPLProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const Instruction& parent_instruction, - const ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const = 0; - - virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; -}; - -using OMPLPlanProfileMap = std::unordered_map; - -/** @todo Currently OMPL does not have support of composite profile everything is handled by the plan profile */ - -} // namespace tesseract_planning - -#ifdef SWIG -%template(OMPLPlanProfileMap) std::unordered_map>; -%tesseract_command_language_add_profile_type(OMPLPlanProfile); -#endif // SWIG - -#endif // TESSERACT_MOTION_PLANNERS_OMPL_OMPL_PROFILE_H diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_waypoint_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_waypoint_profile.h new file mode 100644 index 00000000000..73f56d768fb --- /dev/null +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_waypoint_profile.h @@ -0,0 +1,23 @@ +#ifndef TESSERACT_PLANNING_OMPL_PROFILE_OMPL_WAYPOINT_PROFILE_H +#define TESSERACT_PLANNING_OMPL_PROFILE_OMPL_WAYPOINT_PROFILE_H + +#include + +namespace tesseract_planning +{ +class OMPLWaypointProfile : public WaypointProfile +{ +public: + std::any create(const Instruction& instruction, const tesseract_environment::Environment& env) const override; + +private: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int) + { + } +}; + +} // namespace tesseract_planning + +#endif // TESSERACT_PLANNING_OMPL_PROFILE_OMPL_WAYPOINT_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 5d087bbc17a..00000000000 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/serialize.h +++ /dev/null @@ -1,44 +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 -#include - -namespace tesseract_planning -{ -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/types.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/types.h deleted file mode 100644 index 5c463067140..00000000000 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/types.h +++ /dev/null @@ -1,43 +0,0 @@ -/** - * @file types.h - * @brief Tesseract OMPL types - * - * @author Levi Armstrong - * @date June 22, 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_TYPES_H -#define TESSERACT_MOTION_PLANNERS_OMPL_TYPES_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include - -namespace tesseract_planning -{ -using OMPLStateExtractor = std::function(const ompl::base::State*)>; -} -#endif // TESSERACT_MOTION_PLANNERS_OMPL_TYPES_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 0794396bd7a..6278d9a9868 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 @@ -34,8 +34,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include -#include +#include namespace tesseract_planning { @@ -72,35 +71,6 @@ void processLongestValidSegment(const ompl::base::StateSpacePtr& state_space_ptr void processLongestValidSegment(const ompl::base::StateSpacePtr& state_space_ptr, const tesseract_collision::CollisionCheckConfig& collision_check_config); -/** - * @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 - * @return True if in collision otherwise false - */ -bool checkStateInCollision(OMPLProblem& prob, - const Eigen::VectorXd& state, - tesseract_collision::ContactResultMap& contact_map); - -/** - * @brief For the provided problem check if the state is in collision - * @param prob The OMPL Problem - * @param state The joint state - * @return True if in collision otherwise false - */ -bool checkStateInCollision(OMPLProblem& prob, const Eigen::VectorXd& state); - -/** - * @brief Default State sampler which uses the weights information to scale the sampled state. This is use full - * when you state space has mixed units like meters and radian. - * @param space The ompl state space. - * @return OMPL state sampler shared pointer - */ -ompl::base::StateSamplerPtr allocWeightedRealVectorStateSampler(const ompl::base::StateSpace* space, - const Eigen::VectorXd& weights, - const Eigen::MatrixX2d& limits); - } // namespace tesseract_planning #endif // TESSERACT_MOTION_PLANNERS_OMPL_UTILS_H diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/weighted_real_vector_state_sampler.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/weighted_real_vector_state_sampler.h index 80a43789160..18556f32399 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/weighted_real_vector_state_sampler.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/weighted_real_vector_state_sampler.h @@ -48,9 +48,18 @@ namespace tesseract_planning class WeightedRealVectorStateSampler : public ompl::base::StateSampler { public: + /** + * @brief Constructor + * @param space + * @param weights + * @param bounds + * @param rng_seed Seed for the OMPL random number generator to produce deterministic sampling. The seed is only set + * if the input value is greater than greater than or equal to zero + */ WeightedRealVectorStateSampler(const ompl::base::StateSpace* space, const Eigen::Ref& weights, - const Eigen::Ref& bounds); + const Eigen::Ref& bounds, + long rng_seed = -1); void sampleUniform(ompl::base::State* state) override; diff --git a/tesseract_motion_planners/ompl/src/deserialize.cpp b/tesseract_motion_planners/ompl/src/deserialize.cpp deleted file mode 100644 index fde78fad744..00000000000 --- a/tesseract_motion_planners/ompl/src/deserialize.cpp +++ /dev/null @@ -1,126 +0,0 @@ -/** - * @file deserialize.cpp - * @brief Provide methods for deserialize instructions to xml and deserialization - * - * @author Tyler Marr - * @date August 21, 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -#include -#include -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; - tinyxml2::XMLError 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 83dd9032b0d..bd6cd12a92b 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -32,18 +32,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include - -#include - -#include #include -#include -#include -#include -#include +#include +#include #include - +#include +#include #include #include @@ -53,7 +47,7 @@ bool checkStartState(const ompl::base::ProblemDefinitionPtr& prob_def, const Eigen::Ref& state, const OMPLStateExtractor& extractor) { - if (!(prob_def->getStartStateCount() >= 1)) + if (prob_def->getStartStateCount() < 1) return false; for (unsigned i = 0; i < prob_def->getStartStateCount(); ++i) @@ -86,449 +80,364 @@ bool checkGoalState(const ompl::base::ProblemDefinitionPtr& prob_def, return false; } -/** @brief Construct a basic planner */ -OMPLMotionPlanner::OMPLMotionPlanner(std::string name) - : name_(std::move(name)), status_category_(std::make_shared(name_)) +std::vector> createOMPLStates(const std::vector& joint_states, + const ompl::base::SpaceInformationPtr& si) { - if (name_.empty()) - throw std::runtime_error("OMPLMotionPlanner name is empty!"); -} + std::vector> states; + states.reserve(joint_states.size()); -const std::string& OMPLMotionPlanner::getName() const { return name_; } + for (const auto& js : joint_states) + { + ompl::base::ScopedState<> state(si->getStateSpace()); + for (unsigned i = 0; i < js.size(); ++i) + state[i] = js[static_cast(i)]; -bool OMPLMotionPlanner::terminate() -{ - CONSOLE_BRIDGE_logWarn("Termination of ongoing optimization is not implemented yet"); - return false; + states.push_back(state); + } + + return states; } -tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& request, - PlannerResponse& response, - bool verbose) const +std::shared_ptr plan(const ompl::geometric::SimpleSetupPtr& simple_setup, + const OMPLPlannerParameters& params, + const unsigned n_output_states) { - if (!checkUserInput(request)) // NOLINT + // Create an OMPL planner that can run multiple OMPL planners in parallel + auto parallel_plan = std::make_shared(simple_setup->getProblemDefinition()); + + // Add all the specified OMPL planners to the parallel planner + std::vector planners; + planners.reserve(params.planners.size()); + for (const auto& factory : params.planners) { - response.status = - tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::ErrorInvalidInput, status_category_); - return response.status; + ompl::base::PlannerPtr planner = factory->create(simple_setup->getSpaceInformation()); + + // // TODO: construct planner from the saved planner data (only valid for multi-query planners, such as PRM) + // if (request.data != nullptr) + // { + // auto planner_data = std::static_pointer_cast(request.data); + // planner = factory->create(*planner_data); + // if (!planner) + // { + // CONSOLE_BRIDGE_logWarn("Unable to construct planner with saved PlannerData; constructing planner from + // scratch instead"); planner = factory->create(simple_setup->getSpaceInformation()); + // } + // } + // else + // { + // planner = factory->create(simple_setup->getSpaceInformation()); + // } + + // Add the planner to the parallel planner object and save a pointer to it + parallel_plan->addPlanner(planner); + planners.push_back(planner); } - std::vector problem; - if (request.data) + + ompl::base::PlannerStatus status; + if (!params.optimize) { - problem = *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(params.planning_time, 1, static_cast(params.max_solutions), false); } else { - try - { - problem = createProblems(request); - } - catch (std::exception& e) - { - CONSOLE_BRIDGE_logError("OMPLPlanner failed to generate problem: %s.", e.what()); - response.status = - tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::ErrorInvalidInput, status_category_); - return response.status; - } - - response.data = std::make_shared>(problem); - } - - // If the verbose set the log level to debug. - if (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& p : problem) - { - 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(params.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(params.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 (!pdef->hasOptimizationObjective()) { - if (status != ompl::base::PlannerStatus::EXACT_SOLUTION) - status = localResult; - - if (!pdef->hasOptimizationObjective()) - { - CONSOLE_BRIDGE_logDebug("Terminating early since there is no optimization objective specified"); - break; - } - - 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)) - { - CONSOLE_BRIDGE_logDebug("Terminating early since solution path satisfies the optimization objective"); - break; - } - - if (pdef->getSolutionCount() >= static_cast(p->max_solutions)) - { - CONSOLE_BRIDGE_logDebug("Terminating early since %u solutions were generated", p->max_solutions); - break; - } + CONSOLE_BRIDGE_logDebug("Terminating early since there is no optimization objective specified"); + break; } - } - } - if (status != ompl::base::PlannerStatus::EXACT_SOLUTION) - { - response.status = tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::ErrorFailedToFindValidSolution, - status_category_); - return response.status; - } + ompl::base::Cost obj_cost = pdef->getSolutionPath()->cost(pdef->getOptimizationObjective()); + CONSOLE_BRIDGE_logDebug("Motion Objective Cost: %f", obj_cost.value()); - if (p->simplify) - { - p->simple_setup->simplifySolution(); - } - 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); + if (pdef->getOptimizationObjective()->isSatisfied(obj_cost)) + { + CONSOLE_BRIDGE_logDebug("Terminating early since solution path satisfies the optimization objective"); + break; + } + + if (pdef->getSolutionCount() >= static_cast(params.max_solutions)) + { + CONSOLE_BRIDGE_logDebug("Terminating early since %u solutions were generated", params.max_solutions); + break; + } } } } - // Flatten the results to make them easier to process - response.results = request.seed; - std::vector> results_flattened = - flattenProgramToPattern(response.results, request.instructions); - std::vector> instructions_flattened = flattenProgram(request.instructions); + // Check the planner status + if (status != ompl::base::PlannerStatus::EXACT_SOLUTION) + throw std::runtime_error("Planning failed to find exact solution"); - std::size_t instructions_idx = 0; // Index for each input instruction - - // Handle the start instruction - const auto& plan_instruction = instructions_flattened.at(0).get().as(); - if (plan_instruction.isStart()) + // The trajectory must have at least as many states as the seed trajectory + if (n_output_states > 2) { - const auto& p = problem[0]; - - // Get the results - tesseract_common::TrajArray trajectory = p->getTrajectory(); + const std::size_t n_states = simple_setup->getSolutionPath().getStateCount(); - // Enforce limits - for (Eigen::Index i = 0; i < trajectory.rows(); i++) - tesseract_common::enforcePositionLimits(trajectory.row(i), p->manip->getLimits().joint_limits); - - assert(checkStartState(p->simple_setup->getProblemDefinition(), trajectory.row(0), p->extractor)); - assert(checkGoalState(p->simple_setup->getProblemDefinition(), trajectory.bottomRows(1).transpose(), p->extractor)); - - // Copy the start instruction - assert(instructions_idx == 0); - assert(isMoveInstruction(results_flattened[0].get())); - auto& move_instruction = results_flattened[0].get().as(); - move_instruction.getWaypoint().as().position = trajectory.row(0); - instructions_idx++; - } - - // Loop over remaining instructions - std::size_t prob_idx = 0; - for (; instructions_idx < instructions_flattened.size(); instructions_idx++) - { - if (isPlanInstruction(instructions_flattened.at(instructions_idx).get())) + if (n_states < n_output_states) { - const auto& p = problem[prob_idx]; - - // Get the results - tesseract_common::TrajArray trajectory = p->getTrajectory(); - - assert(checkStartState(p->simple_setup->getProblemDefinition(), trajectory.row(0), p->extractor)); - assert( - checkGoalState(p->simple_setup->getProblemDefinition(), trajectory.bottomRows(1).transpose(), p->extractor)); - - // Loop over the flattened results and add them to response if the input was a plan instruction - auto& move_instructions = results_flattened[instructions_idx].get().as(); - // Adjust result index to align final point since start instruction is already handled - Eigen::Index result_index = trajectory.rows() - static_cast(move_instructions.size()); - for (auto& instruction : move_instructions) - instruction.as().getWaypoint().as().position = trajectory.row(result_index++); + // Upsample the number of states to match the length of the seed + simple_setup->getSolutionPath().interpolate(n_output_states); + } + else if (n_states > n_output_states && params.simplify) + { + // Simplify the solution + simple_setup->simplifySolution(); - // Increment the problem - prob_idx++; + // Upsample the number of states to match the length of the seed if required + if (simple_setup->getSolutionPath().getStateCount() < n_output_states) + simple_setup->getSolutionPath().interpolate(n_output_states); + } + else + { + // Trajectory length matches or exceeds seed length and simplification is disabled, so no need to simplify or + // interpolate } } - response.status = tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::SolutionFound, status_category_); - return response.status; -} + // Extract the planner data from each planner and concatenate + // TODO: Investigate whether it is problematic to concatenate planner data from multiple different planners. The + // getPlannerData function handles new vertex and edge indices correctly, but vertices that are close enough in the + // space to be considered equivalent are not merged or connected. Also different planners add edges in different ways + // and set different vertex/edge properties, so it may not make sense to combine them + auto planner_data = std::make_shared(simple_setup->getSpaceInformation()); + for (const ompl::base::PlannerPtr& planner : planners) + planner->getPlannerData(*planner_data); -void OMPLMotionPlanner::clear() { parallel_plan_ = nullptr; } + // The planning data is actually owned by the planner instances themselves. Deep copy the planning information by + // decoupling from the planner instances so the data is not lost when the planner instances go out of scope + planner_data->decoupleFromPlanner(); -MotionPlanner::Ptr OMPLMotionPlanner::clone() const { return std::make_shared(name_); } + return planner_data; +} -bool OMPLMotionPlanner::checkUserInput(const PlannerRequest& request) +CompositeInstruction buildTrajectoryInstruction(const tesseract_common::TrajArray& trajectory, + const CompositeInstruction& seed) { - // Check that parameters are valid - if (request.env == nullptr) + // Initialize the output by copying the input program and flattening it + CompositeInstruction output(seed); + + // The results composite will only have as many states as the seed, but the OMPL trajectory might require more states + // In this case, we need to insert more states into the composite to cover the difference + // Remember the composite does not include the start state, so compare its size with one less than the size of the + // trajectory + if (static_cast(seed.size()) < trajectory.rows() - 1) { - CONSOLE_BRIDGE_logError("In TrajOptPlannerUniversalConfig: env is a required parameter and has not been set"); - return false; + const std::size_t diff = static_cast(trajectory.rows() - 1) - output.size(); + output.insert(output.end(), diff, output.back()); } - if (request.instructions.empty()) + // Overwrite the contents of each copied waypoint + for (Eigen::Index i = 0; i < trajectory.rows(); ++i) { - CONSOLE_BRIDGE_logError("TrajOptPlannerUniversalConfig requires at least one instruction"); - return false; + // The first trajectory state goes into the start instruction + if (i == 0) + { + output.getStartInstruction().as().getWaypoint().as().position = trajectory.row(i); + } + else + { + // Subsequent trajectory states go into the composite instruction + // The index into the composite of these states is one less than the index of the trajectory state since the first + // trajectory state was saved outside the composite + const auto composite_idx = static_cast(i - 1); + auto& move_instruction = output.at(composite_idx).as(); + move_instruction.getWaypoint().as().position = trajectory.row(i); + } } - return true; + return output; } -OMPLProblem::Ptr createOMPLSubProblem(const PlannerRequest& request, - const tesseract_kinematics::JointGroup::ConstPtr& manip) +/** @brief Construct a basic planner */ +OMPLMotionPlanner::OMPLMotionPlanner(std::string name) : name_(std::move(name)) { - auto sub_prob = std::make_unique(); - sub_prob->env = request.env; - sub_prob->env_state = request.env_state; - sub_prob->manip = manip; - sub_prob->contact_checker = request.env->getDiscreteContactManager(); - sub_prob->contact_checker->setCollisionObjectsTransform(request.env_state.link_transforms); - sub_prob->contact_checker->setActiveCollisionObjects(manip->getActiveLinkNames()); - return sub_prob; + if (name_.empty()) + throw std::runtime_error("OMPLMotionPlanner name is empty!"); } -std::vector OMPLMotionPlanner::createProblems(const PlannerRequest& request) const +const std::string& OMPLMotionPlanner::getName() const { return name_; } + +bool OMPLMotionPlanner::terminate() { - std::vector problem; - tesseract_kinematics::JointGroup::Ptr manip; + CONSOLE_BRIDGE_logWarn("Termination of ongoing optimization is not implemented yet"); + return false; +} - // Assume all the plan instructions have the same manipulator as the composite - assert(!request.instructions.getManipulatorInfo().empty()); +void OMPLMotionPlanner::clear() {} - const ManipulatorInfo& composite_mi = request.instructions.getManipulatorInfo(); +tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& request, + PlannerResponse& response, + bool verbose) const +{ + // If the verbose set the log level to debug. + if (verbose) + console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG); - if (composite_mi.manipulator.empty()) - throw std::runtime_error("OMPL, manipulator is empty!"); + auto status_category_ = std::make_shared(name_); try { - tesseract_kinematics::KinematicGroup::Ptr kin_group; - std::string error_msg; - if (composite_mi.manipulator_ik_solver.empty()) - { - kin_group = request.env->getKinematicGroup(composite_mi.manipulator); - error_msg = "Failed to find kinematic group for manipulator '" + composite_mi.manipulator + "'"; - } - else + // Check the format of the request + if (!checkUserInput(request)) // NOLINT + throw std::runtime_error("Input is invalid"); + + // Get the planner profile + OMPLPlannerParameters params; { - 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 + "'"; + const std::string profile_name = + getProfileString(name_, request.instructions.getProfile(), request.plan_profile_remapping); + PlannerProfile::ConstPtr pp = request.profiles->getPlannerProfile(name_, profile_name); + params = std::any_cast(pp->create()); } - if (kin_group == nullptr) + // Get the composite profile + ompl::geometric::SimpleSetupPtr simple_setup; + OMPLStateExtractor extractor; { - CONSOLE_BRIDGE_logError("%s", error_msg.c_str()); - throw std::runtime_error(error_msg); + const std::string profile_name = + getProfileString(name_, request.instructions.getProfile(), request.composite_profile_remapping); + CompositeProfile::ConstPtr cp = request.profiles->getCompositeProfile(name_, profile_name); + std::tie(simple_setup, extractor) = + std::any_cast(cp->create(request.instructions, *request.env)); } - 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); - - std::vector joint_names = manip->getJointNames(); - std::vector active_link_names = manip->getActiveLinkNames(); + // Copy the meta-data from the request instruction into the response and clear any child instructions from the + // response + response.results = request.instructions; + response.results.clear(); - // Check and make sure it does not contain any composite instruction - for (const auto& instruction : request.instructions) - if (isCompositeInstruction(instruction)) - throw std::runtime_error("OMPL planner does not support child composite instructions."); + // Set up the output trajectory to be a composite of composites + response.results.reserve(request.instructions.size()); - int index = 0; - Waypoint start_waypoint{ NullWaypoint() }; - Instruction placeholder_instruction{ NullInstruction() }; - const Instruction* start_instruction = nullptr; - if (request.instructions.hasStartInstruction()) - { - assert(isPlanInstruction(request.instructions.getStartInstruction())); - start_instruction = &(request.instructions.getStartInstruction()); - if (isPlanInstruction(*start_instruction)) - { - const auto& temp = start_instruction->as(); - assert(temp.isStart()); - start_waypoint = temp.getWaypoint(); - } - else + // Loop over each pair of waypoints + for (std::size_t i = 0; i < request.instructions.size(); ++i) { - throw std::runtime_error("OMPL DefaultProblemGenerator: Unsupported start instruction type!"); - } - } - else - { - Eigen::VectorXd current_jv = request.env_state.getJointValues(joint_names); - StateWaypoint swp(joint_names, current_jv); - - MoveInstruction temp_move(swp, MoveInstructionType::START); - placeholder_instruction = temp_move; - start_instruction = &placeholder_instruction; - start_waypoint = swp; - } + simple_setup->clearStartStates(); + simple_setup->clear(); - // Transform plan instructions into ompl problem - for (std::size_t i = 0; i < request.instructions.size(); ++i) - { - const auto& instruction = request.instructions[i]; - if (isPlanInstruction(instruction)) - { - assert(isPlanInstruction(instruction)); - const auto& plan_instruction = instruction.as(); - - assert(isCompositeInstruction(request.seed[i])); - const auto& seed_composite = request.seed[i].as(); - - // Get Plan Profile - std::string profile = plan_instruction.getProfile(); - profile = getProfileString(name_, profile, request.plan_profile_remapping); - auto cur_plan_profile = - getProfile(name_, profile, *request.profiles, std::make_shared()); - cur_plan_profile = applyProfileOverrides(name_, profile, cur_plan_profile, plan_instruction.profile_overrides); - if (!cur_plan_profile) - throw std::runtime_error("OMPLMotionPlannerDefaultConfig: Invalid profile"); - - /** @todo Should check that the joint names match the order of the manipulator */ - OMPLProblem::Ptr sub_prob = createOMPLSubProblem(request, manip); - cur_plan_profile->setup(*sub_prob); - sub_prob->n_output_states = static_cast(seed_composite.size()) + 1; - - if (plan_instruction.isLinear()) + // Add the start state(s) { - /** @todo Add support for linear motion to ompl planner */ - if (isCartesianWaypoint(plan_instruction.getWaypoint()) || isJointWaypoint(plan_instruction.getWaypoint()) || - isStateWaypoint(start_waypoint)) + std::vector start_states; + + // Get the start waypoint profile and add the states to the SimpleSetup + if (i == 0) { - // TODO Currently skipping linear moves until SE3 motion planning is implemented. - problem.push_back(nullptr); - ++index; + const auto& pi = request.instructions.getStartInstruction().as(); + const std::string profile_name = + getProfileString(name_, pi.getProfile(), request.composite_profile_remapping); + WaypointProfile::ConstPtr p = request.profiles->getWaypointProfile(name_, profile_name); + start_states = std::any_cast>(p->create(pi, *request.env)); } else { - throw std::runtime_error("OMPLMotionPlannerDefaultConfig: unknown waypoint type"); + // Use the last state of the previous trajectory as the single start state for this plan + const auto& mi = response.results.back().as().back().as(); + const auto& sw = mi.getWaypoint().as(); + start_states.push_back(sw.position); } + + // Add the states to the SimpleSetup + auto states = createOMPLStates(start_states, simple_setup->getSpaceInformation()); + std::for_each(states.begin(), states.end(), [&simple_setup](const ompl::base::ScopedState<>& state) { + simple_setup->addStartState(state); + }); } - else if (plan_instruction.isFreespace()) + + // Add the goal waypoint(s) { - if (isJointWaypoint(plan_instruction.getWaypoint()) || isStateWaypoint(plan_instruction.getWaypoint())) - { - assert(checkJointPositionFormat(joint_names, plan_instruction.getWaypoint())); - const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction.getWaypoint()); - cur_plan_profile->applyGoalStates( - *sub_prob, cur_position, plan_instruction, composite_mi, active_link_names, index); - - ompl::base::ScopedState<> start_state(sub_prob->simple_setup->getStateSpace()); - if (isJointWaypoint(start_waypoint) || isStateWaypoint(start_waypoint)) - { - assert(checkJointPositionFormat(joint_names, start_waypoint)); - const Eigen::VectorXd& prev_position = getJointPosition(start_waypoint); - cur_plan_profile->applyStartStates( - *sub_prob, prev_position, *start_instruction, composite_mi, active_link_names, index); - } - else if (isCartesianWaypoint(start_waypoint)) - { - const auto& prev_wp = start_waypoint.as(); - cur_plan_profile->applyStartStates( - *sub_prob, prev_wp, *start_instruction, composite_mi, active_link_names, index); - } - else - { - throw std::runtime_error("OMPLMotionPlannerDefaultConfig: unknown waypoint type"); - } - - problem.push_back(std::move(sub_prob)); - ++index; - } - else if (isCartesianWaypoint(plan_instruction.getWaypoint())) - { - const auto& cur_wp = plan_instruction.getWaypoint().as(); - cur_plan_profile->applyGoalStates( - *sub_prob, cur_wp, plan_instruction, composite_mi, active_link_names, index); - - if (index == 0) - { - ompl::base::ScopedState<> start_state(sub_prob->simple_setup->getStateSpace()); - if (isJointWaypoint(start_waypoint) || isStateWaypoint(start_waypoint)) - { - assert(checkJointPositionFormat(joint_names, start_waypoint)); - const Eigen::VectorXd& prev_position = getJointPosition(start_waypoint); - cur_plan_profile->applyStartStates( - *sub_prob, prev_position, *start_instruction, composite_mi, active_link_names, index); - } - else if (isCartesianWaypoint(start_waypoint)) - { - const auto& prev_wp = start_waypoint.as(); - cur_plan_profile->applyStartStates( - *sub_prob, prev_wp, *start_instruction, composite_mi, active_link_names, index); - } - else - { - throw std::runtime_error("OMPLMotionPlannerDefaultConfig: unknown waypoint type"); - } - } - else - { - /** @todo Update. Extract the solution for the previous plan and set as the start */ - assert(false); - } - - problem.push_back(std::move(sub_prob)); - ++index; - } + const auto& pi = request.instructions[i].as(); + + const std::string profile_name = getProfileString(name_, pi.getProfile(), request.composite_profile_remapping); + WaypointProfile::ConstPtr p = request.profiles->getWaypointProfile(name_, profile_name); + + auto states = std::any_cast>(p->create(pi, *request.env)); + auto ompl_states = createOMPLStates(states, simple_setup->getSpaceInformation()); + + auto goal_states = std::make_shared(simple_setup->getSpaceInformation()); + std::for_each(ompl_states.begin(), ompl_states.end(), [&goal_states](const ompl::base::ScopedState<>& state) { + goal_states->addState(state); + }); + + simple_setup->setGoal(goal_states); } - else + + // The number of states in the seed is the size of the composite instruction plus one for the start state + const unsigned n_seed_states = static_cast(request.seed.at(i).as().size()) + 1; + + // Plan + auto planner_data = plan(simple_setup, params, n_seed_states); + + // Save the combined planner data in the response + // response.data = std::static_pointer_cast(planner_data); + + // Get the results + tesseract_common::TrajArray trajectory = toTrajArray(simple_setup->getSolutionPath(), extractor); + assert(checkStartState(simple_setup->getProblemDefinition(), trajectory.row(0), extractor)); + assert(checkGoalState(simple_setup->getProblemDefinition(), trajectory.bottomRows(1).transpose(), extractor)); + + // Enforce limits { - throw std::runtime_error("OMPLMotionPlannerDefaultConfig: Unsupported!"); + const std::string manipulator = request.instructions.getManipulatorInfo().manipulator; + auto joint_limits = request.env->getJointGroup(manipulator)->getLimits().joint_limits; + for (Eigen::Index i = 0; i < trajectory.rows(); i++) + tesseract_common::enforcePositionLimits(trajectory.row(i), joint_limits); } - start_waypoint = plan_instruction.getWaypoint(); /** @todo need to extract the solution for Cartesian waypoints - to work correctly*/ - start_instruction = &instruction; + // Construct the output trajectory instruction and add it to the response + response.results.push_back(buildTrajectoryInstruction(trajectory, request.seed[i].as())); } + + // Set top-level composite start instruction to first waypoint of first trajectory + response.results.setStartInstruction(response.results.at(0).as().getStartInstruction()); + + response.status = tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::SolutionFound, status_category_); + } + catch (const std::exception& ex) + { + CONSOLE_BRIDGE_logError(ex.what()); + response.status = + tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::ErrorFailedToFindValidSolution, status_category_); } - return problem; + return response.status; +} + +MotionPlanner::Ptr OMPLMotionPlanner::clone() const { return std::make_shared(name_); } + +bool OMPLMotionPlanner::checkUserInput(const PlannerRequest& request) +{ + // Check that parameters are valid + if (request.env == nullptr) + throw std::runtime_error("Environment is invalid (nullptr)"); + + if (request.instructions.empty()) + throw std::runtime_error("Request contains no instructions"); + + if (request.instructions.size() != request.seed.size()) + throw std::runtime_error("Instruction size (" + std::to_string(request.instructions.size()) + + ") does not match seed size (" + std::to_string(request.seed.size()) + ")"); + + return true; } } // namespace tesseract_planning diff --git a/tesseract_motion_planners/ompl/src/ompl_problem.cpp b/tesseract_motion_planners/ompl/src/ompl_problem.cpp deleted file mode 100644 index 236a25a6002..00000000000 --- a/tesseract_motion_planners/ompl/src/ompl_problem.cpp +++ /dev/null @@ -1,38 +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. - */ - -#include -#include - -namespace tesseract_planning -{ -tesseract_common::TrajArray OMPLProblem::getTrajectory() const -{ - assert(extractor != nullptr); - return toTrajArray(this->simple_setup->getSolutionPath(), extractor); -} - -} // namespace tesseract_planning diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp new file mode 100644 index 00000000000..3a4d7e5c321 --- /dev/null +++ b/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp @@ -0,0 +1,144 @@ +#include + +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace tesseract_planning +{ +std::any OMPLCompositeProfileRVSS::create(const CompositeInstruction& instruction, + const tesseract_environment::Environment& env) const +{ + for (const Instruction& inst : instruction) + { + const auto& goal_instruction = inst.as(); + if (goal_instruction.getPlanType() != PlanInstructionType::FREESPACE) + throw std::runtime_error("Only freespace plan instruction types are currently supported"); + } + + // Get the joint group information from the composite instruction and environment + const tesseract_kinematics::JointGroup::ConstPtr joint_group = + env.getJointGroup(instruction.getManipulatorInfo().manipulator); + const std::vector joint_names = joint_group->getJointNames(); + const Eigen::MatrixX2d limits = joint_group->getLimits().joint_limits; + const Eigen::Index dof = joint_group->numJoints(); + + // Set up the extractor + OMPLStateExtractor extractor = [dof](const ompl::base::State* state) -> Eigen::Map { + return tesseract_planning::RealVectorStateSpaceExtractor(state, static_cast(dof)); + }; + + // Construct the real vector state space + auto rss = std::make_shared(); + for (unsigned i = 0; i < dof; ++i) + { + rss->addDimension(joint_names[i], limits(i, 0), limits(i, 1)); + } + + // Create the state sampler + if (state_sampler_allocator) + { + rss->setStateSamplerAllocator(state_sampler_allocator); + } + else + { + Eigen::VectorXd weights = Eigen::VectorXd::Ones(dof); + rss->setStateSamplerAllocator( + [weights, limits, this](const ompl::base::StateSpace* state_space) -> ompl::base::StateSamplerPtr { + return std::make_shared(state_space, weights, limits, this->rng_seed); + }); + } + + // Setup Longest Valid Segment + processLongestValidSegment(rss, collision_check_config); + + // Create the SimpleSetup from the state space + auto simple_setup = std::make_shared(rss); + + // Setup state checking functionality + ompl::base::StateValidityCheckerPtr custom_validity_checker{ nullptr }; + { + auto csvc = std::make_shared(); + if (state_validator_allocator != nullptr) + { + custom_validity_checker = state_validator_allocator(simple_setup->getSpaceInformation()); + csvc->addStateValidator(custom_validity_checker); + } + else + { + switch (collision_check_config.type) + { + case tesseract_collision::CollisionEvaluatorType::DISCRETE: + case tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE: + { + auto svc = std::make_shared( + simple_setup->getSpaceInformation(), env, joint_group, collision_check_config, extractor); + csvc->addStateValidator(svc); + break; + } + default: + CONSOLE_BRIDGE_logDebug("No collision state validator added"); + } + simple_setup->setStateValidityChecker(csvc); + } + } + + // Setup motion validation (i.e. collision checking) + if (motion_validator_allocator != nullptr) + { + simple_setup->getSpaceInformation()->setMotionValidator( + motion_validator_allocator(simple_setup->getSpaceInformation())); + } + else + { + ompl::base::MotionValidatorPtr mv{ nullptr }; + + switch (collision_check_config.type) + { + case tesseract_collision::CollisionEvaluatorType::DISCRETE: + case tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE: + { + // Collision checking is preformed using the state validator which this calls. + mv = std::make_shared(simple_setup->getSpaceInformation()); + break; + } + case tesseract_collision::CollisionEvaluatorType::CONTINUOUS: + case tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS: + { + mv = std::make_shared(simple_setup->getSpaceInformation(), + custom_validity_checker, + env, + joint_group, + collision_check_config, + extractor); + break; + } + default: + CONSOLE_BRIDGE_logDebug("No collision validator added"); + } + + simple_setup->getSpaceInformation()->setMotionValidator(mv); + } + + // make sure the planners run until the time limit, and get the best possible solution + if (optimization_objective_allocator) + { + simple_setup->getProblemDefinition()->setOptimizationObjective( + optimization_objective_allocator(simple_setup->getSpaceInformation())); + } + + // Return the composite data + return std::make_tuple(simple_setup, extractor); +} + +} // namespace tesseract_planning 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 24d5696b8bf..00000000000 --- a/tesseract_motion_planners/ompl/src/profile/ompl_default_plan_profile.cpp +++ /dev/null @@ -1,773 +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 -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include -#include - -#include -#include - -#include -#include -#include -#include - -#include - -namespace tesseract_planning -{ -OMPLDefaultPlanProfile::OMPLDefaultPlanProfile(const tinyxml2::XMLElement& xml_element) -{ - 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"); - - tinyxml2::XMLError 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 Instruction& parent_instruction, - const 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(isPlanInstruction(parent_instruction)); - const auto& base_instruction = parent_instruction.as(); - assert(!(manip_info.empty() && base_instruction.getManipulatorInfo().empty())); - ManipulatorInfo mi = manip_info.getCombined(base_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::satisfiesPositionLimits(solution, limits.joint_limits)) - { - tesseract_common::enforcePositionLimits(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 Instruction& /*parent_instruction*/, - const 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::satisfiesPositionLimits(solution, limits.joint_limits)) - { - tesseract_common::enforcePositionLimits(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 Instruction& parent_instruction, - const 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(isPlanInstruction(parent_instruction)); - const auto& base_instruction = parent_instruction.as(); - assert(!(manip_info.empty() && base_instruction.getManipulatorInfo().empty())); - ManipulatorInfo mi = manip_info.getCombined(base_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::satisfiesPositionLimits(solution, limits.joint_limits)) - { - tesseract_common::enforcePositionLimits(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 Instruction& /*parent_instruction*/, - const 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) - { - Eigen::VectorXd solution = joint_waypoint; - - if (tesseract_common::satisfiesPositionLimits(solution, limits.joint_limits)) - { - tesseract_common::enforcePositionLimits(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())); - } -} - -} // namespace tesseract_planning diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp new file mode 100644 index 00000000000..ddbf416ba4b --- /dev/null +++ b/tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp @@ -0,0 +1,133 @@ +#include +#include + +namespace tesseract_planning +{ +void checkCollision(const Eigen::VectorXd& state, + const tesseract_environment::Environment& env, + const tesseract_kinematics::JointGroup::ConstPtr& manip) +{ + tesseract_collision::DiscreteContactManager::Ptr contact_checker = env.getDiscreteContactManager(); + tesseract_common::TransformMap link_transforms = manip->calcFwdKin(state); + + for (const auto& link_name : contact_checker->getActiveCollisionObjects()) + contact_checker->setCollisionObjectsTransform(link_name, link_transforms[link_name]); + + tesseract_collision::ContactResultMap contact_map; + contact_checker->contactTest(contact_map, tesseract_collision::ContactTestType::FIRST); + + if (!contact_map.empty()) + { + std::stringstream ss; + ss << "State is in collision"; + for (const auto& contact_vec : contact_map) + for (const auto& contact : contact_vec.second) + ss << "\n\tLinks: " << contact.link_names[0] << ", " << contact.link_names[1] + << " Distance: " << contact.distance; + throw std::runtime_error(ss.str()); + } +} + +Eigen::VectorXd updateLimits(const Eigen::Ref& joint_waypoint, + const tesseract_common::KinematicLimits& limits) +{ + if (!tesseract_common::satisfiesPositionLimits(joint_waypoint, limits.joint_limits)) + throw std::runtime_error("State violates joint limits"); + + Eigen::VectorXd tmp(joint_waypoint); + tesseract_common::enforcePositionLimits(tmp, limits.joint_limits); + return tmp; +} + +tesseract_kinematics::IKSolutions getValidIKSolutions(const Eigen::Isometry3d& cartesian_waypoint, + const ManipulatorInfo& mi, + const tesseract_environment::Environment& env) +{ + 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!"); + + // Get the kinematics group for solving IK + tesseract_kinematics::KinematicGroup::ConstPtr manip = + env.getKinematicGroup(mi.manipulator, mi.manipulator_ik_solver); + + Eigen::Isometry3d tcp_offset = env.findTCPOffset(mi); + Eigen::Isometry3d tcp_frame_cwp = cartesian_waypoint * tcp_offset.inverse(); + tesseract_kinematics::KinGroupIKInput ik_input(tcp_frame_cwp, mi.working_frame, mi.tcp_frame); + const tesseract_kinematics::IKSolutions joint_solutions = + manip->calcInvKin({ ik_input }, Eigen::VectorXd::Zero(manip->numJoints())); + + // Assemble all the valid solutionss + tesseract_common::KinematicLimits limits = manip->getLimits(); + tesseract_kinematics::IKSolutions valid_solutions; + valid_solutions.reserve(joint_solutions.size()); + for (const Eigen::VectorXd& js : joint_solutions) + { + try + { + // Update the joint solution based on the kinematic limits + Eigen::VectorXd solution = updateLimits(js, limits); + + // Ensure the solution is collision-free + checkCollision(solution, env, manip); + + // Add the solution to the container + valid_solutions.push_back(solution); + + // Add the redundant solutions + auto redundant_solutions = tesseract_kinematics::getRedundantSolutions( + solution, limits.joint_limits, manip->getRedundancyCapableJointIndices()); + valid_solutions.insert(valid_solutions.end(), redundant_solutions.begin(), redundant_solutions.end()); + } + catch (const std::exception& ex) + { + CONSOLE_BRIDGE_logDebug(ex.what()); + continue; + } + } + + if (valid_solutions.empty()) + throw std::runtime_error("All states are either in collision or outside limits"); + + return valid_solutions; +} + +std::any OMPLWaypointProfile::create(const Instruction& instruction, + const tesseract_environment::Environment& env) const +{ + const auto& plan_instruction = instruction.as(); + const tesseract_common::ManipulatorInfo& mi = plan_instruction.getManipulatorInfo(); + const Waypoint& waypoint = plan_instruction.getWaypoint(); + + if (isCartesianWaypoint(waypoint)) + { + const auto& cw = waypoint.as(); + return getValidIKSolutions(cw, mi, env); + } + + if (isJointWaypoint(waypoint)) + { + const auto& jw = waypoint.as(); + const Eigen::VectorXd updated_state = updateLimits(jw, env.getJointGroup(mi.manipulator)->getLimits()); + checkCollision(updated_state, env, env.getJointGroup(mi.manipulator)); + return std::vector{ updated_state }; + } + + if (isStateWaypoint(waypoint)) + { + const auto& sw = waypoint.as(); + Eigen::Map state(sw.position.data(), sw.position.size()); + const Eigen::VectorXd updated_state = updateLimits(state, env.getJointGroup(mi.manipulator)->getLimits()); + checkCollision(updated_state, env, env.getJointGroup(mi.manipulator)); + return std::vector{ updated_state }; + } + + throw std::runtime_error("Unsupported waypoint type"); +} + +} // namespace tesseract_planning diff --git a/tesseract_motion_planners/ompl/src/serialize.cpp b/tesseract_motion_planners/ompl/src/serialize.cpp deleted file mode 100644 index b2549465fe1..00000000000 --- a/tesseract_motion_planners/ompl/src/serialize.cpp +++ /dev/null @@ -1,81 +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 -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#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 17da4838f1e..c560d884b09 100644 --- a/tesseract_motion_planners/ompl/src/utils.cpp +++ b/tesseract_motion_planners/ompl/src/utils.cpp @@ -100,31 +100,4 @@ 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) -{ - tesseract_common::TransformMap link_transforms = prob.manip->calcFwdKin(state); - - for (const auto& link_name : prob.contact_checker->getActiveCollisionObjects()) - prob.contact_checker->setCollisionObjectsTransform(link_name, link_transforms[link_name]); - - prob.contact_checker->contactTest(contact_map, tesseract_collision::ContactTestType::FIRST); - - return (!contact_map.empty()); -} - -bool checkStateInCollision(OMPLProblem& prob, const Eigen::VectorXd& state) -{ - tesseract_collision::ContactResultMap contact_map; - return checkStateInCollision(prob, state, contact_map); -} - -ompl::base::StateSamplerPtr allocWeightedRealVectorStateSampler(const ompl::base::StateSpace* space, - const Eigen::VectorXd& weights, - const Eigen::MatrixX2d& limits) -{ - return std::make_shared(space, weights, limits); -} - } // namespace tesseract_planning diff --git a/tesseract_motion_planners/ompl/src/weighted_real_vector_state_sampler.cpp b/tesseract_motion_planners/ompl/src/weighted_real_vector_state_sampler.cpp index 9694ffcddd9..98872e10489 100644 --- a/tesseract_motion_planners/ompl/src/weighted_real_vector_state_sampler.cpp +++ b/tesseract_motion_planners/ompl/src/weighted_real_vector_state_sampler.cpp @@ -39,10 +39,13 @@ namespace tesseract_planning { WeightedRealVectorStateSampler::WeightedRealVectorStateSampler(const ompl::base::StateSpace* space, const Eigen::Ref& weights, - const Eigen::Ref& bounds) + const Eigen::Ref& bounds, + long rng_seed) : ompl::base::StateSampler(space), weights_(weights), bounds_(bounds) { assert(bounds_.rows() == weights_.size()); + if (rng_seed >= 0) + rng_.setLocalSeed(static_cast(rng_seed)); } void WeightedRealVectorStateSampler::sampleUniform(ompl::base::State* state) diff --git a/tesseract_motion_planners/test/ompl_planner_tests.cpp b/tesseract_motion_planners/test/ompl_planner_tests.cpp index 864f23bb1c4..bc49d7bbe06 100644 --- a/tesseract_motion_planners/test/ompl_planner_tests.cpp +++ b/tesseract_motion_planners/test/ompl_planner_tests.cpp @@ -26,22 +26,7 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - #include - #include #include #include @@ -52,8 +37,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include -#include +#include +#include +#include #include #include @@ -69,9 +55,7 @@ using namespace tesseract_kinematics; using namespace tesseract_planning; using namespace tesseract_planning::profile_ns; -const static int SEED = 1; -const static std::vector start_state = { -0.5, 0.5, 0.0, -1.3348, 0.0, 1.4959, 0.0 }; -const static std::vector end_state = { 0.5, 0.5, 0.0, -1.3348, 0.0, 1.4959, 0.0 }; +const static long SEED = 1; std::string locateResource(const std::string& url) { @@ -127,10 +111,63 @@ template class OMPLTestFixture : public ::testing::Test { public: - OMPLTestFixture() { configurator = std::make_shared(); } using ::testing::Test::Test; - std::shared_ptr configurator; - OMPLMotionPlanner ompl_planner; + + OMPLTestFixture() + : env(std::make_shared()) + , manip("manipulator", "base_link", "tool0") + , configurator(std::make_shared()) + { + // Load scene and srdf + auto locator = std::make_shared(locateResource); + tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"); + tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"); + EXPECT_TRUE(this->env->init(urdf_path, srdf_path, locator)); + + // Add box to environment + addBox(*env); + } + + std::shared_ptr createCompositeProfile() + { + auto composite_profile = std::make_shared(); + composite_profile->collision_check_config.contact_manager_config.margin_data_override_type = + tesseract_collision::CollisionMarginOverrideType::OVERRIDE_DEFAULT_MARGIN; + composite_profile->collision_check_config.contact_manager_config.margin_data.setDefaultCollisionMargin(0.025); + composite_profile->collision_check_config.longest_valid_segment_length = 0.1; + composite_profile->collision_check_config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; + composite_profile->rng_seed = SEED; + + return composite_profile; + } + + std::shared_ptr createPlannerProfile() + { + auto planner_profile = std::make_shared(); + planner_profile->params.planning_time = 5.0; + planner_profile->params.optimize = false; + planner_profile->params.max_solutions = 2; + planner_profile->params.simplify = false; + planner_profile->params.planners = { this->configurator, this->configurator }; + return planner_profile; + } + + /** @brief Motion planning environment */ + Environment::Ptr env; + /** @brief Manipulator information for planning */ + const ManipulatorInfo manip; + /** @brief OMPL planner factory */ + const std::shared_ptr configurator; + /** @brief Tesseract OMPL motion planner */ + const OMPLMotionPlanner ompl_planner; + /** @brief Arbitrary trajectory start state */ + const std::vector start_state_ = { -0.6, 0.5, 0.0, -1.3348, 0.0, 1.4959, 0.0 }; + /** @brief Arbitrary trajectory end state */ + const std::vector end_state_ = { 0.6, 0.5, 0.0, -1.3348, 0.0, 1.4959, 0.0 }; + /** @brief Motion planning profile name */ + const std::string profile_name_ = "TEST_PROFILE"; + /** @brief Number of steps in the seed trajectory, equivalent to the the number of states minus one */ + const int seed_steps_ = 10; }; using Implementations = ::testing::Types(locateResource); - Environment::Ptr env = std::make_shared(); - tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"); - tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"); - EXPECT_TRUE(env->init(urdf_path, srdf_path, locator)); - - ManipulatorInfo manip; - manip.manipulator = "manipulator"; - manip.working_frame = "base_link"; - manip.tcp_frame = "tool0"; - - // Step 2: Add box to environment - addBox(*env); - - // Step 3: Create ompl planner config and populate it - auto joint_group = env->getJointGroup(manip.manipulator); - auto cur_state = env->getState(); + const auto joint_group = this->env->getJointGroup(this->manip.manipulator); + const auto cur_state = this->env->getState(); // Specify a start waypoint - JointWaypoint wp1(joint_group->getJointNames(), - Eigen::Map(start_state.data(), static_cast(start_state.size()))); + const JointWaypoint wp1( + joint_group->getJointNames(), + Eigen::Map(this->start_state_.data(), static_cast(this->start_state_.size()))); // Specify a end waypoint - JointWaypoint wp2(joint_group->getJointNames(), - Eigen::Map(end_state.data(), static_cast(end_state.size()))); + const JointWaypoint wp2( + joint_group->getJointNames(), + Eigen::Map(this->end_state_.data(), static_cast(this->end_state_.size()))); // Define Start Instruction - PlanInstruction start_instruction(wp1, PlanInstructionType::START, "TEST_PROFILE"); + const PlanInstruction start_instruction(wp1, PlanInstructionType::START, this->profile_name_, this->manip); // Define Plan Instructions - PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, "TEST_PROFILE"); - PlanInstruction plan_f2(wp1, PlanInstructionType::FREESPACE, "TEST_PROFILE"); + const PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, this->profile_name_, this->manip); + const PlanInstruction plan_f2(wp1, PlanInstructionType::FREESPACE, this->profile_name_, this->manip); // Create a program CompositeInstruction program; + program.setProfile(this->profile_name_); program.setStartInstruction(start_instruction); - program.setManipulatorInfo(manip); + program.setManipulatorInfo(this->manip); program.push_back(plan_f1); program.push_back(plan_f2); - // Create a seed - CompositeInstruction seed = generateSeed(program, cur_state, env, 3.14, 1.0, 3.14, 10); - - // Create Profiles - 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 }; - - // Profile Dictionary - auto profiles = std::make_shared(); - profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); - // Create Planner Request PlannerRequest request; request.instructions = program; - request.seed = seed; - request.env = env; + request.seed = generateSeed(program, cur_state, this->env, 3.14, 1.0, 3.14, this->seed_steps_); + request.env = this->env; request.env_state = cur_state; - request.profiles = profiles; - // Create Planner and solve - OMPLMotionPlanner ompl_planner; + // Add the profiles + { + auto d = std::make_unique(); + d->planner_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = this->createPlannerProfile(); + d->composite_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = this->createCompositeProfile(); + d->waypoint_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = std::make_shared(); + request.profiles = std::move(d); + } + PlannerResponse planner_response; - auto status = ompl_planner.solve(request, planner_response); + auto status = this->ompl_planner.solve(request, planner_response); if (!status) { CONSOLE_BRIDGE_logError("CI Error: %s", status.message().c_str()); } - EXPECT_TRUE(&status); + ASSERT_TRUE(&status); EXPECT_TRUE(planner_response.results.hasStartInstruction()); - EXPECT_EQ(getMoveInstructionCount(planner_response.results), 21); // 10 per segment + start a instruction + EXPECT_GE(getMoveInstructionCount(planner_response.results), 2 * this->seed_steps_ + 1); EXPECT_EQ(planner_response.results.size(), 2); EXPECT_TRUE(wp1.isApprox(getJointPosition(getFirstMoveInstruction(planner_response.results)->getWaypoint()), 1e-5)); EXPECT_TRUE(wp2.isApprox( getJointPosition( getLastMoveInstruction(planner_response.results.front().as())->getWaypoint()), 1e-5)); - EXPECT_TRUE(wp1.isApprox(getJointPosition(getLastMoveInstruction(planner_response.results)->getWaypoint()), 1e-5)); +} - // Check for start state in collision error - std::vector swp = { 0, 0.7, 0.0, 0, 0.0, 0, 0.0 }; +TYPED_TEST(OMPLTestFixture, StartStateInCollision) +{ + const auto joint_group = this->env->getJointGroup(this->manip.manipulator); + const auto cur_state = this->env->getState(); - // Define New Start Instruction - wp1 = Eigen::Map(swp.data(), static_cast(swp.size())); - wp1.joint_names = joint_group->getJointNames(); + // Check for start state in collision error + const std::vector swp = { 0, 0.7, 0.0, 0, 0.0, 0, 0.0 }; + const JointWaypoint wp1(joint_group->getJointNames(), + Eigen::Map(swp.data(), static_cast(swp.size()))); + const PlanInstruction start_instruction(wp1, PlanInstructionType::START, this->profile_name_, this->manip); - start_instruction = PlanInstruction(wp1, PlanInstructionType::START, "TEST_PROFILE"); + // Specify a end waypoint + const JointWaypoint wp2( + joint_group->getJointNames(), + Eigen::Map(this->end_state_.data(), static_cast(this->end_state_.size()))); + const PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, this->profile_name_, this->manip); // Create a new program - program = CompositeInstruction(); + CompositeInstruction program; + program.setProfile(this->profile_name_); program.setStartInstruction(start_instruction); - program.setManipulatorInfo(manip); + program.setManipulatorInfo(this->manip); program.push_back(plan_f1); - // Create a new seed - seed = generateSeed(program, cur_state, env, 3.14, 1.0, 3.14, 10); - // Update Configuration + PlannerRequest request; request.instructions = program; - request.seed = seed; - request.data = nullptr; // Note: Nust clear the saved problem or it will use it instead. + request.seed = generateSeed(program, cur_state, this->env, 3.14, 1.0, 3.14, this->seed_steps_); + request.env = this->env; + request.env_state = cur_state; + + // Add the profiles + { + auto d = std::make_unique(); + d->planner_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = this->createPlannerProfile(); + d->composite_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = this->createCompositeProfile(); + d->waypoint_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = std::make_shared(); + request.profiles = std::move(d); + } // Solve - status = ompl_planner.solve(request, planner_response); + PlannerResponse planner_response; + auto status = this->ompl_planner.solve(request, planner_response); EXPECT_FALSE(status); +} - // Check for end state in collision error - swp = start_state; - std::vector ewp = { 0, 0.7, 0.0, 0, 0.0, 0, 0.0 }; - - wp1 = Eigen::Map(swp.data(), static_cast(swp.size())); - wp1.joint_names = joint_group->getJointNames(); - - wp2 = Eigen::Map(ewp.data(), static_cast(ewp.size())); - wp2.joint_names = joint_group->getJointNames(); +TYPED_TEST(OMPLTestFixture, EndStateInCollision) +{ + const auto joint_group = this->env->getJointGroup(this->manip.manipulator); + const auto cur_state = this->env->getState(); - // Define Start Instruction - start_instruction = PlanInstruction(wp1, PlanInstructionType::START, "TEST_PROFILE"); + // Specify a start waypoint + const JointWaypoint wp1( + joint_group->getJointNames(), + Eigen::Map(this->start_state_.data(), static_cast(this->start_state_.size()))); + const PlanInstruction start_instruction(wp1, PlanInstructionType::START, this->profile_name_, this->manip); - // Define Plan Instructions - plan_f1 = PlanInstruction(wp2, PlanInstructionType::FREESPACE, "TEST_PROFILE"); + // Check for end state in collision error + const std::vector ewp = { 0, 0.7, 0.0, 0, 0.0, 0, 0.0 }; + const JointWaypoint wp2(joint_group->getJointNames(), + Eigen::Map(ewp.data(), static_cast(ewp.size()))); + const PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, this->profile_name_, this->manip); // Create a new program - program = CompositeInstruction(); + CompositeInstruction program; + program.setProfile(this->profile_name_); program.setStartInstruction(start_instruction); - program.setManipulatorInfo(manip); + program.setManipulatorInfo(this->manip); program.push_back(plan_f1); - // Create a new seed - seed = generateSeed(program, cur_state, env, 3.14, 1.0, 3.14, 10); - - // Update Configuration + PlannerRequest request; + request.seed = generateSeed(program, cur_state, this->env, 3.14, 1.0, 3.14, this->seed_steps_); request.instructions = program; - request.seed = seed; - request.data = nullptr; // Note: Nust clear the saved problem or it will use it instead. + request.env = this->env; + request.env_state = cur_state; + + // Add the profiles + { + auto d = std::make_unique(); + d->planner_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = this->createPlannerProfile(); + d->composite_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = this->createCompositeProfile(); + d->waypoint_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = std::make_shared(); + request.profiles = std::move(d); + } // Set new configuration and solve - status = ompl_planner.solve(request, planner_response); + PlannerResponse planner_response; + auto status = this->ompl_planner.solve(request, planner_response); EXPECT_FALSE(status); } -TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianGoalPlannerUnit) +TYPED_TEST(OMPLTestFixture, JointStartCartesianGoal) { - EXPECT_EQ(ompl::RNG::getSeed(), SEED) << "Randomization seed does not match expected: " << ompl::RNG::getSeed() - << " vs. " << SEED; - - // Step 1: Load scene and srdf - auto locator = std::make_shared(locateResource); - Environment::Ptr env = std::make_shared(); - tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"); - tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"); - EXPECT_TRUE(env->init(urdf_path, srdf_path, locator)); - - // Set manipulator - ManipulatorInfo manip; - manip.tcp_frame = "tool0"; - manip.manipulator = "manipulator"; - manip.working_frame = "base_link"; - - // Step 2: Add box to environment - addBox(*(env)); - - // Step 3: Create ompl planner config and populate it - auto kin_group = env->getKinematicGroup(manip.manipulator); - auto cur_state = env->getState(); + const auto kin_group = this->env->getKinematicGroup(this->manip.manipulator); + const auto cur_state = this->env->getState(); // Specify a start waypoint - JointWaypoint wp1(kin_group->getJointNames(), - Eigen::Map(start_state.data(), static_cast(start_state.size()))); + const JointWaypoint wp1( + kin_group->getJointNames(), + Eigen::Map(this->start_state_.data(), static_cast(this->start_state_.size()))); // Specify a end waypoint - auto goal_jv = Eigen::Map(end_state.data(), static_cast(end_state.size())); - Eigen::Isometry3d goal = kin_group->calcFwdKin(goal_jv).at(manip.tcp_frame); - CartesianWaypoint wp2 = goal; + const auto goal_jv = + Eigen::Map(this->end_state_.data(), static_cast(this->end_state_.size())); + const Eigen::Isometry3d goal = kin_group->calcFwdKin(goal_jv).at(this->manip.tcp_frame); + const CartesianWaypoint wp2 = goal; // Define Start Instruction - PlanInstruction start_instruction(wp1, PlanInstructionType::START, "TEST_PROFILE"); + const PlanInstruction start_instruction(wp1, PlanInstructionType::START, this->profile_name_, this->manip); // Define Plan Instructions - PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, "TEST_PROFILE"); + const PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, this->profile_name_, this->manip); + const PlanInstruction plan_f2(wp1, PlanInstructionType::FREESPACE, this->profile_name_, this->manip); // Create a program CompositeInstruction program; + program.setProfile(this->profile_name_); program.setStartInstruction(start_instruction); - program.setManipulatorInfo(manip); + program.setManipulatorInfo(this->manip); program.push_back(plan_f1); - - // Create a seed - CompositeInstruction seed = generateSeed(program, cur_state, env, 3.14, 1.0, 3.14, 10); - - // Create Profiles - 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 }; - - // Profile Dictionary - auto profiles = std::make_shared(); - profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); + program.push_back(plan_f2); // Create Planner Request PlannerRequest request; request.instructions = program; - request.seed = seed; - request.env = env; + request.seed = generateSeed(program, cur_state, this->env, 3.14, 1.0, 3.14, this->seed_steps_); + request.env = this->env; request.env_state = cur_state; - request.profiles = profiles; - // Create Planner and solve - OMPLMotionPlanner ompl_planner; + // Add the profiles + { + auto d = std::make_unique(); + d->planner_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = this->createPlannerProfile(); + d->composite_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = this->createCompositeProfile(); + d->waypoint_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = std::make_shared(); + request.profiles = std::move(d); + } + PlannerResponse planner_response; - auto status = ompl_planner.solve(request, planner_response); + auto status = this->ompl_planner.solve(request, planner_response); if (!status) { CONSOLE_BRIDGE_logError("CI Error: %s", status.message().c_str()); } - EXPECT_TRUE(&status); - EXPECT_TRUE(planner_response.results.hasStartInstruction()); - EXPECT_EQ(getMoveInstructionCount(planner_response.results), 11); + ASSERT_TRUE(&status); + ASSERT_TRUE(planner_response.results.hasStartInstruction()); + EXPECT_GE(getMoveInstructionCount(planner_response.results), 2 * this->seed_steps_ + 1); EXPECT_TRUE(wp1.isApprox(getJointPosition(getFirstMoveInstruction(planner_response.results)->getWaypoint()), 1e-5)); + EXPECT_TRUE(wp1.isApprox(getJointPosition(getLastMoveInstruction(planner_response.results)->getWaypoint()), 1e-5)); - Eigen::Isometry3d check_goal = - kin_group->calcFwdKin(getJointPosition(getLastMoveInstruction(planner_response.results)->getWaypoint())) - .at(manip.tcp_frame); + const MoveInstruction* cart_move = + getLastMoveInstruction(planner_response.results.front().as()); + const Eigen::VectorXd& cart_move_joints = getJointPosition(cart_move->getWaypoint()); + const Eigen::Isometry3d check_goal = kin_group->calcFwdKin(cart_move_joints).at(this->manip.tcp_frame); EXPECT_TRUE(wp2.isApprox(check_goal, 1e-3)); } -TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianStartPlannerUnit) +TYPED_TEST(OMPLTestFixture, CartesianStartJointGoal) { - EXPECT_EQ(ompl::RNG::getSeed(), SEED) << "Randomization seed does not match expected: " << ompl::RNG::getSeed() - << " vs. " << SEED; - - // Step 1: Load scene and srdf - auto locator = std::make_shared(locateResource); - Environment::Ptr env = std::make_shared(); - tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"); - tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"); - EXPECT_TRUE(env->init(urdf_path, srdf_path, locator)); - - // Set manipulator - ManipulatorInfo manip; - manip.tcp_frame = "tool0"; - manip.manipulator = "manipulator"; - manip.working_frame = "base_link"; - - // Step 2: Add box to environment - addBox(*(env)); - - // Step 3: Create ompl planner config and populate it - auto kin_group = env->getKinematicGroup(manip.manipulator); - auto cur_state = env->getState(); + const auto kin_group = this->env->getKinematicGroup(this->manip.manipulator); + const auto cur_state = this->env->getState(); // Specify a start waypoint - auto start_jv = Eigen::Map(start_state.data(), static_cast(start_state.size())); - Eigen::Isometry3d start = kin_group->calcFwdKin(start_jv).at(manip.tcp_frame); - CartesianWaypoint wp1 = start; + const auto start_jv = + Eigen::Map(this->start_state_.data(), static_cast(this->start_state_.size())); + const Eigen::Isometry3d start = kin_group->calcFwdKin(start_jv).at(this->manip.tcp_frame); + const CartesianWaypoint wp1 = start; // Specify a end waypoint - JointWaypoint wp2(kin_group->getJointNames(), - Eigen::Map(end_state.data(), static_cast(end_state.size()))); + const JointWaypoint wp2( + kin_group->getJointNames(), + Eigen::Map(this->end_state_.data(), static_cast(this->end_state_.size()))); // Define Start Instruction - PlanInstruction start_instruction(wp1, PlanInstructionType::START, "TEST_PROFILE"); + const PlanInstruction start_instruction(wp1, PlanInstructionType::START, this->profile_name_, this->manip); // Define Plan Instructions - PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, "TEST_PROFILE"); + const PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, this->profile_name_, this->manip); + const PlanInstruction plan_f2(wp1, PlanInstructionType::FREESPACE, this->profile_name_, this->manip); // Create a program CompositeInstruction program; + program.setProfile(this->profile_name_); program.setStartInstruction(start_instruction); - program.setManipulatorInfo(manip); + program.setManipulatorInfo(this->manip); program.push_back(plan_f1); - - // Create a seed - CompositeInstruction seed = generateSeed(program, cur_state, env, 3.14, 1.0, 3.14, 10); - - // Create Profiles - 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 }; - - // Profile Dictionary - auto profiles = std::make_shared(); - profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); + program.push_back(plan_f2); // Create Planner Request PlannerRequest request; request.instructions = program; - request.seed = seed; - request.env = env; + request.seed = generateSeed(program, cur_state, this->env, 3.14, 1.0, 3.14, this->seed_steps_); + request.env = this->env; request.env_state = cur_state; - request.profiles = profiles; - // Create Planner and solve - OMPLMotionPlanner ompl_planner; + // Add the profiles + { + auto d = std::make_unique(); + d->planner_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = this->createPlannerProfile(); + d->composite_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = this->createCompositeProfile(); + d->waypoint_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = std::make_shared(); + request.profiles = std::move(d); + } + PlannerResponse planner_response; - auto status = ompl_planner.solve(request, planner_response); + auto status = this->ompl_planner.solve(request, planner_response); if (!status) { CONSOLE_BRIDGE_logError("CI Error: %s", status.message().c_str()); } - EXPECT_TRUE(&status); - EXPECT_TRUE(planner_response.results.hasStartInstruction()); - EXPECT_EQ(getMoveInstructionCount(planner_response.results), 11); - EXPECT_TRUE(wp2.isApprox(getJointPosition(getLastMoveInstruction(planner_response.results)->getWaypoint()), 1e-5)); - - Eigen::Isometry3d check_start = - kin_group->calcFwdKin(getJointPosition(getFirstMoveInstruction(planner_response.results)->getWaypoint())) - .at(manip.tcp_frame); - EXPECT_TRUE(wp1.isApprox(check_start, 1e-3)); + ASSERT_TRUE(&status); + ASSERT_TRUE(planner_response.results.hasStartInstruction()); + EXPECT_GE(getMoveInstructionCount(planner_response.results), 2 * this->seed_steps_ + 1); + + // Check the start/end Cartesian coordinate against the target waypoint + auto check_cartesian_pose = [&](const MoveInstruction* mi) { + const Eigen::VectorXd& joints = getJointPosition(mi->getWaypoint()); + const Eigen::Isometry3d pose = kin_group->calcFwdKin(joints).at(this->manip.tcp_frame); + EXPECT_TRUE(wp1.isApprox(pose, 1e-3)); + }; + check_cartesian_pose(getFirstMoveInstruction(planner_response.results)); + check_cartesian_pose(getLastMoveInstruction(planner_response.results)); + + // Check the joint move + const MoveInstruction* last_mi = getLastMoveInstruction(planner_response.results.front().as()); + EXPECT_TRUE(wp2.isApprox(getJointPosition(last_mi->getWaypoint()), 1e-5)); } // TEST(OMPLMultiPlanner, OMPLMultiPlannerUnit) // NOLINT @@ -512,7 +499,7 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianStartPlannerUnit) // addBox(*(env)); // // Step 3: Create ompl planner config and populate it -// auto kin = env->getManipulatorManager()->getFwdKinematicSolver(manip.manipulator); +// auto kin = this->env->getManipulatorManager()->getFwdKinematicSolver(manip.manipulator); // std::vector swp = start_state; // std::vector ewp = end_state; @@ -531,7 +518,6 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianStartPlannerUnit) // ompl_config->collision_continuous = true; // ompl_config->collision_check = true; // ompl_config->simplify = false; -// ompl_config->n_output_states = 50; // // Set the planner configuration // ompl_planner.setConfiguration(ompl_config); @@ -544,7 +530,7 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianStartPlannerUnit) // CONSOLE_BRIDGE_logError("CI Error: %s", status.message().c_str()); // } // EXPECT_TRUE(&status); -// EXPECT_EQ(ompl_planning_response.joint_trajectory.trajectory.rows(), ompl_config->n_output_states); +// EXPECT_GE(ompl_planning_response.joint_trajectory.trajectory.rows(), request.seed.size()); // // Check for start state in collision error // swp = { 0, 0.7, 0.0, 0, 0.0, 0, 0.0 }; @@ -580,8 +566,7 @@ int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); - // Set the randomization seed for the planners to get repeatable results - ompl::RNG::setSeed(SEED); + ompl::RNG::setSeed(static_cast(SEED)); return RUN_ALL_TESTS(); } diff --git a/tesseract_motion_planners/test/profile_dictionary_tests.cpp b/tesseract_motion_planners/test/profile_dictionary_tests.cpp index 5159c79055e..1ee93399384 100644 --- a/tesseract_motion_planners/test/profile_dictionary_tests.cpp +++ b/tesseract_motion_planners/test/profile_dictionary_tests.cpp @@ -28,6 +28,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include #include struct ProfileBase diff --git a/tesseract_motion_planners/test/serialize_test.cpp b/tesseract_motion_planners/test/serialize_test.cpp index 7737fa17621..cc340d89cb4 100644 --- a/tesseract_motion_planners/test/serialize_test.cpp +++ b/tesseract_motion_planners/test/serialize_test.cpp @@ -33,10 +33,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include -#include -#include - #include #include #include @@ -76,43 +72,6 @@ TrajOptDefaultPlanProfile getTrajOptPlanProfile() return plan_profile; } -OMPLDefaultPlanProfile getOMPLPlanProfile() -{ - OMPLDefaultPlanProfile ompl_profile; - - ompl_profile.simplify = true; - - ompl_profile.planners.push_back(std::make_shared()); - - ompl_profile.planners.push_back(std::make_shared()); - - ompl_profile.planners.push_back(std::make_shared()); - - ompl_profile.planners.push_back(std::make_shared()); - - ompl_profile.planners.push_back(std::make_shared()); - - ompl_profile.planners.push_back(std::make_shared()); - - ompl_profile.planners.push_back(std::make_shared()); - - ompl_profile.planners.push_back(std::make_shared()); - - ompl_profile.planners.push_back(std::make_shared()); - - ompl_profile.planners.push_back(std::make_shared()); - - ompl_profile.planners.push_back(std::make_shared()); - - ompl_profile.planners.push_back(std::make_shared()); - - ompl_profile.planners.push_back(std::make_shared()); - - ompl_profile.planners.push_back(std::make_shared()); - - return ompl_profile; -} - DescartesDefaultPlanProfile genDescartesPlanProfile() { DescartesDefaultPlanProfile descartes_profile; @@ -158,23 +117,6 @@ TEST(TesseractMotionPlannersTrajoptSerializeUnit, SerializeTrajoptDefaultPlanToX EXPECT_TRUE(plan_profile.term_type == imported_plan_profile.term_type); } -TEST(TesseractMotionPlannersOMPLSerializeUnit, SerializeOMPLDefaultPlanToXml) // NOLINT -{ - // Write program to file - OMPLDefaultPlanProfile plan_profile = getOMPLPlanProfile(); - 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); -} - TEST(TesseractMotionPlannersDescartesSerializeUnit, SerializeDescartesDefaultPlanToXml) // NOLINT { // Write program to file diff --git a/tesseract_process_managers/examples/freespace_example_program.h b/tesseract_process_managers/examples/freespace_example_program.h index f62ee2d5ef8..018eac8e76a 100644 --- a/tesseract_process_managers/examples/freespace_example_program.h +++ b/tesseract_process_managers/examples/freespace_example_program.h @@ -14,24 +14,24 @@ inline CompositeInstruction freespaceExampleProgramIIWA( const std::string& composite_profile = DEFAULT_PROFILE_KEY, const std::string& freespace_profile = DEFAULT_PROFILE_KEY) { - CompositeInstruction program( - composite_profile, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + const ManipulatorInfo manip("manipulator", "base_link", "tool0"); + CompositeInstruction program(composite_profile, CompositeInstructionOrder::ORDERED, manip); // Start Joint Position for the program std::vector joint_names = { "joint_a1", "joint_a2", "joint_a3", "joint_a4", "joint_a5", "joint_a6", "joint_a7" }; Waypoint wp1 = StateWaypoint(joint_names, Eigen::VectorXd::Zero(7)); - PlanInstruction start_instruction(wp1, PlanInstructionType::START); + PlanInstruction start_instruction(wp1, PlanInstructionType::START, freespace_profile, manip); program.setStartInstruction(start_instruction); // Define target pose Waypoint wp2 = CartesianWaypoint(goal); - PlanInstruction plan_f0(wp2, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f0(wp2, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f0.setDescription("freespace_motion"); program.push_back(plan_f0); Waypoint wp3 = JointWaypoint(joint_names, Eigen::VectorXd::Zero(7)); - PlanInstruction plan_f1(wp3, PlanInstructionType::FREESPACE); + PlanInstruction plan_f1(wp3, PlanInstructionType::FREESPACE, freespace_profile, manip); program.push_back(plan_f1); return program; @@ -42,23 +42,23 @@ inline CompositeInstruction freespaceExampleProgramABB( const std::string& composite_profile = DEFAULT_PROFILE_KEY, const std::string& freespace_profile = DEFAULT_PROFILE_KEY) { - CompositeInstruction program( - composite_profile, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + const ManipulatorInfo manip("manipulator", "base_link", "tool0"); + CompositeInstruction program(composite_profile, CompositeInstructionOrder::ORDERED, manip); // Start Joint Position for the program std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; Waypoint wp1 = StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)); - PlanInstruction start_instruction(wp1, PlanInstructionType::START); + PlanInstruction start_instruction(wp1, PlanInstructionType::START, freespace_profile, manip); program.setStartInstruction(start_instruction); // Define target pose Waypoint wp2 = CartesianWaypoint(goal); - PlanInstruction plan_f0(wp2, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f0(wp2, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f0.setDescription("freespace_motion"); program.push_back(plan_f0); Waypoint wp3 = JointWaypoint(joint_names, Eigen::VectorXd::Zero(6)); - PlanInstruction plan_f1(wp3, PlanInstructionType::FREESPACE); + PlanInstruction plan_f1(wp3, PlanInstructionType::FREESPACE, freespace_profile, manip); program.push_back(plan_f1); return program; diff --git a/tesseract_process_managers/examples/freespace_manager_example.cpp b/tesseract_process_managers/examples/freespace_manager_example.cpp index a169561fab2..bafcaf023a0 100644 --- a/tesseract_process_managers/examples/freespace_manager_example.cpp +++ b/tesseract_process_managers/examples/freespace_manager_example.cpp @@ -68,6 +68,7 @@ int main() // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; diff --git a/tesseract_process_managers/examples/memory_usage_example.cpp b/tesseract_process_managers/examples/memory_usage_example.cpp index c090927d6d8..46e10ec123b 100644 --- a/tesseract_process_managers/examples/memory_usage_example.cpp +++ b/tesseract_process_managers/examples/memory_usage_example.cpp @@ -151,6 +151,7 @@ int main() // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env), 5); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Solve process plan using Clock = std::chrono::high_resolution_clock; diff --git a/tesseract_process_managers/examples/raster_dt_example_program.h b/tesseract_process_managers/examples/raster_dt_example_program.h index 6efcbaddeef..73289134112 100644 --- a/tesseract_process_managers/examples/raster_dt_example_program.h +++ b/tesseract_process_managers/examples/raster_dt_example_program.h @@ -36,20 +36,20 @@ namespace tesseract_planning inline CompositeInstruction rasterDTExampleProgram(const std::string& freespace_profile = DEFAULT_PROFILE_KEY, const std::string& process_profile = "PROCESS") { - CompositeInstruction program( - DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + const ManipulatorInfo manip("manipulator", "base_link", "tool0"); + CompositeInstruction program(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, manip); // Start Joint Position for the program std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; StateWaypoint swp1 = StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)); - PlanInstruction start_instruction(swp1, PlanInstructionType::START); + PlanInstruction start_instruction(swp1, PlanInstructionType::START, freespace_profile, manip); program.setStartInstruction(start_instruction); Waypoint wp1 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -0.3, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)); // Define from start composite instruction - PlanInstruction plan_f0(wp1, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f0(wp1, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f0.setDescription("from_start_plan"); CompositeInstruction from_start(freespace_profile); from_start.setDescription("from_start"); @@ -75,25 +75,25 @@ inline CompositeInstruction rasterDTExampleProgram(const std::string& freespace_ Waypoint wp7 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(x, 0.3, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)); - CompositeInstruction raster_segment(freespace_profile); + CompositeInstruction raster_segment(freespace_profile, CompositeInstructionOrder::ORDERED, manip); raster_segment.setDescription("Raster #" + std::to_string(i + 1)); if (i == 0 || i == 2) { - raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, process_profile)); + raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, process_profile, manip)); } else { - raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, process_profile)); + raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, process_profile, manip)); } program.push_back(raster_segment); @@ -108,21 +108,21 @@ inline CompositeInstruction rasterDTExampleProgram(const std::string& freespace_ CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8 + (i * 0.1), -0.3, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)); - PlanInstruction plan_f1(wp7, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1(wp7, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1.setDescription("transition_from_end_plan"); - PlanInstruction plan_f1_dt(wp7_dt, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1_dt(wp7_dt, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1_dt.setDescription("transition_to_start_plan"); - CompositeInstruction transition_from_end(freespace_profile); + CompositeInstruction transition_from_end(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition_from_end.setDescription("transition_from_end"); transition_from_end.push_back(plan_f1); - CompositeInstruction transition_to_start(freespace_profile); + CompositeInstruction transition_to_start(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition_to_start.setDescription("transition_to_start"); transition_to_start.push_back(plan_f1_dt); - CompositeInstruction transition("transition dual", CompositeInstructionOrder::UNORDERED); + CompositeInstruction transition("transition dual", CompositeInstructionOrder::UNORDERED, manip); transition.push_back(transition_from_end); transition.push_back(transition_to_start); program.push_back(transition); @@ -137,30 +137,30 @@ inline CompositeInstruction rasterDTExampleProgram(const std::string& freespace_ CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8 + (i * 0.1), 0.3, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)); - PlanInstruction plan_f1(wp1, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1(wp1, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1.setDescription("transition_from_end_plan"); - PlanInstruction plan_f1_dt(wp1_dt, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1_dt(wp1_dt, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1_dt.setDescription("transition_to_start_plan"); - CompositeInstruction transition_from_end(freespace_profile); + CompositeInstruction transition_from_end(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition_from_end.setDescription("transition_from_end"); transition_from_end.push_back(plan_f1); - CompositeInstruction transition_to_start(freespace_profile); + CompositeInstruction transition_to_start(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition_to_start.setDescription("transition_to_start"); transition_to_start.push_back(plan_f1_dt); - CompositeInstruction transition("transition dual", CompositeInstructionOrder::UNORDERED); + CompositeInstruction transition("transition dual", CompositeInstructionOrder::UNORDERED, manip); transition.push_back(transition_from_end); transition.push_back(transition_to_start); program.push_back(transition); } } - PlanInstruction plan_f2(swp1, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f2(swp1, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f2.setDescription("to_end_plan"); - CompositeInstruction to_end; + CompositeInstruction to_end(freespace_profile, CompositeInstructionOrder::ORDERED, manip); to_end.setDescription("to_end"); to_end.push_back(plan_f2); program.push_back(to_end); diff --git a/tesseract_process_managers/examples/raster_example_program.h b/tesseract_process_managers/examples/raster_example_program.h index a76532c9433..73a8bc1a94d 100644 --- a/tesseract_process_managers/examples/raster_example_program.h +++ b/tesseract_process_managers/examples/raster_example_program.h @@ -36,20 +36,20 @@ namespace tesseract_planning inline CompositeInstruction rasterExampleProgram(const std::string& freespace_profile = DEFAULT_PROFILE_KEY, const std::string& process_profile = "PROCESS") { - CompositeInstruction program( - DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + const ManipulatorInfo manip("manipulator", "base_link", "tool0"); + CompositeInstruction program(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, manip); // Start Joint Position for the program std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; StateWaypoint swp1 = StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)); - PlanInstruction start_instruction(swp1, PlanInstructionType::START); + PlanInstruction start_instruction(swp1, PlanInstructionType::START, freespace_profile, manip); program.setStartInstruction(start_instruction); Waypoint wp1 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -0.3, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)); // Define from start composite instruction - PlanInstruction plan_f0(wp1, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f0(wp1, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f0.setDescription("from_start_plan"); CompositeInstruction from_start(freespace_profile); from_start.setDescription("from_start"); @@ -75,25 +75,25 @@ inline CompositeInstruction rasterExampleProgram(const std::string& freespace_pr Waypoint wp7 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(x, 0.3, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)); - CompositeInstruction raster_segment(process_profile); + CompositeInstruction raster_segment(process_profile, CompositeInstructionOrder::ORDERED, manip); raster_segment.setDescription("Raster #" + std::to_string(i + 1)); if (i == 0 || i == 2) { - raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, process_profile)); + raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, process_profile, manip)); } else { - raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, process_profile)); + raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, process_profile, manip)); } program.push_back(raster_segment); @@ -104,10 +104,10 @@ inline CompositeInstruction rasterExampleProgram(const std::string& freespace_pr CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8 + ((i + 1) * 0.1), 0.3, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)); - PlanInstruction plan_f1(wp7, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1(wp7, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1.setDescription("transition_from_end_plan"); - CompositeInstruction transition(freespace_profile); + CompositeInstruction transition(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition.setDescription("transition_from_end"); transition.push_back(plan_f1); program.push_back(transition); @@ -118,19 +118,19 @@ inline CompositeInstruction rasterExampleProgram(const std::string& freespace_pr CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8 + ((i + 1) * 0.1), -0.3, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)); - PlanInstruction plan_f1(wp1, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1(wp1, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1.setDescription("transition_from_end_plan"); - CompositeInstruction transition(freespace_profile); + CompositeInstruction transition(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition.setDescription("transition_from_end"); transition.push_back(plan_f1); program.push_back(transition); } } - PlanInstruction plan_f2(swp1, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f2(swp1, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f2.setDescription("to_end_plan"); - CompositeInstruction to_end(freespace_profile); + CompositeInstruction to_end(freespace_profile, CompositeInstructionOrder::ORDERED, manip); to_end.setDescription("to_end"); to_end.push_back(plan_f2); program.push_back(to_end); @@ -141,14 +141,14 @@ inline CompositeInstruction rasterExampleProgram(const std::string& freespace_pr inline CompositeInstruction rasterOnlyExampleProgram(const std::string& freespace_profile = DEFAULT_PROFILE_KEY, const std::string& process_profile = "PROCESS") { - CompositeInstruction program( - DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + const ManipulatorInfo manip("manipulator", "base_link", "tool0"); + CompositeInstruction program(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, manip); Waypoint wp1 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -0.3, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)); // Define start instruction - PlanInstruction start_instruction(wp1, PlanInstructionType::START); + PlanInstruction start_instruction(wp1, PlanInstructionType::START, process_profile, manip); start_instruction.setDescription("Start Instruction"); program.setStartInstruction(start_instruction); @@ -170,25 +170,25 @@ inline CompositeInstruction rasterOnlyExampleProgram(const std::string& freespac Waypoint wp7 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(x, 0.3, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)); - CompositeInstruction raster_segment(process_profile); + CompositeInstruction raster_segment(process_profile, CompositeInstructionOrder::ORDERED, manip); raster_segment.setDescription("Raster #" + std::to_string(i + 1)); if (i == 0 || i == 2) { - raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, process_profile)); + raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, process_profile, manip)); } else { - raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, process_profile)); + raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, process_profile, manip)); } program.push_back(raster_segment); @@ -199,10 +199,10 @@ inline CompositeInstruction rasterOnlyExampleProgram(const std::string& freespac CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8 + ((i + 1) * 0.1), 0.3, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)); - PlanInstruction plan_f1(wp7, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1(wp7, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1.setDescription("transition_from_end_plan"); - CompositeInstruction transition(freespace_profile); + CompositeInstruction transition(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition.setDescription("transition_from_end"); transition.push_back(plan_f1); program.push_back(transition); @@ -213,10 +213,10 @@ inline CompositeInstruction rasterOnlyExampleProgram(const std::string& freespac CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8 + ((i + 1) * 0.1), -0.3, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)); - PlanInstruction plan_f1(wp1, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1(wp1, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1.setDescription("transition_from_end_plan"); - CompositeInstruction transition(freespace_profile); + CompositeInstruction transition(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition.setDescription("transition_from_end"); transition.push_back(plan_f1); program.push_back(transition); diff --git a/tesseract_process_managers/examples/raster_manager_example.cpp b/tesseract_process_managers/examples/raster_manager_example.cpp index 53eece8042d..32f68886a15 100644 --- a/tesseract_process_managers/examples/raster_manager_example.cpp +++ b/tesseract_process_managers/examples/raster_manager_example.cpp @@ -69,13 +69,14 @@ int main() // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; request.name = process_planner_names::RASTER_G_FT_PLANNER_NAME; // Define the program - CompositeInstruction program = rasterExampleProgram(); + CompositeInstruction program = rasterExampleProgram(DEFAULT_PROFILE_KEY, DEFAULT_PROFILE_KEY); request.instructions = Instruction(program); // Print Diagnostics diff --git a/tesseract_process_managers/examples/raster_waad_dt_example_program.h b/tesseract_process_managers/examples/raster_waad_dt_example_program.h index dd92a9d79b1..59b1bf6970f 100644 --- a/tesseract_process_managers/examples/raster_waad_dt_example_program.h +++ b/tesseract_process_managers/examples/raster_waad_dt_example_program.h @@ -39,22 +39,22 @@ inline CompositeInstruction rasterWAADDTExampleProgram(const std::string& freesp const std::string& process_profile = "PROCESS", const std::string& departure_profile = "DEPARTURE") { - CompositeInstruction program( - DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + const ManipulatorInfo manip("manipulator", "base_link", "tool0"); + CompositeInstruction program(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, manip); // Start Joint Position for the program std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; StateWaypoint swp1 = StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)); - PlanInstruction start_instruction(swp1, PlanInstructionType::START); + PlanInstruction start_instruction(swp1, PlanInstructionType::START, freespace_profile, manip); program.setStartInstruction(start_instruction); Waypoint wp1 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -0.3, 0.85) * Eigen::Quaterniond(0, 0, -1.0, 0)); // Define from start composite instruction - PlanInstruction plan_f0(wp1, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f0(wp1, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f0.setDescription("from_start_plan"); - CompositeInstruction from_start(freespace_profile); + CompositeInstruction from_start(freespace_profile, CompositeInstructionOrder::ORDERED, manip); from_start.setDescription("from_start"); from_start.push_back(plan_f0); program.push_back(from_start); @@ -84,45 +84,45 @@ inline CompositeInstruction rasterWAADDTExampleProgram(const std::string& freesp Waypoint wpd = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(x, 0.3, 0.85) * Eigen::Quaterniond(0, 0, -1.0, 0)); - CompositeInstruction approach_segment(approach_profile); - CompositeInstruction process_segment(process_profile); - CompositeInstruction departure_segment(departure_profile); + CompositeInstruction approach_segment(approach_profile, CompositeInstructionOrder::ORDERED, manip); + CompositeInstruction process_segment(process_profile, CompositeInstructionOrder::ORDERED, manip); + CompositeInstruction departure_segment(departure_profile, CompositeInstructionOrder::ORDERED, manip); approach_segment.setDescription("Raster Approach #" + std::to_string(i + 1)); process_segment.setDescription("Raster Process #" + std::to_string(i + 1)); departure_segment.setDescription("Raster Departure #" + std::to_string(i + 1)); if (i == 0 || i == 2) { // Approach - approach_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, approach_profile)); + approach_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, approach_profile, manip)); // Process - process_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, process_profile)); + process_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, process_profile, manip)); // Departure - departure_segment.push_back(PlanInstruction(wpd, PlanInstructionType::LINEAR, departure_profile)); + departure_segment.push_back(PlanInstruction(wpd, PlanInstructionType::LINEAR, departure_profile, manip)); } else { // Approach - approach_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, approach_profile)); + approach_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, approach_profile, manip)); - process_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, process_profile)); + process_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, process_profile, manip)); // Departure - departure_segment.push_back(PlanInstruction(wpa, PlanInstructionType::LINEAR, departure_profile)); + departure_segment.push_back(PlanInstruction(wpa, PlanInstructionType::LINEAR, departure_profile, manip)); } - CompositeInstruction raster_segment; + CompositeInstruction raster_segment(process_profile, CompositeInstructionOrder::ORDERED, manip); raster_segment.push_back(approach_segment); raster_segment.push_back(process_segment); raster_segment.push_back(departure_segment); @@ -139,21 +139,21 @@ inline CompositeInstruction rasterWAADDTExampleProgram(const std::string& freesp CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8 + (i * 0.1), -0.3, 0.85) * Eigen::Quaterniond(0, 0, -1.0, 0)); - PlanInstruction plan_f1(wp, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1(wp, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1.setDescription("transition_from_end_plan"); - PlanInstruction plan_f1_dt(wp_dt, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1_dt(wp_dt, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1_dt.setDescription("transition_to_start_plan"); - CompositeInstruction transition_from_end(freespace_profile); + CompositeInstruction transition_from_end(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition_from_end.setDescription("transition_from_end"); transition_from_end.push_back(plan_f1); - CompositeInstruction transition_to_start(freespace_profile); + CompositeInstruction transition_to_start(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition_to_start.setDescription("transition_to_start"); transition_to_start.push_back(plan_f1_dt); - CompositeInstruction transition("transition dual", CompositeInstructionOrder::UNORDERED); + CompositeInstruction transition("transition dual", CompositeInstructionOrder::UNORDERED, manip); transition.push_back(transition_from_end); transition.push_back(transition_to_start); program.push_back(transition); @@ -168,30 +168,30 @@ inline CompositeInstruction rasterWAADDTExampleProgram(const std::string& freesp CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8 + (i * 0.1), 0.3, 0.85) * Eigen::Quaterniond(0, 0, -1.0, 0)); - PlanInstruction plan_f1(wp, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1(wp, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1.setDescription("transition_from_end_plan"); - PlanInstruction plan_f1_dt(wp_dt, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1_dt(wp_dt, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1_dt.setDescription("transition_to_start_plan"); - CompositeInstruction transition_from_end(freespace_profile); + CompositeInstruction transition_from_end(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition_from_end.setDescription("transition_from_end"); transition_from_end.push_back(plan_f1); - CompositeInstruction transition_to_start(freespace_profile); + CompositeInstruction transition_to_start(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition_to_start.setDescription("transition_to_start"); transition_to_start.push_back(plan_f1_dt); - CompositeInstruction transition("transition dual", CompositeInstructionOrder::UNORDERED); + CompositeInstruction transition("transition dual", CompositeInstructionOrder::UNORDERED, manip); transition.push_back(transition_from_end); transition.push_back(transition_to_start); program.push_back(transition); } } - PlanInstruction plan_f2(swp1, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f2(swp1, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f2.setDescription("to_end_plan"); - CompositeInstruction to_end(freespace_profile); + CompositeInstruction to_end(freespace_profile, CompositeInstructionOrder::ORDERED, manip); to_end.setDescription("to_end"); to_end.push_back(plan_f2); program.push_back(to_end); diff --git a/tesseract_process_managers/examples/raster_waad_example_program.h b/tesseract_process_managers/examples/raster_waad_example_program.h index 91807b2c7c0..8e3b59f5756 100644 --- a/tesseract_process_managers/examples/raster_waad_example_program.h +++ b/tesseract_process_managers/examples/raster_waad_example_program.h @@ -39,22 +39,22 @@ inline CompositeInstruction rasterWAADExampleProgram(const std::string& freespac const std::string& process_profile = "PROCESS", const std::string& departure_profile = "DEPARTURE") { - CompositeInstruction program( - DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + const ManipulatorInfo manip("manipulator", "base_link", "tool0"); + CompositeInstruction program(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, manip); // Start Joint Position for the program std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; StateWaypoint swp1 = StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)); - PlanInstruction start_instruction(swp1, PlanInstructionType::START); + PlanInstruction start_instruction(swp1, PlanInstructionType::START, freespace_profile, manip); program.setStartInstruction(start_instruction); Waypoint wp1 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -0.3, 0.85) * Eigen::Quaterniond(0, 0, -1.0, 0)); // Define from start composite instruction - PlanInstruction plan_f0(wp1, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f0(wp1, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f0.setDescription("from_start_plan"); - CompositeInstruction from_start(freespace_profile); + CompositeInstruction from_start(freespace_profile, CompositeInstructionOrder::ORDERED, manip); from_start.setDescription("from_start"); from_start.push_back(plan_f0); program.push_back(from_start); @@ -84,45 +84,45 @@ inline CompositeInstruction rasterWAADExampleProgram(const std::string& freespac Waypoint wpd = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(x, 0.3, 0.85) * Eigen::Quaterniond(0, 0, -1.0, 0)); - CompositeInstruction approach_segment(approach_profile); - CompositeInstruction process_segment(process_profile); - CompositeInstruction departure_segment(departure_profile); + CompositeInstruction approach_segment(approach_profile, CompositeInstructionOrder::ORDERED, manip); + CompositeInstruction process_segment(process_profile, CompositeInstructionOrder::ORDERED, manip); + CompositeInstruction departure_segment(departure_profile, CompositeInstructionOrder::ORDERED, manip); approach_segment.setDescription("Raster Approach #" + std::to_string(i + 1)); process_segment.setDescription("Raster Process #" + std::to_string(i + 1)); departure_segment.setDescription("Raster Departure #" + std::to_string(i + 1)); if (i == 0 || i == 2) { // Approach - approach_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, approach_profile)); + approach_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, approach_profile, manip)); // Process - process_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, process_profile)); + process_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, process_profile, manip)); // Departure - departure_segment.push_back(PlanInstruction(wpd, PlanInstructionType::LINEAR, departure_profile)); + departure_segment.push_back(PlanInstruction(wpd, PlanInstructionType::LINEAR, departure_profile, manip)); } else { // Approach - approach_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, approach_profile)); + approach_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, approach_profile, manip)); - process_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, process_profile)); + process_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, process_profile, manip)); // Departure - departure_segment.push_back(PlanInstruction(wpa, PlanInstructionType::LINEAR, departure_profile)); + departure_segment.push_back(PlanInstruction(wpa, PlanInstructionType::LINEAR, departure_profile, manip)); } - CompositeInstruction raster_segment; + CompositeInstruction raster_segment(process_profile, CompositeInstructionOrder::ORDERED, manip); raster_segment.push_back(approach_segment); raster_segment.push_back(process_segment); raster_segment.push_back(departure_segment); @@ -135,10 +135,10 @@ inline CompositeInstruction rasterWAADExampleProgram(const std::string& freespac CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8 + ((i + 1) * 0.1), 0.3, 0.85) * Eigen::Quaterniond(0, 0, -1.0, 0)); - PlanInstruction plan_f1(wp7, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1(wp7, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1.setDescription("transition_from_end_plan"); - CompositeInstruction transition(freespace_profile); + CompositeInstruction transition(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition.setDescription("transition_from_end"); transition.push_back(plan_f1); program.push_back(transition); @@ -149,19 +149,19 @@ inline CompositeInstruction rasterWAADExampleProgram(const std::string& freespac CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8 + ((i + 1) * 0.1), -0.3, 0.85) * Eigen::Quaterniond(0, 0, -1.0, 0)); - PlanInstruction plan_f1(wp1, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1(wp1, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1.setDescription("transition_from_end_plan"); - CompositeInstruction transition(freespace_profile); + CompositeInstruction transition(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition.setDescription("transition_from_end"); transition.push_back(plan_f1); program.push_back(transition); } } - PlanInstruction plan_f2(swp1, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f2(swp1, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f2.setDescription("to_end_plan"); - CompositeInstruction to_end(freespace_profile); + CompositeInstruction to_end(freespace_profile, CompositeInstructionOrder::ORDERED, manip); to_end.setDescription("to_end"); to_end.push_back(plan_f2); program.push_back(to_end); diff --git a/tesseract_process_managers/examples/taskflow_profiling_example.cpp b/tesseract_process_managers/examples/taskflow_profiling_example.cpp index 97d20fecc45..c906f805ccc 100644 --- a/tesseract_process_managers/examples/taskflow_profiling_example.cpp +++ b/tesseract_process_managers/examples/taskflow_profiling_example.cpp @@ -99,6 +99,7 @@ int main() // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); for (int idx = -10; idx < 10; idx++) { diff --git a/tesseract_process_managers/include/tesseract_process_managers/core/process_planning_server.h b/tesseract_process_managers/include/tesseract_process_managers/core/process_planning_server.h index 9fd828cdcf3..f559acbeeb2 100644 --- a/tesseract_process_managers/include/tesseract_process_managers/core/process_planning_server.h +++ b/tesseract_process_managers/include/tesseract_process_managers/core/process_planning_server.h @@ -115,6 +115,11 @@ class ProcessPlanningServer */ std::vector getAvailableProcessPlanners() const; + /** + * @brief Loads the default profiles for the various motion planners + */ + void loadDefaultProfiles(); + #ifndef SWIG /** * @brief Execute a process planning request. diff --git a/tesseract_process_managers/src/core/process_planning_server.cpp b/tesseract_process_managers/src/core/process_planning_server.cpp index 57e96922aec..d307ad032fa 100644 --- a/tesseract_process_managers/src/core/process_planning_server.cpp +++ b/tesseract_process_managers/src/core/process_planning_server.cpp @@ -29,19 +29,29 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include #include #include #include #include -#include -#include -#include -#include -#include -#include - -#include +// Default profiles +// Descartes +#include +// OMPL +#include +#include +#include +// Simple +#include +// Trajopt +#include +#include +#include +// Trajopt IFOPT +#include +#include namespace tesseract_planning { @@ -109,6 +119,42 @@ std::vector ProcessPlanningServer::getAvailableProcessPlanners() co return planners; } +void ProcessPlanningServer::loadDefaultProfiles() +{ + // Add TrajOpt Default profiles + profiles_->addProfile( + profile_ns::TRAJOPT_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, std::make_shared()); + profiles_->addProfile( + profile_ns::TRAJOPT_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, std::make_shared()); + profiles_->addProfile( + profile_ns::TRAJOPT_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, std::make_shared()); + + // Add TrajOpt IFOPT Default profiles + profiles_->addProfile(profile_ns::TRAJOPT_IFOPT_DEFAULT_NAMESPACE, + DEFAULT_PROFILE_KEY, + std::make_shared()); + profiles_->addProfile(profile_ns::TRAJOPT_IFOPT_DEFAULT_NAMESPACE, + DEFAULT_PROFILE_KEY, + std::make_shared()); + + // Add Descartes Default profiles + profiles_->addProfile>(profile_ns::DESCARTES_DEFAULT_NAMESPACE, + DEFAULT_PROFILE_KEY, + std::make_shared>()); + + // Add OMPL Default profiles + profiles_->planner_profiles[profile_ns::OMPL_DEFAULT_NAMESPACE][DEFAULT_PROFILE_KEY] = + std::make_shared(); + profiles_->composite_profiles[profile_ns::OMPL_DEFAULT_NAMESPACE][DEFAULT_PROFILE_KEY] = + std::make_shared(); + profiles_->waypoint_profiles[profile_ns::OMPL_DEFAULT_NAMESPACE][DEFAULT_PROFILE_KEY] = + std::make_shared(); + + // Add Simple Default profiles + profiles_->addProfile( + profile_ns::SIMPLE_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, std::make_shared()); +} + ProcessPlanningFuture ProcessPlanningServer::run(const ProcessPlanningRequest& request) { CONSOLE_BRIDGE_logInform("Tesseract Planning Server Received Request!"); diff --git a/tesseract_process_managers/test/tesseract_process_managers_unit.cpp b/tesseract_process_managers/test/tesseract_process_managers_unit.cpp index 4d4198f321b..4eeee04f21a 100644 --- a/tesseract_process_managers/test/tesseract_process_managers_unit.cpp +++ b/tesseract_process_managers/test/tesseract_process_managers_unit.cpp @@ -139,7 +139,7 @@ TEST_F(TesseractProcessManagerUnit, RasterSimpleMotionPlannerFixedSizeAssignPlan std::string freespace_profile = DEFAULT_PROFILE_KEY; std::string process_profile = "PROCESS"; - tesseract_planning::CompositeInstruction program = rasterExampleProgram(); + tesseract_planning::CompositeInstruction program = rasterExampleProgram(freespace_profile, process_profile); EXPECT_FALSE(program.getManipulatorInfo().empty()); program.setManipulatorInfo(manip); @@ -181,7 +181,7 @@ TEST_F(TesseractProcessManagerUnit, RasterSimpleMotionPlannerLVSPlanProfileTest) std::string freespace_profile = DEFAULT_PROFILE_KEY; std::string process_profile = "PROCESS"; - tesseract_planning::CompositeInstruction program = rasterExampleProgram(); + tesseract_planning::CompositeInstruction program = rasterExampleProgram(freespace_profile, process_profile); EXPECT_FALSE(program.getManipulatorInfo().empty()); program.setManipulatorInfo(manip); @@ -220,7 +220,7 @@ TEST_F(TesseractProcessManagerUnit, RasterSimpleMotionPlannerLVSPlanProfileTest) TEST_F(TesseractProcessManagerUnit, RasterSimpleMotionPlannerDefaultLVSNoIKPlanProfileTest) // NOLINT { std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; + std::string process_profile = DEFAULT_PROFILE_KEY; tesseract_planning::CompositeInstruction program = rasterExampleProgram(); EXPECT_FALSE(program.getManipulatorInfo().empty()); @@ -336,6 +336,7 @@ TEST_F(TesseractProcessManagerUnit, RasterProcessManagerDefaultPlanProfileTest) // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; @@ -343,7 +344,7 @@ TEST_F(TesseractProcessManagerUnit, RasterProcessManagerDefaultPlanProfileTest) // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; + std::string process_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterExampleProgram(freespace_profile, process_profile); request.instructions = Instruction(program); @@ -372,6 +373,7 @@ TEST_F(TesseractProcessManagerUnit, RasterProcessManagerDefaultLVSPlanProfileTes // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; @@ -379,7 +381,7 @@ TEST_F(TesseractProcessManagerUnit, RasterProcessManagerDefaultLVSPlanProfileTes // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; + std::string process_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterExampleProgram(freespace_profile, process_profile); request.instructions = Instruction(program); @@ -408,6 +410,7 @@ TEST_F(TesseractProcessManagerUnit, RasterGlobalProcessManagerDefaultPlanProfile // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; @@ -415,7 +418,7 @@ TEST_F(TesseractProcessManagerUnit, RasterGlobalProcessManagerDefaultPlanProfile // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; + std::string process_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterExampleProgram(freespace_profile, process_profile); request.instructions = Instruction(program); @@ -444,6 +447,7 @@ TEST_F(TesseractProcessManagerUnit, RasterGlobalProcessManagerDefaultLVSPlanProf // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; @@ -451,7 +455,7 @@ TEST_F(TesseractProcessManagerUnit, RasterGlobalProcessManagerDefaultLVSPlanProf // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; + std::string process_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterExampleProgram(freespace_profile, process_profile); request.instructions = Instruction(program); @@ -480,6 +484,7 @@ TEST_F(TesseractProcessManagerUnit, RasterOnlyProcessManagerDefaultPlanProfileTe // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; @@ -487,7 +492,7 @@ TEST_F(TesseractProcessManagerUnit, RasterOnlyProcessManagerDefaultPlanProfileTe // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; + std::string process_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterOnlyExampleProgram(freespace_profile, process_profile); request.instructions = Instruction(program); @@ -516,6 +521,7 @@ TEST_F(TesseractProcessManagerUnit, RasterOnlyProcessManagerDefaultLVSPlanProfil // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; @@ -523,7 +529,7 @@ TEST_F(TesseractProcessManagerUnit, RasterOnlyProcessManagerDefaultLVSPlanProfil // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; + std::string process_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterOnlyExampleProgram(freespace_profile, process_profile); request.instructions = Instruction(program); @@ -552,6 +558,7 @@ TEST_F(TesseractProcessManagerUnit, RasterOnlyGlobalProcessManagerDefaultPlanPro // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; @@ -559,7 +566,7 @@ TEST_F(TesseractProcessManagerUnit, RasterOnlyGlobalProcessManagerDefaultPlanPro // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; + std::string process_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterOnlyExampleProgram(freespace_profile, process_profile); request.instructions = Instruction(program); @@ -588,6 +595,7 @@ TEST_F(TesseractProcessManagerUnit, RasterOnlyGlobalProcessManagerDefaultLVSPlan // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; @@ -595,7 +603,7 @@ TEST_F(TesseractProcessManagerUnit, RasterOnlyGlobalProcessManagerDefaultLVSPlan // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; + std::string process_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterOnlyExampleProgram(freespace_profile, process_profile); request.instructions = Instruction(program); @@ -624,6 +632,7 @@ TEST_F(TesseractProcessManagerUnit, RasterDTProcessManagerDefaultPlanProfileTest // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; @@ -631,7 +640,7 @@ TEST_F(TesseractProcessManagerUnit, RasterDTProcessManagerDefaultPlanProfileTest // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; + std::string process_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterDTExampleProgram(freespace_profile, process_profile); request.instructions = Instruction(program); @@ -660,6 +669,7 @@ TEST_F(TesseractProcessManagerUnit, RasterDTProcessManagerDefaultLVSPlanProfileT // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; @@ -667,7 +677,7 @@ TEST_F(TesseractProcessManagerUnit, RasterDTProcessManagerDefaultLVSPlanProfileT // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; + std::string process_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterDTExampleProgram(freespace_profile, process_profile); request.instructions = Instruction(program); @@ -696,6 +706,7 @@ TEST_F(TesseractProcessManagerUnit, RasterWAADProcessManagerDefaultPlanProfileTe // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; @@ -703,9 +714,9 @@ TEST_F(TesseractProcessManagerUnit, RasterWAADProcessManagerDefaultPlanProfileTe // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string approach_profile = "APPROACH"; - std::string process_profile = "PROCESS"; - std::string departure_profile = "DEPARTURE"; + std::string approach_profile = DEFAULT_PROFILE_KEY; + std::string process_profile = DEFAULT_PROFILE_KEY; + std::string departure_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterWAADExampleProgram(freespace_profile, approach_profile, process_profile, departure_profile); @@ -739,6 +750,7 @@ TEST_F(TesseractProcessManagerUnit, RasterWAADProcessManagerDefaultLVSPlanProfil // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; @@ -746,9 +758,9 @@ TEST_F(TesseractProcessManagerUnit, RasterWAADProcessManagerDefaultLVSPlanProfil // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string approach_profile = "APPROACH"; - std::string process_profile = "PROCESS"; - std::string departure_profile = "DEPARTURE"; + std::string approach_profile = DEFAULT_PROFILE_KEY; + std::string process_profile = DEFAULT_PROFILE_KEY; + std::string departure_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterWAADExampleProgram(freespace_profile, approach_profile, process_profile, departure_profile); @@ -782,6 +794,7 @@ TEST_F(TesseractProcessManagerUnit, RasterWAADDTProcessManagerDefaultPlanProfile // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; @@ -789,9 +802,9 @@ TEST_F(TesseractProcessManagerUnit, RasterWAADDTProcessManagerDefaultPlanProfile // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string approach_profile = "APPROACH"; - std::string process_profile = "PROCESS"; - std::string departure_profile = "DEPARTURE"; + std::string approach_profile = DEFAULT_PROFILE_KEY; + std::string process_profile = DEFAULT_PROFILE_KEY; + std::string departure_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterWAADDTExampleProgram(freespace_profile, approach_profile, process_profile, departure_profile); @@ -825,6 +838,7 @@ TEST_F(TesseractProcessManagerUnit, RasterWAADDTProcessManagerDefaultLVSPlanProf // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; @@ -832,9 +846,9 @@ TEST_F(TesseractProcessManagerUnit, RasterWAADDTProcessManagerDefaultLVSPlanProf // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string approach_profile = "APPROACH"; - std::string process_profile = "PROCESS"; - std::string departure_profile = "DEPARTURE"; + std::string approach_profile = DEFAULT_PROFILE_KEY; + std::string process_profile = DEFAULT_PROFILE_KEY; + std::string departure_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterWAADDTExampleProgram(freespace_profile, approach_profile, process_profile, departure_profile);