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

Handle unsupported position constraints in OMPL (backport #2417) #2622

Open
wants to merge 1 commit into
base: iron
Choose a base branch
from
Open

Handle unsupported position constraints in OMPL (backport #2417) #2622

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
60 changes: 47 additions & 13 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 @@ -364,17 +364,15 @@ ompl::base::ConstraintPtr createOMPLConstraints(const moveit::core::RobotModelCo
const std::string& group,
const moveit_msgs::msg::Constraints& constraints)
{
// TODO(bostoncleek): does this reach the end w/o a return ?

const std::size_t num_dofs = robot_model->getJointModelGroup(group)->getVariableCount();
const std::size_t num_pos_con = constraints.position_constraints.size();
const std::size_t num_ori_con = constraints.orientation_constraints.size();

// This factory method contains template code to support position and/or orientation constraints.
// If the specified constraints are invalid, a nullptr is returned.
std::vector<ompl::base::ConstraintPtr> ompl_constraints;
if (num_pos_con > 1)
const std::size_t num_dofs = robot_model->getJointModelGroup(group)->getVariableCount();

// Parse Position Constraints
if (!constraints.position_constraints.empty())
{
<<<<<<< HEAD
RCLCPP_WARN(LOGGER, "Only a single position constraint is supported. Using the first one.");
}
if (num_ori_con > 1)
Expand All @@ -385,27 +383,63 @@ ompl::base::ConstraintPtr createOMPLConstraints(const moveit::core::RobotModelCo
{
BaseConstraintPtr pos_con;
if (constraints.name == "use_equality_constraints")
=======
if (constraints.position_constraints.size() > 1)
>>>>>>> b0401e91a (Handle unsupported position constraints in OMPL (#2417))
{
RCLCPP_WARN(getLogger(), "Only a single position constraint is supported. Using the first one.");
}

const auto& primitives = constraints.position_constraints.at(0).constraint_region.primitives;
if (primitives.size() > 1)
{
RCLCPP_WARN(getLogger(), "Only a single position primitive is supported. Using the first one.");
}
if (primitives.empty() || primitives.at(0).type != shape_msgs::msg::SolidPrimitive::BOX)
{
pos_con = std::make_shared<EqualityPositionConstraint>(robot_model, group, num_dofs);
RCLCPP_ERROR(getLogger(), "Unable to plan with the requested position constraint. "
"Only BOX primitive shapes are supported as constraint region.");
}
else
{
pos_con = std::make_shared<BoxConstraint>(robot_model, group, num_dofs);
BaseConstraintPtr pos_con;
if (constraints.name == "use_equality_constraints")
{
pos_con = std::make_shared<EqualityPositionConstraint>(robot_model, group, num_dofs);
}
else
{
pos_con = std::make_shared<BoxConstraint>(robot_model, group, num_dofs);
}
pos_con->init(constraints);
ompl_constraints.emplace_back(pos_con);
}
pos_con->init(constraints);
ompl_constraints.emplace_back(pos_con);
}
if (num_ori_con > 0)

// Parse Orientation Constraints
if (!constraints.orientation_constraints.empty())
{
if (constraints.orientation_constraints.size() > 1)
{
RCLCPP_WARN(getLogger(), "Only a single orientation constraint is supported. Using the first one.");
}

auto ori_con = std::make_shared<OrientationConstraint>(robot_model, group, num_dofs);
ori_con->init(constraints);
ompl_constraints.emplace_back(ori_con);
}
if (num_pos_con < 1 && num_ori_con < 1)

// Check if we have any constraints to plan with
if (ompl_constraints.empty())
{
<<<<<<< HEAD
RCLCPP_ERROR(LOGGER, "No path constraints found in planning request.");
=======
RCLCPP_ERROR(getLogger(), "Failed to parse any supported path constraints from planning request.");
>>>>>>> b0401e91a (Handle unsupported position constraints in OMPL (#2417))
return nullptr;
}

return std::make_shared<ompl::base::ConstraintIntersection>(num_dofs, ompl_constraints);
}
} // namespace ompl_interface
6 changes: 6 additions & 0 deletions 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 @@ -374,6 +374,12 @@ PlanningContextManager::getPlanningContext(const planning_interface::PlannerConf
ompl::base::ConstraintPtr ompl_constraint =
createOMPLConstraints(robot_model_, config.group, req.path_constraints);

// Fail if ompl constraints could not be parsed successfully
if (!ompl_constraint)
{
return ModelBasedPlanningContextPtr();
}

// 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.
Expand Down
Loading