Feature/71 height fields #216
Conversation
|
Thanks for your pull request! It looks like this may be your first contribution to a Google open source project. Before we can look at your pull request, you'll need to sign a Contributor License Agreement (CLA). View this failed invocation of the CLA check for more information. For the most up to date status, view the checks section at the bottom of the pull request. |
|
Do you have fps performance benchmarks for these hfields? I'm interested in fps for a simple robot against a plane and a simple robot against an hfield. Thanks! |
mujoco_warp/_src/types.py
Outdated
| @@ -350,6 +351,10 @@ class WrapType(enum.IntEnum): | |||
| # unsupported: PULLEY, SPHERE, CYLINDER | |||
|
|
|||
|
|
|||
| class vec4f(wp.types.vector(length=4, dtype=wp.float32)): | |||
There was a problem hiding this comment.
vec4f is natively supported, not sure that this is needed.
Using wp.vec4f directly should be possible.
| @@ -786,6 +797,7 @@ class Model: | |||
| nsensordata: int | |||
| nlsp: int # warp only | |||
| npair: int | |||
| nhfield: int | |||
There was a problem hiding this comment.
You don't have nhfielddata?
There was a problem hiding this comment.
What do you mean?
There was a problem hiding this comment.
In Line 670 you mention that the size of hfield_data is (nhfielddata,).
You should probably add the nhfielddata property here, even if it is not used.
| <geom type="box" size="0.1 0.1 0.1" rgba="0.8 0.2 0.2 1"/> | ||
| </body> | ||
|
|
||
| <!-- Boxes -> |
There was a problem hiding this comment.
Is this file needs some cleanup?
Or it is just temporary for your testing?
BTW, is it used in the test?
There was a problem hiding this comment.
I used it for development. I see other xml files also have some stuff commented out. What's wrong with mine? No, it's not used in UTs
| @@ -86,8 +86,9 @@ def test_geom_type(self): | |||
|
|
|||
| # TODO(team): sdf | |||
|
|
|||
| with self.assertRaises(NotImplementedError): | |||
| mjwarp.put_model(mjm) | |||
| # seems to fail coz all above's implemented now | |||
There was a problem hiding this comment.
Then you can probably delete the test.
There was a problem hiding this comment.
I can. Are you sure no one will miss it?
mujoco_warp/_src/collision_hfield.py
Outdated
|
|
||
| Args: | ||
| m: Model containing geometry data | ||
| hfield_geom: Index of the height field geometry |
There was a problem hiding this comment.
hfield_geom doesn't seem to be the correct name.
Maybe something like hfieldid?
There was a problem hiding this comment.
Is there some naming convention I can read?
There was a problem hiding this comment.
Not to my knowledge.
Following the naming for similar properties is your best bet.
| # Convert grid coordinates to local space x, y coordinates | ||
| x0 = wp.float32(i0) * x_scale - size[0] | ||
| y0 = wp.float32(j0) * y_scale - size[1] | ||
| x1 = wp.float32(i1) * x_scale - size[0] |
There was a problem hiding this comment.
Very minor:
I guess this should be more optimized
x1 = x0 + x_scale
y1 = y0 + y_scale
There was a problem hiding this comment.
I understand the logic, but isn't it more confusing that it is now?
There was a problem hiding this comment.
Both are equivalent to me. But if you think the original version is better, it's fine for me.
mujoco_warp/_src/collision_hfield.py
Outdated
| z11 = m.hfield_data[base_addr + j1 * ncol + i1] | ||
|
|
||
| # Scale heights to range [0, 1] and then to [z_bottom, z_top] | ||
| z_range = size[2] |
There was a problem hiding this comment.
Not sure to understand this.
In hfield_data we have the raw data right?
So when do we scale to [0, 1]?
| z11 = z11 * z_range | ||
|
|
||
| # Set bottom z-value | ||
| z_bottom = -size[3] |
There was a problem hiding this comment.
mujoco_warp/_src/collision_hfield.py
Outdated
| other_margin = m.geom_margin[other_geom] | ||
| bound_radius = other_rbound + other_margin | ||
|
|
||
| # Calculate grid coordinates that bound the other geometry |
There was a problem hiding this comment.
Is this comment correct?
There was a problem hiding this comment.
What's wrong with it?
There was a problem hiding this comment.
It seems to me that what you calculate is the resolution of your hfield.
| d.collision_pairid[pairid] = m.nxn_pairid[nxnid] | ||
| d.collision_worldid[pairid] = worldid | ||
| # For height field, add a collision pair for every | ||
| # triangle that can potentially collide |
There was a problem hiding this comment.
We will probably need to check performance here.
Adding a collision pair for every vertex may lead to large number of collision pairs, which would slow down the code and requires large amount of memory.
There was a problem hiding this comment.
For each triangle, not each vertex. I create 2 pairs/triangles for each HF cell. If you don't have a HF with absurdly small cells, a body of an average size will collide with up to 4 cells - 8 triangles. Each pair is just 2 integers (plus I added an integer for tri index), and they are already pre-allocated, it's not like I allocate them when I add a pair. For the performance, for GPU it doesn't matter if it's 8 or 800 - they are all computed simultaneously. And, after all, what are the alternatives? Would like to hear them )
|
There are some unexpected contact points that are generated. |
This pull request introduces support for heightfield geometries in the collision detection system of the
mujoco_warplibrary. The main changes include adding heightfield-specific data structures, functions, and logic to handle heightfield collisions, as well as extending the existing collision detection pipeline to integrate this new geometry type.Heightfield Geometry Support
HFIELDas a new geometry type in theGeomTypeenum and updated theModelandDataclasses to include heightfield-specific attributes such ashfield_size,hfield_data, andhfield_nrow/ncol. (mujoco_warp/_src/types.py, [1] [2] [3] [4]HFPrismclass and functionsget_hfield_overlap_rangeandget_hfield_triangle_prismto calculate overlap regions and triangular prisms for heightfield collision detection. (mujoco_warp/_src/collision_hfield.py, mujoco_warp/_src/collision_hfield.pyR1-R159)Collision Pipeline Enhancements
_gjk_support_geom: Updated_gjk_support_geomto compute support points for heightfield geometries by iterating over the vertices of the triangular prism. (mujoco_warp/_src/collision_convex.py, mujoco_warp/_src/collision_convex.pyR108-R116)_add_geom_pairto add collision pairs for all potentially colliding heightfield triangles. (mujoco_warp/_src/collision_driver.py, [1] [2] [3]Data and Model Updates
put_modelto populate heightfield-related attributes from the Mujoco model, and adjustedmake_datato initialize heightfield-specific data arrays. (mujoco_warp/_src/io.py, [1] [2] [3]General Codebase Adjustments
_geom: Enhanced the_geomfunction to include heightfield triangle prism vertices when the geometry type isHFIELD. (mujoco_warp/_src/collision_primitive.py, [1] [2]mujoco_warp/_src/collision_driver.py, [1];mujoco_warp/_src/collision_primitive.py, [2]These changes collectively enable efficient and accurate collision detection for heightfield geometries, expanding the library's capabilities for handling complex terrain and surface interactions.