Owned by Penn State Networked Robotics Systems Laboratory http://nrsl.mne.psu.edu

Using PyGame to display motion planning algorithms in 2 dimensions (2D display, higher dimension state space), integration with Gazebo to simulate 3-D vehicle motion.

This PyGame simulation works on Ubuntu 18 or 16, and Python 2.7. If using Python 3.x, may need to change print statements, and some other small things. Gazebo simulation is only for Ubuntu 16 and ROS Kinetic. Will not work with Ubuntu18 and ROS Melodic (already tested). Also only tested Gazebo simulator with Python 2.7.

Relevant algorithms:

RRT*: https://arxiv.org/abs/1105.1186

i-Nash Trajectory-based multi-robot planning: http://php.scripts.psu.edu/muz16/pdf/Zhu-Otte-ICRA14.pdf

i-Nash Policy-based multi-robot planning: http://php.scripts.psu.edu/muz16/pdf/DJ-MZ-AR-IFAC15.pdf

Below are some gifs as code was developed over time.

First gif is RRT but for multiple robots, with UI allowing user to enter any number of robots and click the start point and destination point.

Next 3 gifs are the iNash trajectory algorithm without the pathGeneration() or inter-robot collision checking yet; the difference between the three is the near_radius size, notice the difference in how many connections tend to form from a given vertex.

Next gif is iNash trajectory with non-optimal pathGeneration added. Still no inter-robot collision checking

Next gif is iNash trajectory with optimal path chosen added. No inter-robot collision checking. Notice that without edges being added from new vertices TOWARDS the existing tree (as explained in the algorithm, this is to avoid directed cycles), the paths to select from are still limited, with any optimal options more likely to be minor differences towards the end of the path. Trees no longer displayed to more easily see the path choices. The blue path is the first path found. The orange paths are displaying a shorter path found later in time.

For a larger number of robots, we will want each robot to have a larger variety of paths. Not just in the number of possible paths, but the number of very different paths. This will increase likelihood of avoiding collisions. Attempt below was: once one path is found, enable the ability of adding edges in the reverse direction, so that more paths can end up at the goal. This requires checking for directed cycles before adding, and added too much computational complexity.

Here is the first "final" algorithm, with no additions to the exact design in the paper (ignoring additions attempted in previous gif). First gif shows the tree, second one only displays paths. Inter-robot collision checking is now included. Paths do not display unless they have no inter-robot collision.

First attempt at using an approximate-nearest and approximate-near searching algorithm resulted in very non-random and less-exploring tree structure:

Used a k-d tree structure and subtree-pruning (as explained conceptually at https://www.cs.cmu.edu/~ckingsf/bioinfo-lectures/kdtrees.pdf, though I have some differences in my design) to improve computational complexity of near/nearest searching. Finding exact near/nearest, not approximate. Also improved pathGen computational complexity by storing previously calculated paths for given vertices. The two windows in the gif below show the speed differences real-time.

Took the above optimized algorithm and incorporated sampling of the entire state space (4 states: x position, y position, x velocity, y velocity). The top plot shows x,y position, the second plot shows x position vs x velocity, the third plot shows y position vs y velocity. 2nd and 3rd plots only displaying path once found. Notice the difference in the way the tree looks in the position plot, with some portions overlapping - this is because of the extra dimensions - only the 2-D projection of the tree is overlapped, not the actual 4-D tree. Note this still does not include state dynamics. Tree edges are straight lines, not calculated trajectories, yet. The only dynamic constraint included currently is max velocity. Note many edges may not actually be feasible yet - for example, if the x value increases but the straight line has a negative velocity, this makes no sense.

The above algorithm was used, with dynamically-feasible locally-optimal trajectories calculated for the point-mass double-integrator problem for all edges. The solution is: Solve free-final-time bang-bang in 2 dimensions - position and velocity. Solve this twice, once for x and once for y. Keep the one that takes longer, and re-calculate the other as a fixed-final-time problem ("sub-bang-bang" similar to bang-bang except not the max control input). The gif below displays the results. Straight edges for the paths are displayed with smaller lines, and the thicker lines are the feasible trajectories. Notice the trajectories have smoother curves, however they result in unnecessary swirling around and turning. This is because while position path may be going right, the discrete points may require a velocity in the reverse direction. This could be improved with biased-velocity sampling or some other procedure.

Below is a display of the 2-D simulation of the policy algorithm. Notice the fast-slow-fast-slow etc behavior, due to bang-bang control for a point-mass robot.

Below is the same simulation as above, but for one robot so that the velocity plots are easier to see and understand. Notice here there is a required large change in x position, so the algorithm decided to use a biased x velocity sampling where it is more likely for values to be positive. Notice also how there is still some oscillation in the y direction, since there is no y biasing.

The y oscillation issue is fixed via velocity biasing towards zero. When a goal is not far away in a given dimension, the velocity is biased to be near zero in that dimension. See below for improvements. Notice less oscillation.

Algorithm was edited so that each robot has dual-tree forming, meaning trees form from both the starting point and the goal point and they grow towards each other. This method is used in RRT and RRT* because it increases the speed at which a path is found; here, it is even more important because it increases the path variety, meaning that it increases the likelihood that a robot can find a path that does not collide with other robots. Below is a gif showing the slow-motion tree forming for a single robot.

This gif below displays for a single robot an example of increased path variety. The path in black is the first path chosen by the robot, then the next one chosen because it has a lower cost is grey. Note previously the changes would only occur close to the goalset, whereas now path differences can occur at any point along the path.

The gif below is similar to above, except instead of only displaying paths that the robot had chosen at some point in time, it displays all possible paths. Note these are only paths that are found in the split second between clicking the second grey point and the blue button, and yet it still found that many :)

This gif below displays the same thing, but for more robots so you can more easily see the reasoning for wanting more path variety.

Here is a display of the same code but with only the chosen paths being displayed, and the robots moving along the chosen path.

Here are some displays with a larger number of robots with more intersections, without paths being displayed, so that you can more easily see the actual robot movement and the lack of collisions.

Linearity of computational complexity in relation to the number of robots was confirmed through an automation of 225 simulation cases, with data shown below.

Creating a simulation with Gazebo could be the next step towards reality, with some difficulties; possibilities could be: the discrete-vertex path values to Gazebo to control i-robot or ardrone. There does not seem to be an easy method of sending continuous time commands, and we do not know the state model of the 3d robots. So, position commands may be simpler and may make more sense. However, without a powerful enough computer, Gazebo simulations are tough. This gif below, for example, is a recording on my computer of a simple 4-robot 4-pts procedure (simply commands "move to point x, y" for four points on repeat) designed by someone else. Note, my computer has an intel-i5-9400f CPU and a Radeon RX590 GPU, and was running VirtualBox Ubuntu16 with ~7GB dedicated to running VirtualBox, and below is the rate at which it displayed.