-
Notifications
You must be signed in to change notification settings - Fork 287
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
Bullet & ODE heightmaps #1069
Conversation
I'll have to check about the test failure, not sure what it could be, it's passing on my system. |
Never mind about my last comment, I only was testing my added test which passes, but failures are due to issue #717 ;) |
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. |
Thank you for the PR! I'll take a look at this PR this weekend. Regarding target branch, I switched it to |
Hi JS, thanks! Have a lovely weekend :) |
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.
Sorry for the late response. I was occupied by other work. 😓
- 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()
- 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( |
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 believe this is meant to be dGeomHeightfieldDataBuildDouble
.
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.
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]), |
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.
Nit: heights.data()
would be more C++ style.
} | ||
dGeomHeightfieldDataBuildSingle( | ||
odeHeightfieldID, | ||
&(heights[0]), |
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.
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]); |
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.
EXPECT_EQ(shape->getHeightField()[2], heights2[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.
Yes :o
dart/dynamics/HeightmapShape.cpp
Outdated
{ | ||
it1 = std::swap_ranges(it2, it2+mWidth, it1); | ||
it2 -= mWidth; | ||
} |
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.
This whole function can be simplified as mHeights = mHeights.rowwise().reverse().eval()
once we change the data type to Eigen::Matrix
.
dart/dynamics/HeightmapShape.hpp
Outdated
size_t mWidth, mDepth; | ||
|
||
/// \brief height field. Must be of size mWidth*mDepth. | ||
mutable std::vector<HeightType> mHeights; |
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 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()
.
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.
Yes that makes sense, I haven't thought about using Eigen types actually. Will add this.
Hi JS, |
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:
Thanks again for looking into this! |
Just checked the CI and saw that there is a new failure with the changes for the y-values flipping in the test |
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 |
Thanks for the updates!
Quick answer: Eigen supports two kinds of storage orders: column major and row major. So
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! |
Still getting the following failures:
I think this is related to issue #717? Though I'm unsure about the I'm also unsure why Codacity uses an old version of I haven't got much experience with the automated check tools, sorry for the silly questions (and the post edits) ;) |
Those failures are due to that 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. |
…bullet_heightmap_js # Conflicts: # dart/dynamics/HeightmapShape.hpp
@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. |
Thanks for the PR. |
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 Report
@@ 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
|
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 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. |
Hi JS, I see what you mean with the 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 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 In terms of names, I would maybe suggest to add a parameter 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 :) |
Yes.
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. |
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.
@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!
Awesome, thanks for merging this :) I agree that the issue with the Y-values should be re-visited later, so that |
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 bulletbtHeightfieldTerrainShape
) is created from it. If we create and filldynamics::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. Howeverdynamics::HeightmapShape
cannot provide this information; we could instead queryWorld
for the usedCollisionDetector
, 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 dartWorld
, 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
inBulletCollisionObject
?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.