---
# System prepended metadata

title: kings Planner for Instock technical interview
tags: [ROBOTICS, path planning]

---

# kings Planner for Instock technical interview
###### tags: `path planning`, `ROBOTICS`
###### Assignment introduction: [The link](https://docs.google.com/document/d/1OuxEgQZhpVOiA2Ey-UwR8KYV_ftqRZBey1ZAhPB8vC4/edit?usp=sharing)
## Code organization in main folder
    ./root
    ├── README.pdf                      # Document explaining the implementation
    ├── README.md                       # Readme file for the workspace
    ├── planner.py                      # The code of the main Planner
    ├── solution.txt                    # Output of all paths of the king
    ├── problem-tests                   # The folder of testing data                         
    │   ├──1
    │   │   ├── kings.txt               # the start and goal position of kings
    │   │   ├── map.txt                 # Info of grid 
    │   ├──2 ...

## Envorinment
* programming language: python (3.9.2)
* OS: Linux, ubuntu 20.04

## Run the code(Manually)
``` bash
python planner.py
```

## Change test data
If you want to change the test data, please add a specific file name to the command.

For example
``` bash
python planner.py --kings ./problem-tests/2/kings.txt --map ./problem-tests/2/map.txt
```
The default is ``--kings ./problem-tests/1/kings.txt --map ./problem-tests/1/map.txt``

## Solution
![solution](https://imgur.com/zFbe4M4.png)
The above sample is the solution.txt generated based on the first set of test data(``./problem-tests/2``). According to the instructions, each line is a step for a king. When the first king reaches the goal, it will continue with the second king until the end.

## Main Code Explanation
At the beginning, I did basic A* algorithm on all kings and stored the path. Then set the global time step to start moving individual kings and check whether there are collisions (8 cells are blocked around a king).

The strategy to resolve the collision is to move to a nearby valid position(others 3 cells), reuse A* algorithm to calculate the path to the goal(target) based on this current position, and update the original path.

Of particular note is that kings who plan their roads earlier will receive higher priority. If the latter king enters the restricted area, it will move to other valid areas nearby.

If there are only some kings that cannot reach the goal, there will be no corresponding path in the ``solution.txt``. If no king reaches the goal, the terminal will display "Plan does not exist" and no ``solution.txt`` will be generated.

* noted: From the instock simulation video, AMR will only move adjacent to 4 cells, so the kings in this assignment will only move adjacent to 4 cells.

## Future Work:

The current method is A* replanner and it cannot improve the success rate, especially in a limited space. The path planned by A* will give priority to reaching the destination and will not avoid other kings. Until it is necessary to pause or move, it will already cause traffic jams and make it impossible to move. This is the disadvantage of A* replanner.

The better solutions to this problem can be D* or Dynamic window approach. For D*, changing the cost function based on the A*  avoid collisions in advance based on the kings ahead(increasing weight/cost). For ynamic window approach, considering the speed rate change(suitable pause in advance) may avoid collision with other kings or entering the restricted area.

## Reference:

A* search algorithm:
https://en.wikipedia.org/wiki/A*_search_algorithm

Hart, P. E.; Nilsson, N.J.; Raphael, B. (1968). "A Formal Basis for the Heuristic Determination of Minimum Cost Paths". IEEE Transactions on Systems Science and Cybernetics. 4 (2): 100–7. doi:10.1109/TSSC.1968.300136.

python document
https://www.python.org/

ChatGPT(for text grammar and syntax issues)
https://chat.openai.com/

D* search algorithm:
https://en.wikipedia.org/wiki/D*

Dynamic window approach
https://en.wikipedia.org/wiki/Dynamic_window_approach

Fox D , Burgard W , Thrun S . The dynamic window approach to collision avoidance[J]. IEEE Robotics & Automation Magazine, 1997, 4(1):23-33.