One of the simplest MoveIt! user interfaces is through the Python-based Move Group Interface. These wrappers provide functionality for most operations that the average user will likely need, specifically setting joint or pose goals, creating motion plans, moving the robot, adding objects into the environment and attaching/detaching objects from the robot.
Watch this quick YouTube video demo to see the power of the Move Group Python interface!
If you haven’t already done so, make sure you’ve completed the steps in Getting Started.
Open two shells. Start RViz and wait for everything to finish loading in the first shell:
roslaunch mara_moveit_config demo.launch
Now run the Python code directly in the other shell using rosrun. Note in some instances you may need to make the python script executable:
rosrun mara_moveit_tutorials move_group_python_interface_tutorial.py
In RViz, we should be able to see the following:
rosrun command in between each step
Note: the entire code can be seen here in the tutorials GitHub repository.
To use the Python MoveIt! interfaces, we will import the moveit_commander namespace. This namespace provides us with a MoveGroupCommander class, a PlanningSceneInterface class, and a RobotCommander class. (More on these below)
We also import rospy and some messages that we will use:
import sys import copy import rospy import moveit_commander import moveit_msgs.msg import geometry_msgs.msg from math import pi from std_msgs.msg import String from moveit_commander.conversions import pose_to_list
moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
Instantiate a RobotCommander object. Provides information such as the robot’s kinematic model and the robot’s current joint states
robot = moveit_commander.RobotCommander()
Instantiate a MoveGroupCommander object. This object is an interface to a planning group (group of joints). In this tutorial the group is the primary arm joints in the MARA Robot, so we set the group’s name to “manipulator”. If you are using a different robot, change this value to the name of your robot arm planning group. This interface can be used to plan and execute motions:
group_name = "manipulator" move_group = moveit_commander.MoveGroupCommander(group_name)
Create a DisplayTrajectory ROS publisher which is used to display trajectories in Rviz:
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20)
# We can get the name of the reference frame for this robot: planning_frame = move_group.get_planning_frame() print "============ Planning frame: %s" % planning_frame # We can also print the name of the end-effector link for this group: eef_link = move_group.get_end_effector_link() print "============ End effector link: %s" % eef_link # We can get a list of all the groups in the robot: group_names = robot.get_group_names() print "============ Available Planning Groups:", robot.get_group_names() # Sometimes for debugging it is useful to print the entire state of the # robot: print "============ Printing robot state" print robot.get_current_state() print ""
The MARA’s zero configuration is at a singularity so the first thing we want to do is move it to a slightly better configuration.
# We can get the joint values from the group and adjust some of the values: joint_goal = move_group.get_current_joint_values() joint_goal = 0 joint_goal = -pi/4 joint_goal = 0 joint_goal = -pi/2 joint_goal = 0 joint_goal = pi/3 # The go command can be called with joint values, poses, or without any # parameters if you have already set the pose or joint target for the group move_group.go(joint_goal, wait=True) # Calling ``stop()`` ensures that there is no residual movement move_group.stop()
We can plan a motion for this group to a desired pose for the end-effector:
pose_goal = geometry_msgs.msg.Pose() pose_goal.orientation.w = 0.5 pose_goal.orientation.x = -0.5 pose_goal.orientation.y = -0.5 pose_goal.orientation.z = -0.5 pose_goal.position.x = -0.19 pose_goal.position.y = -0.26 pose_goal.position.z = 1.54 group.set_pose_target(pose_goal)
Now, we call the planner to compute the plan and execute it.
plan = move_group.go(wait=True) # Calling `stop()` ensures that there is no residual movement move_group.stop() # It is always good to clear your targets after planning with poses. # Note: there is no equivalent function for clear_joint_value_targets() move_group.clear_pose_targets()
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 Python shell, set scale = 1.0.
waypoints =  wpose = group.get_current_pose().pose wpose.position.z -= scale * 0.05 # First move up (z) wpose.position.y += scale * 0.1 # and sideways (y) waypoints.append(copy.deepcopy(wpose)) wpose.position.x += scale * 0.1 # Second move forward/backwards in (x) waypoints.append(copy.deepcopy(wpose)) wpose.position.y -= scale * 0.05 # Third move sideways (y) waypoints.append(copy.deepcopy(wpose)) # We want the Cartesian path to be interpolated at a resolution of 1 cm # which is why we will specify 0.01 as the eef_step in Cartesian # translation. We will disable the jump threshold by setting it to 0.0 disabling: (plan, fraction) = group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold # Note: We are just planning, not asking move_group to actually move the robot yet: return plan, fraction
You can ask RViz to visualize a plan (aka trajectory) for you. But the group.plan() method does this automatically so this is not that useful here (it just displays the same trajectory again):
A DisplayTrajectory msg has two primary fields, trajectory_start and trajectory. We populate the trajectory_start with our current robot state to copy over any AttachedCollisionObjects and add our plan to the trajectory.
display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = robot.get_current_state() display_trajectory.trajectory.append(plan) # Publish display_trajectory_publisher.publish(display_trajectory);
Use execute if you would like the robot to follow the plan that has already been computed:
Note: The robot’s current joint state must be within some tolerance of the first waypoint in the RobotTrajectory or
execute() will fail
First, we will create a box in the planning scene at the location of the left finger:
box_pose = geometry_msgs.msg.PoseStamped() box_pose.header.frame_id = "ee_link" box_pose.pose.orientation.w = 1.0 box_pose.pose.position.z = 0.07 # slightly above the end effector box_name = "box" scene.add_box(box_name, box_pose, size=(0.1, 0.1, 0.1))
If the Python node dies before publishing a collision object update message, the message could get lost and the box will not appear. To ensure that the updates are made, we wait until we see the changes reflected in the
get_known_object_names() lists. For the purpose of this tutorial, we call this function after adding, removing, attaching or detaching an object in the planning scene. We then wait until the updates have been made or timeout seconds have passed
start = rospy.get_time() seconds = rospy.get_time() while (seconds - start < timeout) and not rospy.is_shutdown(): # Test if the box is in attached objects attached_objects = scene.get_attached_objects([box_name]) is_attached = len(attached_objects.keys()) > 0 # Test if the box is in the scene. # Note that attaching the box will remove it from known_objects is_known = box_name in scene.get_known_object_names() # Test if we are in the expected state if (box_is_attached == is_attached) and (box_is_known == is_known): return True # Sleep so that we give other threads time on the processor rospy.sleep(0.1) seconds = rospy.get_time() # If we exited the while loop without returning then we timed out return False
Next, we will attach the box to the MARA's EndEffector. Manipulating objects requires the robot be able to touch them without the planning scene reporting the contact as a collision. By adding link names to the
touch_links array, we are telling the planning scene to ignore collisions between those links and the box. For the MARA robot, we set
grasping_group = 'endeffector'. If you are using a different robot, you should change this value to the name of your end effector group name.
grasping_group = 'ee_link' touch_links = robot.get_link_names(group=grasping_group) scene.attach_box(eef_link, box_name, touch_links=touch_links)
We can also detach and remove the object from the planning scene:
We can remove the box from the world.
Note: The object must be detached before we can remove it from the world
The entire launch file is here on GitHub. All the code in this tutorial can be run from the
mara_moveit_tutorials package that you have as part of your MoveIt! setup.