diff --git a/CMakeLists.txt b/CMakeLists.txt index 30af510b34..e620cfeaf5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -78,9 +78,16 @@ add_subdirectory(doc/examples/robot_model_and_robot_state) # add_subdirectory(doc/examples/creating_moveit_plugins/lerp_motion_planner) # add_subdirectory(doc/examples/visualizing_collisions) add_subdirectory(doc/examples/moveit_cpp) +<<<<<<< HEAD # add_subdirectory(doc/examples/collision_environments) # add_subdirectory(doc/examples/visualizing_collisions) # add_subdirectory(doc/examples/bullet_collision_checker) +======= +add_subdirectory(doc/examples/persistent_scenes_and_states) +add_subdirectory(doc/examples/pilz_industrial_motion_planner) +add_subdirectory(doc/examples/planning_scene_ros_api) +add_subdirectory(doc/examples/planning_scene) +>>>>>>> 7498cc2 (Pilz examples and update to ROS2 (#548)) add_subdirectory(doc/examples/realtime_servo) add_subdirectory(doc/how_to_guides/chomp_planner) add_subdirectory(doc/how_to_guides/using_ompl_constrained_planning) diff --git a/doc/examples/pilz_industrial_motion_planner/CMakeLists.txt b/doc/examples/pilz_industrial_motion_planner/CMakeLists.txt new file mode 100644 index 0000000000..2ef8f5af91 --- /dev/null +++ b/doc/examples/pilz_industrial_motion_planner/CMakeLists.txt @@ -0,0 +1,19 @@ +add_executable(pilz_move_group src/pilz_move_group.cpp) +target_include_directories(pilz_move_group PUBLIC include) +ament_target_dependencies(pilz_move_group ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +add_executable(pilz_mtc src/pilz_mtc.cpp) +target_include_directories(pilz_mtc PUBLIC include) +ament_target_dependencies(pilz_mtc ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +install( + TARGETS + pilz_move_group + pilz_mtc + DESTINATION + lib/${PROJECT_NAME} +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) diff --git a/doc/examples/pilz_industrial_motion_planner/launch/pilz_mtc.launch.py b/doc/examples/pilz_industrial_motion_planner/launch/pilz_mtc.launch.py new file mode 100644 index 0000000000..cf0b38e246 --- /dev/null +++ b/doc/examples/pilz_industrial_motion_planner/launch/pilz_mtc.launch.py @@ -0,0 +1,19 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("moveit_resources_panda").to_dict() + + # Pilz + MTC Demo node + pilz_mtc_demo = Node( + package="moveit2_tutorials", + executable="pilz_mtc", + output="screen", + parameters=[ + moveit_config, + ], + ) + + return LaunchDescription([pilz_mtc_demo]) diff --git a/doc/examples/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst b/doc/examples/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst index 027c08fa8d..84e78a0e0f 100644 --- a/doc/examples/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst +++ b/doc/examples/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst @@ -1,56 +1,48 @@ -:moveit1: - -.. - Once updated for MoveIt 2, remove all lines above title (including this comment and :moveit1: tag) - Pilz Industrial Motion Planner ============================== ``pilz_industrial_motion_planner`` provides a trajectory generator to plan standard robot -motions like PTP, LIN, CIRC with the interface of a MoveIt PlannerManager -plugin. +motions like point-to-point, linear, and circular with MoveIt. -User Interface MoveGroup ------------------------- - -This package implements the ``planning_interface::PlannerManager`` -interface of MoveIt. By loading the corresponding planning pipeline -(``pilz_industrial_motion_planner_planning_pipeline.launch.xml`` in your +By loading the corresponding planning pipeline +(``pilz_industrial_motion_planner_planning_planner.yaml`` in your ``*_moveit_config`` package), the trajectory generation -functionalities can be accessed through the user interface (c++, python -or rviz) provided by the ``move_group`` node, e.g. -``/plan_kinematics_path`` service and ``/move_group`` action. For -detailed usage tutorials please refer to :doc:`/doc/examples/move_group_interface/move_group_interface_tutorial`. +functionalities can be accessed through the user interface (C++, Python +or RViz) provided by the ``move_group`` node, e.g. +``/plan_kinematic_path`` service and ``/move_action`` action. +For detailed usage tutorials, please refer to +:doc:`/doc/examples/moveit_cpp/moveitcpp_tutorial` and +:doc:`/doc/examples/move_group_interface/move_group_interface_tutorial`. Joint Limits ------------ -For all commands the planner uses maximum velocities and accelerations from -the parameter server. Using the MoveIt setup assistant the file ``joint_limits.yaml`` +The planner uses maximum velocities and accelerations from the +parameters of the ROS node that is operating the Pilz planning pipeline. +Using the MoveIt Setup Assistant the file ``joint_limits.yaml`` is auto-generated with proper defaults and loaded during startup. -The limits on the parameter server override the limits from the URDF robot description. +The specified limits override the limits from the URDF robot description. Note that while setting position limits and velocity limits is possible -in both the URDF and the parameter server setting acceleration limits is -only possible via the parameter server. In extension to the common -``has_acceleration`` and ``max_acceleration`` parameter we added the -ability to also set ``has_deceleration`` and -``max_deceleration``\ (<0.0). +in both the URDF and a parameter file, setting acceleration limits is +only possible using a parameter file. In addition to the common +``has_acceleration`` and ``max_acceleration`` parameters, we added the +ability to also set ``has_deceleration`` and ``max_deceleration``\ (<0.0). The limits are merged under the premise that the limits from the -parameter server must be stricter or at least equal to the parameters +node parameters must be stricter or at least equal to the parameters set in the URDF. -Currently the calculated trajectory will respect the limits by using the +Currently, the calculated trajectory will respect the limits by using the strictest combination of all limits as a common limit for all joints. Cartesian Limits ---------------- -For cartesian trajectory generation (LIN/CIRC) the planner needs an -information about the maximum speed in 3D cartesian space. Namely +For Cartesian trajectory generation (LIN/CIRC), the planner needs +information about the maximum speed in 3D Cartesian space. Namely, translational/rotational velocity/acceleration/deceleration need to be -set on the parameter server like this: +set in the node parameters like this: .. code:: yaml @@ -60,22 +52,25 @@ set on the parameter server like this: max_trans_dec: -5 max_rot_vel: 1.57 +You can specify Cartesian velocity and acceleration limits in a file named +``pilz_cartesian_limits.yaml`` in your ``*_moveit_config`` package. + The planners assume the same acceleration ratio for translational and -rotational trapezoidal shapes. So the rotational acceleration is -calculated as ``max\_trans\_acc / max\_trans\_vel \* max\_rot\_vel`` (and -for deceleration accordingly). +rotational trapezoidal shapes. The rotational acceleration is +calculated as ``max_trans_acc / max_trans_vel * max_rot_vel`` +(and for deceleration accordingly). Planning Interface ------------------ -This package uses ``moveit_msgs::MotionPlanRequest`` and ``moveit_msgs::MotionPlanResponse`` +This package uses ``moveit_msgs::msgs::MotionPlanRequest`` and ``moveit_msgs::msg::MotionPlanResponse`` as input and output for motion planning. The parameters needed for each planning algorithm are explained below. -For a general introduction on how to fill a ``MotionPlanRequest`` see +For a general introduction on how to fill a ``MotionPlanRequest``, see :ref:`move_group_interface-planning-to-pose-goal`. -You can specify "PTP", "LIN" or "CIRC" as the ``planner\_id``of the ``MotionPlanRequest``. +You can specify ``"PTP"``, ``"LIN"`` or ``"CIRC"`` as the ``planner_id`` of the ``MotionPlanRequest``. The PTP motion command ---------------------- @@ -95,12 +90,12 @@ phases as the lead axis. PTP Input Parameters in ``moveit_msgs::MotionPlanRequest`` ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -- ``planner_id``: PTP -- ``group_name``: name of the planning group +- ``planner_id``: ``"PTP"`` +- ``group_name``: the name of the planning group - ``max_velocity_scaling_factor``: scaling factor of maximal joint velocity - ``max_acceleration_scaling_factor``: scaling factor of maximal joint acceleration/deceleration -- ``start_state/joint_state/(name, position and velocity``: joint name/position/velocity(optional) of the start state. -- ``goal_constraints`` (goal can be given in joint space or Cartesian space) +- ``start_state/joint_state/(name, position and velocity)``: joint name/position/velocity (optional) of the start state. +- ``goal_constraints``: (goal can be given in joint space or Cartesian space) - for a goal in joint space - ``goal_constraints/joint_constraints/joint_name``: goal joint name - ``goal_constraints/joint_constraints/position``: goal joint position @@ -123,13 +118,13 @@ PTP Planning Result in ``moveit_msg::MotionPlanResponse`` positions/velocities/accelerations of all joints (same order as the joint names) and time from start. The last point will have zero velocity and acceleration. -- ``group_name``: name of the planning group +- ``group_name``: the name of the planning group - ``error_code/val``: error code of the motion planning The LIN motion command ---------------------- -This planner generates linear Cartesian trajectory between goal and +This planner generates a linear Cartesian trajectory between goal and start poses. The planner uses the Cartesian limits to generate a trapezoidal velocity profile in Cartesian space. The translational motion is a linear interpolation between start and goal position vector. @@ -143,8 +138,8 @@ violation of joint space limits. LIN Input Parameters in ``moveit_msgs::MotionPlanRequest`` ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -- ``planner_id``: LIN -- ``group_name``: name of the planning group +- ``planner_id``: ``"LIN"`` +- ``group_name``: the name of the planning group - ``max_velocity_scaling_factor``: scaling factor of maximal Cartesian translational/rotational velocity - ``max_acceleration_scaling_factor``: scaling factor of maximal @@ -184,7 +179,7 @@ LIN Planning Result in ``moveit_msg::MotionPlanResponse`` positions/velocities/accelerations of all joints (same order as the joint names) and time from start. The last point will have zero velocity and acceleration. -- ``group_name``: name of the planning group +- ``group_name``: the name of the planning group - ``error_code/val``: error code of the motion planning The CIRC motion command @@ -206,7 +201,7 @@ velocity/acceleration/deceleration need to be set and the planner uses these limits to generate a trapezoidal velocity profile in Cartesian space. The rotational motion is quaternion slerp between start and goal orientation. The translational and rotational motion is synchronized in -time. This planner only accepts start state with zero velocity. Planning +time. This planner only accepts start state with zero velocity. The planning result is a joint trajectory. The user needs to adapt the Cartesian velocity/acceleration scaling factor if motion plan fails due to violation of joint limits. @@ -214,8 +209,8 @@ violation of joint limits. CIRC Input Parameters in ``moveit_msgs::MotionPlanRequest`` ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -- ``planner_id``: CIRC -- ``group_name``: name of the planning group +- ``planner_id``: ``"CIRC"`` +- ``group_name``: the name of the planning group - ``max_velocity_scaling_factor``: scaling factor of maximal Cartesian translational/rotational velocity - ``max_acceleration_scaling_factor``: scaling factor of maximal @@ -262,42 +257,87 @@ CIRC Planning Result in ``moveit_msg::MotionPlanResponse`` positions/velocities/accelerations of all joints (same order as the joint names) and time from start. The last point will have zero velocity and acceleration. -- ``group_name``: name of the planning group +- ``group_name``: the name of the planning group - ``error_code/val``: error code of the motion planning -Example -------- +Examples +-------- By running :: - roslaunch prbt_moveit_config demo.launch + ros2 launch moveit2_tutorials demo.launch.py rviz_config:=panda_hello_moveit.rviz -the user can interact with the planner through rviz. +you can interact with the planner through the RViz MotionPlanning panel. .. figure:: rviz_planner.png :alt: rviz figure +To use the planner through the MoveGroup Interface, refer to +:codedir:`the MoveGroup Interface C++ example `. +To run this, execute the following commands in separate Terminals: + +:: + + ros2 launch moveit2_tutorials demo.launch.py rviz_config:=panda_hello_moveit.rviz + ros2 run moveit2_tutorials pilz_move_group + + +To use the planner using MoveIt Task Constructor, refer to +:codedir:`the MoveIt Task Constructor C++ example `. +To run this, execute the following commands in separate Terminals: + +:: + + ros2 launch moveit2_tutorials mtc_demo.launch.py + ros2 launch moveit2_tutorials pilz_mtc.launch.py + Using the planner ----------------- The *pilz_industrial_motion_planner::CommandPlanner* is provided as a MoveIt Motion Planning Pipeline and, therefore, can be used with all other manipulators using MoveIt. Loading the plugin requires the param -``/move_group/planning_plugin`` to be set to ``pilz_industrial_motion_planner::CommandPlanner`` +``/move_group//planning_plugin`` to be set to ``pilz_industrial_motion_planner/CommandPlanner`` before the ``move_group`` node is started. +For example, the `panda_moveit_config package +`_ +has a ``pilz_industrial_motion_planner`` pipeline set up as follows: + + +:: + + ros2 param get /move_group pilz_industrial_motion_planner.planning_plugin + + String value is: pilz_industrial_motion_planner/CommandPlanner + -To use the command planner cartesian limits have to be defined. The +To use the command planner, Cartesian limits have to be defined. The limits are expected to be under the namespace -``_planning``. Where ```` refers -to the parameter under which the URDF is loaded. E.g. if the URDF was -loaded into ``/robot_description`` the cartesian limits have to be -defined at ``/robot_description_planning``. +``_planning``, where ```` refers +to the parameter name under which the URDF is loaded. +For example, if the URDF was loaded into ``/robot_description`` the +Cartesian limits have to be defined at ``/robot_description_planning``. + +You can set these using a ``pilz_cartesian_limits.yaml`` file in your +``*_moveit_config`` package. +An example showing this file can be found in `panda_moveit_config +`_. + +To verify the limits were set correctly, you can check the parameters for your +``move_group`` node. For example, + +:: + + ros2 param list /move_group --filter .*cartesian_limits + + /move_group: + robot_description_planning.cartesian_limits.max_rot_vel + robot_description_planning.cartesian_limits.max_trans_acc + robot_description_planning.cartesian_limits.max_trans_dec + robot_description_planning.cartesian_limits.max_trans_vel -An example showing the cartesian limits which have to be defined can be -found in `prbt_moveit_config -`_. Sequence of multiple segments ============================= @@ -316,9 +356,10 @@ multiple groups (e.g. "Manipulator", "Gripper") User interface sequence capability ---------------------------------- -A specialized MoveIt capability takes a -``moveit_msgs::MotionSequenceRequest`` as input. The request contains a -list of subsequent goals as described above and an additional +A specialized MoveIt functionality known as the +:moveit_codedir:`command list manager` +takes a ``moveit_msgs::msg::MotionSequenceRequest`` as input. +The request contains a list of subsequent goals as described above and an additional ``blend_radius`` parameter. If the given ``blend_radius`` in meter is greater than zero, the corresponding trajectory is merged together with the following goal such that the robot does not stop at the current @@ -330,8 +371,8 @@ the trajectory it would have taken without blending. .. figure:: blend_radius.png :alt: blend figure -Implementation details are available `as pdf -`_. +Implementation details are available +:moveit_codedir:`as PDF`. Restrictions for ``MotionSequenceRequest`` ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -345,18 +386,18 @@ Restrictions for ``MotionSequenceRequest`` Action interface ~~~~~~~~~~~~~~~~ -In analogy to the ``MoveGroup`` action interface the user can plan and +In analogy to the ``MoveGroup`` action interface, the user can plan and execute a ``moveit_msgs::MotionSequenceRequest`` through the action server at ``/sequence_move_group``. In one point the ``MoveGroupSequenceAction`` differs from the standard MoveGroup capability: If the robot is already at the goal position, the path is still executed. The underlying PlannerManager can check, if the -constraints of an individual ``moveit_msgs::MotionPlanRequest`` are +constraints of an individual ``moveit_msgs::msg::MotionPlanRequest`` are already satisfied but the ``MoveGroupSequenceAction`` capability doesn't implement such a check to allow moving on a circular or comparable path. -See the ``pilz_robot_programming`` package for an `example python script +See the ``pilz_robot_programming`` package for an `ROS1 python script `_ that shows how to use the capability. @@ -364,5 +405,5 @@ Service interface ~~~~~~~~~~~~~~~~~ The service ``plan_sequence_path`` allows the user to generate a joint -trajectory for a ``moveit_msgs::MotionSequenceRequest``. The trajectory is -returned and not executed. +trajectory for a ``moveit_msgs::msg::MotionSequenceRequest``. +The trajectory is returned and not executed. diff --git a/doc/examples/pilz_industrial_motion_planner/rviz_planner.png b/doc/examples/pilz_industrial_motion_planner/rviz_planner.png index 759be7e3a4..6ceb5b0468 100644 Binary files a/doc/examples/pilz_industrial_motion_planner/rviz_planner.png and b/doc/examples/pilz_industrial_motion_planner/rviz_planner.png differ diff --git a/doc/examples/pilz_industrial_motion_planner/src/pilz_move_group.cpp b/doc/examples/pilz_industrial_motion_planner/src/pilz_move_group.cpp new file mode 100644 index 0000000000..b66b9fd2b6 --- /dev/null +++ b/doc/examples/pilz_industrial_motion_planner/src/pilz_move_group.cpp @@ -0,0 +1,183 @@ +#include +#include + +#include +#include +#include + +/** + * Pilz Example -- MoveGroup Interface + * + * To run this example, first run this launch file: + * ros2 launch moveit2_tutorials demo.launch.py rviz_config:=panda_hello_moveit.rviz + * + * For best results, hide the "MotionPlanning" widget in RViz. + * + * Then, run this file: + * ros2 run moveit2_tutorials pilz_move_group + */ + +int main(int argc, char* argv[]) +{ + // Initialize ROS and create the Node + rclcpp::init(argc, argv); + auto const node = std::make_shared( + "pilz_move_group_node", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); + + // We spin up a SingleThreadedExecutor so MoveItVisualTools interact with ROS + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + auto spinner = std::thread([&executor]() { executor.spin(); }); + + // Create a ROS logger + auto const logger = rclcpp::get_logger("pilz_move_group"); + + // Create the MoveIt MoveGroup Interface + using moveit::planning_interface::MoveGroupInterface; + auto move_group_interface = MoveGroupInterface(node, "panda_arm"); + + // Construct and initialize MoveItVisualTools + auto moveit_visual_tools = + moveit_visual_tools::MoveItVisualTools{ node, "panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC, + move_group_interface.getRobotModel() }; + moveit_visual_tools.deleteAllMarkers(); + moveit_visual_tools.loadRemoteControl(); + + // Create closures for visualization + auto const draw_title = [&moveit_visual_tools](const auto& text) { + auto const text_pose = [] { + auto msg = Eigen::Isometry3d::Identity(); + msg.translation().z() = 1.0; + return msg; + }(); + moveit_visual_tools.publishText(text_pose, text, rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE); + }; + auto const prompt = [&moveit_visual_tools](const auto& text) { moveit_visual_tools.prompt(text); }; + auto const draw_trajectory_tool_path = + [&moveit_visual_tools, jmg = move_group_interface.getRobotModel()->getJointModelGroup("panda_arm")]( + auto const trajectory) { moveit_visual_tools.publishTrajectoryLine(trajectory, jmg); }; + + // Helper to plan and execute motion + auto const plan_and_execute = [&](const std::string& title) { + prompt("Press 'Next' in the RVizVisualToolsGui window to plan"); + draw_title("Planning " + title); + moveit_visual_tools.trigger(); + auto const [success, plan] = [&move_group_interface] { + moveit::planning_interface::MoveGroupInterface::Plan msg; + auto const ok = static_cast(move_group_interface.plan(msg)); + return std::make_pair(ok, msg); + }(); + + // Execute the plan + if (success) + { + draw_trajectory_tool_path(plan.trajectory_); + moveit_visual_tools.trigger(); + prompt("Press 'Next' in the RVizVisualToolsGui window to execute"); + draw_title("Executing " + title); + moveit_visual_tools.trigger(); + move_group_interface.execute(plan); + } + else + { + RCLCPP_ERROR(logger, "Planning failed!"); + draw_title("Planning Failed!"); + moveit_visual_tools.trigger(); + } + }; + + // Plan and execute a multi-step sequence using Pilz + move_group_interface.setPlanningPipelineId("pilz_industrial_motion_planner"); + + { + // Move to a pre-grasp pose + move_group_interface.setPlannerId("PTP"); + auto const pre_grasp_pose = [] { + geometry_msgs::msg::PoseStamped msg; + msg.header.frame_id = "world"; + msg.pose.orientation.x = 1.0; + msg.pose.orientation.y = 0.0; + msg.pose.orientation.z = 0.0; + msg.pose.orientation.w = 0.0; + msg.pose.position.x = 0.6; + msg.pose.position.y = -0.2; + msg.pose.position.z = 0.6; + return msg; + }(); + move_group_interface.setPoseTarget(pre_grasp_pose, "panda_hand"); + plan_and_execute("[PTP] Approach"); + } + + { + // Move in a linear trajectory to a grasp pose using the LIN planner. + move_group_interface.setPlannerId("LIN"); + auto const grasp_pose = [] { + geometry_msgs::msg::PoseStamped msg; + msg.header.frame_id = "world"; + msg.pose.orientation.x = 1.0; + msg.pose.orientation.y = 0.0; + msg.pose.orientation.z = 0.0; + msg.pose.orientation.w = 0.0; + msg.pose.position.x = 0.6; + msg.pose.position.y = -0.2; + msg.pose.position.z = 0.4; + return msg; + }(); + move_group_interface.setPoseTarget(grasp_pose, "panda_hand"); + plan_and_execute("[LIN] Grasp"); + } + + { + // Move in a circular arc motion using the CIRC planner. + move_group_interface.setPlannerId("CIRC"); + auto const goal_pose = [] { + geometry_msgs::msg::PoseStamped msg; + msg.header.frame_id = "world"; + msg.pose.orientation.x = 0.7071; + msg.pose.orientation.y = 0.0; + msg.pose.orientation.z = 0.0; + msg.pose.orientation.w = 0.7071; + msg.pose.position.x = 0.6; + msg.pose.position.y = 0.0; + msg.pose.position.z = 0.6; + return msg; + }(); + move_group_interface.setPoseTarget(goal_pose, "panda_hand"); + + // Set a constraint pose. This is the center of the arc. + auto const center_pose = [] { + geometry_msgs::msg::PoseStamped msg; + msg.header.frame_id = "world"; + msg.pose.orientation.x = 1.0; + msg.pose.orientation.y = 0.0; + msg.pose.orientation.z = 0.0; + msg.pose.orientation.w = 0.0; + msg.pose.position.x = 0.6; + msg.pose.position.y = 0.0; + msg.pose.position.z = 0.4; + return msg; + }(); + moveit_msgs::msg::Constraints constraints; + constraints.name = "center"; // Change to "interim" to use an intermediate point on arc instead. + moveit_msgs::msg::PositionConstraint pos_constraint; + pos_constraint.header.frame_id = center_pose.header.frame_id; + pos_constraint.link_name = "panda_hand"; + pos_constraint.constraint_region.primitive_poses.push_back(center_pose.pose); + pos_constraint.weight = 1.0; + constraints.position_constraints.push_back(pos_constraint); + move_group_interface.setPathConstraints(constraints); + + plan_and_execute("[CIRC] Turn"); + } + + { + // Move back home using the PTP planner. + move_group_interface.setPlannerId("PTP"); + move_group_interface.setNamedTarget("ready"); + plan_and_execute("[PTP] Return"); + } + + // Shutdown ROS + rclcpp::shutdown(); + return 0; +} diff --git a/doc/examples/pilz_industrial_motion_planner/src/pilz_mtc.cpp b/doc/examples/pilz_industrial_motion_planner/src/pilz_mtc.cpp new file mode 100644 index 0000000000..b45f6f128f --- /dev/null +++ b/doc/examples/pilz_industrial_motion_planner/src/pilz_mtc.cpp @@ -0,0 +1,300 @@ +#include +#include +#include +#include + +#if __has_include() +#include +#else +#include +#endif + +/** + * Pilz Example -- MoveIt Task Constructor + * + * To run this example, first run this launch file: + * ros2 launch moveit2_tutorials mtc_demo.launch.py + * + * Then, run this file through its own launch file: + * + */ + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("pilz_mtc"); +namespace mtc = moveit::task_constructor; + +class MTCTaskNode +{ +public: + MTCTaskNode(const rclcpp::NodeOptions& options); + + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface(); + + void doTask(); + + void setupPlanningScene(); + +private: + // Compose an MTC task from a series of stages. + mtc::Task createTask(); + mtc::Task task_; + rclcpp::Node::SharedPtr node_; +}; + +rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MTCTaskNode::getNodeBaseInterface() +{ + return node_->get_node_base_interface(); +} + +MTCTaskNode::MTCTaskNode(const rclcpp::NodeOptions& options) + : node_{ std::make_shared("pilz_mtc_node", options) } +{ +} + +void MTCTaskNode::doTask() +{ + task_ = createTask(); + + try + { + task_.init(); + } + catch (mtc::InitStageException& e) + { + RCLCPP_ERROR_STREAM(LOGGER, e); + return; + } + + if (!task_.plan()) + { + RCLCPP_ERROR_STREAM(LOGGER, "Task planning failed"); + return; + } + task_.introspection().publishSolution(*task_.solutions().front()); + + auto result = task_.execute(*task_.solutions().front()); + if (result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + { + RCLCPP_ERROR_STREAM(LOGGER, "Task execution failed"); + return; + } + + return; +} + +mtc::Task MTCTaskNode::createTask() +{ + mtc::Task task; + task.stages()->setName("pilz example task"); + task.loadRobotModel(node_); + + const auto& arm_group_name = "panda_arm"; + const auto& hand_group_name = "hand"; + const auto& hand_frame = "panda_hand"; + + // Set task properties + task.setProperty("group", arm_group_name); + task.setProperty("eef", hand_group_name); + task.setProperty("ik_frame", hand_frame); + + // Set up a simple joint interpolation planner for the gripper + auto interpolation_planner = std::make_shared(); + + // Set up 3 separate pilz planners with different IDs + auto pilz_ptp_planner = std::make_shared(node_, "pilz_industrial_motion_planner"); + pilz_ptp_planner->setPlannerId("PTP"); + pilz_ptp_planner->setProperty("max_velocity_scaling_factor", 0.5); + pilz_ptp_planner->setProperty("max_acceleration_scaling_factor", 0.5); + + auto pilz_lin_planner = std::make_shared(node_, "pilz_industrial_motion_planner"); + pilz_lin_planner->setPlannerId("LIN"); + pilz_lin_planner->setProperty("max_velocity_scaling_factor", 0.2); + pilz_lin_planner->setProperty("max_acceleration_scaling_factor", 0.2); + + auto pilz_circ_planner = std::make_shared(node_, "pilz_industrial_motion_planner"); + pilz_circ_planner->setPlannerId("CIRC"); + pilz_circ_planner->setProperty("max_velocity_scaling_factor", 0.3); + pilz_circ_planner->setProperty("max_acceleration_scaling_factor", 0.3); + + { + // Start at current state. + auto stage_state_current = std::make_unique("current"); + task.add(std::move(stage_state_current)); + } + + { + // Go to a pre-grasp pose using the PTP planner. + auto stage = std::make_unique("go to approach", pilz_ptp_planner); + stage->setGroup(arm_group_name); + stage->setIKFrame(hand_frame); + + // Set the approach pose. + auto const goal_pose = [] { + geometry_msgs::msg::PoseStamped msg; + msg.header.frame_id = "world"; + msg.pose.orientation.x = 1.0; + msg.pose.orientation.y = 0.0; + msg.pose.orientation.z = 0.0; + msg.pose.orientation.w = 0.0; + msg.pose.position.x = 0.6; + msg.pose.position.y = -0.2; + msg.pose.position.z = 0.6; + return msg; + }(); + stage->setGoal(goal_pose); + + task.add(std::move(stage)); + } + + { + // Move in a straight line towards the grasp using the LIN planner. + auto stage = std::make_unique("go to grasp", pilz_lin_planner); + stage->setGroup(arm_group_name); + stage->setIKFrame(hand_frame); + + // Set the acceleration and velocity scaling factors. + pilz_lin_planner->setProperty("max_velocity_scaling_factor", 0.3); + pilz_lin_planner->setProperty("max_acceleration_scaling_factor", 0.3); + + // Set the post-approach pose. + auto const goal_pose = [] { + geometry_msgs::msg::PoseStamped msg; + msg.header.frame_id = "world"; + msg.pose.orientation.x = 1.0; + msg.pose.orientation.y = 0.0; + msg.pose.orientation.z = 0.0; + msg.pose.orientation.w = 0.0; + msg.pose.position.x = 0.6; + msg.pose.position.y = -0.2; + msg.pose.position.z = 0.4; + return msg; + }(); + stage->setGoal(goal_pose); + + task.add(std::move(stage)); + } + + { + // Close the hand. + auto stage = std::make_unique("close hand", interpolation_planner); + stage->setGroup(hand_group_name); + stage->setGoal("close"); + task.add(std::move(stage)); + } + + { + // Move in a circular arc using the CIRC planner. + auto stage = std::make_unique("move circle", pilz_circ_planner); + stage->setGroup(arm_group_name); + stage->setIKFrame(hand_frame); + + // Set the pose goal. + auto const goal_pose = [] { + geometry_msgs::msg::PoseStamped msg; + msg.header.frame_id = "world"; + msg.pose.orientation.x = 0.7071; + msg.pose.orientation.y = 0.0; + msg.pose.orientation.z = 0.0; + msg.pose.orientation.w = 0.7071; + msg.pose.position.x = 0.6; + msg.pose.position.y = 0.0; + msg.pose.position.z = 0.6; + return msg; + }(); + stage->setGoal(goal_pose); + + // Define the arc center pose. + auto const center_pose = [] { + geometry_msgs::msg::PoseStamped msg; + msg.header.frame_id = "world"; + msg.pose.orientation.x = 1.0; + msg.pose.orientation.y = 0.0; + msg.pose.orientation.z = 0.0; + msg.pose.orientation.w = 0.0; + msg.pose.position.x = 0.6; + msg.pose.position.y = 0.0; + msg.pose.position.z = 0.4; + return msg; + }(); + + // Define the arc constraint. + moveit_msgs::msg::PositionConstraint pos_constraint; + pos_constraint.header.frame_id = center_pose.header.frame_id; + pos_constraint.link_name = hand_frame; + pos_constraint.constraint_region.primitive_poses.resize(1); + pos_constraint.constraint_region.primitive_poses[0] = center_pose.pose; + pos_constraint.weight = 1.0; + + moveit_msgs::msg::Constraints path_constraints; + path_constraints.name = "center"; // Change to "interim" to use an intermediate point on arc instead. + path_constraints.position_constraints.resize(1); + path_constraints.position_constraints[0] = pos_constraint; + stage->setPathConstraints(path_constraints); + + task.add(std::move(stage)); + } + + { + // Open the hand. + auto stage = std::make_unique("open hand", interpolation_planner); + stage->setGroup(hand_group_name); + stage->setGoal("open"); + task.add(std::move(stage)); + } + + { + // Move in a straight line away from the grasp using the LIN planner. + auto stage = std::make_unique("retract grasp", pilz_lin_planner); + stage->setGroup(arm_group_name); + stage->setIKFrame(hand_frame); + + // Set the retract ppose + auto const goal_pose = [] { + geometry_msgs::msg::PoseStamped msg; + msg.header.frame_id = "world"; + msg.pose.orientation.x = 0.7071; + msg.pose.orientation.y = 0.0; + msg.pose.orientation.z = 0.0; + msg.pose.orientation.w = 0.7071; + msg.pose.position.x = 0.6; + msg.pose.position.y = 0.1; + msg.pose.position.z = 0.6; + return msg; + }(); + stage->setGoal(goal_pose); + + task.add(std::move(stage)); + } + + { + // Return to the original ready pose using the PTP planner. + auto stage = std::make_unique("return home", pilz_ptp_planner); + stage->setGroup(arm_group_name); + stage->setGoal("ready"); + task.add(std::move(stage)); + } + + return task; +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + rclcpp::NodeOptions options; + options.automatically_declare_parameters_from_overrides(true); + + auto mtc_task_node = std::make_shared(options); + rclcpp::executors::MultiThreadedExecutor executor; + + auto spin_thread = std::make_unique([&executor, &mtc_task_node]() { + executor.add_node(mtc_task_node->getNodeBaseInterface()); + executor.spin(); + executor.remove_node(mtc_task_node->getNodeBaseInterface()); + }); + + mtc_task_node->doTask(); + + spin_thread->join(); + rclcpp::shutdown(); + return 0; +}