From 200cbc65602a29e416f608c08c6d0b3d54ae3b90 Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Sun, 21 Mar 2021 23:37:43 +0900 Subject: [PATCH 1/4] Update MGI tutorial --- .../src/move_group_interface_tutorial.cpp | 24 +++++++++++++++---- 1 file changed, 19 insertions(+), 5 deletions(-) diff --git a/doc/move_group_interface/src/move_group_interface_tutorial.cpp b/doc/move_group_interface/src/move_group_interface_tutorial.cpp index 6d81543b4..db878601d 100644 --- a/doc/move_group_interface/src/move_group_interface_tutorial.cpp +++ b/doc/move_group_interface/src/move_group_interface_tutorial.cpp @@ -115,6 +115,9 @@ int main(int argc, char** argv) ROS_INFO_NAMED("tutorial", "Available Planning Groups:"); std::copy(move_group_interface.getJointModelGroupNames().begin(), move_group_interface.getJointModelGroupNames().end(), std::ostream_iterator(std::cout, ", ")); + + // We can get the planning pipeline that is set (OMPL by default): + ROS_INFO_NAMED("tutorial", "Current pipeline: %s", move_group_interface.getPlanningPipelineId().c_str()); // Start the demo // ^^^^^^^^^^^^^^^^^^^^^^^^^ @@ -152,17 +155,28 @@ int main(int argc, char** argv) visual_tools.trigger(); visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); - // Finally, to execute the trajectory stored in my_plan, you could use the following method call: + // To execute the trajectory stored in my_plan, you can use the following method call: // Note that this can lead to problems if the robot moved in the meanwhile. // move_group_interface.execute(my_plan); + // You can also use a different planning pipeline (OMPL by default), such as the Pilz + // industrial motion planner: + // move_group_interface.setPlanningPipelineId("pilz_industrial_motion_planner"); + // move_group_interface.setPlannerId("LIN"); + + // Or, to plan only one motion with a different planner: + + success = (move_group_interface.plan(my_plan, "pilz_industrial_motion_planner", "LIN") == moveit::planning_interface::MoveItErrorCode::SUCCESS); + visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); + visual_tools.trigger(); + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + // Moving to a pose goal // ^^^^^^^^^^^^^^^^^^^^^ // - // If you do not want to inspect the planned trajectory, - // the following is a more robust combination of the two-step plan+execute pattern shown above - // and should be preferred. Note that the pose goal we had set earlier is still active, - // so the robot will try to move to that goal. + // If you do not want to inspect the planned trajectory, you can call the "move" + // command to combine the plan+execute pattern shown above. Note that the pose goal + // we set earlier is still active, so the robot will try to move to that goal. // move_group_interface.move(); From c0ce0fdeaab2978db60388f47e0e37d1b2d0040f Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Mon, 5 Apr 2021 00:27:02 +0900 Subject: [PATCH 2/4] Add planning pipeline/planner ID parameter example The new pose works for both PTP and LIN. The previous one fails for LIN. --- .../move_group_python_interface_tutorial.py | 23 ++++++++++++------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py b/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py index e25aec5ff..4b7371bfc 100755 --- a/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py +++ b/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py @@ -178,12 +178,12 @@ def go_to_joint_state(self): ## We use the constant `tau = 2*pi `_ for convenience: # We get the joint values from the group and change some of the values: joint_goal = move_group.get_current_joint_values() - joint_goal[0] = 0 - joint_goal[1] = -tau/8 - joint_goal[2] = 0 - joint_goal[3] = -tau/4 - joint_goal[4] = 0 - joint_goal[5] = tau/6 # 1/6 of a turn + joint_goal[0] = -tau/8 + joint_goal[1] = -tau/6 + joint_goal[2] = tau/6 # 1/6 of a turn + joint_goal[3] = -tau/3 + joint_goal[4] = -tau/12 + joint_goal[5] = tau/4 joint_goal[6] = 0 # The go command can be called with joint values, poses, or without any @@ -220,8 +220,15 @@ def go_to_pose_goal(self): move_group.set_pose_target(pose_goal) - ## Now, we call the planner to compute the plan and execute it. - plan = move_group.go(wait=True) + ## We call the planner to compute the plan and execute it. + plan = move_group.plan() + # "move_group.execute(plan, wait=True)" would execute the plan + + ## We can also specify a planner directly. The default planning pipeline in Moveit is OMPL. + ## The Pilz Industrial Motion planner offers PTP, LIN and CIRC motions. + ## See `the Pilz tutorial <../pilz_industrial_motion_planner/pilz_industrial_motion_planner.html>`_ for details. + plan = move_group.go(wait=True, planning_pipeline="pilz_industrial_motion_planner", planner_id="LIN") + # Calling `stop()` ensures that there is no residual movement move_group.stop() # It is always good to clear your targets after planning with poses. From 17f7c77d929307df16e9b5e5878d23b89eb20278 Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Mon, 5 Apr 2021 00:28:02 +0900 Subject: [PATCH 3/4] Clean up --- .../scripts/move_group_python_interface_tutorial.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py b/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py index 4b7371bfc..629c7defe 100755 --- a/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py +++ b/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py @@ -255,7 +255,7 @@ def plan_cartesian_path(self, scale=1): ## Cartesian Paths ## ^^^^^^^^^^^^^^^ ## You can plan a Cartesian path directly by specifying a list of waypoints - ## for the end-effector to go through. If executing interactively in a + ## for the end-effector to go through. If executing interactively in a ## Python shell, set scale = 1.0. ## waypoints = [] @@ -309,7 +309,7 @@ def display_trajectory(self, plan): display_trajectory.trajectory_start = robot.get_current_state() display_trajectory.trajectory.append(plan) # Publish - display_trajectory_publisher.publish(display_trajectory); + display_trajectory_publisher.publish(display_trajectory) ## END_SUB_TUTORIAL From 555875fa655397869f9865d5baca03e274e3d793 Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Tue, 4 May 2021 15:26:31 +0900 Subject: [PATCH 4/4] Fix comment Co-authored-by: Dave Coleman --- .../scripts/move_group_python_interface_tutorial.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py b/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py index 629c7defe..8cd81e494 100755 --- a/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py +++ b/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py @@ -220,7 +220,7 @@ def go_to_pose_goal(self): move_group.set_pose_target(pose_goal) - ## We call the planner to compute the plan and execute it. + ## We call the planner to compute the plan. plan = move_group.plan() # "move_group.execute(plan, wait=True)" would execute the plan