This project contains the visual cognition and motion planning part of a cognitive wiping robot. The task is assumed to be wiping off the words written on a white board.
Install Intel® RealSense™ SDK 2.0 following official instruction
This project uses python 3.7. To install required packages:
pip install -r requirements.txt
Choose region of interest manually as working area. In this case, the largest contour max_cnt is usually the bounding rectangular of RoI.
Optionally:
- Detect the largest contour
max_cntor - Detect the bounding rectangular or
- Detect the contour of largest white area as working area..
Default: Reserve only contours inside max_cnt as stains to be cleaned.
Option 1: Bounding rectangles can be used instead of contours.
However, the size of rectangular kernel needs to be set manually to get a suitable bounding rectangle.
The wiping task can be formed as a Covering Traveling Salesman Problem. To guarantee that the cleaner will pass each pixel of stains, each point on contours c is checked if it lies within an e-neighborhood of current nodes, where the value of e depends on the size of the tool (e.g. radius of the brush) used to accomplish the task. If not, c is added to the node_list.
As is shown in the image above, blue point stands for the starting point, red points are all the nodes for planning algorithms and green shadows marks the area the brush covers.
The task of passing all the nodes is then formed as a Traveling Salesman Problem, and will be solved by following algorithms.
Starting from the initial position (current position of end effector, marked as blue), select the nearst node to be the next move.
The computation for nn is quite efficient, but the path is obviously not optimal:
The path is planned by dynamic programming. However, the computation time is relatively long.
The path is planned by ant colony algorithm. Area covered by the brush along the path is marked as blue shadow. Here the population is set as 50 and it runs for 200 iterations.
Path planned with neighborhood radius e=10:
The computation for ac takes longer, but the resulted path is also more cost-efficient:
Path planned with neighborhood radius e=5:
To detect stains and plan the path of the end-effector:
python viewer.py
As is shown in the demo, the RoI is firstly selected as the working area. The planned path (marked as blue shadow) is always able to cover all the words.
Note that this demo uses Nearst Neighbor as planning algorithm. For more complex algorithms like Dynamic Programming and Ant Colony, real-time planning and response are not possible.
Results above were obatined under the assumption that nodes are all connected to each other. This guarantees that a path will be found, but also leads to computational burdens in planning. To speed up the planning process, a graph G=(V,E) is constructed following steps in J.Hess2012. The idea is that each node is only connected to top k nearest neighbors within a radius r. In this way, number of edges to be explored is supposed to be cut down. Below is the graph constructed with k=5 and r=30.
Below, two attempts to implement the planning based on this idea are introduced.
An intuitive method is to modify the distance matrix. The entries(costs) that corresponde to unconnected edges are changed into some extremely large numbers. In this way, these edges will not (possibly) chosen by the planning algorithm.
The path planned by graph-based ant colony algorihtm with k=10 and r=70:
and with k=20 and r=70:
As is shown in the screenshots above, tuning k and r has the potential to find a slight more cost-efficient path. However, this method did not descrease the number of iterations in the algorithm, and thus cannot really speed up the computation.
At each node, graph-based path planning will ignore the unconnected nodes, i.e. only interate over the nodes adjecent to it in the graph. However, as each node is only allowed to be visited once, this method usually fails in finding a valid path.