There a few problems to tackle in this assignment.
Problem the first: Create a path from pose A to pose B that will not hit any objects.
Problem two: Starting at any point on the field, push a ball from the center of the soccer field to the goal.
Problem three: Play soccer.
The information available to tackle these problems are overhead cameras. Localilzation is not an issue here.
Objects are defined by type and id number, so the obstacles and their locations are already known and are updated
continuously by overhead cameras.
What makes these problems interesting are the translation of these problems from the ideal computerized world to the difficult to control real world. Path planning is really simple if I want to go from A to B. There are many algorithms that exist that solve this problem. I can then just go, following the path. No problem. Bring that into the real world, I have to correct accidentally leaving the path, slow updates of knowledge (ie, latency), etc.
Figuring out how to kick a ball is also quite difficult. First the path planning algorithm has to get me behind the ball. From there, I have to figure out how to orient myself so that I might be able to score. If I kick the ball, the ball might end up to the side and then I have to figure out how to go back and kick the ball again and so on.
Our approach to this problem can be broken into a few parts: state logic, path planning, and controller
First, the state logic to play soccer can be simplified to the following:
Restart
If there is a ball on the field, go to Offense and seek the ball
If there is no ball on the field, stay in Restart and go to restart position
Offense
If there is no ball on the field, enter Restart state
If I am close enough to the ball to kick it and oriented toward the ball/goal vector, enter Kick state
Else If my path to the kick point is still valid, continue my path
Else If my path to the kick point is NOT valid, make a new one
If I cannot make a new path, ignore path validity and drive straight for the kick point
Kick
If there is no ball on the field, enter restart state
If I am not close enough to the ball, enter Offense seek state
If I am not oriented toward the ball, enter Offense seek state
Otherwise, go in the ball/goal vector pushing the ball toward the goal
Defense
Not implemented
Transition to Offense seek state
Second, path planning was done using a single direction rapidly exploring random search (RRS) of the continuous space of the world. The advantage of RRS over other path planning algorithms is not necessarly the speed with which it creates optimal paths, but rather the speed by which several new existing paths can be created in the event that obstacles move and a path becomes invalid. Also, by searching the continuous space (or at least, discretized by a small order of magnitude in floating point), RRS allows for a better search than by discretizing the space at a higher order of magnitude. I will touch more on the advantages and disadvantages of RRS in the conclusion. After the path is created, it is simplified by greedily removing any nodes in the path that can be reached from a current node. It is less than optimal, but fast and simple to compute.
Finally, the controller is a proportional-derivative control built nearly identically to the previous project. The difference here is not the centering of an object in the camera, but rather the centering of the robot's yaw with the relative angle to the next waypoint in the path as given by the overhead camera.
Pseudocode for each of the three parts of the approach are as follows
I like to believe so, especially since the above pseudocode is c++ code transfered into quasi-pseudocode. Taken apart, I believe the individual parts will work with interchangeable abstractions. Our P-D controller will work with any list of points. Our Path Planner will work with any list of objects and their size. Our state logic will work with any valid controller and path planner.
We validate our tests by doing unit tests on each of the three parts of our approach. First, path planning was tested by creating a world with objects, a start, and a goal. The tests started off simple with no objects on the field (so a path should go straight from the start to the goal) to more complicated tests which can be seen in the following images of attempts at path planning. First there is the original path as created by RRS and then the path after it has been simplified.
Path following was tested by creating a simple state machine which says, Robot, go to this point. We started out with simple tests such as a path of one point. Then with four points, and so on. There is no path planning in these tests. Unfortunately, we did not film these tests or have proof that we did them. However, it can be noted from our other videos, that our robot can follow a path and instructions
Our state machine was tested by combining the previous two parts of the robot and each state and transition tested independently. Once again, no video of each of these specific tests, but the final videos should adequately illustrate our results.
As previously state, our tests were done incrementally building upon previously constructed code.
Path planning tests went as follows:
Connect start node to goal node without any objects in world
Add objects to world, not blocking the straight path from start to goal
Add an object to the world blocking the straight path from start to goal
Add walls of objects to the world blocking simple paths from start to goal
Path following tests went as follows:
Go straight from robot start location to a specified x,y coordinate on the field
Go to a list of x,y coordinates on the field in succession from first to last
Path planning/following tests went as follows:
Go to a list of x,y coordinates as given by the path planning algorithm
Go to a list of x,y coordinates given by the path planning algorithm and move objects (test updating of the path)
State tests went as follows:
Test robot off the field - it should not move - (ALL_STOP)
Test robot start on the field, remove from field - it should stop moving (ALL_STOP)
Test robot start on field with ball off field - it should enter the restart state and behave accordingly
Test robot start on, ball off, put ball on - it should move toward the ball
Test robot start on, ball off, put ball on, take ball off - it should enter the restart state and behave accordingly
Test robot on field anywhere, ball at center of field - it should get to the kick position of the ball
Test robot on field anywhere, ball at center of field - it should try to kick the ball toward the goal
Test robot on, ball on opposite side, obstacles in path - it should navigate around the obstacles to get to the ball
Test robot on, ball on opposite side, moving obstacles in path - it should navigate around the obstacles
Test robot on, ball moving around field - it should always reconstruct a path toward the ball.
I hope so. If implemented in the same way with the same code under the same conditions, someone reproducing our implementation will be able to produce similar results. We ran many tests multiple times and got pretty consistent results by the time the robot was completed. We expect the same of people reproducing this.
Path planning Rapidly exploring random search is a valid way to create a path through a world with a simple enough search space.
Path following It worked before, and it still works.
State logic It is difficult to control a state machine that exists as a loop. Perhaps another approach should be considered. It has been suggested that decision making based on the state of objects in the world and performing actions according to that may be an improvement on the approach so that the robot does not get stuck in an incorrect portion of the state logic. However, with a correct state machine, this should be a moot point.
Path planning Rapidly exploring random search as we have implemented it, is quite successful.
First, RRS is very fast to perform and create a path, especially if the path is very short and simple. With a large number of samples, the general direction of the goal will probably be reached and a path probably found. If not, up the number of samples and try again. This is especially successful in the rlab because there are not enough objects to make a long, difficult path.
Secondly, there is no discretization of the world space into a grid (aside from the degree of accuracy from floating point numbers), so there are no resolution issues. Discritizing the world space into large nodes (ie, like a graph for performing Dijkstra's or A* search) might hide multiple obstacles or hide space that is actually safe to move through, but an obstacle might just be in the space enough to completely remove it from viable path space. This is a major disadvantage to graph searches that RRS does not have.
Finally, updating the world and updating the path are simple and fast. In a linear time operation, the path can be validated simply by performing intersections with the world as the RRS would do during a normal path creation. If the path proves to be invalid because an object moved, it can quickly recreate the path without any major computation (as would happen with heavier, more time complex, algorithms).
Path following Proportional-derivative controller successfully corrects the yaw of the robot as determined by the overhead cameras and the desired yaw as computed from the path's waypoints. It is a simple and fast correction.
State logic It is simple casing. Check for preconditions and postconditions of a state and move to the next one accordingly.
Path planning Rapidly exploring random search, as we have implemented it, fails in a couple of ways.
First, it cannot decide whether or not a path exists. Rather than determining that, it tries some number of sample points from the world space and gives up if it could not find a path. It's actually quite human of it to behave this way. A human will try to find a way through an obstacle course only for so long before it decides it has better things to do or that it might just be impossible.
Secondly, RRS does not produce the optimal path, just a valid one. Because it is a random sample algorithm, some samples are better than others. Occassionaly, it will find a slow, long path. Other times it will find one really simple path. As we have implemented RRS, the greedy cleaning of the path does not always produce and optimized simplified path. I have briefly looked into improving this, but I could not come up with a simple and fast (ie, elegant) solution. The trade off for this is the speed with which the search tree can be built and a path, if one exists, can be found.
Thirdly, the intersection code as we have written it is less than optimal for speed. It performs three intersections: the line as it is, and the lines representing the sides of the robot if it were to travel down the center line. This improves the validity of the path, but not perfectly. These intersections check every obstacle in the field. In a larger space with thousands or millions of obstacles, this would be quite slow. Spatial partitioning could be used to improve the speed. Because the nature of the field is that there are always objects moving, a grid-based partitioning algorithm would improve this algorithm on a larger scale than the current setup. Fortunately, the world for our robots is simple enough that the necessity for such an algorithm is really low. Scaling this up would require the speed up. Similarly, finding the closest node in the search tree could be improved. It is an O(n) operation currently, but partitioning may help with that as well.
Finally, we implemented RRS as a single direction search. Making it a bidirectional search would improve the chances of finding a path from start to goal.
Path following Path following is simple enough that the only forseeable improvements would be tweaking the speed settings so that it is able to turn on a dime if necessary or drive hard and fast as necessary.
State logic It was difficult to debug the state logic occassionaly because it would get stuck in a loop and it would be difficult to decide what loop it was doing and why it was doing it.