-
Notifications
You must be signed in to change notification settings - Fork 957
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
Conversation
There was a problem hiding this 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?
809ea8b
to
6292546
Compare
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 |
That's no problem.
No, our test coverage does not suffice.
This has to be pointed out to the user and we have to add an item to 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 The same applies to Personally, I would merge these patches in |
There was a problem hiding this 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.
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 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. |
I agree with @gavanderhoorn. I just noticed that this PR is targeting melodic, not master. |
6292546
to
b4f2058
Compare
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! |
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. |
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 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. |
Closing and reopening to trigger Travis. |
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 (I've updated 100+ repositories over at |
@peci1, could you please rebase onto latest master, such that I can merge this? |
b4f2058
to
0d27388
Compare
Rebased. I just wonder whether it is now more efficient to write
or
If I'm not mistaken, after calling On the other hand, Of course, the most efficient is probably
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. |
On Mon, Apr 27, 2020 at 04:25:00PM -0700, Martin Pecka wrote:
transform.linear().transpose()
but that clouds a bit the intention of inverting the matrix.
I clearly prefer this version, and I don't think it's confusing...
If you like, add an appropriate comment.
|
…Eigen::Transform::rotation().
0d27388
to
50b4019
Compare
Okay, I changed utils.cpp to use |
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 Any other opinions? |
@peci1 could you please add the Isometry checks to get this merged? It's been dangling for almost two months now... |
67e4c77
to
9b7b7fb
Compare
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 |
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 |
True. You could add
yes, please. You're talking about |
There was a problem hiding this 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.
moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h
Show resolved
Hide resolved
|
||
// 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 |
There was a problem hiding this comment.
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 |
There was a problem hiding this comment.
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 |
There was a problem hiding this comment.
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 |
There was a problem hiding this comment.
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 |
There was a problem hiding this comment.
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)); |
There was a problem hiding this comment.
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 |
There was a problem hiding this comment.
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 |
There was a problem hiding this comment.
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) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
see above.
There was a problem hiding this 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...
@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 |
but what are user inputs?
As a general rule of thumb, everything is.
After all, MoveIt exports almost everything as public API.
That's the crux of the matter.
Are data read from TF user inputs or not? I think this needs to be better specified.
Great question. I would argue they can be treated as sanitized because other systems, prominently RViz, but possibly every TF listener should warn if something invalid is published there.
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?
No. Eigen::Isometry3d *is* an annotation already, even if Eigen fails to verify the invariant.
If anything we could wrap the class and add check, but that seems overkill too.
|
9b7b7fb
to
6f03985
Compare
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 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):
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. |
You could temporarily replace the |
6f03985
to
561b240
Compare
@simonschmeisser good idea. When built with |
On Sat, May 23, 2020 at 02:24:51AM -0700, Martin Pecka wrote:
I just wonder nobody has noticed that debug-build tests are sooo slow.
CI runs tests without debug.
Locally, I notice this issue a lot, but it was not important enough to me to work on it.
There are a number of tests with unreasonably long runtime, in many cases by excessive test iterations, as in the case you cite.
I would appreciate efforts to limit test repetitions to a sane amount.
|
561b240
to
d15682b
Compare
Codecov Report
@@ 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
Continue to review full report at Codecov.
|
There was a problem hiding this 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...
Thanks goes to you, @peci1, for all your work and patience! |
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, callinglinear()
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.