Contents

  1. Introduction
  2. Get the Code
    1. Rosinstall
    2. Subversion
  3. Lattice Planner
    1. Uses
    2. Task
  4. Arm Planner
  5. Cart Planner
    1. Task
  6. Troubleshooting

Introduction

The SBPL is a versatile library of graph searches designed to be environment-agnostic. In Max Likhachev's session, you learned about the different types of graph search algorithms that are currently available and their theoretical properties.

During this tutorial, we'll get a chance to try out three different applications of the SBPL and learn how to use them, including:

  • Lattice Planner (navigation)
  • Arm Planner (manipulation)
  • Cart Planner (navigation while pushing a cart)

Get the Code

Rosinstall

All code needed for this tutorial can be downloaded with this: sbpl_lab.rosinstall rosinstall file. 

rosinstall ~/ros/sbpl_lab/ {download location}/sbpl_lab.rosinstall

Then add it to your ROS_PACKAGE_PATH: 

export ROS_PACKAGE_PATH=~/ros/sbpl_lab:${ROS_PACKAGE_PATH}

Subversion

If you choose not to use rosinstall to get the code, you can check out the needed code directly from the svn repositories.

Stacks

Packages

Lattice Planner

The lattice planner plans for the navigation of the base of the robot in {x,y,theta}. It plans smooth and feasible paths given a set of "motion primitives", short and kinematically feasible motions.

The SBPL Lattice Planner package is a ROS wrapper around the planner itself which can be found in the SBPL package.

Uses

Base Global Planner

It conforms to the navigation stack's base_global_planner API so it can be used as the global planner. NavFn is currently the default global planner of the navigation stack so to switch to using the lattice planner, only two ros parameters must be set in your move_base launch file of choice (For the PR2 --> pr2_2dnav/pr2_2dnav.launch). 

    <param name="base_global_planner" value="SBPLLatticePlanner" />
    <param name="SBPLLatticePlanner/primitive_filename" value="$(find sbpl)/matlab/mprim/pr2.mprim" />

There are a bunch of parameters which can be tweaked however the default values generally work well and the above parameters are the only ones that are required: For example, in launch/move_base/move_base_sbpl.xml: 

   <param name="move_base_node/SBPLLatticePlanner/initial_epsilon" value="10.0" />

Note that while using the navigation stack's default settings (with NavFn as the base_global_planner), you can set 3D goals for the robot however NavFn is actually just a 2D planner {x,y}. It computes the global plan in 2D and the goal theta is planned for only during the final meter of the trip. The assumption that the goal orientation can be satisfied at the end of the trip is not always true in cluttered environments and is more often the case when not using a robot assumed to be circular or on a non-holonomic platform.

This is not true with the lattice planner, which plans in 3D from the start of the search. Thus, there are some cases when the lattice planner can find a path to the goal when navfn can't.

Recovery Behaviour

More recently, the lattice planner has been added as a "recovery behaviour" for the navigation stack. Recovery behaviours are used when the robot finds itself in a situation where the global planner can't find a safe trajectory at all or it finds a trajectory that then gets rejected by the local controller as being unsafe. In these situations, the robot is practically frozen in place and before it gives up on reaching the goal, it attempts a list of escape measures first. The two most obvious behaviours you may have seen are the robot tries to spin in place and plan again and/or the local costmap is cleared out and a new map is recorded, hopefully, without any garbage laser hits that were there before.

The lattice planner has been added as a recovery behaviour. In such a situation, the lattice planner would be given a goal a meter away with the hopes that full 3D planning can help the robot escape.

Using the lattice planner as an escape behaviour is not yet released in cturtle. Feel free to try it out on your robots by checking it out of the navigation_experimental stack:

https://code.ros.org/svn/ros-pkg/stacks/navigation_experimental/trunk/sbpl_recovery/

Task

Let's all go ahead and try out the lattice planner on the PR2 in stage. Since it's based on the navigation stack, running the planner and setting goals programmatically and in rviz should be very familiar to you.

Follow the instructions here, 2.2 How to Use.

  • Check out the tweakable parameters below the instructions and feel free to play with them or ask any questions you may have.
  • Try using multiple sets of motion primitives. Different sets of primitives can generate dramatically different paths (especially in cluttered environments).
    • "/move_base_node/SBPLLatticePlanner/primitive_filename"=(find sbpl)/{...}:
    • matlab/mprim/pr2.mprim - Motion primitives that only allow forward, backward, turn-in-place, and forward-and-turn-arc motions
    • matlab/mprim/pr2sides.mprim - Same as pr2.mprim (previous) but includes strafing left and right as allowable motions

You can read about creating a list of motion primitives of your own here (it requires matlab).

Arm Planner

The SBPL Arm Planner is a search based planner that plans in the 7D joint space of the arm. Unlike other planners - it plans to a pose constraint, {x,y,z,r,p,y}, on the wrist of the arm rather than to a joint constraint. The joint configuration at the goal is chosen by the cost function instead.

The sbpl_arm_planner package is out of date and the temporary location of the code can be found in the rosinstall file above.

Just like all of the other motion planners in ros-pkgs, the planner can be called by issueing a request to move_arm with the "planner_service_name" set to "sbpl_planning/plan_path". Remember, to send pose constraints not joint constraints.

Example Motion Primitives

Shoulder Pan:

alt text

Elbow Pitch:

alt text

Forearm Roll:

alt text

Wrist Pitch:

alt text

Cart Planner

The cart planner is a brand new sbpl-based planner that is capable of generating motion plans for the PR2 to follow while pushing a cart. The sbpl_cart_planner is just the planning component of the cart_pushing stack. The cart_pushing stack contains the complete extension to the navigation stack that performs all of the necessary integration to make everything work.

You can read more about cart_pushing here, Cart Pushing Stack

Feel free to check out the planner's tweakable parameters here, SBPL Cart Planner

Example Motion Primitives

A turning motion (without an articulated cart):

alt text

A turning motion (with an articulated cart):

alt text

Task

Let's all go ahead and try out the cart pushing stack in stage. Since it's based on the navigation stack, running the planner and setting goals programmatically and in rviz should be very familiar to you.

Tutorial: Running the cart pushing stack in Stage

  • Try using both sets of motion primitives available by changing the primitive_filename rosparam. Read about the 2 available sets in 3.3 Configuring the motion primitives. Depending on the environment, they should return different paths.

The tutorial is self explanatory and easy to follow but I thought I'd reiterate a few key things you should consider:

  • Initializing the cart planner requires that the motion primitives must be precomputed to speed up the actual planning. This process can take up to 50 seconds. Be patient :). (In the future, we'll parse a text file that contains the precomputed data to speed up initialization)

  • The cart planner plans in 4D, so planning from one end of a 2000x2000 map to the other end takes time. We suggest you play around with shorter planning requests to get a feel for how it works. If you find that the planner is not returning a path within the allocated time frame, then feel free to increase the allocated_time rosparam and relaunch the planner (list of planner params). You can also try increasing the epsilon rosparam to see if raising the bounds on the sub-optimality of the solution returns a path within the given time frame.

Note: The cart_pushing stack only works on the PR2 but it can work with any holonomic cart. Check out the tutorial on configuring a new cart for details on describing your cart's dimensions so that you can use it with the cart pushing stack.

Troubleshooting

'Stage isn't loading up correctly'

When launching stageros, depending on your OS, you might see the following error message when stage tries to load:

err: unable to open color database: No such file or directory (try adding rgb.txt's location to your STAGEPATH) (/tmp/buildd/ros-cturtle-simulator-stage-1.2.1/debian/ros-cturtle-simulator-stage/opt/ros/cturtle/stacks/simulator_stage/stage/build/Stage-3.2.2-Source/libstage/color.cc Color)

If you see the above error, then check to see if you have a file called "rgb.txt" in /etc/X11. If you don't, then download it from here: rgb.txt and put it in /etc/X11. Then set your STAGEPATH environment variable: 

export STAGEPATH=/etc/X11

Wiki: Events/CoTeSys-ROS-School/SBPL_Lab (last edited 2010-11-06 14:32:08 by MoritzTenorth)

Except where otherwise noted, the ROS wiki is licensed under the
Creative Commons Attribution 3.0