Amazon Warehouse

Goal of the Project

The objective of this exercise is to implement the logic that allows a 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.

Project Overview

This project was made in different stages.

1. First stage:

    Holonomic robot.

    Checking its ability to fit into narrow spaces by representing the robot as a single pixel and computing the path using one-pixel steps.

2. Second stage:
    Holonomic robot

    Checking whether the path is valid by using the robot’s geometry. A bounding box is created based on the robot’s size and rotated according to its orientation so the planner can determine whether the robot can follow the path using basic trigonometry.

hl = ROBOT_LENGTH / 2

hw = ROBOT_WIDTH / 2

verts = np.array([

[hl, -hw],

[hl, hw],

[-hl, -hw],

[-hl, hw]

])

c = math.cos(theta)

s = math.sin(theta)

pts = []

for lx, ly in verts:

rx = lx * c - ly * s

ry = lx * s + ly * c

px, py = coord_to_pixel(x + rx, y + ry)

pts.append((px, py))

 The algorithm iterates over the bounding-box vertices, rotates them according to the robot’s orientation using sine and cosine, and stores the transformed vertices in the points list.

 3:

    Ackerman robot:

    This stage included tuning the P controller values, adding a reverse state, adding an ALIGN state that orients the robot to the heading expected by the planner before generating the next plan, and selecting an appropriate turning radius by testing several candidate paths and choosing the one the robot could follow with high angular velocity.

     The same valid state checking using bounding box.

 

IMPLEMENTATION: 

 

 states:

     

PLAN

The robot computes a path to its next target.
It uses OMPL to generate a valid trajectory and converts it into waypoints.
When the plan is ready, it switches to GO.

GO

The robot follows the planned waypoints:

• Moves toward the next point

• Adjusts speed and steering based on orientation error

• Automatically switches to reverse if the turn is too sharp and the angle is reversed. 

• When the final waypoint is reached, it either aligns (ALIGN) or stops (STOP) depending on orientation

ALIGN 

If the robot reaches the goal position but the orientation is incorrect, it corrects its heading.
It turns slowly until the angle is within tolerance, then transitions to STOP.

STOP

The robot stops for 3 seconds.
After the pause:

• If it is not carrying a shelf → transition to LIFT

• If it is carrying a shelf → transition to PUT DOWN

This state works as a synchronization step between actions

LIFT

The robot lifts the shelf.
It updates its size to include the carried shelf and temporarily removes the obstacle from the map.
Then it returns to PLAN to compute the route to the drop-off location.

PUT DOWN

The robot places the shelf on the ground.
The obstacle is restored in the map and the robot’s size returns to normal.
If more shelves remain, it goes back to PLAN; otherwise, it enters DONE.

DONE

Final state.
The robot stops completely and the task is finished.

 

Problems and Solutions

During the development of this project, several issues appeared that required specific adjustments to ensure correct navigation and smooth motion:

• Orientation mismatch at the end of the plan
  The planner sometimes produced a final orientation that did not exactly match the robot’s actual heading when reaching the goal.
  This caused planning failures because OMPL expected the robot to be aligned before generating the next trajectory.
  To fix this, I introduced the ALIGN state, allowing the robot to correct its final orientation before switching to STOP or generating a new plan.

• Planning time too short
  When the allowed planning time was small, OMPL produced overly complex paths with aggressive turns.
  Due to the robot’s Ackermann steering, these paths were difficult to follow and caused large orientation errors.
  To reduce this problem, I increased the planning time to 20 seconds, which produced smoother and more feasible trajectories.

• Need for interpolation
  The raw OMPL output contained large jumps between states, which made the robot movement unstable.
  I solved this by interpolating the plan, generating a smoother set of intermediate waypoints for more consistent tracking.

• System orientation reference problem
  An additional issue appeared related to how the orientation system behaved.

   It was necessary to verify the system’s orientation conventions, and adjustments of ±π radians had to be applied depending on the context.

  f I set the planner’s goal orientation to 0 radians, it planned a trajectory that oriented the car backwards. If i plan adding math.pi radians to the goal it plans and sets the robot looking to the front. Nevertheless, the robot orientation is with 0 radians by looking to the left. To make 0 radians correspond to “facing forward” and compute the orientation to each waypoint correctly, I had to subtract π/2 radians.

  Situations in which I had to add or reduce angles:

   Moving:

  The robot's yaw value looking to the front is 0 radians thanks to the first correction: 

  yaw = pose.yaw + math.pi/2

  and the goal stablished by the planner is π to make it look to the front. When the car is with yaw = 0 radians the error must be 0 at the goal. So:

  err_goal = normalize_angle(goal_theta - math.pi - yaw)

  Planning:

  the car initial orientation is 0 radians to the left with the car looking to the front. So when looking at the robots orientation with 

  pose = HAL.getPose3d()

  pose.yaw 

   This returns -pi/2 so I had to reduce pi/2 when calculating the plan with the initial orientation of 0º

   

  Video: 

    Warehouse_1_Ackerman.

  The shelves are manually rotated because the friction of the simulator causes them to change their intended orientation when the robot sets them down.

  
  

  Warehouse_1_holonomic.

  The shelves were being moved by hand because, as the robot transported them, their orientation shifted. When the robot set them down, the shelves ended up in an unexpected position, which could cause the robot to collide with them.