This paper tries to generate motion that requires specific
pose, position, and orientation in specific times while
avoiding collisions in an efficient way.
Given 2 constrained frames and having a way to fill in
between, we can repeatitively do it for more complex ones.
First using random sampling and connecting the viable
positions, we create a graph that we use dijikstra's
algorithm to find a path.
Then it does 2 searches one from the start
constraint and one from the end constraint.
Then it does random search to find a place to stitch
the two results at a place where the error is below the
threshold. Stitching is done by adding displacements.
Since this is a type of bi-directional search,
maybe it might be able to tune the search distance
so we don't need a full search to the end and to the
start. Reducing the search len by 30%~50% might give
better performance, but higher chance of a higher
error stitch.