42 namespace bind_planning_component
45 plan(std::shared_ptr<moveit_cpp::PlanningComponent>& planning_component,
46 std::shared_ptr<moveit_cpp::PlanningComponent::PlanRequestParameters>& single_plan_parameters,
47 std::shared_ptr<moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters>& multi_plan_parameters,
49 std::optional<const moveit::planning_pipeline_interfaces::SolutionSelectionFunction> solution_selection_function,
50 std::optional<moveit::planning_pipeline_interfaces::StoppingCriterionFunction> stopping_criterion_callback)
53 if (single_plan_parameters && multi_plan_parameters)
55 throw std::invalid_argument(
"Cannot specify both single and multi plan parameters");
59 if (single_plan_parameters)
62 std::shared_ptr<const moveit_cpp::PlanningComponent::PlanRequestParameters> const_single_plan_parameters =
63 std::const_pointer_cast<const moveit_cpp::PlanningComponent::PlanRequestParameters>(single_plan_parameters);
65 return planning_component->plan(*const_single_plan_parameters,
planning_scene);
67 else if (multi_plan_parameters)
70 std::shared_ptr<const moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters> const_multi_plan_parameters =
71 std::const_pointer_cast<const moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters>(
72 multi_plan_parameters);
74 if (solution_selection_function && stopping_criterion_callback)
76 return planning_component->plan(*const_multi_plan_parameters, std::ref(*solution_selection_function),
79 else if (solution_selection_function)
81 return planning_component->plan(*const_multi_plan_parameters, std::ref(*solution_selection_function),
nullptr,
84 else if (stopping_criterion_callback)
86 return planning_component->plan(*const_multi_plan_parameters,
92 return planning_component->plan(*const_multi_plan_parameters,
101 throw std::invalid_argument(
"Cannot specify planning scene without specifying plan parameters");
103 return planning_component->plan();
107 bool setGoal(std::shared_ptr<moveit_cpp::PlanningComponent>& planning_component,
108 std::optional<std::string> configuration_name, std::optional<moveit::core::RobotState> robot_state,
109 std::optional<geometry_msgs::msg::PoseStamped> pose_stamped_msg, std::optional<std::string> pose_link,
110 std::optional<std::vector<moveit_msgs::msg::Constraints>> motion_plan_constraints)
113 if (configuration_name && robot_state)
115 throw std::invalid_argument(
"Cannot specify both configuration name and robot state");
117 else if (configuration_name && pose_stamped_msg)
119 throw std::invalid_argument(
"Cannot specify both configuration name and pose msg");
121 else if (configuration_name && motion_plan_constraints)
123 throw std::invalid_argument(
"Cannot specify both configuration name and motion plan constraints");
125 else if (robot_state && pose_stamped_msg)
127 throw std::invalid_argument(
"Cannot specify both robot state and pose msg");
129 else if (robot_state && motion_plan_constraints)
131 throw std::invalid_argument(
"Cannot specify both robot state and motion plan constraints");
133 else if (pose_stamped_msg && motion_plan_constraints)
135 throw std::invalid_argument(
"Cannot specify both pose goal and motion plan constraints");
137 else if ((pose_stamped_msg && !pose_link) || (!pose_stamped_msg && pose_link))
139 throw std::invalid_argument(
"Must specify both message and corresponding link");
143 if (!configuration_name && !robot_state && !pose_stamped_msg && !pose_link && !motion_plan_constraints)
145 throw std::invalid_argument(
"Must specify at least one argument");
149 if (configuration_name)
151 return planning_component->setGoal(*configuration_name);
154 else if (robot_state)
156 return planning_component->setGoal(*robot_state);
159 else if (pose_stamped_msg && pose_link)
161 return planning_component->setGoal(*pose_stamped_msg, *pose_link);
166 return planning_component->setGoal(*motion_plan_constraints);
170 bool setStartState(std::shared_ptr<moveit_cpp::PlanningComponent>& planning_component,
171 std::optional<std::string> configuration_name, std::optional<moveit::core::RobotState> robot_state)
174 if (configuration_name && robot_state)
176 throw std::invalid_argument(
"Cannot specify both configuration name and robot state");
180 if (!configuration_name && !robot_state)
182 throw std::invalid_argument(
"Must specify at least one argument");
186 if (configuration_name)
188 return planning_component->setStartState(*configuration_name);
193 return planning_component->setStartState(*robot_state);
200 std::shared_ptr<moveit_cpp::PlanningComponent::PlanRequestParameters>>(m,
"PlanRequestParameters",
202 Planner parameters provided with a MotionPlanRequest.
204 .def(py::init([](std::shared_ptr<moveit_cpp::MoveItCpp>& moveit_cpp, const std::string& ns) {
205 const rclcpp::Node::SharedPtr& node =
moveit_cpp->getNode();
207 params.
load(node, ns);
212 str: The planner id to use.
216 str: The planning pipeline to use.
220 int: The number of planning attempts to make.
224 float: The amount of time to spend planning.
226 .def_readwrite("max_velocity_scaling_factor",
229 float: The maximum velocity scaling factor that can be used.
231 .def_readwrite("max_acceleration_scaling_factor",
234 float: The maximum scaling factor that can be used.
241 std::shared_ptr<moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters>>(
242 m,
"MultiPipelinePlanRequestParameters",
244 Planner parameters provided with a MotionPlanRequest.
246 .def(py::init([](std::shared_ptr<moveit_cpp::MoveItCpp>& moveit_cpp,
247 const std::vector<std::string>& planning_pipeline_names) {
248 const rclcpp::Node::SharedPtr& node =
moveit_cpp->getNode();
252 .def_readonly(
"multi_plan_request_parameters",
257 py::class_<moveit_cpp::PlanningComponent, std::shared_ptr<moveit_cpp::PlanningComponent>>(m,
"PlanningComponent",
259 Represents a joint model group and motion plans corresponding to this joint model group.
262 .def(py::init<const std::string&,
const std::shared_ptr<moveit_cpp::MoveItCpp>&>(),
263 py::arg(
"joint_model_group_name"), py::arg(
"moveit_py_instance"),
265 Constructs a PlanningComponent instance.
268 joint_model_group_name (str): The name of the joint model group to plan for.
269 moveit_py_instance (:py:class:`moveit_py.core.MoveItPy`): The MoveItPy instance to use.
274 dict: The joint values for targets specified by name.
279 str: The name of the planning group to plan for.
284 list of str: The names of the named robot states available as targets.
290 Set the start state of the plan to the current state of the robot.
294 py::arg(
"configuration_name") =
nullptr, py::arg(
"robot_state") =
nullptr,
296 Set the start state of the plan to the given robot state.
299 configuration_name (str): The name of the configuration to use as the start state.
300 robot_state (:py:class:`moveit_msgs.msg.RobotState`): The robot state to use as the start state.
304 py::return_value_policy::reference_internal,
306 Returns the current start state for the planning component.
310 .def(
"set_goal_state",
311 py::overload_cast<std::shared_ptr<moveit_cpp::PlanningComponent>&, std::optional<std::string>,
312 std::optional<moveit::core::RobotState>, std::optional<geometry_msgs::msg::PoseStamped>,
313 std::optional<std::string>, std::optional<std::vector<moveit_msgs::msg::Constraints>>>(
315 py::arg(
"configuration_name") =
nullptr, py::arg(
"robot_state") =
nullptr,
316 py::arg(
"pose_stamped_msg") =
nullptr, py::arg(
"pose_link") =
nullptr,
317 py::arg(
"motion_plan_constraints") =
nullptr,
319 Set the goal state for the planning component.
322 configuration_name (str): The name of the configuration to set the goal to.
323 robot_state (moveit_py.core.RobotState): The state to set the goal to.
324 pose_stamped_msg (geometry_msgs.msg.PoseStamped): A PoseStamped ros message.
325 pose_link (str): The name of the link for which the pose constraint is specified.
326 motion_plan_constraints (list): The motion plan constraints to set the goal to.
333 py::arg(
"multi_plan_parameters") =
nullptr, py::arg(
"planning_scene") =
nullptr,
334 py::arg(
"solution_selection_function") =
nullptr, py::arg(
"stopping_criterion_callback") =
nullptr,
335 py::return_value_policy::move,
337 Plan a motion plan using the current start and goal states.
340 plan_parameters (moveit_py.core.PlanParameters): The parameters to use for planning.
344 py::return_value_policy::move,
346 Set the path constraints generated from a moveit msg Constraints.
349 path_constraints (moveit_msgs.msg.Constraints): The path constraints.
354 py::arg(
"min_z"), py::arg(
"max_x"), py::arg(
"max_y"), py::arg(
"max_z"),
356 Specify the workspace bounding box. The box is specified in the planning frame (i.e. relative to the robot root link start position). The workspace applies only to the root joint of a mobile robot (driving base, quadrotor) and does not limit the workspace of a robot arm.
359 min_x (float): The minimum x value of the workspace.
360 min_y (float): The minimum y value of the workspace.
361 min_z (float): The minimum z value of the workspace.
362 max_x (float): The maximum x value of the workspace.
363 max_y (float): The maximum y value of the workspace.
364 max_z (float): The maximum z value of the workspace.
369 Remove the workspace bounding box from planning.
void setStartStateToCurrentState()
Set the start state for planning to be the current state of the robot.
std::map< std::string, double > getNamedTargetStateValues(const std::string &name)
Get the joint values for targets specified by name.
void unsetWorkspace()
Remove the workspace bounding box from planning.
const std::string & getPlanningGroupName() const
Get the name of the planning group.
void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
Specify the workspace bounding box. The box is specified in the planning frame (i....
bool setPathConstraints(const moveit_msgs::msg::Constraints &path_constraints)
Set the path constraints generated from a moveit msg Constraints.
const std::vector< std::string > getNamedTargetStates()
Get the names of the named robot states available as targets.
moveit::core::RobotStatePtr getStartState()
Get the considered start state if defined, otherwise get the current state.
::planning_interface::MotionPlanResponse getShortestSolution(const std::vector<::planning_interface::MotionPlanResponse > &solutions)
Function that returns the shortest solution out of a vector of solutions based on robot_trajectory::p...
bool setGoal(std::shared_ptr< moveit_cpp::PlanningComponent > &planning_component, std::optional< std::string > configuration_name, std::optional< moveit::core::RobotState > robot_state, std::optional< geometry_msgs::msg::PoseStamped > pose_stamped_msg, std::optional< std::string > pose_link, std::optional< std::vector< moveit_msgs::msg::Constraints >> motion_plan_constraints)
bool setStartState(std::shared_ptr< moveit_cpp::PlanningComponent > &planning_component, std::optional< std::string > configuration_name, std::optional< moveit::core::RobotState > robot_state)
void initMultiPlanRequestParameters(py::module &m)
void initPlanningComponent(py::module &m)
void initPlanRequestParameters(py::module &m)
planning_interface::MotionPlanResponse plan(std::shared_ptr< moveit_cpp::PlanningComponent > &planning_component, std::shared_ptr< moveit_cpp::PlanningComponent::PlanRequestParameters > &single_plan_parameters, std::shared_ptr< moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters > &multi_plan_parameters, std::shared_ptr< planning_scene::PlanningScene > &planning_scene, std::optional< const moveit::planning_pipeline_interfaces::SolutionSelectionFunction > solution_selection_function, std::optional< moveit::planning_pipeline_interfaces::StoppingCriterionFunction > stopping_criterion_callback)
This namespace includes the central class for representing planning contexts.
Planner parameters provided with the MotionPlanRequest.
std::vector< PlanRequestParameters > plan_request_parameter_vector
Planner parameters provided with the MotionPlanRequest.
void load(const rclcpp::Node::SharedPtr &node, const std::string ¶m_namespace="")
double max_velocity_scaling_factor
double max_acceleration_scaling_factor
std::string planning_pipeline
Response to a planning query.