From 792626f18de813f0d0883b3d57680c84fc30d433 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Thu, 26 Dec 2024 12:28:44 -0600 Subject: [PATCH] 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 475291fa4b..1835e305c6 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 0000000000..ff41061c7e --- /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 0000000000..efe9900aa8 --- /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