Fix cable routing control and training semantics

Author: maxkra15

source/isaaclab_tasks/changelog.d/maximiliank-cable-routing-yam.minor.rst

@@ -5,7 +5,7 @@ Added
   actions, smooth one-meter self-avoiding cable resets, single-span wrap validation, heterogeneous
   fixture resets, and Newton MJWarp/VBD proxy coupling.
 * Added a pinned Robot Menagerie YAM USD package with native Newton collision and mimic schemas,
-  plus contact-capacity stress validation for dense cable interactions.
+  plus explicit contact-buffer headroom for dense cable interactions.
 * Added pinned ManipulationNet task-board and F1 USD visuals with primitive-only Newton collision,
   reproducible STL conversion metadata, and front-edge dual-YAM placement.
 * Added three staged round-peg routing goals plus an explicit seven-goal task spanning both
@@ -21,6 +21,10 @@ Fixed
 * Fixed non-finite terminal cable states so they are reset with finite route metrics and rewards
   instead of aborting synchronized multi-GPU training; robot/action failures are now sanitized at
   the physics boundary and terminated with finite policy observations and rewards.
+* Held each relative joint target across control decimation, clamped it to the authored limits,
+  represented gripper actions by their binary command state, and made terminal success and failure
+  rewards independent of control frequency. Newton actuator graph capture and complete-episode PPO
+  startup are now enabled for training.
 * Applied explicit Newton contact materials to YAM and fixture assets, proxied every collidable YAM
   link into the cable solver, and declared the task's Newton and contrib runtime dependencies.
 * Avoided redundant or manager-order-dependent route evaluation and expensive ordinary cable

source/isaaclab_tasks/isaaclab_tasks/contrib/cable_routing/agents/rsl_rl_ppo_cfg.py

@@ -54,7 +54,8 @@ class CableRoutingGaussianDistributionCfg(RslRlMLPModelCfg.GaussianDistributionC
 class CableRoutingPPORunnerCfg(RslRlOnPolicyRunnerCfg):
     """PPO configuration for the goal-conditioned bimanual cable-routing policy."""
 
-    num_steps_per_env = 32
+    num_steps_per_env = 36
+    init_at_random_ep_len = False
     max_iterations = 15000
     save_interval = 250
     experiment_name = "yam_cable_routing"

source/isaaclab_tasks/isaaclab_tasks/contrib/cable_routing/cable_routing_env_cfg.py

@@ -390,7 +390,10 @@ class PolicyCfg(ObsGroup):
                 "right_ee_cfg": SceneEntityCfg("yam_right", body_names=["link_6"]),
             },
         )
-        actions = ObsTerm(func=mdp.finite_last_action)
+        actions = ObsTerm(
+            func=mdp.finite_last_action,
+            params={"binary_action_names": ("left_gripper", "right_gripper")},
+        )
 
         def __post_init__(self) -> None:
             self.enable_corruption = False
@@ -502,13 +505,29 @@ class EventCfg:
 class RewardsCfg:
     """Sparse ordered-route success reward with safety and control penalties."""
 
-    success = RewTerm(func=mdp.route_success, weight=20.0, params={"command_name": "route"})
+    success = RewTerm(
+        func=mdp.route_success,
+        weight=20.0,
+        params={
+            "command_name": "route",
+            "failure_termination_names": ("invalid_cable", "invalid_robot_or_action"),
+        },
+    )
+    failure = RewTerm(
+        func=mdp.route_failure,
+        weight=-20.0,
+        params={"termination_names": ("invalid_cable", "invalid_robot_or_action")},
+    )
     stretch = RewTerm(
         func=mdp.cable_stretch,
         weight=-0.25,
         params={"cable_cfg": SceneEntityCfg("cable"), "rest_length": CABLE_SEGMENT_LENGTH},
     )
-    action_rate = RewTerm(func=mdp.finite_action_rate_l2, weight=-0.002)
+    action_rate = RewTerm(
+        func=mdp.finite_action_rate_l2,
+        weight=-0.002,
+        params={"binary_action_names": ("left_gripper", "right_gripper")},
+    )
     left_joint_velocity = RewTerm(
         func=mdp.finite_joint_vel_l2,
         weight=-0.0001,
@@ -559,6 +578,7 @@ class CableRoutingEnvCfg(ManagerBasedRLEnvCfg):
     terminations: TerminationsCfg = TerminationsCfg()
     sim: SimulationCfg = SimulationCfg(
         dt=1.0 / 120.0,
+        use_newton_actuators=True,
         physics=NewtonCfg(
             solver_cfg=CouplerProxyCfg(
                 entries=[

source/isaaclab_tasks/isaaclab_tasks/contrib/cable_routing/mdp/__init__.pyi

@@ -28,6 +28,7 @@ __all__ = [
     "cable_relative_joint_gap",
     "cable_stretch",
     "cable_unrouted_mask",
+    "canonical_task_actions",
     "finite_action_rate_l2",
     "finite_joint_pos_rel",
     "finite_joint_vel_l2",
@@ -45,6 +46,7 @@ __all__ = [
     "reset_peg_offsets",
     "robot_or_action_invalid",
     "route_complete",
+    "route_failure",
     "route_progress",
     "route_success",
     "route_task_state",
@@ -66,6 +68,7 @@ from .actions import (
     FiniteBinaryJointPositionActionCfg,
     FiniteRelativeJointPositionAction,
     FiniteRelativeJointPositionActionCfg,
+    canonical_task_actions,
 )
 from .cable_geometry import cable_relative_joint_gap
 from .commands import CableRoutingCommand, CableRoutingCommandCfg
@@ -122,6 +125,7 @@ from .rewards import (
     finite_action_rate_l2,
     finite_joint_vel_l2,
     grippers_near_cable,
+    route_failure,
     route_progress,
     route_success,
 )

source/isaaclab_tasks/isaaclab_tasks/contrib/cable_routing/mdp/actions.py

@@ -7,6 +7,8 @@
 
 from __future__ import annotations
 
+from collections.abc import Sequence
+
 import torch
 
 from isaaclab.envs.mdp.actions import BinaryJointPositionAction, RelativeJointPositionAction
@@ -19,12 +21,47 @@ def _finite_unit_actions(actions: torch.Tensor) -> torch.Tensor:
     return torch.nan_to_num(actions, nan=0.0, posinf=1.0, neginf=-1.0).clamp(-1.0, 1.0)
 
 
+def canonical_task_actions(env, actions: torch.Tensor, binary_action_names: Sequence[str]) -> torch.Tensor:
+    """Return finite unit actions with binary terms represented only by their selected state."""
+    actions = _finite_unit_actions(actions)
+    binary_names = set(binary_action_names)
+    unknown_names = binary_names.difference(env.action_manager.active_terms)
+    if unknown_names:
+        raise ValueError(f"Unknown binary action terms: {sorted(unknown_names)}.")
+
+    chunks: list[torch.Tensor] = []
+    offset = 0
+    for name, dim in zip(env.action_manager.active_terms, env.action_manager.action_term_dim):
+        term_actions = actions[:, offset : offset + dim]
+        if name in binary_names:
+            term_actions = torch.where(
+                term_actions < 0.0, -torch.ones_like(term_actions), torch.ones_like(term_actions)
+            )
+        chunks.append(term_actions)
+        offset += dim
+    return torch.cat(chunks, dim=1)
+
+
 class FiniteRelativeJointPositionAction(RelativeJointPositionAction):
-    """Relative joint action that cannot forward non-finite targets to physics."""
+    """Finite relative joint action that holds one limit-clamped target per policy step."""
 
     def process_actions(self, actions: torch.Tensor) -> None:
         super().process_actions(_finite_unit_actions(actions))
 
+        # The stock relative action adds the delta to the live joint position in apply_actions(),
+        # which is called once per simulation step. Resolve the absolute target here instead so
+        # control semantics do not change with environment decimation or Newton graph capture.
+        current = self._asset.data.joint_pos.torch[:, self._joint_ids]
+        default = self._asset.data.default_joint_pos.torch[:, self._joint_ids]
+        current = torch.where(torch.isfinite(current), current, default)
+        limits = self._asset.data.soft_joint_pos_limits.torch[:, self._joint_ids]
+        target = current + self._processed_actions
+        target = torch.where(torch.isfinite(target), target, default)
+        self._processed_actions = torch.maximum(torch.minimum(target, limits[..., 1]), limits[..., 0])
+
+    def apply_actions(self) -> None:
+        self._asset.set_joint_position_target_index(target=self.processed_actions, joint_ids=self._joint_ids)
+
 
 class FiniteBinaryJointPositionAction(BinaryJointPositionAction):
     """Binary gripper action that cannot forward non-finite commands to physics."""

source/isaaclab_tasks/isaaclab_tasks/contrib/cable_routing/mdp/observations.py

@@ -12,6 +12,7 @@
 from isaaclab.managers import SceneEntityCfg
 
 from ..yam_frames import yam_contact_frame_position_w
+from .actions import canonical_task_actions
 
 
 def route_task_state(env, command_name: str) -> torch.Tensor:
@@ -76,10 +77,20 @@ def active_goal_geometry(
     return torch.nan_to_num(geometry, nan=0.0, posinf=0.0, neginf=0.0)
 
 
-def finite_last_action(env, action_name: str | None = None) -> torch.Tensor:
-    """Return the bounded action that the task's action terms can apply."""
-    action = env.action_manager.action if action_name is None else env.action_manager.get_term(action_name).raw_actions
-    return torch.nan_to_num(action, nan=0.0, posinf=1.0, neginf=-1.0).clamp(-1.0, 1.0)
+def finite_last_action(
+    env,
+    action_name: str | None = None,
+    binary_action_names: tuple[str, ...] = (),
+) -> torch.Tensor:
+    """Return bounded actions with binary terms represented by their physical command state."""
+    if action_name is None:
+        return canonical_task_actions(env, env.action_manager.action, binary_action_names)
+
+    action = env.action_manager.get_term(action_name).raw_actions
+    action = torch.nan_to_num(action, nan=0.0, posinf=1.0, neginf=-1.0).clamp(-1.0, 1.0)
+    if action_name in binary_action_names:
+        action = torch.where(action < 0.0, -torch.ones_like(action), torch.ones_like(action))
+    return action
 
 
 def finite_joint_pos_rel(env, asset_cfg: SceneEntityCfg) -> torch.Tensor:

source/isaaclab_tasks/isaaclab_tasks/contrib/cable_routing/mdp/rewards.py

@@ -12,6 +12,7 @@
 from isaaclab.managers import SceneEntityCfg
 
 from ..yam_frames import yam_contact_frame_position_w
+from .actions import canonical_task_actions
 from .cable_geometry import cable_relative_joint_gap
 
 
@@ -40,17 +41,32 @@ def route_progress(env, command_name: str) -> torch.Tensor:
     return command.route_progress_delta
 
 
-def route_success(env, command_name: str) -> torch.Tensor:
-    """Return one for environments that have completed their sampled route."""
+def route_success(
+    env,
+    command_name: str,
+    failure_termination_names: tuple[str, ...] = (),
+) -> torch.Tensor:
+    """Return a unit-integral pulse for valid terminal route success."""
     command = env.command_manager.get_term(command_name)
     command.ensure_route_state_current(update_reward_delta=True)
-    return command.succeeded.float()
+    succeeded = command.succeeded.clone()
+    for term_name in failure_termination_names:
+        succeeded &= ~env.termination_manager.get_term(term_name)
+    return succeeded.float() / max(float(env.step_dt), 1.0e-8)
+
+
+def route_failure(env, termination_names: tuple[str, ...]) -> torch.Tensor:
+    """Return one unit-integral failure pulse for invalid terminal transitions."""
+    failed = torch.zeros(env.num_envs, dtype=torch.bool, device=env.device)
+    for term_name in termination_names:
+        failed |= env.termination_manager.get_term(term_name)
+    return failed.float() / max(float(env.step_dt), 1.0e-8)
 
 
-def finite_action_rate_l2(env) -> torch.Tensor:
-    """Penalize changes between the finite, bounded actions applied by this task."""
-    action = torch.nan_to_num(env.action_manager.action, nan=0.0, posinf=1.0, neginf=-1.0).clamp(-1.0, 1.0)
-    previous = torch.nan_to_num(env.action_manager.prev_action, nan=0.0, posinf=1.0, neginf=-1.0).clamp(-1.0, 1.0)
+def finite_action_rate_l2(env, binary_action_names: tuple[str, ...] = ()) -> torch.Tensor:
+    """Penalize changes in finite arm actions and binary gripper command states."""
+    action = canonical_task_actions(env, env.action_manager.action, binary_action_names)
+    previous = canonical_task_actions(env, env.action_manager.prev_action, binary_action_names)
     return torch.square(action - previous).sum(dim=1)
 
 

source/isaaclab_tasks/test/contrib/test_cable_routing_cfg.py

@@ -33,6 +33,7 @@
 from isaaclab_contrib.deformable import VBDSolverCfg
 
 import isaaclab_tasks.contrib.cable_routing  # noqa: F401
+from isaaclab_tasks.contrib.cable_routing.agents.rsl_rl_ppo_cfg import CableRoutingPPORunnerCfg
 from isaaclab_tasks.contrib.cable_routing.cable_routing_env_cfg import (
     BOARD_SIZE,
     BOARD_THICKNESS,
@@ -63,11 +64,14 @@
     CableRoutingSevenGoalsEnvCfg,
     CableRoutingTier1PegsEnvCfg,
 )
+from isaaclab_tasks.contrib.cable_routing.mdp.actions import FiniteRelativeJointPositionAction
 from isaaclab_tasks.contrib.cable_routing.mdp.commands import (
     CableRoutingCommand,
     CableRoutingCommandCfg,
     _route_goal_marker_data,
 )
+from isaaclab_tasks.contrib.cable_routing.mdp.observations import finite_last_action
+from isaaclab_tasks.contrib.cable_routing.mdp.rewards import finite_action_rate_l2, route_failure, route_success
 
 _TASK_CONFIGS = {
     "IsaacContrib-CableRouting-YAM": "CableRoutingEnvCfg",
@@ -123,6 +127,99 @@ def test_cable_routing_actions_are_relative_joint_position_with_binary_grippers(
     assert resolved_policy_action_dim == 14
 
 
+def test_relative_joint_action_holds_one_limit_clamped_target() -> None:
+    """The relative target is resolved once instead of advancing during decimation."""
+    captured: dict[str, torch.Tensor] = {}
+    joint_position = torch.tensor(((0.95, -0.95),))
+    asset = SimpleNamespace(
+        data=SimpleNamespace(
+            joint_pos=SimpleNamespace(torch=joint_position),
+            default_joint_pos=SimpleNamespace(torch=torch.zeros_like(joint_position)),
+            soft_joint_pos_limits=SimpleNamespace(torch=torch.tensor((((-1.0, 1.0), (-1.0, 1.0)),))),
+        )
+    )
+
+    def set_target(*, target: torch.Tensor, joint_ids: list[int]) -> None:
+        captured["target"] = target.clone()
+        captured["joint_ids"] = torch.tensor(joint_ids)
+
+    asset.set_joint_position_target_index = set_target
+    term = FiniteRelativeJointPositionAction.__new__(FiniteRelativeJointPositionAction)
+    term.cfg = SimpleNamespace(clip=None)
+    term._asset = asset
+    term._joint_ids = [0, 1]
+    term._raw_actions = torch.zeros((1, 2))
+    term._scale = 0.1
+    term._offset = 0.0
+
+    term.process_actions(torch.tensor(((torch.inf, -1.0),)))
+    joint_position.zero_()
+    term.apply_actions()
+    term.apply_actions()
+
+    torch.testing.assert_close(term.raw_actions, torch.tensor(((1.0, -1.0),)))
+    torch.testing.assert_close(captured["target"], torch.tensor(((1.0, -1.0),)))
+    torch.testing.assert_close(captured["joint_ids"], torch.tensor((0, 1)))
+
+
+def test_gripper_observation_and_action_rate_use_binary_command_state() -> None:
+    """Equivalent continuous gripper magnitudes have identical policy semantics and cost."""
+    manager = SimpleNamespace(
+        active_terms=["left_arm", "left_gripper", "right_arm", "right_gripper"],
+        action_term_dim=[2, 1, 2, 1],
+        action=torch.tensor(((0.1, -0.2, 0.01, 0.3, -0.4, -0.01),)),
+        prev_action=torch.tensor(((0.1, -0.2, 0.9, 0.3, -0.4, -0.8),)),
+    )
+    env = SimpleNamespace(action_manager=manager)
+    binary_names = ("left_gripper", "right_gripper")
+
+    observation = finite_last_action(env, binary_action_names=binary_names)
+    action_rate = finite_action_rate_l2(env, binary_action_names=binary_names)
+
+    torch.testing.assert_close(observation, torch.tensor(((0.1, -0.2, 1.0, 0.3, -0.4, -1.0),)))
+    torch.testing.assert_close(action_rate, torch.zeros(1))
+
+    manager.prev_action[:, -1] = 0.8
+    torch.testing.assert_close(finite_action_rate_l2(env, binary_names), torch.tensor((4.0,)))
+
+
+def test_terminal_outcome_rewards_are_control_frequency_independent() -> None:
+    """Configured terminal weights are exact one-time returns, with failure overriding success."""
+    command = SimpleNamespace(
+        succeeded=torch.tensor((True, False, True)),
+        ensure_route_state_current=lambda **_kwargs: None,
+    )
+    term_values = {
+        "invalid_cable": torch.tensor((False, True, False)),
+        "invalid_robot_or_action": torch.tensor((False, False, True)),
+    }
+    step_dt = 1.0 / 30.0
+    env = SimpleNamespace(
+        num_envs=3,
+        device="cpu",
+        step_dt=step_dt,
+        command_manager=SimpleNamespace(get_term=lambda _name: command),
+        termination_manager=SimpleNamespace(get_term=lambda name: term_values[name]),
+    )
+    failure_names = tuple(term_values)
+
+    success_return = route_success(env, "route", failure_names) * 20.0 * step_dt
+    failure_return = route_failure(env, failure_names) * -20.0 * step_dt
+
+    torch.testing.assert_close(success_return, torch.tensor((20.0, 0.0, 0.0)))
+    torch.testing.assert_close(failure_return, torch.tensor((0.0, -20.0, -20.0)))
+
+
+def test_cable_routing_ppo_starts_from_complete_1p2_second_rollouts() -> None:
+    """The first SuccessMonitor outcomes come from complete episodes and sufficiently long rollouts."""
+    env_cfg = CableRoutingEnvCfg()
+    runner_cfg = CableRoutingPPORunnerCfg()
+
+    assert env_cfg.sim.use_newton_actuators
+    assert runner_cfg.num_steps_per_env * env_cfg.decimation * env_cfg.sim.dt == pytest.approx(1.2)
+    assert not runner_cfg.init_at_random_ep_len
+
+
 def test_cable_routing_manager_terms_use_menagerie_joint_and_body_names() -> None:
     """Test observations, events, and rewards resolve against the canonical YAM names."""
     cfg = CableRoutingEnvCfg()
@@ -155,6 +252,7 @@ def test_cable_routing_rewards_are_sparse_success_with_penalties_only() -> None:
     """Test every route variant excludes dense geometric reward shaping."""
     expected_reward_names = {
         "success",
+        "failure",
         "stretch",
         "action_rate",
         "left_joint_velocity",
@@ -512,6 +610,7 @@ def test_cable_routing_uses_disjoint_newton_collision_ownership() -> None:
     physics = cfg.sim.physics
 
     assert isinstance(physics, NewtonCfg)
+    assert cfg.sim.use_newton_actuators
     assert physics.num_substeps == 10
     assert physics.use_cuda_graph
     assert physics.default_shape_cfg.ke == 4.0e4