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

Jump to bottom

wip support AtlasStateSpace #2

Open
wants to merge 2 commits into
base: ompl-constrained-linear-system
Choose a base branch
from
Open

wip support AtlasStateSpace #2

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
8591641
wip support AtlasStateSpace
gautz Jun 22, 2021
58562fd
reverse anchorChart(goal)
gautz Jun 25, 2021
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
2 changes: 2 additions & 0 deletions moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -207,6 +207,8 @@ class PlanningContextManager

protected:
typedef std::function<const ModelBasedStateSpaceFactoryPtr&(const std::string&)> StateSpaceFactoryTypeSelector;
// ros::M_string::const_iterator constrained_state_space_type_iterator_ ;
// std::string constrained_state_space_type_;

ConfiguredPlannerAllocator plannerSelector(const std::string& planner) const;

Expand Down
4 changes: 2 additions & 2 deletions moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -543,7 +543,7 @@ void LinearSystemPositionConstraint::function(const Eigen::Ref<const Eigen::Vect
out[1] = (end_position_.z() - start_position_.z())*(cartesianPosition.x() - start_position_.x())-(end_position_.x() - start_position_.x())*(cartesianPosition.z() - start_position_.z());
// (xb-xa)(y-ya)-(yb-ya)(x-xa)=0
// residual[2] = (end_position_.x() - start_position_.x())*(cartesianPosition.y() - start_position_.y())-(end_position_.y() - start_position_.y())*(cartesianPosition.x() - start_position_.x());
out[2]=0.0;
// out[2]=0.0;
// for (std::size_t dim{ 0 }; dim < 3; ++dim)
// {
// if (is_dim_constrained_[dim])
Expand Down Expand Up @@ -587,7 +587,7 @@ void LinearSystemPositionConstraint::function(const Eigen::Ref<const Eigen::Vect
// // ROS_INFO_STREAM_NAMED(LOGNAME, "Residual of the start position " << residual);

// center_position_ =center_position;
out[2] = 0.0; // unbounded dimension
// out[2] = 0.0; // unbounded dimension
}
else if(knot_number_>3) {
ROS_ERROR_STREAM_NAMED(LOGNAME, "Splines are currently not supported. Number of knots given: " << knot_number_);
Expand Down
45 changes: 44 additions & 1 deletion moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,11 +49,15 @@
#include <moveit/profiler/profiler.h>
#include <moveit/utils/lexical_casts.h>

#include <moveit_msgs/Constraints.h>

#include <ompl/config.h>
#include <ompl/base/samplers/UniformValidStateSampler.h>
#include <ompl/base/goals/GoalLazySamples.h>
#include <ompl/tools/config/SelfConfig.h>
#include <ompl/base/spaces/SE3StateSpace.h>
#include <ompl/base/spaces/constraint/AtlasStateSpace.h>
#include <ompl/base/spaces/constraint/TangentBundleStateSpace.h>
#include <ompl/datastructures/PDF.h>
// TODO: remove when ROS Melodic and older are no longer supported
#if OMPL_VERSION_VALUE < 1005000
Expand Down Expand Up @@ -120,11 +124,49 @@ void ompl_interface::ModelBasedPlanningContext::configure(const ros::NodeHandle&
if (spec_.constrained_state_space_)
{
// convert the input state to the corresponding OMPL state
ompl::base::ScopedState<> ompl_start_state(spec_.constrained_state_space_);
ob::ScopedState<> ompl_start_state(spec_.constrained_state_space_);
spec_.state_space_->copyToOMPLState(ompl_start_state.get(), getCompleteInitialRobotState());
ompl_simple_setup_->setStartState(ompl_start_state);
ompl_simple_setup_->setStateValidityChecker(
ob::StateValidityCheckerPtr(std::make_shared<ConstrainedPlanningStateValidityChecker>(this)));
ROS_INFO_STREAM("start state from getCompleteInitialRobotState: " << getCompleteInitialRobotState());

std::map<std::string, std::string> cfg = getSpecificationConfig();
auto it = cfg.find("constrained_state_space");
if (it != cfg.end())
{
// extract joint state goal from planning request
// auto constraints = request_.goal_constraints[0];
// ROS_INFO_STREAM("goal constraints from planning request: " << constraints);

// ob::ScopedState<> goal(spec_.constrained_state_space_);
// goal->as<ob::ConstrainedStateSpace::StateType>()->copy(goal_joint_positions);
// ROS_INFO_STREAM("goal reconstructed from planning request: " << goal);

// for Atlas and TangentBundle, the start and goal states have to be anchored.
if (it->second == "TangentBundleStateSpace")
{
ROS_INFO_NAMED("model_based_planning_context",
"trying to anchor start states for constrained tangent bundle state space.");
spec_.constrained_state_space_->as<ob::TangentBundleStateSpace>()->anchorChart(ompl_start_state.get());
ROS_INFO_NAMED("model_based_planning_context",
"trying to anchor goal states for constrained tangent bundle state space.");
// spec_.constrained_state_space_->as<ob::TangentBundleStateSpace>()->anchorChart(goal.get());
Copy link
Owner Author

Choose a reason for hiding this comment

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

We need the goal as a joint state here to be able to anchorChart().
Or maybe anchorChart should be called somewhere else in the interface?

ROS_INFO_NAMED("model_based_planning_context",
"start and goal states anchored for constrained tangent bundle state space.");
}
else if (it->second == "AtlasStateSpace")
{
ROS_INFO_NAMED("model_based_planning_context",
"trying to anchor start states for constrained atlas state space.");
spec_.constrained_state_space_->as<ob::AtlasStateSpace>()->anchorChart(ompl_start_state.get());
ROS_INFO_NAMED("model_based_planning_context",
"trying to anchor goal states for constrained atlas state space.");
// spec_.constrained_state_space_->as<ob::AtlasStateSpace>()->anchorChart(goal.get());
ROS_INFO_NAMED("model_based_planning_context",
"start and goal states anchored for constrained atlas state space.");
}
}
}
else
{
Expand All @@ -133,6 +175,7 @@ void ompl_interface::ModelBasedPlanningContext::configure(const ros::NodeHandle&
spec_.state_space_->copyToOMPLState(ompl_start_state.get(), getCompleteInitialRobotState());
ompl_simple_setup_->setStartState(ompl_start_state);
ompl_simple_setup_->setStateValidityChecker(std::make_shared<StateValidityChecker>(this));
ROS_INFO_NAMED("model_based_planning_context", "start and goal states not anchored.");
}

if (path_constraints_ && constraints_library_)
Expand Down
3 changes: 2 additions & 1 deletion moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -179,7 +179,8 @@ void ompl_interface::OMPLInterface::loadPlannerConfigurations()
// the set of planning parameters that can be specific for the group (inherited by configurations of that group)
static const std::string KNOWN_GROUP_PARAMS[] = { "projection_evaluator", "longest_valid_segment_fraction",
"enforce_joint_model_state_space",
"enforce_constrained_state_space" };
"enforce_constrained_state_space",
"constrained_state_space" };

// get parameters specific for the robot planning group
std::map<std::string, std::string> specific_group_params;
Expand Down
29 changes: 28 additions & 1 deletion moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,8 @@

#include <ompl/base/ConstrainedSpaceInformation.h>
#include <ompl/base/spaces/constraint/ProjectedStateSpace.h>
#include <ompl/base/spaces/constraint/TangentBundleStateSpace.h>
#include <ompl/base/spaces/constraint/AtlasStateSpace.h>

#include <moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h>
#include <moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.h>
Expand Down Expand Up @@ -358,11 +360,34 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana
ompl::base::ConstraintPtr ompl_constraint =
ompl_interface::createOMPLConstraint(robot_model_, config.group, req.path_constraints);

// Create a constrained state space of type "projected state space".
// Other types are available, so we probably should add another setting to ompl_planning.yaml
// to choose between them.
// const std::map<std::string, std::string>& config = config.config;
// if (config.empty())
// return;
std::map<std::string, std::string> cfg = config.config;
auto it = cfg.find("constrained_state_space");
if (it != cfg.end())
{
if (it->second == "TangentBundleStateSpace")
{
context_spec.constrained_state_space_ =
std::make_shared<ob::TangentBundleStateSpace>(context_spec.state_space_, ompl_constraint);
ROS_INFO_NAMED("planning_context_manager", "Created a constrained state space of type: tangent bundle state space.");
}
else if (it->second == "AtlasStateSpace")
{
context_spec.constrained_state_space_ =
std::make_shared<ob::AtlasStateSpace>(context_spec.state_space_, ompl_constraint);
ROS_INFO_NAMED("planning_context_manager", "Created a constrained state space of type: atlas state space.");
}}
else // ProjectedStateSpace
{
// Create a constrained state space of type "projected state space".
context_spec.constrained_state_space_ =
std::make_shared<ob::ProjectedStateSpace>(context_spec.state_space_, ompl_constraint);
ROS_INFO_NAMED("planning_context_manager", "Created a constrained state space of type: projected state space.");
}

// Pass the constrained state space to ompl simple setup through the creation of a
// ConstrainedSpaceInformation object. This makes sure the state space is properly initialized.
Expand Down Expand Up @@ -524,6 +549,8 @@ ompl_interface::ModelBasedPlanningContextPtr ompl_interface::PlanningContextMana
// in JointModelStateSpace.
StateSpaceFactoryTypeSelector factory_selector;
auto constrained_planning_iterator = pc->second.config.find("enforce_constrained_state_space");
// constrained_state_space_type_iterator_ = pc->second.config.find("constrained_state_space");
// constrained_state_space_type_ = constrained_state_space_type_iterator->second;
auto joint_space_planning_iterator = pc->second.config.find("enforce_joint_model_state_space");

if (constrained_planning_iterator != pc->second.config.end() &&
Expand Down