OMPL Amazon warehouse

 OPML AMAZON WAREHOUSE

The objective of this exercise is to empower our robot to transfer a designated shelf from its initial location to a distinct area, utilizing a supplied map and shelf locations as points of reference. In my scenario, I have chosen the initial point as the destination. Additionally, it is crucial to highlight that we were constrained to crafting movement paths solely through the utilization of the OMPL library, a factor that has notably streamlined the entire task.

How have I achieved it?

First version

The initial step involved configuring the robot to generate routes using OMPL. To achieve this, it was necessary to establish bidirectional mappings between the map and real-world coordinates, enabling seamless coordinate base transformations. Subsequently, I approximated the robot as a point (or pixel, in this context) and applied erosion to the map using a kernel size equivalent to the robot's pixel size plus an inflated area, akin to the approach employed by the ROS nav2 package. This strategy was implemented to prevent collisions and resulted in generated routes such as:

Fig1. OMPL path without optimizations. (P0 = origin; P1 = dest)

Once the robot was capable of generating routes, I decided to investigate in the OMPL documentation if they could be optimized since the generated routes often lacked sense (to a human), as we can see in the previous image. Surprisingly, I found that it's possible to create customized optimization parameters in OMPL. The library already includes some default parameters, such as minimizing the route length. Additionally, I needed to ensure compatibility with a planner that supports these optimization parameters.

After testing various options, I opted for the LazyPRM planner because:

• InformedRRTstar: Does not always converge
• PRM: It always worked, but it was slower.
• RRT*: Does not always converge
• TRRT: Does not always converge

Producing significantly improved routes as a result.

Fig2. OMPL path with optimizations. (P0 = origin; P1 = dest)

Finally, the remaining aspect to address was the movement, i.e., translating the obtained route into understandable velocities for the robot. To accomplish this, I decided to repurpose the controller used in the tele-taxi practice (a robot navigating using the wavefront algorithm to obtain a navigation route and then execute it). Essentially, this controller first converts all pixel coordinates of the routes to metric coordinates and then adjusts the robot's orientation and movement based on the relative distance to the next point in the route.

You can see a demo here:

auxiliar link

Second version

In this second iteration, I chose to leverage the functions provided by OMPL to establish geometric, kinematic, and dynamic constraints, which are essential for crafting tailored optimal paths for the robot used in this scenario. In this specific exercise, our robot is holonomic, implying the absence of kinematic constraints. Dynamic limitations were accounted for in the controller section by adjusting the robot's velocities based on the route points and the maximum robot acceleration, eliminating the need to communicate this to OMPL.

Regarding geometric constraints, our robot adopts a rectangular configuration. Thus, we simply needed to verify a rectangle composed of multiple pixels, with a size equivalent to that of the robot (an easily calculable dimension using real-world to map conversions). Additionally, we slightly increased the size of this rectangle to ensure route safety against potential collisions. We utilized this pixel rectangle to validate the suitability of states that will be employed to generate our path using OMPL and no longer needing to manipulate the map for route construction. We also had to take into account that once the robot had grasped the shelf, the map needed to be updated. At this stage, the legs of the shelf ceased to be an obstacle, and furthermore, the geometry of the robot changed, increasing both the width and length of its geometric constraints.

The result of the planned routes, taking into consideration the robot's inflated area as a geometric constraint for the validation of route states and to enhance safety, is as follows:

Fig3: Path planning with the robot size as a geometric constraint. (P0 = origin; P1 = dest)

Fig4: Path planning with the shelve size as a geometric constraint. (P0 = origin; P1 = dest)

You can see a demo here: