Amazon Warehouse
Goal
The objective of this practice is to implement the logic that allows a holonomic logistics robot 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 will be to find the shortest path to complete the task.
Instructions
This is the preferred way for running the exercise.
Installing and Launching

Download Docker. Windows users should choose WSL 2 backend Docker installation if possible, as it has better performance than HyperV.

Pull the current distribution of Robotics Academy Docker Image:
docker pull jderobot/roboticsacademy:latest

In order to obtain optimal performance, Docker should be using multiple CPU cores. In case of Docker for Mac or Docker for Windows, the VM should be assigned a greater number of cores.

It is recommended to use the latest image. However, older distributions of RADI (RoboticsAcademy Docker Image) can be found here.
How to perform the exercises?

Start a new docker container of the image and keep it running in the background:
docker run rm it p 7164:7164 p 2303:2303 p 1905:1905 p 8765:8765 p 6080:6080 p 1108:1108 p 7163:7163 jderobot/roboticsacademy

On the local machine navigate to 127.0.0.1:7164/ in the browser and choose the desired exercise.

Wait for the Connect button to turn green and display “Connected”. Click on the “Launch” button and wait for some time until an alert appears with the message
Connection Established
and button displays “Ready”. 
The exercise can be used after the alert.
Enable GPU Acceleration
 Follow the advanced launching instructions from here.
Where to insert the code?
In the launched webpage, type your code in the text editor,
from GUI import GUI
from HAL import HAL
# Enter sequential code!
while True:
# Enter iterative code!
Using the Interface

Control Buttons: The control buttons enable the control of the interface. Play button sends the code written by User to the Robot. Stop button stops the code that is currently running on the Robot. Save button saves the code on the local machine. Load button loads the code from the local machine. Reset button resets the simulation(primarily, the position of the robot).

Brain and GUI Frequency: This input shows the running frequency of the iterative part of the code (under the
while True:
). A smaller value implies the code runs less number of times. A higher value implies the code runs a large number of times. The numerator is the one set as the Measured Frequency who is the one measured by the computer (a frequency of execution the computer is able to maintain despite the commanded one) and the input (denominator) is the Target Frequency which is the desired frequency by the student. The student should adjust the Target Frequency according to the Measured Frequency. 
RTF (Real Time Factor): The RTF defines how much real time passes with each step of simulation time. A RTF of 1 implies that simulation time is passing at the same speed as real time. The lower the value the slower the simulation will run, which will vary depending on the computer.

Debugging Console: This shows the error messages related to the student’s code that is sent. The student can also use it to visualize the output of the
print()
function.
Robot API
from HAL import HAL
 to import the HAL(Hardware Abstraction Layer) library class. This class contains the functions that sends and receives information to and from the Hardware (Gazebo).from GUI import GUI
 to import the GUI (Graphical User Interface) library class. This class contains the functions used to view the debugging information, like image widgets.HAL.getPose3d()
 returns x,y and theta components of the robot in world coordinatesHAL.setV()
 to set the linear speedHAL.setW()
 to set the angular speedHAL.lift()
 to lift the platformHAL.putdown()
 to put down the platformGUI.showPath(array)
 shows a path on the map. The parameter should be a 2D array containing each of the points of the pathGUI.getMap(url)
 returns a numpy array with the image data in a 3 dimensional array (R, G, B, A). The URL of the Amazon Warehouse World 1 is ‘/RoboticsAcademy/exercises/static/exercises/amazon_warehouse_newmanager/resources/images/map.png’. The URL of the Amazon Warehouse World 2 is ‘/RoboticsAcademy/exercises/static/exercises/amazon_warehouse_newmanager/resources/images/map_2.png’.
Supporting information
There are two warehouses to choose from:
Warehouse 1:
 The warehouse size is 20.62 meters long and 13.6 meters wide.
 The shelves coordinates from 1 to 6 are: (3.728, 0.579), (3.728, 1.242), (3.728, 3.039), (3.728, 4.827), (3.728, 6.781), (3.728, 8.665).
 You can get the warehouse’s map from there: /RoboticsAcademy/exercises/static/exercises/amazon_warehouse_newmanager/resources/images/map.png
Warehouse 2:
 The warehouse size is 34 meters long and 22 meters wide.
 The shelves coordinates from 1 to 9 are: (8.5, 6.0), (1.5, 6.0), (5.5, 6.0), (8.5, 4.0), (1.5, 4.0), (5.5, 4.0), (1.0, 15.5), (2, 11.5), (1.0, 15.0).
 The separation distance between two neighboring shelves is 2 m on the x axis and 1.5 m on the y axis.
 You can get the warehouse’s map from there: /RoboticsAcademy/exercises/static/exercises/amazon_warehouse_newmanager/resources/images/map_2.png
Theory
This exercise is a motion planning problem. Motion planning is a Jderobot Academy already has an exercise dedicated for this, which I’d definitely recommend the readers to check it out, so the challenge in this exercise isn’t to implement a motion planning algorithm but learning to use the OMPL (Open Motion Planning Library) for our purpose.
Open Motion Planning Library
OMPL is a library for samplingbased motion planning, offering many stateoftheart planning algorithms such as PRM, RRT, KPIECE, etc.
As you can see in the diagram above, some key components of OMPL are:
 State Space defines the possible configurations that a robot can have. For example:
 RealVectorStateSpace: represents an Euclidean space
 SO2StateSpace, SO3StateSpace: represents rotations in 2D and 3D
 SE2StateSpace, SE3StateSpace: combines translations and rotations in 2D and 3D
 …
 State Validaty Checker determines if the configuration is valid, that is to say the configuration doesn’t collides with an enviroment obstacle and respects the constraints of the robot.
 Control Space defines the movements that a robot can have.
 State Propagator indicates the evolution of the system after applying a control.
 Space Information is the container that holds the state space, the state validity checker, and other information needed for planning.
 Planner responsible for generating a path from the start to the goal in the configuration space. OMPL supports a variety of planners, such as RRT, PRM, and FMT*.
 Path is the output of the planner, which is a sequence of states representing a trajectory for the robot to follow.
The following example shows a 2D point robot inside a 10x10 space with an ovular obstacle at position (5.5, 5.5):
from ompl import base as ob
from ompl import geometric as og
import math
from math import sqrt
import numpy as np
import matplotlib.pyplot as plt
# specify valid state condition
def isStateValid(state):
x = state.getX()
y = state.getY()
if sqrt(pow(x  obstacle[0], 2) + pow(y  obstacle[1], 2))  obstacle[2] <= 0:
return False
return True
def plan():
# Construct the robot state space in which we're planning. We're
# planning in [0,1]x[0,1], a subset of R^2.
space = ob.SE2StateSpace()
# set state space's lower and upper bounds
bounds = ob.RealVectorBounds(2)
bounds.setLow(0, dimensions[0])
bounds.setLow(1, dimensions[1])
bounds.setHigh(0, dimensions[2])
bounds.setHigh(1, dimensions[3])
space.setBounds(bounds)
# construct a space information instance for this state space
si = ob.SpaceInformation(space)
# set state validity checking for this space
si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
# Set our robot's starting and goal state
start = ob.State(space)
start().setX(0)
start().setY(0)
start().setYaw(math.pi / 4)
goal = ob.State(space)
goal().setX(10)
goal().setY(10)
goal().setYaw(math.pi / 4)
# create a problem instance
pdef = ob.ProblemDefinition(si)
# set the start and goal states
pdef.setStartAndGoalStates(start, goal)
# create a planner for the defined space
planner = og.RRTConnect(si)
# set the problem we are trying to solve for the planner
planner.setProblemDefinition(pdef)
# perform setup steps for the planner
planner.setup()
# solve the problem and print the solution if exists
solved = planner.solve(1.0)
if solved:
print(pdef.getSolutionPath())
plot_path(pdef.getSolutionPath(), dimensions)
def create_numpy_path(states):
lines = states.splitlines()
length = len(lines)  1
array = np.zeros((length, 2))
for i in range(length):
array[i][0] = float(lines[i].split(" ")[0])
array[i][1] = float(lines[i].split(" ")[1])
return array
def plot_path(solution_path, dimensions):
matrix = solution_path.printAsMatrix()
path = create_numpy_path(matrix)
x, y = path.T
ax = plt.gca()
ax.plot(x, y, 'r')
ax.plot(x, y, 'go')
ax.axis(xmin=dimensions[0], xmax=dimensions[2], ymin=dimensions[1], ymax=dimensions[3])
ax.add_patch(plt.Circle((obstacle[0], obstacle[1]), radius=obstacle[2]))
plt.show()
if __name__ == "__main__":
dimensions = [0, 0, 10, 10]
obstacle = [5.5, 5.5, 1] # [x, y, radius]
plan()
Output:
Info: RRTConnect: Space information setup was not yet called. Calling now.
Debug: RRTConnect: Planner range detected to be 3.142586
Info: RRTConnect: Starting planning with 1 states already in datastructure
Info: RRTConnect: Created 9 states (4 start + 5 goal)
Geometric path with 7 states
Compound state [
RealVectorState [0 0]
SO2State [0.785398]
]
Compound state [
RealVectorState [1.26041 2.4087]
SO2State [0.0626893]
]
Compound state [
RealVectorState [3.38076 4.59672]
SO2State [0.128782]
]
Compound state [
RealVectorState [3.66427 7.34115]
SO2State [0.895879]
]
Compound state [
RealVectorState [4.7318 7.51811]
SO2State [0.808152]
]
Compound state [
RealVectorState [7.7113 8.01201]
SO2State [0.563304]
]
Compound state [
RealVectorState [10 10]
SO2State [0.785398]
]
Plot:
To better understand how to use the library, it is highly recommended to go to the tutorials and demos sections of the official website.
Conversion From 3D to 2D
Robot Localization is the process of determining, where robot is located with respect to it’s environment. Localization is a an important resource to us in solving this exercise. Localization can be accomplished in any way possible, be it Monte Carlo, Particle Filter, or even Offline Algorithms. Since, we have a map available to us, offline localization is the best way to move forward. Offline Localization will involve converting from a 3D environment scan to a 2D map. There are again numerous ways to do it, but the technique used in exercise is using transformation matrices.
Transformation Matrices
In simple terms, transformation is an invertible function that maps a set X to itself. Geometrically, it moves a point to some other location in some space. Algebraically, all the transformations can be mapped using matrix representation. In order to apply transformation on a point, we multiply the point with the specific transformation matrix to get the new location. Some important transformations are:
 Translation
Translation of Euclidean Space(2D or 3D world) moves every point by a fixed distance in the same direction
 Rotation
Rotation spins the object around a fixed point, known as center of rotation.
 Scaling
Scaling enlarges or diminishes objects, by a certain given scale factor.
 Shear
Shear rotates one axis so that the axes are no longer perpendicular.
In order to apply multiple transformations all at the same time, we use the concept of Transformation Matrix which enables us to multiply a single matrix for all the operations at once!
Transformation Matrix
In our case we need to map a 3D Point in gazebo, to a 2D matrix map of our house. The equation used in the exercise was:
Coordinate to Pixel Conversion Equation
In order to carry out the inverse operation of 3D to 2D, we can simply multiply, the pixel vector with the inverse of the transformation matrix to get the gazebo vector. The inverse of the matrix exists because the mapping is invertible and we do not care about the z coordinate of the environment, implying that each point in gazebo corresponds to a single point in the map.
Hints
Simple hints provided to help you solve the Amazon Warehouse exercise. Please note that the full solution has not been provided. Also, the hints are more related to the reference solution, since multiple solutions are possible for this exercise.
Ideas for solving the exercise
Define the robot as a 2D point and thicken the obstacles’ edges according to the robot’s radius to avoid collision, in this case, the state space can simply be an Euclidean space and all those black pixels will be invalid states. Maybe this demo can help you!
How to find the shortest path?
The library offers the possibility to set an optimization objective, which could be a great help in finding the shortest path. Check out in this tutorial how to do it!
Points to consider
 The loaded shelf is no longer a obstacle but part of the robot, then:
 the robot’s geometry changes, its “radius” increases
 remember to exclude the shelf itself when defining invalid states
Important points to remember
 Convert the coordinates from meter to pixel before representing with GUI.showPath(array).
Videos
Demonstrative video of completed solution
 Contributors: Lucía Lishan Chen Huang, Blanca Soria Rubio, Jose María Cañas
 Maintained by Lucía Lishan Chen Huang.