Fixed bugs in computation of AABB in LinkModel and RobotState. #699

peci1 · 2017-11-29T16:47:20Z

Fixed bugs in computation of AABB in LinkModel and RobotState.

Discussion started here: #516 (comment)

If you visualize the bounding box computed for the PR2 robot, it is plainly wrong:

This is the result with my fix:

I added tests that should check for most of the fail cases I've found. Here's a visualization of the "Complex" test:

There were 2 sources of wrong computations:

Assuming that when I take the 2 extremal points of a bbox and then apply an affine transform, these points remain extremal even in the new frame
Computing just the extents and ignoring any transformation of the collision elements inside a link

I'm almost sure the code is breaking ABI. Now the question is if there is a way (and will) to make it ABI compatible, or if this should just get cherry-picked to upstream. However, I think the consequences of these bugs can be pretty severe and it might be nice to have them fixed in all releases.

The code also might be optimized for performance, which I'm not good at.

Checklist

Required: Code is auto formatted using clang-format
Extended the tutorials / documentation, if necessary reference
Include a screenshot if changing a GUI
Optional: Created tests, which fail without this PR reference
Optional: Decide if this should be cherry-picked to other current ROS branches (Indigo, Jade, Kinetic)

simonschmeisser · 2017-11-29T21:50:50Z

I prefer breaking ABI over breaking stuff! Good catch!

peci1 · 2017-11-29T22:11:00Z

@simonschmeisser I was more asking for investigation if the fix could be rewritten in a way not to break ABI :) Maybe people know some good tricks.

The basic ABI breaker seems to be addition of Eigen::Vector3d centered_bounding_box_offset_ member to LinkModel, which IMO increases sizes of instances of LinkModel. There is a need for storing this offset somewhere. Couldn't we hide it somewhere? :) Computing it on each access would not be very efficient.

davetcoleman

Thanks so much for these changes!

davetcoleman · 2017-11-30T17:40:17Z

moveit_core/robot_model/include/moveit/robot_model/aabb.h

+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2013, Ioan A. Sucan


this is non-standard - Ioan worked for Willow at this time so we don't need both Copyright names, just Willow's

Ah, missed this one. Should I also update the year at Willow Garage?

davetcoleman · 2017-11-30T17:41:01Z

moveit_core/robot_model/include/moveit/robot_model/aabb.h

+/* Author: Martin Pecka */
+
+#ifndef MOVEIT_CORE_AABB_H
+#define MOVEIT_CORE_AABB_H


should be moveit_robot_model_aabb in all caps

davetcoleman · 2017-11-30T17:41:28Z

moveit_core/robot_model/src/aabb.cpp

+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2013, Ioan A. Sucan


davetcoleman · 2017-11-30T17:52:58Z

moveit_core/robot_state/src/robot_state.cpp

 void robot_state::RobotState::computeAABB(std::vector<double>& aabb) const
 {
  BOOST_VERIFY(checkLinkTransforms());

  aabb.clear();
+  core::AABB _aabb;


no underscore before variable name

davetcoleman · 2017-11-30T17:58:14Z

moveit_core/robot_state/src/robot_state.cpp

  std::vector<const LinkModel*> links = robot_model_->getLinkModelsWithCollisionGeometry();
  for (std::size_t i = 0; i < links.size(); ++i)
  {
-    const Eigen::Affine3d& t = getGlobalLinkTransform(links[i]);
+    Eigen::Affine3d t = getGlobalLinkTransform(links[i]);  // intentional copy, we will translate


can we use better variable names here besides one letter names? i know this was already here like this

Glad! I just wanted the PR to be minimal, but if you think it's time to become more verbose, I'm in ;)

you have a good point, you can do it in a separate PR if you want

feel free to change it as part of this request too, if you like.

davetcoleman · 2017-11-30T17:59:05Z

moveit_core/robot_state/test/test_aabb.cpp

+    return file_string;
+  }
+
+  virtual void SetUp(){};


lowercase first letter

I don't think so...

ROS style guide:

In general, function and class method names are camelCased, and arguments are under_scored

http://wiki.ros.org/CppStyleGuide#Function_.2BAC8_Methods

It's overriding https://github.com/google/googletest/blob/master/googletest/include/gtest/gtest.h#L430 , which is SetUp.

davetcoleman · 2017-11-30T17:59:33Z

moveit_core/robot_state/test/test_aabb.cpp

+
+  robot_state::RobotState loadModel(const std::string urdf, const std::string srdf)
+  {
+    boost::shared_ptr<urdf::ModelInterface> parsed_urdf(urdf::parseURDF(urdf));


i know we have mixed API, but is it possible to use std::shared_ptr here instead?

This is targeted at indigo-devel. Wouldn't that be a problem? The system gcc on Trusty doesn't know about C++11 (I think).

Ah yes, it should be boost if you are on indigo. Preferably this would only target kinetic, but I understand you might have constraints for this. When we cherry-pick this to kinetic we'll have to switch to std::shared_ptr

davetcoleman · 2017-11-30T18:00:28Z

moveit_core/robot_state/test/test_aabb.cpp

+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();


these tests are awesome, good job!

v4hn · 2017-12-03T15:57:47Z

I prefer breaking ABI over breaking stuff!

Does the broken AABB computation actually break something for many users? I don't find references to computeAABB outside the visualization code. Let's avoid ABI breaks in indigo unless they are critical to safety.

The basic ABI breaker seems to be addition of Eigen::Vector3d centered_bounding_box_offset_ member to LinkModel, which IMO increases sizes of instances of LinkModel. There is a need for storing this offset somewhere. Couldn't we hide it somewhere? :) Computing it on each access would not be very efficient.

@peci1 Yes it's very inefficient to recompute on each access. I am in favor of merging this request almost as-is in kinetic - modulo @davetcoleman's usual nitpicks. Thank you so much for contributing!

Given that the code is not much-used in the MoveIt code base and assuming it is not safety-critical, It would be great if we could find an enduser-friendly solution for indigo.
I see two alternatives:

recompute the offsets each time ignoring the overhead
store the offsets in a global map<const LinkModel*, Eigen::Vector3d> (including mutex)

Both are sub-optimal, but would retain ABI. Would you be willing to adapt the changeset in a second request for indigo (or file the current state in a new patch for kinetic and update this one)?

simonschmeisser · 2017-12-03T18:28:00Z

@v4hn you are probably right about this being used for visualization only. Actual collision stuff is handed over to fcl here https://github.com/ros-planning/moveit/blob/4bc43c56ca2a5293f3ef8d55796ec586cfde5f69/moveit_core/collision_detection_fcl/src/collision_robot_fcl.cpp#L39 and then the AABB seems to be computed here https://github.com/flexible-collision-library/fcl/blob/43048336c34a01156dc216e8534ffb2788675ddf/include/fcl/narrowphase/collision_object-inl.h#L118

@peci1 could you maybe check if the AABB is handled better in fcl?

peci1 · 2017-12-04T16:23:41Z

I did the little fixed that should make the changes eligible to be merged to kinetic-devel. I tested it there and it seemed to me all tests pass.

Regarding indigo-devel, I'll start implementing the global map+mutex solution.

peci1 · 2017-12-05T12:44:18Z

Or, it would also be an option to completely get rid of this custom AABB computation and leave it to FCL. But I don't know if introducing dependency robot_state -> collision_detection_fcl would be possible/wanted. What do you think?

v4hn · 2017-12-05T14:44:25Z

Or, it would also be an option to completely get rid of this custom AABB computation and leave it to FCL. But I don't know if introducing dependency robot_state -> collision_detection_fcl would be possible/wanted. What do you think?

De-facto we already have a hard dependency on FCL, although the user can replace the FCL collision checker on startup.
I believe both options are feasible. You spent the time to fix the current non-fcl implementation, so I slightly prefer your code, but I guess we can leave this choice to you.
At first glance I wouldn't expect a big difference in performance either, but I might be wrong.

Also added a way to visualize both AABBs and OBBs in RViz to verify.

peci1 · 2017-12-29T15:42:25Z

The way I computed the AABB was still buggy - I first transformed the two extremal vertices and then permuted their vertices; but the correct way is to permute the untransformed vertices and the transform the permutations.

Before the fix (notice the very thin darker thing - that's the AABB!):

After the fix:

Anyways, I think FCL has nicer code for computing the AABB, so I adapted it to be used here. First I thought of just calling the computeBV<AABB, Box>() FCL function, but it'd be too costly integration (they have different data types for both the extents, AABB, and they use floats instead of doubles).

I also finally added a piece of code that allows for easy visualization of the computed BBs directly from the test case. It's #ifdef'd out by default, so it brings no additional dependencies for anybody not interested in the visualization.

I'll cherry-pick this change to #703 and then I'll start adapting this indigo version to use the global map to retain ABI compatibility.

peci1 · 2017-12-29T16:26:22Z

Done. Let's see how the tests go :)

peci1 · 2018-01-03T09:58:05Z

Both indigo and kinetic PRs now pass the tests, and I've addressed the last set of comments from rhascke in them. So from my point of view, the PR is ready.

rhaschke · 2018-01-03T11:25:52Z

I'm not sure how to proceed on this and #703. Both of them address the same issue, once in Indigo and once in Kinetic. We should ensure that both change sets are identical (except required adaptions). My suggestion is to continue with #703 only (as this addresses the current master branch) and laterr backport those merged changes (with necessary adaptions) to Indigo. Thereby we can ensure consistency and reduce review overhead.
Hence, I'm closing this PR in favour of #703 for now.
@peci1 Please, file a new PR if #703 was merged. Or, if you disagree with the propose procedure, re-open the PR.

peci1 · 2018-01-03T11:28:45Z

@rhaschke All changes to kinetic-devel were cherry-picked from this branch, so the changesets should be identical.

peci1 · 2018-01-03T11:45:11Z

@rhaschke So, do you think there is need to open a new PR for Indigo, now when the kinetic version was merged?

rhaschke · 2018-01-03T12:24:22Z

@peci1 Yes, please open a new PR containing

a cherry-pick of Kinetic's merge-commit f6b85c6
your additional changes for Indigo

peci1 added 2 commits November 29, 2017 17:32

Fixed computation of a the AABB of links and models.

9844019

clang-format run

8d3fd69

davetcoleman reviewed Nov 30, 2017

View reviewed changes

Little fixes following Dave Colemans' suggestions.

1920470

peci1 mentioned this pull request Dec 4, 2017

Fixed bugs in computation of AABB in LinkModel and RobotState (kinetic) #703

Merged

Fixed buggy AABB computation.

4ad0a4f

Also added a way to visualize both AABBs and OBBs in RViz to verify.

peci1 added 2 commits December 29, 2017 16:50

Added a test for the AABB of a specific rotated link on the PR2.

6b81518

Retaining ABI compatibility for indigo.

f4dfc64

peci1 added 4 commits January 2, 2018 12:34

Fixed build for Indigo.

93ef4df

Fixed formatting.

e0a9a35

Removing const from the global map value type, it doesn't make sense.

400bf99

Addressed rhaschke's comments.

b6f0cf0

rhaschke closed this Jan 3, 2018

peci1 mentioned this pull request Jan 9, 2018

Fixed bugs in computation of AABB in LinkModel and RobotState. #738

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fixed bugs in computation of AABB in LinkModel and RobotState. #699

Fixed bugs in computation of AABB in LinkModel and RobotState. #699

peci1 commented Nov 29, 2017

simonschmeisser commented Nov 29, 2017

peci1 commented Nov 29, 2017

davetcoleman left a comment

davetcoleman Nov 30, 2017

peci1 Nov 30, 2017

davetcoleman Nov 30, 2017

davetcoleman Nov 30, 2017

davetcoleman Nov 30, 2017

davetcoleman Nov 30, 2017

peci1 Nov 30, 2017

davetcoleman Dec 1, 2017

v4hn Dec 3, 2017

davetcoleman Nov 30, 2017

peci1 Nov 30, 2017

davetcoleman Dec 1, 2017

peci1 Dec 1, 2017

davetcoleman Nov 30, 2017

peci1 Nov 30, 2017

davetcoleman Dec 1, 2017

davetcoleman Nov 30, 2017

v4hn commented Dec 3, 2017

simonschmeisser commented Dec 3, 2017

peci1 commented Dec 4, 2017

peci1 commented Dec 5, 2017

v4hn commented Dec 5, 2017

peci1 commented Dec 29, 2017

peci1 commented Dec 29, 2017

peci1 commented Jan 3, 2018

rhaschke commented Jan 3, 2018

peci1 commented Jan 3, 2018 •

edited

Loading

peci1 commented Jan 3, 2018

rhaschke commented Jan 3, 2018

Fixed bugs in computation of AABB in LinkModel and RobotState. #699

Fixed bugs in computation of AABB in LinkModel and RobotState. #699

Conversation

peci1 commented Nov 29, 2017