!

Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Moveit Tutorial

Getting started

First of all you need to install the moveit itself, installing moveit is easy just open a new terminal and type the following command

$ sudo apt-get install ros-indigo-moveit-full

Moveit

NOTE: Right now there only two komodo model which support moveit functionality

• Komodo with arm.
• Komodo with elevator and arm.

Moveit api

About the API

In MoveIt!, the primary user interface is through the RobotCommander class. It provides functionality for most operations that a user may want to carry out, specifically setting joint or pose goals, creating motion plans, moving the robot, adding objects into the environment and attaching/detaching objects from the robot.

Setup

To use the python interface to move_group, import the moveit_commander module. We also import rospy and some messages that we will use.

   1 import sys
   2 import copy
   3 import rospy
   4 import moveit_commander
   5 import moveit_msgs.msg
   6 import geometry_msgs.msg

First initialize moveit_commander and rospy.

   1 print "============ Starting tutorial setup"
   2 moveit_commander.roscpp_initialize(sys.argv)
   3 rospy.init_node('move_group_python_interface_tutorial', anonymous=True)

Instantiate a RobotCommander object. This object is an interface to the robot as a whole.

   1 robot = moveit_commander.RobotCommander()

Instantiate a PlanningSceneInterface object. This object is an interface to the world surrounding the robot.

   1 scene = moveit_commander.PlanningSceneInterface()

Instantiate a MoveGroupCommander object. This object is an interface to one group of joints. In this case the group is the joints in the arm. This interface can be used to plan and execute motions on the arm.

   1 group = moveit_commander.MoveGroupCommander("arm")

We create this DisplayTrajectory publisher which is used below to publish trajectories for RVIZ to visualize.

   1 display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',                                      moveit_msgs.msg.DisplayTrajectory)

Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up.

   1 print "============ Waiting for RVIZ..."
   2 rospy.sleep(10)
   3 print "============ Starting tutorial "

Getting Basic Information

We can get the name of the reference frame for this robot

   1 print "============ Reference frame: %s" % group.get_planning_frame()

We can also print the name of the end-effector link for this group

   1 print "============ Reference frame: %s" % group.get_end_effector_link()

We can get a list of all the groups in the robot

   1 print "============ Robot Groups:"
   2 print robot.get_group_names()

Sometimes for debugging it is useful to print the entire state of the robot.

   1 print "============ Printing robot state"
   2 print robot.get_current_state()
   3 print "============"

Planning to a Pose goal

We can plan a motion for this group to a desired pose for the end-effector

   1 print "============ Generating plan 1"
   2 pose_target = geometry_msgs.msg.Pose()
   3 pose_target.orientation.w = 1.0
   4 pose_target.position.x = 0.7
   5 pose_target.position.y = -0.05
   6 pose_target.position.z = 1.1
   7 group.set_pose_target(pose_target)

Now, we call the planner to compute the plan and visualize it if successful Note that we are just planning, not asking move_group to actually move the robot

   1 plan1 = group.plan()
   2 
   3 print "============ Waiting while RVIZ displays plan1..."
   4 rospy.sleep(5)

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).

   1 print "============ Visualizing plan1"
   2 display_trajectory = moveit_msgs.msg.DisplayTrajectory()
   3 
   4 display_trajectory.trajectory_start = robot.get_current_state()
   5 display_trajectory.trajectory.append(plan1)
   6 display_trajectory_publisher.publish(display_trajectory);
   7 
   8 print "============ Waiting while plan1 is visualized (again)..."
   9 rospy.sleep(5)

Moving to a pose goal

Moving to a pose goal is similar to the step above except we now use the go() function. Note that the pose goal we had set earlier is still active and so the robot will try to move to that goal. We will not use that function in this tutorial since it is a blocking function and requires a controller to be active and report success on execution of a trajectory.

   1 # Uncomment below line when working with a real robot
   2 # group.go(wait=True)

Planning to a joint-space goal

Let’s set a joint space goal and move towards it. First, we will clear the pose target we had just set.

   1 group.clear_pose_targets()

Then, we will get the current set of joint values for the group

   1 group_variable_values = group.get_current_joint_values()
   2 print "============ Joint values: ", group_variable_values

Now, let’s modify one of the joints, plan to the new joint space goal and visualize the plan

   1 group_variable_values[0] = 1.0
   2 group.set_joint_value_target(group_variable_values)
   3 
   4 plan2 = group.plan()
   5 
   6 print "============ Waiting while RVIZ displays plan2..."
   7 rospy.sleep(5)

Cartesian Paths

You can plan a cartesian path directly by specifying a list of waypoints for the end-effector to go through.

   1 waypoints = []
   2 
   3 # start with the current pose
   4 waypoints.append(group.get_current_pose().pose)
   5 
   6 # first orient gripper and move forward (+x)
   7 wpose = geometry_msgs.msg.Pose()
   8 wpose.orientation.w = 1.0
   9 wpose.position.x = waypoints[0].position.x + 0.1
  10 wpose.position.y = waypoints[0].position.y
  11 wpose.position.z = waypoints[0].position.z
  12 waypoints.append(copy.deepcopy(wpose))
  13 
  14 # second move down
  15 wpose.position.z -= 0.10
  16 waypoints.append(copy.deepcopy(wpose))
  17 
  18 # third move to the side
  19 wpose.position.y += 0.05
  20 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 specify the jump threshold as 0.0, effectively disabling it.

   1 (plan3, fraction) = group.compute_cartesian_path(
   2                              waypoints,   # waypoints to follow
   3                              0.01,        # eef_step
   4                              0.0)         # jump_threshold
   5 
   6 print "============ Waiting while RVIZ displays plan3..."
   7 rospy.sleep(5)

Adding/Removing Objects and Attaching/Detaching Objects

First, we will define the collision object message

   1 collision_object = moveit_msgs.msg.CollisionObject()

When finished shut down moveit_commander.

   1 moveit_commander.roscpp_shutdown()