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.

Sign up for GitHub

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

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
4878e05
Removed OMPL state extractor concept
marip8 Dec 23, 2024
4684840
Moved RVSS components to separate subdirectory
marip8 Dec 26, 2024
74d7dda
Added state converter function back to RVSS motion and state validators
marip8 Dec 26, 2024
e2a756d
Remove unused include
marip8 Dec 26, 2024
8cbdbce
Added function to OMPL profile for generating state space object inde…
marip8 Dec 26, 2024
27f376e
Added state converter object in RVSS profile
marip8 Dec 26, 2024
792626f
Added a constrained RVSS plan profile
marip8 Dec 26, 2024
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
8 changes: 5 additions & 3 deletions tesseract_motion_planners/ompl/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,15 +6,17 @@ 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/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()

Expand Down
70 changes: 70 additions & 0 deletions .../ompl/include/tesseract_motion_planners/ompl/profile/ompl_constrained_rvss_plan_profile.h
View file
Open in desktop
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
29 changes: 21 additions & 8 deletions tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,18 +33,23 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <Eigen/Geometry>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_motion_planners/ompl/types.h>

#include <tesseract_common/fwd.h>
#include <tesseract_kinematics/core/fwd.h>
#include <tesseract_environment/fwd.h>
#include <tesseract_command_language/fwd.h>

#include <tesseract_command_language/profile.h>

namespace ompl::base
{
class StateSpace;
using StateSpacePtr = std::shared_ptr<StateSpace>;
}

namespace ompl::geometric
{
class SimpleSetup;
class PathGeometric;
}

namespace tesseract_planning
Expand Down Expand Up @@ -72,12 +77,6 @@ class OMPLPlanProfile : public Profile
*/
virtual std::unique_ptr<OMPLSolverConfig> 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
Expand All @@ -92,7 +91,21 @@ class OMPLPlanProfile : public Profile
const tesseract_common::ManipulatorInfo& composite_mi,
const std::shared_ptr<const tesseract_environment::Environment>& 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<const tesseract_environment::Environment>& env) const = 0;
Comment on lines +97 to +99
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

To mirror what is currently being done in assignTrajectory, do we also need to pass in the UUIDs of the start and end waypoints or those waypoints themselves?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I like this change. I was using the minimum needed but you could also pass the waypoint or instruction.


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<const tesseract_environment::Environment>& env) const = 0;

friend class boost::serialization::access;
template <class Archive>
void serialize(Archive&, const unsigned int); // NOLINT
Expand Down
26 changes: 16 additions & 10 deletions ...nners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_motion_planners/ompl/profile/ompl_profile.h>
#include <tesseract_motion_planners/ompl/ompl_solver_config.h>
#include <tesseract_motion_planners/ompl/real_vector_state_space/utils.h>

#include <tesseract_collision/core/fwd.h>
#include <tesseract_collision/core/types.h>
Expand All @@ -46,8 +47,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP

namespace ompl::base
{
class State;
class StateSampler;
class StateSpace;
class StateValidityChecker;
class MotionValidator;
class OptimizationObjective;
Expand Down Expand Up @@ -76,15 +77,20 @@ class OMPLRealVectorPlanProfile : public OMPLPlanProfile

std::unique_ptr<OMPLSolverConfig> createSolverConfig() const override;

OMPLStateExtractor createStateExtractor(const tesseract_kinematics::JointGroup& manip) const override;

std::unique_ptr<ompl::geometric::SimpleSetup>
createSimpleSetup(const MoveInstructionPoly& start_instruction,
const MoveInstructionPoly& end_instruction,
const tesseract_common::ManipulatorInfo& composite_mi,
const std::shared_ptr<const tesseract_environment::Environment>& env) const override;

CompositeInstruction convertPath(const ompl::geometric::PathGeometric& path,
const tesseract_common::ManipulatorInfo& composite_mi,
const std::shared_ptr<const tesseract_environment::Environment>& env) const override;

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

static void applyGoalStates(ompl::geometric::SimpleSetup& simple_setup,
const tesseract_kinematics::KinGroupIKInput& ik_input,
const tesseract_kinematics::KinematicGroup& manip,
Expand Down Expand Up @@ -126,8 +132,7 @@ class OMPLRealVectorPlanProfile : public OMPLPlanProfile
virtual std::unique_ptr<ompl::base::StateValidityChecker>
createStateValidator(const ompl::geometric::SimpleSetup& simple_setup,
const std::shared_ptr<const tesseract_environment::Environment>& env,
const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip,
const OMPLStateExtractor& state_extractor) const;
const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip) const;

/**
* @brief Create collision state validator
Expand All @@ -140,8 +145,7 @@ class OMPLRealVectorPlanProfile : public OMPLPlanProfile
virtual std::unique_ptr<ompl::base::StateValidityChecker>
createCollisionStateValidator(const ompl::geometric::SimpleSetup& simple_setup,
const std::shared_ptr<const tesseract_environment::Environment>& env,
const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip,
const OMPLStateExtractor& state_extractor) const;
const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip) const;

/**
* @brief Create motion validator
Expand All @@ -155,7 +159,6 @@ class OMPLRealVectorPlanProfile : public OMPLPlanProfile
createMotionValidator(const ompl::geometric::SimpleSetup& simple_setup,
const std::shared_ptr<const tesseract_environment::Environment>& env,
const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip,
const OMPLStateExtractor& state_extractor,
const std::shared_ptr<ompl::base::StateValidityChecker>& svc_without_collision) const;

/**
Expand All @@ -169,13 +172,16 @@ class OMPLRealVectorPlanProfile : public OMPLPlanProfile
virtual std::unique_ptr<ompl::base::OptimizationObjective>
createOptimizationObjective(const ompl::geometric::SimpleSetup& simple_setup,
const std::shared_ptr<const tesseract_environment::Environment>& env,
const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip,
const OMPLStateExtractor& state_extractor) const;
const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip) const;

/** @brief Function used to convert OMPL states into joint states */
StateConverterFn state_converter_;

friend class boost::serialization::access;
template <class Archive>
void serialize(Archive&, const unsigned int); // NOLINT
};

} // namespace tesseract_planning

BOOST_CLASS_EXPORT_KEY(tesseract_planning::OMPLRealVectorPlanProfile)
Expand Down
15 changes: 8 additions & 7 deletions ...anners/ompl/continuous_motion_validator.h → ...state_space/continuous_motion_validator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
Expand All @@ -34,11 +34,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <mutex>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_motion_planners/ompl/types.h>
#include <tesseract_collision/core/fwd.h>
#include <tesseract_kinematics/core/fwd.h>
#include <tesseract_environment/fwd.h>

#include <tesseract_motion_planners/ompl/real_vector_state_space/utils.h>

namespace ompl::base
{
class SpaceInformation;
Expand All @@ -58,7 +59,7 @@ class ContinuousMotionValidator : public ompl::base::MotionValidator
const tesseract_environment::Environment& env,
std::shared_ptr<const tesseract_kinematics::JointGroup> manip,
const tesseract_collision::CollisionCheckConfig& collision_check_config,
OMPLStateExtractor extractor);
StateConverterFn state_converter);

bool checkMotion(const ompl::base::State* s1, const ompl::base::State* s2) const override;

Expand Down Expand Up @@ -91,8 +92,8 @@ class ContinuousMotionValidator : public ompl::base::MotionValidator
/** @brief A list of active links */
std::vector<std::string> links_;

/** @brief This will extract an Eigen::VectorXd from the OMPL State */
OMPLStateExtractor extractor_;
/** @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
Expand All @@ -108,4 +109,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
16 changes: 8 additions & 8 deletions ...planners/ompl/state_collision_validator.h → ...r_state_space/state_collision_validator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
Expand All @@ -34,12 +34,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <mutex>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_motion_planners/ompl/types.h>

#include <tesseract_environment/fwd.h>
#include <tesseract_kinematics/core/fwd.h>
#include <tesseract_collision/core/fwd.h>

#include <tesseract_motion_planners/ompl/real_vector_state_space/utils.h>

namespace ompl::base
{
class SpaceInformation;
Expand All @@ -56,7 +56,7 @@ class StateCollisionValidator : public ompl::base::StateValidityChecker
const tesseract_environment::Environment& env,
std::shared_ptr<const tesseract_kinematics::JointGroup> manip,
const tesseract_collision::CollisionCheckConfig& collision_check_config,
OMPLStateExtractor extractor);
StateConverterFn state_converter);

bool isValid(const ompl::base::State* state) const override;

Expand All @@ -70,8 +70,8 @@ class StateCollisionValidator : public ompl::base::StateValidityChecker
/** @brief A list of active links */
std::vector<std::string> links_;

/** @brief This will extract an Eigen::VectorXd from the OMPL State */
OMPLStateExtractor extractor_;
/** @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
Expand All @@ -86,4 +86,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
Loading
Loading