Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft: Revise OMPL planner and profile to support different types of state spaces #555

Open
wants to merge 7 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Added a constrained RVSS plan profile
  • Loading branch information
marip8 committed Dec 26, 2024
commit 792626f18de813f0d0883b3d57680c84fc30d433
1 change: 1 addition & 0 deletions tesseract_motion_planners/ompl/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Original file line number Diff line number Diff line change
@@ -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/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <vector>
#include <memory>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h>

namespace ompl::base
{
class Constraint;
using ConstraintPtr = std::shared_ptr<Constraint>;
}

namespace tesseract_planning
{
class OMPLConstrainedRvssPlanProfile : public OMPLRealVectorPlanProfile
{
public:
OMPLConstrainedRvssPlanProfile();

protected:
virtual std::vector<ompl::base::ConstraintPtr> createConstraints(const tesseract_common::ManipulatorInfo& mi,
const std::shared_ptr<const tesseract_environment::Environment>& env) const;

ompl::base::StateSpacePtr createStateSpace(const tesseract_common::ManipulatorInfo& mi,
const std::shared_ptr<const tesseract_environment::Environment>& env) const override;

friend class boost::serialization::access;
template <class Archive>
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
Original file line number Diff line number Diff line change
@@ -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/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/base/spaces/constraint/ProjectedStateSpace.h>
#include <boost/serialization/base_object.hpp>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_motion_planners/ompl/profile/ompl_constrained_rvss_plan_profile.h>

#include <tesseract_environment/environment.h>

namespace tesseract_planning
{
OMPLConstrainedRvssPlanProfile::OMPLConstrainedRvssPlanProfile()
: OMPLRealVectorPlanProfile()
{
// Update the state converter function
state_converter_ = &fromConstrainedRealVectorStateSpace;
}

std::vector<ompl::base::ConstraintPtr> OMPLConstrainedRvssPlanProfile::createConstraints(const tesseract_common::ManipulatorInfo& /*mi*/,
const std::shared_ptr<const tesseract_environment::Environment>& /*env*/) const
{
return {};
}

ompl::base::StateSpacePtr OMPLConstrainedRvssPlanProfile::createStateSpace(const tesseract_common::ManipulatorInfo& mi,
const std::shared_ptr<const tesseract_environment::Environment>& 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<ompl::base::ConstraintIntersection>(rvss->getDimension(), createConstraints(mi, env));

// Create the projected state space
return std::make_shared<ompl::base::ProjectedStateSpace>(rvss, constraints);
}

template <class Archive>
void OMPLConstrainedRvssPlanProfile::serialize(Archive& ar, const unsigned int /*version*/)
{
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLRealVectorPlanProfile);
}

} // namespace tesseract_planning

#include <tesseract_common/serialization.h>
TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::OMPLConstrainedRvssPlanProfile)
BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::OMPLConstrainedRvssPlanProfile)

#endif // OMPL_LESS_1_4_0
Loading