[FEATURE] Add get_height_at() and get_normal_at() for terrain entities

[vlordier]

Summary

• Adds get_height_at(x, y) method to query interpolated terrain height at world position
• Adds get_normal_at(x, y) method to compute surface normal at world position
• Both methods handle boundary conditions (return 0 for height outside terrain, [0,0,1] for normal)
• Use bilinear interpolation for height from height field
• Compute normal from height field gradient using central differences

Changes

• genesis/engine/entities/rigid_entity/rigid_entity.py: Added two methods to RigidEntity class:
  • get_height_at(x, y) - Returns interpolated height using bilinear sampling from terrain height field
  • get_normal_at(x, y) - Returns unit normal vector computed from height field gradient
• Both methods use @gs.assert_built decorator and include detailed docstrings

Usage

# Create terrain entity
terrain = scene.add_entity(
    gs.morphs.Terrain(
        height_field=height_field_array,
        horizontal_scale=0.25,
        vertical_scale=0.1
    )
)
scene.build()

# Query terrain properties at world position
height = terrain.get_height_at(3.0, 4.5)  # Returns interpolated height
normal = terrain.get_normal_at(3.0, 4.5)  # Returns unit normal vector [nx, ny, nz]

# Useful for:
# - Locomotion rewards (feet fitting terrain surface)
# - Terrain-following control
# - Contact detection and response tuning

Backward Compatible

• No changes to existing APIs
• Methods only available on entities with terrain morphology

Closes #2094

[claude]

Claude Code Review

This pull request is from a fork — automated review is disabled. A repository maintainer can comment @claude review to run a one-time review.

[chatgpt-codex-connector]

💡 Codex Review

Here are some automated review suggestions for this pull request.

Reviewed commit: 9d4eaef3cf

ℹ️ About Codex in GitHub

Your team has set up Codex to review pull requests in this repo. Reviews are triggered when you

• Open a pull request for review
• Mark a draft as ready
• Comment "@codex review".

If Codex has suggestions, it will comment; otherwise it will react with 👍.

Codex can also answer questions or update the PR. Try commenting "@codex address that feedback".

[Copilot]

Pull request overview

This PR adds world-space query helpers on terrain entities to retrieve the terrain height and surface normal at a given (x, y) position, and also introduces a history_length option for the contact force sensor’s output buffering.

Changes:

• Add RigidEntity.get_height_at(x, y) and RigidEntity.get_normal_at(x, y) for terrain-backed entities.
• Add history_length to ContactForce sensor options and adjust sensor buffering to support longer histories.
• Add a small terrain query script (test_terrain.py) demonstrating usage.

Reviewed changes

Copilot reviewed 4 out of 4 changed files in this pull request and generated 9 comments.

Show a summary per file

File	Description
genesis/engine/entities/rigid_entity/rigid_entity.py	Adds terrain height/normal query APIs on RigidEntity.
genesis/engine/sensors/contact_force.py	Adds history-aware reads and metadata for contact force sensors.
genesis/engine/sensors/sensor_manager.py	Expands ring buffer length calculation to account for history_length.
genesis/options/sensors/options.py	Adds history_length option to ContactForce sensor configuration.
test_terrain.py	Adds a standalone script demonstrating terrain queries.

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[duburcqa]

@claude review

[claude]

🔴 In get_height_at(), when x0 or y0 is in-bounds but x1 or y1 is out-of-bounds (i.e., the query falls in the strip between the last grid node and the terrain boundary), the method returns the non-zero edge height hf[x0, y0]*v_scale + terrain_pos[2] instead of terrain_pos[2]. This contradicts the PR's stated behavior of 'return 0 for height outside terrain'. Remove the inner if 0 <= x0 < hf.shape[0] and 0 <= y0 < hf.shape[1] guard so any position where bilinear interpolation is impossible returns only terrain_pos[2].

Extended reasoning...

Bug: Edge-strip queries return non-zero height instead of 0

The boundary handling block in get_height_at() (rigid_entity.py ~line 4194) reads:

if x0 < 0 or y0 < 0 or x1 >= hf.shape[0] or y1 >= hf.shape[1]:
    if 0 <= x0 < hf.shape[0] and 0 <= y0 < hf.shape[1]:
        return hf[x0, y0] * v_scale + terrain_pos[2]
    return terrain_pos[2]

The outer if fires whenever any grid index is out of range — including the common case where x0 is valid but x1 = x0 + 1 is one past the last row. The inner if then tests only whether (x0, y0) are in-bounds (which they are in this case) and returns the edge height. The final return terrain_pos[2] (the correct out-of-bounds value) is only reached when x0 or y0 are themselves out of range.

Concrete proof

Terrain with shape (100, 100), h_scale=0.25:

• Last valid grid index: 99 → last valid world coordinate: 99 × 0.25 = 24.75 m
• Query at x = 24.8 m:
  • x_idx = 24.8 / 0.25 = 99.2
  • x0 = 99, x1 = 100
  • Outer condition: x1 >= hf.shape[0] → True (enters outer if)
  • Inner condition: 0 <= x0 < 100 → True (enters inner if)
  • Returns: hf[99, y0] * v_scale + terrain_pos[2] ← non-zero edge value
  • Expected per documentation: terrain_pos[2] (height = 0 above terrain base)

Why existing code doesn't prevent it

The PR description explicitly states "Both methods handle boundary conditions (return 0 for height outside terrain)". The docstring is consistent. However, the implementation has a three-tier inconsistency:

1. (x0, y0) and (x1, y1) both in-bounds → bilinear interpolation (correct)
2. (x0, y0) in-bounds but (x1, y1) out-of-bounds → returns edge height (wrong)
3. (x0, y0) out-of-bounds → returns 0 (correct)

Tier 2 covers the strip x in [(N-1)*h_scale, N*h_scale), which is physically outside the last defined grid cell. A user relying on the documented 0-return contract for out-of-terrain queries will silently get wrong values in this narrow but real strip.

Fix

Remove the inner guard entirely:

if x0 < 0 or y0 < 0 or x1 >= hf.shape[0] or y1 >= hf.shape[1]:
    return terrain_pos[2]

This ensures every position where bilinear interpolation cannot be performed returns the documented out-of-bounds value.

_{🔬 also observed by copilot-pull-request-reviewer}

[duburcqa]

This PR contains tons of unrelated changes.