Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Improve performance by calling Eigen::Transform::linear() instead of Eigen::Transform::rotation(). #1964

Merged
merged 2 commits into from
May 27, 2020

Conversation

peci1
Copy link
Contributor

@peci1 peci1 commented Mar 22, 2020

Description

As shown in moveit/geometric_shapes#125 (comment), Eigen doesn't yet contain the optimizations that would save computing SVD in case rotation() is called on an isometry. However, calling linear() gives the same result for isometries and doesn't need to compute SVD. In geometric_shapes, this change cut computation times tenfold for the affected functions.

This PR is more like an opening to a discussion. I don't expect it to be merged as is. Maybe it should be split per package, or there may be other concerns.

Copy link
Contributor

@v4hn v4hn left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

any reason why we should not merge this as-is?
could you please fix the format error in CI?

@peci1 peci1 force-pushed the optimize_rotation_performance branch from 809ea8b to 6292546 Compare March 22, 2020 10:51
@peci1
Copy link
Contributor Author

peci1 commented Mar 22, 2020

Fixed. I haven't thought a simple sed with a shorter string could generate formatting errors, but well, one line got shorter so much that a newline could be deleted from the middle of it :)

From my point of view, there are no reasons for not merging this. It just seemed as quite a big change to me (meaning spanning a lot of packages). And I don't know if moveit test coverage is good enough to be sure this didn't actually break anything (which it should not, but...).

And it will (silently) reveal all the places where somebody uses an invalid rotation, like I did e.g. here: moveit/geometric_shapes@8873450 . The computeRotationScaling() did actually cope with most kinds of invalid rotation by putting all the wrong numbers into the scaling matrix which is then thrown away. The updated behavior will pass the invalid rotation further completely silently, and the only visible change would be that some code will start acting weird. Passing an invalid transform is of course a problem of the user of the API, but well, some people might feel sad about it.

@v4hn
Copy link
Contributor

v4hn commented Mar 23, 2020

It just seemed as quite a big change to me (meaning spanning a lot of packages).

That's no problem.

I don't know if moveit test coverage is good enough to be sure this didn't actually break anything (which it should not, but...).

No, our test coverage does not suffice.
Feel free to contribute (even) more tests.

And it will (silently) reveal all the places where somebody uses an invalid rotation. [...]
The updated behavior will pass the invalid rotation further completely silently, and the only visible change would be that some code will start acting weird

This has to be pointed out to the user and we have to add an item to MIGRATION.md and we should consider merging in master only. Eigen does not validate Isometries at all, because it is rather expensive to do.
This change in behavior, garbage-in-garbage-out over silent normalization, affects computeCartesianPath, as well as user code maintaining their own CollisionWorld/adding objects outside the regular Quaternion representation, which is/should be normalized on our end via the planning scene.

Notice that users will have to live with the change either way in the long run, as it will automatically take effect with Eigen's next release.

Your test-case shows how easy it is to accidentally break an Isometry, e.g., via Eigen::AngleAxis3d(phi, Vector3d(1.0,1.0,1.0)) and retracting a normalization guarantee is no lightweight change. It frustrates me that we apparently did not do that as part of the Isometry3d transition for melodic.

The same applies to geometric_shapes, of course.

Personally, I would merge these patches in melodic-devel, still, but that is mostly because we keep pushing more and more things in the master branch and noetic and future release progress is very much uncertain.
Looking for opinions from other maintainers, @gavanderhoorn @rhaschke @jrgnicho @davetcoleman .

@v4hn v4hn added the needs discussion Different maintainers might disagree here label Mar 23, 2020
Copy link
Contributor

@rhaschke rhaschke left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

For me, this looks good. No need to split into different PRs for different packages.

@gavanderhoorn
Copy link
Contributor

I would be OK with breaking things for others, but as always, breaking things means you'll get unhappy customers.

Customers in an OSS setting are potential contributors.

If this is done in master and mentioned in the migration guide, this could be acceptable though.

Breaking melodic is not a good idea. It's going to make a lot of people unhappy, and we would be violating our (implicit) contract with them. master is a free-for-all. Until we cut a release from it.

@rhaschke
Copy link
Contributor

I agree with @gavanderhoorn. I just noticed that this PR is targeting melodic, not master.
@peci1, could you please rebase onto master, please, and then target this PR against master?

@peci1 peci1 force-pushed the optimize_rotation_performance branch from 6292546 to b4f2058 Compare March 26, 2020 01:54
@peci1 peci1 changed the base branch from melodic-devel to master March 26, 2020 01:55
@simonschmeisser
Copy link
Contributor

Most of these changes seem not to be user facing? Maybe we can exclude all places where user-generated data is used and then backport the speed gains to melodic. Especially the AABB calculation should be quite beneficial!

@peci1
Copy link
Contributor Author

peci1 commented Mar 26, 2020

I retargeted to master.

@simonschmeisser I thought exactly the same. But as I don't see into moveit that much, I probably couldn't do that work myself.

As CI is down now, I ran the tests locally and all are green.

@peci1
Copy link
Contributor Author

peci1 commented Mar 26, 2020

A wrap-up from WG meeting: I'll add asserts to the code which will check the isometries. These asserts will be compiled out in release builds. This change should be mentioned in Migration.md and should have a thread on discourse. Having all this done, it should be considered for backporting to melodic-devel.

We also discussed issuing a ROS_WARN_ONCE() or something like that with a message to the user that if his code starts behaving weird, he should run in debug mode and see if any of the asserts fails for him and where do the wrong data originate. But I'm not sure there was a broad agreement on this warning message. And it is unclear what's the best place to issue this warning.

Also, tutorials should be checked and notes should be added to places where beginners (or even pros) might create a non-isometry when not being careful.

@rhaschke
Copy link
Contributor

Closing and reopening to trigger Travis.

@rhaschke rhaschke closed this Mar 27, 2020
@rhaschke rhaschke reopened this Mar 27, 2020
@gavanderhoorn
Copy link
Contributor

gavanderhoorn commented Mar 27, 2020

If what we're seeing with Travis is indeed caused by the app vs other integration, then the branch protection rules must be updated to require the new Travis PR builds. I believe continuous-integration/travis-ci is the old one, which isn't run any more, so the status will never be updated.

(I've updated 100+ repositories over at ros-industrial and ros-industrial-consortium with the same symptoms)

@rhaschke
Copy link
Contributor

@peci1, could you please rebase onto latest master, such that I can merge this?

@peci1
Copy link
Contributor Author

peci1 commented Apr 27, 2020

Rebased.

I just wonder whether it is now more efficient to write

transform.linear().inverse()

or

transform.inverse().linear()

If I'm not mistaken, after calling linear(), the fact that the transform is an isometry and linear() is just pure rotation gets lost... So I'd bet the subsequent inverse will do the costly full inverse.

On the other hand, transform.inverse() is optimized for isometries to actually use the transposition on its linear part. But it creates an unnecessary 4x4 matrix instead of the needed 3x3...

Of course, the most efficient is probably

transform.linear().transpose()

but that clouds a bit the intention of inverting the matrix. Of course, people with experience will probably see inversion and transposition as equivalent, but still, the semantics gets a little clouded...

Anyways, the only use I found was in https://github.com/ros-planning/moveit/blob/ad88e18ae604e0533456c89eb0c6694bb0b63081/moveit_core/kinematic_constraints/src/utils.cpp#L570 . So maybe it's not that important at all... It just caught my interest.

@v4hn
Copy link
Contributor

v4hn commented Apr 28, 2020 via email

@peci1 peci1 force-pushed the optimize_rotation_performance branch from 0d27388 to 50b4019 Compare April 28, 2020 15:46
@peci1
Copy link
Contributor Author

peci1 commented Apr 28, 2020

Okay, I changed utils.cpp to use linear().transpose()

@v4hn
Copy link
Contributor

v4hn commented Apr 28, 2020

Personally, I would prefer to have the additional Isometry check in debug mode for the general MoveIt code base too. I guess we could reuse the assert definition in geometric_shapes, although we should probably put it in a new header there for that.

Any other opinions?

@v4hn
Copy link
Contributor

v4hn commented May 18, 2020

@peci1 could you please add the Isometry checks to get this merged? It's been dangling for almost two months now...

@peci1 peci1 force-pushed the optimize_rotation_performance branch 4 times, most recently from 67e4c77 to 9b7b7fb Compare May 21, 2020 11:34
@peci1
Copy link
Contributor Author

peci1 commented May 21, 2020

I added the isometry checks. They revealed two tests with "homemade" quaternions that did not result in proper rotations. That's all.

Now Travis complains about warnings that macro ASSERT_ISOMETRY() is being redefined. So what's the proper way to proceed with reusing the macro from geometric_shapes? Should I first issue a PR to geometric_shapes master branch that would expose this macro, and once it is merged, I'd reuse it here?

@peci1
Copy link
Contributor Author

peci1 commented May 21, 2020

I also tried to document most places in the code with a hint why the assertion needs to be done, or why it isn't needed.

I just still haven't decided whether it's better to put the assertion right after the place where the isometry is constructed, or whether it is better to put it right before the place where linear() is called.

@v4hn v4hn removed the needs discussion Different maintainers might disagree here label May 21, 2020
@v4hn
Copy link
Contributor

v4hn commented May 21, 2020

Now Travis complains about warnings that macro ASSERT_ISOMETRY() is being redefined.

True. You could add MOVEIT_*, but reusing is probably better as we already depend on geometric_shapes.

So what's the proper way to proceed with reusing the macro from geometric_shapes? Should I first issue a PR to geometric_shapes master branch that would expose this macro, and once it is merged, I'd reuse it here?

yes, please. You're talking about noetic-devel though.

Copy link
Contributor

@v4hn v4hn left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

For very workload-intense cases (constraint evaluation) I would move the asserts to configuration time, as mentioned in multiple places inline.

The second main issue is that you assumed getGlobalFrameTransform and getFrameInfo are unsanitized, but these methods should be trusted, possibly adding the assert inside if necessary.
I went through a large part of the patch and eventually stopped commenting on it.


// if this constraint is with respect a mobile frame, we need to convert this rotation to the root frame of the
// model
if (sampling_pose_.orientation_constraint_->mobileReferenceFrame())
{
const Eigen::Isometry3d& t = ks.getFrameTransform(sampling_pose_.orientation_constraint_->getReferenceFrame());
Eigen::Isometry3d rt(t.rotation() * quat);
quat = Eigen::Quaterniond(rt.rotation());
ASSERT_ISOMETRY(t) // getFrameTransform() could return non-isometry
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

No it could not.
If we want that assert, it should be in RobotState::getFrameInfo()

@@ -563,9 +568,12 @@ bool IKConstraintSampler::sampleHelper(moveit::core::RobotState& state, const mo
// we need to convert this transform to the frame expected by the IK solver
// both the planning frame and the frame for the IK are assumed to be robot links
Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat);
ikq = reference_state.getFrameTransform(ik_frame_).inverse() * ikq;
const Eigen::Isometry3d& ik_frame_transform = reference_state.getFrameTransform(ik_frame_);
ASSERT_ISOMETRY(ik_frame_transform) // getFrameTransform() could return non-isometry
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

See above.

@@ -574,7 +582,8 @@ bool IKConstraintSampler::sampleHelper(moveit::core::RobotState& state, const mo
Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat);
ikq = ikq * eef_to_ik_tip_transform_;
point = ikq.translation();
quat = Eigen::Quaterniond(ikq.rotation());
ASSERT_ISOMETRY(ikq) // eef_to_ik_tip_transform_ could be non-isometry
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Could you please check eef_to_ik_tip_transform_ instead and move the check to the two lines in `loadIKSolver(), where it is set?
No need for useless repetitive checks even in debug mode.

@@ -595,18 +597,24 @@ ConstraintEvaluationResult OrientationConstraint::decide(const moveit::core::Rob
return ConstraintEvaluationResult(true, 0.0);

Eigen::Vector3d xyz;
const Eigen::Isometry3d& globalLinkTransform = state.getGlobalLinkTransform(link_model_);
ASSERT_ISOMETRY(globalLinkTransform) // getGlobalLinkTransform() could return a non-isometry
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

see above

@@ -567,7 +568,8 @@ bool kinematic_constraints::resolveConstraintFrames(const moveit::core::RobotSta
if (c.link_name != robot_link->getName())
{
c.link_name = robot_link->getName();
Eigen::Quaterniond link_name_to_robot_link(transform.linear().inverse() *
ASSERT_ISOMETRY(transform) // getFrameInfo() could return a non-isometry
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

See above.

EXPECT_TRUE(vc.configure(vcm, tf));
EXPECT_TRUE(vc.decide(robot_state, true).satisfied);

// a little bit more puts it over
vcm.target_pose.pose.orientation.y = 0.06;
vcm.target_pose.pose.orientation.w = .9981;
vcm.target_pose.pose.orientation.w = sqrt(1 - pow(vcm.target_pose.pose.orientation.y, 2));
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

👍

@@ -60,9 +61,10 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons
{
// this is the Cartesian pose we start from, and have to move in the direction indicated
const Eigen::Isometry3d& start_pose = start_state->getGlobalLinkTransform(link);
ASSERT_ISOMETRY(start_pose); // getGlobalLinkTransform() could return a non-isometry
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

see above.

@@ -87,12 +89,14 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons

// this is the Cartesian pose we start from, and we move in the direction indicated
Eigen::Isometry3d start_pose = start_state->getGlobalLinkTransform(link);
ASSERT_ISOMETRY(start_pose) // getGlobalLinkTransform() could return a non-isometry
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

see above.

@@ -1209,6 +1210,7 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link
jacobian = Eigen::MatrixXd::Zero(rows, columns);

Eigen::Isometry3d link_transform = reference_transform * getGlobalLinkTransform(link);
ASSERT_ISOMETRY(link_transform)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

see above.

Copy link
Contributor

@rhaschke rhaschke left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would propose to restrict the isometry asserts to user inputs only. If entered the framework once in the correct shape, the isometry shouldn't be lost...

@peci1
Copy link
Contributor Author

peci1 commented May 22, 2020

@rhaschke okay, but what are user inputs? Are data read from TF user inputs or not? I think this needs to be better specified.

@v4hn Would it make sense then to create some kind of annotation (at least in function documentation) that would tell whether the returned isometry can be trusted? I was even thinking about creating a new type like UnsafeIsometry3d which would be used for user inputs, but that seems like an overkill to me. On the other hand, it would make the API crystal clear.

@v4hn
Copy link
Contributor

v4hn commented May 22, 2020 via email

@peci1 peci1 force-pushed the optimize_rotation_performance branch from 9b7b7fb to 6f03985 Compare May 22, 2020 19:13
@peci1
Copy link
Contributor Author

peci1 commented May 22, 2020

I've just pushed a new version which moves the check more towards the places where the values are set. I also updated documentation so that it mentions if a transform can be trusted to be an isometry. There would probably be much more places which would need this update, but these are all that have something to do with calling linear() instead of rotation() (at least in moveit code).

I tried running tests locally in debug mode, but some of them take enormous time to complete (I tried the same with current master and the behavior is more or less the same, so I assume this isn't a problem introduced in this PR):

-- run_tests.py: execute commands
  /media/data/ros/devel-debug/lib/moveit_ros_robot_interaction/locked_robot_state_test --gtest_output=xml:/media/data/ros/build-debug/test_results/moveit_ros_robot_interaction/gtest-locked_robot_state_test.xml
[==========] Running 16 tests from 1 test case.
[----------] Global test environment set-up.
[----------] 16 tests from LockedRobotState
[ RUN      ] LockedRobotState.load
[ INFO] [1590159631.586877101] [ros.moveit_core.robot_model]: Loading robot model 'one_robot'...
[       OK ] LockedRobotState.load (12 ms)
[ RUN      ] LockedRobotState.URDF_sanity
[       OK ] LockedRobotState.URDF_sanity (0 ms)
[ RUN      ] LockedRobotState.robotStateChanged
[       OK ] LockedRobotState.robotStateChanged (1 ms)
[ RUN      ] LockedRobotState.set1
[       OK ] LockedRobotState.set1 (8792 ms)
[ RUN      ] LockedRobotState.set2
[       OK ] LockedRobotState.set2 (15115 ms)
[ RUN      ] LockedRobotState.set3
[       OK ] LockedRobotState.set3 (24244 ms)
[ RUN      ] LockedRobotState.mod1
[       OK ] LockedRobotState.mod1 (282483 ms)
[ RUN      ] LockedRobotState.mod2
[       OK ] LockedRobotState.mod2 (132392 ms)
[ RUN      ] LockedRobotState.mod3
[       OK ] LockedRobotState.mod3 (13359 ms)
[ RUN      ] LockedRobotState.set1mod1
[       OK ] LockedRobotState.set1mod1 (92560 ms)
[ RUN      ] LockedRobotState.set2mod1
[       OK ] LockedRobotState.set2mod1 (128344 ms)
[ RUN      ] LockedRobotState.set1mod2
[       OK ] LockedRobotState.set1mod2 (222076 ms)
[ RUN      ] LockedRobotState.set3mod1
[       OK ] LockedRobotState.set3mod1 (183312 ms)
[ RUN      ] LockedRobotState.set1mod3
[       OK ] LockedRobotState.set1mod3 (399067 ms)
[ RUN      ] LockedRobotState.set3mod3
[       OK ] LockedRobotState.set3mod3 (575074 ms)
[ RUN      ] LockedRobotState.set3mod3c3
[       OK ] LockedRobotState.set3mod3c3 (559277 ms)
[----------] 16 tests from LockedRobotState (2636109 ms total)

[----------] Global test environment tear-down
[==========] 16 tests from 1 test case ran. (2636112 ms total)
[  PASSED  ] 16 tests.

Yes, 2636 second to do 16 tests. This is the worst one, though. In release mode, this whole test suite takes about 15 seconds.

I also tried debug-compiled moveit with release-compiled geometric_shapes, but the speedup was negligible.

@simonschmeisser
Copy link
Contributor

You could temporarily replace the #ifdef NDEBUG by something different and run the tests in release mode. The slowdown is most likely because Eigen does "a few more tests" in debug mode.

@peci1 peci1 force-pushed the optimize_rotation_performance branch from 6f03985 to 561b240 Compare May 23, 2020 01:49
@peci1
Copy link
Contributor Author

peci1 commented May 23, 2020

@simonschmeisser good idea. When built with -O3 and without NDEBUG, the tests run reasonably fast. I just wonder nobody has noticed that debug-build tests are sooo slow. I also tested several past versions (back to 1.0.3), and all of them have the tests so slow, so it is nothing newly introduced.

@v4hn
Copy link
Contributor

v4hn commented May 23, 2020 via email

@peci1 peci1 force-pushed the optimize_rotation_performance branch from 561b240 to d15682b Compare May 24, 2020 22:52
@codecov-commenter
Copy link

Codecov Report

Merging #1964 into master will increase coverage by 3.72%.
The diff coverage is 77.90%.

Impacted file tree graph

@@            Coverage Diff             @@
##           master    #1964      +/-   ##
==========================================
+ Coverage   54.07%   57.80%   +3.72%     
==========================================
  Files         319      328       +9     
  Lines       25019    25690     +671     
==========================================
+ Hits        13530    14850    +1320     
+ Misses      11489    10840     -649     
Impacted Files Coverage Δ
...tection/include/moveit/collision_detection/world.h 88.88% <ø> (ø)
.../constraint_samplers/default_constraint_samplers.h 72.72% <ø> (ø)
moveit_core/distance_field/src/distance_field.cpp 26.02% <ø> (ø)
...oveit/kinematic_constraints/kinematic_constraint.h 93.10% <ø> (ø)
moveit_core/planning_scene/src/planning_scene.cpp 59.37% <0.00%> (+1.68%) ⬆️
...bot_model/include/moveit/robot_model/joint_model.h 72.72% <ø> (ø)
...veit_core/robot_model/src/revolute_joint_model.cpp 89.76% <0.00%> (-0.72%) ⬇️
...t_state/include/moveit/robot_state/attached_body.h 100.00% <ø> (ø)
...ution_manager/src/trajectory_execution_manager.cpp 47.75% <0.00%> (+0.89%) ⬆️
...kinematic_constraints/src/kinematic_constraint.cpp 72.80% <50.00%> (+0.06%) ⬆️
... and 81 more

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update ad88e18...d15682b. Read the comment docs.

Copy link
Contributor

@v4hn v4hn left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Good to go from my perspective.

Thank you for reiterating over multiple rounds of feedback @peci1 🥇

Let's get this merged and finally be done with the Isometry-related svd in the background...

@v4hn v4hn added the awaits 2nd review one maintainer approved this request label May 27, 2020
@rhaschke rhaschke merged commit 272eaa0 into moveit:master May 27, 2020
@peci1
Copy link
Contributor Author

peci1 commented May 27, 2020

Thank you too, @v4hn and @rhaschke for getting this done ;)

@rhaschke
Copy link
Contributor

Thanks goes to you, @peci1, for all your work and patience!

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
awaits 2nd review one maintainer approved this request
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

6 participants