Added a check to ensure that the shortest path is followed when inter… #14
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
stefanscherzinger
suggested changes
Aug 11, 2022
There was a problem hiding this comment.
Thanks @urmahp for this fix!
Your contribution is very welcome!
I would recommend, however, that we split this PR into two parts:
- Your fix for the sign (see my small suggestions)
- The changes of coordinates when handling body-local quaternion velocities and accelerations. While
2.seems reasonable (thanks for the paper reference), I feel we need a separate integration test to cover this use case. I think we currently don't do much velocity or acceleration conditions in the waypoints. But that's exactly where these mechanism show themselves. If you agree, We could take the opportunity to clarify this aspect in a separate PR.
cartesian_trajectory_interpolation/src/cartesian_trajectory.cpp
Outdated
Show resolved
Hide resolved
cartesian_trajectory_interpolation/src/cartesian_trajectory.cpp
Outdated
Show resolved
Hide resolved
cartesian_trajectory_interpolation/src/cartesian_trajectory.cpp
Outdated
Show resolved
Hide resolved
cartesian_trajectory_interpolation/test/cartesian_trajectory_test.cpp
Outdated
Show resolved
Hide resolved
cartesian_trajectory_interpolation/test/cartesian_trajectory_test.cpp
Outdated
Show resolved
Hide resolved
Thans for the feedback! I will split the PR into to two PR's and change the suggestions. I agree that using conjugate looks cleaner, when flipping the sign, however it wouldn't be correct since only flipping the sign on the complex part would result in a different orientation. You have to flip the sign of the whole quaternion in order to get the same orientation. We could use it to negate the complex part and then negate the real part afterwards. |
…polating between two orientations Addded unit tests to verify the changes. If the dot product between two orientations is negative, the sign is flipped on one of them to ensure that we take the shortest path. Without this we could end up interpolating between two identical orientations, but with opposite signs.
urmahp
force-pushed
the
interpolation_quat
branch
from
a5520f2
to
de5c994
Compare
cartesian_trajectory_interpolation/src/cartesian_trajectory.cpp
Outdated
Show resolved
Hide resolved
Co-authored-by: Stefan Scherzinger <[email protected]>
stefanscherzinger
approved these changes
Aug 11, 2022
|
how this is integrated to the controller, can you share the steps please, I have an ur3e |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
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.
…polating between two orientations
Changed the order of multiplication when calculating the quaternion based velocity and acceleration, since the velocity and acceleration is given in the body-local reference frame.
The math supporting this change, can be found here.
Added unit tests to verify the changes.
If the dot product between two orientations is negative, the sign is flipped on one of them to ensure that we take the shortest path. Without this we could end up interpolating between two identical orientations, but with opposite signs.
This should fix the issue.
To test the following motion will fail on the old version, but work with these changes.
start_pose.position = [-0.63316, -0.50412, 0.07724]
start_pose.orientation = [0.1046779, -0.7045626, 0.1234662, 0.6909343]
end_pose.position = [-0.78590, -0.19334, 0.07724]
end_pose.orientation = [0.0485966, 0.7107247, 0.031284, -0.7010921]