From 859164105e276cded2189f7aa00c58593e0784eb Mon Sep 17 00:00:00 2001 From: Gauthier Hentz Date: Tue, 22 Jun 2021 12:34:24 +0200 Subject: [PATCH 1/2] wip support AtlasStateSpace --- .../ompl_interface/planning_context_manager.h | 2 + .../src/detail/ompl_constraints.cpp | 4 +- .../src/model_based_planning_context.cpp | 56 ++++++++++++++++++- .../ompl_interface/src/ompl_interface.cpp | 3 +- .../src/planning_context_manager.cpp | 29 +++++++++- 5 files changed, 89 insertions(+), 5 deletions(-) diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h index 3912b8a3a3..faf8d71928 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h @@ -207,6 +207,8 @@ class PlanningContextManager protected: typedef std::function StateSpaceFactoryTypeSelector; + // ros::M_string::const_iterator constrained_state_space_type_iterator_ ; + // std::string constrained_state_space_type_; ConfiguredPlannerAllocator plannerSelector(const std::string& planner) const; diff --git a/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp b/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp index 17552d6d1b..d66dbc56da 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp @@ -543,7 +543,7 @@ void LinearSystemPositionConstraint::function(const Eigen::Ref3) { ROS_ERROR_STREAM_NAMED(LOGNAME, "Splines are currently not supported. Number of knots given: " << knot_number_); diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index f8edb660f0..9985125ad5 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -49,11 +49,15 @@ #include #include +#include + #include #include #include #include #include +#include +#include #include // TODO: remove when ROS Melodic and older are no longer supported #if OMPL_VERSION_VALUE < 1005000 @@ -120,11 +124,60 @@ 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(this))); + ROS_INFO_STREAM("start state from getCompleteInitialRobotState: " << getCompleteInitialRobotState()); + + std::map cfg = getSpecificationConfig(); + auto it = cfg.find("constrained_state_space"); + if (it != cfg.end()) + { + // extract goal from planning request + auto constraints = request_.goal_constraints[0]; + ROS_INFO_STREAM("goal constraints from planning request: " << constraints); + + // auto num_dofs_ = robot_model->getJointModelGroup(request_.group_name())->getVariableCount(); + Eigen::VectorXd goal_joint_positions(6); + ROS_INFO_STREAM("num goal constraints: " << request_.goal_constraints.size()); + std::size_t joint_index{ 0 }; + for (auto& joint_constraint : request_.goal_constraints[0].joint_constraints) + { + // ROS_INFO_STREAM("name: " << joint_constraint.joint_name << " value: " << joint_constraint.position); + goal_joint_positions[joint_index] = joint_constraint.position; + joint_index++; + } + + ob::ScopedState<> goal(spec_.constrained_state_space_); + goal->as()->copy(goal_joint_positions); + ROS_INFO_STREAM("goal reconstructed from planning request: " << goal); + + // ROS_INFO_NAMED("model_based_planning_context", "trying to anchor start states for constrained state space."); + // spec_.constrained_state_space_->anchorChart(ompl_start_state.get()); + // ROS_INFO_NAMED("model_based_planning_context", "trying to anchor goal states for constrained state space."); + // spec_.constrained_state_space_->anchorChart(goal.get()); + // ROS_INFO_NAMED("model_based_planning_context", "start and goal states anchored for constrained state space."); + + // 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()->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()->anchorChart(goal.get()); + 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()->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()->anchorChart(goal.get()); + ROS_INFO_NAMED("model_based_planning_context", "start and goal states anchored for constrained atlas state space."); +} + } } else { @@ -133,6 +186,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(this)); + ROS_INFO_NAMED("model_based_planning_context", "start and goal states not anchored."); } if (path_constraints_ && constraints_library_) diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp index e69cb52d5e..726e3bec26 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp @@ -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 specific_group_params; diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index 49a36a55f8..50d1651f1a 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -69,6 +69,8 @@ #include #include +#include +#include #include #include @@ -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& config = config.config; + // if (config.empty()) + // return; + std::map 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(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(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(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. @@ -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() && From 58562fdb14072d682dfaf190511cdf8cb989f8da Mon Sep 17 00:00:00 2001 From: Gauthier Hentz Date: Fri, 25 Jun 2021 16:59:20 +0200 Subject: [PATCH 2/2] reverse anchorChart(goal) --- .../src/model_based_planning_context.cpp | 85 ++++++++----------- 1 file changed, 37 insertions(+), 48 deletions(-) diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index 9985125ad5..678a58d6f4 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -129,55 +129,44 @@ void ompl_interface::ModelBasedPlanningContext::configure(const ros::NodeHandle& ompl_simple_setup_->setStartState(ompl_start_state); ompl_simple_setup_->setStateValidityChecker( ob::StateValidityCheckerPtr(std::make_shared(this))); - ROS_INFO_STREAM("start state from getCompleteInitialRobotState: " << getCompleteInitialRobotState()); + ROS_INFO_STREAM("start state from getCompleteInitialRobotState: " << getCompleteInitialRobotState()); - std::map cfg = getSpecificationConfig(); - auto it = cfg.find("constrained_state_space"); + std::map cfg = getSpecificationConfig(); + auto it = cfg.find("constrained_state_space"); if (it != cfg.end()) - { - // extract goal from planning request - auto constraints = request_.goal_constraints[0]; - ROS_INFO_STREAM("goal constraints from planning request: " << constraints); - - // auto num_dofs_ = robot_model->getJointModelGroup(request_.group_name())->getVariableCount(); - Eigen::VectorXd goal_joint_positions(6); - ROS_INFO_STREAM("num goal constraints: " << request_.goal_constraints.size()); - std::size_t joint_index{ 0 }; - for (auto& joint_constraint : request_.goal_constraints[0].joint_constraints) - { - // ROS_INFO_STREAM("name: " << joint_constraint.joint_name << " value: " << joint_constraint.position); - goal_joint_positions[joint_index] = joint_constraint.position; - joint_index++; - } - - ob::ScopedState<> goal(spec_.constrained_state_space_); - goal->as()->copy(goal_joint_positions); - ROS_INFO_STREAM("goal reconstructed from planning request: " << goal); - - // ROS_INFO_NAMED("model_based_planning_context", "trying to anchor start states for constrained state space."); - // spec_.constrained_state_space_->anchorChart(ompl_start_state.get()); - // ROS_INFO_NAMED("model_based_planning_context", "trying to anchor goal states for constrained state space."); - // spec_.constrained_state_space_->anchorChart(goal.get()); - // ROS_INFO_NAMED("model_based_planning_context", "start and goal states anchored for constrained state space."); - - // 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()->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()->anchorChart(goal.get()); - 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()->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()->anchorChart(goal.get()); - ROS_INFO_NAMED("model_based_planning_context", "start and goal states anchored for constrained atlas state space."); -} - } + { + // 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()->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()->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()->anchorChart(goal.get()); + 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()->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()->anchorChart(goal.get()); + ROS_INFO_NAMED("model_based_planning_context", + "start and goal states anchored for constrained atlas state space."); + } + } } else { @@ -186,7 +175,7 @@ else if (it->second == "AtlasStateSpace") spec_.state_space_->copyToOMPLState(ompl_start_state.get(), getCompleteInitialRobotState()); ompl_simple_setup_->setStartState(ompl_start_state); ompl_simple_setup_->setStateValidityChecker(std::make_shared(this)); - ROS_INFO_NAMED("model_based_planning_context", "start and goal states not anchored."); + ROS_INFO_NAMED("model_based_planning_context", "start and goal states not anchored."); } if (path_constraints_ && constraints_library_)