Hi! I'm a Robotic researcher familiar with Python, C++, and Robotic libraries like ROS. I'd like to help you with this problem.
I have some ideas of the solution, but still need to get more information. After obtaining a detailed explanation of the problem, I'll firstly try to search online for existing methods based on the effectiveness and reproducibility of the algorithm. If not, I"ll try setting up a Mathematical model to analyze the problem.
Then I'll write a well documented program for this problem, so that you can also understand what each function does.
After that, I'll try coming up with examples and test the code.
I"ll try finishing it in 4 days, and probably come up an early demo.
Please feel free to contact me, and I'll keep you posted about my progress during the project.
Thanks :-)