MoveIt! is a set of tools for mobile manipulation in ROS. The main web page (http://moveit.ros.org) contains documentation, tutorials, and installation instructions as well as example demonstrations with several robotic arms (or robots) that use MoveIt! for manipulation tasks, such as grasping, picking and placing, or simple motion planning with inverse kinematics.
The library incorporates a fast inverse kinematics solver (as part of the motion planning primitives), state-of-the-art algorithms for manipulation, grasping 3D perception (usually in the form of point clouds), kinematics, control, and navigation. Apart from the backend, it provides an easy-to-use GUI to configure new robotic arms with the MoveIt! and RViz plugins to develop motion planning tasks in an intuitive way.
In this chapter, we will see how we can create a simple robotic arm in the URDF format and how we can define motion planning groups with the MoveIt! configuration tool. For a single arm, we will have a single group, so that later we can use the inverse kinematics solvers to perform manipulation tasks specified from the RViz interface. A pick and place task is used to illustrate the capabilities and tools of MoveIt!.
The first section explains the MoveIt! architecture, explaining the basic concepts used in the framework, such as joint groups and planning scene, and general concepts such as trajectory planning, (inverse) kinematics, and collision checking concerns. Then, we will show how you can integrate an arm into MoveIt!, creating the planning groups and scene. Next, we will show you how you can perform motion planning with collisions and how you can incorporate point clouds, which will allow you to avoid collisions with dynamic obstacles.
Finally, perception and object recognition tools will be explained and later used in a pick and place demonstration. For this demonstration, we will use the MoveIt! plugin for RViz.
The architecture of MoveIt! is depicted in the following diagram taken from the concepts sections of its official documentation at http://moveit.ros.org/documentation/concepts/. Here, we describe the main concepts in brief. In order to install MoveIt!, you only have to run this command:
$ sudo apt-get install ros-hydro-moveit-full
Alternatively, you can install all the dependencies of the code that comes with this chapter by running the following command from a workspace that contains it:
$ rosdep install --from-paths src -iy
The following diagram (Figure 1) shows the architecture of MoveIt!:
Figure 1: MoveIt! architecture diagram
In the center of the architecture, we have the move_group element. The main idea is that we have to define groups of joints and other elements to perform moving actions using motion planning algorithms. These algorithms consider a scene with objects to interact with and the joints characteristics of the group.
A group is defined using standard ROS tools and definition languages, such as YAML, URDF, and SDF. In brief, we have to define the joints that are part of a group with their joint limits. Similarly, we define the end effector tools, such as a gripper and perception sensors. The robot must expose JointTrajectoryAction controllers so that the output of the motion planning can be planned and executed on the robot hardware (or simulator). In order to monitor the execution, /joint_states is also needed by means of the robot state publisher. All this is provided by the ROS control as well as specific sensor drivers. Note that MoveIt! provides a GUI wizard to define the joint groups for a given robot, which can be called directly as follows:
$ roslaunch moveit_setup_assistant setup_assistant.launch
Tip
Downloading the example code
You can download the example code files from your account at http://www.packtpub.com for all the Packt Publishing books you have purchased. If you purchased this book elsewhere, you can visit http://www.packtpub.com/support and register to have the files e-mailed directly to you. You can also download these code files from https://github.com/AaronMR/ROS_Book_Hydro.
Once move_group is configured properly, we can interface with it. MoveIt! provides a C++ and a Python API to do so and also an RViz plugin that integrates seamlessly and allows us to send motion goals, plan them, and send (execute) them on the robot, as shown in Figure 2:
Figure 2: MoveIt! integration for simulated manipulator in Gazebo
Motion planning deals with the problem of moving the arm to a configuration, allowing you to reach a pose with the end effector without crashing the move group with any obstacle, that is, the links themselves or other objects perceived by sensors (usually as point clouds) or violating the joint limits. The MoveIt! user interface allows you to use different libraries for motion planning, such as OMPL (http://ompl.kavrakilab.org), using ROS actions or services.
A motion plan request is sent to the motion planning, which takes care of avoiding collisions (including self-collisions) and finds a trajectory for all the joints in the groups that move the arm so that it reaches the goal requested. Such a goal consists of a location in joint space or an end effector pose, which could include an object (for example, if the gripper picks up something) as well as kinematic constraints:
- Position constraints: These restrict the position of a link
- Orientation constraints: These restrict the orientation of a link
- Visibility constraints: These restrict a point on a link to be visible in a particular zone (it falls inside the sensor visibility cone)
- User-specified constraints: These are provided with a user-defined callback
The result of the motion plan is a trajectory that moves the arm to the target goal location. This trajectory also avoids collisions and satisfies the velocity and acceleration constraints at the joint level.
Finally, MoveIt! has a motion planning pipeline made of motion planners and plan request adapters. The latter are components that allow the preprocessing and postprocessing of the motion plan request. For example, preprocessing is useful when the initial state of the arm is outside joint limits; postprocessing is useful to convert paths into time-parameterized trajectories. Some of the default motion planning adapters provided by MoveIt! are as follows:
FixStartStateBounds: This fixes the initial/start state to be inside the joint limits specified in the URDF. Without this adapter, when the joints are outside the joint limits, the motion planner would not be able to find any plan since the arm is already violating the joint limits. The adapter will move the joints into the joint limits but only when the joint state is not outside by a large amount since, in such cases, it is not necessarily the best solution.FixWorkspaceBounds: This defines a default workspace to plan a 10 x 10 x 10 m3 cube.FixStartStateCollision: This will attempt to sample a collision-free configuration near a given configuration in collision. It will do that by disturbing the joint states only by a small amount.FixStartStatePathConstraints: This is applied when the initial state does not obey the given path constraints. It will attempt to find a plan from the initial configuration to a new one that satisfies the path constraints, which will then be used as the initial state for motion planning.AddTimeParameterization: This will time-parameterize the kinematic path typically generated by the motion planner (a path that does not obey any velocity or acceleration constraints) by applying velocity and acceleration constraints given in thejoint_limits.yamlfile of the robot.
The planning scene represents the world around the robot as well as the robot state. This is maintained by the planning scene monitor shown in the next diagram, taken from the concepts section of its official documentation at http://moveit.ros.org/documentation/concepts/. It is a subpart of move_group, which listens to joint_states, the sensor information (usually point clouds), and the world geometry, which is provided by the user input on the planning_scene topic. This is shown in Figure 3:
Figure 3: MoveIt! planning scene diagram
The world geometry monitor uses an occupancy map monitor to build a 3D representation of the environment around the robot and augments it with the planning_scene topic information, such as objects (for example, grasping objects); an octomap is used to register all this information. In order to generate the 3D representation, MoveIt! supports different sensors to perceive the environment by means of plugins supplying two kinds of inputs:
- Point Clouds: This is handled by a point cloud occupancy map updater plugin.
- Depth images: This is handled by the depth image occupancy map updater plugin, including a self-filter that removes visible parts of the robot from the depth map using the robot state information to that end.
Forward kinematics and its Jacobians are integrated in the RobotState class. On the other hand, for inverse kinematics, MoveIt! provides a default plugin that uses a numerical Jacobian-based solver that is automatically configured by the setup assistant. As with other components of MoveIt!, the users can write their own inverse kinematics plugins, such as IKFast.
The CollisionWorld object of the planning scene is used to configure collision checking using the Flexible Collision Library (FCL) package. The collision objects supported are meshes, primitive shapes, for example, boxes, cylinders, cones, spheres, and planes, and an octomap.
Collision checking is a very expensive operation that usually accounts for 90 percent of motion planning. For that reason, an Allowed Collision Matrix (ACM) is used to encode a Boolean value that indicates whether collision checking is needed for two pairs of bodies (on the robot or in the world); a value of 1 indicates that collision checking is not needed for a pair of objects. This is the case for bodies that are very far from each other, so they would never collide.