Trim tree unsolvable state

[nichwall]

Tree trimming needs to ignore previous state when getting the next step. If the distance between our current and the target Pose and the new current and target Pose is identical, the solver will get stuck in a loop switching back and forth between these two states. Some method of keeping track of the last iteration needs to be implemented