From ad8b371d9828f74a19da9e0c54f529ebe1efa827 Mon Sep 17 00:00:00 2001 From: Maximilian Krause <99733341+maxkra15@users.noreply.github.com> Date: Thu, 17 Sep 2026 16:05:07 +0200 Subject: [PATCH 1/5] Fix Franka Pour collider variant (#7866) # Description Pin the Franka Pour robot asset to its authored `Colliders=convex_hulls` variant. The primitive default omits the arm collision meshes (`link0_c` through `link7_c`) required by the task, preventing the current Newton MPM environment from starting correctly. This keeps the current 70% / 735-particle layout unchanged. ## Type of change - Bug fix (non-breaking change which fixes an issue) ## Release backport - [x] Backport this pull request to the active release branch after it merges into `develop` ## Screenshots Not applicable. ## Validation - `uv run --extra test python -m pytest -q source/isaaclab_tasks/test/contrib/test_franka_pour_env_cfg.py` (13 passed) - `uv run isaaclab -f` - Local Newton GL playback with the published `model_6997.pt` checkpoint and one current-layout environment ## Checklist - [x] I have read and understood the contribution guidelines - [x] I have run the pre-commit checks - [x] Documentation changes are not required for this task configuration fix - [x] My changes generate no new warnings - [x] I have added a test that proves the asset variant contract - [x] I have added a changelog fragment for `isaaclab_tasks` - [x] My name already exists in `CONTRIBUTORS.md` --- .../changelog.d/fix-franka-pour-convex-hulls.rst | 5 +++++ .../isaaclab_tasks/contrib/franka_pour/pour_env_cfg.py | 1 + .../isaaclab_tasks/test/contrib/test_franka_pour_env_cfg.py | 1 + 3 files changed, 7 insertions(+) create mode 100644 source/isaaclab_tasks/changelog.d/fix-franka-pour-convex-hulls.rst diff --git a/source/isaaclab_tasks/changelog.d/fix-franka-pour-convex-hulls.rst b/source/isaaclab_tasks/changelog.d/fix-franka-pour-convex-hulls.rst new file mode 100644 index 000000000000..3826c316199c --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/fix-franka-pour-convex-hulls.rst @@ -0,0 +1,5 @@ +Fixed +^^^^^ + +* Fixed the Franka Pour task selecting the primitive robot collider variant, + which omitted the arm collision meshes required by the task. diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_pour/pour_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_pour/pour_env_cfg.py index 2662affebd7a..775aeb222ec8 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_pour/pour_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_pour/pour_env_cfg.py @@ -339,6 +339,7 @@ class PourSceneCfg(InteractiveSceneCfg): ) robot = FRANKA_PANDA_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") robot.spawn.usd_path = FRANKA_POUR_ROBOT_USD_PATH + robot.spawn.variants = {"Colliders": "convex_hulls"} robot.spawn.func = spawn_franka_with_arm_collisions robot.spawn.articulation_props.enabled_self_collisions = True robot.actuators = { diff --git a/source/isaaclab_tasks/test/contrib/test_franka_pour_env_cfg.py b/source/isaaclab_tasks/test/contrib/test_franka_pour_env_cfg.py index 17e936d5f5b1..37e4f5fd78dd 100644 --- a/source/isaaclab_tasks/test/contrib/test_franka_pour_env_cfg.py +++ b/source/isaaclab_tasks/test/contrib/test_franka_pour_env_cfg.py @@ -111,6 +111,7 @@ def test_reset_dataset_contract_stores_root_relative_robot_asset_path(): robot_asset = _reset_dataset_task_contract(cfg)["robot_asset"] assert robot_asset == "Robots/FrankaEmika/franka_panda.usda" assert f"{ISAACLAB_NUCLEUS_DIR}/{robot_asset}" == FRANKA_POUR_ROBOT_ASSET_ID + assert cfg.scene.robot.spawn.variants == {"Colliders": "convex_hulls"} def test_capacity_resolution_only_updates_world_dependent_solver_limits(): From c5d33efb97cb5bc2db9c976d8636c0751ad4f2b8 Mon Sep 17 00:00:00 2001 From: ooctipus Date: Thu, 17 Sep 2026 07:09:10 -0700 Subject: [PATCH 2/5] Fix Franka Lift reset sampling after the collider default changed (#7870) Franka Lift can stall while filling its reset-state bank: the updated `franka_panda.usda` defaults to `Colliders=primitives`, whose first-link capsule extends 6 cm below the ground. The existing ground-clearance criterion rejects every candidate, leaving the RSL-RL export batch at `0/2048` until its 600-second timeout. This PR selects `Colliders=convex_hulls` on the robot configuration shared by rigid Franka Lift and Reorient, restoring the previous collision geometry and successful reset sampling. The affected asset and reset setup originated in [#6457](https://github.com/isaac-sim/IsaacLab/pull/6457) ([564a0083a0](https://github.com/isaac-sim/IsaacLab/commit/564a0083a00a11a07a47c469ddda3b41331a0ccf)), then moved into the current Lift tasks in [#6732](https://github.com/isaac-sim/IsaacLab/pull/6732) ([73106da3f7](https://github.com/isaac-sim/IsaacLab/commit/73106da3f71679a3d174e9219b855b02aa87390e)). The triggering change is external: the production Franka asset's top layer now selects primitive colliders and reports `Last-Modified: 2026-09-15 22:49:27 GMT`. Controlled comparisons found 0 valid states in 128 draws with the new default; explicit convex hulls and the preserved old asset both produced 63 valid states from the same draws. The same reset hang appeared in CI for #7838, #7608, #7832, #7834, and #7866. [The #7866 RL log](https://github.com/isaac-sim/IsaacLab/actions/runs/35202342707/job/105139978507) shows the batch blocked on Franka Lift after environment setup. #7866 fixes the independently configured Franka Pour task; it does not change Lift's robot configuration. This change covers Lift/Reorient only. Validation: - Fresh regression check on this branch: both Lift/Reorient cases failed without the production fix; all 9 tests in `test_lift_env_cfg.py` passed with it. - `uv run isaaclab -f` passed. - Earlier GPU validation of the identical Lift change completed the original 2,048-state harvest and reset in 22.169 seconds; the existing four-task RSL-RL export batch passed in 107.92 seconds. - Local GPU validation used PyTorch 2.11+cu128, Newton 1.6rc1, and RSL-RL 5.4.1. The identical Lift fix also passed [the full locked-environment RL CI job in #7865](https://github.com/isaac-sim/IsaacLab/actions/runs/35207648168/job/105158711330). Reset-bank sizes, clearance criteria, export behavior, the global Franka defaults, and Pour remain unchanged. --- .../fix-franka-lift-collider-variant.rst | 5 ++++ .../core/lift/config/franka/franka_env_cfg.py | 2 ++ .../test/core/test_lift_env_cfg.py | 27 +++++++++++++++++++ 3 files changed, 34 insertions(+) create mode 100644 source/isaaclab_tasks/changelog.d/fix-franka-lift-collider-variant.rst diff --git a/source/isaaclab_tasks/changelog.d/fix-franka-lift-collider-variant.rst b/source/isaaclab_tasks/changelog.d/fix-franka-lift-collider-variant.rst new file mode 100644 index 000000000000..c52a004b6e7e --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/fix-franka-lift-collider-variant.rst @@ -0,0 +1,5 @@ +Fixed +^^^^^ + +* Fixed Franka Lift and Reorient reset sampling with updated Franka assets by explicitly selecting the convex-hull + arm colliders used by the tasks' clearance criteria. diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka/franka_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka/franka_env_cfg.py index 26c063d78da2..b244c84dcab0 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka/franka_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka/franka_env_cfg.py @@ -24,6 +24,8 @@ # legacy asset so the upstream franka tasks keep their demos and baselines. FRANKA_PANDA_LIFT_CFG = FRANKA_PANDA_CFG.copy() FRANKA_PANDA_LIFT_CFG.spawn.usd_path = f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/franka_panda.usda" +# Reset clearance was calibrated for these arm meshes; the asset's primitive colliders intersect the ground. +FRANKA_PANDA_LIFT_CFG.spawn.variants = {"Colliders": "convex_hulls"} FRANKA_PANDA_LIFT_CFG.actuators = { # Inspired by libfranka's joint_impedance_control.cpp. ``actuator_velocity_limit`` # remains the soft task-limit snapshot; ``joint_velocity_limit`` is the diff --git a/source/isaaclab_tasks/test/core/test_lift_env_cfg.py b/source/isaaclab_tasks/test/core/test_lift_env_cfg.py index e9ab35c1dbba..eb6de255122b 100644 --- a/source/isaaclab_tasks/test/core/test_lift_env_cfg.py +++ b/source/isaaclab_tasks/test/core/test_lift_env_cfg.py @@ -10,9 +10,13 @@ import pytest import torch +from pxr import Usd + from isaaclab.managers import CommandTerm +from isaaclab.sim import select_usd_variants from isaaclab_tasks.core.lift import mdp +from isaaclab_tasks.core.lift.config.franka.franka_env_cfg import FrankaLiftEnvCfg, FrankaReorientEnvCfg from isaaclab_tasks.core.lift.config.franka_soft.franka_soft_env_cfg import FrankaSoftEnvCfg from isaaclab_tasks.core.lift.mdp.commands.pose_commands import ( CableUniformPoseCommand, @@ -58,6 +62,29 @@ def test_franka_soft_robot_physics_variant_matches_backend( assert cfg.scene.robot.spawn.variants == {"Physics": expected_physics} +@pytest.mark.parametrize("cfg_type", [FrankaLiftEnvCfg, FrankaReorientEnvCfg]) +def test_franka_rigid_tasks_select_collision_meshes_for_reset_clearance(cfg_type) -> None: + """Reset validation keeps the original arm meshes when the asset defaults to capsules.""" + cfg = cfg_type() + stage = Usd.Stage.CreateInMemory() + robot = stage.DefinePrim("/Robot", "Xform") + colliders = robot.GetVariantSets().AddVariantSet("Colliders") + for selection, prim_path, prim_type in ( + ("convex_hulls", "/Robot/link1_c/link1_c", "Mesh"), + ("primitives", "/Robot/link1_capsule", "Capsule"), + ): + colliders.AddVariant(selection) + colliders.SetVariantSelection(selection) + with colliders.GetVariantEditContext(): + stage.DefinePrim(prim_path, prim_type) + colliders.SetVariantSelection("primitives") + + select_usd_variants("/Robot", cfg.scene.robot.spawn.variants or {}, stage=stage) + + assert stage.GetPrimAtPath("/Robot/link1_c/link1_c").IsValid() + assert not stage.GetPrimAtPath("/Robot/link1_capsule").IsValid() + + def test_camera_normalization_is_stationary() -> None: """RGB and depth normalization must not depend on per-frame statistics.""" rgb = torch.tensor([0.0, 127.5, 255.0]) From 24f61d963cfb81e65c448e84bde53b163da47812 Mon Sep 17 00:00:00 2001 From: Kelly Guo Date: Thu, 17 Sep 2026 07:21:50 -0700 Subject: [PATCH 3/5] Update nightly image pull request automation (#7867) # Description Update the nightly Isaac Sim image job so its generated pull request body follows the repository PR template. Automatically comment `run-ci` when the workflow opens a PR or pushes a new update commit, while avoiding duplicate triggers when an existing update branch is unchanged. This addresses the manual follow-up needed on #7773. No new dependencies are required. Validation: - Parsed the workflow as YAML. - Validated the workflow with `actionlint` (excluding its pre-existing `client-id`/`app-id` metadata mismatch for `actions/create-github-app-token@v3`). - Checked the embedded shell with `bash -n`. - Mocked new, changed, and unchanged PR cases and verified `run-ci` is posted exactly when needed. - `uv run isaaclab -f` could not run because the repository lockfile does not support macOS ARM64. ## Type of change - Infrastructure update ## Release backport - [ ] Backport this pull request to the active release branch after it merges into `develop` ## Screenshots Not applicable. ## Checklist Docker and GPU tests run on demand. Push the commits you want tested, then comment `run-ci` on the pull request. - [x] I have read and understood the [contribution guidelines](https://isaac-sim.github.io/IsaacLab/main/source/refs/contributing.html) - [ ] I have run the [`pre-commit` checks](https://pre-commit.com/) with `./isaaclab.sh --format` - [x] I have made corresponding changes to the documentation (not applicable; workflow-only change) - [x] My changes generate no new warnings - [x] I have added tests that prove my fix is effective or that my feature works (mocked workflow cases) - [x] I have added a changelog fragment under `source//changelog.d/` for every touched package (not applicable; no source package changed) - [x] I have added my name to the `CONTRIBUTORS.md` or my name already exists there --- .github/workflows/nightly-isaacsim-image.yml | 36 ++++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/.github/workflows/nightly-isaacsim-image.yml b/.github/workflows/nightly-isaacsim-image.yml index 7759da6d5ebb..d16355cd8ff8 100644 --- a/.github/workflows/nightly-isaacsim-image.yml +++ b/.github/workflows/nightly-isaacsim-image.yml @@ -178,6 +178,7 @@ jobs: CURRENT_PIN: ${{ steps.pin.outputs.current }} CANDIDATE_PIN: ${{ steps.pin.outputs.candidate }} DIGEST: ${{ steps.pin.outputs.digest }} + BRANCH_CHANGED: ${{ steps.pin.outputs.branch_changed }} run: | set -euo pipefail @@ -186,6 +187,8 @@ jobs: title="[CI] Bump Isaac Sim image to $short_digest" body_file="$RUNNER_TEMP/isaacsim-image-update.md" { + echo "# Description" + echo echo "This automated draft updates CI to the current Isaac Sim nightly image." echo echo "| Field | Value |" @@ -198,6 +201,30 @@ jobs: echo "Source: https://registry.ngc.nvidia.com/orgs/0947644777160149/teams/internal/containers/isaac-sim/tags" echo echo "New PRs are opened as drafts so maintainers can merge after the CI results are acceptable." + echo + echo "## Type of change" + echo + echo "- Infrastructure update" + echo + echo "## Release backport" + echo + echo '- [ ] Backport this pull request to the active release branch after it merges into `develop`' + echo + echo "## Screenshots" + echo + echo "Not applicable." + echo + echo "## Checklist" + echo + echo "Docker and GPU tests run on demand. This workflow comments \`run-ci\` after opening the pull request or updating its commit." + echo + echo '- [ ] I have read and understood the [contribution guidelines](https://isaac-sim.github.io/IsaacLab/main/source/refs/contributing.html)' + echo '- [ ] I have run the [`pre-commit` checks](https://pre-commit.com/) with `./isaaclab.sh --format`' + echo '- [ ] I have made corresponding changes to the documentation' + echo '- [ ] My changes generate no new warnings' + echo '- [ ] I have added tests that prove my fix is effective or that my feature works' + echo '- [ ] I have added a changelog fragment under `source//changelog.d/` for every touched package (do **not** edit `CHANGELOG.rst` or bump `extension.toml` — CI handles that)' + echo '- [ ] I have added my name to the `CONTRIBUTORS.md` or my name already exists there' } > "$body_file" repository_owner=${REPOSITORY%%/*} @@ -207,6 +234,7 @@ jobs: -f head="$repository_owner:$UPDATE_BRANCH" \ --jq '.[0].number // empty') + pr_created=false if [ -n "$pr_number" ]; then pr_url=$(gh api --method PATCH "repos/$REPOSITORY/pulls/$pr_number" \ -f title="$title" \ @@ -222,10 +250,18 @@ jobs: -F body=@"$body_file" \ -F draft=true \ --jq '.html_url') + pr_number=${pr_url##*/} + pr_created=true echo "Opened draft PR: $pr_url" echo "Draft PR: $pr_url" >> "$GITHUB_STEP_SUMMARY" fi + if [ "$BRANCH_CHANGED" = "true" ] || [ "$pr_created" = "true" ]; then + gh api --method POST "repos/$REPOSITORY/issues/$pr_number/comments" \ + -f body='run-ci' >/dev/null + echo "Triggered CI on PR #$pr_number." + fi + - name: Report no-op or dry run if: ${{ steps.pin.outputs.changed != 'true' || inputs.dry_run }} env: From 0921d32621dc849da9e9458f03a7f5940255043f Mon Sep 17 00:00:00 2001 From: ooctipus Date: Thu, 17 Sep 2026 08:04:52 -0700 Subject: [PATCH 4/5] Let standalone demos complete the health check after startup (#7869) A standalone demo that becomes ready at 299 seconds is currently terminated at the 300-second startup deadline, before its five-second soak can finish. This caused the arms smoke test in #7608 and #7674 to fail after printing its setup and reset messages. Stop applying the startup deadline once the readiness marker has been observed, while preserving the existing 300-second startup and five-second soak settings. The unconditional deadline was introduced by [#6704](https://github.com/isaac-sim/IsaacLab/pull/6704), commit [16083cc85d3](https://github.com/isaac-sim/IsaacLab/commit/16083cc85d318e234dad1825af2554c0dae06db1). This fix does not address the separate arms failures in #7838 and #7866 that never reached readiness within 300 seconds. Validation: the deterministic regression fails on develop and passes with this change; all 11 supervisor checks pass; `uv run isaaclab -f` passes. --- .../fix-standalone-soak-deadline.skip | 1 + .../test/app/standalone_script_cases.py | 2 +- .../test/app/test_standalone_scripts.py | 32 +++++++++++++++++++ 3 files changed, 34 insertions(+), 1 deletion(-) create mode 100644 source/isaaclab/changelog.d/fix-standalone-soak-deadline.skip diff --git a/source/isaaclab/changelog.d/fix-standalone-soak-deadline.skip b/source/isaaclab/changelog.d/fix-standalone-soak-deadline.skip new file mode 100644 index 000000000000..f08e4d9ff050 --- /dev/null +++ b/source/isaaclab/changelog.d/fix-standalone-soak-deadline.skip @@ -0,0 +1 @@ +Allowed standalone smoke tests that reached readiness before the startup deadline to complete their full soak interval. diff --git a/source/isaaclab/test/app/standalone_script_cases.py b/source/isaaclab/test/app/standalone_script_cases.py index d9a5e89351b7..f220f033ab16 100644 --- a/source/isaaclab/test/app/standalone_script_cases.py +++ b/source/isaaclab/test/app/standalone_script_cases.py @@ -489,7 +489,7 @@ def read_available_output(timeout: float) -> bool: _terminate_process_group(process) returncode = process.poll() break - if now - start_time >= startup_timeout: + if ready_at is None and now - start_time >= startup_timeout: _terminate_process_group(process) returncode = process.poll() break diff --git a/source/isaaclab/test/app/test_standalone_scripts.py b/source/isaaclab/test/app/test_standalone_scripts.py index 3d0b8c6480cf..a43cb2349b27 100644 --- a/source/isaaclab/test/app/test_standalone_scripts.py +++ b/source/isaaclab/test/app/test_standalone_scripts.py @@ -22,6 +22,7 @@ import sys from dataclasses import replace from pathlib import Path +from unittest import mock import pytest import standalone_script_cases as script_cases @@ -371,6 +372,37 @@ def test_subprocess_supervisor_soaks_then_stops_process_group(): assert result.elapsed < 2.0 +def test_subprocess_supervisor_completes_soak_after_startup_deadline(monkeypatch): + """Readiness just before the startup deadline must still receive the full soak.""" + process = mock.Mock(returncode=None) + process.poll.side_effect = lambda: process.returncode + process.communicate.return_value = (b"", None) + selector = mock.Mock() + now = 0.0 + poll_times = iter((299.0, 300.0, 304.0)) + + def select(timeout): + nonlocal now + if timeout == 0.0: + return [] + now = next(poll_times) + if now == 299.0: + return [(mock.Mock(fileobj=process.stdout), script_cases.selectors.EVENT_READ)] + return [] + + selector.select.side_effect = select + monkeypatch.setattr(script_cases.subprocess, "Popen", lambda *args, **kwargs: process) + monkeypatch.setattr(script_cases.selectors, "DefaultSelector", lambda: selector) + monkeypatch.setattr(script_cases.os, "read", lambda *args: b"READY\n") + monkeypatch.setattr(script_cases.time, "monotonic", lambda: now) + monkeypatch.setattr(script_cases, "_terminate_process_group", lambda process: setattr(process, "returncode", -15)) + + result = run_until_ready(["demo.py"], r"READY", startup_timeout=300.0, soak_time=5.0) + assert result.ready + assert result.stopped_after_soak + assert result.elapsed == 304.0 + + def test_subprocess_supervisor_ignores_fatal_output_after_intentional_teardown(monkeypatch): """Fatal-looking output caused by intentional teardown must not fail a healthy launch.""" From e8363313c1441788a4619157954b1842d3857759 Mon Sep 17 00:00:00 2001 From: ooctipus Date: Thu, 17 Sep 2026 09:16:11 -0700 Subject: [PATCH 5/5] Fix operational-space feedback and task-frame consistency (#7868) `OperationalSpaceControllerAction` and the OSC integration-test helper used link-origin poses and Jacobians together with center-of-mass velocity feedback. When a link rotates with an offset center of mass, these quantities describe different points, producing incorrect velocity feedback. The task-frame test also copied rounded command quaternions directly into reference frames, producing inconsistent composed targets. Use `body_link_vel_w` and `root_link_vel_w` in both OSC callers, and normalize the test's cloned task-frame quaternion before coordinate conversion. Controller gains, raw command fixtures, convergence tolerances, and step budgets match the base branch; the nullspace damping ratio remains 1.0. Two direct regressions guard these contracts: a moving, fixed-base Franka checks each caller's velocity against its link Jacobian multiplied by measured joint velocity, and a numerical check verifies that the same rounded command resolves to the same absolute target through root and task frames without mutating its inputs. Provenance: - [#5400](https://github.com/isaac-sim/IsaacLab/pull/5400) migrated both OSC callers to `body_link_jacobian_w` while retaining the COM velocity accessors. - The rounded fixtures and unnormalized task-frame copy originated in [#913](https://github.com/isaac-sim/IsaacLab/pull/913). [#7624](https://github.com/isaac-sim/IsaacLab/pull/7624) later normalized absolute pose commands, while the separately supplied frame remained unnormalized. The exact change that first exposed the convergence failures has not been bisected; these references establish the input inconsistencies, not the first failing CI run. Validation: - Restoring COM feedback makes both velocity regression cases fail, with a maximum linear-velocity discrepancy of 0.01469 m/s versus the 1e-4 assertion tolerance. Both pass with link-origin feedback. - Removing frame normalization makes the target-equivalence regression fail with a quaternion-component discrepancy of 0.00010675 versus the 1e-6 tolerance. It passes with the correction. - The previously failing `test_franka_taskframe_pose_abs` and original `test_franka_pose_abs_with_nullspace_centering` both pass with damping 1.0. - Full OSC integration file: **21 passed** in 113 seconds, including all three new regression cases. - `uv run isaaclab -f` passes. The earlier RL, legacy-rendering, and contrib failures have matching failures and passing corresponding jobs in separate fixes: #7870, #7871, and #7866, respectively. Those fixes remain separate from these OSC corrections. --------- Co-authored-by: Kelly Guo --- .../changelog.d/fix-osc-link-velocity.rst | 5 ++ .../envs/mdp/actions/task_space_actions.py | 6 +- .../controllers/test_operational_space.py | 73 ++++++++++++++++++- 3 files changed, 78 insertions(+), 6 deletions(-) create mode 100644 source/isaaclab/changelog.d/fix-osc-link-velocity.rst diff --git a/source/isaaclab/changelog.d/fix-osc-link-velocity.rst b/source/isaaclab/changelog.d/fix-osc-link-velocity.rst new file mode 100644 index 000000000000..87556a701d28 --- /dev/null +++ b/source/isaaclab/changelog.d/fix-osc-link-velocity.rst @@ -0,0 +1,5 @@ +Fixed +^^^^^ + +* Fixed operational-space controller velocity feedback to use the same link origins as the end-effector pose and + Jacobian in the action term and integration tests. diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py index ea9d319938c5..e34343ef3194 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py @@ -712,10 +712,10 @@ def _compute_ee_pose(self): def _compute_ee_velocity(self): """Computes the velocity of the ee frame in root frame.""" - # Extract end-effector velocity in the world frame - self._ee_vel_w[:] = self._asset.data.body_vel_w.torch[:, self._ee_body_idx, :] + # Match the link-origin reference point used by the pose and Jacobian. + self._ee_vel_w[:] = self._asset.data.body_link_vel_w.torch[:, self._ee_body_idx, :] # Compute the relative velocity in the world frame - relative_vel_w = self._ee_vel_w - self._asset.data.root_vel_w.torch + relative_vel_w = self._ee_vel_w - self._asset.data.root_link_vel_w.torch # Convert ee velocities from world to root frame root_quat_w = self._asset.data.root_quat_w.torch diff --git a/source/isaaclab/test/controllers/test_operational_space.py b/source/isaaclab/test/controllers/test_operational_space.py index ab4c46acc2d9..1018d3c34726 100644 --- a/source/isaaclab/test/controllers/test_operational_space.py +++ b/source/isaaclab/test/controllers/test_operational_space.py @@ -12,6 +12,8 @@ """Rest everything follows.""" +from types import SimpleNamespace + import numpy as np import pytest import torch @@ -31,6 +33,7 @@ ## from isaaclab.envs import ManagerBasedEnv, ManagerBasedEnvCfg from isaaclab.envs.mdp.actions.actions_cfg import OperationalSpaceControllerActionCfg +from isaaclab.envs.mdp.actions.task_space_actions import OperationalSpaceControllerAction from isaaclab.managers import ObservationGroupCfg as ObsGroup from isaaclab.managers import ObservationTermCfg as ObsTerm from isaaclab.managers import SceneEntityCfg @@ -861,6 +864,23 @@ def test_franka_hybrid_variable_kp_impedance(sim): ) +@pytest.mark.isaacsim_ci +def test_task_frame_conversion_preserves_absolute_target(): + """A rounded pose command must resolve to the same target through either reference frame.""" + osc_cfg = OperationalSpaceControllerCfg(target_types=["pose_abs"]) + osc = OperationalSpaceController(osc_cfg, num_envs=1, device="cpu") + target_b = torch.tensor([[0.5, -0.4, 0.6, 0.707, 0.0, 0.0, 0.707]]) + command = target_b.clone() + resolved_targets = [] + for frame in ("root", "task"): + converted_command, task_frame_pose_b = _convert_to_task_frame(osc, command, target_b, frame) + osc.set_command(converted_command, current_task_frame_pose_b=task_frame_pose_b) + resolved_targets.append(osc.desired_ee_pose_b.clone()) + + torch.testing.assert_close(resolved_targets[0], resolved_targets[1], atol=1e-6, rtol=0.0) + torch.testing.assert_close(command, target_b, atol=0.0, rtol=0.0) + + @pytest.mark.isaacsim_ci def test_franka_taskframe_pose_abs(sim): """Test absolute pose control in task frame with fixed impedance and inertial dynamics decoupling.""" @@ -1336,6 +1356,51 @@ class _FloatingBaseOscEnvCfg(ManagerBasedEnvCfg): sim: sim_utils.SimulationCfg = sim_utils.SimulationCfg(dt=0.01) +@pytest.mark.isaacsim_ci +@pytest.mark.parametrize("feedback_source", ["test_helper", "action"]) +def test_franka_velocity_feedback_matches_jacobian(sim, feedback_source): + """Both OSC callers must measure velocity at the link origin used by the Jacobian.""" + sim_context, num_envs, robot_cfg, *_ = sim + robot = Articulation(cfg=robot_cfg) + sim_context.reset() + arm_joint_ids, _ = robot.find_joints("panda_joint.*") + ee_frame_idx = robot.find_bodies("panda_hand")[0][0] + + joint_vel = torch.zeros_like(robot.data.default_joint_vel.torch) + joint_vel[:, arm_joint_ids] = torch.linspace(0.1, 0.7, len(arm_joint_ids), device=sim_context.device) + robot.write_joint_state_to_sim_index(position=robot.data.default_joint_pos.torch, velocity=joint_vel) + sim_context.step(render=False) + robot.update(sim_context.get_physics_dt()) + + # Angular motion and the hand's COM offset must expose the reference-point mismatch. + assert not torch.allclose( + robot.data.body_com_vel_w.torch[:, ee_frame_idx, :3], + robot.data.body_link_vel_w.torch[:, ee_frame_idx, :3], + atol=1e-4, + rtol=1e-4, + ) + if feedback_source == "test_helper": + states = _update_states(robot, ee_frame_idx, arm_joint_ids, sim_context, None, num_envs) + jacobian_b, _, _, _, ee_vel_b, _, _, _, _, joint_vel = states + else: + env = SimpleNamespace(scene={"robot": robot}, sim=sim_context, num_envs=num_envs, device=sim_context.device) + action_cfg = OperationalSpaceControllerActionCfg( + asset_name="robot", + joint_names=["panda_joint.*"], + body_name="panda_hand", + controller_cfg=OperationalSpaceControllerCfg(target_types=["pose_abs"]), + ) + action_term = OperationalSpaceControllerAction(action_cfg, env) + action_term._compute_ee_jacobian() + action_term._compute_ee_velocity() + jacobian_b, ee_vel_b = action_term._jacobian_b, action_term._ee_vel_b + joint_vel = robot.data.joint_vel.torch[:, arm_joint_ids] + + # With a stationary fixed base, the link twist must equal J(q) * q_dot. + expected_vel_b = torch.bmm(jacobian_b, joint_vel.unsqueeze(-1)).squeeze(-1) + torch.testing.assert_close(ee_vel_b, expected_vel_b, atol=1e-4, rtol=1e-4) + + @pytest.mark.isaacsim_ci def test_floating_base_osc_action_term_indexing(): """Regression test for #4999 / PR #5107: verify OperationalSpaceControllerAction uses correct @@ -1635,9 +1700,9 @@ def _update_states( ) ee_pose_b = torch.cat([ee_pos_b, ee_quat_b], dim=-1) - # Compute the current velocity of the end-effector - ee_vel_w = robot.data.body_vel_w.torch[:, ee_frame_idx, :] # Extract end-effector velocity in the world frame - root_vel_w = robot.data.root_vel_w.torch # Extract root velocity in the world frame + # Match the link-origin reference point used by the pose and Jacobian. + ee_vel_w = robot.data.body_link_vel_w.torch[:, ee_frame_idx, :] + root_vel_w = robot.data.root_link_vel_w.torch relative_vel_w = ee_vel_w - root_vel_w # Compute the relative velocity in the world frame ee_lin_vel_b = quat_apply_inverse(robot.data.root_quat_w.torch, relative_vel_w[:, 0:3]) # From world to root frame ee_ang_vel_b = quat_apply_inverse(robot.data.root_quat_w.torch, relative_vel_w[:, 3:6]) @@ -1754,6 +1819,8 @@ def _convert_to_task_frame( # Convert target commands from base to the task frame command = command.clone() task_frame_pose_b = ee_target_pose_b.clone() + # Rounded goal quaternions must define a unit rotation when used as a reference frame. + task_frame_pose_b[:, 3:] /= torch.linalg.vector_norm(task_frame_pose_b[:, 3:], dim=-1, keepdim=True) cmd_idx = 0 for target_type in osc.cfg.target_types: