[IRC] Reset the restPositions while fixed - make consistent StartPosition #205
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
In 3instruments_collis, we can notice a slight bump at the beginning of the deployment of the catheter.
One of the reason is that the first liberated point (when the deployed length increases and need to "free" one DOF) when freed, first go its restPosition. And in this scene, the restPosition is set by the RegularGrid (and is a linear set line of points)
So to fix this, while "fixing" the undeployed tool, the restPosition is also set at startPosition.
EDIT: actually this PR is much more important than only fixing the "bump".
Now, the starting orientation of the beam is solely set by the startingPos data, and no more from the orientation (?) of the grid which was weird.
Illustration of the bump:

Before
After

And illustration of the shenanigans of the bug in the state position: (this is supposed to go along the Z axis)

Before
After
