diff --git a/source/isaaclab/isaaclab/envs/mdp/__init__.pyi b/source/isaaclab/isaaclab/envs/mdp/__init__.pyi index 399035f2c09c..be8f8b8b05bc 100644 --- a/source/isaaclab/isaaclab/envs/mdp/__init__.pyi +++ b/source/isaaclab/isaaclab/envs/mdp/__init__.pyi @@ -137,6 +137,8 @@ __all__ = [ "joint_pos_out_of_manual_limit", "joint_vel_out_of_limit", "joint_vel_out_of_manual_limit", + "body_lin_vel_out_of_limit", + "body_ang_vel_out_of_limit", "root_height_below_minimum", "time_out", ] @@ -285,6 +287,8 @@ from .terminations import ( joint_pos_out_of_manual_limit, joint_vel_out_of_limit, joint_vel_out_of_manual_limit, + body_lin_vel_out_of_limit, + body_ang_vel_out_of_limit, root_height_below_minimum, time_out, ) diff --git a/source/isaaclab/isaaclab/envs/mdp/terminations.py b/source/isaaclab/isaaclab/envs/mdp/terminations.py index 1936f5dd50b6..6cdab3839d3b 100644 --- a/source/isaaclab/isaaclab/envs/mdp/terminations.py +++ b/source/isaaclab/isaaclab/envs/mdp/terminations.py @@ -150,6 +150,41 @@ def joint_effort_out_of_limit( return torch.any(out_of_limits, dim=1) +""" +Body velocity terminations. +""" + + +def body_lin_vel_out_of_limit( + env: ManagerBasedRLEnv, + max_velocity: float, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +) -> torch.Tensor: + """Terminate when any body's linear velocity magnitude exceeds the limit [m/s]. + + Also terminates on NaN velocities, which indicate a solver singularity. + """ + asset: Articulation = env.scene[asset_cfg.name] + body_vel = wp.to_torch(asset.data.body_lin_vel_w)[:, asset_cfg.body_ids] + speed = torch.linalg.norm(body_vel, dim=-1) + return torch.any((speed > max_velocity) | torch.isnan(speed), dim=1) + + +def body_ang_vel_out_of_limit( + env: ManagerBasedRLEnv, + max_velocity: float, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +) -> torch.Tensor: + """Terminate when any body's angular velocity magnitude exceeds the limit [rad/s]. + + Also terminates on NaN velocities, which indicate a solver singularity. + """ + asset: Articulation = env.scene[asset_cfg.name] + body_vel = wp.to_torch(asset.data.body_ang_vel_w)[:, asset_cfg.body_ids] + speed = torch.linalg.norm(body_vel, dim=-1) + return torch.any((speed > max_velocity) | torch.isnan(speed), dim=1) + + """ Contact sensor. """ diff --git a/source/isaaclab/isaaclab/terrains/terrain_generator.py b/source/isaaclab/isaaclab/terrains/terrain_generator.py index 58c0c85be9d1..39413478c0fb 100644 --- a/source/isaaclab/isaaclab/terrains/terrain_generator.py +++ b/source/isaaclab/isaaclab/terrains/terrain_generator.py @@ -18,7 +18,6 @@ from isaaclab.utils.timer import Timer from isaaclab.utils.warp import convert_to_warp_mesh -from .trimesh.utils import make_border from .utils import color_meshes_by_height, find_flat_patches if TYPE_CHECKING: @@ -270,25 +269,48 @@ def _generate_curriculum_terrains(self): """ def _add_terrain_border(self): - """Add a surrounding border over all the sub-terrains into the terrain meshes.""" - # border parameters - border_size = ( - self.cfg.num_rows * self.cfg.size[0] + 2 * self.cfg.border_width, - self.cfg.num_cols * self.cfg.size[1] + 2 * self.cfg.border_width, - ) - inner_size = (self.cfg.num_rows * self.cfg.size[0], self.cfg.num_cols * self.cfg.size[1]) - border_center = ( - self.cfg.num_rows * self.cfg.size[0] / 2, - self.cfg.num_cols * self.cfg.size[1] / 2, - -self.cfg.border_height / 2, - ) - # border mesh - border_meshes = make_border(border_size, inner_size, height=abs(self.cfg.border_height), position=border_center) - border = trimesh.util.concatenate(border_meshes) - # update the faces to have minimal triangles - selector = ~(np.asarray(border.triangles)[:, :, 2] < -0.1).any(1) - border.update_faces(selector) - # add the border to the list of meshes + """Add a surrounding border over all the sub-terrains into the terrain meshes. + + The border is built as a subdivided grid mesh (matching ``horizontal_scale``) + rather than large box primitives. Large triangles on the border can cause + collision detection failures in some physics backends (e.g. Newton/MuJoCo). + """ + bw = self.cfg.border_width + if bw <= 0: + return + + inner_w = self.cfg.num_rows * self.cfg.size[0] + inner_l = self.cfg.num_cols * self.cfg.size[1] + hs = self.cfg.horizontal_scale + + def _make_grid_strip(x0: float, y0: float, width: float, length: float) -> trimesh.Trimesh: + """Create a flat subdivided mesh strip at z=0 with grid spacing ``hs``.""" + nx = max(int(np.ceil(width / hs)), 1) + 1 + ny = max(int(np.ceil(length / hs)), 1) + 1 + xs = np.linspace(x0, x0 + width, nx) + ys = np.linspace(y0, y0 + length, ny) + xx, yy = np.meshgrid(xs, ys, indexing="ij") + vertices = np.zeros((nx * ny, 3)) + vertices[:, 0] = xx.flatten() + vertices[:, 1] = yy.flatten() + faces = [] + for i in range(nx - 1): + for j in range(ny - 1): + v0 = i * ny + j + v1 = v0 + 1 + v2 = (i + 1) * ny + j + v3 = v2 + 1 + faces.append([v0, v2, v1]) + faces.append([v1, v2, v3]) + return trimesh.Trimesh(vertices=vertices, faces=np.array(faces, dtype=np.uint32)) + + strips = [ + _make_grid_strip(-bw, -bw, inner_w + 2 * bw, bw), # bottom + _make_grid_strip(-bw, inner_l, inner_w + 2 * bw, bw), # top + _make_grid_strip(-bw, 0.0, bw, inner_l), # left + _make_grid_strip(inner_w, 0.0, bw, inner_l), # right + ] + border = trimesh.util.concatenate(strips) self.terrain_meshes.append(border) def _add_sub_terrain( diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/rough_env_cfg.py index 38d6b9d0546e..772e4575ce77 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_d/rough_env_cfg.py @@ -3,13 +3,13 @@ # # SPDX-License-Identifier: BSD-3-Clause +from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg, NewtonCollisionPipelineCfg +from isaaclab_physx.physics import PhysxCfg + +from isaaclab.sim import SimulationCfg from isaaclab.utils import configclass -from isaaclab_tasks.manager_based.locomotion.velocity.velocity_env_cfg import ( - EventsCfg, - LocomotionVelocityRoughEnvCfg, - StartupEventsCfg, -) +from isaaclab_tasks.manager_based.locomotion.velocity.velocity_env_cfg import LocomotionVelocityRoughEnvCfg from isaaclab_tasks.utils import PresetCfg ## @@ -19,20 +19,27 @@ @configclass -class AnymalDPhysxEventsCfg(EventsCfg, StartupEventsCfg): - pass - - -@configclass -class AnymalDEventsCfg(PresetCfg): - default = AnymalDPhysxEventsCfg() - newton = EventsCfg() +class RoughPhysicsCfg(PresetCfg): + default = PhysxCfg(gpu_max_rigid_patch_count=10 * 2**15) + newton = NewtonCfg( + solver_cfg=MJWarpSolverCfg( + njmax=200, + nconmax=100, + cone="pyramidal", + impratio=1.0, + integrator="implicitfast", + use_mujoco_contacts=False, + ), + collision_cfg=NewtonCollisionPipelineCfg(max_triangle_pairs=2_500_000), + num_substeps=1, + debug_mode=False, + ) physx = default @configclass class AnymalDRoughEnvCfg(LocomotionVelocityRoughEnvCfg): - events: AnymalDEventsCfg = AnymalDEventsCfg() + sim: SimulationCfg = SimulationCfg(physics=RoughPhysicsCfg()) def __post_init__(self): # post init of parent diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py index 81f38e756975..05412e914ba1 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py @@ -27,7 +27,7 @@ from isaaclab.utils.noise import UniformNoiseCfg as Unoise import isaaclab_tasks.manager_based.locomotion.velocity.mdp as mdp -from isaaclab_tasks.utils import PresetCfg +from isaaclab_tasks.utils import PresetCfg, preset ## # Pre-defined configs @@ -161,6 +161,50 @@ def __post_init__(self): class EventsCfg: """Configuration for events.""" + # startup + physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=".*"), + "static_friction_range": (0.8, 0.8), + "dynamic_friction_range": (0.6, 0.6), + "restitution_range": (0.0, 0.0), + "num_buckets": 64, + }, + ) + + add_base_mass = EventTerm( + func=mdp.randomize_rigid_body_mass, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names="base"), + "mass_distribution_params": (-5.0, 5.0), + "operation": "add", + }, + ) + + base_com = preset( + default=EventTerm( + func=mdp.randomize_rigid_body_com, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names="base"), + "com_range": {"x": (-0.05, 0.05), "y": (-0.05, 0.05), "z": (-0.01, 0.01)}, + }, + ), + newton=None, + ) + + collider_offsets = EventTerm( + func=mdp.randomize_rigid_body_collider_offsets, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=".*"), + "contact_offset_distribution_params": (0.01, 0.01), + }, + ) + # reset base_external_force_torque = EventTerm( func=mdp.apply_external_force_torque, @@ -206,41 +250,6 @@ class EventsCfg: ) -@configclass -class StartupEventsCfg: - # startup - physics_material = EventTerm( - func=mdp.randomize_rigid_body_material, - mode="startup", - params={ - "asset_cfg": SceneEntityCfg("robot", body_names=".*"), - "static_friction_range": (0.8, 0.8), - "dynamic_friction_range": (0.6, 0.6), - "restitution_range": (0.0, 0.0), - "num_buckets": 64, - }, - ) - - add_base_mass = EventTerm( - func=mdp.randomize_rigid_body_mass, - mode="startup", - params={ - "asset_cfg": SceneEntityCfg("robot", body_names="base"), - "mass_distribution_params": (-5.0, 5.0), - "operation": "add", - }, - ) - - base_com = EventTerm( - func=mdp.randomize_rigid_body_com, - mode="startup", - params={ - "asset_cfg": SceneEntityCfg("robot", body_names="base"), - "com_range": {"x": (-0.05, 0.05), "y": (-0.05, 0.05), "z": (-0.01, 0.01)}, - }, - ) - - @configclass class RewardsCfg: """Reward terms for the MDP.""" @@ -286,6 +295,10 @@ class TerminationsCfg: func=mdp.illegal_contact, params={"sensor_cfg": SceneEntityCfg("contact_forces", body_names="base"), "threshold": 1.0}, ) + body_lin_vel = DoneTerm( + func=mdp.body_lin_vel_out_of_limit, + params={"max_velocity": 20.0, "asset_cfg": SceneEntityCfg("robot", body_names=".*")}, + ) @configclass @@ -313,7 +326,7 @@ class LocomotionVelocityRoughEnvCfg(ManagerBasedRLEnvCfg): # MDP settings rewards: RewardsCfg = RewardsCfg() terminations: TerminationsCfg = TerminationsCfg() - events: EventsCfg = MISSING + events: EventsCfg = EventsCfg() curriculum: CurriculumCfg = CurriculumCfg() def __post_init__(self):