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

Bullet & ODE heightmaps #1069

Merged
merged 31 commits into from
Jul 22, 2018
Merged

Bullet & ODE heightmaps #1069

merged 31 commits into from
Jul 22, 2018

Conversation

JenniferBuehler
Copy link
Contributor

@JenniferBuehler JenniferBuehler commented May 11, 2018

This adds support for heightmaps with Bullet or ODE collision detectors.
I'm also planning to add FCL support by using the Octomap, but I think this should be a separate PR.

There are a couple of things yet to discuss and agree on before this could be merged.

1. Flipping Y values for bullet

I still don't find the solution for flipping the Y values proposed here very pretty (flipping y is required for bullet so that it has the same pose as the heightmap in ODE). I've thought about a couple of different ways how to to do this, but all of them have something undesirable about it. The main issue is that dynamics::HeightmapShape is built (possibly externally) before the bullet collision object (and with it the bullet btHeightfieldTerrainShape) is created from it. If we create and fill dynamics::HeightmapShape externally (e.g. from within the Gazebo DARTHeightmapShape), we would need to know at this point whether the Y values need to be flipped or not, so we need to know which collision detector is used. However dynamics::HeightmapShape cannot provide this information; we could instead query World for the used CollisionDetector, and if it's bullet, then flip the Y values. But this imposes too much required knowledge about the internals for the user, and it also requires a pointer to the dart World, so I don't consider this a good idea. It's best if the flipping of Y values happens automatically "behind the scenes". Which brings up the next issue: ideally we'd like to have the option to use an existing height value buffer and pass it into dynamics::HeightmapShape::setHeightField() (at the moment, a local copy of the vector is kept, which can be a waste of memory if the caller aims to keep their height field vector as well -that's the case for gazebo's HeightmapShape). But modifying an external caller's buffer data is not such a good idea. I'm not quite sure which solution would be more desirable, they are both not so ideal. So for now, I am just copying the height values vector, and flipping the y values in BulletCollisionDetector (see also comment in the code linked).

Any thoughts on this? Which solution would you consider more suitable, or you have other ideas?

2. Relative transforms

Bullet moves the origin of the heightmap to the center of its AABB (see also comment in btHeightfieldTerrainShape). This is different to ODE, so again we need to make the behavior uniform. What's not so lovely is that BulletCollisionDetector::createBulletCollisionShape() only returns a btCollisionShape, which has no information about a geometry's permanent relative transform to its parent object. So this relative transform has to be returned additionally from where it is created. For now I've solved this extra return value via an output parameter in the affected functions. This relative transform is then "dragged around" as parameter until it is remembered in BulletCollisionObject, where the relative transform can then always be applied when the shape is re-positioned.

What's not so nice about this: We only need this extra relative transform for the heightmap shape, all other shapes don't use it. Although maybe in future there will be other shapes for which relative transforms have to be applied to the geometry in order to make it uniform with how the same shape is handled in other collision detectors (?).

Similar issue for ODE, which uses Y as up axis instead of Z and therefore requires a rotation, but for ODE I found a solution which is less invasive (temporarily remembering the relative transform in the geometry itself, until a permanent geometry offset can be set after it is assigned to the body, which overwrites the geometry transform).

Is the proposed solution OK, or is it totally not desirable to keep an extra btTransform in BulletCollisionObject?

3. Target branch

I'm not sure which branch I should submit this PR to, I've just used release-6.5 for now. It would be good to have this available for Gazebo ASAP, see also implementation in the dart_heightmap_with_bullet branch for Gazebo.

@JenniferBuehler
Copy link
Contributor Author

I'll have to check about the test failure, not sure what it could be, it's passing on my system.

@jslee02 jslee02 added this to the DART 6.6.0 milestone May 12, 2018
@JenniferBuehler
Copy link
Contributor Author

Never mind about my last comment, I only was testing my added test which passes, but failures are due to issue #717 ;)

@JenniferBuehler
Copy link
Contributor Author

See also gazebo pull request 2956.

I have used gazebo to visually check the results, it all looks fine both with ODE and bullet.

It would be nice to have a visualization of the Heightmap shapes in DART as well, however we'd have to decide how to do this, e.g. reading from image file and which libraries to use etc. I'm happy to add this once I know your opinion on what would be the most desirable way to go about this.

@jslee02 jslee02 changed the base branch from release-6.5 to release-6.6 May 12, 2018 10:00
@jslee02
Copy link
Member

jslee02 commented May 12, 2018

Thank you for the PR! I'll take a look at this PR this weekend.

Regarding target branch, I switched it to release-6.6 because I'm planning to release 6.5.0 today. Once this PR is merged, I'll release 6.6 with this change so that this can be used in Gazebo ASAP.

@JenniferBuehler
Copy link
Contributor Author

Hi JS, thanks! Have a lovely weekend :)

@JenniferBuehler JenniferBuehler changed the title WIP: Bullet & ODE heightmaps Bullet & ODE heightmaps Jun 9, 2018
Copy link
Member

@jslee02 jslee02 left a comment

Choose a reason for hiding this comment

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

Sorry for the late response. I was occupied by other work. 😓

  1. Flipping Y values for bullet

It seems flipping Y values is unavoidable when the underlying collision detectors have different conventions. We may need to pick one convention that is more widely used. Do you know which convention is more widely used?

I suggest using Eigen::Matrix<HeightType, Eigen::Dynamic, Eigen::Dynamic> instead of std::vector<HeightType> because we can take advantage of Eigen's vectorized operations for HeightmapShape::flipY(). btHeightfieldTerrainShape also can be constructed with Eigen::Matrix by passing the pointer to the data (i.e., matrix.data()).

We may don't want in-place flipping for HeightmapShape when creating bullet collision shape. We could get an unexpected result if we create two collision shapes (calling the function twice). I guess we will usually create one bullet collision shape from one HeightmapShape, but there is nothing prevent us creating more than two. Instead, I suggest to have a function that returns flipped data something like Heightmap::getYFlippedHeightField()

  1. Relative transforms

Is it possible to shift the heightmap data before creating btHeightfieldTerrainShape so that we don't need to maintain relative transform?

return;
}

dGeomHeightfieldDataBuildSingle(
Copy link
Member

Choose a reason for hiding this comment

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

I believe this is meant to be dGeomHeightfieldDataBuildDouble.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Yes, indeed. I must have updated this function with copy and paste from the single precision version... oupsies! Thanks for finding this.


dGeomHeightfieldDataBuildSingle(
odeHeightfieldID,
&(heights[0]),
Copy link
Member

Choose a reason for hiding this comment

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

Nit: heights.data() would be more C++ style.

}
dGeomHeightfieldDataBuildSingle(
odeHeightfieldID,
&(heights[0]),
Copy link
Member

Choose a reason for hiding this comment

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

Nit: heights.data() would be more C++ style.

shape->flipY();
EXPECT_EQ(shape->getHeightField()[0], heights2[4]);
EXPECT_EQ(shape->getHeightField()[1], heights2[5]);
EXPECT_EQ(shape->getHeightField()[2], heights2[3]);
Copy link
Member

Choose a reason for hiding this comment

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

EXPECT_EQ(shape->getHeightField()[2], heights2[2]);?

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Yes :o

{
it1 = std::swap_ranges(it2, it2+mWidth, it1);
it2 -= mWidth;
}
Copy link
Member

Choose a reason for hiding this comment

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

This whole function can be simplified as mHeights = mHeights.rowwise().reverse().eval() once we change the data type to Eigen::Matrix.

size_t mWidth, mDepth;

/// \brief height field. Must be of size mWidth*mDepth.
mutable std::vector<HeightType> mHeights;
Copy link
Member

Choose a reason for hiding this comment

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

I suggest using Eigen::Matrix<HeightType, Eigen::Dynamic, Eigen::Dynamic> instead of std::vector<HeightType> because we can take advantage of Eigen's vectorized operations for HeightmapShape::flipY().

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Yes that makes sense, I haven't thought about using Eigen types actually. Will add this.

@JenniferBuehler
Copy link
Contributor Author

Hi JS,
thanks for looking at this! Now it's me who doesn't get around to look at this, I'm currently at a fair and will then be on holiday, so it will take a couple of weeks until I get around to it.
Cheers!

@JenniferBuehler
Copy link
Contributor Author

JenniferBuehler commented Jul 14, 2018

Thanks for looking at this, and sorry for the late response.

I've now changed it to Eigen::Matrix in the last commit.

Regarding your comments:

  1. Flipping Y values: I'm not sure which convention is more widely used either. In Gazebo, the y values are also flipped for bullet only, that's why I also did it here for bullet instead of for ODE. Not sure if there is a convention at all... also whether z or y is used as up axis varies. +z axis up is the most widely used standard, and so one could also argue that the values should be increasing row-wise (rows along +x) in +y direction, so everything goes towards +. However one could also argue that it should be -y instead (like it is interpreted now), because in image processing the rows are read top-down. This is how ODE understands it. Makes sense too, because the height fields are read from images to begin with. I don't mind either way, which do you prefer? I can change it.
    As for your idea with the GetYFlippedHeightField() function: also, good point. Side effect is that if we add a GetYFlippedHeightField() (and hence never modify the buffer in dart::HeightmapShape), then we also have to store yet another copy of the height field with the flipped values for bullet, because btHeightfieldTerrainShape takes a pointer to the data buffer (I'm pretty sure it doesn't copy it again but admittedly haven't checked the source, the documentation doesn't mention it). So in the example of using this in Gazebo, there will then be 1) the height field data in physics::HeightmapShape, and 2) there is the copied data in dart::HeightmapShape, and then we would 3) additionally need the flipped Y values for bullet stored locally somewhere. That's not exactly awesome ;) I suggest instead to add a flag to dart::HeightMapshape indicating whether the y values are flipped or not, and then the user just has to un-flip the values if they want the original data. In BulletCollisionDetector where flipY() is called, we'd then only flip if the data is not already flipped. What do you think about this solution?

  2. Relative transforms: Yes that is possible. I've made the initial implementation as removed from modified buffers as possible, but now that we pretty much decided to always keep that copy in dart::HeightmapShape anyway, we can do that.
    However similar as in point (1) we will have to keep track of locally applied transforms to the data as well. But I understand if you'd like to keep this PR as light as possible. So we can either keep it as-is, including transforms, or I can do the transform of the modified heightmap for bullet. Let me know what you prefer.

Thanks again for looking into this!

@JenniferBuehler
Copy link
Contributor Author

JenniferBuehler commented Jul 14, 2018

Just checked the CI and saw that there is a new failure with the changes for the y-values flipping in the test testHeightmapBox, hang in there while I fix this.

@JenniferBuehler
Copy link
Contributor Author

JenniferBuehler commented Jul 14, 2018

Found the issue, actually it's a bit annoying. Eigen doesn't store the data in the same order as required for the height values buffer (i.e. returned from data() in std::vector). So in the end we can't do heights.data() to pass the data into btHeightfieldTerrainShape, but have to transform the data to the right order first (see implementation in HeightmapShape::getHeightFieldAsVector()). This means that we'll also have to store the std::vector with the values in the right order separately.
Are you sure you would much prefer the Eigen::Matrix version just for the simplified flipY()?

@jslee02
Copy link
Member

jslee02 commented Jul 14, 2018

Thanks for the updates!

Eigen doesn't store the data in the same order as required for the height values buffer (i.e. returned from data() in std::vector).

Quick answer: Eigen supports two kinds of storage orders: column major and row major. So Eigen::Matrix<HeightType, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> would be helpful. I'm reading the code why getHeightFieldAsVector() is necessary.

Are you sure you would much prefer the Eigen::Matrix version just for the simplified flipY()?

There is another reason I would prefer Eigen::Matrix version: Eigen supports vectorized operations so we can speed up performance for free without changing the code!

@JenniferBuehler
Copy link
Contributor Author

JenniferBuehler commented Jul 15, 2018

Still getting the following failures:

[  FAILED  ] COLLISION.SimpleFrames
[  FAILED  ] COLLISION.BoxBox
[  FAILED  ] COLLISION.Filter
[  FAILED  ] COLLISION.CreateCollisionGroupFromVariousObject

I think this is related to issue #717? Though I'm unsure about the Filter test, maybe this is due to the new name issue conflicts? Not sure why this happens here, checked the diff to glut_macos which passes, and didn't see anything obvious...

I'm also unsure why Codacity uses an old version of test_Collision.cpp (I removed the const-ness from the variable, which is a variable one can play around with to test. So the values shouldn't change during runtime, but Codacity doesn't like it being used in the conditional expression).

I haven't got much experience with the automated check tools, sorry for the silly questions (and the post edits) ;)

@jslee02
Copy link
Member

jslee02 commented Jul 15, 2018

Those failures are due to that btTransform isn't set by calling this function. This is the case that a btCollisionShape instance was created from the same DART ShapeFrame; the function just returns the same btCollisionShape without setting relativeTransform under the current implementation.

I fixed this in this branch (forking it out of your branch). Let me create a PR of it targeting to your branch by hopefully today.

@jslee02
Copy link
Member

jslee02 commented Jul 16, 2018

@JenniferBuehler I've create a PR (https://github.com/JenniferBuehler/dart/pull/1) that includes some fixes and updates targeting to be merged into this PR. Please take a look at it when you have a chance.

@JenniferBuehler
Copy link
Contributor Author

Thanks for the PR.
Now all tests are passing, except the heightmapBox test is still failing for double, I'll fix this up later because I gotta run now.

@JenniferBuehler
Copy link
Contributor Author

JenniferBuehler commented Jul 17, 2018

Ok, so double is actually not supported in bullet, which only supports float, unsigned char and short. This is not clearly explained in the documentation, but you can see it in the bullet source here.

I have removed the support of double height fields for bullet, and also got rid of the test for it. It would be nice to automatically convert a double field to float, but then again we have to maintain duplicate storage of the height field (the question would also be where to store this)... so I think altogether it's better to just not support this, or what do you think?

@codecov
Copy link

codecov bot commented Jul 17, 2018

Codecov Report

Merging #1069 into release-6.6 will increase coverage by 0.06%.
The diff coverage is 64.51%.

@@               Coverage Diff               @@
##           release-6.6    #1069      +/-   ##
===============================================
+ Coverage        56.52%   56.59%   +0.06%     
===============================================
  Files              316      320       +4     
  Lines            24398    24524     +126     
===============================================
+ Hits             13791    13879      +88     
- Misses           10607    10645      +38
Impacted Files Coverage Δ
...art/dynamics/detail/TranslationalJoint2DAspect.hpp 60% <ø> (ø) ⬆️
dart/collision/bullet/BulletCollisionObject.hpp 100% <ø> (ø) ⬆️
dart/common/detail/LockableReference-impl.hpp 35.29% <0%> (-1.07%) ⬇️
dart/collision/bullet/BulletCollisionShape.hpp 100% <100%> (ø)
dart/collision/bullet/BulletCollisionShape.cpp 100% <100%> (ø)
dart/dynamics/HeightmapShape.hpp 100% <100%> (ø)
dart/collision/bullet/BulletCollisionDetector.cpp 64.45% <61.19%> (+2.25%) ⬆️
dart/dynamics/detail/HeightmapShape-impl.hpp 62.85% <62.85%> (ø)
dart/collision/bullet/BulletCollisionObject.cpp 80% <88.88%> (ø) ⬆️
dart/math/Geometry.hpp 68.96% <0%> (-2.47%) ⬇️
... and 4 more

@jslee02
Copy link
Member

jslee02 commented Jul 18, 2018

It would be nice to automatically convert a double field to float, but then again we have to maintain duplicate storage of the height field (the question would also be where to store this)... so I think altogether it's better to just not support this, or what do you think?

I'm happy with the current implement. The automatic conversion can be revisited later if it's necessary.

Also, the current implementation about relative transform looks good. My last concern is the flipping Y. The current implementation can be a problem when we use the height map shape in different collision detectors (e.g., ODE and Bullet) when the collision detector have different convention on how to store the height data.

Here is my proposal to address the issue:

The idea is supporting the two convention explicitly by adding a few more public API to HeightmapShape such as getHeightField1()/getHeightField2() and setHeightField1()/setHeightField2() (names are TBD). This means basically the shape should maintain two data for the two conventions. The motivation of doing this is to get the right data for queries with different conventions. Of course this can be inefficient for example when the height data is set by setHeightField1() and the data is extracted by getHeightField2(), which requires data conversion from convention 1 to convention 2 internally. But I think it's the users responsibility to be consistent in setting and getting the height data.

We can additionally provide cleaning functions that deallocates the data of unused convention. If the user knows the data of a particular convention won't be used, then the data can be deleted by calling the function. Note that the cleaning functions shouldn't remove the data if the data is the only one (we need to hold at least one data of a convention so that it can be converted into other convention).

If this sounds good then I'll add this functionality. Let me know what you think.

Caveat: Please consider that I wrote this in motion so the explanation can be unclear and can contain many errors.

@JenniferBuehler
Copy link
Contributor Author

Hi JS,

I see what you mean with the HeightmapShape flipped Y values. Yes if the data is used in different contexts or even different collision engines, then the buffer in HeightmapShape should not be manipluated. Good point!

I agree with your idea to maintain duplicate data only if two conventions are used at the same time. So if I understood this correctly, the flipped-y version of the height field will only be generated once getHeightfield2() has been called?

I'm not quite sure how you envision the automatic cleanup function, but it sounds good to have one :) So do you mean if the user subsequently to calling getHeightfield2() calls cleanUnsuedData(), and nobody has called getHeightfield1() in the meantime either, then heightfield1 will be deleted, instead of heightfield2? Then we'd have to keep track of calls to the functions, not sure if that's very pretty...

In terms of names, I would maybe suggest to add a parameter conventionID or so, i.e. getHeightfield(uint8 conventionID)... this could also be an enum (e.g. HEIGHTFIELD_REGULAR, HEIGHTFIELD_Y_FILPPED, etc). This means that if in future any other conventions are added, it could easily be integrated, instead of then having a getHeightfield3() or so. The height fields themselves can then be stored in an array indexed by the convention IDs.
But that's only one idea, I'm happy with two names too.

If you're happy to add this to the PR, it would be awesome, then you can do the solution you would like to have right away :)

@jslee02
Copy link
Member

jslee02 commented Jul 20, 2018

the flipped-y version of the height field will only be generated once getHeightfield2() has been called?

Yes.

So do you mean if the user subsequently to calling getHeightfield2() calls cleanUnsuedData(), and nobody has called getHeightfield1() in the meantime either, then heightfield1 will be deleted, instead of heightfield2? Then we'd have to keep track of calls to the functions, not sure if that's very pretty...

I agree. That is one possible implementation but I'm trying to find a better way. My initial thought was that the cleanup functionality isn't automatic, but the cleanup function should be called by the caller. Then the automation is done by collision detector's claim/reclaim mechanism. However, this isn't also not that clean I think. Let me think about this further.

In the meantime, I think this PR is good to go. @mxgrey Please let me know if you'd like to review this PR further.

Copy link
Member

@jslee02 jslee02 left a comment

Choose a reason for hiding this comment

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

@JenniferBuehler This is a great set of changes for DART (and hopefully for Gazebo as well)! We can revisit the issues raised here later, but I think they shouldn't block merging this PR.

Thank you for your contribution!

@jslee02 jslee02 merged commit 52120ed into dartsim:release-6.6 Jul 22, 2018
@JenniferBuehler
Copy link
Contributor Author

Awesome, thanks for merging this :) I agree that the issue with the Y-values should be re-visited later, so that HeightmapShape can be re-used. For now I'm happy that the PR has been merged so that the Gazebo PR can move forward too. Thanks!

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants