Fixed bugs in computation of AABB in LinkModel and RobotState (kinetic) #703
Conversation
Also added a way to visualize both AABBs and OBBs in RViz to verify.
| aabb.resize(6, 0.0); | ||
| if (!bounding_box.isEmpty()) | ||
| { | ||
| aabb[0] = bounding_box.min()[0]; |
There was a problem hiding this comment.
This assignment is very verbose. What about:
Map<VectorXd, Unaligned, InnerStride<2>>(aabb.data(), 3) = bounding_box.min();
Map<VectorXd, Unaligned, InnerStride<2>>(aabb.data()+1, 3) = bounding_box.max();| Eigen::Vector3d ei = shapes::computeShapeExtents(shapes_[i].get()); | ||
| Eigen::Vector3d p1 = collision_origin_transform_[i] * (-ei / 2.0); | ||
| Eigen::Vector3d p2 = collision_origin_transform_[i] * (-p1); | ||
| Eigen::Vector3d extents = shapes::computeShapeExtents(shapes_[i].get()); |
There was a problem hiding this comment.
extends seems to be used for non-meshes only. Hence computation of shape extends should be moved into the non-meshes if-branch below.
| a[i] = std::min(a[i], p1[i]); | ||
| for (int i = 0; i < 3; ++i) | ||
| b[i] = std::max(b[i], p2[i]); | ||
| const shapes::Mesh* mesh = dynamic_cast<const shapes::Mesh*>(shapes_[i].get()); |
There was a problem hiding this comment.
Can you add a comment, why meshes are handled differently?
I guess, with your approach, you compute the smallest AABB for the mesh given the transformation, while otherwise an AABB of the mesh (in default pose) is computed and transformed afterwards, which usually gives a larger box.
There was a problem hiding this comment.
You're right it deserves a comment, since the reasoning is different - shapes::computeShapeExtents only gives the extents and doesn't provide info about the position of the center of the mesh in these extents.
|
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. |
Fixed bugs in computation of AABB in LinkModel and RobotState.
Fixed bugs in computation of AABB in LinkModel and RobotState.
|
Has this fix been pushed to the Kinetic Debian release? I'm on 0.9.11-0xenial-20180319-133135-0800, but am still seeing this bug. |
|
No, there wasn't as new release since this PR was merged. |
|
Thanks for also releasing for indigo. However, I've noticed that this PR (and its indigo counterpart) isn't referenced in the changelog (to kinetic, it was added in 0.9.12; to indigo in 0.7.14). Should I make PRs against kinetic-devel and indigo-devel adding the mention to the appropriate places, or is it a no-go to edit changelogs of already released versions? I think mentioning this in the changelog is important because people might find things broken (or fixed :) ) and start searching for the cause. |
|
@130s @rhaschke as they do the releases at the moment. Personally I believe it is not important to add the notes here anymore. |
|
Fully agree. I don't see the need to add this to an already released changelog as well. |
As requested in moveit#703
As requested in moveit#703
Port of #699 to kinetic-devel.