diff --git a/newton/examples/basic/example_basic_conveyor.py b/newton/examples/basic/example_basic_conveyor.py index b807f50146..92b6ad4166 100644 --- a/newton/examples/basic/example_basic_conveyor.py +++ b/newton/examples/basic/example_basic_conveyor.py @@ -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}" @@ -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__": diff --git a/newton/examples/basic/example_basic_heightfield.py b/newton/examples/basic/example_basic_heightfield.py index 26c1a14486..ec578e7363 100644 --- a/newton/examples/basic/example_basic_heightfield.py +++ b/newton/examples/basic/example_basic_heightfield.py @@ -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__": diff --git a/newton/examples/basic/example_basic_pendulum.py b/newton/examples/basic/example_basic_pendulum.py index 2a882e9089..732e140c9e 100644 --- a/newton/examples/basic/example_basic_pendulum.py +++ b/newton/examples/basic/example_basic_pendulum.py @@ -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( diff --git a/newton/examples/basic/example_basic_plotting.py b/newton/examples/basic/example_basic_plotting.py index 73345b13ef..ff4f2ce7ad 100644 --- a/newton/examples/basic/example_basic_plotting.py +++ b/newton/examples/basic/example_basic_plotting.py @@ -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() diff --git a/newton/examples/basic/example_basic_shapes.py b/newton/examples/basic/example_basic_shapes.py index 7aa1b38160..a81267ca55 100644 --- a/newton/examples/basic/example_basic_shapes.py +++ b/newton/examples/basic/example_basic_shapes.py @@ -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 diff --git a/newton/examples/basic/example_basic_urdf.py b/newton/examples/basic/example_basic_urdf.py index c87c80117b..ed13e08aa6 100644 --- a/newton/examples/basic/example_basic_urdf.py +++ b/newton/examples/basic/example_basic_urdf.py @@ -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 diff --git a/newton/examples/cable/example_cable_bundle_hysteresis.py b/newton/examples/cable/example_cable_bundle_hysteresis.py index cc5e958a5d..dd28f57d2e 100644 --- a/newton/examples/cable/example_cable_bundle_hysteresis.py +++ b/newton/examples/cable/example_cable_bundle_hysteresis.py @@ -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}" + ) @staticmethod def create_parser(): diff --git a/newton/examples/cable/example_cable_pile.py b/newton/examples/cable/example_cable_pile.py index 4f085b3489..d27a1ead35 100644 --- a/newton/examples/cable/example_cable_pile.py +++ b/newton/examples/cable/example_cable_pile.py @@ -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: @@ -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__": diff --git a/newton/examples/cable/example_cable_twist.py b/newton/examples/cable/example_cable_twist.py index 0590159244..1768e11114 100644 --- a/newton/examples/cable/example_cable_twist.py +++ b/newton/examples/cable/example_cable_twist.py @@ -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): @@ -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__": diff --git a/newton/examples/cable/example_cable_y_junction.py b/newton/examples/cable/example_cable_y_junction.py index 51ac343e59..5ba246bbd6 100644 --- a/newton/examples/cable/example_cable_y_junction.py +++ b/newton/examples/cable/example_cable_y_junction.py @@ -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] diff --git a/newton/examples/cloth/example_cloth_bending.py b/newton/examples/cloth/example_cloth_bending.py index 8fd8d10c9c..34dc81510a 100644 --- a/newton/examples/cloth/example_cloth_bending.py +++ b/newton/examples/cloth/example_cloth_bending.py @@ -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: diff --git a/newton/examples/cloth/example_cloth_franka.py b/newton/examples/cloth/example_cloth_franka.py index 9772dfcce2..669ab59541 100644 --- a/newton/examples/cloth/example_cloth_franka.py +++ b/newton/examples/cloth/example_cloth_franka.py @@ -622,23 +622,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, up to 18.9 on some GPU architectures + max_vel = np.max(np.linalg.norm(particle_qd, axis=1)) + assert max_vel < 22.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, ) diff --git a/newton/examples/cloth/example_cloth_h1.py b/newton/examples/cloth/example_cloth_h1.py index 0683b3fecd..05b836b34e 100644 --- a/newton/examples/cloth/example_cloth_h1.py +++ b/newton/examples/cloth/example_cloth_h1.py @@ -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): diff --git a/newton/examples/cloth/example_cloth_hanging.py b/newton/examples/cloth/example_cloth_hanging.py index c8c586a7da..adce91a2da 100644 --- a/newton/examples/cloth/example_cloth_hanging.py +++ b/newton/examples/cloth/example_cloth_hanging.py @@ -11,6 +11,7 @@ # ########################################################################### +import numpy as np import warp as wp import newton @@ -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) diff --git a/newton/examples/cloth/example_cloth_poker_cards.py b/newton/examples/cloth/example_cloth_poker_cards.py index 578b0419b3..39768a7d5c 100644 --- a/newton/examples/cloth/example_cloth_poker_cards.py +++ b/newton/examples/cloth/example_cloth_poker_cards.py @@ -280,18 +280,29 @@ def test_final(self): particle_q = self.state_0.particle_q.numpy() particle_qd = self.state_0.particle_qd.numpy() - # Check velocity (cards should be settling) - max_vel = np.max(np.linalg.norm(particle_qd, axis=1)) - assert max_vel < 0.5, f"Cards moving too fast: max_vel={max_vel:.4f} m/s" + # Centroid check: observed [0.0, 0.0, 0.22] + centroid = np.mean(particle_q, axis=0) + assert np.allclose(centroid, [0.0, 0.0, 0.22], atol=[0.02, 0.02, 0.02]), f"Centroid drift: {centroid}" + + # Bounding box check: observed 0.11 + bbox_size = np.max(np.max(particle_q, axis=0) - np.min(particle_q, axis=0)) + assert bbox_size < 0.12, f"Bbox too large: {bbox_size}" + + # Ground penetration check: observed min_z=0.20 + min_z = np.min(particle_q[:, 2]) + assert min_z > 0.18, f"Ground penetration: min_z={min_z}" - # Check bbox size is reasonable (not exploding) - min_pos = np.min(particle_q, axis=0) - max_pos = np.max(particle_q, axis=0) - bbox_size = np.linalg.norm(max_pos - min_pos) - assert bbox_size < 2.0, f"Bounding box exploded: size={bbox_size:.2f}" + # Velocity check: observed max_vel=0.003 + max_vel = np.max(np.linalg.norm(particle_qd, axis=1)) + assert max_vel < 0.005, f"Excessive velocity: {max_vel}" - # Check no excessive penetration - assert min_pos[2] > -0.1, f"Excessive penetration: z_min={min_pos[2]:.4f}" + # Body state check (cube and sphere) + newton.examples.test_body_state( + self.model, + self.state_0, + "body velocities are within a reasonable range", + lambda q, qd: max(abs(qd)) < 0.75, + ) if __name__ == "__main__": diff --git a/newton/examples/cloth/example_cloth_rollers.py b/newton/examples/cloth/example_cloth_rollers.py index 44ef089766..a538650e18 100644 --- a/newton/examples/cloth/example_cloth_rollers.py +++ b/newton/examples/cloth/example_cloth_rollers.py @@ -496,28 +496,35 @@ def render(self): self.viewer.end_frame() def test_final(self): - """Test that cloth centroid has moved (unrolling has started).""" + """Test that cloth centroid has moved and state is within expected bounds.""" # Get cloth particle positions (exclude cylinder particles) particle_q = self.state_0.particle_q.numpy() + particle_qd = self.state_0.particle_qd.numpy() cloth_q = particle_q[: self.num_cloth_verts] + cloth_qd = particle_qd[: self.num_cloth_verts] # Calculate center of mass com = np.mean(cloth_q, axis=0) - # Initial COM is at X ≈ -25.72 (near cylinder 1 at X=-27.2) - # After 200 frames (~3.3 seconds), expect COM to shift noticeably + # Centroid shift check: initial COM X ~ -25.72, observed COM [-12.12, 50.02, 6.23] initial_com_x = -25.72 - min_shift = 5.0 # Require at least 5 units of movement to verify simulation is working - + min_shift = 5.0 actual_shift = com[0] - initial_com_x - assert actual_shift > min_shift, ( f"Cloth centroid hasn't moved enough: shift={actual_shift:.1f} < {min_shift:.1f}, COM X={com[0]:.1f}" ) - # Ensure bbox hasn't exploded - bbox_size = np.linalg.norm(np.max(cloth_q, axis=0) - np.min(cloth_q, axis=0)) - assert bbox_size < 150.0, f"Bbox exploded: size={bbox_size:.2f}" + # Centroid check (cm scale): observed [-12.12, 50.02, 6.23] to [-15.90, 49.49, 6.60] + # Active rollers cause higher run-to-run variation, especially in X + assert np.allclose(com, [-14.0, 50.0, 6.4], atol=[5.0, 3.0, 3.0]), f"Centroid drift: {com}" + + # Bounding box check (cm scale): observed 120.0 + bbox_size = np.max(np.max(cloth_q, axis=0) - np.min(cloth_q, axis=0)) + assert bbox_size < 126.0, f"Bbox too large: {bbox_size}" + + # Velocity check (cm/s): observed max_vel=63.80-104.34 + max_vel = np.max(np.linalg.norm(cloth_qd, axis=1)) + assert max_vel < 160.0, f"Excessive velocity: {max_vel}" if __name__ == "__main__": diff --git a/newton/examples/cloth/example_cloth_style3d.py b/newton/examples/cloth/example_cloth_style3d.py index d2630dba97..1eb249854d 100644 --- a/newton/examples/cloth/example_cloth_style3d.py +++ b/newton/examples/cloth/example_cloth_style3d.py @@ -1,6 +1,7 @@ # SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers # SPDX-License-Identifier: Apache-2.0 +import numpy as np import warp as wp from pxr import Usd @@ -166,12 +167,31 @@ def step(self): self.sim_time += self.frame_dt def test_final(self): - p_lower = wp.vec3(-0.5, -0.2, 0.9) - p_upper = wp.vec3(0.5, 0.2, 1.6) - newton.examples.test_particle_state( + particle_q = self.state_0.particle_q.numpy() + particle_qd = self.state_0.particle_qd.numpy() + + # Centroid check: observed [0.0, 0.0, 1.20] + centroid = np.mean(particle_q, axis=0) + assert np.allclose(centroid, [0.0, 0.0, 1.20], atol=[0.02, 0.02, 0.06]), f"Centroid drift: {centroid}" + + # Bounding box check: observed 0.88 + bbox_size = np.max(np.max(particle_q, axis=0) - np.min(particle_q, axis=0)) + assert bbox_size < 0.93, f"Bbox too large: {bbox_size}" + + # Ground penetration check: observed min_z=0.94 + min_z = np.min(particle_q[:, 2]) + assert min_z > 0.92, f"Ground penetration: min_z={min_z}" + + # Velocity check: observed max_vel=0.18-0.29 (CPU), up to 0.47 (GPU) + max_vel = np.max(np.linalg.norm(particle_qd, axis=1)) + assert max_vel < 0.55, f"Excessive velocity: {max_vel}" + + # Body state check (1 avatar body) + newton.examples.test_body_state( + self.model, self.state_0, - "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)) < 1.0, ) def render(self): diff --git a/newton/examples/cloth/example_cloth_twist.py b/newton/examples/cloth/example_cloth_twist.py index 9e4604277b..cdf159a3ec 100644 --- a/newton/examples/cloth/example_cloth_twist.py +++ b/newton/examples/cloth/example_cloth_twist.py @@ -282,18 +282,24 @@ def render(self): self.viewer.end_frame() def test_final(self): - p_lower = wp.vec3(-0.6, -0.9, -0.6) - p_upper = wp.vec3(0.6, 0.9, 0.6) - 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)) < 1.5, - ) + particle_q = self.state_0.particle_q.numpy() + particle_qd = self.state_0.particle_qd.numpy() + + # Centroid check: observed [0.0, 0.0, 0.0] + centroid = np.mean(particle_q, axis=0) + assert np.allclose(centroid, [0.0, 0.0, 0.0], atol=0.02), f"Centroid drift: {centroid}" + + # Bounding box check: observed 1.50 + bbox_size = np.max(np.max(particle_q, axis=0) - np.min(particle_q, axis=0)) + assert bbox_size < 1.58, f"Bbox too large: {bbox_size}" + + # Min Z check (no ground plane, twisted): observed min_z=-0.086 + min_z = np.min(particle_q[:, 2]) + assert min_z > -0.11, f"Min Z too low: min_z={min_z}" + + # Velocity check: upstream #2480 observed >1.0 on CI; use their 1.5 ceiling + max_vel = np.max(np.linalg.norm(particle_qd, axis=1)) + assert max_vel < 1.5, f"Excessive velocity: {max_vel}" if __name__ == "__main__": diff --git a/newton/examples/contacts/example_nut_bolt_hydro.py b/newton/examples/contacts/example_nut_bolt_hydro.py index cac336ca28..64f44c781b 100644 --- a/newton/examples/contacts/example_nut_bolt_hydro.py +++ b/newton/examples/contacts/example_nut_bolt_hydro.py @@ -372,7 +372,7 @@ def test_final(self): body_q = self.state_0.body_q.numpy() # Check bolts stayed in place - max_bolt_displacement = 0.02 # 2 cm tolerance + max_bolt_displacement = 0.015 # 1.5 cm tolerance (non-deterministic contact sim) for i, bolt_idx in enumerate(self.bolt_body_indices): current_pos = body_q[bolt_idx][:3] initial_pos = self.bolt_initial_transforms[i][:3] diff --git a/newton/examples/contacts/example_nut_bolt_sdf.py b/newton/examples/contacts/example_nut_bolt_sdf.py index 7a7adf9f30..c96791fac5 100644 --- a/newton/examples/contacts/example_nut_bolt_sdf.py +++ b/newton/examples/contacts/example_nut_bolt_sdf.py @@ -344,7 +344,7 @@ def test_final(self): body_q = self.state_0.body_q.numpy() # Check bolts stayed in place - max_bolt_displacement = 0.02 # 2 cm tolerance + max_bolt_displacement = 0.015 # 1.5 cm tolerance (non-deterministic contact sim) for i, bolt_idx in enumerate(self.bolt_body_indices): current_pos = body_q[bolt_idx][:3] initial_pos = self.bolt_initial_transforms[i][:3] diff --git a/newton/examples/contacts/example_pyramid.py b/newton/examples/contacts/example_pyramid.py index 8007421411..b1a174ad1c 100644 --- a/newton/examples/contacts/example_pyramid.py +++ b/newton/examples/contacts/example_pyramid.py @@ -182,6 +182,7 @@ def test_final(self): within ``max_displacement`` of its initial position. """ body_q = self.state_0.body_q.numpy() + body_qd = self.state_0.body_qd.numpy() max_displacement = 0.5 # [m] for idx in self.top_body_indices: current_pos = body_q[idx, :3] @@ -191,6 +192,11 @@ def test_final(self): f"Top cube body {idx}: displaced {displacement:.4f} m (max allowed {max_displacement:.4f} m)" ) + # Verify all bodies have settled (low velocity) + for idx in range(self.model.body_count): + vel = float(np.linalg.norm(body_qd[idx])) + assert vel < 0.01, f"Body {idx} not settled: vel={vel:.6f}" + @staticmethod def create_parser(): parser = newton.examples.create_parser() diff --git a/newton/examples/ik/example_ik_custom.py b/newton/examples/ik/example_ik_custom.py index a9acd28b4b..7c89f41046 100644 --- a/newton/examples/ik/example_ik_custom.py +++ b/newton/examples/ik/example_ik_custom.py @@ -15,6 +15,7 @@ # Command: python -m newton.examples ik_custom ########################################################################### +import numpy as np import warp as wp import newton @@ -299,7 +300,32 @@ def step(self): self.sim_time += self.frame_dt def test_final(self): - pass + newton.eval_fk(self.model, self.model.joint_q, self.model.joint_qd, self.state) + + body_q = self.state.body_q.numpy() + body_qd = self.state.body_qd.numpy() + + # Verify end-effector reached target + ee_pos = body_q[self.ee_index, :3] + ee_target = np.array([0.095, 0.0, 0.903]) + assert np.allclose(ee_pos, ee_target, atol=0.01), ( + f"End-effector position {ee_pos} too far from target {ee_target}" + ) + + # Verify collision avoidance: check that all monitored links maintain + # clearance from the obstacle sphere + obstacle_center = np.array([self.obstacle_center[0], self.obstacle_center[1], self.obstacle_center[2]]) + for name, link_idx, link_radius in self.links_to_check_collision: + link_pos = body_q[link_idx, :3] + dist = np.linalg.norm(link_pos - obstacle_center) + min_clearance = self.obstacle_radius + link_radius + assert dist >= min_clearance - 0.02, ( + f"Link {name} (body {link_idx}) at {link_pos} too close to obstacle: " + f"dist={dist:.4f}, min_clearance={min_clearance:.4f}" + ) + + # Verify all body velocities are zero + assert np.allclose(body_qd, 0.0, atol=1e-10), "Non-zero body velocities in IK example" def render(self): self.viewer.begin_frame(self.sim_time) diff --git a/newton/examples/ik/example_ik_franka.py b/newton/examples/ik/example_ik_franka.py index 0f96eaee2e..d07c39b35a 100644 --- a/newton/examples/ik/example_ik_franka.py +++ b/newton/examples/ik/example_ik_franka.py @@ -12,6 +12,7 @@ # Command: python -m newton.examples ik_franka ########################################################################### +import numpy as np import warp as wp import newton @@ -139,7 +140,20 @@ def step(self): self.sim_time += self.frame_dt def test_final(self): - pass + # Update body transforms from the IK solution + newton.eval_fk(self.model, self.model.joint_q, self.model.joint_qd, self.state) + + # Verify end-effector reached target position + body_q = self.state.body_q.numpy() + ee_pos = body_q[self.ee_index, :3] + ee_target = np.array([0.0967, 0.0, 0.9034]) + assert np.allclose(ee_pos, ee_target, atol=0.01), ( + f"End-effector position {ee_pos} too far from target {ee_target}" + ) + + # Verify all body velocities are zero (IK is quasi-static) + body_qd = self.state.body_qd.numpy() + assert np.allclose(body_qd, 0.0, atol=1e-10), "Non-zero body velocities in IK example" def render(self): self.viewer.begin_frame(self.sim_time) diff --git a/newton/examples/ik/example_ik_h1.py b/newton/examples/ik/example_ik_h1.py index c470636917..f3698122a5 100644 --- a/newton/examples/ik/example_ik_h1.py +++ b/newton/examples/ik/example_ik_h1.py @@ -14,6 +14,7 @@ # Command: python -m newton.examples ik_h1 ########################################################################### +import numpy as np import warp as wp import newton @@ -147,7 +148,26 @@ def step(self): self.sim_time += self.frame_dt def test_final(self): - pass + newton.eval_fk(self.model, self.model.joint_q, self.model.joint_qd, self.state) + + body_q = self.state.body_q.numpy() + body_qd = self.state.body_qd.numpy() + + # Verify end-effector positions match expected IK solution + ee_expected = { + 5: np.array([0.039, 0.203, 0.126]), # left_foot (left_ankle_link) + 10: np.array([0.039, -0.203, 0.126]), # right_foot (right_ankle_link) + 16: np.array([0.279, 0.214, 1.188]), # left_hand + 33: np.array([0.279, -0.214, 1.188]), # right_hand + } + for idx, expected_pos in ee_expected.items(): + actual_pos = body_q[idx, :3] + assert np.allclose(actual_pos, expected_pos, atol=0.01), ( + f"EE body {idx} at {actual_pos}, expected ~{expected_pos}" + ) + + # Verify all body velocities are zero + assert np.allclose(body_qd, 0.0, atol=1e-10), "Non-zero body velocities in IK example" def render(self): self.viewer.begin_frame(self.sim_time) diff --git a/newton/examples/mpm/example_mpm_anymal.py b/newton/examples/mpm/example_mpm_anymal.py index 98fe229e99..ffeb0c449b 100644 --- a/newton/examples/mpm/example_mpm_anymal.py +++ b/newton/examples/mpm/example_mpm_anymal.py @@ -274,13 +274,13 @@ def test_final(self): self.model, self.state_0, "all bodies are above the ground", - lambda q, qd: q[2] > 0.1, + lambda q, qd: q[2] > 0.15, ) newton.examples.test_body_state( self.model, self.state_0, "the robot went in the right direction", - lambda q, qd: q[1] > 0.9, # This threshold assumes 100 frames + lambda q, qd: q[1] > 0.95, # This threshold assumes 100 frames ) forward_vel_min = wp.spatial_vector(-0.2, 0.9, -0.2, -0.8, -1.5, -0.5) @@ -299,6 +299,16 @@ def test_final(self): lambda q, qd: q[2] > -1.1 * voxel_size, ) + # Empirical centroid, bbox, and velocity checks (100 frames) + positions = self.state_0.particle_q.numpy() + centroid = np.mean(positions, axis=0) + bbox = np.max(np.max(positions, axis=0) - np.min(positions, axis=0)) + + assert abs(centroid[0] - (-0.001)) < max(0.02, 0.001 * 0.05), f"Centroid X out of range: {centroid[0]}" + assert abs(centroid[1] - 1.012) < max(0.02, 1.012 * 0.05), f"Centroid Y out of range: {centroid[1]}" + assert abs(centroid[2] - 0.059) < max(0.02, 0.059 * 0.05), f"Centroid Z out of range: {centroid[2]}" + assert bbox < 3.42 + max(0.05, 3.42 * 0.05), f"Bounding box too large: {bbox}" + def render(self): self.viewer.begin_frame(self.sim_time) self.viewer.log_state(self.state_0) diff --git a/newton/examples/mpm/example_mpm_beam_twist.py b/newton/examples/mpm/example_mpm_beam_twist.py index 9fc7f77f2f..3c296590da 100644 --- a/newton/examples/mpm/example_mpm_beam_twist.py +++ b/newton/examples/mpm/example_mpm_beam_twist.py @@ -251,6 +251,20 @@ def test_final(self): lambda q, qd: wp.length(q) < 10.0, ) + # Empirical centroid, bbox, and velocity checks (100 frames) + positions = self.state_0.particle_q.numpy() + velocities = self.state_0.particle_qd.numpy() + centroid = np.mean(positions, axis=0) + bbox = np.max(np.max(positions, axis=0) - np.min(positions, axis=0)) + max_vel = np.max(np.linalg.norm(velocities, axis=1)) + + assert abs(centroid[0] - 2.46) < max(0.02, 2.46 * 0.05), f"Centroid X out of range: {centroid[0]}" + assert abs(centroid[1] - 0.46) < max(0.02, 0.46 * 0.05), f"Centroid Y out of range: {centroid[1]}" + assert abs(centroid[2] - 0.42) < max(0.02, 0.42 * 0.05), f"Centroid Z out of range: {centroid[2]}" + assert bbox < 4.92 + max(0.05, 4.92 * 0.05), f"Bounding box too large: {bbox}" + assert max_vel < max(0.002, 1.05 * 1.5), f"Max velocity too high: {max_vel}" + assert np.min(positions[:, 2]) > -0.20 - 0.02, f"Min Z too low: {np.min(positions[:, 2])}" + def render_ui(self, imgui): _changed, self.show_stress = imgui.checkbox("Show Stress", self.show_stress) diff --git a/newton/examples/mpm/example_mpm_grain_rendering.py b/newton/examples/mpm/example_mpm_grain_rendering.py index 2a61722487..7b9db9d1b5 100644 --- a/newton/examples/mpm/example_mpm_grain_rendering.py +++ b/newton/examples/mpm/example_mpm_grain_rendering.py @@ -75,6 +75,20 @@ def test_final(self): lambda q, qd: q[2] > -0.05, ) + # Empirical centroid, bbox, and velocity checks (10 frames) + positions = self.state_0.particle_q.numpy() + velocities = self.state_0.particle_qd.numpy() + centroid = np.mean(positions, axis=0) + bbox = np.max(np.max(positions, axis=0) - np.min(positions, axis=0)) + max_vel = np.max(np.linalg.norm(velocities, axis=1)) + + assert abs(centroid[0] - 0.0) < 0.02, f"Centroid X out of range: {centroid[0]}" + assert abs(centroid[1] - 0.0) < 0.02, f"Centroid Y out of range: {centroid[1]}" + assert abs(centroid[2] - 0.88) < max(0.02, 0.88 * 0.05), f"Centroid Z out of range: {centroid[2]}" + assert bbox < 1.87 + max(0.05, 1.87 * 0.05), f"Bounding box too large: {bbox}" + assert max_vel < max(0.002, 1.63 * 1.5), f"Max velocity too high: {max_vel}" + assert np.min(positions[:, 2]) > -0.001 - 0.02, f"Min Z too low: {np.min(positions[:, 2])}" + def render(self): self.viewer.begin_frame(self.sim_time) self.viewer.log_state(self.state_0) diff --git a/newton/examples/mpm/example_mpm_granular.py b/newton/examples/mpm/example_mpm_granular.py index 8ce7299ddd..80c68f85ee 100644 --- a/newton/examples/mpm/example_mpm_granular.py +++ b/newton/examples/mpm/example_mpm_granular.py @@ -153,6 +153,20 @@ def test_final(self): max_z = np.max(self.state_0.particle_q.numpy()[:, 2]) assert max_z > 0.8, "All particles have collapsed" + # Empirical centroid, bbox, and velocity checks (100 frames, cube collider) + positions = self.state_0.particle_q.numpy() + velocities = self.state_0.particle_qd.numpy() + centroid = np.mean(positions, axis=0) + bbox = np.max(np.max(positions, axis=0) - np.min(positions, axis=0)) + max_vel = np.max(np.linalg.norm(velocities, axis=1)) + + assert abs(centroid[0] - (-0.31)) < max(0.02, 0.31 * 0.05), f"Centroid X out of range: {centroid[0]}" + assert abs(centroid[1] - 0.0) < 0.02, f"Centroid Y out of range: {centroid[1]}" + assert abs(centroid[2] - 0.28) < max(0.02, 0.28 * 0.05), f"Centroid Z out of range: {centroid[2]}" + assert bbox < 8.97 + max(0.05, 8.97 * 0.05), f"Bounding box too large: {bbox}" + assert max_vel < max(0.002, 5.26 * 1.5), f"Max velocity too high: {max_vel}" + assert np.min(positions[:, 2]) > -0.001 - 0.02, f"Min Z too low: {np.min(positions[:, 2])}" + def render(self): self.viewer.begin_frame(self.sim_time) self.viewer.log_state(self.state_0) diff --git a/newton/examples/mpm/example_mpm_multi_material.py b/newton/examples/mpm/example_mpm_multi_material.py index be57fed65e..3c12bc41c4 100644 --- a/newton/examples/mpm/example_mpm_multi_material.py +++ b/newton/examples/mpm/example_mpm_multi_material.py @@ -90,6 +90,20 @@ def test_final(self): lambda q, qd: q[2] > -0.05, ) + # Empirical centroid, bbox, and velocity checks (10 frames) + positions = self.state_0.particle_q.numpy() + velocities = self.state_0.particle_qd.numpy() + centroid = np.mean(positions, axis=0) + bbox = np.max(np.max(positions, axis=0) - np.min(positions, axis=0)) + max_vel = np.max(np.linalg.norm(velocities, axis=1)) + + assert abs(centroid[0] - 0.0) < 0.02, f"Centroid X out of range: {centroid[0]}" + assert abs(centroid[1] - 0.0) < 0.02, f"Centroid Y out of range: {centroid[1]}" + assert abs(centroid[2] - 0.56) < max(0.02, 0.56 * 0.05), f"Centroid Z out of range: {centroid[2]}" + assert bbox < 1.52 + max(0.05, 1.52 * 0.05), f"Bounding box too large: {bbox}" + assert max_vel < max(0.002, 1.64 * 1.5), f"Max velocity too high: {max_vel}" + assert np.min(positions[:, 2]) > -0.008 - 0.02, f"Min Z too low: {np.min(positions[:, 2])}" + def render(self): self.viewer.begin_frame(self.sim_time) self.viewer.log_points( diff --git a/newton/examples/mpm/example_mpm_snow_ball.py b/newton/examples/mpm/example_mpm_snow_ball.py index 75b786e105..49a3006674 100644 --- a/newton/examples/mpm/example_mpm_snow_ball.py +++ b/newton/examples/mpm/example_mpm_snow_ball.py @@ -356,9 +356,23 @@ def test_final(self): newton.examples.test_particle_state( self.state_0, "all particles are within the terrain domain", - lambda q, qd: q[2] > -20.0 and q[2] < 30.0, + lambda q, qd: q[2] > -7.0 and q[2] < 17.0, ) + # Empirical centroid, bbox, and velocity checks (30 frames, --voxel-size 0.2) + positions = self.state_0.particle_q.numpy() + velocities = self.state_0.particle_qd.numpy() + centroid = np.mean(positions, axis=0) + bbox = np.max(np.max(positions, axis=0) - np.min(positions, axis=0)) + max_vel = np.max(np.linalg.norm(velocities, axis=1)) + + assert abs(centroid[0] - (-0.05)) < max(0.02, 0.05 * 0.05), f"Centroid X out of range: {centroid[0]}" + assert abs(centroid[1] - (-0.30)) < max(0.02, 0.30 * 0.05), f"Centroid Y out of range: {centroid[1]}" + assert abs(centroid[2] - 1.27) < max(0.02, 1.27 * 0.05), f"Centroid Z out of range: {centroid[2]}" + assert bbox < 22.05 + max(0.05, 22.05 * 0.05), f"Bounding box too large: {bbox}" + assert max_vel < max(0.002, 4.90 * 1.5), f"Max velocity too high: {max_vel}" + assert np.min(positions[:, 2]) > -6.28 - 0.02, f"Min Z too low: {np.min(positions[:, 2])}" + def render_ui(self, imgui): _changed, self.show_compression = imgui.checkbox("Show Compression", self.show_compression) diff --git a/newton/examples/mpm/example_mpm_twoway_coupling.py b/newton/examples/mpm/example_mpm_twoway_coupling.py index 7643b1b8b9..8a08ff7926 100644 --- a/newton/examples/mpm/example_mpm_twoway_coupling.py +++ b/newton/examples/mpm/example_mpm_twoway_coupling.py @@ -273,6 +273,11 @@ def test_final(self): lambda q, qd: q[2] > -voxel_size, ) + # Empirical body velocity check (80 frames, 6 bodies) + body_qd = self.state_0.body_qd.numpy() + max_body_vel = np.max(np.linalg.norm(body_qd[:, 3:], axis=1)) + assert max_body_vel < max(0.002, 0.81 * 1.5), f"Max body velocity too high: {max_body_vel}" + def render(self): self.viewer.begin_frame(self.sim_time) self.viewer.log_state(self.state_0) diff --git a/newton/examples/mpm/example_mpm_viscous.py b/newton/examples/mpm/example_mpm_viscous.py index c1b53cb5d3..8ce3748599 100644 --- a/newton/examples/mpm/example_mpm_viscous.py +++ b/newton/examples/mpm/example_mpm_viscous.py @@ -98,6 +98,19 @@ def test_final(self): if below_funnel == 0: raise ValueError("No particles flowed through the funnel") + # Empirical centroid, bbox, and velocity checks (30 frames, --voxel-size 0.01) + velocities = self.state_0.particle_qd.numpy() + centroid = np.mean(positions, axis=0) + bbox = np.max(np.max(positions, axis=0) - np.min(positions, axis=0)) + max_vel = np.max(np.linalg.norm(velocities, axis=1)) + + assert abs(centroid[0] - 0.0) < 0.02, f"Centroid X out of range: {centroid[0]}" + assert abs(centroid[1] - 0.0) < 0.02, f"Centroid Y out of range: {centroid[1]}" + assert abs(centroid[2] - 0.30) < max(0.02, 0.30 * 0.05), f"Centroid Z out of range: {centroid[2]}" + assert bbox < 0.25 + max(0.05, 0.25 * 0.05), f"Bounding box too large: {bbox}" + assert max_vel < max(0.002, 1.52 * 1.5), f"Max velocity too high: {max_vel}" + assert np.min(positions[:, 2]) > 0.10 - 0.02, f"Min Z too low: {np.min(positions[:, 2])}" + def render(self): self.viewer.begin_frame(self.sim_time) self.viewer.log_state(self.state_0) diff --git a/newton/examples/multiphysics/example_softbody_dropping_to_cloth.py b/newton/examples/multiphysics/example_softbody_dropping_to_cloth.py index c0bcb450b7..914948ac3f 100644 --- a/newton/examples/multiphysics/example_softbody_dropping_to_cloth.py +++ b/newton/examples/multiphysics/example_softbody_dropping_to_cloth.py @@ -132,17 +132,32 @@ def step(self): self.sim_time += self.frame_dt def test_final(self): - # Test that bounding box size is reasonable (not exploding) + # Empirical state at 200 frames (CUDA, 6 runs): + # centroid x in [-0.02, 0.02], y in [0.04, 0.20], z in [0.92, 0.97] + # bbox_diag in [2.85, 3.67], min_z in [0.09, 0.88], max_vel in [1.0, 5.2] + # Cloth sag introduces run-to-run variation in y and z. particle_q = self.state_0.particle_q.numpy() + particle_qd = self.state_0.particle_qd.numpy() + min_pos = np.min(particle_q, axis=0) max_pos = np.max(particle_q, axis=0) + centroid = np.mean(particle_q, axis=0) bbox_size = np.linalg.norm(max_pos - min_pos) - # Check bbox size is reasonable (cloth stretches as soft body deforms it) - assert bbox_size < 20.0, f"Bounding box exploded: size={bbox_size:.2f}" + # Centroid check: wide tolerance to cover observed variation + assert abs(centroid[0]) < 0.10, f"Centroid x drifted: {centroid[0]:.3f}" + assert -0.05 < centroid[1] < 0.30, f"Centroid y out of range: {centroid[1]:.3f}" + assert 0.85 < centroid[2] < 1.05, f"Centroid z out of range: {centroid[2]:.3f}" + + # Bounding box diagonal check: observed up to 3.67, allow up to 4.0 + assert bbox_size < 4.0, f"Bounding box too large: {bbox_size:.2f}" + + # Min Z check: observed as low as 0.09 when cloth sags, allow down to -0.05 + assert min_pos[2] > -0.05, f"Excessive ground penetration: z_min={min_pos[2]:.4f}" - # Check no excessive penetration - assert min_pos[2] > -0.5, f"Excessive penetration: z_min={min_pos[2]:.4f}" + # Velocity check: observed up to 5.2, allow up to 8.0 + max_vel = np.max(np.linalg.norm(particle_qd, axis=1)) + assert max_vel < 8.0, f"Particle velocity too high: {max_vel:.2f}" def render(self): self.viewer.begin_frame(self.sim_time) diff --git a/newton/examples/multiphysics/example_softbody_gift.py b/newton/examples/multiphysics/example_softbody_gift.py index 2fabd975d4..e3802c9dc4 100644 --- a/newton/examples/multiphysics/example_softbody_gift.py +++ b/newton/examples/multiphysics/example_softbody_gift.py @@ -264,17 +264,32 @@ def step(self): self.sim_time += self.frame_dt def test_final(self): - # Test that bounding box size is reasonable (not exploding) + # Empirical state at 200 frames (CUDA, non-deterministic due to tumbling): + # local 5 runs: bbox_diag in [6.6, 7.3], min_z in [0.09, 3.9], + # max_vel in [3.8, 11.0], centroid z in [2.0, 5.9] + # CI g7e.2xlarge: centroid z as low as 1.23 (blocks spread wider) particle_q = self.state_0.particle_q.numpy() + particle_qd = self.state_0.particle_qd.numpy() + min_pos = np.min(particle_q, axis=0) max_pos = np.max(particle_q, axis=0) + centroid = np.mean(particle_q, axis=0) bbox_size = np.linalg.norm(max_pos - min_pos) - # Check bbox size is reasonable - assert bbox_size < 10.0, f"Bounding box exploded: size={bbox_size:.2f}" + # Bounding box diagonal check: observed max 7.3, allow up to 10.0 + assert bbox_size < 10.0, f"Bounding box too large: {bbox_size:.2f}" + + # Min Z check: observed min 0.09 across runs, allow down to -0.1 + assert min_pos[2] > -0.1, f"Excessive ground penetration: z_min={min_pos[2]:.4f}" + + # Centroid Z check: blocks always settle above ground + # CI observed 1.23 on g7e (blocks tumble and spread differently per GPU) + assert centroid[2] > 0.8, f"Centroid z too low (collapsed): {centroid[2]:.3f}" + assert centroid[2] < 7.0, f"Centroid z too high (exploding): {centroid[2]:.3f}" - # Check no excessive penetration - assert min_pos[2] > -0.5, f"Excessive penetration: z_min={min_pos[2]:.4f}" + # Velocity check: observed max 11.0, allow up to 16.5 + max_vel = np.max(np.linalg.norm(particle_qd, axis=1)) + assert max_vel < 16.5, f"Particle velocity too high: {max_vel:.2f}" def render(self): self.viewer.begin_frame(self.sim_time) diff --git a/newton/examples/robot/example_robot_allegro_hand.py b/newton/examples/robot/example_robot_allegro_hand.py index 907a12421b..54fd110c8a 100644 --- a/newton/examples/robot/example_robot_allegro_hand.py +++ b/newton/examples/robot/example_robot_allegro_hand.py @@ -212,8 +212,8 @@ def test_final(self): world_pos = wp.vec3(*self.initial_world_positions[i]) # Test hand bodies - must stay near initial position - hand_lower = world_pos - wp.vec3(0.5, 0.5, 0.5) - hand_upper = world_pos + wp.vec3(0.5, 0.5, 0.5) + hand_lower = world_pos - wp.vec3(0.3, 0.3, 0.3) + hand_upper = world_pos + wp.vec3(0.3, 0.3, 0.3) hand_body_indices = np.arange(num_bodies_per_world - 1, dtype=np.int32) + world_offset newton.examples.test_body_state( self.model, diff --git a/newton/examples/robot/example_robot_anymal_c_walk.py b/newton/examples/robot/example_robot_anymal_c_walk.py index 5eebccf014..ebb6bac675 100644 --- a/newton/examples/robot/example_robot_anymal_c_walk.py +++ b/newton/examples/robot/example_robot_anymal_c_walk.py @@ -331,7 +331,7 @@ def test_final(self): self.model, self.state_0, "all bodies are above the ground", - lambda q, qd: q[2] > 0.1, + lambda q, qd: q[2] > 0.2, ) newton.examples.test_body_state( diff --git a/newton/examples/robot/example_robot_anymal_d.py b/newton/examples/robot/example_robot_anymal_d.py index c7dfe329b8..8bf8eba73f 100644 --- a/newton/examples/robot/example_robot_anymal_d.py +++ b/newton/examples/robot/example_robot_anymal_d.py @@ -153,8 +153,7 @@ def test_final(self): self.model, self.state_0, "body velocities are small", - lambda q, qd: max(abs(qd)) - < 0.25, # Relaxed from 0.1 - collision pipeline has residual velocities up to ~0.2 + lambda q, qd: max(abs(qd)) < 0.01, ) # fmt: on diff --git a/newton/examples/robot/example_robot_g1.py b/newton/examples/robot/example_robot_g1.py index 26bc04f8c9..a73ef910b8 100644 --- a/newton/examples/robot/example_robot_g1.py +++ b/newton/examples/robot/example_robot_g1.py @@ -141,15 +141,14 @@ def test_final(self): self.model, self.state_0, "all bodies are above the ground", - lambda q, qd: q[2] > 0.0, + lambda q, qd: q[2] > -0.01, ) # fmt: off newton.examples.test_body_state( self.model, self.state_0, "all body velocities are small", - lambda q, qd: max(abs(qd)) - < 0.015, # Relaxed from 0.005 - G1 has higher residual velocities with collision pipeline + lambda q, qd: max(abs(qd)) < 0.002, ) # fmt: on diff --git a/newton/examples/robot/example_robot_h1.py b/newton/examples/robot/example_robot_h1.py index 04ef3df5c5..71f0a7e273 100644 --- a/newton/examples/robot/example_robot_h1.py +++ b/newton/examples/robot/example_robot_h1.py @@ -136,13 +136,13 @@ def test_final(self): self.model, self.state_0, "all bodies are above the ground", - lambda q, qd: q[2] > 0.0, + lambda q, qd: q[2] > -0.01, ) newton.examples.test_body_state( self.model, self.state_0, "all body velocities are small", - lambda q, qd: max(abs(qd)) < 5e-3, + lambda q, qd: max(abs(qd)) < 0.002, ) @staticmethod diff --git a/newton/examples/robot/example_robot_policy.py b/newton/examples/robot/example_robot_policy.py index b537808c58..d5bae11d2d 100644 --- a/newton/examples/robot/example_robot_policy.py +++ b/newton/examples/robot/example_robot_policy.py @@ -405,7 +405,15 @@ def test_final(self): self.model, self.state_0, "all bodies are above the ground", - lambda q, qd: q[2] > 0.0, + lambda q, qd: q[2] > -0.01, + ) + vel_min = wp.spatial_vector(-8.0, -8.0, -8.0, -8.0, -8.0, -8.0) + vel_max = wp.spatial_vector(8.0, 8.0, 8.0, 8.0, 8.0, 8.0) + newton.examples.test_body_state( + self.model, + self.state_0, + "body velocities are within bounds", + lambda q, qd: newton.math.vec_inside_limits(qd, vel_min, vel_max), ) diff --git a/newton/examples/robot/example_robot_ur10.py b/newton/examples/robot/example_robot_ur10.py index e96a101b4c..bd511717d3 100644 --- a/newton/examples/robot/example_robot_ur10.py +++ b/newton/examples/robot/example_robot_ur10.py @@ -193,7 +193,31 @@ def render(self): self.viewer.end_frame() def test_final(self): - pass + # All bodies above ground + newton.examples.test_body_state( + self.model, + self.state_0, + "all bodies are above the ground", + lambda q, qd: q[2] > 0.0, + ) + if self.device.is_cuda: + # After 500 frames the robot follows its trajectory smoothly + newton.examples.test_body_state( + self.model, + self.state_0, + "all body velocities are bounded", + lambda q, qd: max(abs(qd)) < 5.0, + ) + else: + # CPU runs only 10 frames; the robot is still accelerating from + # random initial configs, so velocities can be large. Only check + # the simulation hasn't diverged. + newton.examples.test_body_state( + self.model, + self.state_0, + "all body velocities are finite", + lambda q, qd: max(abs(qd)) < 100.0, + ) @staticmethod def create_parser(): diff --git a/newton/examples/selection/example_selection_articulations.py b/newton/examples/selection/example_selection_articulations.py index 3a73ba0bf9..e91dd217c7 100644 --- a/newton/examples/selection/example_selection_articulations.py +++ b/newton/examples/selection/example_selection_articulations.py @@ -290,7 +290,13 @@ def test_final(self): self.model, self.state_0, "all bodies are above the ground", - lambda q, qd: q[2] > 0.01, + lambda q, qd: q[2] > 0.1, + ) + newton.examples.test_body_state( + self.model, + self.state_0, + "all bodies have reasonable velocity", + lambda q, qd: wp.length(qd) < 10.0, ) @staticmethod diff --git a/newton/examples/selection/example_selection_materials.py b/newton/examples/selection/example_selection_materials.py index daac68eed5..b06f126688 100644 --- a/newton/examples/selection/example_selection_materials.py +++ b/newton/examples/selection/example_selection_materials.py @@ -250,8 +250,25 @@ def test_final(self): self.model, self.state_0, "all bodies are above the ground", - lambda q, qd: q[2] > 0.01, + lambda q, qd: q[2] > 0.5, ) + if wp.get_device().is_cuda: + # After 100 frames the ants have settled + newton.examples.test_body_state( + self.model, + self.state_0, + "all bodies have settled", + lambda q, qd: wp.length(qd) < 0.1, + ) + else: + # CPU runs only 10 frames; ants are still moving. + # Check that the simulation hasn't diverged. + newton.examples.test_body_state( + self.model, + self.state_0, + "all body velocities are finite", + lambda q, qd: wp.length(qd) < 100.0, + ) @staticmethod def create_parser(): diff --git a/newton/examples/selection/example_selection_multiple.py b/newton/examples/selection/example_selection_multiple.py index 4b9df68b36..0c0a4b73f1 100644 --- a/newton/examples/selection/example_selection_multiple.py +++ b/newton/examples/selection/example_selection_multiple.py @@ -268,7 +268,13 @@ def test_final(self): self.model, self.state_0, "all bodies are above the ground", - lambda q, qd: q[2] > 0.01, + lambda q, qd: q[2] > 0.1, + ) + newton.examples.test_body_state( + self.model, + self.state_0, + "all bodies have reasonable velocity", + lambda q, qd: wp.length(qd) < 10.0, ) @staticmethod diff --git a/newton/examples/sensors/example_sensor_imu.py b/newton/examples/sensors/example_sensor_imu.py index 9d5a8de396..28da0eb765 100644 --- a/newton/examples/sensors/example_sensor_imu.py +++ b/newton/examples/sensors/example_sensor_imu.py @@ -175,6 +175,20 @@ def test_final(self): np.testing.assert_allclose(np.linalg.norm(acc[i]), gravity_mag, rtol=0.05) assert abs(acc[i][expected_axis]) > gravity_mag * 0.95 + # Verify cubes have settled at rest on the ground + newton.examples.test_body_state( + self.model, + self.state_0, + "cubes resting at correct height", + lambda q, qd: wp.abs(q[2] - 0.2) < 0.05, + ) + newton.examples.test_body_state( + self.model, + self.state_0, + "cubes have zero velocity", + lambda q, qd: wp.length(qd) < 1e-3, + ) + def render(self): self.viewer.begin_frame(self.sim_time) self.viewer.log_state(self.state_0) diff --git a/newton/examples/sensors/example_sensor_tiled_camera.py b/newton/examples/sensors/example_sensor_tiled_camera.py index 39189fd509..78a26a1b66 100644 --- a/newton/examples/sensors/example_sensor_tiled_camera.py +++ b/newton/examples/sensors/example_sensor_tiled_camera.py @@ -429,6 +429,20 @@ def test_final(self): assert depth_image.shape == (24, 1, self.sensor_render_height, self.sensor_render_width) assert depth_image.min() < depth_image.max() + # Verify all bodies are above the ground and fully settled + newton.examples.test_body_state( + self.model, + self.state, + "all bodies above ground", + lambda q, qd: q[2] >= 0.0, + ) + newton.examples.test_body_state( + self.model, + self.state, + "all bodies have zero velocity", + lambda q, qd: wp.length(qd) < 1e-3, + ) + def gui(self, ui): show_compile_kernel_info = False diff --git a/newton/examples/softbody/example_softbody_franka.py b/newton/examples/softbody/example_softbody_franka.py index b5f329b15a..53c40df7ff 100644 --- a/newton/examples/softbody/example_softbody_franka.py +++ b/newton/examples/softbody/example_softbody_franka.py @@ -372,23 +372,51 @@ def render(self): self.viewer.end_frame() def test_final(self): - p_lower = wp.vec3(-0.5, -1.0, -0.05) - p_upper = wp.vec3(0.5, 0.0, 0.6) + # Empirical state at 1000 frames (CUDA): + # duck particles: centroid=[-0.001, -0.500, 0.228], bbox_diag=0.117 + # min_pos=[-0.031, -0.538, 0.201], max_pos=[0.034, -0.455, 0.252] + # max_vel=0.021, max_body_vel=4.6e-5 + particle_q = self.state_0.particle_q.numpy() + particle_qd = self.state_0.particle_qd.numpy() + + centroid = np.mean(particle_q, axis=0) + min_pos = np.min(particle_q, axis=0) + max_pos = np.max(particle_q, axis=0) + bbox_size = np.linalg.norm(max_pos - min_pos) + + # Centroid check: observed [-0.001, -0.500, 0.228] + expected_centroid = np.array([-0.001, -0.500, 0.228]) + centroid_tol = np.maximum(0.02, np.abs(expected_centroid) * 0.05) + for i, axis in enumerate("xyz"): + assert abs(centroid[i] - expected_centroid[i]) < centroid_tol[i], ( + f"Duck centroid {axis} drifted: {centroid[i]:.4f} vs expected {expected_centroid[i]:.4f}" + ) + + # Bounding box diagonal check: observed 0.117, allow up to 0.17 + assert bbox_size < 0.17, f"Duck bounding box too large: {bbox_size:.4f}" + + # Min Z check: observed 0.201, allow down to 0.18 + assert min_pos[2] > 0.18, f"Duck ground penetration: z_min={min_pos[2]:.4f}" + + # Tightened particle position bounds (duck sits on table near origin) + p_lower = wp.vec3(-0.05, -0.56, 0.18) + p_upper = wp.vec3(0.06, -0.44, 0.28) newton.examples.test_particle_state( self.state_0, - "particles are within a reasonable volume", + "duck particles are within the expected 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)) < 2.0, - ) + + # Particle velocity check: observed max 0.021, allow up to 0.06 + max_vel = np.max(np.linalg.norm(particle_qd, axis=1)) + assert max_vel < 0.06, f"Particle velocity too high: {max_vel:.4f}" + + # Body velocity check: observed max 4.6e-5, allow up to 0.002 newton.examples.test_body_state( self.model, self.state_0, - "body velocities are within a reasonable range", - lambda q, qd: max(abs(qd)) < 0.7, + "robot body velocities are within expected range", + lambda q, qd: max(abs(qd)) < 0.002, ) diff --git a/newton/examples/softbody/example_softbody_hanging.py b/newton/examples/softbody/example_softbody_hanging.py index 0073fac5ff..a50b454229 100644 --- a/newton/examples/softbody/example_softbody_hanging.py +++ b/newton/examples/softbody/example_softbody_hanging.py @@ -12,6 +12,7 @@ # ########################################################################### +import numpy as np import warp as wp import newton @@ -119,19 +120,44 @@ def step(self): self.sim_time += self.frame_dt def test_final(self): - # Test that particles are in a reasonable range (soft body may settle or deform) - # We check that they haven't exploded or collapsed completely - # 4 grids, each roughly 1.2 x 0.4 x 0.4 in size, positioned along Y-axis - # Initial positions: Y from 1.0 to ~3.2, X from 0 to 1.2, Z around 1.0 to 1.4 - # With fix_left=True, grids hang and sag significantly towards the ground - p_lower = wp.vec3(-1.0, -0.5, 0.0) - p_upper = wp.vec3(3.0, 4.0, 3.0) + # Empirical state at 120 frames (CUDA, deterministic): + # centroid=[0.57, 2.10, 1.03], bbox_diag=2.78, max_vel=1.26 + # min_pos=[0.0, 1.0, 0.23], max_pos=[1.23, 3.20, 1.40] + particle_q = self.state_0.particle_q.numpy() + particle_qd = self.state_0.particle_qd.numpy() + + min_pos = np.min(particle_q, axis=0) + max_pos = np.max(particle_q, axis=0) + centroid = np.mean(particle_q, axis=0) + + # Centroid check: observed [0.57, 2.10, 1.03], tolerance ±max(0.02, |val|*0.05) + expected_centroid = np.array([0.57, 2.10, 1.03]) + centroid_tol = np.maximum(0.02, np.abs(expected_centroid) * 0.05) + for i, axis in enumerate("xyz"): + assert abs(centroid[i] - expected_centroid[i]) < centroid_tol[i], ( + f"Centroid {axis} drifted: {centroid[i]:.3f} vs expected {expected_centroid[i]:.3f}" + ) + + # Bounding box diagonal check: observed 2.78, allow up to 2.92 + bbox_size = np.linalg.norm(max_pos - min_pos) + assert bbox_size < 2.92, f"Bounding box too large: {bbox_size:.2f}" + + # Particle position bounds (tightened from observed min/max with small margin) + p_lower = wp.vec3(-0.02, 0.98, 0.21) + p_upper = wp.vec3(1.25, 3.22, 1.42) newton.examples.test_particle_state( self.state_0, - "particles are within a reasonable volume", + "particles are within the expected volume", lambda q, _qd: newton.math.vec_inside_limits(q, p_lower, p_upper), ) + # Min Z check: observed 0.23, allow down to 0.21 + assert min_pos[2] > 0.21, f"Excessive ground penetration: z_min={min_pos[2]:.4f}" + + # Velocity check: observed max 1.26, allow up to 1.89 + max_vel = np.max(np.linalg.norm(particle_qd, axis=1)) + assert max_vel < 1.89, f"Particle velocity too high: {max_vel:.2f}" + def render(self): self.viewer.begin_frame(self.sim_time) self.viewer.log_state(self.state_0)