TASK 4. AMAZON WAREHOUSE: OMPL

INTRODUCTION

The objective of this task is to implement the logic that allows an holonomic logistics robot to deliver shelves to deliver shelves to the required place by making use of the location of the robot. The robot is equipped with a map and knows its current location in it. The main objective is to find a path using OMPL(Open Motion Planning Library).

To achieve this, it has been necessary to create a state machine composed of the following states:

With the states of lift and put down being built-in functions.

In this post I am going to cover the following points: 

• Dimensions
• OMPL
• OMPL: Obstacles.
• OMPL: Dimensions.
• OMPL: Plan
• Navigation
• Videos 
• Conclusion
  

DIMENSIONS

In order to work in the same units and provide coherence and ease to calculations, it is necessary to relate the dimensions of the map to those of the 3D world.

Map dimensions:

• Height: 415 pixels
  
• Width: 279 pixels
  

Real-world dimensions:

• Long: 20.62 m
  
• Wide: 13.6 m 
  

However, both do not have the origin at the same point. Based on the description in the statement, I managed to establish the following relationship:

 

 Being h = height and w = width.

After deriving this relationship, I ultimately decided to use the linear regression technique to obtain the equations. The resulting equations are as follows:

• From the real world (3D) to the map (2D):
  
  • x_map = 139,5 - 20,515*x_real_world.
  • y_map = 207,5 - 20,126*y_real_world.
    

• From the map (2D) to the real world (3D):
  • x_real_world = 6,8 - 0,049*x_map.
    
  • y_real_world = 10,310 - 0.05*y_map.
    
   

Now that we are working in the same units, we can begin to address the problem.

 

OMPL

Open Motion Planning Library is a library for sampling-based motion planning. That involves generating feasible paths for robots or other entities to navigate through their environments, avoiding obstacles and achieving specific goals. 

In this task, it is necessary to use OMPL to generate the two paths that the robot must follow: the outbound path (where the robot is defined as a point) and the return path (where the robot carries the shelf and cannot be defined as a point).

Based on the description in the statement and thorough research, I have discovered that OMPL is divided into three parts: obstacles, dimensions, and planning.

OMPL: OBSTACLES

The robot does not have any sensors to navigate in the real world; it only has actuators that allow it to move with linear and angular velocity, lift and lower the platform. However, it does have a map of its entire environment.

Therefore, obstacles will be extracted from the map.

To do this, it is necessary to obtain the image from: "/RoboticsAcademy/exercises/static/exercises/amazon_warehouse_newmanager/resources/images/map.png".

The erode operation is used to enlarge the obstacles.

A global variable is defined to contain the obstacles. Inside it, there is an array that includes the position of each black pixel, each with a dimension of 2, to avoid paths too close to the obstacles.

OMPL: DIMENSIONS

Since the obstacles are obtained from the map, the dimensions will be set in the units of the map.

OMPL: PLAN

To finally generate the plan, it is necessary to define the following:

• The state space the robot operates in: in this case, it is SE2StateSpace().
  
• Set the dimensions of the environment in pixels, subtracting the dimensions of the robot. 
  
• Contruct a space information instance for this state space.
• Set state validity checking for this space. Within this function, define whether a state is valid or not by checking if the state is inside any obstacle or collides with the robot.
• Convert the initial and goal positions to map coordinates (because obstacles are in the map) and set these initial and final states in OMPL.
• Create a problem instance.
• Create a planner for the defined state. There are many planners available, but I ultimately decided to use PRM (Probabilistic Roadmap), which is a popular motion planning algorithm. PRM works by constructing a roadmap in the configuration space of the robot.
• Set the problem we are trying to solve for the planner.
  
• Perform setup steps for the planner and solve it.
  

Here are some examples of generating paths to different shelves to reach the goal:

• Shelf 1: 

 

• Shelf 3:
  

 

• Shelf 6:
  

  

It can be observed that all paths reach the defined destination. Among all the planners tested, PRM has consistently provided a path to the destination despite the various constraints imposed for testing purposes.

To represent them using showPath(), I needed to invert the x and y of each point and add extra points to facilitate navigation.

As mentioned before, the robot varies its dimensions throughout the execution of its task, so it is necessary to define two paths: the outbound path as a point and the return path as a rectangle.

NAVIGATION:

At this point, the only thing left to do is implementing the navigation part.

Finally, what I decided to implement is: using the conversion from the navigation map into real world (defined before), I take all the time into account the distance from the objective to the actual position in meters.

With that differences I calculate the atan and I compare it with my current angle and if that difference is big (means that the angle is no close to the objective), the robot turns. And if the difference is small (means that you are aligned to the objective), the robot goes ahead.

VIDEOS:

Here you can see my final results. I hope you like them!

• video1 (x2 speed): 

• video2 (x2 speed):

CONCLUSION: 

I feel that this task has been quite interesting and it has helped me to learn how OMPL works.