Skip to content
Open
Show file tree
Hide file tree
Changes from 2 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 5 additions & 2 deletions newton/examples/basic/example_basic_conveyor.py
Original file line number Diff line number Diff line change
Expand Up @@ -405,6 +405,7 @@ def render(self):

def test_final(self):
body_q = self.state_0.body_q.numpy()
body_qd = self.state_0.body_qd.numpy()
belt_z = float(body_q[self.belt_body][2])
assert abs(belt_z - BELT_CENTER_Z) < 0.15, f"Belt body drifted off the conveyor plane: z={belt_z:.4f}"

Expand All @@ -413,8 +414,10 @@ def test_final(self):
y = float(body_q[body_idx][1])
z = float(body_q[body_idx][2])
assert np.isfinite(x) and np.isfinite(y) and np.isfinite(z), f"Bag {body_idx} has non-finite pose values."
assert z > -0.5, f"Bag body {body_idx} fell through the floor: z={z:.4f}"
assert abs(x) < 4.0 and abs(y) < 4.0, f"Bag body {body_idx} left the scene bounds: ({x:.3f}, {y:.3f})"
assert z > 0.5, f"Bag body {body_idx} fell below conveyor surface: z={z:.4f}"
assert abs(x) < 2.5 and abs(y) < 2.5, f"Bag body {body_idx} left the scene bounds: ({x:.3f}, {y:.3f})"
vel = float(np.linalg.norm(body_qd[body_idx]))
assert vel < 8.0, f"Bag body {body_idx} moving too fast: vel={vel:.4f}"


if __name__ == "__main__":
Expand Down
5 changes: 4 additions & 1 deletion newton/examples/basic/example_basic_heightfield.py
Original file line number Diff line number Diff line change
Expand Up @@ -120,9 +120,12 @@ def render(self):
def test_final(self):
"""Verify all spheres are resting on the heightfield (not fallen through)."""
body_q = self.state_0.body_q.numpy()
body_qd = self.state_0.body_qd.numpy()
for body_idx in self.sphere_bodies:
z = float(body_q[body_idx, 2])
assert z > -1.0, f"Sphere body {body_idx} fell through heightfield: z={z:.4f}"
assert z > -0.2, f"Sphere body {body_idx} fell through heightfield: z={z:.4f}"
vel = float(np.linalg.norm(body_qd[body_idx]))
assert vel < 15.0, f"Sphere body {body_idx} moving too fast: vel={vel:.4f}"


if __name__ == "__main__":
Expand Down
2 changes: 1 addition & 1 deletion newton/examples/basic/example_basic_pendulum.py
Original file line number Diff line number Diff line change
Expand Up @@ -127,7 +127,7 @@ def check_velocities(_, qd):
# velocity outside the plane of the pendulum should be close to zero
check = abs(qd[0]) < 1e-4 and abs(qd[6]) < 1e-4
# velocity in the plane of the pendulum should be reasonable
check = check and abs(qd[1]) < 10.0 and abs(qd[2]) < 5.0 and abs(qd[3]) < 10.0 and abs(qd[4]) < 10.0
check = check and abs(qd[1]) < 8.0 and abs(qd[2]) < 5.0 and abs(qd[3]) < 8.0 and abs(qd[4]) < 8.0
return check

newton.examples.test_body_state(
Expand Down
9 changes: 8 additions & 1 deletion newton/examples/basic/example_basic_plotting.py
Original file line number Diff line number Diff line change
Expand Up @@ -137,7 +137,14 @@ def test_final(self):
self.model,
self.state_0,
"bodies above ground",
lambda q, qd: q[2] > -0.1,
lambda q, qd: q[2] > 0.1,
)
# Verify the humanoid is settled (low velocity)
newton.examples.test_body_state(
self.model,
self.state_0,
"bodies have low velocity",
lambda q, qd: wp.length(qd) < 0.5,
)

self._plot()
Expand Down
2 changes: 1 addition & 1 deletion newton/examples/basic/example_basic_shapes.py
Original file line number Diff line number Diff line change
Expand Up @@ -207,7 +207,7 @@ def test_final(self):
self.model,
self.state_0,
"box at rest pose",
lambda q, qd: newton.math.vec_allclose(q, box_q, atol=0.1),
lambda q, qd: newton.math.vec_allclose(q, box_q, atol=1e-2),
[4],
)
# we only test that the bunny didn't fall through the ground and didn't slide too far
Expand Down
2 changes: 1 addition & 1 deletion newton/examples/basic/example_basic_urdf.py
Original file line number Diff line number Diff line change
Expand Up @@ -144,7 +144,7 @@ def test_final(self):
self.model,
self.state_0,
"quadruped links are not moving too fast",
lambda q, qd: max(abs(qd)) < 0.15,
lambda q, qd: max(abs(qd)) < 0.05,
)

bodies_per_world = self.model.body_count // self.world_count
Expand Down
26 changes: 25 additions & 1 deletion newton/examples/cable/example_cable_bundle_hysteresis.py
Original file line number Diff line number Diff line change
Expand Up @@ -418,7 +418,31 @@ def render(self):

def test_final(self):
"""Test cable bundle hysteresis simulation for stability and correctness (called after simulation)."""
pass
if self.state_0.body_q is not None and self.state_0.body_qd is not None:
body_positions = self.state_0.body_q.numpy()
body_velocities = self.state_0.body_qd.numpy()

# Numerical stability
assert np.isfinite(body_positions).all(), "Non-finite values in body positions"
assert np.isfinite(body_velocities).all(), "Non-finite values in body velocities"

# Exclude kinematic obstacle bodies (they teleport during release phase)
obstacle_set = set(self.obstacle_bodies)
cable_indices = [i for i in range(body_positions.shape[0]) if i not in obstacle_set]
cable_positions = body_positions[cable_indices]
cable_velocities = body_velocities[cable_indices]

# Position bounds for cable bodies (observed z_range=[0.019, 0.300])
z_positions = cable_positions[:, 2]
min_z = np.min(z_positions)
max_z = np.max(z_positions)
assert min_z > -0.05, f"Cable penetrated ground: min_z = {min_z:.3f}"
assert max_z < 0.6, f"Cables too high: max_z = {max_z:.3f}"

# Velocity bounds for cable bodies (observed max_body_vel ~1.41)
assert (np.abs(cable_velocities) < 3.0).all(), (
f"Cable velocities too large: max = {np.max(np.abs(cable_velocities)):.3f}"
)
Comment on lines +421 to +445
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

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

⚠️ Potential issue | 🟠 Major

Fail fast when body state is missing instead of silently passing.

Right now, this test can pass without executing any assertions if either array is None. That creates a false-green path for regressions in state setup.

✅ Proposed fix
     def test_final(self):
         """Test cable bundle hysteresis simulation for stability and correctness (called after simulation)."""
-        if self.state_0.body_q is not None and self.state_0.body_qd is not None:
-            body_positions = self.state_0.body_q.numpy()
-            body_velocities = self.state_0.body_qd.numpy()
+        if self.state_0.body_q is None or self.state_0.body_qd is None:
+            raise RuntimeError("Body state is not available.")
+
+        body_positions = self.state_0.body_q.numpy()
+        body_velocities = self.state_0.body_qd.numpy()
 
-            # Numerical stability
-            assert np.isfinite(body_positions).all(), "Non-finite values in body positions"
-            assert np.isfinite(body_velocities).all(), "Non-finite values in body velocities"
+        # Numerical stability
+        assert np.isfinite(body_positions).all(), "Non-finite values in body positions"
+        assert np.isfinite(body_velocities).all(), "Non-finite values in body velocities"
 
-            # Exclude kinematic obstacle bodies (they teleport during release phase)
-            obstacle_set = set(self.obstacle_bodies)
-            cable_indices = [i for i in range(body_positions.shape[0]) if i not in obstacle_set]
-            cable_positions = body_positions[cable_indices]
-            cable_velocities = body_velocities[cable_indices]
+        # Exclude kinematic obstacle bodies (they teleport during release phase)
+        obstacle_set = set(self.obstacle_bodies)
+        cable_indices = [i for i in range(body_positions.shape[0]) if i not in obstacle_set]
+        cable_positions = body_positions[cable_indices]
+        cable_velocities = body_velocities[cable_indices]

As per coding guidelines: newton/examples/**/*.py: “Implement test_final() — runs after the example completes to verify simulation state is valid.”

📝 Committable suggestion

‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.

Suggested change
if self.state_0.body_q is not None and self.state_0.body_qd is not None:
body_positions = self.state_0.body_q.numpy()
body_velocities = self.state_0.body_qd.numpy()
# Numerical stability
assert np.isfinite(body_positions).all(), "Non-finite values in body positions"
assert np.isfinite(body_velocities).all(), "Non-finite values in body velocities"
# Exclude kinematic obstacle bodies (they teleport during release phase)
obstacle_set = set(self.obstacle_bodies)
cable_indices = [i for i in range(body_positions.shape[0]) if i not in obstacle_set]
cable_positions = body_positions[cable_indices]
cable_velocities = body_velocities[cable_indices]
# Position bounds for cable bodies (observed z_range=[0.019, 0.300])
z_positions = cable_positions[:, 2]
min_z = np.min(z_positions)
max_z = np.max(z_positions)
assert min_z > -0.05, f"Cable penetrated ground: min_z = {min_z:.3f}"
assert max_z < 0.6, f"Cables too high: max_z = {max_z:.3f}"
# Velocity bounds for cable bodies (observed max_body_vel ~1.41)
assert (np.abs(cable_velocities) < 3.0).all(), (
f"Cable velocities too large: max = {np.max(np.abs(cable_velocities)):.3f}"
)
if self.state_0.body_q is None or self.state_0.body_qd is None:
raise RuntimeError("Body state is not available.")
body_positions = self.state_0.body_q.numpy()
body_velocities = self.state_0.body_qd.numpy()
# Numerical stability
assert np.isfinite(body_positions).all(), "Non-finite values in body positions"
assert np.isfinite(body_velocities).all(), "Non-finite values in body velocities"
# Exclude kinematic obstacle bodies (they teleport during release phase)
obstacle_set = set(self.obstacle_bodies)
cable_indices = [i for i in range(body_positions.shape[0]) if i not in obstacle_set]
cable_positions = body_positions[cable_indices]
cable_velocities = body_velocities[cable_indices]
# Position bounds for cable bodies (observed z_range=[0.019, 0.300])
z_positions = cable_positions[:, 2]
min_z = np.min(z_positions)
max_z = np.max(z_positions)
assert min_z > -0.05, f"Cable penetrated ground: min_z = {min_z:.3f}"
assert max_z < 0.6, f"Cables too high: max_z = {max_z:.3f}"
# Velocity bounds for cable bodies (observed max_body_vel ~1.41)
assert (np.abs(cable_velocities) < 3.0).all(), (
f"Cable velocities too large: max = {np.max(np.abs(cable_velocities)):.3f}"
)
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.

In `@newton/examples/cable/example_cable_bundle_hysteresis.py` around lines 421 -
445, The test currently skips all checks when self.state_0.body_q or
self.state_0.body_qd is None; add an explicit fail-fast assertion at the start
of the validation (e.g., in test_final or the block containing the shown code)
that both self.state_0.body_q and self.state_0.body_qd are not None with a clear
message like "Missing final body state: body_q or body_qd is None", then proceed
with the existing numpy conversions and assertions; reference
self.state_0.body_q and self.state_0.body_qd to locate where to add this check.


@staticmethod
def create_parser():
Expand Down
5 changes: 3 additions & 2 deletions newton/examples/cable/example_cable_pile.py
Original file line number Diff line number Diff line change
Expand Up @@ -235,7 +235,7 @@ def test_final(self):
layers = 4

# Use same tolerance for ground penetration and pile height offset
tolerance = 0.1 # Soft contacts allow some penetration and gaps
tolerance = 0.05 # Observed min_z ~0.009; tighter than previous 0.1

# Calculate maximum expected height for SETTLED pile
# After gravity settles the pile, cables should be stacked:
Expand Down Expand Up @@ -267,7 +267,8 @@ def test_final(self):
)

# Test 3: Velocity should be reasonable (pile shouldn't explode)
assert (np.abs(body_velocities) < 5e2).all(), "Velocities too large"
# Observed max_body_vel ~0.94 at 20 frames
assert (np.abs(body_velocities) < 2.0).all(), "Velocities too large (>2.0)"


if __name__ == "__main__":
Expand Down
11 changes: 7 additions & 4 deletions newton/examples/cable/example_cable_twist.py
Original file line number Diff line number Diff line change
Expand Up @@ -288,8 +288,8 @@ def test_final(self):
# Test 1: Check for numerical stability (NaN/inf values and reasonable ranges)
assert np.isfinite(body_positions).all(), "Non-finite values in body positions"
assert np.isfinite(body_velocities).all(), "Non-finite values in body velocities"
assert (np.abs(body_positions) < 1e3).all(), "Body positions too large (>1000)"
assert (np.abs(body_velocities) < 5e2).all(), "Body velocities too large (>500)"
assert (np.abs(body_positions) < 1e2).all(), "Body positions too large (>100)"
assert (np.abs(body_velocities) < 2.0).all(), "Body velocities too large (>2.0)"

# Test 2: Check cable connectivity (joint constraints)
for cable_idx, cable_bodies in enumerate(self.cable_bodies_list):
Expand All @@ -310,9 +310,12 @@ def test_final(self):

# Test 3: Check ground interaction
# Cables should stay near ground (z~=0) since they start on the ground plane
ground_tolerance = 0.5 # Larger tolerance for zigzag cables with dynamic spinning
min_z = np.min(body_positions[:, 2]) # Z positions (Newton uses Z-up)
z_positions = body_positions[:, 2] # Z positions (Newton uses Z-up)
min_z = np.min(z_positions)
max_z = np.max(z_positions)
ground_tolerance = 0.05 # Observed min_z ~0.018
assert min_z > -ground_tolerance, f"Cable penetrated ground too much: min_z = {min_z:.3f}"
assert max_z < 0.15, f"Cable rose too high above ground: max_z = {max_z:.3f}"


if __name__ == "__main__":
Expand Down
17 changes: 14 additions & 3 deletions newton/examples/cable/example_cable_y_junction.py
Original file line number Diff line number Diff line change
Expand Up @@ -206,10 +206,21 @@ def test_final(self):
raise ValueError("NaN/Inf in cable body transforms.")

pos = body_q_np[:, 0:3]
if np.max(np.abs(pos[:, 0])) > 2.0 or np.max(np.abs(pos[:, 1])) > 2.0:
if np.max(np.abs(pos[:, 0])) > 1.0 or np.max(np.abs(pos[:, 1])) > 1.0:
raise ValueError("Cable bodies drifted too far in X/Y.")
if np.min(pos[:, 2]) < -0.2 or np.max(pos[:, 2]) > 3.0:
raise ValueError("Cable bodies out of Z bounds.")
# Observed z_range=[0.793, 1.250]; tighten from [-0.2, 3.0] accordingly
if np.min(pos[:, 2]) < 0.5 or np.max(pos[:, 2]) > 1.5:
raise ValueError(
f"Cable bodies out of Z bounds: min_z={np.min(pos[:, 2]):.3f}, max_z={np.max(pos[:, 2]):.3f}"
)

# Velocity sanity check: observed max_body_vel ~3.71
body_qd_np = self.state_0.body_qd.numpy()[rod_bodies]
if not np.all(np.isfinite(body_qd_np)):
raise ValueError("NaN/Inf in cable body velocities.")
max_vel = np.max(np.abs(body_qd_np))
if max_vel > 8.0:
raise ValueError(f"Cable body velocity too large: {max_vel:.3f} > 8.0")

# Pinned body should not drift.
q_now = self.state_0.body_q.numpy()[self.pinned_body]
Expand Down
31 changes: 18 additions & 13 deletions newton/examples/cloth/example_cloth_bending.py
Original file line number Diff line number Diff line change
Expand Up @@ -134,20 +134,25 @@ def render(self):
self.viewer.end_frame()

def test_final(self):
# Test that particles have come to rest (lenient velocity threshold)
newton.examples.test_particle_state(
self.state_0,
"particles have come close to a rest",
lambda q, qd: max(abs(qd)) < 0.5,
)
particle_q = self.state_0.particle_q.numpy()
particle_qd = self.state_0.particle_qd.numpy()

# Test that particles haven't drifted too far from initial x,y position
# Initial position was (0, 0, 10), so check x,y are within reasonable bounds
newton.examples.test_particle_state(
self.state_0,
"particles stayed near initial x,y position",
lambda q, qd: abs(q[0]) < 5.0 and abs(q[1]) < 5.0,
)
centroid = np.mean(particle_q, axis=0)
bbox_size = np.max(np.max(particle_q, axis=0) - np.min(particle_q, axis=0))
min_z = np.min(particle_q[:, 2])
max_vel = np.max(np.linalg.norm(particle_qd, axis=1))

if wp.get_device().is_cuda:
# Tight CUDA checks: observed centroid [-2.41, 2.04, 0.75]
assert np.allclose(centroid, [-2.41, 2.04, 0.75], atol=[0.13, 0.11, 0.04]), f"Centroid drift: {centroid}"
assert bbox_size < 3.11, f"Bbox too large: {bbox_size}"
assert min_z > 0.07, f"Ground penetration: min_z={min_z}"
assert max_vel < 0.018, f"Excessive velocity: {max_vel}"
else:
# CPU produces a different final state; check basic sanity only
assert bbox_size < 6.0, f"Bbox too large: {bbox_size}"
assert min_z > -0.1, f"Ground penetration: min_z={min_z}"
assert max_vel < 5.0, f"Excessive velocity: {max_vel}"

# Test that spring/edge lengths haven't stretched too much from rest length
if self.model.spring_count > 0:
Expand Down
30 changes: 17 additions & 13 deletions newton/examples/cloth/example_cloth_franka.py
Original file line number Diff line number Diff line change
Expand Up @@ -660,23 +660,27 @@ def render(self):
self.model.shape_scale = self.sim_shape_scale

def test_final(self):
p_lower = wp.vec3(-36.0, -95.0, -5.0)
p_upper = wp.vec3(36.0, 5.0, 56.0)
newton.examples.test_particle_state(
self.state_0,
"particles are within a reasonable volume",
lambda q, qd: newton.math.vec_inside_limits(q, p_lower, p_upper),
)
newton.examples.test_particle_state(
self.state_0,
"particle velocities are within a reasonable range",
lambda q, qd: max(abs(qd)) < 200.0,
)
particle_q = self.state_0.particle_q.numpy()
particle_qd = self.state_0.particle_qd.numpy()

# Centroid check (cm scale): observed [0.12, -56.85, 21.54]
centroid = np.mean(particle_q, axis=0)
assert np.allclose(centroid, [0.12, -56.85, 21.54], atol=[2.85, 2.85, 1.08]), f"Centroid drift: {centroid}"

# Bounding box check (cm scale): observed 66.10
bbox_size = np.max(np.max(particle_q, axis=0) - np.min(particle_q, axis=0))
assert bbox_size < 69.41, f"Bbox too large: {bbox_size}"

# Velocity check (cm/s): observed max_vel=7.99-12.02 across GPU architectures
max_vel = np.max(np.linalg.norm(particle_qd, axis=1))
assert max_vel < 18.0, f"Excessive velocity: {max_vel}"

# Body state check (9 bodies - Franka robot)
newton.examples.test_body_state(
self.model,
self.state_0,
"body velocities are within a reasonable range",
lambda q, qd: max(abs(qd)) < 70.0,
lambda q, qd: max(abs(qd)) < 21.83,
)


Expand Down
29 changes: 24 additions & 5 deletions newton/examples/cloth/example_cloth_h1.py
Original file line number Diff line number Diff line change
Expand Up @@ -304,12 +304,31 @@ def step(self):
self.frame_index += 1

def test_final(self):
p_lower = wp.vec3(-0.3, -0.8, 0.8)
p_upper = wp.vec3(0.5, 0.8, 1.8)
newton.examples.test_particle_state(
particle_q = self.state.particle_q.numpy()
particle_qd = self.state.particle_qd.numpy()

# Centroid check: observed [0.02, 0.0, 1.30]
centroid = np.mean(particle_q, axis=0)
assert np.allclose(centroid, [0.02, 0.0, 1.30], atol=[0.05, 0.05, 0.10]), f"Centroid drift: {centroid}"

# Bounding box check: observed 0.74
bbox_size = np.max(np.max(particle_q, axis=0) - np.min(particle_q, axis=0))
assert bbox_size < 0.78, f"Bbox too large: {bbox_size}"

# Ground penetration check: observed min_z=0.96
min_z = np.min(particle_q[:, 2])
assert min_z > 0.94, f"Ground penetration: min_z={min_z}"

# Velocity check: observed max_vel=0.97-2.27
max_vel = np.max(np.linalg.norm(particle_qd, axis=1))
assert max_vel < 3.5, f"Excessive velocity: {max_vel}"

# Body state check (46 bodies - H1 robot)
newton.examples.test_body_state(
self.model,
self.state,
"particles are within a reasonable volume",
lambda q, qd: newton.math.vec_inside_limits(q, p_lower, p_upper),
"body velocities are within a reasonable range",
lambda q, qd: max(abs(qd)) < 5.0,
)

def render(self):
Expand Down
48 changes: 34 additions & 14 deletions newton/examples/cloth/example_cloth_hanging.py
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
#
###########################################################################

import numpy as np
import warp as wp

import newton
Expand Down Expand Up @@ -183,21 +184,40 @@ def step(self):
self.sim_time += self.frame_dt

def test_final(self):
if self.solver_type != "style3d":
newton.examples.test_particle_state(
self.state_0,
"particles are above the ground",
lambda q, qd: q[2] > 0.0,
)
particle_q = self.state_0.particle_q.numpy()
particle_qd = self.state_0.particle_qd.numpy()

centroid = np.mean(particle_q, axis=0)
bbox_size = np.max(np.max(particle_q, axis=0) - np.min(particle_q, axis=0))
max_vel = np.max(np.linalg.norm(particle_qd, axis=1))

min_x = -float(self.sim_width) * 0.11
p_lower = wp.vec3(min_x, -4.0, -1.8)
p_upper = wp.vec3(0.1, 7.0, 4.0)
newton.examples.test_particle_state(
self.state_0,
"particles are within a reasonable volume",
lambda q, qd: newton.math.vec_inside_limits(q, p_lower, p_upper),
)
if wp.get_device().is_cuda:
# Tight CUDA checks (full-duration runs)
if self.solver_type == "style3d":
# Style3D: observed centroid [-1.60, -0.43, 1.34]
assert np.allclose(centroid, [-1.60, -0.43, 1.34], atol=[0.30, 0.30, 0.30]), (
f"Centroid drift (style3d): {centroid}"
)
else:
# VBD: observed centroid [-1.80, 0.35, 1.20], min_z=0.049
min_z = np.min(particle_q[:, 2])
assert min_z > 0.02, f"Ground penetration: min_z={min_z}"
assert np.allclose(centroid, [-1.80, 0.35, 1.20], atol=[0.30, 0.30, 0.30]), (
f"Centroid drift (vbd): {centroid}"
)

# Bounding box check: observed 3.95
assert bbox_size < 4.20, f"Bbox too large: {bbox_size}"

# Velocity check: observed max_vel=5.46 (still draping)
assert max_vel < 8.19, f"Excessive velocity: {max_vel}"
else:
# CPU tests run with fewer frames; cloth may barely have moved.
# Only check that the simulation didn't explode.
assert np.all(np.isfinite(particle_q)), "Non-finite particle positions"
assert np.all(np.isfinite(particle_qd)), "Non-finite particle velocities"
assert bbox_size < 10.0, f"Bbox too large: {bbox_size}"
assert max_vel < 50.0, f"Excessive velocity: {max_vel}"

def render(self):
self.viewer.begin_frame(self.sim_time)
Expand Down
Loading
Loading