From 24d3effcb29d1ddbd0ba4d0a1cec7191ce4c3e9a Mon Sep 17 00:00:00 2001 From: Sylvester Kaczmarek <16242628+sylvesterkaczmarek@users.noreply.github.com> Date: Thu, 1 Oct 2026 01:25:47 +0100 Subject: [PATCH] Refresh HolonomicController when wheel geometry changes Signed-off-by: Sylvester Kaczmarek <16242628+sylvesterkaczmarek@users.noreply.github.com> --- .../python/nodes/OgnHolonomicController.py | 10 +++ ...test_holonomic_controller_input_refresh.py | 64 +++++++++++++++++++ 2 files changed, 74 insertions(+) create mode 100644 source/extensions/isaacsim.robot.wheeled_robots.nodes/python/tests/test_holonomic_controller_input_refresh.py diff --git a/source/extensions/isaacsim.robot.wheeled_robots.nodes/python/nodes/OgnHolonomicController.py b/source/extensions/isaacsim.robot.wheeled_robots.nodes/python/nodes/OgnHolonomicController.py index 7e091209f1..3ae8d281c2 100644 --- a/source/extensions/isaacsim.robot.wheeled_robots.nodes/python/nodes/OgnHolonomicController.py +++ b/source/extensions/isaacsim.robot.wheeled_robots.nodes/python/nodes/OgnHolonomicController.py @@ -132,6 +132,16 @@ def compute(db: OgnHolonomicControllerDatabase) -> bool: state = db.per_instance_state try: + if state.initialized and ( + not np.array_equal(db.inputs.wheelRadius, state.wheel_radius) + or not np.array_equal(db.inputs.wheelPositions, state.wheel_positions) + or not np.array_equal(db.inputs.wheelOrientations, state.wheel_orientations) + or not np.array_equal(db.inputs.mecanumAngles, state.mecanum_angles) + or not np.array_equal(db.inputs.wheelAxis, state.wheel_axis) + or not np.array_equal(db.inputs.upAxis, state.up_axis) + ): + state.initialized = False + if not state.initialized: stop = False error_log = "" diff --git a/source/extensions/isaacsim.robot.wheeled_robots.nodes/python/tests/test_holonomic_controller_input_refresh.py b/source/extensions/isaacsim.robot.wheeled_robots.nodes/python/tests/test_holonomic_controller_input_refresh.py new file mode 100644 index 0000000000..199394d6c3 --- /dev/null +++ b/source/extensions/isaacsim.robot.wheeled_robots.nodes/python/tests/test_holonomic_controller_input_refresh.py @@ -0,0 +1,64 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Regression coverage for HolonomicController runtime configuration refresh.""" + +import isaacsim.core.experimental.utils.app as app_utils +import isaacsim.core.experimental.utils.stage as stage_utils +import numpy as np +import omni.graph.core as og +import omni.graph.core.tests as ogts +import omni.kit.stage_templates + + +class TestHolonomicControllerInputRefresh(ogts.OmniGraphTestCase): + """Verify changed wheel geometry is applied without resetting the graph.""" + + async def setUp(self) -> None: + await stage_utils.create_new_stage_async() + stage_utils.set_stage_up_axis("Z") + stage_utils.set_stage_units(meters_per_unit=1.0) + await app_utils.update_app_async() + + async def tearDown(self) -> None: + app_utils.stop() + await app_utils.update_app_async() + await omni.kit.stage_templates.new_stage_async() + + async def test_wheel_radius_change_rebuilds_controller(self) -> None: + graph, [controller], _, _ = og.Controller.edit( + {"graph_path": "/ActionGraph"}, + { + og.Controller.Keys.CREATE_NODES: [ + ("HolonomicController", "isaacsim.robot.wheeled_robots.HolonomicController"), + ], + og.Controller.Keys.SET_VALUES: [ + ("HolonomicController.inputs:wheelRadius", [0.04, 0.04, 0.04]), + ( + "HolonomicController.inputs:wheelPositions", + [ + [-0.0980432, 0.000636773, -0.050501], + [0.0493475, -0.084525, -0.050501], + [0.0495291, 0.0856937, -0.050501], + ], + ), + ( + "HolonomicController.inputs:wheelOrientations", + [[0, 0, 0, 1], [0.866, 0, 0, -0.5], [0.866, 0, 0, 0.5]], + ), + ("HolonomicController.inputs:mecanumAngles", [90, 90, 90]), + ("HolonomicController.inputs:inputVelocity", [1.0, 1.0, 0.1]), + ], + }, + ) + + await og.Controller.evaluate(graph) + first = np.asarray(og.Controller(og.Controller.attribute("outputs:jointVelocityCommand", controller)).get()) + + og.Controller.attribute("inputs:wheelRadius", controller).set([0.08, 0.08, 0.08]) + await og.Controller.evaluate(graph) + second = np.asarray(og.Controller(og.Controller.attribute("outputs:jointVelocityCommand", controller)).get()) + + self.assertTrue(np.all(np.isfinite(first))) + self.assertTrue(np.all(np.isfinite(second))) + self.assertTrue(np.allclose(second, first / 2.0, rtol=1e-4, atol=1e-4))