From 4878e0518ea1596c961b309e7893f94ad2796953 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Mon, 23 Dec 2024 11:13:02 -0600 Subject: [PATCH 1/7] Removed OMPL state extractor concept --- .../ompl/continuous_motion_validator.h | 7 +- .../ompl/profile/ompl_profile.h | 16 +- .../profile/ompl_real_vector_plan_profile.h | 16 +- .../ompl/state_collision_validator.h | 8 +- .../tesseract_motion_planners/ompl/types.h | 44 ------ .../tesseract_motion_planners/ompl/utils.h | 41 +++-- .../ompl/src/continuous_motion_validator.cpp | 9 +- .../ompl/src/ompl_motion_planner.cpp | 147 +----------------- .../profile/ompl_real_vector_plan_profile.cpp | 69 +++++--- .../ompl/src/state_collision_validator.cpp | 6 +- tesseract_motion_planners/ompl/src/utils.cpp | 112 +++++++++---- 11 files changed, 183 insertions(+), 292 deletions(-) delete mode 100644 tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/types.h diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/continuous_motion_validator.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/continuous_motion_validator.h index fd7d501833d..5ca4fb75a51 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/continuous_motion_validator.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/continuous_motion_validator.h @@ -34,7 +34,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include #include #include #include @@ -57,8 +56,7 @@ class ContinuousMotionValidator : public ompl::base::MotionValidator ompl::base::StateValidityCheckerPtr state_validator, const tesseract_environment::Environment& env, std::shared_ptr manip, - const tesseract_collision::CollisionCheckConfig& collision_check_config, - OMPLStateExtractor extractor); + const tesseract_collision::CollisionCheckConfig& collision_check_config); bool checkMotion(const ompl::base::State* s1, const ompl::base::State* s2) const override; @@ -91,9 +89,6 @@ class ContinuousMotionValidator : public ompl::base::MotionValidator /** @brief A list of active links */ std::vector links_; - /** @brief This will extract an Eigen::VectorXd from the OMPL State */ - OMPLStateExtractor extractor_; - // The items below are to cache the contact manager based on thread ID. Currently ompl is multi // threaded but the methods used to implement collision checking are not thread safe. To prevent // reconstructing the collision environment for every check this will cache a contact manager diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h index c4e337e3304..50c8af4d97b 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h @@ -33,8 +33,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include - #include #include #include @@ -45,6 +43,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace ompl::geometric { class SimpleSetup; +class PathGeometric; } namespace tesseract_planning @@ -72,12 +71,6 @@ class OMPLPlanProfile : public Profile */ virtual std::unique_ptr createSolverConfig() const = 0; - /** - * @brief Create the state extractor - * @return The OMPL state extractor - */ - virtual OMPLStateExtractor createStateExtractor(const tesseract_kinematics::JointGroup& manip) const = 0; - /** * @brief Create OMPL Simple Setup * @param start_instruction The start instruction @@ -92,6 +85,13 @@ class OMPLPlanProfile : public Profile const tesseract_common::ManipulatorInfo& composite_mi, const std::shared_ptr& env) const = 0; + /** + * @brief Convert an OMPL defined path into a composite instruction that can be returned by the planner + */ + virtual CompositeInstruction convertPath(const ompl::geometric::PathGeometric& path, + const tesseract_common::ManipulatorInfo& composite_mi, + const std::shared_ptr& env) const = 0; + protected: friend class boost::serialization::access; template diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h index 5115e6116ef..477839b5286 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h @@ -76,14 +76,16 @@ class OMPLRealVectorPlanProfile : public OMPLPlanProfile std::unique_ptr createSolverConfig() const override; - OMPLStateExtractor createStateExtractor(const tesseract_kinematics::JointGroup& manip) const override; - std::unique_ptr createSimpleSetup(const MoveInstructionPoly& start_instruction, const MoveInstructionPoly& end_instruction, const tesseract_common::ManipulatorInfo& composite_mi, const std::shared_ptr& env) const override; + CompositeInstruction convertPath(const ompl::geometric::PathGeometric& path, + const tesseract_common::ManipulatorInfo& composite_mi, + const std::shared_ptr& env) const override; + protected: static void applyGoalStates(ompl::geometric::SimpleSetup& simple_setup, const tesseract_kinematics::KinGroupIKInput& ik_input, @@ -126,8 +128,7 @@ class OMPLRealVectorPlanProfile : public OMPLPlanProfile virtual std::unique_ptr createStateValidator(const ompl::geometric::SimpleSetup& simple_setup, const std::shared_ptr& env, - const std::shared_ptr& manip, - const OMPLStateExtractor& state_extractor) const; + const std::shared_ptr& manip) const; /** * @brief Create collision state validator @@ -140,8 +141,7 @@ class OMPLRealVectorPlanProfile : public OMPLPlanProfile virtual std::unique_ptr createCollisionStateValidator(const ompl::geometric::SimpleSetup& simple_setup, const std::shared_ptr& env, - const std::shared_ptr& manip, - const OMPLStateExtractor& state_extractor) const; + const std::shared_ptr& manip) const; /** * @brief Create motion validator @@ -155,7 +155,6 @@ class OMPLRealVectorPlanProfile : public OMPLPlanProfile createMotionValidator(const ompl::geometric::SimpleSetup& simple_setup, const std::shared_ptr& env, const std::shared_ptr& manip, - const OMPLStateExtractor& state_extractor, const std::shared_ptr& svc_without_collision) const; /** @@ -169,8 +168,7 @@ class OMPLRealVectorPlanProfile : public OMPLPlanProfile virtual std::unique_ptr createOptimizationObjective(const ompl::geometric::SimpleSetup& simple_setup, const std::shared_ptr& env, - const std::shared_ptr& manip, - const OMPLStateExtractor& state_extractor) const; + const std::shared_ptr& manip) const; friend class boost::serialization::access; template diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/state_collision_validator.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/state_collision_validator.h index a69f6121b9b..8cd474dfdb1 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/state_collision_validator.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/state_collision_validator.h @@ -34,8 +34,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include - #include #include #include @@ -55,8 +53,7 @@ class StateCollisionValidator : public ompl::base::StateValidityChecker StateCollisionValidator(const ompl::base::SpaceInformationPtr& space_info, const tesseract_environment::Environment& env, std::shared_ptr manip, - const tesseract_collision::CollisionCheckConfig& collision_check_config, - OMPLStateExtractor extractor); + const tesseract_collision::CollisionCheckConfig& collision_check_config); bool isValid(const ompl::base::State* state) const override; @@ -70,9 +67,6 @@ class StateCollisionValidator : public ompl::base::StateValidityChecker /** @brief A list of active links */ std::vector links_; - /** @brief This will extract an Eigen::VectorXd from the OMPL State */ - OMPLStateExtractor extractor_; - // The items below are to cache the contact manager based on thread ID. Currently ompl is multi // threaded but the methods used to implement collision checking are not thread safe. To prevent // reconstructing the collision environment for every check this will cache a contact manager 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 857885d8a10..00000000000 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/types.h +++ /dev/null @@ -1,44 +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 -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -namespace ompl::base -{ -class State; -} - -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 cf40b56186a..70c92a80474 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 @@ -33,11 +33,10 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include - #include #include #include +#include namespace ompl::base { @@ -55,21 +54,6 @@ class PathGeometric; namespace tesseract_planning { -Eigen::Map RealVectorStateSpaceExtractor(const ompl::base::State* s1, unsigned dimension); - -#ifndef OMPL_LESS_1_4_0 -Eigen::Map ConstrainedStateSpaceExtractor(const ompl::base::State* s1); -#endif - -/** - * @brief Convert an ompl path to tesseract TrajArray - * @param path OMPL Path - * @param extractor This function understands the type of state space and converts it to an eigen vector. - * @return Tesseract TrajArray - */ -tesseract_common::TrajArray toTrajArray(const ompl::geometric::PathGeometric& path, - const OMPLStateExtractor& extractor); - /** * @brief Given longest valid fraction and length it will set the correct information of the state space * @param state_space_ptr OMPL State Space @@ -111,6 +95,29 @@ ompl::base::StateSamplerPtr allocWeightedRealVectorStateSampler(const ompl::base const Eigen::VectorXd& weights, const Eigen::MatrixX2d& limits); +/** + * @brief Converts an OMPL state into a vector of doubles + * @param s1 OMPL state + * @param dimension Size of the state (e.g., number of joints) + * @return + */ +Eigen::Map fromRealVectorStateSpace(const ompl::base::State* s1, unsigned dimension); + +/** + * @brief Converts + * @param path + * @return + */ +tesseract_common::TrajArray fromRealVectorStateSpace(const ompl::geometric::PathGeometric& path); + +// long assignTrajectory(tesseract_planning::CompositeInstruction& output, +// boost::uuids::uuid start_uuid, +// boost::uuids::uuid end_uuid, +// long start_index, +// const std::vector& joint_names, +// const tesseract_common::TrajArray& traj, +// const bool format_result_as_input); + } // namespace tesseract_planning #endif // TESSERACT_MOTION_PLANNERS_OMPL_UTILS_H diff --git a/tesseract_motion_planners/ompl/src/continuous_motion_validator.cpp b/tesseract_motion_planners/ompl/src/continuous_motion_validator.cpp index 45c3451e7ce..01e5b22ff9f 100644 --- a/tesseract_motion_planners/ompl/src/continuous_motion_validator.cpp +++ b/tesseract_motion_planners/ompl/src/continuous_motion_validator.cpp @@ -31,6 +31,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include #include #include @@ -44,13 +45,11 @@ ContinuousMotionValidator::ContinuousMotionValidator( ompl::base::StateValidityCheckerPtr state_validator, const tesseract_environment::Environment& env, std::shared_ptr manip, - const tesseract_collision::CollisionCheckConfig& collision_check_config, - OMPLStateExtractor extractor) + const tesseract_collision::CollisionCheckConfig& collision_check_config) : MotionValidator(space_info) , state_validator_(std::move(state_validator)) , manip_(std::move(manip)) , continuous_contact_manager_(env.getContinuousContactManager()) - , extractor_(std::move(extractor)) { links_ = manip_->getActiveLinkNames(); @@ -139,8 +138,8 @@ bool ContinuousMotionValidator::continuousCollisionCheck(const ompl::base::State } mutex_.unlock(); - Eigen::Map start_joints = extractor_(s1); - Eigen::Map finish_joints = extractor_(s2); + Eigen::Map start_joints = fromRealVectorStateSpace(s1, si_->getStateDimension()); + Eigen::Map finish_joints = fromRealVectorStateSpace(s2, si_->getStateDimension()); tesseract_common::TransformMap state0 = manip_->calcFwdKin(start_joints); tesseract_common::TransformMap state1 = manip_->calcFwdKin(finish_joints); diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index db6dc4c5c54..7f090d2bb36 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -38,7 +38,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include #include #include #include @@ -60,43 +59,6 @@ using CachedSimpleSetupsPtr = std::shared_ptr; namespace tesseract_planning { -bool checkStartState(const ompl::base::ProblemDefinitionPtr& prob_def, - const Eigen::Ref& state, - const OMPLStateExtractor& extractor) -{ - if (!(prob_def->getStartStateCount() >= 1)) - return false; - - for (unsigned i = 0; i < prob_def->getStartStateCount(); ++i) - if (extractor(prob_def->getStartState(i)).isApprox(state, 1e-5)) - return true; - - return false; -} - -bool checkGoalState(const ompl::base::ProblemDefinitionPtr& prob_def, - const Eigen::Ref& state, - const OMPLStateExtractor& extractor) -{ - ompl::base::GoalPtr goal = prob_def->getGoal(); - if (goal->getType() == ompl::base::GoalType::GOAL_STATE) - return extractor(prob_def->getGoal()->as()->getState()).isApprox(state, 1e-5); - - if (goal->getType() == ompl::base::GoalType::GOAL_STATES) - { - auto* goal_states = prob_def->getGoal()->as(); - for (unsigned i = 0; i < goal_states->getStateCount(); ++i) - if (extractor(goal_states->getState(i)).isApprox(state, 1e-5)) - return true; - } - else - { - CONSOLE_BRIDGE_logWarn("checkGoalStates: Unsupported Goal Type!"); - return true; - } - return false; -} - /** @brief Construct a basic planner */ OMPLMotionPlanner::OMPLMotionPlanner(std::string name) : MotionPlanner(std::move(name)) {} @@ -202,69 +164,6 @@ std::pair parallelPlan(ompl::geometric::SimpleSetup& simple_s return std::make_pair(true, "SUCCESS"); } -long OMPLMotionPlanner::assignTrajectory(tesseract_planning::CompositeInstruction& output, - boost::uuids::uuid start_uuid, - boost::uuids::uuid end_uuid, - long start_index, - const std::vector& joint_names, - const tesseract_common::TrajArray& traj, - const bool format_result_as_input) -{ - bool found{ false }; - Eigen::Index row{ 0 }; - auto& ci = output.getInstructions(); - for (auto it = ci.begin() + static_cast(start_index); it != ci.end(); ++it) - { - if (it->isMoveInstruction()) - { - auto& mi = it->as(); - if (mi.getUUID() == start_uuid) - found = true; - - if (mi.getUUID() == end_uuid) - { - std::vector extra; - for (; row < traj.rows() - 1; ++row) - { - MoveInstructionPoly child = mi.createChild(); - if (format_result_as_input) - { - JointWaypointPoly jwp = mi.createJointWaypoint(); - jwp.setIsConstrained(false); - jwp.setNames(joint_names); - jwp.setPosition(traj.row(row)); - child.assignJointWaypoint(jwp); - } - else - { - StateWaypointPoly swp = mi.createStateWaypoint(); - swp.setNames(joint_names); - swp.setPosition(traj.row(row)); - child.assignStateWaypoint(swp); - } - - extra.emplace_back(child); - } - - assignSolution(mi, joint_names, traj.row(row), format_result_as_input); - - if (!extra.empty()) - ci.insert(it, extra.begin(), extra.end()); - - start_index += static_cast(extra.size()); - break; - } - - if (found) - assignSolution(mi, joint_names, traj.row(row++), format_result_as_input); - } - - ++start_index; - } - - return start_index; -} - PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const { PlannerResponse response; @@ -280,7 +179,7 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const // Assume all the plan instructions have the same manipulator as the composite tesseract_common::ManipulatorInfo composite_mi = request.instructions.getManipulatorInfo(); - assert(!composite_mi.empty()); + // assert(!composite_mi.empty()); // Flatten the input for planning auto move_instructions = request.instructions.flatten(&moveFilter); @@ -293,14 +192,9 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const cached_simple_setups.reserve(move_instructions.size()); // Transform plan instructions into ompl problem - unsigned num_output_states = 1; - long start_index{ 0 }; - std::size_t segment{ 1 }; - std::reference_wrapper start_instruction = move_instructions.front(); for (std::size_t i = 1; i < move_instructions.size(); ++i) { - ++num_output_states; - + std::reference_wrapper start_instruction = move_instructions[i - 1]; std::reference_wrapper end_instruction = move_instructions[i]; const auto& end_move_instruction = end_instruction.get().as(); const auto& waypoint = end_move_instruction.getWaypoint(); @@ -318,15 +212,13 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const // Get end state kinematics data tesseract_common::ManipulatorInfo end_mi = composite_mi.getCombined(end_move_instruction.getManipulatorInfo()); - tesseract_kinematics::JointGroup::UPtr manip = request.env->getJointGroup(end_mi.manipulator); // Create problem data const auto& start_move_instruction = start_instruction.get().as(); std::unique_ptr solver_config = cur_plan_profile->createSolverConfig(); - OMPLStateExtractor extractor = cur_plan_profile->createStateExtractor(*manip); std::shared_ptr simple_setup; - if (cached_simple_setups.empty() || segment > cached_simple_setups.size()) + if (cached_simple_setups.empty() || i > cached_simple_setups.size()) { simple_setup = cur_plan_profile->createSimpleSetup(start_move_instruction, end_move_instruction, composite_mi, request.env); @@ -334,11 +226,11 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const } else { - simple_setup = cached_simple_setups.at(segment - 1); + simple_setup = cached_simple_setups.at(i - 1); } // Parallel Plan problem - auto status = parallelPlan(*simple_setup, *solver_config, num_output_states); + auto status = parallelPlan(*simple_setup, *solver_config, 2); if (!status.first) { response.successful = false; @@ -348,33 +240,10 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const } // Extract Solution - const std::vector joint_names = manip->getJointNames(); - const Eigen::MatrixX2d joint_limits = manip->getLimits().joint_limits; - tesseract_common::TrajArray traj = toTrajArray(simple_setup->getSolutionPath(), extractor); - assert(checkStartState(simple_setup->getProblemDefinition(), traj.row(0), extractor)); - assert(checkGoalState(simple_setup->getProblemDefinition(), traj.bottomRows(1).transpose(), extractor)); - assert(traj.rows() >= num_output_states); - - // Enforce limits - for (Eigen::Index i = 0; i < traj.rows(); i++) - { - assert(tesseract_common::satisfiesLimits(traj.row(i), joint_limits, 1e-4)); - tesseract_common::enforceLimits(traj.row(i), joint_limits); - } + CompositeInstruction output = cur_plan_profile->convertPath(simple_setup->getSolutionPath(), composite_mi, request.env); - // Assign trajectory to results - start_index = assignTrajectory(response.results, - start_move_instruction.getUUID(), - end_move_instruction.getUUID(), - start_index, - joint_names, - traj, - request.format_result_as_input); - - // Reset data for next segment - start_instruction = end_instruction; - num_output_states = 1; - ++segment; + // Concatenate into results + std::copy(response.results.end(), output.begin(), output.end()); } response.successful = true; diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp index e77b80db7b0..61ae63332a8 100644 --- a/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp +++ b/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp @@ -38,7 +38,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + #include +#include +#include +#include #include #include @@ -71,14 +76,6 @@ std::unique_ptr OMPLRealVectorPlanProfile::createSolverConfig( return std::make_unique(solver_config); } -OMPLStateExtractor OMPLRealVectorPlanProfile::createStateExtractor(const tesseract_kinematics::JointGroup& manip) const -{ - const auto dof = static_cast(manip.numJoints()); - return [dof](const ompl::base::State* state) -> Eigen::Map { - return tesseract_planning::RealVectorStateSpaceExtractor(state, dof); - }; -} - std::unique_ptr OMPLRealVectorPlanProfile::createSimpleSetup(const MoveInstructionPoly& start_instruction, const MoveInstructionPoly& end_instruction, @@ -113,31 +110,28 @@ OMPLRealVectorPlanProfile::createSimpleSetup(const MoveInstructionPoly& start_in // Create Simple Setup from state space auto simple_setup = std::make_unique(state_space_ptr); - // Create state extractor - OMPLStateExtractor state_extractor = createStateExtractor(*manip); - // Setup state validators auto csvc = std::make_shared(); ompl::base::StateValidityCheckerPtr svc_without_collision = - createStateValidator(*simple_setup, env, manip, state_extractor); + createStateValidator(*simple_setup, env, manip); if (svc_without_collision != nullptr) csvc->addStateValidator(svc_without_collision); - auto svc_collision = createCollisionStateValidator(*simple_setup, env, manip, state_extractor); + auto svc_collision = createCollisionStateValidator(*simple_setup, env, manip); if (svc_collision != nullptr) csvc->addStateValidator(std::move(svc_collision)); simple_setup->setStateValidityChecker(csvc); // Setup motion validation (i.e. collision checking) - auto mv = createMotionValidator(*simple_setup, env, manip, state_extractor, svc_without_collision); + auto mv = createMotionValidator(*simple_setup, env, manip, svc_without_collision); if (mv != nullptr) simple_setup->getSpaceInformation()->setMotionValidator(std::move(mv)); // make sure the planners run until the time limit, and get the best possible solution if (solver_config.optimize) { - auto obj = createOptimizationObjective(*simple_setup, env, manip, state_extractor); + auto obj = createOptimizationObjective(*simple_setup, env, manip); if (obj != nullptr) simple_setup->getProblemDefinition()->setOptimizationObjective(std::move(obj)); } @@ -434,8 +428,7 @@ OMPLRealVectorPlanProfile::createStateSamplerAllocator( std::unique_ptr OMPLRealVectorPlanProfile::createStateValidator( const ompl::geometric::SimpleSetup& /*simple_setup*/, const std::shared_ptr& /*env*/, - const std::shared_ptr& /*manip*/, - const OMPLStateExtractor& /*state_extractor*/) const + const std::shared_ptr& /*manip*/) const { return nullptr; } @@ -443,14 +436,13 @@ std::unique_ptr OMPLRealVectorPlanProfile::cre std::unique_ptr OMPLRealVectorPlanProfile::createCollisionStateValidator( const ompl::geometric::SimpleSetup& simple_setup, const std::shared_ptr& env, - const std::shared_ptr& manip, - const OMPLStateExtractor& state_extractor) const + const std::shared_ptr& manip) const { if (collision_check_config.type == tesseract_collision::CollisionEvaluatorType::DISCRETE || collision_check_config.type == tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE) { return std::make_unique( - simple_setup.getSpaceInformation(), *env, manip, collision_check_config, state_extractor); + simple_setup.getSpaceInformation(), *env, manip, collision_check_config); } return nullptr; @@ -460,7 +452,6 @@ std::unique_ptr OMPLRealVectorPlanProfile::createMo const ompl::geometric::SimpleSetup& simple_setup, const std::shared_ptr& env, const std::shared_ptr& manip, - const OMPLStateExtractor& state_extractor, const std::shared_ptr& svc_without_collision) const { if (collision_check_config.type != tesseract_collision::CollisionEvaluatorType::NONE) @@ -472,8 +463,7 @@ std::unique_ptr OMPLRealVectorPlanProfile::createMo svc_without_collision, *env, manip, - collision_check_config, - state_extractor); + collision_check_config); } // Collision checking is preformed using the state validator which this calls. @@ -486,12 +476,41 @@ std::unique_ptr OMPLRealVectorPlanProfile::createMo std::unique_ptr OMPLRealVectorPlanProfile::createOptimizationObjective( const ompl::geometric::SimpleSetup& simple_setup, const std::shared_ptr& /*env*/, - const std::shared_ptr& /*manip*/, - const OMPLStateExtractor& /*state_extractor*/) const + const std::shared_ptr& /*manip*/) const { return std::make_unique(simple_setup.getSpaceInformation()); } +CompositeInstruction OMPLRealVectorPlanProfile::convertPath(const ompl::geometric::PathGeometric& path, + const tesseract_common::ManipulatorInfo& composite_mi, + const std::shared_ptr& env) const +{ + tesseract_common::TrajArray traj = fromRealVectorStateSpace(path); + + // Get kinematics + tesseract_kinematics::JointGroup::Ptr manip = env->getJointGroup(composite_mi.manipulator); + const std::vector joint_names = manip->getJointNames(); + const Eigen::MatrixX2d limits = manip->getLimits().joint_limits; + + CompositeInstruction output; + output.reserve(static_cast(traj.rows())); + + for (Eigen::Index i = 0; i < traj.rows(); i++) + { + // Enforce limits + assert(tesseract_common::satisfiesLimits(traj.row(i), limits, 1e-4)); + tesseract_common::enforceLimits(traj.row(i), limits); + + // Convert to move instruction + MoveInstruction mi(StateWaypointPoly(StateWaypoint(joint_names, traj.row(i))), MoveInstructionType::FREESPACE); + + // Append to composite instruction + output.push_back(mi); + } + + return output; +} + template void OMPLRealVectorPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) { diff --git a/tesseract_motion_planners/ompl/src/state_collision_validator.cpp b/tesseract_motion_planners/ompl/src/state_collision_validator.cpp index 00cba119007..e56f64f6bc1 100644 --- a/tesseract_motion_planners/ompl/src/state_collision_validator.cpp +++ b/tesseract_motion_planners/ompl/src/state_collision_validator.cpp @@ -44,12 +44,10 @@ StateCollisionValidator::StateCollisionValidator( const ompl::base::SpaceInformationPtr& space_info, const tesseract_environment::Environment& env, std::shared_ptr manip, - const tesseract_collision::CollisionCheckConfig& collision_check_config, - OMPLStateExtractor extractor) + const tesseract_collision::CollisionCheckConfig& collision_check_config) : StateValidityChecker(space_info) , manip_(std::move(manip)) , contact_manager_(env.getDiscreteContactManager()) - , extractor_(std::move(extractor)) { links_ = manip_->getActiveLinkNames(); @@ -75,7 +73,7 @@ bool StateCollisionValidator::isValid(const ompl::base::State* state) const } mutex_.unlock(); - Eigen::Map finish_joints = extractor_(state); + Eigen::Map finish_joints = fromRealVectorStateSpace(state, si_->getStateDimension()); tesseract_common::TransformMap state1 = manip_->calcFwdKin(finish_joints); for (const auto& link_name : links_) diff --git a/tesseract_motion_planners/ompl/src/utils.cpp b/tesseract_motion_planners/ompl/src/utils.cpp index 36cbde39cf2..a24e3cf5967 100644 --- a/tesseract_motion_planners/ompl/src/utils.cpp +++ b/tesseract_motion_planners/ompl/src/utils.cpp @@ -40,6 +40,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include + #include #include @@ -47,34 +49,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { -Eigen::Map RealVectorStateSpaceExtractor(const ompl::base::State* s1, unsigned dimension) -{ - assert(dynamic_cast(s1) != nullptr); - const auto* s = s1->template as(); - return Eigen::Map{ s->values, dimension }; -} - -#ifndef OMPL_LESS_1_4_0 -Eigen::Map ConstrainedStateSpaceExtractor(const ompl::base::State* s1) -{ - assert(dynamic_cast(s1) != nullptr); - const Eigen::Map& s = *(s1->template as()); - return s; -} -#endif - -tesseract_common::TrajArray toTrajArray(const ompl::geometric::PathGeometric& path, const OMPLStateExtractor& extractor) -{ - const auto n_points = static_cast(path.getStateCount()); - const auto dof = static_cast(path.getSpaceInformation()->getStateDimension()); - - tesseract_common::TrajArray result(n_points, dof); - for (long i = 0; i < n_points; ++i) - result.row(i) = extractor(path.getState(static_cast(i))); - - return result; -} - void processLongestValidSegment(const ompl::base::StateSpacePtr& state_space_ptr, double longest_valid_segment_fraction, double longest_valid_segment_length) @@ -129,4 +103,86 @@ ompl::base::StateSamplerPtr allocWeightedRealVectorStateSampler(const ompl::base return std::make_shared(space, weights, limits); } +Eigen::Map fromRealVectorStateSpace(const ompl::base::State* s1, unsigned dimension) +{ + assert(dynamic_cast(s1) != nullptr); + const auto* s = s1->template as(); + return Eigen::Map{ s->values, dimension }; +} + +tesseract_common::TrajArray fromRealVectorStateSpace(const ompl::geometric::PathGeometric& path) +{ + const auto n_points = static_cast(path.getStateCount()); + const auto dof = static_cast(path.getSpaceInformation()->getStateDimension()); + + tesseract_common::TrajArray result(n_points, dof); + for (long i = 0; i < n_points; ++i) + result.row(i) = fromRealVectorStateSpace(path.getState(static_cast(i)), dof); + + return result; +} + +// long assignTrajectory(tesseract_planning::CompositeInstruction& output, +// boost::uuids::uuid start_uuid, +// boost::uuids::uuid end_uuid, +// long start_index, +// const std::vector& joint_names, +// const tesseract_common::TrajArray& traj, +// const bool format_result_as_input) +// { +// bool found{ false }; +// Eigen::Index row{ 0 }; +// auto& ci = output.getInstructions(); +// for (auto it = ci.begin() + static_cast(start_index); it != ci.end(); ++it) +// { +// if (it->isMoveInstruction()) +// { +// auto& mi = it->as(); +// if (mi.getUUID() == start_uuid) +// found = true; + +// if (mi.getUUID() == end_uuid) +// { +// std::vector extra; +// for (; row < traj.rows() - 1; ++row) +// { +// MoveInstructionPoly child = mi.createChild(); +// if (format_result_as_input) +// { +// JointWaypointPoly jwp = mi.createJointWaypoint(); +// jwp.setIsConstrained(false); +// jwp.setNames(joint_names); +// jwp.setPosition(traj.row(row)); +// child.assignJointWaypoint(jwp); +// } +// else +// { +// StateWaypointPoly swp = mi.createStateWaypoint(); +// swp.setNames(joint_names); +// swp.setPosition(traj.row(row)); +// child.assignStateWaypoint(swp); +// } + +// extra.emplace_back(child); +// } + +// assignSolution(mi, joint_names, traj.row(row), format_result_as_input); + +// if (!extra.empty()) +// ci.insert(it, extra.begin(), extra.end()); + +// start_index += static_cast(extra.size()); +// break; +// } + +// if (found) +// assignSolution(mi, joint_names, traj.row(row++), format_result_as_input); +// } + +// ++start_index; +// } + +// return start_index; +// } + } // namespace tesseract_planning From 46848408447a28ac3dd5d15cb11567886c0f2f3d Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Thu, 26 Dec 2024 11:24:43 -0600 Subject: [PATCH 2/7] Moved RVSS components to separate subdirectory --- tesseract_motion_planners/ompl/CMakeLists.txt | 7 +- .../continuous_motion_validator.h | 6 +- .../state_collision_validator.h | 6 +- .../ompl/real_vector_state_space/utils.h | 65 +++++++++++++++++++ .../weighted_real_vector_state_sampler.h | 4 +- .../tesseract_motion_planners/ompl/utils.h | 26 -------- .../profile/ompl_real_vector_plan_profile.cpp | 9 +-- .../continuous_motion_validator.cpp | 4 +- .../state_collision_validator.cpp | 4 +- .../src/real_vector_state_space/utils.cpp | 64 ++++++++++++++++++ .../weighted_real_vector_state_sampler.cpp | 2 +- tesseract_motion_planners/ompl/src/utils.cpp | 27 -------- 12 files changed, 151 insertions(+), 73 deletions(-) rename tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/{ => real_vector_state_space}/continuous_motion_validator.h (93%) rename tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/{ => real_vector_state_space}/state_collision_validator.h (91%) create mode 100644 tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/utils.h rename tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/{ => real_vector_state_space}/weighted_real_vector_state_sampler.h (94%) rename tesseract_motion_planners/ompl/src/{ => real_vector_state_space}/continuous_motion_validator.cpp (97%) rename tesseract_motion_planners/ompl/src/{ => real_vector_state_space}/state_collision_validator.cpp (94%) create mode 100644 tesseract_motion_planners/ompl/src/real_vector_state_space/utils.cpp rename tesseract_motion_planners/ompl/src/{ => real_vector_state_space}/weighted_real_vector_state_sampler.cpp (97%) diff --git a/tesseract_motion_planners/ompl/CMakeLists.txt b/tesseract_motion_planners/ompl/CMakeLists.txt index 220864a8eef..475291fa4b9 100644 --- a/tesseract_motion_planners/ompl/CMakeLists.txt +++ b/tesseract_motion_planners/ompl/CMakeLists.txt @@ -7,14 +7,15 @@ set(OMPL_SRC src/profile/ompl_profile.cpp src/profile/ompl_real_vector_plan_profile.cpp src/compound_state_validator.cpp - src/continuous_motion_validator.cpp src/discrete_motion_validator.cpp src/ompl_motion_planner.cpp src/ompl_planner_configurator.cpp src/ompl_solver_config.cpp - src/state_collision_validator.cpp src/utils.cpp - src/weighted_real_vector_state_sampler.cpp) + src/real_vector_state_space/continuous_motion_validator.cpp + src/real_vector_state_space/state_collision_validator.cpp + src/real_vector_state_space/weighted_real_vector_state_sampler.cpp + src/real_vector_state_space/utils.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/continuous_motion_validator.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/continuous_motion_validator.h similarity index 93% rename from tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/continuous_motion_validator.h rename to tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/continuous_motion_validator.h index 5ca4fb75a51..5a3b00386e2 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/continuous_motion_validator.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/continuous_motion_validator.h @@ -23,8 +23,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TESSERACT_MOTION_PLANNERS_CONTINUOUS_MOTION_VALIDATOR_H -#define TESSERACT_MOTION_PLANNERS_CONTINUOUS_MOTION_VALIDATOR_H +#ifndef TESSERACT_MOTION_PLANNERS_OMPL_REAL_VECTOR_STATE_SPACE_CONTINUOUS_MOTION_VALIDATOR_H +#define TESSERACT_MOTION_PLANNERS_OMPL_REAL_VECTOR_STATE_SPACE_CONTINUOUS_MOTION_VALIDATOR_H #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH @@ -103,4 +103,4 @@ class ContinuousMotionValidator : public ompl::base::MotionValidator }; } // namespace tesseract_planning -#endif // TESSERACT_MOTION_PLANNERS_CONTINUOUS_MOTION_VALIDATOR_H +#endif // TESSERACT_MOTION_PLANNERS_OMPL_REAL_VECTOR_STATE_SPACE_CONTINUOUS_MOTION_VALIDATOR_H diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/state_collision_validator.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/state_collision_validator.h similarity index 91% rename from tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/state_collision_validator.h rename to tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/state_collision_validator.h index 8cd474dfdb1..d036b3bd81a 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/state_collision_validator.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/state_collision_validator.h @@ -23,8 +23,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TESSERACT_MOTION_PLANNERS_OMPL_STATE_COLLISION_VALIDATOR_H -#define TESSERACT_MOTION_PLANNERS_OMPL_STATE_COLLISION_VALIDATOR_H +#ifndef TESSERACT_MOTION_PLANNERS_OMPL_REAL_VECTOR_STATE_SPACE_STATE_COLLISION_VALIDATOR_H +#define TESSERACT_MOTION_PLANNERS_OMPL_REAL_VECTOR_STATE_SPACE_STATE_COLLISION_VALIDATOR_H #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH @@ -80,4 +80,4 @@ class StateCollisionValidator : public ompl::base::StateValidityChecker }; } // namespace tesseract_planning -#endif // TESSERACT_MOTION_PLANNERS_OMPL_STATE_COLLISION_VALIDATOR_H +#endif // TESSERACT_MOTION_PLANNERS_OMPL_REAL_VECTOR_STATE_SPACE_STATE_COLLISION_VALIDATOR_H diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/utils.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/utils.h new file mode 100644 index 00000000000..54ab7badaf5 --- /dev/null +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/utils.h @@ -0,0 +1,65 @@ +/** + * @file utils.h + * @brief Tesseract OMPL real vector state space utility functions + * + * @author Levi Armstrong + * @date February 17, 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_REAL_VECTOR_STATE_SPACE_UTILS_H +#define TESSERACT_MOTION_PLANNERS_OMPL_REAL_VECTOR_STATE_SPACE_UTILS_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace ompl::base +{ +class State; +} // namespace ompl::base + +namespace ompl::geometric +{ +class PathGeometric; +} + +namespace tesseract_planning +{ +/** + * @brief Converts an OMPL state into a vector of doubles + * @param s1 OMPL state + * @param dimension Size of the state (e.g., number of joints) + * @return + */ +Eigen::Map fromRealVectorStateSpace(const ompl::base::State* s1, unsigned dimension); + +/** + * @brief Converts + * @param path + * @return + */ +tesseract_common::TrajArray fromRealVectorStateSpace(const ompl::geometric::PathGeometric& path); + +} // namespace tesseract_planning + +#endif // TESSERACT_MOTION_PLANNERS_OMPL_REAL_VECTOR_STATE_SPACE_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/real_vector_state_space/weighted_real_vector_state_sampler.h similarity index 94% rename from tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/weighted_real_vector_state_sampler.h rename to tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/weighted_real_vector_state_sampler.h index cc7999943f8..a5dc7161a19 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/real_vector_state_space/weighted_real_vector_state_sampler.h @@ -26,8 +26,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TESSERACT_MOTION_PLANNERS_WEIGHTED_REAL_VECTOR_STATE_SAMPLER_H -#define TESSERACT_MOTION_PLANNERS_WEIGHTED_REAL_VECTOR_STATE_SAMPLER_H +#ifndef TESSERACT_MOTION_PLANNERS_OMPL_WEIGHTED_REAL_VECTOR_STATE_SAMPLER_H +#define TESSERACT_MOTION_PLANNERS_OMPL_WEIGHTED_REAL_VECTOR_STATE_SAMPLER_H #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH 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 70c92a80474..9904020c43e 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 @@ -40,7 +40,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace ompl::base { -class State; class StateSpace; using StateSpacePtr = std::shared_ptr; class StateSampler; @@ -85,31 +84,6 @@ bool checkStateInCollision(tesseract_collision::ContactResultMap& contact_map, const tesseract_kinematics::JointGroup& manip, const Eigen::VectorXd& state); -/** - * @brief Default State sampler which uses the weights information to scale the sampled state. This is use full - * 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); - -/** - * @brief Converts an OMPL state into a vector of doubles - * @param s1 OMPL state - * @param dimension Size of the state (e.g., number of joints) - * @return - */ -Eigen::Map fromRealVectorStateSpace(const ompl::base::State* s1, unsigned dimension); - -/** - * @brief Converts - * @param path - * @return - */ -tesseract_common::TrajArray fromRealVectorStateSpace(const ompl::geometric::PathGeometric& path); - // long assignTrajectory(tesseract_planning::CompositeInstruction& output, // boost::uuids::uuid start_uuid, // boost::uuids::uuid end_uuid, diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp index 61ae63332a8..c5741e304cf 100644 --- a/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp +++ b/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp @@ -49,12 +49,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include - #include -#include #include -#include #include +#include +#include +#include +#include #include #include @@ -421,7 +422,7 @@ OMPLRealVectorPlanProfile::createStateSamplerAllocator( Eigen::MatrixX2d limits = manip->getLimits().joint_limits; Eigen::VectorXd weights = Eigen::VectorXd::Ones(manip->numJoints()); return [weights, limits](const ompl::base::StateSpace* state_space) -> ompl::base::StateSamplerPtr { - return allocWeightedRealVectorStateSampler(state_space, weights, limits); + return std::make_shared(state_space, weights, limits); }; } diff --git a/tesseract_motion_planners/ompl/src/continuous_motion_validator.cpp b/tesseract_motion_planners/ompl/src/real_vector_state_space/continuous_motion_validator.cpp similarity index 97% rename from tesseract_motion_planners/ompl/src/continuous_motion_validator.cpp rename to tesseract_motion_planners/ompl/src/real_vector_state_space/continuous_motion_validator.cpp index 01e5b22ff9f..7e4fe690288 100644 --- a/tesseract_motion_planners/ompl/src/continuous_motion_validator.cpp +++ b/tesseract_motion_planners/ompl/src/real_vector_state_space/continuous_motion_validator.cpp @@ -30,8 +30,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include -#include +#include +#include #include #include diff --git a/tesseract_motion_planners/ompl/src/state_collision_validator.cpp b/tesseract_motion_planners/ompl/src/real_vector_state_space/state_collision_validator.cpp similarity index 94% rename from tesseract_motion_planners/ompl/src/state_collision_validator.cpp rename to tesseract_motion_planners/ompl/src/real_vector_state_space/state_collision_validator.cpp index e56f64f6bc1..a8bf437e42d 100644 --- a/tesseract_motion_planners/ompl/src/state_collision_validator.cpp +++ b/tesseract_motion_planners/ompl/src/real_vector_state_space/state_collision_validator.cpp @@ -30,8 +30,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include -#include +#include +#include #include #include diff --git a/tesseract_motion_planners/ompl/src/real_vector_state_space/utils.cpp b/tesseract_motion_planners/ompl/src/real_vector_state_space/utils.cpp new file mode 100644 index 00000000000..7a75c2dede7 --- /dev/null +++ b/tesseract_motion_planners/ompl/src/real_vector_state_space/utils.cpp @@ -0,0 +1,64 @@ +/** + * @file utils.cpp + * @brief Tesseract OMPL real vector state space utility functions + * + * @author Levi Armstrong + * @date February 17, 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 + +// #ifndef OMPL_LESS_1_4_0 +// #include +// #endif + +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +#include + +namespace tesseract_planning +{ +Eigen::Map fromRealVectorStateSpace(const ompl::base::State* s1, unsigned dimension) +{ + assert(dynamic_cast(s1) != nullptr); + const auto* s = s1->template as(); + return Eigen::Map{ s->values, dimension }; +} + +tesseract_common::TrajArray fromRealVectorStateSpace(const ompl::geometric::PathGeometric& path) +{ + const auto n_points = static_cast(path.getStateCount()); + const auto dof = static_cast(path.getSpaceInformation()->getStateDimension()); + + tesseract_common::TrajArray result(n_points, dof); + for (long i = 0; i < n_points; ++i) + result.row(i) = fromRealVectorStateSpace(path.getState(static_cast(i)), dof); + + return result; +} + +} // namespace tesseract_planning diff --git a/tesseract_motion_planners/ompl/src/weighted_real_vector_state_sampler.cpp b/tesseract_motion_planners/ompl/src/real_vector_state_space/weighted_real_vector_state_sampler.cpp similarity index 97% rename from tesseract_motion_planners/ompl/src/weighted_real_vector_state_sampler.cpp rename to tesseract_motion_planners/ompl/src/real_vector_state_space/weighted_real_vector_state_sampler.cpp index 9694ffcddd9..6b4ad84f661 100644 --- a/tesseract_motion_planners/ompl/src/weighted_real_vector_state_sampler.cpp +++ b/tesseract_motion_planners/ompl/src/real_vector_state_space/weighted_real_vector_state_sampler.cpp @@ -33,7 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include +#include namespace tesseract_planning { diff --git a/tesseract_motion_planners/ompl/src/utils.cpp b/tesseract_motion_planners/ompl/src/utils.cpp index a24e3cf5967..667237672ca 100644 --- a/tesseract_motion_planners/ompl/src/utils.cpp +++ b/tesseract_motion_planners/ompl/src/utils.cpp @@ -38,7 +38,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include #include @@ -96,32 +95,6 @@ bool checkStateInCollision(tesseract_collision::ContactResultMap& contact_map, return (!contact_map.empty()); } -ompl::base::StateSamplerPtr allocWeightedRealVectorStateSampler(const ompl::base::StateSpace* space, - const Eigen::VectorXd& weights, - const Eigen::MatrixX2d& limits) -{ - return std::make_shared(space, weights, limits); -} - -Eigen::Map fromRealVectorStateSpace(const ompl::base::State* s1, unsigned dimension) -{ - assert(dynamic_cast(s1) != nullptr); - const auto* s = s1->template as(); - return Eigen::Map{ s->values, dimension }; -} - -tesseract_common::TrajArray fromRealVectorStateSpace(const ompl::geometric::PathGeometric& path) -{ - const auto n_points = static_cast(path.getStateCount()); - const auto dof = static_cast(path.getSpaceInformation()->getStateDimension()); - - tesseract_common::TrajArray result(n_points, dof); - for (long i = 0; i < n_points; ++i) - result.row(i) = fromRealVectorStateSpace(path.getState(static_cast(i)), dof); - - return result; -} - // long assignTrajectory(tesseract_planning::CompositeInstruction& output, // boost::uuids::uuid start_uuid, // boost::uuids::uuid end_uuid, From 74d7ddae1f2f824d28e001ec1c82201b325fa9ad Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Thu, 26 Dec 2024 11:52:19 -0600 Subject: [PATCH 3/7] Added state converter function back to RVSS motion and state validators --- .../continuous_motion_validator.h | 8 ++++++- .../state_collision_validator.h | 8 ++++++- .../ompl/real_vector_state_space/utils.h | 22 +++++++++++++++---- .../profile/ompl_real_vector_plan_profile.cpp | 7 +++--- .../continuous_motion_validator.cpp | 9 ++++---- .../state_collision_validator.cpp | 7 +++--- .../src/real_vector_state_space/utils.cpp | 22 +++++++++++++------ 7 files changed, 60 insertions(+), 23 deletions(-) diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/continuous_motion_validator.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/continuous_motion_validator.h index 5a3b00386e2..814df604d93 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/continuous_motion_validator.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/continuous_motion_validator.h @@ -38,6 +38,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include + namespace ompl::base { class SpaceInformation; @@ -56,7 +58,8 @@ class ContinuousMotionValidator : public ompl::base::MotionValidator ompl::base::StateValidityCheckerPtr state_validator, const tesseract_environment::Environment& env, std::shared_ptr manip, - const tesseract_collision::CollisionCheckConfig& collision_check_config); + const tesseract_collision::CollisionCheckConfig& collision_check_config, + StateConverterFn state_converter); bool checkMotion(const ompl::base::State* s1, const ompl::base::State* s2) const override; @@ -89,6 +92,9 @@ class ContinuousMotionValidator : public ompl::base::MotionValidator /** @brief A list of active links */ std::vector links_; + /** @brief Function to convert an OMPL state (typically of type RealVectorStateSpace::StateType or ConstrainedStateSpace::StateType) into a vector of doubles representing a joint state */ + StateConverterFn state_converter_; + // The items below are to cache the contact manager based on thread ID. Currently ompl is multi // threaded but the methods used to implement collision checking are not thread safe. To prevent // reconstructing the collision environment for every check this will cache a contact manager diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/state_collision_validator.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/state_collision_validator.h index d036b3bd81a..c3053055de8 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/state_collision_validator.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/state_collision_validator.h @@ -38,6 +38,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include + namespace ompl::base { class SpaceInformation; @@ -53,7 +55,8 @@ class StateCollisionValidator : public ompl::base::StateValidityChecker StateCollisionValidator(const ompl::base::SpaceInformationPtr& space_info, const tesseract_environment::Environment& env, std::shared_ptr manip, - const tesseract_collision::CollisionCheckConfig& collision_check_config); + const tesseract_collision::CollisionCheckConfig& collision_check_config, + StateConverterFn state_converter); bool isValid(const ompl::base::State* state) const override; @@ -67,6 +70,9 @@ class StateCollisionValidator : public ompl::base::StateValidityChecker /** @brief A list of active links */ std::vector links_; + /** @brief Function to convert an OMPL state (typically of type RealVectorStateSpace::StateType or ConstrainedStateSpace::StateType) into a vector of doubles representing a joint state */ + StateConverterFn state_converter_; + // The items below are to cache the contact manager based on thread ID. Currently ompl is multi // threaded but the methods used to implement collision checking are not thread safe. To prevent // reconstructing the collision environment for every check this will cache a contact manager diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/utils.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/utils.h index 54ab7badaf5..7c8abd93f2e 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/utils.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/real_vector_state_space/utils.h @@ -29,6 +29,7 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -46,19 +47,32 @@ class PathGeometric; namespace tesseract_planning { /** - * @brief Converts an OMPL state into a vector of doubles + * @brief Typedef for a function that can convert an OMPL base state object into a vector of contiguous double values + */ +using StateConverterFn = std::function(const ompl::base::State*, unsigned dim)>; + +/** + * @brief Implementation of StateConverterFn that converts an OMPL state into a vector of doubles by casting through a RealVectorStateSpace::StateType OMPL state * @param s1 OMPL state * @param dimension Size of the state (e.g., number of joints) * @return */ Eigen::Map fromRealVectorStateSpace(const ompl::base::State* s1, unsigned dimension); +#ifndef OMPL_LESS_1_4_0 /** - * @brief Converts - * @param path + * @brief Implementation of StateConverterFn that converts an OMPL state into a vector of doubles by casting through a ProjectedStateSpace::StateType OMPL state + * @param s1 OMPL state + * @param dimension Size of the state (e.g., number of joints) * @return */ -tesseract_common::TrajArray fromRealVectorStateSpace(const ompl::geometric::PathGeometric& path); +Eigen::Map fromConstrainedRealVectorStateSpace(const ompl::base::State* s1, unsigned dimension); +#endif + +/** + * @brief Converts an OMPL path into a trajectory array by using the input state converter to convert the path waypoints into joint state vectors + */ +tesseract_common::TrajArray fromOMPL(const ompl::geometric::PathGeometric& path, StateConverterFn state_converter); } // namespace tesseract_planning diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp index c5741e304cf..f99e9ee2903 100644 --- a/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp +++ b/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp @@ -443,7 +443,7 @@ std::unique_ptr OMPLRealVectorPlanProfile::cre collision_check_config.type == tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE) { return std::make_unique( - simple_setup.getSpaceInformation(), *env, manip, collision_check_config); + simple_setup.getSpaceInformation(), *env, manip, collision_check_config, &fromRealVectorStateSpace); } return nullptr; @@ -464,7 +464,8 @@ std::unique_ptr OMPLRealVectorPlanProfile::createMo svc_without_collision, *env, manip, - collision_check_config); + collision_check_config, + &fromRealVectorStateSpace); } // Collision checking is preformed using the state validator which this calls. @@ -486,7 +487,7 @@ CompositeInstruction OMPLRealVectorPlanProfile::convertPath(const ompl::geometri const tesseract_common::ManipulatorInfo& composite_mi, const std::shared_ptr& env) const { - tesseract_common::TrajArray traj = fromRealVectorStateSpace(path); + tesseract_common::TrajArray traj = fromOMPL(path, &fromRealVectorStateSpace); // Get kinematics tesseract_kinematics::JointGroup::Ptr manip = env->getJointGroup(composite_mi.manipulator); diff --git a/tesseract_motion_planners/ompl/src/real_vector_state_space/continuous_motion_validator.cpp b/tesseract_motion_planners/ompl/src/real_vector_state_space/continuous_motion_validator.cpp index 7e4fe690288..12e859be78f 100644 --- a/tesseract_motion_planners/ompl/src/real_vector_state_space/continuous_motion_validator.cpp +++ b/tesseract_motion_planners/ompl/src/real_vector_state_space/continuous_motion_validator.cpp @@ -31,7 +31,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include #include #include @@ -45,11 +44,13 @@ ContinuousMotionValidator::ContinuousMotionValidator( ompl::base::StateValidityCheckerPtr state_validator, const tesseract_environment::Environment& env, std::shared_ptr manip, - const tesseract_collision::CollisionCheckConfig& collision_check_config) + const tesseract_collision::CollisionCheckConfig& collision_check_config, + StateConverterFn state_converter) : MotionValidator(space_info) , state_validator_(std::move(state_validator)) , manip_(std::move(manip)) , continuous_contact_manager_(env.getContinuousContactManager()) + , state_converter_(state_converter) { links_ = manip_->getActiveLinkNames(); @@ -138,8 +139,8 @@ bool ContinuousMotionValidator::continuousCollisionCheck(const ompl::base::State } mutex_.unlock(); - Eigen::Map start_joints = fromRealVectorStateSpace(s1, si_->getStateDimension()); - Eigen::Map finish_joints = fromRealVectorStateSpace(s2, si_->getStateDimension()); + Eigen::Map start_joints = state_converter_(s1, si_->getStateDimension()); + Eigen::Map finish_joints = state_converter_(s2, si_->getStateDimension()); tesseract_common::TransformMap state0 = manip_->calcFwdKin(start_joints); tesseract_common::TransformMap state1 = manip_->calcFwdKin(finish_joints); diff --git a/tesseract_motion_planners/ompl/src/real_vector_state_space/state_collision_validator.cpp b/tesseract_motion_planners/ompl/src/real_vector_state_space/state_collision_validator.cpp index a8bf437e42d..bd50347397e 100644 --- a/tesseract_motion_planners/ompl/src/real_vector_state_space/state_collision_validator.cpp +++ b/tesseract_motion_planners/ompl/src/real_vector_state_space/state_collision_validator.cpp @@ -31,7 +31,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include #include #include @@ -44,10 +43,12 @@ StateCollisionValidator::StateCollisionValidator( const ompl::base::SpaceInformationPtr& space_info, const tesseract_environment::Environment& env, std::shared_ptr manip, - const tesseract_collision::CollisionCheckConfig& collision_check_config) + const tesseract_collision::CollisionCheckConfig& collision_check_config, + StateConverterFn state_converter) : StateValidityChecker(space_info) , manip_(std::move(manip)) , contact_manager_(env.getDiscreteContactManager()) + , state_converter_(state_converter) { links_ = manip_->getActiveLinkNames(); @@ -73,7 +74,7 @@ bool StateCollisionValidator::isValid(const ompl::base::State* state) const } mutex_.unlock(); - Eigen::Map finish_joints = fromRealVectorStateSpace(state, si_->getStateDimension()); + Eigen::Map finish_joints = state_converter_(state, si_->getStateDimension()); tesseract_common::TransformMap state1 = manip_->calcFwdKin(finish_joints); for (const auto& link_name : links_) diff --git a/tesseract_motion_planners/ompl/src/real_vector_state_space/utils.cpp b/tesseract_motion_planners/ompl/src/real_vector_state_space/utils.cpp index 7a75c2dede7..71f0858377d 100644 --- a/tesseract_motion_planners/ompl/src/real_vector_state_space/utils.cpp +++ b/tesseract_motion_planners/ompl/src/real_vector_state_space/utils.cpp @@ -30,16 +30,14 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include -// #ifndef OMPL_LESS_1_4_0 -// #include -// #endif +#ifndef OMPL_LESS_1_4_0 +#include +#endif TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include - namespace tesseract_planning { Eigen::Map fromRealVectorStateSpace(const ompl::base::State* s1, unsigned dimension) @@ -49,14 +47,24 @@ Eigen::Map fromRealVectorStateSpace(const ompl::base::State* s1 return Eigen::Map{ s->values, dimension }; } -tesseract_common::TrajArray fromRealVectorStateSpace(const ompl::geometric::PathGeometric& path) +#ifndef OMPL_LESS_1_4_0 +Eigen::Map fromConstrainedRealVectorStateSpace(const ompl::base::State* s1, unsigned /*dimension*/) +{ + assert(dynamic_cast(s1) != nullptr); + const Eigen::Map& s = *(s1->template as()); + return s; +} +#endif + +tesseract_common::TrajArray fromRealVectorStateSpace(const ompl::geometric::PathGeometric& path, + StateConverterFn state_converter) { const auto n_points = static_cast(path.getStateCount()); const auto dof = static_cast(path.getSpaceInformation()->getStateDimension()); tesseract_common::TrajArray result(n_points, dof); for (long i = 0; i < n_points; ++i) - result.row(i) = fromRealVectorStateSpace(path.getState(static_cast(i)), dof); + result.row(i) = state_converter(path.getState(static_cast(i)), dof); return result; } From e2a756db56ef98e75680fbfe6c9905ee0a28cea7 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Thu, 26 Dec 2024 11:52:31 -0600 Subject: [PATCH 4/7] Remove unused include --- tesseract_motion_planners/ompl/src/utils.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/tesseract_motion_planners/ompl/src/utils.cpp b/tesseract_motion_planners/ompl/src/utils.cpp index 667237672ca..dd5f51c5782 100644 --- a/tesseract_motion_planners/ompl/src/utils.cpp +++ b/tesseract_motion_planners/ompl/src/utils.cpp @@ -31,10 +31,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include -#ifndef OMPL_LESS_1_4_0 -#include -#endif - TESSERACT_COMMON_IGNORE_WARNINGS_POP #include From 8cbdbcef97860fc02f18d3fd5018438aa8f87f85 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Thu, 26 Dec 2024 11:54:25 -0600 Subject: [PATCH 5/7] Added function to OMPL profile for generating state space object independently of the simple setup --- .../ompl/profile/ompl_profile.h | 13 ++++++ .../profile/ompl_real_vector_plan_profile.h | 5 ++- .../profile/ompl_real_vector_plan_profile.cpp | 42 +++++++++++-------- 3 files changed, 41 insertions(+), 19 deletions(-) diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h index 50c8af4d97b..93157054572 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h @@ -40,6 +40,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +namespace ompl::base +{ +class StateSpace; +using StateSpacePtr = std::shared_ptr; +} + namespace ompl::geometric { class SimpleSetup; @@ -93,6 +99,13 @@ class OMPLPlanProfile : public Profile const std::shared_ptr& env) const = 0; protected: + /** + * @brief Creates an OMPL StateSpace object that con be used internally to construct the SimpleSetup object + * @return + */ + ompl::base::StateSpacePtr virtual createStateSpace(const tesseract_common::ManipulatorInfo& composite_mi, + const std::shared_ptr& env) const = 0; + friend class boost::serialization::access; template void serialize(Archive&, const unsigned int); // NOLINT diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h index 477839b5286..fde4850b32f 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h @@ -46,8 +46,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace ompl::base { +class State; class StateSampler; -class StateSpace; class StateValidityChecker; class MotionValidator; class OptimizationObjective; @@ -87,6 +87,9 @@ class OMPLRealVectorPlanProfile : public OMPLPlanProfile const std::shared_ptr& env) const override; protected: + ompl::base::StateSpacePtr createStateSpace(const tesseract_common::ManipulatorInfo& composite_mi, + const std::shared_ptr& env) const override; + static void applyGoalStates(ompl::geometric::SimpleSetup& simple_setup, const tesseract_kinematics::KinGroupIKInput& ik_input, const tesseract_kinematics::KinematicGroup& manip, diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp index f99e9ee2903..5f43b2b07b7 100644 --- a/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp +++ b/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp @@ -47,6 +47,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include + #include #include #include @@ -77,6 +78,28 @@ std::unique_ptr OMPLRealVectorPlanProfile::createSolverConfig( return std::make_unique(solver_config); } +ompl::base::StateSpacePtr OMPLRealVectorPlanProfile::createStateSpace(const tesseract_common::ManipulatorInfo& mi, + const std::shared_ptr& env) const +{ + // Get kinematics + tesseract_kinematics::JointGroup::Ptr manip = env->getJointGroup(mi.manipulator); + const auto dof = static_cast(manip->numJoints()); + const std::vector joint_names = manip->getJointNames(); + const Eigen::MatrixX2d limits = manip->getLimits().joint_limits; + + // Construct the OMPL state space for this manipulator + auto rvss = std::make_shared(); + for (unsigned i = 0; i < dof; ++i) + rvss->addDimension(joint_names[i], limits(i, 0), limits(i, 1)); + + rvss->setStateSamplerAllocator(createStateSamplerAllocator(env, manip)); + + // Setup Longest Valid Segment + processLongestValidSegment(rvss, collision_check_config); + + return rvss; +} + std::unique_ptr OMPLRealVectorPlanProfile::createSimpleSetup(const MoveInstructionPoly& start_instruction, const MoveInstructionPoly& end_instruction, @@ -90,26 +113,9 @@ OMPLRealVectorPlanProfile::createSimpleSetup(const MoveInstructionPoly& start_in // Get kinematics tesseract_kinematics::JointGroup::Ptr manip = env->getJointGroup(end_mi.manipulator); - const auto dof = static_cast(manip->numJoints()); - const std::vector joint_names = manip->getJointNames(); - const Eigen::MatrixX2d limits = manip->getLimits().joint_limits; - - // Construct the OMPL state space for this manipulator - ompl::base::StateSpacePtr state_space_ptr; - - auto rss = std::make_shared(); - for (unsigned i = 0; i < dof; ++i) - rss->addDimension(joint_names[i], limits(i, 0), limits(i, 1)); - - rss->setStateSamplerAllocator(createStateSamplerAllocator(env, manip)); - - state_space_ptr = rss; - - // Setup Longest Valid Segment - processLongestValidSegment(state_space_ptr, collision_check_config); // Create Simple Setup from state space - auto simple_setup = std::make_unique(state_space_ptr); + auto simple_setup = std::make_unique(createStateSpace(end_mi, env)); // Setup state validators auto csvc = std::make_shared(); From 27f376ee4309099ee309d87cf2acdcfcca1663f1 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Thu, 26 Dec 2024 12:28:23 -0600 Subject: [PATCH 6/7] Added state converter object in RVSS profile --- .../ompl/profile/ompl_real_vector_plan_profile.h | 5 +++++ .../ompl/src/profile/ompl_real_vector_plan_profile.cpp | 7 ++++--- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h index fde4850b32f..f651d5a7445 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h @@ -36,6 +36,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include #include @@ -173,10 +174,14 @@ class OMPLRealVectorPlanProfile : public OMPLPlanProfile const std::shared_ptr& env, const std::shared_ptr& manip) const; + /** @brief Function used to convert OMPL states into joint states */ + StateConverterFn state_converter_; + friend class boost::serialization::access; template void serialize(Archive&, const unsigned int); // NOLINT }; + } // namespace tesseract_planning BOOST_CLASS_EXPORT_KEY(tesseract_planning::OMPLRealVectorPlanProfile) diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp index 5f43b2b07b7..bfdf9009a62 100644 --- a/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp +++ b/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp @@ -68,6 +68,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { OMPLRealVectorPlanProfile::OMPLRealVectorPlanProfile() + : state_converter_(&fromRealVectorStateSpace) { solver_config.planners = { std::make_shared(), std::make_shared() }; @@ -449,7 +450,7 @@ std::unique_ptr OMPLRealVectorPlanProfile::cre collision_check_config.type == tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE) { return std::make_unique( - simple_setup.getSpaceInformation(), *env, manip, collision_check_config, &fromRealVectorStateSpace); + simple_setup.getSpaceInformation(), *env, manip, collision_check_config, state_converter_); } return nullptr; @@ -471,7 +472,7 @@ std::unique_ptr OMPLRealVectorPlanProfile::createMo *env, manip, collision_check_config, - &fromRealVectorStateSpace); + state_converter_); } // Collision checking is preformed using the state validator which this calls. @@ -493,7 +494,7 @@ CompositeInstruction OMPLRealVectorPlanProfile::convertPath(const ompl::geometri const tesseract_common::ManipulatorInfo& composite_mi, const std::shared_ptr& env) const { - tesseract_common::TrajArray traj = fromOMPL(path, &fromRealVectorStateSpace); + tesseract_common::TrajArray traj = fromOMPL(path, state_converter_); // Get kinematics tesseract_kinematics::JointGroup::Ptr manip = env->getJointGroup(composite_mi.manipulator); From 792626f18de813f0d0883b3d57680c84fc30d433 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Thu, 26 Dec 2024 12:28:44 -0600 Subject: [PATCH 7/7] Added a constrained RVSS plan profile --- tesseract_motion_planners/ompl/CMakeLists.txt | 1 + .../ompl_constrained_rvss_plan_profile.h | 70 ++++++++++++++++ .../ompl_constrained_rvss_plan_profile.cpp | 79 +++++++++++++++++++ 3 files changed, 150 insertions(+) create mode 100644 tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_constrained_rvss_plan_profile.h create mode 100644 tesseract_motion_planners/ompl/src/profile/ompl_constrained_rvss_plan_profile.cpp diff --git a/tesseract_motion_planners/ompl/CMakeLists.txt b/tesseract_motion_planners/ompl/CMakeLists.txt index 475291fa4b9..1835e305c61 100644 --- a/tesseract_motion_planners/ompl/CMakeLists.txt +++ b/tesseract_motion_planners/ompl/CMakeLists.txt @@ -6,6 +6,7 @@ link_directories(BEFORE ${OMPL_LIBRARY_DIRS}) set(OMPL_SRC src/profile/ompl_profile.cpp src/profile/ompl_real_vector_plan_profile.cpp + src/profile/ompl_constrained_rvss_plan_profile.cpp src/compound_state_validator.cpp src/discrete_motion_validator.cpp src/ompl_motion_planner.cpp diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_constrained_rvss_plan_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_constrained_rvss_plan_profile.h new file mode 100644 index 00000000000..ff41061c7ef --- /dev/null +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_constrained_rvss_plan_profile.h @@ -0,0 +1,70 @@ +/** + * @file ompl_constrained_rvss_plan_profile.h + * @brief Tesseract OMPL constrained real vector state space plan profile + * + * @author Michael Ripperger + * @date December 26, 2024 + * @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_PROFILE_CONSTRAINED_RVSS_PLAN_PROFILE_H +#define TESSERACT_MOTION_PLANNERS_OMPL_PROFILE_CONSTRAINED_RVSS_PLAN_PROFILE_H + +#ifndef OMPL_LESS_1_4_0 + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace ompl::base +{ +class Constraint; +using ConstraintPtr = std::shared_ptr; +} + +namespace tesseract_planning +{ +class OMPLConstrainedRvssPlanProfile : public OMPLRealVectorPlanProfile +{ +public: + OMPLConstrainedRvssPlanProfile(); + +protected: + virtual std::vector createConstraints(const tesseract_common::ManipulatorInfo& mi, + const std::shared_ptr& env) const; + + ompl::base::StateSpacePtr createStateSpace(const tesseract_common::ManipulatorInfo& mi, + const std::shared_ptr& env) const override; + + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT +}; + +} // namespace tesseract_planning + +BOOST_CLASS_EXPORT_KEY(tesseract_planning::OMPLConstrainedRvssPlanProfile) + +#endif // OMPL_LESS_1_4_0 + +#endif // TESSERACT_MOTION_PLANNERS_OMPL_PROFILE_CONSTRAINED_RVSS_PLAN_PROFILE_H diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_constrained_rvss_plan_profile.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_constrained_rvss_plan_profile.cpp new file mode 100644 index 00000000000..efe9900aa80 --- /dev/null +++ b/tesseract_motion_planners/ompl/src/profile/ompl_constrained_rvss_plan_profile.cpp @@ -0,0 +1,79 @@ +/** + * @file ompl_constrained_rvss_plan_profile.cpp + * @brief + * + * @author Michael Ripperger + * @date December 26, 2024 + * @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 OMPL_LESS_1_4_0 + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +#include + +namespace tesseract_planning +{ +OMPLConstrainedRvssPlanProfile::OMPLConstrainedRvssPlanProfile() + : OMPLRealVectorPlanProfile() +{ + // Update the state converter function + state_converter_ = &fromConstrainedRealVectorStateSpace; +} + +std::vector OMPLConstrainedRvssPlanProfile::createConstraints(const tesseract_common::ManipulatorInfo& /*mi*/, + const std::shared_ptr& /*env*/) const +{ + return {}; +} + +ompl::base::StateSpacePtr OMPLConstrainedRvssPlanProfile::createStateSpace(const tesseract_common::ManipulatorInfo& mi, + const std::shared_ptr& env) const +{ + // Create the ambient real vector state space + ompl::base::StateSpacePtr rvss = OMPLRealVectorPlanProfile::createStateSpace(mi, env); + + // Create the constraints + auto constraints = std::make_shared(rvss->getDimension(), createConstraints(mi, env)); + + // Create the projected state space + return std::make_shared(rvss, constraints); +} + +template +void OMPLConstrainedRvssPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLRealVectorPlanProfile); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::OMPLConstrainedRvssPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::OMPLConstrainedRvssPlanProfile) + +#endif // OMPL_LESS_1_4_0