Planning
Motion Planning Subsystem Description
The motion planning subsystem is currently on schedule and is meeting or on track to meet a few major goals
The first goal is that planning should act as a generic service for other subsystems. Right now there are a few generic planning calls, such as move to tote, move to bin, and move to home, that can be requested by any subsystem and function without any major issues. As the project moves forward, there may be additional generic plan types to be implemented, so an easy framework has been made for adding more. All requests also receive a response that indicates a whether the plan was successful or not.
With the help of experience graphs, planning has also become more robust within bins. Experience graphs are a way of modifying a heuristic for a search based planner with the goal of reducing the number of expanded states during search, which in turn reduces planning time. Since the Amazon Robotics Challenge has a tightly constrained and mostly static environment, experience graphs are a useful tool. In the past, a relatively rigid set of poses needed to be reached in a specific order to enter bins, but experience graphs has made the system more robust. Even fairly difficult plans, such as planning from one bin to another can be found reliably in a few seconds without the need to specifically plan to certain waypoints outside the relevant bins. This will lead to more generic and simple state machine in the future, but has not yet been used in any demonstrations on the physical robot. The following figure shows a visualization of some of the cached trajectories that were used to verify the usefulness of the experience graph planner. New trajectories will need to be acquired once the new shelf is fully designed. This process should be simplified by using a short trajectory capturing script that was written for the original experience graph.
The motion planning subsystem also experiences some jerky motion during grasping attempts. These issues are caused by a series of short motion requests to increment the gripper downwards, and the jerkiness becomes most apparent when the vision system has estimated an item pose in the wrong area. This poor estimation may cause grasping to increment down slowly towards the bottom of the bin, which takes a long time and exacerbates jerkiness. The reason this issue is brought up here in planning, is that the solution lives in planning. By implementing motion planning with the ROS actionlib package there may be a convenient way to make downward planning within a bin smooth. This has not been implemented yet.
So far modeling for motion planning is done entirely using ROS and RViz. This model includes all the important collision models such as the mount, the UR5 arm, and the shelf. Since the motion planning system uses these collision models to generate plans in the real world, it is important that these models reflect the real world as accurately as possible. On a few occasions we witnessed unexpected motion plans and had to update the models accordingly.
Planning Implementation
Planning for the system utilizes the SBPL planner and Rviz. Two primary planners have been employed: EGWA* and LARA*. EGWA* is experience-graph-weighted-A*, and will be used for for repetitive motions such as moving to pre-grasp and camera poses. This planner works by caching successful trajectories for planning based off of Weighted A* . Once cached, these plans take substantially less processing time, and are “guaranteed” in a sense if they have been validated by hand before being cached. The main advantage of EGWA* for our system is the near perfect repeatability of the planner, which makes the system robust. LARA* will be primarily used to generate grasping plans in order to pick up objects. Due to the dynamic positioning of the objects in the bins, cached trajectories would not be useful.
The planning scene was used to verify hardware designs before construction. This includes appropriate placing of bins and any obstructing hardware, as well as arm positioning and mounting. The planner acts in conjunction with other ROS nodes in order to control all DOF for the system, including the linear slide rail and any actuators on the grasper.
Motion Planning Testing
For testing the system, there are three types of tests that have been conducted. The first is a test that checks the configuration space of the arm. Using a script from last year’s team, we are able to feed in a list of relevant end effector poses to the an inverse kinematics program that determines how many of the listed poses were reachable. This type of test helps us evaluate the effects of new hardware by giving us an accurate measure of the arm’s configuration space. This type of test will be critical for finalizing the design of our new shelf.
Another type of test for planning is a planning time test. This type of test characterizes the amount of time needed to generate a plan. Some slight modifications to the planning server have been made in order to record in a text file how long it took to generate a plan. The values in the file can then be plotted and interpreted easily.