diff --git a/docs/api/newton.rst b/docs/api/newton.rst index 00d7c1207..803a3baea 100644 --- a/docs/api/newton.rst +++ b/docs/api/newton.rst @@ -60,7 +60,7 @@ newton * - Name - Value - * - JOINT_LIMIT_UNLIMITED - - 10000000000.0 + * - MAXVAL + - 1e10 * - __version__ - 0.2.0 diff --git a/newton/__init__.py b/newton/__init__.py index d45eb5161..b6c002d5b 100644 --- a/newton/__init__.py +++ b/newton/__init__.py @@ -17,12 +17,14 @@ # core # ================================================================================== from ._src.core import ( + MAXVAL, Axis, AxisType, ) from ._version import __version__ __all__ = [ + "MAXVAL", "Axis", "AxisType", "__version__", @@ -53,7 +55,6 @@ # sim # ================================================================================== from ._src.sim import ( # noqa: E402 - JOINT_LIMIT_UNLIMITED, BroadPhaseMode, CollisionPipeline, CollisionPipelineUnified, @@ -73,7 +74,6 @@ ) __all__ += [ - "JOINT_LIMIT_UNLIMITED", "BroadPhaseMode", "CollisionPipeline", "CollisionPipelineUnified", diff --git a/newton/_src/core/__init__.py b/newton/_src/core/__init__.py index 57d1b4c02..afafe0076 100644 --- a/newton/_src/core/__init__.py +++ b/newton/_src/core/__init__.py @@ -26,11 +26,13 @@ velocity_at_point, ) from .types import ( + MAXVAL, Axis, AxisType, ) __all__ = [ + "MAXVAL", "Axis", "AxisType", "quat_between_axes", diff --git a/newton/_src/core/types.py b/newton/_src/core/types.py index f732f31e0..fd4f9bca5 100644 --- a/newton/_src/core/types.py +++ b/newton/_src/core/types.py @@ -65,6 +65,15 @@ def flag_to_int(flag): # Warp vector types vec5 = wp.types.vector(length=5, dtype=wp.float32) +# Large finite value used as sentinel (matches MuJoCo's mjMAXVAL) +MAXVAL = 1e10 +"""Large finite sentinel value for 'no limit' / 'no hit' / 'invalid' markers. + +Use this instead of infinity to avoid verify_fp false positives. +For comparisons with volume-sampled data, use `>= wp.static(MAXVAL * 0.99)` to handle +interpolation-induced floating-point errors. +""" + class Axis(IntEnum): """Enumeration of axes in 3D space.""" @@ -181,6 +190,7 @@ def axis_to_vec3(axis: AxisType | Vec3) -> wp.vec3: __all__ = [ + "MAXVAL", "Axis", "AxisType", "Devicelike", diff --git a/newton/_src/geometry/collision_primitive.py b/newton/_src/geometry/collision_primitive.py index b479e085b..22dc1911a 100644 --- a/newton/_src/geometry/collision_primitive.py +++ b/newton/_src/geometry/collision_primitive.py @@ -34,13 +34,15 @@ Returns (single contact): (distance: float, position: vec3, normal: vec3) Returns (multi contact): (distances: vecN, positions: matNx3, normals: vecN or matNx3) - Use wp.inf for unpopulated contact slots. + Use MAXVAL for unpopulated contact slots. """ from typing import Any import warp as wp +from newton._src.core.types import MAXVAL + # Local type definitions for use within kernels _vec8f = wp.types.vector(8, wp.float32) _mat23f = wp.types.matrix((2, 3), wp.float32) @@ -221,11 +223,11 @@ def collide_capsule_capsule( Returns: Tuple containing: - contact_dist: Vector of contact distances (wp.inf for invalid contacts) + contact_dist: Vector of contact distances (MAXVAL for invalid contacts) contact_pos: Matrix of contact positions (one per row) contact_normal: Shared contact normal vector (from capsule 1 toward capsule 2) """ - contact_dist = wp.vec2(wp.inf, wp.inf) + contact_dist = wp.vec2(MAXVAL, MAXVAL) contact_pos = _mat23f() contact_normal = wp.vec3() @@ -400,7 +402,7 @@ def collide_plane_box( Returns: Tuple containing: - contact_dist: Vector of contact distances (wp.inf for unpopulated contacts) + contact_dist: Vector of contact distances (MAXVAL for unpopulated contacts) contact_pos: Matrix of contact positions (one per row) contact_normal: contact normal vector """ @@ -408,7 +410,7 @@ def collide_plane_box( corner = wp.vec3() center_dist = wp.dot(box_pos - plane_pos, plane_normal) - dist = wp.vec4(wp.inf) + dist = wp.vec4(MAXVAL) pos = _mat43f() # test all corners, pick bottom 4 @@ -540,7 +542,7 @@ def collide_plane_cylinder( """ # Initialize output matrices - contact_dist = wp.vec4(wp.inf) + contact_dist = wp.vec4(MAXVAL) contact_pos = _mat43f() contact_count = 0 @@ -668,7 +670,7 @@ def collide_box_box( Returns: Tuple containing: - contact_dist: Vector of contact distances (wp.inf for unpopulated contacts) + contact_dist: Vector of contact distances (MAXVAL for unpopulated contacts) contact_pos: Matrix of contact positions (one per row) contact_normals: Matrix of contact normal vectors (one per row) """ @@ -676,7 +678,7 @@ def collide_box_box( # Initialize output matrices contact_dist = _vec8f() for i in range(8): - contact_dist[i] = wp.inf + contact_dist[i] = MAXVAL contact_pos = _mat83f() contact_normals = _mat83f() contact_count = 0 @@ -1182,7 +1184,7 @@ def collide_capsule_box( Returns: Tuple containing: - contact_dist: Vector of contact distances (wp.inf for unpopulated contacts) + contact_dist: Vector of contact distances (MAXVAL for unpopulated contacts) contact_pos: Matrix of contact positions (one per row) contact_normals: Matrix of contact normal vectors (one per row) """ @@ -1357,7 +1359,7 @@ def collide_capsule_box( c1 = wp.where((ee2 > 0) == w_neg, 1, 2) if cltype == -4: # invalid type - return wp.vec2(wp.inf), _mat23f(), _mat23f() + return wp.vec2(MAXVAL), _mat23f(), _mat23f() if cltype >= 0 and cltype // 3 != 1: # closest to a corner of the box c1 = axisdir ^ clcorner @@ -1488,7 +1490,7 @@ def collide_capsule_box( # collide with sphere using core function dist2, pos2, normal2 = collide_sphere_box(s2_pos_g, capsule_radius, box_pos, box_rot, box_size) else: - dist2 = wp.inf + dist2 = MAXVAL pos2 = wp.vec3() normal2 = wp.vec3() diff --git a/newton/_src/geometry/kernels.py b/newton/_src/geometry/kernels.py index 58dcf5f8e..84d4ca252 100644 --- a/newton/_src/geometry/kernels.py +++ b/newton/_src/geometry/kernels.py @@ -15,6 +15,8 @@ import warp as wp +from newton._src.core.types import MAXVAL + from . import collision_primitive as primitive from .broad_phase_common import binary_search from .flags import ParticleFlags, ShapeFlags @@ -2112,8 +2114,8 @@ def handle_contact_pairs( # Flip the normal since we flipped the arguments normal = -neg_normal - # Check if this contact point is valid (primitive function returns wp.inf for invalid contacts) - if distance >= 1.0e5: # Use a reasonable threshold instead of exact wp.inf comparison + # Check if this contact point is valid (primitive function returns MAXVAL for invalid contacts) + if distance >= MAXVAL: return elif ( diff --git a/newton/_src/geometry/narrow_phase.py b/newton/_src/geometry/narrow_phase.py index 4394d5683..f1b5fc202 100644 --- a/newton/_src/geometry/narrow_phase.py +++ b/newton/_src/geometry/narrow_phase.py @@ -20,6 +20,8 @@ import warp as wp +from newton._src.core.types import MAXVAL + from ..geometry.collision_core import ( ENABLE_TILE_BVH_QUERY, check_infinite_plane_bsphere_overlap, @@ -375,7 +377,7 @@ def narrow_phase_primitive_kernel( contact_pos_0 = wp.vec3(positions[0, 0], positions[0, 1], positions[0, 2]) # Check if second contact is valid (parallel axes case) - if dists[1] < wp.inf: + if dists[1] < MAXVAL: contact_dist_1 = dists[1] contact_pos_1 = wp.vec3(positions[1, 0], positions[1, 1], positions[1, 2]) num_contacts = 2 diff --git a/newton/_src/geometry/sdf_contact.py b/newton/_src/geometry/sdf_contact.py index ae6b74269..4816c443b 100644 --- a/newton/_src/geometry/sdf_contact.py +++ b/newton/_src/geometry/sdf_contact.py @@ -17,6 +17,8 @@ import warp as wp +from newton._src.core.types import MAXVAL + from ..geometry.contact_data import ContactData from ..geometry.sdf_utils import SDFData @@ -200,7 +202,7 @@ def sample_sdf_extrapolated( sparse_idx = wp.volume_world_to_index(sdf_data.sparse_sdf_ptr, sdf_pos) sparse_dist = wp.volume_sample_f(sdf_data.sparse_sdf_ptr, sparse_idx, wp.Volume.LINEAR) - if sparse_dist >= wp.inf or wp.isnan(sparse_dist): + if sparse_dist >= wp.static(MAXVAL * 0.99) or wp.isnan(sparse_dist): # Fallback to coarse grid when sparse sample is diluted by background coarse_idx = wp.volume_world_to_index(sdf_data.coarse_sdf_ptr, sdf_pos) return wp.volume_sample_f(sdf_data.coarse_sdf_ptr, coarse_idx, wp.Volume.LINEAR) @@ -208,7 +210,7 @@ def sample_sdf_extrapolated( return sparse_dist else: # Point is outside extent - project to boundary - eps = 1e-2 * sdf_data.sparse_voxel_size # slightly shrink to avoid sampling NaN + eps = 1e-2 * sdf_data.sparse_voxel_size # slightly shrink to avoid sampling background clamped_pos = wp.min(wp.max(sdf_pos, lower + eps), upper - eps) dist_to_boundary = wp.length(sdf_pos - clamped_pos) @@ -260,7 +262,7 @@ def sample_sdf_grad_extrapolated( sparse_idx = wp.volume_world_to_index(sdf_data.sparse_sdf_ptr, sdf_pos) sparse_dist = wp.volume_sample_grad_f(sdf_data.sparse_sdf_ptr, sparse_idx, wp.Volume.LINEAR, gradient) - if sparse_dist >= wp.inf or wp.isnan(sparse_dist): + if sparse_dist >= wp.static(MAXVAL * 0.99) or wp.isnan(sparse_dist): # Fallback to coarse grid when sparse sample is diluted by background coarse_idx = wp.volume_world_to_index(sdf_data.coarse_sdf_ptr, sdf_pos) coarse_dist = wp.volume_sample_grad_f(sdf_data.coarse_sdf_ptr, coarse_idx, wp.Volume.LINEAR, gradient) diff --git a/newton/_src/geometry/sdf_hydroelastic.py b/newton/_src/geometry/sdf_hydroelastic.py index 2f8b87534..37fad707d 100644 --- a/newton/_src/geometry/sdf_hydroelastic.py +++ b/newton/_src/geometry/sdf_hydroelastic.py @@ -20,6 +20,8 @@ import warp as wp +from newton._src.core.types import MAXVAL + from ..sim.model import Model from .collision_core import sat_box_intersection from .contact_data import ContactData @@ -963,7 +965,9 @@ def sdf_diff_sdf( valB = sample_sdf_extrapolated(sdfB_data, pointB) - is_valid = not (valA >= wp.inf or wp.isnan(valA) or valB >= wp.inf or wp.isnan(valB)) + is_valid = not ( + valA >= wp.static(MAXVAL * 0.99) or wp.isnan(valA) or valB >= wp.static(MAXVAL * 0.99) or wp.isnan(valB) + ) if valA < 0 and valB < 0: diff = k_eff_a * valA - k_eff_b * valB @@ -995,7 +999,9 @@ def sdf_diff_sdf( valB = sample_sdf_extrapolated(sdfB_data, pointB) - is_valid = not (valA >= wp.inf or wp.isnan(valA) or valB >= wp.inf or wp.isnan(valB)) + is_valid = not ( + valA >= wp.static(MAXVAL * 0.99) or wp.isnan(valA) or valB >= wp.static(MAXVAL * 0.99) or wp.isnan(valB) + ) if valA < 0 and valB < 0: diff = k_eff_a * valA - k_eff_b * valB diff --git a/newton/_src/geometry/sdf_mc.py b/newton/_src/geometry/sdf_mc.py index b141bea09..cb25590ef 100644 --- a/newton/_src/geometry/sdf_mc.py +++ b/newton/_src/geometry/sdf_mc.py @@ -27,6 +27,8 @@ import numpy as np import warp as wp +from newton._src.core.types import MAXVAL + #: Corner values for a single voxel (8 corners) vec8f = wp.types.vector(length=8, dtype=wp.float32) @@ -183,7 +185,7 @@ def mc_calc_face( p_scaled = wp.volume_index_to_world(sdf_a, vol_idx) face_verts[vi] = p_scaled depth = wp.volume_sample_f(sdf_a, vol_idx, wp.Volume.LINEAR) - if depth >= wp.inf or wp.isnan(depth): + if depth >= wp.static(MAXVAL * 0.99) or wp.isnan(depth): depth = 0.0 vert_depths[vi] = -depth if depth < 0.0: diff --git a/newton/_src/geometry/sdf_utils.py b/newton/_src/geometry/sdf_utils.py index 768d2cc29..03ea9493e 100644 --- a/newton/_src/geometry/sdf_utils.py +++ b/newton/_src/geometry/sdf_utils.py @@ -18,6 +18,7 @@ import numpy as np import warp as wp +from ..core.types import MAXVAL from ..geometry.kernels import box_sdf, capsule_sdf, cone_sdf, cylinder_sdf, ellipsoid_sdf, sphere_sdf from .sdf_mc import get_mc_tables, int_to_vec3f, mc_calc_face, vec8f from .types import GeoType, Mesh @@ -52,9 +53,9 @@ class SDFData: # Default background value for unallocated voxels in sparse SDF. -# Using inf ensures any trilinear interpolation with unallocated voxels produces inf or NaN, -# allowing detection of unallocated voxels. -SDF_BACKGROUND_VALUE = wp.inf +# Using a large finite value ensures any trilinear interpolation with unallocated voxels +# produces a large value, allowing detection of unallocated voxels. +SDF_BACKGROUND_VALUE = MAXVAL def create_empty_sdf_data() -> SDFData: diff --git a/newton/_src/sensors/sensor_tiled_camera.py b/newton/_src/sensors/sensor_tiled_camera.py index d71a52547..7e83a1266 100644 --- a/newton/_src/sensors/sensor_tiled_camera.py +++ b/newton/_src/sensors/sensor_tiled_camera.py @@ -21,6 +21,7 @@ import numpy as np import warp as wp +from ..core.types import MAXVAL from ..geometry import ShapeFlags from ..sim import Model, State from .warp_raytrace import ClearData, RenderContext, RenderLightType, RenderShapeType @@ -52,8 +53,8 @@ def convert_newton_transform( def compute_mesh_bounds(in_meshes: wp.array(dtype=wp.uint64), out_bounds: wp.array2d(dtype=wp.vec3f)): tid = wp.tid() - min_point = wp.vec3(wp.inf) - max_point = wp.vec3(-wp.inf) + min_point = wp.vec3(MAXVAL) + max_point = wp.vec3(-MAXVAL) if in_meshes[tid] != 0: mesh = wp.mesh_get(in_meshes[tid]) diff --git a/newton/_src/sensors/warp_raytrace/bvh.py b/newton/_src/sensors/warp_raytrace/bvh.py index 635167d19..a074d4988 100644 --- a/newton/_src/sensors/warp_raytrace/bvh.py +++ b/newton/_src/sensors/warp_raytrace/bvh.py @@ -15,6 +15,8 @@ import warp as wp +from newton._src.core.types import MAXVAL + from .types import RenderShapeType @@ -25,8 +27,8 @@ def compute_mesh_bounds( mesh_min_bounds = wp.cw_mul(mesh_min_bounds, scale) mesh_max_bounds = wp.cw_mul(mesh_max_bounds, scale) - min_bound = wp.vec3f(wp.inf) - max_bound = wp.vec3f(-wp.inf) + min_bound = wp.vec3f(MAXVAL) + max_bound = wp.vec3f(-MAXVAL) corner_1 = wp.transform_point(transform, wp.vec3f(mesh_min_bounds[0], mesh_min_bounds[1], mesh_min_bounds[2])) min_bound = wp.min(min_bound, corner_1) @@ -65,8 +67,8 @@ def compute_mesh_bounds( @wp.func def compute_box_bounds(transform: wp.transformf, size: wp.vec3f) -> tuple[wp.vec3f, wp.vec3f]: - min_bound = wp.vec3f(wp.inf) - max_bound = wp.vec3f(-wp.inf) + min_bound = wp.vec3f(MAXVAL) + max_bound = wp.vec3f(-MAXVAL) for x in range(2): for y in range(2): @@ -117,8 +119,8 @@ def compute_plane_bounds(transform: wp.transformf, size: wp.vec3f) -> tuple[wp.v if size[0] <= 0.0 or size[1] <= 0.0: size_scale = 1000.0 - min_bound = wp.vec3f(wp.inf) - max_bound = wp.vec3f(-wp.inf) + min_bound = wp.vec3f(MAXVAL) + max_bound = wp.vec3f(-MAXVAL) for x in range(2): for y in range(2): diff --git a/newton/_src/sensors/warp_raytrace/lighting.py b/newton/_src/sensors/warp_raytrace/lighting.py index 8602cb242..87caebfac 100644 --- a/newton/_src/sensors/warp_raytrace/lighting.py +++ b/newton/_src/sensors/warp_raytrace/lighting.py @@ -15,6 +15,8 @@ import warp as wp +from newton._src.core.types import MAXVAL + from . import ray_cast @@ -54,7 +56,7 @@ def compute_lighting( return light_contribution L = wp.vec3f(0.0, 0.0, 0.0) - dist_to_light = wp.float32(wp.inf) + dist_to_light = wp.float32(MAXVAL) attenuation = wp.float32(1.0) if light_type == 1: # directional light diff --git a/newton/_src/sensors/warp_raytrace/ray.py b/newton/_src/sensors/warp_raytrace/ray.py index 79f6514f1..9c4efae39 100644 --- a/newton/_src/sensors/warp_raytrace/ray.py +++ b/newton/_src/sensors/warp_raytrace/ray.py @@ -17,6 +17,8 @@ import warp as wp +from newton._src.core.types import MAXVAL + class vec6f(wp.types.vector(length=6, dtype=wp.float32)): pass @@ -66,7 +68,7 @@ def ray_compute_quadratic(a: wp.float32, b: wp.float32, c: wp.float32) -> tuple[ """Compute solutions from quadratic: a*x^2 + 2*b*x + c = 0.""" det = b * b - a * c if det < EPSILON: - return wp.inf, wp.vec2f(wp.inf, wp.inf) + return MAXVAL, wp.vec2f(MAXVAL, MAXVAL) det = wp.sqrt(det) # compute the two solutions @@ -81,7 +83,7 @@ def ray_compute_quadratic(a: wp.float32, b: wp.float32, c: wp.float32) -> tuple[ elif x1 >= 0.0: return x1, x else: - return wp.inf, x + return MAXVAL, x @wp.func @@ -99,12 +101,12 @@ def ray_plane( # z-vec not pointing towards front face: reject if enable_backface_culling and ray_direction_local[2] > -EPSILON: - return wp.inf + return MAXVAL # intersection with plane t_hit = -ray_origin_local[2] / ray_direction_local[2] if t_hit < 0.0: - return wp.inf + return MAXVAL p = wp.vec2f( ray_origin_local[0] + t_hit * ray_direction_local[0], ray_origin_local[1] + t_hit * ray_direction_local[1] @@ -114,7 +116,7 @@ def ray_plane( if (size[0] <= 0.0 or wp.abs(p[0]) <= size[0]) and (size[1] <= 0.0 or wp.abs(p[1]) <= size[1]): return t_hit else: - return wp.inf + return MAXVAL @wp.func @@ -127,8 +129,8 @@ def ray_plane_with_normal( ) -> tuple[wp.bool, wp.float32, wp.vec3f]: """Returns distance and normal at which a ray intersects with a plane.""" t_hit = ray_plane(transform, size, enable_backface_culling, ray_origin_world, ray_direction_world) - if t_hit == wp.inf: - return False, wp.inf, wp.vec3f(0.0, 0.0, 0.0) + if t_hit >= MAXVAL: + return False, MAXVAL, wp.vec3f(0.0, 0.0, 0.0) # Local plane normal is +Z; rotate to world space normal_world = wp.transform_vector(transform, wp.vec3f(0.0, 0.0, 1.0)) normal_world = wp.normalize(normal_world) @@ -156,8 +158,8 @@ def ray_sphere_with_normal( ) -> tuple[wp.bool, wp.float32, wp.vec3f]: """Returns distance and normal at which a ray intersects with a sphere.""" t_hit = ray_sphere(pos, dist_sqr, ray_origin_world, ray_direction_world) - if t_hit == wp.inf: - return False, wp.inf, wp.vec3f(0.0, 0.0, 0.0) + if t_hit >= MAXVAL: + return False, MAXVAL, wp.vec3f(0.0, 0.0, 0.0) normal = wp.normalize(ray_origin_world + t_hit * ray_direction_world - pos) return True, t_hit, normal @@ -170,15 +172,15 @@ def ray_capsule( # bounding sphere test ssz = size[0] + size[1] - if ray_sphere(wp.transform_get_translation(transform), ssz * ssz, ray_origin_world, ray_direction_world) == wp.inf: - return wp.inf + if ray_sphere(wp.transform_get_translation(transform), ssz * ssz, ray_origin_world, ray_direction_world) >= MAXVAL: + return MAXVAL # map to local frame ray_origin_local, ray_direction_local = map_ray_to_local(transform, ray_origin_world, ray_direction_world) d_len_sq = wp.dot(ray_direction_local, ray_direction_local) if d_len_sq < EPSILON: - return wp.inf + return MAXVAL inv_d_len = 1.0 / wp.sqrt(d_len_sq) d_local_norm = ray_direction_local * inv_d_len @@ -244,7 +246,7 @@ def ray_capsule( if min_t < 1.0e9: return min_t * inv_d_len - return wp.inf + return MAXVAL @wp.func @@ -253,8 +255,8 @@ def ray_capsule_with_normal( ) -> tuple[wp.bool, wp.float32, wp.vec3f]: """Returns distance and normal at which a ray intersects with a capsule.""" t_hit = ray_capsule(transform, size, ray_origin_world, ray_direction_world) - if t_hit == wp.inf: - return False, wp.inf, wp.vec3f(0.0, 0.0, 0.0) + if t_hit >= MAXVAL: + return False, MAXVAL, wp.vec3f(0.0, 0.0, 0.0) # Compute continuous normal: vector from closest point on axis segment to the hit point ray_origin_local, ray_direction_local = map_ray_to_local(transform, ray_origin_world, ray_direction_world) @@ -297,15 +299,15 @@ def ray_cylinder( """Returns the distance at which a ray intersects with a cylinder.""" # bounding sphere test ssz = size[0] * size[0] + size[1] * size[1] - if ray_sphere(wp.transform_get_translation(transform), ssz, ray_origin_world, ray_direction_world) == wp.inf: - return wp.inf, 0 + if ray_sphere(wp.transform_get_translation(transform), ssz, ray_origin_world, ray_direction_world) >= MAXVAL: + return MAXVAL, 0 # map to local frame ray_origin_local, ray_direction_local = map_ray_to_local(transform, ray_origin_world, ray_direction_world) radius = size[0] height = size[1] - t_hit = wp.inf + t_hit = MAXVAL min_t = 1.0e10 side = 0 @@ -365,8 +367,8 @@ def ray_cylinder_with_normal( ) -> tuple[wp.bool, wp.float32, wp.vec3f]: """Returns distance and normal at which a ray intersects with a cylinder.""" t_hit, hit_side = ray_cylinder(transform, size, ray_origin_world, ray_direction_world) - if t_hit == wp.inf: - return False, wp.inf, wp.vec3f(0.0, 0.0, 0.0) + if t_hit >= MAXVAL: + return False, MAXVAL, wp.vec3f(0.0, 0.0, 0.0) # Compute continuous normal: vector from closest point on axis segment to the hit point ray_origin_local, ray_direction_local = map_ray_to_local(transform, ray_origin_world, ray_direction_world) hit_local = ray_origin_local + t_hit * ray_direction_local @@ -389,8 +391,8 @@ def ray_cone( """Returns the distance at which a ray intersects with a cone.""" # bounding sphere test ssz = size[0] * size[0] + size[1] * size[1] - if ray_sphere(wp.transform_get_translation(transform), ssz, ray_origin_world, ray_direction_world) == wp.inf: - return wp.inf + if ray_sphere(wp.transform_get_translation(transform), ssz, ray_origin_world, ray_direction_world) >= MAXVAL: + return MAXVAL # map to local frame ray_origin_local, ray_direction_local = map_ray_to_local(transform, ray_origin_world, ray_direction_world) @@ -435,16 +437,16 @@ def ray_cone( h = k1 * k1 - k2 * k0 if h < 0.0: - return wp.inf # no intersection + return MAXVAL # no intersection if wp.abs(k2) < EPSILON: - return wp.inf # degenerate case + return MAXVAL # degenerate case t_hit = (-k1 - wp.sqrt(h)) / k2 y = m1 + t_hit * m2 if y < 0.0 or y > m0: - return wp.inf # no intersection + return MAXVAL # no intersection return t_hit @@ -455,8 +457,8 @@ def ray_cone_with_normal( ) -> tuple[wp.bool, wp.float32, wp.vec3f]: """Returns distance and normal at which a ray intersects with a cone.""" t_hit = ray_cone(transform, size, ray_origin_world, ray_direction_world) - if t_hit == wp.inf: - return False, wp.inf, wp.vec3f(0.0, 0.0, 0.0) + if t_hit >= MAXVAL: + return False, MAXVAL, wp.vec3f(0.0, 0.0, 0.0) ray_origin_local, ray_direction_local = map_ray_to_local(transform, ray_origin_world, ray_direction_world) hit_local = ray_origin_local + t_hit * ray_direction_local @@ -495,14 +497,14 @@ def ray_box( # bounding sphere test ssz = wp.dot(size, size) - if ray_sphere(wp.transform_get_translation(transform), ssz, ray_origin_world, ray_direction_world) == wp.inf: - return wp.inf, all + if ray_sphere(wp.transform_get_translation(transform), ssz, ray_origin_world, ray_direction_world) >= MAXVAL: + return MAXVAL, all # map to local frame ray_origin_local, ray_direction_local = map_ray_to_local(transform, ray_origin_world, ray_direction_world) # init solution - t_hit = wp.inf + t_hit = MAXVAL # loop over axes with non-zero vec for i in range(3): @@ -538,8 +540,8 @@ def ray_box_with_normal( ) -> tuple[wp.bool, wp.float32, wp.vec3f]: """Returns distance and normal at which a ray intersects with a box.""" t_hit, all = ray_box(transform, size, ray_origin_world, ray_direction_world) - if t_hit == wp.inf: - return False, wp.inf, wp.vec3f(0.0, 0.0, 0.0) + if t_hit >= MAXVAL: + return False, MAXVAL, wp.vec3f(0.0, 0.0, 0.0) # Select the face by matching the closest intersection time among the 6 faces normal_local = wp.vec3f(0.0, 0.0, 0.0) @@ -576,7 +578,7 @@ def ray_mesh( if not enable_backface_culling or wp.dot(ray_direction_world, query.normal) < 0.0: return True, query.t, wp.normalize(query.normal), query.u, query.v, query.face - return False, wp.inf, wp.vec3f(0.0, 0.0, 0.0), 0.0, 0.0, -1 + return False, MAXVAL, wp.vec3f(0.0, 0.0, 0.0), 0.0, 0.0, -1 @wp.func @@ -608,4 +610,4 @@ def ray_mesh_with_bvh( normal = wp.normalize(normal) return True, query.t, normal, query.u, query.v, query.face, mesh_shape_id - return False, wp.inf, wp.vec3f(0.0, 0.0, 0.0), 0.0, 0.0, -1, -1 + return False, MAXVAL, wp.vec3f(0.0, 0.0, 0.0), 0.0, 0.0, -1, -1 diff --git a/newton/_src/sensors/warp_raytrace/ray_cast.py b/newton/_src/sensors/warp_raytrace/ray_cast.py index 46554e887..c5f37837c 100644 --- a/newton/_src/sensors/warp_raytrace/ray_cast.py +++ b/newton/_src/sensors/warp_raytrace/ray_cast.py @@ -15,6 +15,8 @@ import warp as wp +from newton._src.core.types import MAXVAL + from . import ray from .types import RenderShapeType @@ -75,7 +77,7 @@ def closest_hit_shape( si = shape_enabled[shape_index] hit = wp.bool(False) - hit_dist = wp.float32(wp.inf) + hit_dist = wp.float32(MAXVAL) hit_normal = wp.vec3f(0.0) hit_u = wp.float32(0.0) hit_v = wp.float32(0.0) @@ -319,7 +321,7 @@ def first_hit_shape( while wp.bvh_query_next(query, shape_index, max_dist): si = shape_enabled[shape_index] - dist = wp.float32(wp.inf) + dist = wp.float32(MAXVAL) if shape_types[si] == RenderShapeType.MESH: _h, dist, _n, _u, _v, _f, _mesh_id = ray.ray_mesh_with_bvh( diff --git a/newton/_src/sim/__init__.py b/newton/_src/sim/__init__.py index c7652303f..7a971c45f 100644 --- a/newton/_src/sim/__init__.py +++ b/newton/_src/sim/__init__.py @@ -22,7 +22,6 @@ from .control import Control from .graph_coloring import color_graph, plot_graph from .joints import ( - JOINT_LIMIT_UNLIMITED, EqType, JointType, get_joint_dof_count, @@ -32,7 +31,6 @@ from .style3d import Style3DModel, Style3DModelBuilder __all__ = [ - "JOINT_LIMIT_UNLIMITED", "BroadPhaseMode", "CollisionPipeline", "CollisionPipelineUnified", diff --git a/newton/_src/sim/builder.py b/newton/_src/sim/builder.py index 3ce77743e..2f64c38f8 100644 --- a/newton/_src/sim/builder.py +++ b/newton/_src/sim/builder.py @@ -29,6 +29,7 @@ import warp as wp from ..core.types import ( + MAXVAL, Axis, AxisType, Devicelike, @@ -60,7 +61,6 @@ from ..utils.mesh import MeshAdjacency from .graph_coloring import ColoringAlgorithm, color_rigid_bodies, color_trimesh, combine_independent_particle_coloring from .joints import ( - JOINT_LIMIT_UNLIMITED, EqType, JointType, get_joint_dof_count, @@ -291,8 +291,8 @@ class JointDofConfig: def __init__( self, axis: AxisType | Vec3 = Axis.X, - limit_lower: float = -JOINT_LIMIT_UNLIMITED, - limit_upper: float = JOINT_LIMIT_UNLIMITED, + limit_lower: float = -MAXVAL, + limit_upper: float = MAXVAL, limit_ke: float = 1e4, limit_kd: float = 1e1, target_pos: float = 0.0, @@ -307,9 +307,9 @@ def __init__( self.axis = wp.normalize(axis_to_vec3(axis)) """The 3D axis that this JointDofConfig object describes.""" self.limit_lower = limit_lower - """The lower position limit of the joint axis. Defaults to -JOINT_LIMIT_UNLIMITED (unlimited).""" + """The lower position limit of the joint axis. Defaults to -MAXVAL (unlimited).""" self.limit_upper = limit_upper - """The upper position limit of the joint axis. Defaults to JOINT_LIMIT_UNLIMITED (unlimited).""" + """The upper position limit of the joint axis. Defaults to MAXVAL (unlimited).""" self.limit_ke = limit_ke """The elastic stiffness of the joint axis limits. Defaults to 1e4.""" self.limit_kd = limit_kd @@ -341,8 +341,8 @@ def create_unlimited(cls, axis: AxisType | Vec3) -> ModelBuilder.JointDofConfig: """Creates a JointDofConfig with no limits.""" return ModelBuilder.JointDofConfig( axis=axis, - limit_lower=-JOINT_LIMIT_UNLIMITED, - limit_upper=JOINT_LIMIT_UNLIMITED, + limit_lower=-MAXVAL, + limit_upper=MAXVAL, target_pos=0.0, target_vel=0.0, target_ke=0.0, @@ -2406,11 +2406,11 @@ def add_axis_dim(dim: ModelBuilder.JointDofConfig): if np.isfinite(dim.limit_lower): self.joint_limit_lower.append(dim.limit_lower) else: - self.joint_limit_lower.append(-JOINT_LIMIT_UNLIMITED) + self.joint_limit_lower.append(-MAXVAL) if np.isfinite(dim.limit_upper): self.joint_limit_upper.append(dim.limit_upper) else: - self.joint_limit_upper.append(JOINT_LIMIT_UNLIMITED) + self.joint_limit_upper.append(MAXVAL) for dim in linear_axes: add_axis_dim(dim) diff --git a/newton/_src/sim/joints.py b/newton/_src/sim/joints.py index 38d5f8611..04462c004 100644 --- a/newton/_src/sim/joints.py +++ b/newton/_src/sim/joints.py @@ -100,19 +100,7 @@ class EqType(IntEnum): """Constrains the position or angle of one joint to be a quartic polynomial of another joint (like a prismatic or revolute joint).""" -# Sentinel value for unlimited joint limits -JOINT_LIMIT_UNLIMITED = 1e10 -""" -Sentinel value indicating an unlimited joint limit. - -When used for joint_limit_upper, it means the joint has no upper limit. -When used for joint_limit_lower (as -JOINT_LIMIT_UNLIMITED), it means the joint has no lower limit. -A joint is considered fully unlimited only when both limits are set to these sentinel values. -""" - - __all__ = [ - "JOINT_LIMIT_UNLIMITED", "EqType", "JointType", "get_joint_dof_count", diff --git a/newton/_src/solvers/mujoco/solver_mujoco.py b/newton/_src/solvers/mujoco/solver_mujoco.py index 7b8462543..cf8d6b112 100644 --- a/newton/_src/solvers/mujoco/solver_mujoco.py +++ b/newton/_src/solvers/mujoco/solver_mujoco.py @@ -22,10 +22,9 @@ import numpy as np import warp as wp -from ...core.types import nparray, override, vec5 +from ...core.types import MAXVAL, nparray, override, vec5 from ...geometry import MESH_MAXHULLVERT, GeoType, ShapeFlags from ...sim import ( - JOINT_LIMIT_UNLIMITED, Contacts, Control, EqType, @@ -1818,7 +1817,7 @@ def add_geoms(newton_body_id: int): if joint_actgravcomp is not None: joint_params["actgravcomp"] = joint_actgravcomp[ai] lower, upper = joint_limit_lower[ai], joint_limit_upper[ai] - if lower <= -JOINT_LIMIT_UNLIMITED and upper >= JOINT_LIMIT_UNLIMITED: + if lower <= -MAXVAL and upper >= MAXVAL: joint_params["limited"] = False else: joint_params["limited"] = True @@ -1904,7 +1903,7 @@ def add_geoms(newton_body_id: int): if joint_actgravcomp is not None: joint_params["actgravcomp"] = joint_actgravcomp[ai] lower, upper = joint_limit_lower[ai], joint_limit_upper[ai] - if lower <= -JOINT_LIMIT_UNLIMITED and upper >= JOINT_LIMIT_UNLIMITED: + if lower <= -MAXVAL and upper >= MAXVAL: joint_params["limited"] = False else: joint_params["limited"] = True diff --git a/newton/_src/viewer/viewer.py b/newton/_src/viewer/viewer.py index 08926a91a..f3c4732c4 100644 --- a/newton/_src/viewer/viewer.py +++ b/newton/_src/viewer/viewer.py @@ -35,7 +35,7 @@ solidify_mesh, ) -from ..core.types import nparray +from ..core.types import MAXVAL, nparray from .kernels import compute_hydro_contact_surface_lines, estimate_world_extents @@ -239,8 +239,8 @@ def _get_world_extents(self) -> tuple[float, float, float] | None: num_worlds = self.model.num_worlds # Initialize bounds arrays for all worlds - world_bounds_min = wp.full((num_worlds, 3), wp.inf, dtype=wp.float32, device=self.device) - world_bounds_max = wp.full((num_worlds, 3), -wp.inf, dtype=wp.float32, device=self.device) + world_bounds_min = wp.full((num_worlds, 3), MAXVAL, dtype=wp.float32, device=self.device) + world_bounds_max = wp.full((num_worlds, 3), -MAXVAL, dtype=wp.float32, device=self.device) # Get initial state for body transforms state = self.model.state() diff --git a/newton/tests/test_collision_primitives.py b/newton/tests/test_collision_primitives.py index 80827703a..c896cf789 100644 --- a/newton/tests/test_collision_primitives.py +++ b/newton/tests/test_collision_primitives.py @@ -19,6 +19,7 @@ import warp as wp from newton import geometry +from newton._src.core.types import MAXVAL def check_normal_direction_sphere_sphere(pos1, pos2, normal, tolerance=1e-5): @@ -1330,7 +1331,7 @@ def test_plane_capsule(self): for i, expected_dist in enumerate(expected_distances): # Both contacts should have approximately the same distance for horizontal capsule for j in range(2): - if distances_np[i][j] != float("inf"): + if distances_np[i][j] < MAXVAL * 0.99: self.assertAlmostEqual( distances_np[i][j], expected_dist, @@ -1412,7 +1413,7 @@ def test_plane_box(self): # Verify contact counts and distances tolerance = 0.01 for i in range(len(test_cases)): - valid_contacts = sum(1 for d in distances_np[i] if d != float("inf")) + valid_contacts = sum(1 for d in distances_np[i] if d < MAXVAL * 0.99) expected_count = expected_contact_counts[i] expected_dist = expected_distances[i] @@ -1425,7 +1426,7 @@ def test_plane_box(self): # Check that all valid contact distances match expected value if valid_contacts > 0: for j in range(4): - if distances_np[i][j] != float("inf"): + if distances_np[i][j] < MAXVAL * 0.99: self.assertAlmostEqual( distances_np[i][j], expected_dist, @@ -1595,7 +1596,7 @@ def test_box_box(self): # Count valid contacts for each test case for i in range(len(test_cases)): - valid_contacts = sum(1 for j in range(8) if distances_np[i][j] != float("inf")) + valid_contacts = sum(1 for j in range(8) if distances_np[i][j] < MAXVAL * 0.99) if i == 0: # Separated boxes self.assertEqual(valid_contacts, 0, msg="Separated boxes should have no contacts") @@ -1605,7 +1606,7 @@ def test_box_box(self): # Check that contact normals are unit length and point from box1 into box2 for i in range(len(test_cases)): for j in range(8): - if distances_np[i][j] == float("inf"): + if distances_np[i][j] >= MAXVAL * 0.99: continue # Check normal is unit length @@ -1733,7 +1734,7 @@ def test_box_box_margin(self): # Verify expected contact behavior for each test case for i in range(len(test_cases)): - valid_contacts = sum(1 for j in range(8) if distances_np[i][j] != float("inf")) + valid_contacts = sum(1 for j in range(8) if distances_np[i][j] < MAXVAL * 0.99) expect_contacts = test_cases[i][7] margin = test_cases[i][6] @@ -1757,7 +1758,7 @@ def test_box_box_margin(self): continue for j in range(8): - if distances_np[i][j] == float("inf"): + if distances_np[i][j] >= MAXVAL * 0.99: continue normal = normals_np[i][j] @@ -1841,7 +1842,7 @@ def test_capsule_box(self): tolerance = 0.05 # Slightly larger tolerance for capsule-box collision for i, expected_min_dist in enumerate(expected_min_distances): # Find the minimum distance among valid contacts - valid_distances = [d for d in distances_np[i] if d != float("inf")] + valid_distances = [d for d in distances_np[i] if d < MAXVAL * 0.99] if len(valid_distances) > 0: min_distance = min(valid_distances) self.assertAlmostEqual( @@ -1875,7 +1876,7 @@ def test_capsule_box(self): box_size = np.array(test_cases[i][6]) for j in range(2): # Check up to 2 contacts - if distances_np[i][j] == float("inf") or distances_np[i][j] >= 0: + if distances_np[i][j] >= MAXVAL * 0.99 or distances_np[i][j] >= 0: continue contact_pos = positions_np[i][j] @@ -2112,7 +2113,7 @@ def test_box_box_penetration_depths(self): # Count valid contacts and find deepest penetration valid_contacts = [] for j in range(8): - if distances_np[i][j] != float("inf"): + if distances_np[i][j] < MAXVAL * 0.99: valid_contacts.append(distances_np[i][j]) if expect_contacts: @@ -2164,7 +2165,7 @@ def test_box_box_penetration_depths(self): continue for j in range(8): - if distances_np[i][j] == float("inf"): + if distances_np[i][j] >= MAXVAL * 0.99: continue normal = normals_np[i][j] diff --git a/newton/tests/test_joint_limits.py b/newton/tests/test_joint_limits.py index 8986cb2a7..34e178eee 100644 --- a/newton/tests/test_joint_limits.py +++ b/newton/tests/test_joint_limits.py @@ -18,7 +18,7 @@ import numpy as np import newton -from newton._src.sim.joints import JOINT_LIMIT_UNLIMITED +from newton._src.core.types import MAXVAL from newton._src.utils.import_urdf import parse_urdf @@ -42,8 +42,8 @@ def test_unlimited_joint_defaults(self): # Check that default limits are unlimited lower_limits = model.joint_limit_lower.numpy() upper_limits = model.joint_limit_upper.numpy() - self.assertEqual(lower_limits[0], -JOINT_LIMIT_UNLIMITED) - self.assertEqual(upper_limits[0], JOINT_LIMIT_UNLIMITED) + self.assertEqual(lower_limits[0], -MAXVAL) + self.assertEqual(upper_limits[0], MAXVAL) def test_limited_joint(self): """Test that limited joints work correctly.""" @@ -73,7 +73,7 @@ def test_partially_limited_joint(self): body = builder.add_link() # Add a revolute joint with only upper limit - joint = builder.add_joint_revolute(parent=-1, child=body, limit_lower=-JOINT_LIMIT_UNLIMITED, limit_upper=2.0) + joint = builder.add_joint_revolute(parent=-1, child=body, limit_lower=-MAXVAL, limit_upper=2.0) builder.add_articulation([joint]) # Build model @@ -82,22 +82,20 @@ def test_partially_limited_joint(self): # Check lower is unlimited, upper is limited lower_limits = model.joint_limit_lower.numpy() upper_limits = model.joint_limit_upper.numpy() - self.assertEqual(lower_limits[0], -JOINT_LIMIT_UNLIMITED) + self.assertEqual(lower_limits[0], -MAXVAL) self.assertAlmostEqual(upper_limits[0], 2.0) # Test the other way - only lower limit builder2 = newton.ModelBuilder() body2 = builder2.add_link() - joint2 = builder2.add_joint_revolute( - parent=-1, child=body2, limit_lower=-1.5, limit_upper=JOINT_LIMIT_UNLIMITED - ) + joint2 = builder2.add_joint_revolute(parent=-1, child=body2, limit_lower=-1.5, limit_upper=MAXVAL) builder2.add_articulation([joint2]) model2 = builder2.finalize() lower_limits2 = model2.joint_limit_lower.numpy() upper_limits2 = model2.joint_limit_upper.numpy() self.assertAlmostEqual(lower_limits2[0], -1.5) - self.assertEqual(upper_limits2[0], JOINT_LIMIT_UNLIMITED) + self.assertEqual(upper_limits2[0], MAXVAL) def test_continuous_joint_from_urdf(self): """Test that continuous joints from URDF are unlimited.""" @@ -131,8 +129,8 @@ def test_continuous_joint_from_urdf(self): # Find the continuous joint (should be the first joint) lower_limits = model.joint_limit_lower.numpy() upper_limits = model.joint_limit_upper.numpy() - self.assertEqual(lower_limits[0], -JOINT_LIMIT_UNLIMITED) - self.assertEqual(upper_limits[0], JOINT_LIMIT_UNLIMITED) + self.assertEqual(lower_limits[0], -MAXVAL) + self.assertEqual(upper_limits[0], MAXVAL) def test_joint_d6_with_mixed_limits(self): """Test D6 joint with mixed limited and unlimited axes.""" @@ -153,21 +151,13 @@ def test_joint_d6_with_mixed_limits(self): child=body, linear_axes=[ newton.ModelBuilder.JointDofConfig(axis=newton.Axis.X, limit_lower=-1.0, limit_upper=1.0), - newton.ModelBuilder.JointDofConfig( - axis=newton.Axis.Y, limit_lower=-JOINT_LIMIT_UNLIMITED, limit_upper=JOINT_LIMIT_UNLIMITED - ), - newton.ModelBuilder.JointDofConfig( - axis=newton.Axis.Z, limit_lower=-0.5, limit_upper=JOINT_LIMIT_UNLIMITED - ), + newton.ModelBuilder.JointDofConfig(axis=newton.Axis.Y, limit_lower=-MAXVAL, limit_upper=MAXVAL), + newton.ModelBuilder.JointDofConfig(axis=newton.Axis.Z, limit_lower=-0.5, limit_upper=MAXVAL), ], angular_axes=[ - newton.ModelBuilder.JointDofConfig( - axis=newton.Axis.X, limit_lower=-JOINT_LIMIT_UNLIMITED, limit_upper=JOINT_LIMIT_UNLIMITED - ), + newton.ModelBuilder.JointDofConfig(axis=newton.Axis.X, limit_lower=-MAXVAL, limit_upper=MAXVAL), newton.ModelBuilder.JointDofConfig(axis=newton.Axis.Y, limit_lower=-np.pi / 4, limit_upper=np.pi / 4), - newton.ModelBuilder.JointDofConfig( - axis=newton.Axis.Z, limit_lower=-JOINT_LIMIT_UNLIMITED, limit_upper=np.pi / 2 - ), + newton.ModelBuilder.JointDofConfig(axis=newton.Axis.Z, limit_lower=-MAXVAL, limit_upper=np.pi / 2), ], ) builder.add_articulation([joint]) @@ -182,20 +172,20 @@ def test_joint_d6_with_mixed_limits(self): self.assertAlmostEqual(lower_limits[0], -1.0) # X limited self.assertAlmostEqual(upper_limits[0], 1.0) - self.assertEqual(lower_limits[1], -JOINT_LIMIT_UNLIMITED) # Y unlimited - self.assertEqual(upper_limits[1], JOINT_LIMIT_UNLIMITED) + self.assertEqual(lower_limits[1], -MAXVAL) # Y unlimited + self.assertEqual(upper_limits[1], MAXVAL) self.assertAlmostEqual(lower_limits[2], -0.5) # Z partially limited - self.assertEqual(upper_limits[2], JOINT_LIMIT_UNLIMITED) + self.assertEqual(upper_limits[2], MAXVAL) # Check angular axes - self.assertEqual(lower_limits[3], -JOINT_LIMIT_UNLIMITED) # X rot unlimited - self.assertEqual(upper_limits[3], JOINT_LIMIT_UNLIMITED) + self.assertEqual(lower_limits[3], -MAXVAL) # X rot unlimited + self.assertEqual(upper_limits[3], MAXVAL) self.assertAlmostEqual(lower_limits[4], -np.pi / 4) # Y rot limited self.assertAlmostEqual(upper_limits[4], np.pi / 4) - self.assertEqual(lower_limits[5], -JOINT_LIMIT_UNLIMITED) # Z rot partially limited + self.assertEqual(lower_limits[5], -MAXVAL) # Z rot partially limited self.assertAlmostEqual(upper_limits[5], np.pi / 2) def test_create_unlimited_joint_config(self): @@ -204,8 +194,8 @@ def test_create_unlimited_joint_config(self): config = newton.ModelBuilder.JointDofConfig.create_unlimited(newton.Axis.X) # Check limits are unlimited - self.assertEqual(config.limit_lower, -JOINT_LIMIT_UNLIMITED) - self.assertEqual(config.limit_upper, JOINT_LIMIT_UNLIMITED) + self.assertEqual(config.limit_lower, -MAXVAL) + self.assertEqual(config.limit_upper, MAXVAL) # Check other properties self.assertEqual(config.limit_ke, 0.0) @@ -218,19 +208,17 @@ def test_robustness_of_limit_comparisons(self): body = builder.add_body() # Add joint with unlimited limits - joint = builder.add_joint_revolute( - parent=-1, child=body, limit_lower=-JOINT_LIMIT_UNLIMITED, limit_upper=JOINT_LIMIT_UNLIMITED - ) + joint = builder.add_joint_revolute(parent=-1, child=body, limit_lower=-MAXVAL, limit_upper=MAXVAL) builder.add_articulation([joint]) model = builder.finalize() # Test robust comparisons - # These should work even if JOINT_LIMIT_UNLIMITED changes from wp.inf to a large finite value + # These should work even if MAXVAL changes from wp.inf to a large finite value lower_limits = model.joint_limit_lower.numpy() upper_limits = model.joint_limit_upper.numpy() - self.assertTrue(lower_limits[0] <= -JOINT_LIMIT_UNLIMITED) - self.assertTrue(upper_limits[0] >= JOINT_LIMIT_UNLIMITED) + self.assertTrue(lower_limits[0] <= -MAXVAL) + self.assertTrue(upper_limits[0] >= MAXVAL) if __name__ == "__main__":