This past week, my main focus was writing the robot’s task and motion planner. The current version of the algorithm works by assigning priorities to each robot and then precomputing the paths for all the robots so that they don’t collide. This works by planning in (x, y, theta, time). The map was discretized into a 2D grid where each box is 1cm x 1cm. This resolution can be changed as needed based on the planning time. The planner also takes into account the physical constraints of the robot (collision footprint, turn-radius constraints). Here is a picture of the result of planning for two robots. Paths to the pallet and from the pallet to the drop-off zone are planned at the same time.
