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 1 commit 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
1 commit
Select commit
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
7 changes: 1 addition & 6 deletions ...motion_planners/ompl/include/tesseract_motion_planners/ompl/continuous_motion_validator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,6 @@ 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>
Expand All @@ -57,8 +56,7 @@ class ContinuousMotionValidator : public ompl::base::MotionValidator
ompl::base::StateValidityCheckerPtr state_validator,
const tesseract_environment::Environment& env,
std::shared_ptr<const tesseract_kinematics::JointGroup> 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;

Expand Down Expand Up @@ -91,9 +89,6 @@ 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_;

// 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
Expand Down
16 changes: 8 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,8 +33,6 @@ 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>
Expand All @@ -45,6 +43,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
namespace ompl::geometric
{
class SimpleSetup;
class PathGeometric;
}

namespace tesseract_planning
Expand Down Expand Up @@ -72,12 +71,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,6 +85,13 @@ 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 +91 to +93
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:
friend class boost::serialization::access;
template <class Archive>
Expand Down
16 changes: 7 additions & 9 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 @@ -76,14 +76,16 @@ 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:
static void applyGoalStates(ompl::geometric::SimpleSetup& simple_setup,
const tesseract_kinematics::KinGroupIKInput& ik_input,
Expand Down Expand Up @@ -126,8 +128,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 +141,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 +155,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,8 +168,7 @@ 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;

friend class boost::serialization::access;
template <class Archive>
Expand Down
8 changes: 1 addition & 7 deletions ...t_motion_planners/ompl/include/tesseract_motion_planners/ompl/state_collision_validator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,8 +34,6 @@ 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>
Expand All @@ -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<const tesseract_kinematics::JointGroup> 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;

Expand All @@ -70,9 +67,6 @@ 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_;

// 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
Expand Down
44 changes: 0 additions & 44 deletions tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/types.h
View file
Open in desktop

This file was deleted.

41 changes: 24 additions & 17 deletions tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/utils.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,11 +33,10 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <memory>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_motion_planners/ompl/types.h>

#include <tesseract_common/eigen_types.h>
#include <tesseract_collision/core/fwd.h>
#include <tesseract_kinematics/core/fwd.h>
#include <tesseract_command_language/composite_instruction.h>

namespace ompl::base
{
Expand All @@ -55,21 +54,6 @@ class PathGeometric;

namespace tesseract_planning
{
Eigen::Map<Eigen::VectorXd> RealVectorStateSpaceExtractor(const ompl::base::State* s1, unsigned dimension);

#ifndef OMPL_LESS_1_4_0
Eigen::Map<Eigen::VectorXd> 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
Expand Down Expand Up @@ -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<Eigen::VectorXd> 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<std::string>& joint_names,
// const tesseract_common::TrajArray& traj,
// const bool format_result_as_input);

} // namespace tesseract_planning

#endif // TESSERACT_MOTION_PLANNERS_OMPL_UTILS_H
9 changes: 4 additions & 5 deletions tesseract_motion_planners/ompl/src/continuous_motion_validator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
TESSERACT_COMMON_IGNORE_WARNINGS_POP

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

#include <tesseract_environment/environment.h>
#include <tesseract_kinematics/core/joint_group.h>
Expand All @@ -44,13 +45,11 @@ ContinuousMotionValidator::ContinuousMotionValidator(
ompl::base::StateValidityCheckerPtr state_validator,
const tesseract_environment::Environment& env,
std::shared_ptr<const tesseract_kinematics::JointGroup> 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();

Expand Down Expand Up @@ -139,8 +138,8 @@ bool ContinuousMotionValidator::continuousCollisionCheck(const ompl::base::State
}
mutex_.unlock();

Eigen::Map<Eigen::VectorXd> start_joints = extractor_(s1);
Eigen::Map<Eigen::VectorXd> finish_joints = extractor_(s2);
Eigen::Map<Eigen::VectorXd> start_joints = fromRealVectorStateSpace(s1, si_->getStateDimension());
Eigen::Map<Eigen::VectorXd> finish_joints = fromRealVectorStateSpace(s2, si_->getStateDimension());
Comment on lines +141 to +142
Copy link
Contributor

@Levi-Armstrong Levi-Armstrong Dec 24, 2024 •
edited
Loading

Choose a reason for hiding this comment

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

How would this work with a constrained state space? The intent behind the extractor was to avoid multiple implementations of this class.


tesseract_common::TransformMap state0 = manip_->calcFwdKin(start_joints);
tesseract_common::TransformMap state1 = manip_->calcFwdKin(finish_joints);
Expand Down
Loading
Loading