I found another bug in the code. When we were checking if all intemediary points are safe, and we found one that was not, we stopped without adding a new field. I also added something in the StepSize function that makes sure we are not attempting to use to big of a step size. It should be good now.
Merge branch 'master' of https://github.com/hayesbh/Social-Hierarchical-Learning
Merge branch 'master' of https://github.com/hayesbh/Social-Hierarchical-Learning
I added a field when an intermediary position between current and next motor positions was not safe.