From 5d9231f59f9b746d46aac0430662a525fddce90f Mon Sep 17 00:00:00 2001 From: maxconway Date: Wed, 16 Sep 2026 15:39:08 -0600 Subject: [PATCH 1/7] Restore Isaac Lab ROS digital twin control and sensors --- sim_main.py | 751 +++--------------- src/python/control/RobotController.py | 13 +- src/python/control/action_provider_dds.py | 280 ++----- src/python/control/motor_contract.py | 34 + src/python/control/pacing.py | 29 + src/python/control/unitree_action.py | 83 ++ src/python/dds/common/dds_create.py | 2 + src/python/dds/common/dds_master.py | 35 +- src/python/dds/common/transport.py | 51 ++ src/python/dds/specialized/h12_robot_dds.py | 14 +- src/python/dds/specialized/inspire_dds.py | 6 +- src/python/sensors/livox_dds.py | 68 ++ src/python/sensors/ros_sensor_bridge.py | 253 ++++++ tasks/common_config/camera_configs.py | 3 +- tasks/common_config/ray_caster_configs.py | 94 +-- tasks/common_observations/h12_27dof_state.py | 459 +++-------- tasks/common_observations/inspire_state.py | 187 ++--- .../mdp/observations.py | 6 +- ...gyblock_h12_27dof_inspire_joint_env_cfg.py | 23 +- 19 files changed, 922 insertions(+), 1469 deletions(-) create mode 100644 src/python/control/motor_contract.py create mode 100644 src/python/control/pacing.py create mode 100644 src/python/control/unitree_action.py create mode 100644 src/python/dds/common/transport.py create mode 100644 src/python/sensors/livox_dds.py create mode 100644 src/python/sensors/ros_sensor_bridge.py diff --git a/sim_main.py b/sim_main.py index 4d7db615..55488c7f 100644 --- a/sim_main.py +++ b/sim_main.py @@ -1,663 +1,112 @@ - -# Copyright (c) 2025, Unitree Robotics Co., Ltd. All Rights Reserved. -# License: Apache License, Version 2.0 #!/usr/bin/env python3 -# main.py -import sys -print(sys.stdin) -print(sys.stdin.isatty()) -#breakpoint() -import os -import asyncio - -import traceback -project_root = os.path.dirname(os.path.abspath(__file__)) -os.environ["PROJECT_ROOT"] = project_root - +"""Run the H1-2 Isaac Lab digital-twin interfaces on the simulation DDS domain.""" import argparse -import contextlib -import time -import sys -import signal -import torch -import gymnasium as gym +from contextlib import ExitStack +import os from pathlib import Path -import time -from time import perf_counter - -# Isaac Lab AppLauncher -from isaaclab.app import AppLauncher - -from src.python.dds.common.dds_create import create_dds_objects -# add command line arguments -parser = argparse.ArgumentParser(description="Unitree Simulation") -parser.add_argument("--task", type=str, default="Isaac-PickPlace-G129-Head-Waist-Fix", help="task name") -parser.add_argument("--action_source", type=str, default="dds", - choices=["dds", "file", "trajectory", "policy", "replay","dds_wholebody"], - help="Action source") - - -parser.add_argument("--robot_type", type=str, default="h1_2", help="robot type") -parser.add_argument("--enable_dex1_dds", action="store_true", help="enable gripper DDS") -parser.add_argument("--enable_dex3_dds", action="store_true", help="enable dexterous hand DDS") -parser.add_argument("--enable_inspire_dds", action="store_true", help="enable inspire hand DDS") -parser.add_argument("--stats_interval", type=float, default=10.0, help="statistics print interval (seconds)") - -parser.add_argument("--file_path", type=str, default="/home/unitree/Code/xr_teleoperate/teleop/utils/data", help="file path (when action_source=file)") -parser.add_argument("--generate_data_dir", type=str, default="./data", help="save data dir") -parser.add_argument("--generate_data", action="store_true", default=False, help="generate data") -parser.add_argument("--rerun_log", action="store_true", default=False, help="rerun log") -parser.add_argument("--replay_data", action="store_true", default=False, help="replay data") - -parser.add_argument("--modify_light", action="store_true", default=False, help="modify light") -parser.add_argument("--modify_camera", action="store_true", default=False, help="modify camera") - -# performance analysis parameters -parser.add_argument("--step_hz", type=int, default=100, help="control frequency") -parser.add_argument("--enable_profiling", action="store_true", default=True, help="enable performance analysis") -parser.add_argument("--profile_interval", type=int, default=500, help="performance analysis report interval (steps)") - -parser.add_argument("--model_path", type=str, default="assets/model/policy.onnx", help="model path") -parser.add_argument("--reward_interval", type=int, default=10, help="step interval for reward calculation") -parser.add_argument("--enable_wholebody_dds", action="store_true", default=False, help="enable wh dds") - -parser.add_argument("--physics_dt", type=float, default=None, help="physics time step, e.g., 0.005") -parser.add_argument("--render_interval", type=int, default=None, help="render interval steps (>=1)") -parser.add_argument("--camera_write_interval", type=int, default=None, help="camera write interval steps (>=1)") - - -parser.add_argument("--no_render",action="store_true",default=False,help="disable rendering updates entirely (overrides render interval)",) -parser.add_argument("--public_ip",type=str,default="127.0.0.1",help="public ip") -parser.add_argument("--livestream_type", type=int, default=2, help="livestream type (0: no livestream, 1: WebRTC public network, 2: WebRTC private network)") - -parser.add_argument("--solver_iterations", type=int, default=None, help="physx solver iteration count (e.g., 4)") -parser.add_argument("--gravity_z", type=float, default=None, help="override gravity z (e.g., -9.8)") -parser.add_argument("--skip_cvtcolor", action="store_true", default=False, help="skip cv2.cvtColor if upstream already BGR") - -parser.add_argument("--camera_jpeg", action="store_true", default=True, help="enable JPEG compression for camera frames") -parser.add_argument("--camera_jpeg_quality", type=int, default=85, help="JPEG quality (1-100)") - -parser.add_argument("--physx_substeps", type=int, default=None, help="physx substeps per step") -parser.add_argument("--camera_include", type=str, default="front_camera,left_wrist_camera,right_wrist_camera", help="comma-separated camera names to enable") -parser.add_argument("--camera_exclude", type=str, default="world_camera", help="comma-separated camera names to disable") - -parser.add_argument("--env_reward_interval", type=int, default=5, help="environment reward compute interval (steps)") -parser.add_argument("--seed", type=int, default=42, help="environment seed") -# add AppLauncher parameters -AppLauncher.add_app_launcher_args(parser) -args_cli = parser.parse_args() -if args_cli.no_render: - os.environ["LIVESTREAM"] = str(args_cli.livestream_type) - os.environ["PUBLIC_IP"] = args_cli.public_ip -else: - os.environ["LIVESTREAM"] = "0" - -if args_cli.enable_dex3_dds and args_cli.enable_dex1_dds and args_cli.enable_inspire_dds: - print("Error: enable_dex3_dds and enable_dex1_dds and enable_inspire_dds cannot be enabled at the same time") - print("Please select one of the options") - sys.exit(1) - - -#import pinocchio -app_launcher = AppLauncher(args_cli) -simulation_app = app_launcher.app - -from src.python.control.RobotController import RobotController, ControlConfig -import tasks -from isaaclab_tasks.utils.parse_cfg import parse_env_cfg - -from tools.augmentation_utils import ( - update_light, - batch_augment_cameras_by_name, -) -import sys -print(sys.stdin) -print(sys.stdin.isatty()) -#breakpoint() - -from tools.data_json_load import sim_state_to_json -#from dds.sim_state_dds import * -from src.python.control.action_provider_dds import create_action_provider -#from tools.get_stiffness import get_robot_stiffness_from_env -from tools.get_reward import get_step_reward_value,get_current_rewards -from PIL import Image - -def setup_signal_handlers(controller,dds_manager=None,image_server=None): - """set signal handlers""" - def signal_handler(signum, frame): - print(f"\nreceived signal {signum}, stopping controller...") - try: - controller.stop() - except Exception as e: - print(f"Failed to stop controller: {e}") - try: - if dds_manager is not None: - dds_manager.stop_all_communication() - except Exception as e: - print(f"Failed to stop DDS: {e}") - try: - if image_server is not None: - image_server.stop() - except Exception as e: - print(f"Failed to stop image server: {e}") - signal.signal(signal.SIGINT, signal_handler) - signal.signal(signal.SIGTERM, signal_handler) - - -sys.path.append("/workspace/golem/isaac/mateo_ws/CL_isaaclab_sim/src/cpp/correll-ros2-camera/build") -sys.path.append("/workspace/golem/isaac/mateo_ws/CL_isaaclab_sim/src/cpp/correll-ros2-imu/build") -sys.path.append("/workspace/golem/isaac/mateo_ws/CL_isaaclab_sim/src/cpp/correll-ros2-cloud/build") +import signal +import traceback -#import ros2_point_cloud_node_py -#import ros2_camera_node_py -#import ros2_imu_node_py def main(): - """main function""" - # import cProfile - # import pstats - # import io - # profiler = cProfile.Profile() - # profiler.enable() - #breakpoint() - import os - import atexit - #try: - # os.setpgrp() - # #current_pgid = os.getpgrp() - # #print(f"Setting process group: {current_pgid}") - # - # #def cleanup_process_group(): - # # try: - # # #print(f"Cleaning up process group: {current_pgid}") - # # import signal - # # #os.killpg(current_pgid, signal.SIGTERM) - # # except Exception as e: - # # print(f"Failed to clean up process group: {e}") - # # - # #atexit.register(cleanup_process_group) - # - #except Exception as e: - # print(f"Failed to set process group: {e}") - print("=" * 60) - print("robot control system started") - print(f"Task: {args_cli.task}") - print(f"Action source: {args_cli.action_source}") - print("=" * 60) - - # parse environment configuration - try: - env_cfg = parse_env_cfg(args_cli.task, device=args_cli.device, num_envs=1) - env_cfg.env_name = args_cli.task - except Exception as e: - print(f"Failed to parse environment configuration: {e}") - return - - import isaacsim.core.utils.extensions as exts_utils - # create environment - print("\ncreate environment...") - try: - env_cfg.seed = args_cli.seed - env = gym.make(args_cli.task, cfg=env_cfg).unwrapped - env.seed(args_cli.seed) - try: - sensors_dict = getattr(env.scene, "sensors", {}) - if sensors_dict: - print("Sensors in the environment:") - for name, sensor in sensors_dict.items(): - print(name, sensor) - print("="*60) - except Exception as e: - print(f"[sim] failed to list sensors: {e}") - print(f"\ncreate environment success ...") - try: - env._reward_interval = max(1, int(args_cli.env_reward_interval)) - env._reward_counter = 0 - env._reward_last = None - print(f"[env] reward compute interval set to {env._reward_interval} steps") - except Exception as e: - print(f"[env] failed to set reward interval: {e}") - if args_cli.physics_dt is not None: - try: - env.sim.set_substep_time(args_cli.physics_dt) - print(f"[sim] physics dt set to {args_cli.physics_dt}") - except Exception: - try: - env.sim.dt = args_cli.physics_dt - print(f"[sim] physics dt assigned to env.sim.dt={args_cli.physics_dt}") - except Exception as e: - print(f"[sim] failed to set physics dt: {e}") - headless_mode = bool(getattr(args_cli, "headless", False)) - render_interval = None - if args_cli.render_interval is not None: - try: - render_interval = max(1, int(args_cli.render_interval)) - except Exception as e: - print(f"[sim] invalid render_interval value {args_cli.render_interval}: {e}") - try: - if args_cli.no_render: - env.sim.render_interval = 1_000_000 - env.sim.render_mode = "offscreen" - print("[sim] rendering disabled via --no_render") - elif headless_mode: - env.sim.render_mode = "offscreen" - env.sim.render_interval = render_interval or 1 - print(f"[sim] headless offscreen rendering every {env.sim.render_interval} steps") - elif render_interval is not None: - env.sim.render_interval = render_interval - print(f"[sim] render_interval set to {env.sim.render_interval}") - except Exception as e: - print(f"[sim] failed to configure rendering: {e}") - #if args_cli.camera_write_interval is not None: - # try: - # import tasks.common_observations.camera_state as cam_state - # cam_state._camera_cache['write_interval_steps'] = max(1, int(args_cli.camera_write_interval)) - # print(f"[camera] write interval steps set to {cam_state._camera_cache['write_interval_steps']}") - # except Exception as e: - # print(f"[camera] failed to set write interval: {e}") - - #try: - # if args_cli.solver_iterations is not None: - # env.sim.physx.solver_iteration_count = int(args_cli.solver_iterations) - # print(f"[sim] solver_iteration_count={env.sim.physx.solver_iteration_count}") - # if args_cli.physx_substeps is not None: - # try: - # env.sim.physx.substeps = int(args_cli.physx_substeps) - # except Exception: - # try: - # env.sim.set_substeps(int(args_cli.physx_substeps)) - # except Exception: - # pass - # print(f"[sim] physx_substeps set to {args_cli.physx_substeps}") - # if args_cli.gravity_z is not None: - # g = float(args_cli.gravity_z) - # env.sim.physx.gravity = (0.0, 0.0, g) - # print(f"[sim] gravity set to {env.sim.physx.gravity}") - #except Exception as e: - # print(f"[sim] failed to set physx params: {e}") - #if args_cli.skip_cvtcolor: - # os.environ["CAMERA_SKIP_CVTCOLOR"] = "1" - try: - import tasks.common_observations.camera_state as cam_state - enable_jpeg = bool(args_cli.camera_jpeg) or (os.getenv("CAMERA_JPEG") == "1") - jpeg_quality = int(args_cli.camera_jpeg_quality if args_cli.camera_jpeg else os.getenv("CAMERA_JPEG_QUALITY", args_cli.camera_jpeg_quality)) - cam_state.set_writer_options(enable_jpeg=enable_jpeg, jpeg_quality=jpeg_quality, skip_cvtcolor=args_cli.skip_cvtcolor) - include = [n.strip() for n in (args_cli.camera_include or "").split(',') if n.strip()] - exclude = [n.strip() for n in (args_cli.camera_exclude or "").split(',') if n.strip()] - import omni.kit.viewport.utility as vu - from pxr import Sdf - - vp_api = vu.get_active_viewport() - vp_api.camera_path = "/World/PerspectiveCamera" - try: - cam_state.set_camera_allowlist(include) - except Exception: - pass - try: - sensors_dict = getattr(env.scene, "sensors", {}) - for name, sensor in sensors_dict.items(): - lname = name.lower() - if "camera" not in lname: - continue - if exclude and name in exclude: - for attr_name, value in [("enabled", False), ("is_enabled", False)]: - if hasattr(sensor, attr_name): - try: - setattr(sensor, attr_name, value) - except Exception: - pass - for meth in ("set_active", "disable", "pause"): - if hasattr(sensor, meth): - try: - getattr(sensor, meth)(False) - except Exception: - pass - for attr_name in ("update_period", "_update_period"): - if hasattr(sensor, attr_name): - try: - setattr(sensor, attr_name, 1e6) - except Exception: - pass - elif include and name not in include: - for attr_name in ("update_period", "_update_period"): - if hasattr(sensor, attr_name): - try: - setattr(sensor, attr_name, 1e6) - except Exception: - pass - except Exception as e: - print(f"[camera] failed to tune sensors: {e}") - except Exception as e: - print(f"[camera] failed to apply writer options: {e}") - except Exception as e: - print(f"\nFailed to create environment: {e}") - return - - # get robot stiffness and damping parameters from runtime environment - print("\n" + "="*60) - print("🔍 Getting robot stiffness and damping parameters from runtime environment") - print("="*60) - - #try: - # stiffness_data = get_robot_stiffness_from_env(env) - # if stiffness_data: - # print("✅ Successfully got robot parameters!") - # else: - # print("⚠️ Failed to get robot parameters, will try again after environment reset") - #except Exception as e: - # print(f"⚠️ Error getting robot parameters: {e}") - - print("="*60) - - if not getattr(args_cli, "headless", False) and not args_cli.no_render: - print("\n") - print("*** Please left-click on the Sim window to activate rendering. ***") - print("\n") - else: - print("\n") - print("*** Running without GUI; rendering handled offscreen. ***") - print("\n") - # reset environment - if args_cli.modify_light: - update_light( - prim_path="/World/light", - color=(0.75, 0.75, 0.75), - intensity=500.0, - # position=(1.0, 2.0, 3.0), - radius=0.1, - enabled=True, - cast_shadows=True - ) - if args_cli.modify_camera: - batch_augment_cameras_by_name( - names=["front_cam"], - focal_length=3.0, - horizontal_aperture=22.0, - vertical_aperture=16.0, - exposure=0.8, - focus_distance=1.2 - ) - env.sim.reset() - env.reset() - - # create simplified control configuration - try: - control_config = ControlConfig( - step_hz=args_cli.step_hz, - replay_mode=args_cli.replay_data - ) - except Exception as e: - print(f"Failed to create control configuration: {e}") - return - - # create controller - - if not args_cli.replay_data: - print("========= create image server =========") - try: - #image_server = run_isaacsim_server() - pass - except Exception as e: - print(f"Failed to create image server: {e}") - return - print("========= create image server success =========") - print("========= create dds =========") - try: - dds_manager = create_dds_objects(args_cli,env) - pass - except Exception as e: - print(f"Failed to create dds: {e}") - return - print("========= create dds success =========") - else: - print("========= create dds =========") - #try: - # create_dds_objects_replay(args_cli,env) - #except Exception as e: - # print(f"Failed to create dds: {e}") - # return - #print("========= create dds success =========") - #from tools.data_json_load import get_data_json_list - #print("========= get data json list =========") - #data_idx=0 - #data_json_list = get_data_json_list(args_cli.file_path) - #if args_cli.action_source != "replay": - # args_cli.action_source = "replay" - #print("========= get data json list success =========") - # create action provider - - print(f"\ncreate action provider: {args_cli.action_source}...") - try: - print(f"args_cli.task: {args_cli.task}") - if not args_cli.replay_data and ("Wholebody" in args_cli.task or args_cli.enable_wholebody_dds): - args_cli.action_source = "dds_wholebody" - args_cli.enable_wholebody_dds = True - control_config.use_rl_action_mode = True - action_provider = create_action_provider(env,args_cli) - if action_provider is None: - print("action provider creation failed, exiting") - return - except Exception as e: - print(f"Failed to create action provider: {e}") - return - - # set action provider - print("========= create controller =========") - controller = RobotController(env, control_config) - controller.set_action_provider(action_provider) - print("========= create controller success =========") - - # configure performance analysis - if args_cli.enable_profiling: - controller.set_profiling(True, args_cli.profile_interval) - print(f"performance analysis enabled, report every {args_cli.profile_interval} steps") - else: - controller.set_profiling(False) - print("performance analysis disabled") - + from isaaclab.app import AppLauncher + + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('--task', default='base') + parser.add_argument('--robot_type', choices=['h1_2'], default='h1_2') + parser.add_argument('--action_source', choices=['dds'], default='dds') + parser.add_argument('--enable_inspire_dds', action='store_true') + parser.add_argument('--step_hz', type=float, default=None, + help='Wall-clock control frequency; default matches the environment step time') + parser.add_argument('--physics_dt', type=float, default=None) + parser.add_argument('--render_interval', type=int, default=None) + parser.add_argument('--seed', type=int, default=42) + parser.add_argument('--modify_light', action='store_true') + parser.add_argument('--modify_camera', action='store_true') + parser.add_argument('--enable_profiling', action='store_true') + parser.add_argument('--profile_interval', type=int, default=500) + parser.add_argument('--max_steps', type=int, default=0, + help='Exit after this many environment steps (0 runs until stopped)') + AppLauncher.add_app_launcher_args(parser) + args = parser.parse_args() + if args.physics_dt is not None and not 0 < args.physics_dt < float('inf'): + parser.error('--physics_dt must be finite and positive') + if args.step_hz is not None and not 0 < args.step_hz < float('inf'): + parser.error('--step_hz must be finite and positive') + if args.render_interval is not None and args.render_interval < 1: + parser.error('--render_interval must be positive') + if args.max_steps < 0 or args.profile_interval < 1: + parser.error('--max_steps must be nonnegative and --profile_interval positive') + domain = int(os.environ.get('ROS_DOMAIN_ID') or '1') + if not 1 <= domain <= 232: + parser.error('ROS_DOMAIN_ID must be 1..232; 0 is reserved for the real robot') + os.environ['ROS_DOMAIN_ID'] = str(domain) + os.environ['PROJECT_ROOT'] = str(Path(__file__).resolve().parent) + + with ExitStack() as resources: + app = AppLauncher(args).app + resources.callback(app.close) + def report_exception(exc_type, exc, tb): + if exc is not None: + traceback.print_exception(exc_type, exc, tb) + return False + resources.push(report_exception) + import gymnasium as gym + import torch + import tasks # registers the local Isaac Lab environment + from isaaclab_tasks.utils.parse_cfg import parse_env_cfg + from src.python.control.RobotController import RobotController, ControlConfig + from src.python.control.action_provider_dds import create_action_provider + from src.python.dds.common.dds_create import create_dds_objects + from src.python.sensors.ros_sensor_bridge import RosSensorBridge + + cfg = parse_env_cfg(args.task, device=args.device, num_envs=1) + cfg.seed = args.seed + if args.physics_dt is not None: + cfg.sim.dt = args.physics_dt + if args.render_interval is not None: + cfg.sim.render_interval = args.render_interval + if not args.enable_cameras: + for name in ('front_camera', 'left_wrist_camera', 'right_wrist_camera'): + setattr(cfg.scene, name, None) + if not args.enable_inspire_dds: + cfg.observations.policy.robot_inspire_state = None + env = gym.make(args.task, cfg=cfg).unwrapped + resources.callback(env.close) + if args.modify_light or args.modify_camera: + from tools.augmentation_utils import update_light, batch_augment_cameras_by_name + if args.modify_light: + update_light(prim_path='/World/light', color=(.75, .75, .75), + intensity=500., radius=.1, enabled=True, cast_shadows=True) + if args.modify_camera: + batch_augment_cameras_by_name(names=['front_cam'], focal_length=3., + horizontal_aperture=22., vertical_aperture=16., exposure=.8, focus_distance=1.2) + env.reset() + sensors = RosSensorBridge(env.scene) + resources.callback(sensors.close) + dds = create_dds_objects(args, env) + resources.callback(dds.stop_all_communication) + controller = RobotController(env, ControlConfig(step_hz=args.step_hz or 1 / env.step_dt)) + resources.callback(controller.cleanup) + controller.set_action_provider(create_action_provider(env, args)) + controller.set_profiling(args.enable_profiling, args.profile_interval) + + def stop(signum, frame): + controller.stop() + previous = {sig: signal.signal(sig, stop) for sig in (signal.SIGINT, signal.SIGTERM)} + for sig, handler in previous.items(): + resources.callback(signal.signal, sig, handler) - # set signal handlers - if not args_cli.replay_data: - setup_signal_handlers(controller, dds_manager) - #setup_signal_handlers(controller,dds_manager,image_server) - else: - setup_signal_handlers(controller) - - print("Note: The DDS in Sim transmits messages on channel 1. Please ensure that other DDS instances use the same channel for message exchange by setting: ChannelFactoryInitialize(1).") - try: - # start controller - start asynchronous components - print("========= start controller =========") controller.start() - print("========= start controller success =========") - - ## main loop - execute in main thread to support rendering - #last_stats_time = time.time() - #loop_start_time = time.time() - #loop_count = 0 - #last_loop_time = time.time() - #recent_loop_times = [] # for calculating moving average frequency - # - # - #reward_interval = max(1, args_cli.reward_interval) - - #ros2_camera_node_py.spin_rclcpp() - #camera_node = ros2_camera_node_py.ros2_camera_node(["/realsense/left_hand/aligned_depth_to_color/image_raw/compressed", "/realsense/right_hand/aligned_depth_to_color/image_raw/compressed","/realsense/left_hand/color/image_raw/compressed", "/realsense/right_hand/color/image_raw/compressed"]) - #cloud_node = ros2_point_cloud_node_py.ros2_point_cloud_node(["/lidar/first_link"]) - #imu_node = ros2_imu_node_py.ros2_imu_node("/imu/base_link") - #breakpoint() - with contextlib.suppress(KeyboardInterrupt), torch.inference_mode(): - - while simulation_app.is_running() and controller.is_running: - #breakpoint() - #current_time = time.time() - #loop_count += 1 - if not args_cli.replay_data: - try: - env_state = env.scene.get_state() - env_state_json = sim_state_to_json(env_state) - sim_state = {"init_state":env_state_json,"task_name":args_cli.task} - except Exception as e: - print(f"Failed to get env state: {e}") - raise e - #try: - - # start = perf_counter() - # cloud_data = env.scene['lidar']._data.ray_hits_w.flatten().cpu() - # cloud_node.push_data_to_deque(cloud_data.data_ptr(), cloud_data.shape[0], "/lidar/first_link") - - # imu_node.push_data_to_deque(imu_data.quat_w.cpu().tolist()[0], [0, 0, 0, 0, 0, 0, 0, 0, 0], imu_data.ang_vel_b.cpu().tolist()[0], [0, 0, 0, 0, 0, 0, 0, 0, 0], imu_data.lin_acc_b.cpu().tolist()[0], [0, 0, 0, 0, 0, 0, 0, 0, 0]) - # elapsed = perf_counter() - start - - # print(f"POINT CLOUD PUBLISHING: {elapsed*1000:.3f} ms") - #except Exception as e: - # print(f"faled to publish cloud data") - - #try: - - # imu_data = env.scene['imu']._data - # if not imu_data.quat_w[0][0] == 1: - # start = perf_counter() - - # imu_node.push_data_to_deque(imu_data.quat_w.cpu().tolist()[0], [0, 0, 0, 0, 0, 0, 0, 0, 0], imu_data.ang_vel_b.cpu().tolist()[0], [0, 0, 0, 0, 0, 0, 0, 0, 0], imu_data.lin_acc_b.cpu().tolist()[0], [0, 0, 0, 0, 0, 0, 0, 0, 0]) - # elapsed = perf_counter() - start - # print(f"IMU PUBLISHING: {elapsed*1000:.3f} ms") - - #except Exception as e: - # print(e) - #try: - - - # start = perf_counter() - # rgb_data = env.scene['left_wrist_camera']._data.output['rgb'].reshape((3, 480, 640)).cpu() - # camera_node.push_data_to_deque(rgb_data.data_ptr(), rgb_data.shape[-1], rgb_data.shape[-2], "/realsense/left_hand/color/image_raw/compressed", "INTERLEAVED") - - # elapsed = perf_counter() - start - # print(f"LEFT_RGB PUBLISHING: {elapsed*1000:.3f} ms") - #except Exception as e: - # print(e) - - #try: - - # start = perf_counter() - # rgb_data = env.scene['right_wrist_camera']._data.output['rgb'].reshape((3, 480, 640)).cpu() - # camera_node.push_data_to_deque(rgb_data.data_ptr(), rgb_data.shape[-1], rgb_data.shape[-2], "/realsense/right_hand/color/image_raw/compressed", "INTERLEAVED") - - # elapsed = perf_counter() - start - # print(f"RIGHT_RGB PUBLISHING: {elapsed*1000:.3f} ms") - #except Exception as e: - # print(e) - - #try: - - # depth_data = env.scene['left_wrist_camera']._data.output['distance_to_image_plane'].reshape((1, 480, 640)).cpu() - # valid_mask = torch.isfinite(depth_data) - # valid_values = depth_data[valid_mask] - # vmin, vmax = torch.min(valid_values), torch.max(valid_values) - # depth_clean = torch.where(valid_mask, depth_data, vmin) - # normalized = (depth_clean - vmin) / (vmax - vmin) - # depth_u8 = (normalized * 255).to(torch.uint8) - # camera_node.push_data_to_deque(depth_u8.data_ptr(), depth_u8.shape[-1], depth_u8.shape[-2], "/realsense/left_hand/aligned_depth_to_color/image_raw/compressed", "INTERLEAVED") - #except Exception as e: - # print(e) - - #try: - - # depth_data = env.scene['right_wrist_camera']._data.output['distance_to_image_plane'].reshape((1, 480, 640)).cpu() - # valid_mask = torch.isfinite(depth_data) - # valid_values = depth_data[valid_mask] - # vmin, vmax = torch.min(valid_values), torch.max(valid_values) - # depth_clean = torch.where(valid_mask, depth_data, vmin) - # normalized = (depth_clean - vmin) / (vmax - vmin) - # depth_u8 = (normalized * 255).to(torch.uint8) - # camera_node.push_data_to_deque(depth_u8.data_ptr(), depth_u8.shape[-1], depth_u8.shape[-2], "/realsense/right_hand/aligned_depth_to_color/image_raw/compressed", "INTERLEAVED") - #except Exception as e: - # print(e) - - else: - pass + print(f'[isaac] DDS domain={domain}; physics_dt={env.physics_dt}; control_dt={env.step_dt}', flush=True) + with torch.inference_mode(): + while app.is_running() and controller.is_running: controller.step() - ## check environment state - if env.sim.is_stopped(): - print("\nenvironment stopped") + sensors.publish(env._sim_step_counter * env.physics_dt) + if env.sim.is_stopped() or (args.max_steps and controller.step_count >= args.max_steps): break - except KeyboardInterrupt: - print("\nuser interrupted program") - - except Exception as e: - print(f"\nprogram exception: {e}") - - finally: - # clean up resources - print("\nclean up resources...") - controller.cleanup() - image_server.stop() - env.close() - print("cleanup completed") - # profiler.disable() - # s = io.StringIO() - # ps = pstats.Stats(profiler, stream=s).strip_dirs().sort_stats("time") - # ps.print_stats(30) + print(f'[isaac] completed {controller.step_count} steps', flush=True) - # print(s.getvalue()) -if __name__ == "__main__": - try: - main() - finally: - print("Performing final cleanup...") - - ## Get current process information - #import os - #import subprocess - #import signal - #import time - # - #current_pid = os.getpid() - #print(f"Current main process PID: {current_pid}") - # - #try: - # # Find all related Python processes - # result = subprocess.run(['pgrep', '-f', 'sim_main.py'], - # capture_output=True, text=True) - # if result.returncode == 0: - # pids = result.stdout.strip().split('\n') - # print(f"Found related processes: {pids}") - # - # for pid in pids: - # if pid and pid != str(current_pid): - # try: - # print(f"Terminating child process: {pid}") - # os.kill(int(pid), signal.SIGTERM) - # except ProcessLookupError: - # print(f"Process {pid} does not exist") - # except Exception as e: - # print(f"Failed to terminate process {pid}: {e}") - # - # # Wait for processes to exit - # time.sleep(2) - # - # # Check if there are any remaining processes, force kill them - # result2 = subprocess.run(['pgrep', '-f', 'sim_main.py'], - # capture_output=True, text=True) - # if result2.returncode == 0: - # remaining_pids = result2.stdout.strip().split('\n') - # for pid in remaining_pids: - # if pid and pid != str(current_pid): - # try: - # print(f"Force killing process: {pid}") - # os.kill(int(pid), signal.SIGKILL) - # except Exception as e: - # print(f"Failed to force kill process {pid}: {e}") - # - #except Exception as e: - # print(f"Error during process cleanup: {e}") - # - #try: - # simulation_app.close() - #except Exception as e: - # print(f"Failed to close simulation application: {e}") - # - #print("Program exit completed") - # - ## Force exit - os._exit(0) +if __name__ == '__main__': + main() diff --git a/src/python/control/RobotController.py b/src/python/control/RobotController.py index 1367e80c..191fd02f 100644 --- a/src/python/control/RobotController.py +++ b/src/python/control/RobotController.py @@ -4,6 +4,8 @@ import torch from dataclasses import dataclass from .action_base import ActionProvider +from .pacing import RealtimePacer +from .motor_contract import ACTION_DIM @dataclass @@ -27,7 +29,7 @@ def __init__(self, env, config: ControlConfig): self._last_step_time = 0.0 all_joint_names = env.scene["robot"].data.joint_names - self._last_action = torch.zeros(len(all_joint_names), device=env.device) + self._last_action = torch.zeros((env.num_envs, ACTION_DIM), device=env.device) # pre-calculate the sleep threshold (avoid calculating every time) @@ -65,6 +67,7 @@ def start(self): self.is_running = True self._start_time = time.time() self._last_step_time = self._perf_counter() + self._pacer = RealtimePacer(self._step_interval, clock=self._perf_counter, sleep=self._time_sleep) # start the action provider if self.action_provider: @@ -123,13 +126,7 @@ def step(self): # 3. minimal frequency control (no rendering overhead, use the pre-calculated threshold) sleep_start = perf_counter() - current_time = perf_counter() - if self._last_step_time > 0: - elapsed = current_time - self._last_step_time - sleep_needed = self._step_interval - elapsed - if sleep_needed > self._sleep_threshold: # use the pre-calculated threshold - self._time_sleep(sleep_needed - self._sleep_adjustment) # use the pre-calculated adjustment value - self._last_step_time = current_time + self._pacer.wait() sleep_time = perf_counter() - sleep_start # 4. minimal performance print diff --git a/src/python/control/action_provider_dds.py b/src/python/control/action_provider_dds.py index a7bc13a9..c60aa0ec 100644 --- a/src/python/control/action_provider_dds.py +++ b/src/python/control/action_provider_dds.py @@ -1,229 +1,67 @@ -# Copyright (c) 2025, Unitree Robotics Co., Ltd. All Rights Reserved. -# License: Apache License, Version 2.0 -from .action_base import ActionProvider -from typing import Optional +"""Receive complete Unitree commands without touching physics from DDS threads.""" +import os import torch +from .action_base import ActionProvider +from .motor_contract import ACTION_DIM, BODY_ACTION_DIM, pack_motor_command from ..dds.common.dds_master import dds_manager -def create_action_provider(env,args): - """create action provider based on parameters""" - if args.action_source == "dds": - return DDSActionProvider( - env=env, - args_cli=args - ) - else: - print(f"unknown action source: {args.action_source}") - return None - -class DDSActionProvider(ActionProvider): - """Action provider based on DDS""" - - def __init__(self,env, args_cli): - super().__init__("DDSActionProvider") - self.enable_robot = args_cli.robot_type - print(f"{self.enable_robot=}") - print(f"{args_cli.robot_type=}") - self.enable_inspire = args_cli.enable_inspire_dds - self.env = env - # Initialize DDS communication - self.robot_dds = None - #self.inspire_dds = None - self._setup_dds() - self._setup_joint_mapping() - - def _setup_dds(self): - """Setup DDS communication""" - print(f"enable_robot: {self.enable_robot}") - try: - if self.enable_robot == "h1_2": - self.robot_dds = dds_manager.get_object("h1_2") - if self.robot_dds is None: - print("self.robot_dds: None") - - #if self.enable_inspire: - # self.inspire_dds = dds_manager.get_object("inspire") - print(f"[{self.name}] DDS communication initialized") - except Exception as e: - print(f"[{self.name}] DDS initialization failed: {e}") - - def _setup_joint_mapping(self): - """Setup joint mapping""" - print(self.enable_robot) - if self.enable_robot == "h1_2": - self.arm_joint_mapping = { - "left_shoulder_pitch_joint": 0, - "left_shoulder_roll_joint": 1, - "left_shoulder_yaw_joint": 2, - "left_elbow_joint": 3, - "left_wrist_roll_joint": 4, - "left_wrist_pitch_joint": 5, - "left_wrist_yaw_joint": 6, - "right_shoulder_pitch_joint": 7, - "right_shoulder_roll_joint": 8, - "right_shoulder_yaw_joint": 9, - "right_elbow_joint": 10, - "right_wrist_roll_joint": 11, - "right_wrist_pitch_joint": 12, - "right_wrist_yaw_joint": 13 - } - - self.leg_joint_mapping = { - "left_hip_yaw_joint": 0, - "left_hip_pitch_joint": 1, - "left_hip_roll_joint": 2, - "left_knee_joint": 3, - "left_ankle_pitch_joint": 4, - "left_ankle_roll_joint": 5, - "right_hip_yaw_joint": 6, - "right_hip_pitch_joint": 7, - "right_hip_roll_joint": 8, - "right_knee_joint": 9, - "right_ankle_pitch_joint": 10, - "right_ankle_roll_joint": 11, - "torso_joint":12 - } - - - print(f"self.env.scene['robot'].data.joint_names: {self.env.scene['robot'].data.joint_names}") - self.all_joint_names = self.env.scene["robot"].data.joint_names - self.joint_to_index = {name: i for i, name in enumerate(self.all_joint_names)} - - print(f"{self.joint_to_index=}") - self.arm_action_pose = [self.joint_to_index[name] for name in self.arm_joint_mapping.keys()] - self.arm_action_pose_indices = [self.arm_joint_mapping[name] for name in self.arm_joint_mapping.keys()] - self._arm_target_indices = [self.joint_to_index[name] for name in self.arm_joint_mapping.keys()] - self._arm_source_indices = [idx + 13 for idx in self.arm_joint_mapping.values()] # source data from positions[13:] - - - print(f"{self._arm_target_indices=}") - print(f"{self._arm_source_indices=}") +def create_action_provider(env, args): + if args.action_source != 'dds': + raise ValueError(f'Unsupported action source: {args.action_source}') + return DDSActionProvider(env, args) - self.leg_action_pose = [self.joint_to_index[name] for name in self.leg_joint_mapping.keys()] - self.leg_action_pose_indices = [self.leg_joint_mapping[name] for name in self.leg_joint_mapping.keys()] - self._leg_target_indices = [self.joint_to_index[name] for name in self.leg_joint_mapping.keys()] - print(f"{self._leg_target_indices=}") - self._leg_source_indices = [idx for idx in self.leg_joint_mapping.values()] - print(f"{self._leg_source_indices=}") - - - - - #if self.enable_inspire: - # self.inspire_hand_joint_mapping = { - # "R_pinky_proximal_joint":0, - # "R_ring_proximal_joint":1, - # "R_middle_proximal_joint":2, - # "R_index_proximal_joint":3, - # "R_thumb_proximal_pitch_joint":4, - # "R_thumb_proximal_yaw_joint":5, - # "L_pinky_proximal_joint":6, - # "L_ring_proximal_joint":7, - # "L_middle_proximal_joint":8, - # "L_index_proximal_joint":9, - # "L_thumb_proximal_pitch_joint":10, - # "L_thumb_proximal_yaw_joint":11, - # } - # self.special_joint_mapping = { - # "L_index_intermediate_joint":[9,1], - # "L_middle_intermediate_joint":[8,1], - # "L_pinky_intermediate_joint":[6,1], - # "L_ring_intermediate_joint":[7,1], - # "L_thumb_intermediate_joint":[10,1.5], - # "L_thumb_distal_joint":[10,2.4], - - # "R_index_intermediate_joint":[3,1], - # "R_middle_intermediate_joint":[2,1], - # "R_pinky_intermediate_joint":[0,1], - # "R_ring_intermediate_joint":[1,1], - # "R_thumb_intermediate_joint":[4,1.5], - # "R_thumb_distal_joint":[4,2.4], - # } - - # - ## precompute indices (for vectorization) - - #if self.enable_inspire: - # self._inspire_target_indices = [self.joint_to_index[name] for name in self.inspire_hand_joint_mapping.keys()] - # self._inspire_source_indices = [idx for idx in self.inspire_hand_joint_mapping.values()] - # self._inspire_special_target_indices = [self.joint_to_index[name] for name in self.special_joint_mapping.keys()] - # self._inspire_special_source_indices = [spec[0] for spec in self.special_joint_mapping.values()] - # self._inspire_special_scales = torch.tensor([spec[1] for spec in self.special_joint_mapping.values()], dtype=torch.float32) - - device = self.env.device - self._arm_target_idx_t = torch.tensor(self._arm_target_indices, dtype=torch.long, device=device) - self._arm_source_idx_t = torch.tensor(self._arm_source_indices, dtype=torch.long, device=device) - - self._leg_target_idx_t = torch.tensor(self._leg_target_indices, dtype=torch.long, device=device) - self._leg_source_idx_t = torch.tensor(self._leg_source_indices, dtype=torch.long, device=device) - #if self.enable_inspire: - # self._inspire_target_idx_t = torch.tensor(self._inspire_target_indices, dtype=torch.long, device=device) - # self._inspire_source_idx_t = torch.tensor(self._inspire_source_indices, dtype=torch.long, device=device) - # self._inspire_special_target_idx_t = torch.tensor(self._inspire_special_target_indices, dtype=torch.long, device=device) - # self._inspire_special_source_idx_t = torch.tensor(self._inspire_special_source_indices, dtype=torch.long, device=device) - # self._inspire_special_scales_t = self._inspire_special_scales.to(device) - - self._full_action_buf = torch.zeros(len(self.all_joint_names), device=device, dtype=torch.float32) - self._positions_buf = torch.empty(29, device=device, dtype=torch.float32) - #if self.enable_inspire: - # self._inspire_buf = torch.empty(12, device=device, dtype=torch.float32) - - def get_action(self, env) -> Optional[torch.Tensor]: - """Get action from DDS""" - try: - - print('tessssssssssssssssst') - full_action = self._full_action_buf - full_action.zero_() - if self.enable_robot == "h1_2" and self.robot_dds: - print("222222") - cmd_data = self.robot_dds.get_robot_command() +class DDSActionProvider(ActionProvider): + def __init__(self, env, args_cli): + super().__init__('DDSActionProvider') + if args_cli.robot_type != 'h1_2': + raise ValueError('Only h1_2 is supported') + self.robot_dds = dds_manager.get_object('h1_2') + self.inspire_dds = dds_manager.get_object('inspire') if args_cli.enable_inspire_dds else None + if self.robot_dds is None: + raise RuntimeError('H1-2 DDS bridge is unavailable') + self._action = torch.zeros((1, ACTION_DIM), device=env.device) + self._sequence = None + self._last_command_time = None + self._last_sim_time = None + self.timeout = float(os.environ.get('GOLEM_CMD_TIMEOUT', '0.5')) + if not 0 < self.timeout < float('inf'): + raise ValueError('GOLEM_CMD_TIMEOUT must be finite and positive') + + def start(self): + # Commands arrive in the DDS subscriber; this provider has no worker loop. + self.is_running = True + + def stop(self): + self.is_running = False + + def get_action(self, env): + sim_time = env._sim_step_counter * env.physics_dt + if self._last_sim_time is not None and sim_time < self._last_sim_time: + self._last_command_time = None + self._last_sim_time = sim_time + command = self.robot_dds.get_robot_command() + if command and command.get('sequence') != self._sequence: + self._sequence = command.get('sequence') + try: + body = pack_motor_command(command['motor_cmd'], env.device) + self._action[0, :BODY_ACTION_DIM].copy_(body) + self._last_command_time = sim_time + except (ValueError, KeyError, TypeError): + self._last_command_time = None + if self._last_command_time is None or sim_time - self._last_command_time > self.timeout: + self._action[0, :BODY_ACTION_DIM].zero_() + if self.inspire_dds is not None: + hand = self.inspire_dds.get_inspire_hand_command() + if hand and len(hand.get('positions', ())) == 12: + positions = torch.tensor(hand['positions'], dtype=torch.float32, device=env.device) + if torch.isfinite(positions).all(): + self._action[0, BODY_ACTION_DIM:].copy_(positions) + return self._action - print("333333") - print(cmd_data) - if cmd_data and 'motor_cmd' in cmd_data: - positions = cmd_data['motor_cmd']['positions'] - print(list(enumerate(positions))) - print(f"{len(positions)=}") - print(f"{len(self.all_joint_names)=}") - if len(positions) >= 29: - self._positions_buf[:29].copy_(torch.tensor(positions[:29], dtype=torch.float32, device=self.env.device)) - arm_vals = self._positions_buf.index_select(0, self._arm_source_idx_t) - leg_vals = self._positions_buf.index_select(0, self._leg_source_idx_t) - full_action.index_copy_(0, self._arm_target_idx_t, arm_vals) - full_action.index_copy_(0, self._leg_target_idx_t, leg_vals) - #this full action line actually updates the joint positions in sim - #if self.inspire_dds: - # inspire_cmds = self.inspire_dds.get_inspire_hand_command() - # if inspire_cmds and 'positions' in inspire_cmds: - # inspire_cmds_positions = inspire_cmds['positions'] - # if len(inspire_cmds_positions) >= 12: - # self._inspire_buf.copy_(torch.tensor(inspire_cmds_positions[:12], dtype=torch.float32, device=self.env.device)) - # base_vals = self._inspire_buf.index_select(0, self._inspire_source_idx_t) - # full_action.index_copy_(0, self._inspire_target_idx_t, base_vals) - # special_vals = self._inspire_buf.index_select(0, self._inspire_special_source_idx_t) * self._inspire_special_scales_t - # full_action.index_copy_(0, self._inspire_special_target_idx_t, special_vals) - return full_action.unsqueeze(0) - - except Exception as e: - print(f"[{self.name}] Get DDS action failed: {e}") - return None - - def _convert_to_joint_range(self, value): - """Convert gripper control value to joint angle""" - input_min, input_max = 0.0, 5.6 - output_min, output_max = 0.03, -0.02 - value = max(input_min, min(input_max, value)) - return output_min + (output_max - output_min) * (value - input_min) / (input_max - input_min) - def cleanup(self): - """Clean up DDS resources""" - try: - if self.robot_dds: - self.robot_dds.stop_communication() - if self.inspire_dds: - self.inspire_dds.stop_communication() - except Exception as e: - print(f"[{self.name}] Clean up DDS resources failed: {e}") + self.stop() + for bridge in (self.robot_dds, self.inspire_dds): + if bridge is not None: + bridge.stop_communication() diff --git a/src/python/control/motor_contract.py b/src/python/control/motor_contract.py new file mode 100644 index 00000000..3089ecd1 --- /dev/null +++ b/src/python/control/motor_contract.py @@ -0,0 +1,34 @@ +"""Unitree wire order and per-physics-step motor servo math.""" +from collections.abc import Mapping +import torch + +BODY_JOINT_NAMES = tuple( + f'{side}_{joint}_joint' + for side in ('left', 'right') + for joint in ('hip_yaw', 'hip_pitch', 'hip_roll', 'knee', 'ankle_pitch', 'ankle_roll') +) + ('torso_joint',) + tuple( + f'{side}_{joint}_joint' + for side in ('left', 'right') + for joint in ('shoulder_pitch', 'shoulder_roll', 'shoulder_yaw', 'elbow', 'wrist_roll', 'wrist_pitch', 'wrist_yaw') +) +HAND_JOINT_NAMES = tuple( + f'{side}_{joint}_joint' + for side in ('R', 'L') + for joint in ('pinky_proximal', 'ring_proximal', 'middle_proximal', 'index_proximal', 'thumb_proximal_pitch', 'thumb_proximal_yaw') +) +MOTOR_FIELDS = ('positions', 'velocities', 'torques', 'kp', 'kd', 'mode') +BODY_ACTION_DIM = 6 * len(BODY_JOINT_NAMES) +ACTION_DIM = BODY_ACTION_DIM + len(HAND_JOINT_NAMES) + + +def pack_motor_command(command, device): + """Pack the 27 body motors; unused trailing IDL slots are ignored.""" + if not isinstance(command, Mapping): + raise ValueError("LowCmd must be a mapping") + n = len(BODY_JOINT_NAMES) + if any(len(command.get(field, ())) < n for field in MOTOR_FIELDS): + raise ValueError('LowCmd requires all six fields for 27 motors') + values = torch.tensor([command[field][:n] for field in MOTOR_FIELDS], device=device, dtype=torch.float32) + if not torch.isfinite(values).all() or (values[3:5] < 0).any(): + raise ValueError('LowCmd fields must be finite and gains nonnegative') + return values.flatten() diff --git a/src/python/control/pacing.py b/src/python/control/pacing.py new file mode 100644 index 00000000..b81d38f4 --- /dev/null +++ b/src/python/control/pacing.py @@ -0,0 +1,29 @@ +"""Wall-clock pacing and optional stage timings for the single-step simulator.""" +import math +import time + + +class RealtimePacer: + """Keep a wall deadline without skipping physics or repaying long pauses.""" + + def __init__(self, timestep, max_lag=0.1, *, clock=time.perf_counter, sleep=time.sleep): + if not math.isfinite(timestep) or timestep <= 0: + raise ValueError('timestep must be finite and positive') + if not math.isfinite(max_lag) or max_lag < 0: + raise ValueError('max_lag must be finite and nonnegative') + self.timestep = timestep + self.max_lag = max_lag + self.clock = clock + self.sleep = sleep + self.deadline = clock() + + def wait(self): + self.deadline += self.timestep + now = self.clock() + delay = self.deadline - now + if delay > 0: + self.sleep(delay) + elif -delay > self.max_lag: + # Renderer initialization/debugger pauses must not create minutes + # of catch-up. Only the wall deadline changes, never simulation time. + self.deadline = now - self.max_lag diff --git a/src/python/control/unitree_action.py b/src/python/control/unitree_action.py new file mode 100644 index 00000000..af070fa5 --- /dev/null +++ b/src/python/control/unitree_action.py @@ -0,0 +1,83 @@ +"""Isaac Lab action term implementing the Unitree body servo and Inspire linkage.""" +import torch +from isaaclab.managers import ActionTerm, ActionTermCfg +from isaaclab.utils import configclass +from .motor_contract import BODY_JOINT_NAMES, HAND_JOINT_NAMES, ACTION_DIM, BODY_ACTION_DIM + + +class UnitreeMotorAction(ActionTerm): + """Apply the complete motor command through PhysX implicit drives and Inspire linkage.""" + def __init__(self, cfg, env): + super().__init__(cfg, env) + names = self._asset.data.joint_names + self._body_ids = [names.index(name) for name in BODY_JOINT_NAMES] + self._hand_ids = [names.index(name) for name in HAND_JOINT_NAMES] + self._coupled_ids, sources, scales = [], [], [] + for side, offset in (('R', 0), ('L', 6)): + for name, source, scale in (('pinky_intermediate',0,1.), ('ring_intermediate',1,1.), + ('middle_intermediate',2,1.), ('index_intermediate',3,1.), + ('thumb_intermediate',4,1.5), ('thumb_distal',4,2.4)): + self._coupled_ids.append(names.index(f'{side}_{name}_joint')) + sources.append(offset+source) + scales.append(scale) + self._hand_sources = torch.tensor(sources, device=self.device) + self._hand_scales = torch.tensor(scales, device=self.device) + self._raw_actions = torch.zeros(self.num_envs, ACTION_DIM, device=self.device) + self._kp_cache = torch.full((self.num_envs, len(BODY_JOINT_NAMES)), float("nan"), device=self.device) + self._kd_cache = torch.full_like(self._kp_cache, float("nan")) + + @property + def action_dim(self): + return ACTION_DIM + + @property + def raw_actions(self): + return self._raw_actions + + @property + def processed_actions(self): + return self._raw_actions + + def process_actions(self, actions): + self._raw_actions.copy_(actions) + command = self._raw_actions[:, :BODY_ACTION_DIM].reshape(-1, 6, len(BODY_JOINT_NAMES)) + enabled = command[:, 5] == 1 + kp = torch.where(enabled, command[:, 3], 0.) + kd = torch.where(enabled, command[:, 4], 0.) + # PhysX integrates spring/damping inside each physics solve. Explicit + # held torques are unstable for the wrist inertia at the 5 ms timestep. + # Gain writes cross GPU/CPU, so only perform them when a command changes. + if not torch.equal(kp, self._kp_cache): + self._asset.write_joint_stiffness_to_sim(kp, joint_ids=self._body_ids) + self._kp_cache.copy_(kp) + if not torch.equal(kd, self._kd_cache): + self._asset.write_joint_damping_to_sim(kd, joint_ids=self._body_ids) + self._kd_cache.copy_(kd) + # Isaac's write_*_to_sim methods do not update actuator model buffers; + # mirror gains so applied_torque/LowState estimates use the same servo. + for actuator in self._asset.actuators.values(): + actuator.stiffness.copy_(self._asset.data.joint_stiffness[:, actuator.joint_indices]) + actuator.damping.copy_(self._asset.data.joint_damping[:, actuator.joint_indices]) + + def apply_actions(self): + command = self._raw_actions[:, :BODY_ACTION_DIM].reshape(-1, 6, len(BODY_JOINT_NAMES)) + self._asset.set_joint_position_target(command[:, 0], joint_ids=self._body_ids) + self._asset.set_joint_velocity_target(command[:, 1], joint_ids=self._body_ids) + effort = torch.where(command[:, 5] == 1, command[:, 2], 0.) + self._asset.set_joint_effort_target(effort, joint_ids=self._body_ids) + hands = self._raw_actions[:, BODY_ACTION_DIM:] + self._asset.set_joint_position_target(hands, joint_ids=self._hand_ids) + self._asset.set_joint_position_target(hands[:, self._hand_sources] * self._hand_scales, + joint_ids=self._coupled_ids) + + def reset(self, env_ids=None): + ids = slice(None) if env_ids is None else env_ids + self._raw_actions[ids] = 0 + self._kp_cache[ids] = float("nan") + self._kd_cache[ids] = float("nan") + + +@configclass +class UnitreeMotorActionCfg(ActionTermCfg): + class_type: type = UnitreeMotorAction + asset_name: str = 'robot' diff --git a/src/python/dds/common/dds_create.py b/src/python/dds/common/dds_create.py index d2445d7c..a3ef02d5 100644 --- a/src/python/dds/common/dds_create.py +++ b/src/python/dds/common/dds_create.py @@ -3,6 +3,8 @@ from .dds_master import dds_manager def create_dds_objects(args_cli,env): + # The ROS RMW node must create the shared Cyclone domain first. + dds_manager._init_dds() publish_names = [] subscribe_names = [] if args_cli.robot_type=="h1_2": diff --git a/src/python/dds/common/dds_master.py b/src/python/dds/common/dds_master.py index f95855a8..961d5a01 100644 --- a/src/python/dds/common/dds_master.py +++ b/src/python/dds/common/dds_master.py @@ -1,9 +1,10 @@ # Copyright (c) 2025, Unitree Robotics Co., Ltd. All Rights Reserved. # License: Apache License, Version 2.0 import time +import os import threading from typing import Dict, List, Optional -from unitree_sdk2py.core.channel import ChannelFactoryInitialize +from .transport import initialize as initialize_transport from .dds_base import DDSObject @@ -40,7 +41,6 @@ def __init__(self): self._default_pub_interval: float = 0.01 # 100Hz default self.dds_initialized = False - self._init_dds() print("[DDSManager] DDSManager initialized") def _parse_object_name(self, name: str) -> tuple[str, str]: @@ -57,13 +57,16 @@ def _init_dds(self) -> bool: return True try: - ChannelFactoryInitialize(36) + domain = int(os.environ.get("ROS_DOMAIN_ID") or "1") + if not 1 <= domain <= 232: + raise ValueError("Simulation ROS_DOMAIN_ID must be 1..232; 0 is reserved for the real robot") + initialize_transport(domain) self.dds_initialized = True - print("[DDSManager] DDS system initialized") + print(f"[DDSManager] DDS system initialized on domain {domain}") return True except Exception as e: print(f"[DDSManager] DDS system initialization failed: {e}") - return False + raise def register_object(self, name: str, obj: DDSObject) -> bool: """Register DDS object""" @@ -139,7 +142,6 @@ def set_default_publish_rate(self, hz: float) -> None: def _publish_loop(self) -> None: """Publish loop thread""" - print("PUBLISH LOOP THREAD INIIALIZED") print("[DDSManager] publish loop thread started") while self.publishing_running: @@ -209,9 +211,22 @@ def start_subscribing(self,enable_subscribe_names:List[str]=None): def stop_all_communication(self): - for name, obj in self.objects.items(): - obj.stop_communication() - self.publishing_running=False - self.subscribing_running=False + self.publishing_running = False + self.subscribing_running = False + for obj in self.objects.values(): + obj.stop_communication() + for worker in (self.publish_thread, self.subscribe_thread): + if worker is not None and worker is not threading.current_thread(): + worker.join(timeout=2.0) + for obj in self.objects.values(): + for name in ('subscriber', 'publisher'): + channel = getattr(obj, name, None) + if channel is not None: + channel.Close() + for name in ('input_shm', 'output_shm'): + memory = getattr(obj, name, None) + if memory is not None: + memory.cleanup() + self.objects.clear() # global singleton instance dds_manager = DDSManager() diff --git a/src/python/dds/common/transport.py b/src/python/dds/common/transport.py new file mode 100644 index 00000000..98a8567f --- /dev/null +++ b/src/python/dds/common/transport.py @@ -0,0 +1,51 @@ +"""Unitree channel adapters sharing the Cyclone domain created by ROS RMW. + +The SDK factory explicitly creates a Domain with its own XML configuration, +which conflicts with rmw_cyclonedds_cpp in the same process. An implicit +participant joins the existing domain while preserving SDK channel semantics. +""" +from cyclonedds.domain import DomainParticipant +from unitree_sdk2py.core.channel import Channel + +_participant = None +_domain = None + + +def initialize(domain): + global _participant, _domain + if not 1 <= domain <= 232: + raise ValueError('Simulation DDS domain must be 1..232') + if _participant is not None and _domain != domain: + raise RuntimeError('Cannot switch the active simulation DDS domain') + if _participant is None: + _participant = DomainParticipant(domain) + _domain = domain + + +class ChannelPublisher: + def __init__(self, name, message_type): + if _participant is None: + raise RuntimeError('Initialize simulation DDS transport first') + self.channel = Channel(_participant, name, message_type, None) + + def Init(self): + self.channel.SetWriter(None) + + def Write(self, sample): + return self.channel.Write(sample) + + def Close(self): + self.channel.CloseWriter() + + +class ChannelSubscriber: + def __init__(self, name, message_type): + if _participant is None: + raise RuntimeError('Initialize simulation DDS transport first') + self.channel = Channel(_participant, name, message_type, None) + + def Init(self, handler, queue_len=0): + self.channel.SetReader(None, handler, queue_len) + + def Close(self): + self.channel.CloseReader() diff --git a/src/python/dds/specialized/h12_robot_dds.py b/src/python/dds/specialized/h12_robot_dds.py index aa73cb36..156d2285 100644 --- a/src/python/dds/specialized/h12_robot_dds.py +++ b/src/python/dds/specialized/h12_robot_dds.py @@ -3,7 +3,7 @@ from typing import Any, Dict, Optional # from dds.dds_base import BaseDDSNode, node_manager from ..common.dds_base import DDSObject -from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber +from ..common.transport import ChannelPublisher, ChannelSubscriber from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_, LowCmd_ from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_, unitree_hg_msg_dds__LowState_ from unitree_sdk2py.utils.crc import CRC @@ -28,13 +28,14 @@ def __init__(self,node_name:str="h1_2"): self.crc = CRC() self.low_state = unitree_hg_msg_dds__LowState_() self._initialized = True + self._command_sequence = 0 # setup the shared memory self.setup_shared_memory( input_shm_name="isaac_robot_state", # read the state of the G1 robot from Isaac Lab output_shm_name="dds_robot_cmd", # output the command to Isaac Lab - input_size=3072, - output_size=3072 # output the command to Isaac Lab + input_size=8192, + output_size=8192 # output the command to Isaac Lab ) print(f"[{self.node_name}] G1 robot DDS node initialized") @@ -96,7 +97,7 @@ def dds_publisher(self) -> Any: if imu and len(imu) >= 13: imu_array = np.asarray(imu, dtype=np.float32) - imu_state.quaternion[:] = imu_array[[4, 5, 6, 3]] #[x,y,z,w] + imu_state.quaternion[:] = imu_array[3:7] # Unitree LowState uses WXYZ. imu_state.accelerometer[:] = imu_array[7:10] @@ -135,11 +136,13 @@ def dds_subscriber(self, msg: LowCmd_,datatype:str=None) -> Dict[str, Any]: # extract the command data num_cmd_motors = len(msg.motor_cmd) - print(f"{num_cmd_motors=}") + self._command_sequence += 1 cmd_data = { + "sequence": self._command_sequence, "mode_pr": int(msg.mode_pr), "mode_machine": int(msg.mode_machine), "motor_cmd": { + "mode": [int(msg.motor_cmd[i].mode) for i in range(num_cmd_motors)], "positions": [float(msg.motor_cmd[i].q) for i in range(num_cmd_motors)], "velocities": [float(msg.motor_cmd[i].dq) for i in range(num_cmd_motors)], "torques": [float(msg.motor_cmd[i].tau) for i in range(num_cmd_motors)], @@ -161,7 +164,6 @@ def get_robot_command(self) -> Optional[Dict[str, Any]]: """ if self.output_shm: data = self.output_shm.read_data() - print(f"{data=}") return data return None diff --git a/src/python/dds/specialized/inspire_dds.py b/src/python/dds/specialized/inspire_dds.py index e1969d71..5093a7f2 100644 --- a/src/python/dds/specialized/inspire_dds.py +++ b/src/python/dds/specialized/inspire_dds.py @@ -8,7 +8,7 @@ import threading from typing import Any, Dict, Optional from ..common.dds_base import DDSObject -from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber +from ..common.transport import ChannelPublisher, ChannelSubscriber from unitree_sdk2py.idl.unitree_go.msg.dds_ import MotorCmds_, MotorStates_ from unitree_sdk2py.idl.default import unitree_go_msg_dds__MotorCmd_, unitree_go_msg_dds__MotorState_ import numpy as np @@ -43,9 +43,9 @@ def __init__(self,node_name:str="inspire"): # setup the shared memory self.setup_shared_memory( input_shm_name="isaac_inspire_state", # read the state of the gripper from Isaac Lab - input_size=1024, + input_size=4096, output_shm_name="isaac_inspire_cmd", # output the command to Isaac Lab - output_size=1024, # output the command to Isaac Lab + output_size=4096, # output the command to Isaac Lab ) print(f"[{self.node_name}] Inspire Hand DDS node initialized") diff --git a/src/python/sensors/livox_dds.py b/src/python/sensors/livox_dds.py new file mode 100644 index 00000000..e42088ae --- /dev/null +++ b/src/python/sensors/livox_dds.py @@ -0,0 +1,68 @@ +"""Pinned livox_ros_driver2 message schema on the ROS 2 DDS wire. + +Direct CycloneDDS avoids loading ROS-generated CPython message extensions +from a different Python ABI into Isaac Sim's interpreter. +""" +from dataclasses import dataclass + +from cyclonedds.idl import IdlStruct +from cyclonedds.idl import annotations as annotate +from cyclonedds.idl import types +from unitree_sdk2py.idl.builtin_interfaces.msg.dds_ import Time_ +from unitree_sdk2py.idl.std_msgs.msg.dds_ import Header_ + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class CustomPoint_(IdlStruct, typename="livox_ros_driver2.msg.dds_.CustomPoint_"): + offset_time: types.uint32 + x: types.float32 + y: types.float32 + z: types.float32 + reflectivity: types.uint8 + tag: types.uint8 + line: types.uint8 + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class CustomMsg_(IdlStruct, typename="livox_ros_driver2.msg.dds_.CustomMsg_"): + header: Header_ + timebase: types.uint64 + point_num: types.uint32 + lidar_id: types.uint8 + rsvd: types.array[types.uint8, 3] + points: types.sequence[CustomPoint_] + + +class LivoxPublisher: + def __init__(self, domain): + from cyclonedds.domain import DomainParticipant + from cyclonedds.pub import DataWriter + from cyclonedds.topic import Topic + from cyclonedds.qos import Qos, Policy + from cyclonedds.util import duration + self.participant = DomainParticipant(domain) + self.topic = Topic(self.participant, 'rt/livox/lidar', CustomMsg_) + self.writer = DataWriter(self.participant, self.topic, Qos( + Policy.Reliability.Reliable(max_blocking_time=duration(milliseconds=100)), + Policy.History.KeepLast(5), + )) + + def publish(self, stamp_sec, stamp_nanosec, points): + sample = CustomMsg_( + Header_(Time_(stamp_sec, stamp_nanosec), 'lidar_link'), + stamp_sec * 1_000_000_000 + stamp_nanosec, + len(points), 0, [0, 0, 0], + [CustomPoint_(int(p['timestamp']), float(p['x']), float(p['y']), float(p['z']), + int(p['intensity']), int(p['tag']), int(p['line'])) for p in points], + ) + self.writer.write(sample) + + def close(self): + # CycloneDDS entities release their native handles when references end. + self.writer = None + self.topic = None + self.participant = None diff --git a/src/python/sensors/ros_sensor_bridge.py b/src/python/sensors/ros_sensor_bridge.py new file mode 100644 index 00000000..c5c48864 --- /dev/null +++ b/src/python/sensors/ros_sensor_bridge.py @@ -0,0 +1,253 @@ +"""ROS 2 transport for Isaac Lab camera, ray-caster and IMU samples. + +The ray-caster uses a uniform angular approximation of a MID360 scan. +Livox CustomMsg uses the same CycloneDDS transport as the Unitree bridge. +Only measured sensor channels are exposed, with no world poses or pose TF. +""" +from array import array +import io +import math +import os +from pathlib import Path +import struct +import sys + +import numpy as np +from PIL import Image as PILImage + + +POINT_DTYPE = np.dtype([ + ('x', ' 0) + return np.clip(np.where(valid, depth, 0) * 1000, 0, 65535).astype('= previous and seconds - previous < period - 1e-9: + return False + self._last[name] = seconds + return True + + def publish(self, seconds): + Image, CompressedImage, CameraInfo, Imu, PointCloud2, PointField, Clock, Time = self.types + sec, nanosec = stamp_parts(seconds) + stamp = Time(sec=sec, nanosec=nanosec) + self._publishers['clock'].publish(Clock(clock=stamp)) + for name in self._camera_names: + if self._due(name, seconds): + self._publish_camera(name, stamp) + if 'lidar' in self.scene.sensors and self._due('lidar', seconds): + data = self.scene.sensors['lidar'].data + cfg = self.scene.sensors['lidar'].cfg + pattern = cfg.pattern_cfg + samples = math.ceil((pattern.horizontal_fov_range[1] - pattern.horizontal_fov_range[0]) / pattern.horizontal_res) + points = lidar_payload(as_numpy(data.ray_hits_w[0]), as_numpy(data.pos_w[0]), as_numpy(data.quat_w[0]), samples, cfg.update_period) + msg = PointCloud2() + msg.header.stamp, msg.header.frame_id = stamp, "lidar_link" + msg.height, msg.width = 1, len(points) + msg.fields = [PointField(name=name, offset=POINT_DTYPE.fields[name][1], datatype=kind, count=1) + for name, kind in [('x', 7), ('y', 7), ('z', 7), ('intensity', 7), ('tag', 2), ('line', 2), ('timestamp', 8)]] + msg.is_bigendian, msg.is_dense = False, True + msg.point_step, msg.row_step = POINT_DTYPE.itemsize, POINT_DTYPE.itemsize*len(points) + msg.data = array('B', points.tobytes()) + self._livox.publish(sec, nanosec, points) + self._publishers['lidar'].publish(msg) + self._publishers['lidar_alias'].publish(msg) + if 'imu' in self.scene.sensors and self._due('imu', seconds): + data = self.scene.sensors['imu'].data + msg = Imu() + msg.header.stamp, msg.header.frame_id = stamp, "lidar_link" + # The hardware Livox IMU has no orientation estimate. + msg.orientation_covariance[0] = -1. + for target, values in ((msg.angular_velocity, data.ang_vel_b[0]), (msg.linear_acceleration, data.lin_acc_b[0])): + target.x, target.y, target.z = map(float, as_numpy(values)) + self._publishers['imu'].publish(msg) + self._publishers['imu_alias'].publish(msg) + + def _publish_camera(self, name, stamp): + Image, CompressedImage, CameraInfo, *_ = self.types + sensor = self.scene.sensors[name] + data = sensor.data + frame = CAMERAS[name][1] + rgb = np.ascontiguousarray(as_numpy(data.output['rgb'][0])[..., :3], dtype=np.uint8) + h, w = rgb.shape[:2] + + def raw(key, pixels, encoding, step): + msg = Image() + msg.header.stamp, msg.header.frame_id = stamp, frame + msg.height, msg.width, msg.encoding = h, w, encoding + msg.is_bigendian, msg.step = 0, step + msg.data = array('B', pixels.tobytes()) + self._publishers[(name, key)].publish(msg) + + def compressed(key, payload, format): + msg = CompressedImage() + msg.header.stamp, msg.header.frame_id = stamp, frame + msg.format, msg.data = format, array('B', payload) + self._publishers[(name, key)].publish(msg) + + raw('rgb', rgb, 'rgb8', w*3) + stream = io.BytesIO() + PILImage.fromarray(rgb).save(stream, format='JPEG', quality=80) + compressed('jpeg', stream.getvalue(), 'rgb8; jpeg compressed bgr8') + depth = as_numpy(data.output['distance_to_image_plane'][0]).reshape(h, w) + # A clipped far-plane value is a missing return, as on a RealSense. + depth = np.where(depth >= sensor.cfg.spawn.clipping_range[1]*.999, 0, depth) + mm = depth_millimeters(depth) + raw('depth', mm, '16UC1', w*2) + compressed('png', encode_depth(mm), '16UC1; compressedDepth png') + info = CameraInfo() + info.header.stamp, info.header.frame_id = stamp, frame + info.height, info.width = h, w + info.distortion_model, info.d = 'plumb_bob', [0.]*5 + matrix = as_numpy(data.intrinsic_matrices[0]).astype(float) + info.k = matrix.reshape(-1).tolist() + info.r = np.eye(3).reshape(-1).tolist() + info.p = np.column_stack((matrix, np.zeros(3))).reshape(-1).tolist() + self._publishers[(name, 'info')].publish(info) + + def close(self): + if self._livox is not None: + self._livox.close() + if self.node is not None: + self.node.destroy_node() + self.node = None + if self.context.ok(): + self.context.shutdown() diff --git a/tasks/common_config/camera_configs.py b/tasks/common_config/camera_configs.py index 3c302db4..ec089d50 100644 --- a/tasks/common_config/camera_configs.py +++ b/tasks/common_config/camera_configs.py @@ -120,7 +120,8 @@ def g1_front_camera(cls) -> CameraCfg: @classmethod def h12_front_camera(cls) -> CameraCfg: """front camera configuration""" - return CameraBaseCfg.get_camera_config(prim_path = "/World/envs/env_.*/Robot/camera_link/front_cam") + return CameraBaseCfg.get_camera_config(prim_path="/World/envs/env_.*/Robot/camera_link/front_cam", + data_types=["rgb", "distance_to_image_plane"]) @classmethod def g1_world_camera(cls) -> CameraCfg: """front camera configuration""" diff --git a/tasks/common_config/ray_caster_configs.py b/tasks/common_config/ray_caster_configs.py index c2854d94..cf92cffb 100644 --- a/tasks/common_config/ray_caster_configs.py +++ b/tasks/common_config/ray_caster_configs.py @@ -1,88 +1,40 @@ - -from isaaclab.sensors import RayCasterCfg, MultiMeshRayCaster, MultiMeshRayCasterCfg +"""MID360 angular approximation with tracked scene-object ray targets.""" +from isaaclab.sensors import RayCasterCfg, MultiMeshRayCasterCfg, patterns from isaaclab.utils import configclass -from isaaclab.sensors import patterns + @configclass class RayCasterBaseCfg: - """camera base configuration class - - provide the default configuration for different types of cameras, support scene-specific parameter customization - """ - @classmethod def get_ray_caster_cfg( cls, - prim_path: str = "/World/envs/env_.*/Robot/lidar_link", - debug_vis=True, - ray_alignment='world', - update_period: float = 0.02, - pos_offset: tuple = (0, 0.0, 0), - rot_offset=(0.0, 0.0, 0.0, 0.0) - + prim_path="/World/envs/env_.*/Robot/lidar_link", + debug_vis=False, + ray_alignment="base", + update_period=0.1, + pos_offset=(0., 0., 0.), + rot_offset=(1., 0., 0., 0.), ) -> MultiMeshRayCasterCfg: + fixed = ["/World/envs/env_0/Floor"] + [f"/World/envs/env_0/Wall{i}" for i in range(1, 5)] + fixed += [f"/World/envs/env_0/{name}" for name in ("PackingTable", "PackingTable2")] + moving = [f"/World/envs/env_0/{color}_block" for color in ("Red", "Yellow", "Green", "Pink", "Orange", "White")] + moving.append("/World/envs/env_0/Battery") return MultiMeshRayCasterCfg( - prim_path=prim_path, - debug_vis=True, - mesh_prim_paths=["/World/GroundPlane", "/World/envs/env_0/Wall1/geometry/mesh", "/World/envs/env_0/Wall2/geometry/mesh", "/World/envs/env_0/Wall3/geometry/mesh", "/World/envs/env_0/Wall4/geometry/mesh", "/World/envs/env_0/PackingTable", "/World/envs/env_0/PackingTable2", "/World/envs/env_0/Red_block", "/World/envs/env_0/Yellow_block", "/World/envs/env_0/Green_block", "/World/envs/env_0/Pink_block", "/World/envs/env_0/Orange_block", "/World/envs/env_0/White_block"], - #mesh_prim_paths=["/World/GroundPlane", "/World/envs/env_0/Floor/geometry/mesh", "/World/envs/env_0/Wall1/geometry/mesh", "/World/envs/env_0/Wall2/geometry/mesh", "/World/envs/env_0/Wall3/geometry/mesh", "/World/envs/env_0/Wall4/geometry/mesh", "/World/envs/env_0/PackingTable", "/World/envs/env_0/PackingTable2", "/World/envs/env_0/Red_block", "/World/envs/env_0/Yellow_block", "/World/envs/env_0/Green_block", "/World/envs/env_0/Pink_block", "/World/envs/env_0/Orange_block", "/World/envs/env_0/White_block"], - #mesh_prim_paths=[MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="/World/envs/env_0/Floor"), MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="/World/envs/env_0/Wall1"), MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="/World/envs/env_0/Wall2"), MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="/World/envs/env_0/Wall3"), MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="/World/envs/env_0/Wall4"), MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="/World/envs/env_0/PackingTable"), MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="/World/envs/env_0/PackingTable2"), MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="/World/envs/env_0/Red_block"), MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="/World/envs/env_0/Yellow_block"), MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="/World/envs/env_0/Green_block"), MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="/World/envs/env_0/Pink_block"), MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="/World/envs/env_0/Orange_block"), MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr="/World/envs/env_0/White_block")], - ray_alignment="world", - pattern_cfg=patterns.GridPatternCfg(resolution=0.02, size=(3.8 , 3.8), direction=(0, 0, -1)), + debug_vis=debug_vis, + update_period=update_period, + offset=RayCasterCfg.OffsetCfg(pos=pos_offset, rot=rot_offset), + mesh_prim_paths=fixed + [MultiMeshRayCasterCfg.RaycastTargetCfg(prim_expr=path, track_mesh_transforms=True) for path in moving], + ray_alignment=ray_alignment, + max_distance=40., + # The USD lidar_link is upright. MID360's non-repeating pattern is + # approximated by an elevation/azimuth grid, as in the MuJoCo bridge. + pattern_cfg=patterns.LidarPatternCfg(channels=56, vertical_fov_range=(-7., 52.), horizontal_fov_range=(0., 360.), horizontal_res=1.), ) - # return MultiMeshRayCasterCameraCfg( - # # prim_path="/World/envs/env_.*/Robot/base_link", - # prim_path=prim_path, - # update_period=update_period, - # #data_types=["face_ids", "normals"], - # offset=RayCasterCameraCfg.OffsetCfg( - # pos=pos_offset, - # #rot=(0, 0, -0.7071068, 0.7071068), - # rot=rot_offset, - # convention="ros" - # ), - # pattern_cfg=patterns.PinholeCameraPatternCfg( - # height=500, - # width=500, - # focal_length=24.0, - # horizontal_aperture=20.955, - # ), - # mesh_prim_paths=[ - # MultiMeshRayCasterCameraCfg.RaycastTargetCfg( - # target_prim_expr=env_parameters["inspection_goal_prim_path"], - # track_mesh_transforms=True - # ) - # ], - # update_mesh_ids=True, - # debug_vis=cfg_mode.debug - #) - #return MultiMeshRayCasterCameraCfg( - # prim_path=prim_path, - # update_period=update_period, - # offset=RayCasterCameraCfg.OffsetCfg( - # pos=pos_offset, - # rot=rot_offset, - # convention="ros" - # ), - # debug_vis=debug_vis, - # mesh_prim_paths=["/World/envs/env_0/Floor", "/World/envs/env_0/Wall1", "/World/envs/env_0/Wall2", "/World/envs/env_0/Wall3", "/World/envs/env_0/Wall4", "/World/envs/env_0/PackingTable", "/World/envs/env_0/PackingTable2", "/World/envs/env_0/Red_block", "/World/envs/env_0/Yellow_block", "/World/envs/env_0/Green_block", "/World/envs/env_0/Pink_block", "/World/envs/env_0/Orange_block", "/World/envs/env_0/White_block"], - # pattern_cfg=patterns.LidarPatternCfg( - # channels=100, vertical_fov_range=[-90,90], horizontal_fov_range=[-90,90], horizontal_res=1.0 - - # ), - # ray_alignment=ray_alignment - #) - - - - @configclass class RayCasterPresets: - @classmethod - def livox_lidar(cls) -> RayCasterCfg: - """front camera configuration""" + def livox_lidar(cls) -> MultiMeshRayCasterCfg: return RayCasterBaseCfg.get_ray_caster_cfg() diff --git a/tasks/common_observations/h12_27dof_state.py b/tasks/common_observations/h12_27dof_state.py index 0bb19cd2..dd5b9913 100644 --- a/tasks/common_observations/h12_27dof_state.py +++ b/tasks/common_observations/h12_27dof_state.py @@ -1,252 +1,71 @@ # Copyright (c) 2025, Unitree Robotics Co., Ltd. All Rights Reserved. -# License: Apache License, Version 2.0 -""" -h12_27 dof state -""" +# License: Apache License, Version 2.0 +"""Name-resolved joint observations in the motor wire contract order.""" from __future__ import annotations -import torch -from typing import TYPE_CHECKING -import os - -import sys -from multiprocessing import shared_memory import time - -if TYPE_CHECKING: - from isaaclab.envs import ManagerBasedRLEnv - - import torch - -#TODO OPTIMIZE THIS CODE TO PREVENT UNNECESSARY REPEATED INTERFACE ACQUISITIONS (JUST ASSUME INTERFACE IS GOOD AND PERSISTS) +from src.python.control.motor_contract import BODY_JOINT_NAMES def get_robot_boy_joint_names() -> list[str]: - return [ - # leg joints (12) - # left leg (6)all_joint_names - "left_hip_pitch_joint", - "left_hip_roll_joint", - "left_hip_yaw_joint", - "left_knee_joint", - "left_ankle_pitch_joint", - "left_ankle_roll_joint", - # right leg (6) - "right_hip_pitch_joint", - "right_hip_roll_joint", - "right_hip_yaw_joint", - "right_knee_joint", - "right_ankle_pitch_joint", - "right_ankle_roll_joint", - "torso_joint", - # arm joints (14) - # left arm (7) - "left_shoulder_pitch_joint", - "left_shoulder_roll_joint", - "left_shoulder_yaw_joint", - "left_elbow_joint", - "left_wrist_roll_joint", - "left_wrist_pitch_joint", - "left_wrist_yaw_joint", - # right arm (7) - "right_shoulder_pitch_joint", - "right_shoulder_roll_joint", - "right_shoulder_yaw_joint", - "right_elbow_joint", - "right_wrist_roll_joint", - "right_wrist_pitch_joint", - "right_wrist_yaw_joint", - ] + return list(BODY_JOINT_NAMES) -def get_robot_arm_joint_names() -> list[str]: - return [ - # arm joints (14) - # left arm (7) - "left_shoulder_pitch_joint", - "left_shoulder_roll_joint", - "left_shoulder_yaw_joint", - "left_elbow_joint", - "left_wrist_roll_joint", - "left_wrist_pitch_joint", - "left_wrist_yaw_joint", - # right arm (7) - "right_shoulder_pitch_joint", - "right_shoulder_roll_joint", - "right_shoulder_yaw_joint", - "right_elbow_joint", - "right_wrist_roll_joint", - "right_wrist_pitch_joint", - "right_wrist_yaw_joint", - ] -# global variable to cache the DDS instance -from src.python.dds.common.dds_master import dds_manager _h12_robot_dds = None -_dds_initialized = False -_obs_cache = { - "device": None, - "batch": None, - "boy_idx_t": None, - "boy_idx_batch": None, - "pos_buf": None, - "vel_buf": None, - "torque_buf": None, - "combined_buf": None, - "dds_last_ms": 0, - "dds_min_interval_ms": 20, -} -# IMU acceleration cache: for computing acceleration via velocity differentiation -_imu_acc_cache = { - "prev_vel": None, - "dt": 0.01, - "initialized": False, -} def _get_h12_robot_dds_instance(): - """get the DDS instance, delay initialization""" - global _h12_robot_dds, _dds_initialized - - if not _dds_initialized or _h12_robot_dds is None: - try: - # dynamically import the DDS module - sys.path.insert(0, os.path.join(os.path.dirname(os.path.dirname(__file__)), 'dds')) - from src.python.dds.common.dds_master import dds_manager - print(f"dds_manager: {dds_manager}") - _h12_robot_dds = dds_manager.get_object("h1_2") - print("[h12_state] H12 robot DDS communication instance obtained") - - # register the cleanup function - import atexit - def cleanup_dds(): - try: - if _h12_robot_dds: - dds_manager.unregister_object("h1_2") - print("[h12_state] DDS communication closed correctly") - except Exception as e: - print(f"[h12_state] Error closing DDS: {e}") - atexit.register(cleanup_dds) - - except Exception as e: - print(f"[h12_state] Failed to get H12 robot DDS instance: {e}") - _h12_robot_dds = None - - _dds_initialized = True - + """Retry late DDS initialization; the DDS manager owns cleanup.""" + global _h12_robot_dds + if _h12_robot_dds is None: + from src.python.dds.common.dds_master import dds_manager + _h12_robot_dds = dds_manager.get_object("h1_2") return _h12_robot_dds -def get_robot_boy_joint_states( - env: ManagerBasedRLEnv, - enable_dds: bool = True, -) -> torch.Tensor: - """get the robot body joint states, positions and velocities - - Args: - env: ManagerBasedRLEnv - reinforcement learning environment instance - enable_dds: bool - whether to enable the DDS publish function - - Returns: - torch.Tensor - - the first 29 elements are joint positions - - the middle 29 elements are joint velocities - - the last 29 elements are joint torques - """ - # get all joint states - joint_pos = env.scene["robot"].data.joint_pos - print(f"{joint_pos=}") - - joint_vel = env.scene["robot"].data.joint_vel - - print(f"{joint_vel=}") - joint_torque = env.scene["robot"].data.applied_torque # use applied_torque to get joint torques - - print(f"{joint_torque=}") - device = joint_pos.device - batch = joint_pos.shape[0] - - # get the body joint indices - boy_joint_names = get_robot_boy_joint_names() - all_joint_names = env.scene["robot"].data.joint_names - boy_joint_indices = [all_joint_names.index(name) for name in boy_joint_names] - # print(f"boy_joint_indices: {boy_joint_indices}") - - # 预计算并缓存索引张量(列索引) - global _obs_cache - #if _obs_cache["device"] != device or _obs_cache["boy_idx_t"] is None: - if True: - - all_joint_names = env.scene["robot"].data.joint_names - print(f"{all_joint_names=}") - - boy_joint_indices = [all_joint_names.index(name) for name in boy_joint_names] - print(f"{boy_joint_indices=}") - - #boy_joint_indices = [3, 7, 0, 11, 15, 19, 4, 8, 1, 12, 16, 20, 5, 9, 13, 17, 21, 23, 25, 6, 10, 14, 18, 22, 24, 26] -#[0, 3, 6, 9, 13, 17, 1, 4, 7, 10, 14, 18, 2, 5, 8, 11, 15, 19, 21, 23, 25, 27, 12, 16, 20, 22, 24, 26, 28] - _obs_cache["boy_idx_t"] = torch.tensor(boy_joint_indices, dtype=torch.long, device=device) - _obs_cache["device"] = device - _obs_cache["batch"] = None # force re-init batch-shaped buffers - - idx_t = _obs_cache["boy_idx_t"] - n = idx_t.numel() - - # 预分配/复用 batch 形状索引与输出缓冲 - if _obs_cache["batch"] != batch or _obs_cache["boy_idx_batch"] is None: - _obs_cache["boy_idx_batch"] = idx_t.unsqueeze(0).expand(batch, n) - _obs_cache["pos_buf"] = torch.empty(batch, n, device=device, dtype=joint_pos.dtype) - _obs_cache["vel_buf"] = torch.empty(batch, n, device=device, dtype=joint_pos.dtype) - _obs_cache["torque_buf"] = torch.empty(batch, n, device=device, dtype=joint_pos.dtype) - _obs_cache["combined_buf"] = torch.empty(batch, n * 3, device=device, dtype=joint_pos.dtype) - _obs_cache["batch"] = batch - - idx_batch = _obs_cache["boy_idx_batch"] - pos_buf = _obs_cache["pos_buf"] - vel_buf = _obs_cache["vel_buf"] - torque_buf = _obs_cache["torque_buf"] - combined_buf = _obs_cache["combined_buf"] - - # 使用 gather(out=...) - try: - torch.gather(joint_pos, 1, idx_batch, out=pos_buf) - torch.gather(joint_vel, 1, idx_batch, out=vel_buf) - torch.gather(joint_torque, 1, idx_batch, out=torque_buf) - except TypeError: - pos_buf.copy_(torch.gather(joint_pos, 1, idx_batch)) - vel_buf.copy_(torch.gather(joint_vel, 1, idx_batch)) - torque_buf.copy_(torch.gather(joint_torque, 1, idx_batch)) - - # 组合为一个缓冲,避免 cat 分配 - combined_buf[:, 0:n].copy_(pos_buf) - combined_buf[:, n:2*n].copy_(vel_buf) - combined_buf[:, 2*n:3*n].copy_(torque_buf) - - # write to DDS( - if enable_dds and combined_buf.shape[0] > 0: - try: - import time - now_ms = int(time.time() * 1000) - if now_ms - _obs_cache["dds_last_ms"] >= _obs_cache["dds_min_interval_ms"]: - h12_robot_dds = _get_h12_robot_dds_instance() - if h12_robot_dds: - imu_data = get_robot_imu_data(env) - if imu_data.shape[0] > 0: - h12_robot_dds.write_robot_state( - pos_buf[0].contiguous().cpu().numpy(), - vel_buf[0].contiguous().cpu().numpy(), - torque_buf[0].contiguous().cpu().numpy(), - imu_data[0].contiguous().cpu().numpy(), - ) - _obs_cache["dds_last_ms"] = now_ms - except Exception as e: - print(f"[h1_state] Error writing robot state to DDS: {e}") - - return combined_buf - - +def get_robot_boy_joint_states(env, enable_dds: bool = True) -> torch.Tensor: + """Return positions, velocities and torques (27 each); publish environment zero at most every 20 ms. + The returned tensor is a reusable buffer, valid until the next observation. + """ + robot = env.scene["robot"] + data = robot.data + pos, vel, torque = data.joint_pos, data.joint_vel, data.applied_torque + signature = (id(env.scene), id(robot), tuple(data.joint_names), pos.shape, + pos.device, pos.dtype, vel.dtype, torque.dtype) + cache = getattr(env, "_h12_robot_observation_cache", None) + if cache is None or cache["signature"] != signature: + names = list(data.joint_names) + indices = torch.tensor([names.index(name) for name in BODY_JOINT_NAMES], + dtype=torch.long, device=pos.device) + n = len(BODY_JOINT_NAMES) + combined = torch.empty((pos.shape[0], 3 * n), device=pos.device, dtype=pos.dtype) + cache = {"signature": signature, "indices": indices, + "combined": combined, "last_publish": float("-inf")} + setattr(env, "_h12_robot_observation_cache", cache) + n = len(BODY_JOINT_NAMES) + combined = cache["combined"] + pos_buf, vel_buf, torque_buf = combined[:, :n], combined[:, n:2*n], combined[:, 2*n:] + torch.index_select(pos, 1, cache["indices"], out=pos_buf) + torch.index_select(vel, 1, cache["indices"], out=vel_buf) + torch.index_select(torque, 1, cache["indices"], out=torque_buf) + if enable_dds and pos.shape[0]: + now = time.monotonic() + if now - cache["last_publish"] >= .020: + try: + interface = _get_h12_robot_dds_instance() + if interface is not None: + interface.write_robot_state( + pos_buf[0].cpu().numpy(), vel_buf[0].cpu().numpy(), + torque_buf[0].cpu().numpy(), get_robot_imu_data(env)[0].cpu().numpy()) + cache["last_publish"] = now + except Exception as exc: + print(f"[h1_2_state] Failed to publish state: {exc}") + return combined +def get_robot_arm_joint_names() -> list[str]: + return list(BODY_JOINT_NAMES[13:]) def quat_to_rot_matrix(q): @@ -288,129 +107,73 @@ def quat_to_rot_matrix(q): torch.cat([r00, r01, r02], dim=1).unsqueeze(1), torch.cat([r10, r11, r12], dim=1).unsqueeze(1), torch.cat([r20, r21, r22], dim=1).unsqueeze(1), - ], dim=1) # [B,3,3] but built transposed blocks; fix shape next - - # Currently R is [B,3,3] where rows are correct; reshape properly: - R = R.view(-1, 3, 3) + ], dim=1) return R -def ensure_quat_w_first(quat, assume_w_first=None): - """ - quat: [B,4] unknown ordering. - If assume_w_first is True/False enforce; if None do heuristic: - - if mean(abs(quat[:,0])) > 0.9 -> likely w in index 0 (w,x,y,z) - - elif mean(abs(quat[:,3])) > 0.9 -> likely (x,y,z,w) so reorder - - else keep as is (user should verify). - Returns quat_wxyz: [B,4] (w,x,y,z) - """ - if assume_w_first is True: - return quat - if assume_w_first is False: - # reorder xyzw -> wxyz - return torch.cat([quat[:, 3:4], quat[:, 0:3]], dim=1) - - # heuristic: - b = quat.shape[0] - mean0 = torch.mean(torch.abs(quat[:, 0])) - mean3 = torch.mean(torch.abs(quat[:, 3])) - if mean0 > 0.9: - return quat # already w first - if mean3 > 0.9: - return torch.cat([quat[:, 3:4], quat[:, 0:3]], dim=1) - # ambiguous: default to w-first but warn (can't print here reliably for all contexts) - return quat - -def get_robot_imu_data(env, use_torso_imu: bool = True, quat_w_first: bool = None) -> torch.Tensor: - """ - Returns [batch, 13] = pos(world,3) | quat(w,x,y,z) | acc_body(3) | gyro_body(3) - - accel/gyro are in IMU/body frame (proper accelerometer reading) - - quat_w_first: if None do heuristic, if True assume input quat already (w,x,y,z), - if False assume input quat is (x,y,z,w) - """ - data = env.scene["robot"].data - global _imu_acc_cache - - # --- dt --- - dt = _imu_acc_cache["dt"] - try: - if hasattr(env, "physics_dt"): - dt = float(env.physics_dt) - elif hasattr(env, "step_dt"): - dt = float(env.step_dt) - elif hasattr(env, "dt"): - dt = float(env.dt) - except Exception: - pass - if dt <= 0: - dt = _imu_acc_cache["dt"] - - # --- extract pose & vel --- - if use_torso_imu: - try: - body_names = data.body_names - imu_idx = body_names.index("imu_in_torso") - body_pose = data.body_link_pose_w # [B, N, 7] - body_vel = data.body_link_vel_w # [B, N, 6] - pos = body_pose[:, imu_idx, :3] - quat = body_pose[:, imu_idx, 3:7] - lin_vel = body_vel[:, imu_idx, :3] - ang_vel_world = body_vel[:, imu_idx, 3:6] - except ValueError: - use_torso_imu = False - - if not use_torso_imu: - root_state = data.root_state_w # [B, 13] - pos = root_state[:, :3] - quat = root_state[:, 3:7] - lin_vel = root_state[:, 7:10] - ang_vel_world = root_state[:, 10:13] - - # device/dtype consistency - device = lin_vel.device if isinstance(lin_vel, torch.Tensor) else torch.device("cpu") - quat = quat.to(device) - lin_vel = lin_vel.to(device) - ang_vel_world = ang_vel_world.to(device) - - # initialize prev_vel if needed - if _imu_acc_cache["prev_vel"] is None: - _imu_acc_cache["prev_vel"] = lin_vel.clone().detach().to(device) - _imu_acc_cache["initialized"] = False +def reset_robot_imu_cache(env, env_ids=None): + """Isaac reset event: discard velocity history for the affected environments.""" + cache = getattr(env, "_h12_imu_cache", None) + if cache is None: + return + if env_ids is None: + env._h12_imu_cache = None else: - if _imu_acc_cache["prev_vel"].device != device: - _imu_acc_cache["prev_vel"] = _imu_acc_cache["prev_vel"].to(device) - - # compute a_world - a_world = (lin_vel - _imu_acc_cache["prev_vel"]) / dt # [B,3] - - # gravity in world frame (z-up convention) - g_world = torch.zeros_like(a_world) - g_world[:, 2] = -9.81 - - # subtract gravity (proper acceleration in world frame) - a_world_corrected = a_world - g_world # [B,3] + cache["reset_pending"][env_ids] = True - # prepare quaternion in (w,x,y,z) - quat_wxyz = ensure_quat_w_first(quat, assume_w_first=True) - # build rotation matrices R_body->world ; to convert world->body use R^T - R_body_to_world = quat_to_rot_matrix(quat_wxyz) # [B,3,3] - R_world_to_body = R_body_to_world.transpose(1, 2) # [B,3,3] +def _simulation_time(env): + sim = getattr(env, "sim", None) + if sim is not None and hasattr(sim, "current_time"): + return float(sim.current_time) + # ManagerBasedRLEnv counts physics steps even with control decimation. + return float(env._sim_step_counter) * float(env.physics_dt) - # rotate a_world_corrected to body: a_body = R_world_to_body @ a_world_corrected - a_body = torch.bmm(R_world_to_body, a_world_corrected.unsqueeze(-1)).squeeze(-1) # [B,3] - # rotate angular velocity to body frame as well (if ang_vel_world is indeed in world-frame) - omega_body = torch.bmm(R_world_to_body, ang_vel_world.unsqueeze(-1)).squeeze(-1) - - # handle first frame: prefer returning only gravity-compensated static reading - if not _imu_acc_cache["initialized"]: - # set a_body to rotation of -g_world (so accelerometer reads gravity in body coords) - a_body = torch.bmm(R_world_to_body, (-g_world).unsqueeze(-1)).squeeze(-1) - _imu_acc_cache["initialized"] = True - - # update cache - _imu_acc_cache["prev_vel"] = lin_vel.clone().detach() - _imu_acc_cache["dt"] = dt +def get_robot_imu_data(env, use_torso_imu: bool = True, quat_w_first: bool = None) -> torch.Tensor: + """Return world position, WXYZ quaternion, proper body acceleration and gyro. - imu_data = torch.cat([pos, quat_wxyz, a_body, omega_body], dim=1) - return imu_data + Isaac poses are WXYZ. Velocity differences use elapsed simulation time, + including skipped wall-clock publications. Reset rows start at gravity only. + """ + robot = env.scene["robot"] + data = robot.data + imu_name = next((name for name in ("imu_in_torso", "imu_link") if name in data.body_names), None) + if use_torso_imu and imu_name is not None: + idx = data.body_names.index(imu_name) + pose, velocity = data.body_link_pose_w[:, idx], data.body_link_vel_w[:, idx] + pos, quat = pose[:, :3], pose[:, 3:7] + lin_vel, angular_vel = velocity[:, :3], velocity[:, 3:6] + source = idx + else: + root = data.root_state_w + pos, quat, lin_vel, angular_vel = root[:, :3], root[:, 3:7], root[:, 7:10], root[:, 10:13] + source = None + now = _simulation_time(env) + signature = (id(env.scene), id(robot), source, lin_vel.shape, lin_vel.device, lin_vel.dtype) + cache = getattr(env, "_h12_imu_cache", None) + episode = getattr(env, "episode_length_buf", None) + acceleration = torch.zeros_like(lin_vel) + valid = cache is not None and cache["signature"] == signature and now > cache["time"] + if valid: + acceleration = (lin_vel - cache["velocity"]) / (now - cache["time"]) + if episode is not None and cache["episode"] is not None: + reset = (episode < cache["episode"]) | (episode == 0) + acceleration[reset] = 0 + elif cache is not None and cache["signature"] == signature and now == cache["time"]: + # Repeated observations of one physics sample should have identical IMU data. + acceleration.copy_(cache["acceleration"]) + if episode is not None and cache["episode"] is not None: + acceleration[episode < cache["episode"]] = 0 + if cache is not None and cache["signature"] == signature: + acceleration[cache["reset_pending"]] = 0 + env._h12_imu_cache = {"signature": signature, "time": now, + "reset_pending": torch.zeros(lin_vel.shape[0], dtype=torch.bool, device=lin_vel.device), + "velocity": lin_vel.detach().clone(), "acceleration": acceleration.detach().clone(), + "episode": None if episode is None else episode.clone()} + proper = acceleration.clone() + proper[:, 2] += 9.81 + # No quaternion heuristics: Isaac Lab's documented pose convention is WXYZ. + world_to_body = quat_to_rot_matrix(quat).transpose(1, 2) + accel_body = torch.bmm(world_to_body, proper.unsqueeze(-1)).squeeze(-1) + gyro_body = torch.bmm(world_to_body, angular_vel.unsqueeze(-1)).squeeze(-1) + return torch.cat((pos, quat, accel_body, gyro_body), dim=1) diff --git a/tasks/common_observations/inspire_state.py b/tasks/common_observations/inspire_state.py index 98412f01..77107da7 100644 --- a/tasks/common_observations/inspire_state.py +++ b/tasks/common_observations/inspire_state.py @@ -1,157 +1,66 @@ # Copyright (c) 2025, Unitree Robotics Co., Ltd. All Rights Reserved. -# License: Apache License, Version 2.0 -""" -gripper state -""" - +# License: Apache License, Version 2.0 +"""Name-resolved joint observations in the motor wire contract order.""" from __future__ import annotations +import time import torch -from typing import TYPE_CHECKING -import sys -import os -if TYPE_CHECKING: - from isaaclab.envs import ManagerBasedRLEnv - - -import torch +from src.python.control.motor_contract import HAND_JOINT_NAMES -_obs_cache = { - "device": None, - "batch": None, - "inspire_idx_t": None, - "inspire_idx_batch": None, - "pos_buf": None, - "vel_buf": None, - "torque_buf": None, - "dds_last_ms": 0, - "dds_min_interval_ms": 20, -} - def get_robot_girl_joint_names() -> list[str]: - return [ - "R_pinky_proximal_joint", - "R_ring_proximal_joint", - "R_middle_proximal_joint", - "R_index_proximal_joint", - "R_thumb_proximal_pitch_joint", - "R_thumb_proximal_yaw_joint", - "L_pinky_proximal_joint", - "L_ring_proximal_joint", - "L_middle_proximal_joint", - "L_index_proximal_joint", - "L_thumb_proximal_pitch_joint", - "L_thumb_proximal_yaw_joint", - ] + return list(HAND_JOINT_NAMES) + -# global variable to cache the DDS instance _inspire_dds = None -_dds_initialized = False + def _get_inspire_dds_instance(): - """get the DDS instance, delay initialization""" - global _inspire_dds, _dds_initialized - - if not _dds_initialized or _inspire_dds is None: - try: - # dynamically import the DDS module - sys.path.insert(0, os.path.join(os.path.dirname(os.path.dirname(__file__)), 'dds')) - from src.python.dds.common.dds_master import dds_manager - _inspire_dds = dds_manager.get_object("inspire") - print("[Observations] DDS communication instance obtained") - - # register the cleanup function - import atexit - def cleanup_dds(): - try: - if _inspire_dds: - dds_manager.unregister_object("inspire") - print("[gripper_state] DDS communication closed correctly") - except Exception as e: - print(f"[gripper_state] Error closing DDS: {e}") - atexit.register(cleanup_dds) - - except Exception as e: - print(f"[Observations] Failed to get DDS instances: {e}") - _inspire_dds = None - - _dds_initialized = True - + """Retry late DDS initialization; the DDS manager owns cleanup.""" + global _inspire_dds + if _inspire_dds is None: + from src.python.dds.common.dds_master import dds_manager + _inspire_dds = dds_manager.get_object("inspire") return _inspire_dds +def get_robot_inspire_joint_states(env, enable_dds: bool = True) -> torch.Tensor: + """Return the 12 hand positions; publish environment zero at most every 20 ms. -def get_robot_inspire_joint_states( - env: ManagerBasedRLEnv, - enable_dds: bool = True, -) -> torch.Tensor: - """get the robot gripper joint states and publish them to DDS - - Args: - env: ManagerBasedRLEnv - reinforcement learning environment instance - enable_dds: bool - whether to enable the DDS publish function - - 返回: - torch.Tensor + The returned tensor is a reusable buffer, valid until the next observation. """ - # get the gripper joint states - joint_pos = env.scene["robot"].data.joint_pos - joint_vel = env.scene["robot"].data.joint_vel - joint_torque = env.scene["robot"].data.applied_torque - device = joint_pos.device - batch = joint_pos.shape[0] - - - global _obs_cache - if _obs_cache["device"] != device or _obs_cache["inspire_idx_t"] is None: - inspire_joint_indices = [36, 37, 35, 34, 48, 38, 31, 32, 30, 29, 43, 33] - _obs_cache["inspire_idx_t"] = torch.tensor(inspire_joint_indices, dtype=torch.long, device=device) - _obs_cache["device"] = device - _obs_cache["batch"] = None - idx_t = _obs_cache["inspire_idx_t"] - n = idx_t.numel() - - - if _obs_cache["batch"] != batch or _obs_cache["inspire_idx_batch"] is None: - _obs_cache["inspire_idx_batch"] = idx_t.unsqueeze(0).expand(batch, n) - _obs_cache["pos_buf"] = torch.empty(batch, n, device=device, dtype=joint_pos.dtype) - _obs_cache["vel_buf"] = torch.empty(batch, n, device=device, dtype=joint_pos.dtype) - _obs_cache["torque_buf"] = torch.empty(batch, n, device=device, dtype=joint_pos.dtype) - _obs_cache["batch"] = batch - - idx_batch = _obs_cache["inspire_idx_batch"] - pos_buf = _obs_cache["pos_buf"] - vel_buf = _obs_cache["vel_buf"] - torque_buf = _obs_cache["torque_buf"] - - - try: - torch.gather(joint_pos, 1, idx_batch, out=pos_buf) - torch.gather(joint_vel, 1, idx_batch, out=vel_buf) - torch.gather(joint_torque, 1, idx_batch, out=torque_buf) - except TypeError: - pos_buf.copy_(torch.gather(joint_pos, 1, idx_batch)) - vel_buf.copy_(torch.gather(joint_vel, 1, idx_batch)) - torque_buf.copy_(torch.gather(joint_torque, 1, idx_batch)) - - # publish to DDS (only publish the data of the first environment) - if enable_dds and len(pos_buf) > 0: - try: - import time - now_ms = int(time.time() * 1000) - if now_ms - _obs_cache["dds_last_ms"] >= _obs_cache["dds_min_interval_ms"]: - inspire_dds = _get_inspire_dds_instance() - if inspire_dds: - pos = pos_buf[0].contiguous().cpu().numpy() - vel = vel_buf[0].contiguous().cpu().numpy() - torque = torque_buf[0].contiguous().cpu().numpy() - # write the gripper state to shared memory - inspire_dds.write_inspire_state(pos, vel, torque) - _obs_cache["dds_last_ms"] = now_ms - except Exception as e: - print(f"[gripper_state] Failed to write to shared memory: {e}") - + robot = env.scene["robot"] + data = robot.data + pos, vel, torque = data.joint_pos, data.joint_vel, data.applied_torque + signature = (id(env.scene), id(robot), tuple(data.joint_names), pos.shape, + pos.device, pos.dtype, vel.dtype, torque.dtype) + cache = getattr(env, "_inspire_observation_cache", None) + if cache is None or cache["signature"] != signature: + names = list(data.joint_names) + indices = torch.tensor([names.index(name) for name in HAND_JOINT_NAMES], + dtype=torch.long, device=pos.device) + n = len(HAND_JOINT_NAMES) + combined = torch.empty((pos.shape[0], 3 * n), device=pos.device, dtype=pos.dtype) + cache = {"signature": signature, "indices": indices, + "combined": combined, "last_publish": float("-inf")} + setattr(env, "_inspire_observation_cache", cache) + n = len(HAND_JOINT_NAMES) + combined = cache["combined"] + pos_buf, vel_buf, torque_buf = combined[:, :n], combined[:, n:2*n], combined[:, 2*n:] + torch.index_select(pos, 1, cache["indices"], out=pos_buf) + torch.index_select(vel, 1, cache["indices"], out=vel_buf) + torch.index_select(torque, 1, cache["indices"], out=torque_buf) + if enable_dds and pos.shape[0]: + now = time.monotonic() + if now - cache["last_publish"] >= .020: + try: + interface = _get_inspire_dds_instance() + if interface is not None: + interface.write_inspire_state( + pos_buf[0].cpu().numpy(), vel_buf[0].cpu().numpy(), + torque_buf[0].cpu().numpy()) + cache["last_publish"] = now + except Exception as exc: + print(f"[inspire_state] Failed to publish state: {exc}") return pos_buf - diff --git a/tasks/h1-2_tasks/stack_rgyblock_h12_27dof_inspire/mdp/observations.py b/tasks/h1-2_tasks/stack_rgyblock_h12_27dof_inspire/mdp/observations.py index 1256bcce..d7796f9a 100644 --- a/tasks/h1-2_tasks/stack_rgyblock_h12_27dof_inspire/mdp/observations.py +++ b/tasks/h1-2_tasks/stack_rgyblock_h12_27dof_inspire/mdp/observations.py @@ -1,7 +1,7 @@ # Copyright (c) 2025, Unitree Robotics Co., Ltd. All Rights Reserved. -# License: Apache License, Version 2.0 +# License: Apache License, Version 2.0 from tasks.common_observations.h12_27dof_state import get_robot_boy_joint_states -#from tasks.common_observations.inspire_state import get_robot_inspire_joint_states +from tasks.common_observations.inspire_state import get_robot_inspire_joint_states from tasks.common_observations.camera_state import get_camera_image from tasks.common_observations.imu_state import get_imu_state from tasks.common_observations.lidar_state import get_lidar_state @@ -9,7 +9,7 @@ # ensure functions can be accessed by external modules __all__ = [ "get_robot_boy_joint_states", - #"get_robot_inspire_joint_states", + "get_robot_inspire_joint_states", "get_camera_image", "get_imu_state", "get_lidar_state" diff --git a/tasks/h1-2_tasks/stack_rgyblock_h12_27dof_inspire/stack_rgyblock_h12_27dof_inspire_joint_env_cfg.py b/tasks/h1-2_tasks/stack_rgyblock_h12_27dof_inspire/stack_rgyblock_h12_27dof_inspire_joint_env_cfg.py index 14cb8a2c..5030163f 100644 --- a/tasks/h1-2_tasks/stack_rgyblock_h12_27dof_inspire/stack_rgyblock_h12_27dof_inspire_joint_env_cfg.py +++ b/tasks/h1-2_tasks/stack_rgyblock_h12_27dof_inspire/stack_rgyblock_h12_27dof_inspire_joint_env_cfg.py @@ -17,6 +17,8 @@ from isaaclab.utils import configclass from isaaclab.assets import ArticulationCfg from . import mdp +from src.python.control.unitree_action import UnitreeMotorActionCfg +from tasks.common_observations.h12_27dof_state import reset_robot_imu_cache from isaaclab.envs import ViewerCfg # use Isaac Lab native event system @@ -48,16 +50,16 @@ class ObjectTableSceneCfg(TableRedGreenYellowBlockSceneCfg): # 6. add camera configuration - #front_camera = CameraPresets.h12_front_camera() - #left_wrist_camera = CameraPresets.left_inspire_wrist_camera() - #right_wrist_camera = CameraPresets.right_inspire_wrist_camera() + front_camera = CameraPresets.h12_front_camera() + left_wrist_camera = CameraPresets.left_inspire_wrist_camera() + right_wrist_camera = CameraPresets.right_inspire_wrist_camera() #correll_cam_0 = CameraPresets.correll_camera_0() #correll_cam_1 = CameraPresets.correll_camera_1() #correll_cam_2 = CameraPresets.correll_camera_2() - #imu = ImuPresets.livox_imu() + imu = ImuPresets.livox_imu() - #lidar = RayCasterPresets.livox_lidar() + lidar = RayCasterPresets.livox_lidar() ## # MDP settings @@ -66,7 +68,7 @@ class ObjectTableSceneCfg(TableRedGreenYellowBlockSceneCfg): class ActionsCfg: """defines the action configuration related to robot control, using direct joint angle control """ - joint_pos = mdp.JointPositionActionCfg(asset_name="robot", joint_names=[".*"], scale=1.0, use_default_offset=True) + motor = UnitreeMotorActionCfg(asset_name="robot") @@ -84,7 +86,7 @@ class PolicyCfg(ObsGroup): # 1. robot joint state observation robot_joint_state = ObsTerm(func=mdp.get_robot_boy_joint_states) # 2. gripper joint state observation - #robot_inspire_state = ObsTerm(func=mdp.get_robot_inspire_joint_states) + robot_inspire_state = ObsTerm(func=mdp.get_robot_inspire_joint_states) # 3. camera image observation #camera_image = ObsTerm(func=mdp.get_camera_image) @@ -119,7 +121,7 @@ class RewardsCfg: @configclass class EventCfg: - pass + reset_imu = EventTermCfg(func=reset_robot_imu_cache, mode="reset") #reset_red_block = EventTermCfg( # func=mdp.reset_root_state_uniform, # use uniform distribution reset function # mode="reset", # set event mode to reset @@ -187,6 +189,11 @@ def __post_init__(self): """Post initialization.""" self.decimation = 2 self.episode_length_s = 20.0 + # Body drives stay released until the first valid Unitree command. + for name, actuator in self.scene.robot.actuators.items(): + if name != "hands": + actuator.stiffness = 0.0 + actuator.damping = 0.0 # simulation settings self.sim.dt = 0.005 self.sim.render_interval = self.decimation From 29615de6346a74a32674d4a78fba7148d11beca6 Mon Sep 17 00:00:00 2001 From: maxconway Date: Wed, 16 Sep 2026 16:07:01 -0600 Subject: [PATCH 2/7] Use Magpie H1-2 asset with coupled grippers and ROS services --- sim_main.py | 18 +- src/python/control/action_provider_dds.py | 2 + src/python/control/magpie_control.py | 60 +++++ src/python/control/robot_variant.py | 29 +++ src/python/control/unitree_action.py | 12 +- src/python/sensors/magpie_bridge.py | 174 +++++++++++++ src/python/sensors/magpie_launch.sh | 19 ++ src/python/sensors/magpie_ros.py | 255 ++++++++++++++++++++ tasks/common_config/magpie_spawn.py | 54 +++++ tasks/common_config/robot_variant_config.py | 51 ++++ 10 files changed, 669 insertions(+), 5 deletions(-) create mode 100644 src/python/control/magpie_control.py create mode 100644 src/python/control/robot_variant.py create mode 100644 src/python/sensors/magpie_bridge.py create mode 100644 src/python/sensors/magpie_launch.sh create mode 100644 src/python/sensors/magpie_ros.py create mode 100644 tasks/common_config/magpie_spawn.py create mode 100644 tasks/common_config/robot_variant_config.py diff --git a/sim_main.py b/sim_main.py index 55488c7f..fdf7dd07 100644 --- a/sim_main.py +++ b/sim_main.py @@ -16,6 +16,8 @@ def main(): parser.add_argument('--robot_type', choices=['h1_2'], default='h1_2') parser.add_argument('--action_source', choices=['dds'], default='dds') parser.add_argument('--enable_inspire_dds', action='store_true') + parser.add_argument('--hand_type', choices=['magpie', 'inspire'], default='magpie') + parser.add_argument('--fix_base', action='store_true', help='Fix the root for bench tests; Magpie normally floats') parser.add_argument('--step_hz', type=float, default=None, help='Wall-clock control frequency; default matches the environment step time') parser.add_argument('--physics_dt', type=float, default=None) @@ -29,6 +31,9 @@ def main(): help='Exit after this many environment steps (0 runs until stopped)') AppLauncher.add_app_launcher_args(parser) args = parser.parse_args() + if args.enable_inspire_dds: + args.hand_type = 'inspire' + args.enable_inspire_dds = args.hand_type == 'inspire' if args.physics_dt is not None and not 0 < args.physics_dt < float('inf'): parser.error('--physics_dt must be finite and positive') if args.step_hz is not None and not 0 < args.step_hz < float('inf'): @@ -61,6 +66,8 @@ def report_exception(exc_type, exc, tb): from src.python.sensors.ros_sensor_bridge import RosSensorBridge cfg = parse_env_cfg(args.task, device=args.device, num_envs=1) + from tasks.common_config.robot_variant_config import configure_robot_variant + configure_robot_variant(cfg, args.hand_type, args.fix_base) cfg.seed = args.seed if args.physics_dt is not None: cfg.sim.dt = args.physics_dt @@ -84,6 +91,12 @@ def report_exception(exc_type, exc, tb): env.reset() sensors = RosSensorBridge(env.scene) resources.callback(sensors.close) + if args.hand_type == 'magpie': + from src.python.sensors.magpie_bridge import MagpieBridge + from src.python.control.magpie_control import MagpieController + magpie_bridge = MagpieBridge() + resources.callback(magpie_bridge.close) + env.magpie_controller = MagpieController(env.scene['robot'], magpie_bridge) dds = create_dds_objects(args, env) resources.callback(dds.stop_all_communication) controller = RobotController(env, ControlConfig(step_hz=args.step_hz or 1 / env.step_dt)) @@ -102,7 +115,10 @@ def stop(signum, frame): with torch.inference_mode(): while app.is_running() and controller.is_running: controller.step() - sensors.publish(env._sim_step_counter * env.physics_dt) + sim_time = env._sim_step_counter * env.physics_dt + sensors.publish(sim_time) + if args.hand_type == 'magpie': + env.magpie_controller.publish(sim_time) if env.sim.is_stopped() or (args.max_steps and controller.step_count >= args.max_steps): break print(f'[isaac] completed {controller.step_count} steps', flush=True) diff --git a/src/python/control/action_provider_dds.py b/src/python/control/action_provider_dds.py index c60aa0ec..3587a885 100644 --- a/src/python/control/action_provider_dds.py +++ b/src/python/control/action_provider_dds.py @@ -58,6 +58,8 @@ def get_action(self, env): positions = torch.tensor(hand['positions'], dtype=torch.float32, device=env.device) if torch.isfinite(positions).all(): self._action[0, BODY_ACTION_DIM:].copy_(positions) + if getattr(env, "magpie_controller", None) is not None: + self._action[0, BODY_ACTION_DIM:].copy_(env.magpie_controller.get_joint_targets(env.step_dt)) return self._action def cleanup(self): diff --git a/src/python/control/magpie_control.py b/src/python/control/magpie_control.py new file mode 100644 index 00000000..1f2241af --- /dev/null +++ b/src/python/control/magpie_control.py @@ -0,0 +1,60 @@ +"""Apply Magpie commands on the physics thread and report measured joint state.""" +import torch +from .robot_variant import robot_variant + +OPEN_MM = 110. +CLOSED_RAD = 2.05 +LEVER_M = .05 + + +class MagpieController: + def __init__(self, robot, bridge): + self.robot, self.bridge = robot, bridge + self.ids = [robot.data.joint_names.index(n) for n in robot_variant().hand_joints] + self.targets = robot.data.joint_pos[0, self.ids].clone() + self.pattern = torch.tensor([1., -1., 1., -1., 1., -1.], device=robot.device) + self._force = None + self._pending = None + + def get_joint_targets(self, dt): + commands = self.bridge.get_commands() + forces = [] + for index, side in enumerate(('left', 'right')): + command = commands[side] + target = (OPEN_MM - command['aperture']) / OPEN_MM * CLOSED_RAD + section = slice(index * 6, (index + 1) * 6) + max_step = 3.14 * command['speed'] * dt + desired = self.pattern * target + self.targets[section] += (desired - self.targets[section]).clamp(-max_step, max_step) + forces += [min(10., command['force'] * LEVER_M), 0., 0.] * 2 + force_tuple = tuple(forces) + if force_tuple != self._force: + limits = torch.tensor([forces], device=self.robot.device) + self.robot.write_joint_effort_limit_to_sim(limits, joint_ids=self.ids) + for actuator in self.robot.actuators.values(): + actuator.effort_limit.copy_(self.robot.data.joint_effort_limits[:, actuator.joint_indices]) + self._force = force_tuple + self._pending = commands + return self.targets + + def publish(self, sim_time): + q = self.robot.data.joint_pos[0, self.ids].detach().cpu().numpy() + dq = self.robot.data.joint_vel[0, self.ids].detach().cpu().numpy() + effort = self.robot.data.applied_torque[0, self.ids].detach().cpu().numpy() + target = self.targets.detach().cpu().numpy() + states = {} + for offset, side in ((0, 'left'), (6, 'right')): + left, right = offset, offset + 3 + def aperture(angle): + return max(0., min(OPEN_MM, OPEN_MM * (1. - float(angle) / CLOSED_RAD))) + fingers = [aperture(-q[right]), aperture(q[left])] + moving = max(abs(float(dq[left])), abs(float(dq[right]))) > .02 + force = (abs(float(effort[left])) + abs(float(effort[right]))) / (2. * LEVER_M) + error = max(abs(float(target[left] - q[left])), abs(float(target[right] - q[right]))) + states[side] = dict(position=sum(fingers)/2., finger_positions=fingers, + force=force, is_moving=moving, + contact_detected=not moving and error > .05 and force > .5) + self.bridge.publish(sim_time, states) + if self._pending is not None: + self.bridge.acknowledge(self._pending) + self._pending = None diff --git a/src/python/control/robot_variant.py b/src/python/control/robot_variant.py new file mode 100644 index 00000000..1686a008 --- /dev/null +++ b/src/python/control/robot_variant.py @@ -0,0 +1,29 @@ +"""Robot asset and attachment names shared by configuration and validation.""" +from dataclasses import dataclass + + +@dataclass(frozen=True) +class RobotVariant: + usd_path: str + hand_joints: tuple[str, ...] + head_link: str + lidar_link: str + wrist_links: tuple[str, str] + inspire: bool + + +def robot_variant(name='magpie'): + if name == 'magpie': + return RobotVariant( + 'isaac_assets/robots/h1_2_magpie/h1_2_magpie.usd', + tuple(f'{side}_{finger}_hinge_{i}' for side in ('lg', 'rg') + for finger in ('left', 'right') for i in (1, 2, 3)), + 'head_camera_link', 'livox_link', ('lg_base_top', 'rg_base_top'), False) + if name == 'inspire': + return RobotVariant( + 'isaac_assets/robots/h1_2-26dof-inspire-base-fix-usd/h1_2_26dof_with_inspire_rev_1_0.usd', + tuple(f'{side}_{finger}_joint' for side in ('R', 'L') for finger in ( + 'pinky_proximal', 'ring_proximal', 'middle_proximal', 'index_proximal', + 'thumb_proximal_pitch', 'thumb_proximal_yaw')), + 'camera_link', 'lidar_link', ('L_hand_base_link', 'R_hand_base_link'), True) + raise ValueError(f'Unknown robot variant: {name}') diff --git a/src/python/control/unitree_action.py b/src/python/control/unitree_action.py index af070fa5..51ef0892 100644 --- a/src/python/control/unitree_action.py +++ b/src/python/control/unitree_action.py @@ -3,6 +3,7 @@ from isaaclab.managers import ActionTerm, ActionTermCfg from isaaclab.utils import configclass from .motor_contract import BODY_JOINT_NAMES, HAND_JOINT_NAMES, ACTION_DIM, BODY_ACTION_DIM +from .robot_variant import robot_variant class UnitreeMotorAction(ActionTerm): @@ -11,9 +12,10 @@ def __init__(self, cfg, env): super().__init__(cfg, env) names = self._asset.data.joint_names self._body_ids = [names.index(name) for name in BODY_JOINT_NAMES] - self._hand_ids = [names.index(name) for name in HAND_JOINT_NAMES] + variant = robot_variant(cfg.hand_type) + self._hand_ids = [names.index(name) for name in variant.hand_joints] self._coupled_ids, sources, scales = [], [], [] - for side, offset in (('R', 0), ('L', 6)): + for side, offset in ((('R', 0), ('L', 6)) if variant.inspire else ()): for name, source, scale in (('pinky_intermediate',0,1.), ('ring_intermediate',1,1.), ('middle_intermediate',2,1.), ('index_intermediate',3,1.), ('thumb_intermediate',4,1.5), ('thumb_distal',4,2.4)): @@ -67,8 +69,9 @@ def apply_actions(self): self._asset.set_joint_effort_target(effort, joint_ids=self._body_ids) hands = self._raw_actions[:, BODY_ACTION_DIM:] self._asset.set_joint_position_target(hands, joint_ids=self._hand_ids) - self._asset.set_joint_position_target(hands[:, self._hand_sources] * self._hand_scales, - joint_ids=self._coupled_ids) + if self._coupled_ids: + self._asset.set_joint_position_target(hands[:, self._hand_sources] * self._hand_scales, + joint_ids=self._coupled_ids) def reset(self, env_ids=None): ids = slice(None) if env_ids is None else env_ids @@ -81,3 +84,4 @@ def reset(self, env_ids=None): class UnitreeMotorActionCfg(ActionTermCfg): class_type: type = UnitreeMotorAction asset_name: str = 'robot' + hand_type: str = 'inspire' diff --git a/src/python/sensors/magpie_bridge.py b/src/python/sensors/magpie_bridge.py new file mode 100644 index 00000000..ddd045d5 --- /dev/null +++ b/src/python/sensors/magpie_bridge.py @@ -0,0 +1,174 @@ +"""Private IPC between the physics thread and the native ROS gripper process.""" +import copy +import json +import math +import os +from pathlib import Path +import socket +import subprocess +import tempfile +import threading +import time + + +class MagpieCommandBuffer: + """Validated targets; only the physics thread acknowledges application.""" + def __init__(self): + self.lock = threading.RLock() + self.commands = {side: dict(aperture=110., force=5., speed=1., sequence=0) for side in ('left', 'right')} + self.applied = dict(left=0, right=0) + + def submit(self, side, values): + if side not in self.commands: + raise ValueError('side must be left or right') + if not values or set(values) - {'aperture', 'force', 'speed'}: + raise ValueError('expected aperture, force, or speed') + limits = {'aperture': (0., 110.), 'force': (0., 100.), 'speed': (0., 1.)} + normalized = {} + for key, value in values.items(): + value = float(value) + low, high = limits[key] + if not math.isfinite(value) or not low <= value <= high or (key == 'speed' and value == 0): + raise ValueError(f'{key} outside supported range {limits[key]}') + normalized[key] = value + with self.lock: + self.commands[side].update(normalized) + self.commands[side]['sequence'] += 1 + return self.commands[side]['sequence'] + + def get_commands(self): + with self.lock: + return copy.deepcopy(self.commands) + + def acknowledge(self, commands): + with self.lock: + for side, values in commands.items(): + sequence = int(values['sequence']) + if sequence <= self.commands[side]['sequence']: + self.applied[side] = max(self.applied[side], sequence) + + def is_applied(self, side, sequence): + with self.lock: + return self.applied[side] == sequence + + +class MagpieBridge: + """ROS callbacks cannot touch tensors; all physics access stays in main. + + get_commands/acknowledge are called around applying physical actuator + targets. publish takes measured state dictionaries after a physics step. + """ + def __init__(self): + domain = int(os.environ.get('ROS_DOMAIN_ID', '1')) + if not 1 <= domain <= 232: + raise ValueError('Magpie simulator requires ROS_DOMAIN_ID 1..232') + self.buffer = MagpieCommandBuffer() + self._lock = threading.RLock() + self._send_lock = threading.Lock() + self._pending = {} + self._states = {} + self._last_publish = -math.inf + self._closed = threading.Event() + self._ready = threading.Event() + self._connection = None + self._process = None + self._directory = tempfile.TemporaryDirectory(prefix='golem-magpie-') + self._listener = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) + endpoint = str(Path(self._directory.name) / 'physics.sock') + self._listener.bind(endpoint) + self._listener.listen(1) + self._listener.settimeout(.2) + self._thread = threading.Thread(target=self._receive, name='magpie-physics-ipc', daemon=True) + self._thread.start() + try: + launcher = Path(__file__).with_name('magpie_launch.sh') + self._process = subprocess.Popen(['bash', str(launcher), endpoint], start_new_session=True) + deadline = time.monotonic() + 180. + while not self._ready.wait(.1): + if self._process.poll() is not None: + raise RuntimeError(f'Magpie ROS process exited with {self._process.returncode}') + if time.monotonic() >= deadline: + raise TimeoutError('Magpie ROS process did not become ready in 180 seconds') + except BaseException: + self.close() + raise + + def _send(self, value): + payload = (json.dumps(value, allow_nan=False) + '\n').encode() + with self._send_lock: + if self._connection is not None: + self._connection.sendall(payload) + + def _receive(self): + try: + while not self._closed.is_set(): + try: + connection, _ = self._listener.accept() + break + except socket.timeout: + continue + else: + return + self._connection = connection + with connection.makefile('r') as stream: + for line in stream: + value = json.loads(line) + if value.get('type') == 'ready': + self._ready.set() + continue + request_id = value['id'] + try: + sequence = self.buffer.submit(value['side'], value['values']) + with self._lock: + self._pending[request_id] = (value['side'], sequence) + except (ValueError, KeyError, TypeError) as error: + self._send(dict(type='ack', id=request_id, success=False, message=str(error))) + except (OSError, ValueError): + if not self._closed.is_set(): + self._closed.set() + + def get_commands(self): + if self._process is not None and self._process.poll() is not None: + raise RuntimeError('Magpie ROS process stopped unexpectedly') + return self.buffer.get_commands() + + def acknowledge(self, commands): + self.buffer.acknowledge(commands) + with self._lock: + ready = [(key, side, seq) for key, (side, seq) in self._pending.items() if self.buffer.applied[side] >= seq] + for key, side, sequence in ready: + self._pending.pop(key) + state = self._states.get(side) + applied = self.buffer.is_applied(side, sequence) + self._send(dict(type='ack', id=key, success=state is not None and applied, + message=('Actuator target applied' if state is not None else 'Measured state not available') if applied else 'Target superseded before application', + state=state)) + + def publish(self, sim_time, states): + # Only finite JSON-compatible measured values cross the process boundary. + snapshot = json.loads(json.dumps(states, allow_nan=False)) + with self._lock: + self._states = snapshot + if sim_time < self._last_publish or sim_time - self._last_publish >= .1 - 1e-9: + self._send(dict(type='state', sim_time=float(sim_time), states=snapshot)) + self._last_publish = sim_time + + def close(self): + self._closed.set() + if self._connection is not None: + try: + self._connection.shutdown(socket.SHUT_RDWR) + except OSError: + pass + self._connection.close() + self._listener.close() + if self._process is not None and self._process.poll() is None: + import signal + os.killpg(self._process.pid, signal.SIGTERM) + try: + self._process.wait(timeout=5) + except subprocess.TimeoutExpired: + os.killpg(self._process.pid, signal.SIGKILL) + self._process.wait(timeout=5) + self._thread.join(timeout=2) + self._directory.cleanup() diff --git a/src/python/sensors/magpie_launch.sh b/src/python/sensors/magpie_launch.sh new file mode 100644 index 00000000..6d19e56e --- /dev/null +++ b/src/python/sensors/magpie_launch.sh @@ -0,0 +1,19 @@ +#!/usr/bin/env bash +# The native Humble process shares the ROS graph schema used by the robot stack. +set -eo pipefail +unset PYTHONPATH LD_LIBRARY_PATH LD_PRELOAD PYTHONHOME AMENT_PREFIX_PATH CMAKE_PREFIX_PATH COLCON_PREFIX_PATH ROS_DISTRO ROS_VERSION ROS_PYTHON_VERSION CYCLONEDDS_HOME +export PATH=/usr/bin:/bin:/usr/sbin:/sbin +export AMENT_PYTHON_EXECUTABLE=/usr/bin/python3 +export COLCON_PYTHON_EXECUTABLE=/usr/bin/python3 +export LD_LIBRARY_PATH=/opt/golem-humble-python/lib +source /opt/ros/humble/setup.bash +source /opt/golem-magpie-humble/setup.bash +export PYTHONHOME=/opt/golem-humble-python +export PYTHONPATH=/opt/golem-humble-python/lib/python3.10/site-packages:${PYTHONPATH:-} +export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +# Optional script override also lets CPU transport tests use exactly this runtime. +if [ "${1:-}" = --script ]; then + shift + exec /opt/golem-humble-python/bin/python3.10 "$@" +fi +exec /opt/golem-humble-python/bin/python3.10 "$(dirname "$0")/magpie_ros.py" "$1" diff --git a/src/python/sensors/magpie_ros.py b/src/python/sensors/magpie_ros.py new file mode 100644 index 00000000..69d2194a --- /dev/null +++ b/src/python/sensors/magpie_ros.py @@ -0,0 +1,255 @@ +"""Native ROS process for Magpie services/actions; physics lives in Isaac.""" +import json +import math +import socket +import sys +import threading +import time +import uuid + +import rclpy +from rclpy.action import ActionServer, CancelResponse, GoalResponse +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node +from std_srvs.srv import Trigger +from magpie_msgs.action import DeliGrasp +from magpie_msgs.msg import GripperState +from magpie_msgs.srv import SetGripperForce, SetGripperPosition + + +class PhysicsClient: + def __init__(self, endpoint): + self.connection = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) + self.connection.connect(endpoint) + self.lock = threading.Condition() + self.send_lock = threading.Lock() + self.states = {} + self.sim_time = 0. + self.received_at = -math.inf + self.responses = {} + self.closed = False + self.thread = threading.Thread(target=self._receive, daemon=True) + self.thread.start() + + def send(self, value): + with self.send_lock: + self.connection.sendall((json.dumps(value, allow_nan=False) + '\n').encode()) + + def _receive(self): + try: + with self.connection.makefile('r') as stream: + for line in stream: + value = json.loads(line) + with self.lock: + if value['type'] == 'state': + self.states = value['states'] + self.sim_time = value['sim_time'] + self.received_at = time.monotonic() + else: + self.responses[value['id']] = value + self.lock.notify_all() + finally: + with self.lock: + self.closed = True + self.lock.notify_all() + + def snapshot(self, side): + with self.lock: + if self.closed or time.monotonic() - self.received_at > 5 or side not in self.states: + raise RuntimeError('Fresh measured gripper state unavailable') + return dict(self.states[side]) + + def command(self, side, values): + self.snapshot(side) + request_id = uuid.uuid4().hex + self.send(dict(id=request_id, side=side, values=values)) + deadline = time.monotonic() + 10 + with self.lock: + while request_id not in self.responses: + remaining = deadline - time.monotonic() + if remaining <= 0 or self.closed: + raise RuntimeError('Physics did not acknowledge actuator command') + self.lock.wait(remaining) + response = self.responses.pop(request_id) + if not response['success']: + raise RuntimeError(response['message']) + return response['state'] + + def close(self): + self.connection.shutdown(socket.SHUT_RDWR) + self.connection.close() + self.thread.join(timeout=2) + + +class GripperNode(Node): + def __init__(self, side, physics): + super().__init__(f'isaac_magpie_{side}') + self.side, self.physics = side, physics + self.operations = threading.Lock() + self.goal_lock = threading.Lock() + self.goal_active = False + self.group = ReentrantCallbackGroup() + root = f'/{side}/gripper' + self.publisher = self.create_publisher(GripperState, root + '/state', 10) + self.create_timer(.1, self.publish_state, callback_group=self.group) + self._service_handles = [] + for name, message_type, callback in ( + ('open', Trigger, self.open), ('close', Trigger, self.close_gripper), + ('set_position', SetGripperPosition, self.set_position), + ('set_force', SetGripperForce, self.set_force), + ('calibrate', Trigger, self.calibrate), ('reset_parameters', Trigger, self.reset_parameters), + ): + self._service_handles.append(self.create_service(message_type, root + '/' + name, callback, callback_group=self.group)) + self.action = ActionServer(self, DeliGrasp, root + '/deligrasp', execute_callback=self.execute, + goal_callback=self.accept_goal, cancel_callback=lambda _: CancelResponse.ACCEPT, + callback_group=self.group) + + def publish_state(self): + try: + state = self.physics.snapshot(self.side) + except RuntimeError: + return + msg = GripperState() + msg.position, msg.force = float(state['position']), float(state['force']) + msg.temperature = 25. # No thermal model in the simulator. + msg.is_moving, msg.contact_detected = bool(state['is_moving']), bool(state['contact_detected']) + msg.finger_positions = list(map(float, state['finger_positions'])) + self.publisher.publish(msg) + + def apply(self, response, values, message='Actuator target applied'): + try: + with self.operations: + with self.goal_lock: + if self.goal_active: + raise RuntimeError('DeliGrasp is active; cancel it before a service command') + state = self.physics.command(self.side, values) + response.success, response.message = True, message + if hasattr(response, 'actual_position'): + response.actual_position = float(state['position']) + except (RuntimeError, ValueError, OSError) as error: + response.success, response.message = False, str(error) + return response + + def open(self, request, response): + return self.apply(response, {'aperture': 110.}) + + def close_gripper(self, request, response): + return self.apply(response, {'aperture': 0.}) + + def set_position(self, request, response): + return self.apply(response, {'aperture': request.position, 'speed': request.speed}) + + def set_force(self, request, response): + return self.apply(response, {'force': request.max_force}) + + def calibrate(self, request, response): + return self.apply(response, {'aperture': 110.}, 'Open target applied; aperture calibration comes from asset geometry') + + def reset_parameters(self, request, response): + return self.apply(response, {'force': 5., 'speed': 1.}, 'Default force and speed applied') + + def accept_goal(self, goal): + p = goal.params + valid = all(math.isfinite(v) for v in (p.goal_aperture, p.initial_force, p.additional_closure, p.additional_force)) + valid &= 0 <= p.goal_aperture <= 110 and 0 <= p.initial_force <= 100 + valid &= 0 <= p.additional_closure <= 110 and 0 <= p.additional_force <= 100 + try: + self.physics.snapshot(self.side) + except RuntimeError: + return GoalResponse.REJECT + with self.goal_lock: + if not valid or self.goal_active: + return GoalResponse.REJECT + self.goal_active = True + return GoalResponse.ACCEPT + + def execute(self, goal): + result = DeliGrasp.Result() + force_log = [] + state = None + p = goal.request.params + try: + with self.operations: + target, force = float(p.goal_aperture), float(p.initial_force) + self.physics.command(self.side, {'aperture': target, 'force': force}) + deadline = time.monotonic() + 30 + while True: + state = self.physics.snapshot(self.side) + if goal.is_cancel_requested: + self.physics.command(self.side, {'aperture': state['position']}) + goal.canceled() + result.message = 'Cancelled; holding measured aperture' + return self.finish(result, state, force_log) + self.feedback(goal, state, 'approach') + if abs(state['position'] - target) <= 2.: + break + if time.monotonic() >= deadline: + raise RuntimeError('Measured aperture did not reach the approach target') + time.sleep(.05) + if not p.complete_grasp: + result.success, result.message = True, 'Approach aperture reached; force closure not requested' + goal.succeed() + return self.finish(result, state, force_log) + for iteration in range(8): + state = self.physics.snapshot(self.side) + if goal.is_cancel_requested: + self.physics.command(self.side, {'aperture': state['position']}) + goal.canceled() + result.message = 'Cancelled; holding measured aperture' + return self.finish(result, state, force_log) + force_log.append(float(state['force'])) + self.feedback(goal, state, 'initial_close' if iteration == 0 else 'additional_close') + if state['contact_detected'] and state['force'] >= force: + result.success, result.message = True, 'Measured contact force reached' + goal.succeed() + return self.finish(result, state, force_log) + target = max(0., target - float(p.additional_closure)) + force = min(100., force + float(p.additional_force)) + self.physics.command(self.side, {'aperture': target, 'force': force}) + time.sleep(.2) + raise RuntimeError('Contact force not reached within eight closure steps') + except (RuntimeError, ValueError, OSError) as error: + result.success, result.message = False, str(error) + goal.abort() + return self.finish(result, state, force_log) + finally: + with self.goal_lock: + self.goal_active = False + + @staticmethod + def finish(result, state, force_log): + if state is not None: + result.final_aperture, result.final_force = float(state['position']), float(state['force']) + result.force_log = force_log + return result + + @staticmethod + def feedback(goal, state, phase): + msg = DeliGrasp.Feedback() + msg.current_aperture, msg.current_force = float(state['position']), float(state['force']) + msg.phase = phase + goal.publish_feedback(msg) + + +def main(): + rclpy.init() + physics = PhysicsClient(sys.argv[1]) + nodes = [GripperNode(side, physics) for side in ('left', 'right')] + executor = MultiThreadedExecutor(num_threads=8) + for node in nodes: + executor.add_node(node) + physics.send(dict(type='ready')) + try: + executor.spin() + finally: + executor.shutdown() + for node in nodes: + node.action.destroy() + node.destroy_node() + physics.close() + rclpy.try_shutdown() + + +if __name__ == '__main__': + main() diff --git a/tasks/common_config/magpie_spawn.py b/tasks/common_config/magpie_spawn.py new file mode 100644 index 00000000..f5e47f98 --- /dev/null +++ b/tasks/common_config/magpie_spawn.py @@ -0,0 +1,54 @@ +"""Author Magpie's parallelogram linkages in the spawned stage, not the asset. + +PhysxMimicJointAPI enforces q + gearing*q_reference + offset = 0 and applies +reaction impulses to both joints. The source parallelogram geometry therefore +requires gearing +1 on hinge_2 and -1 on hinge_3, both referencing hinge_1. +""" +import math + + +def add_magpie_couplings(root): + """Constrain the eight passive hinges before articulation initialization.""" + from pxr import PhysxSchema, Usd, UsdPhysics + + joints = {prim.GetName(): prim for prim in Usd.PrimRange(root) + if prim.IsA(UsdPhysics.RevoluteJoint)} + required = [f'{hand}_{finger}_hinge_{i}' for hand in ('lg', 'rg') + for finger in ('left', 'right') for i in (1, 2, 3)] + missing = set(required) - joints.keys() + if missing: + raise ValueError(f'Magpie USD is missing linkage joints: {sorted(missing)}') + for hand in ('lg', 'rg'): + for finger in ('left', 'right'): + prefix = f'{hand}_{finger}_hinge_' + reference = UsdPhysics.RevoluteJoint(joints[prefix + '1']) + lower = reference.GetLowerLimitAttr().Get() + upper = reference.GetUpperLimitAttr().Get() + if not (math.isfinite(lower) and math.isfinite(upper) and lower < upper): + raise ValueError(f'Magpie motor {prefix}1 needs finite joint limits') + for suffix, gearing in (('2', 1.), ('3', -1.)): + follower = UsdPhysics.RevoluteJoint(joints[prefix + suffix]) + # The schema requires limited revolute joints. USD angles use + # degrees; transforming the reference bounds preserves units. + bounds = sorted((-gearing * lower, -gearing * upper)) + follower.CreateLowerLimitAttr(bounds[0]) + follower.CreateUpperLimitAttr(bounds[1]) + mimic = PhysxSchema.PhysxMimicJointAPI.Apply(follower.GetPrim(), 'rotX') + mimic.CreateReferenceJointRel().SetTargets([reference.GetPath()]) + mimic.CreateReferenceJointAxisAttr('rotX') + mimic.CreateGearingAttr(gearing) + mimic.CreateOffsetAttr(0.) + + +def spawn_magpie(prim_path, cfg, translation=None, orientation=None, **kwargs): + """Use Isaac's USD spawner, then couple joints before cloning environments.""" + from isaaclab.sim.spawners.from_files import spawn_from_usd + from isaaclab.sim.utils import clone + + @clone + def spawn_one(path, config, translation=None, orientation=None, **options): + prim = spawn_from_usd(path, config, translation, orientation, **options) + add_magpie_couplings(prim) + return prim + + return spawn_one(prim_path, cfg, translation, orientation, **kwargs) diff --git a/tasks/common_config/robot_variant_config.py b/tasks/common_config/robot_variant_config.py new file mode 100644 index 00000000..7e358aad --- /dev/null +++ b/tasks/common_config/robot_variant_config.py @@ -0,0 +1,51 @@ +"""Select the published robot USD and its matching sensors and actuators.""" +from pathlib import Path +import os +from isaaclab.actuators import ImplicitActuatorCfg +from src.python.control.motor_contract import BODY_JOINT_NAMES +from src.python.control.robot_variant import robot_variant + + +def configure_robot_variant(cfg, name='magpie', fix_base=False): + variant = robot_variant(name) + assets = Path(os.environ.get('GOLEM_ASSETS_ROOT', Path(__file__).resolve().parents[3] / 'CL_Assets')) + usd_path = assets / variant.usd_path + if not usd_path.is_file(): + raise FileNotFoundError(f'Robot USD is missing: {usd_path}; initialize CL_Assets and Git LFS') + robot = cfg.scene.robot + robot.spawn.usd_path = str(usd_path) + if fix_base: + robot.spawn.articulation_props.fix_root_link = True + cfg.actions.motor.hand_type = name + if variant.inspire: + return + robot.init_state.joint_pos = { + name: value for name, value in robot.init_state.joint_pos.items() if name in BODY_JOINT_NAMES + } + robot.init_state.joint_pos.update({name: 0. for name in variant.hand_joints}) + # Body limits come from the new USD instead of the Inspire bench fixture. + for actuator in robot.actuators.values(): + actuator.effort_limit_sim = actuator.effort_limit = None + actuator.velocity_limit_sim = actuator.velocity_limit = None + from .magpie_spawn import spawn_magpie + robot.spawn.func = spawn_magpie + driven = [joint for joint in variant.hand_joints if joint.endswith('_1')] + passive = [joint for joint in variant.hand_joints if not joint.endswith('_1')] + robot.actuators['hands'] = ImplicitActuatorCfg( + joint_names_expr=driven, effort_limit_sim=10., + velocity_limit_sim=3.14, stiffness=10., damping=1.) + robot.actuators['hand_linkage'] = ImplicitActuatorCfg( + joint_names_expr=passive, effort_limit_sim=0., + velocity_limit_sim=3.14, stiffness=0., damping=0.) + cfg.observations.policy.robot_inspire_state = None + root = '/World/envs/env_.*/Robot' + cfg.scene.front_camera.prim_path = f'{root}/{variant.head_link}/front_cam' + cfg.scene.front_camera.offset.rot = (1., 0., 0., 0.) + for sensor, link in zip((cfg.scene.left_wrist_camera, cfg.scene.right_wrist_camera), variant.wrist_links): + sensor.prim_path = f'{root}/{link}/wrist_camera' + # Match the RoboCasa Magpie mount camera: +Z viewing direction. + sensor.offset.pos = (0., 0., .045) + sensor.offset.rot = (0., 0., 0., 1.) + sensor.offset.convention = 'ros' + cfg.scene.imu.prim_path = f'{root}/{variant.lidar_link}' + cfg.scene.lidar.prim_path = f'{root}/{variant.lidar_link}' From 94b992fcbb18abc98556079c6953629e63088618 Mon Sep 17 00:00:00 2001 From: maxconway Date: Wed, 23 Sep 2026 12:32:12 -0600 Subject: [PATCH 3/7] Restore root and joint state on environment reset EventCfg only cleared the IMU history on reset, so env.reset() kept whatever physical state the articulation reached during initialization: the first recorded state could already be displaced and tilted ~40 degrees. Reset the scene to its default root/joint state before clearing the IMU cache. Verified in the GOLEM stack tests: both Isaac cases now start upright at the default pose (z=0.995-1.0 m) instead of a scrambled pre-reset state. Co-Authored-By: Claude Opus 5.5 (1M context) --- .../stack_rgyblock_h12_27dof_inspire_joint_env_cfg.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/tasks/h1-2_tasks/stack_rgyblock_h12_27dof_inspire/stack_rgyblock_h12_27dof_inspire_joint_env_cfg.py b/tasks/h1-2_tasks/stack_rgyblock_h12_27dof_inspire/stack_rgyblock_h12_27dof_inspire_joint_env_cfg.py index 5030163f..8ef7985b 100644 --- a/tasks/h1-2_tasks/stack_rgyblock_h12_27dof_inspire/stack_rgyblock_h12_27dof_inspire_joint_env_cfg.py +++ b/tasks/h1-2_tasks/stack_rgyblock_h12_27dof_inspire/stack_rgyblock_h12_27dof_inspire_joint_env_cfg.py @@ -121,6 +121,8 @@ class RewardsCfg: @configclass class EventCfg: + # Restore root pose/velocity and joint state after initialization and resets. + reset_scene = EventTermCfg(func=base_mdp.reset_scene_to_default, mode="reset") reset_imu = EventTermCfg(func=reset_robot_imu_cache, mode="reset") #reset_red_block = EventTermCfg( # func=mdp.reset_root_state_uniform, # use uniform distribution reset function From 6f89c647cd1b3613b0718ca6e1ec334d260e3ac5 Mon Sep 17 00:00:00 2001 From: humanoid desktop Date: Mon, 19 Oct 2026 15:47:12 -0600 Subject: [PATCH 4/7] Magpie IMU on the MID-360's chip frame, livox_link/livox_imu_link CL_Assets carries the MID-360 IMU chip as livox_imu_link: Livox's offset plus the real robot's measured tilt, as a child of the point frame livox_link. The IMU sensor reads that prim; Isaac Lab's Imu composes it with its rigid ancestor. /livox/imu therefore has the real lever arm and tilt, and the lidar stays on livox_link. Inspire keeps both on lidar_link. Co-Authored-By: Claude Opus 5.5 (1M context) --- src/python/control/robot_variant.py | 5 +++-- tasks/common_config/robot_variant_config.py | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/python/control/robot_variant.py b/src/python/control/robot_variant.py index 1686a008..bf01df94 100644 --- a/src/python/control/robot_variant.py +++ b/src/python/control/robot_variant.py @@ -8,6 +8,7 @@ class RobotVariant: hand_joints: tuple[str, ...] head_link: str lidar_link: str + imu_link: str # the lidar's IMU chip: /livox/imu wrist_links: tuple[str, str] inspire: bool @@ -18,12 +19,12 @@ def robot_variant(name='magpie'): 'isaac_assets/robots/h1_2_magpie/h1_2_magpie.usd', tuple(f'{side}_{finger}_hinge_{i}' for side in ('lg', 'rg') for finger in ('left', 'right') for i in (1, 2, 3)), - 'head_camera_link', 'livox_link', ('lg_base_top', 'rg_base_top'), False) + 'head_camera_link', 'livox_link', 'livox_link/livox_imu_link', ('lg_base_top', 'rg_base_top'), False) if name == 'inspire': return RobotVariant( 'isaac_assets/robots/h1_2-26dof-inspire-base-fix-usd/h1_2_26dof_with_inspire_rev_1_0.usd', tuple(f'{side}_{finger}_joint' for side in ('R', 'L') for finger in ( 'pinky_proximal', 'ring_proximal', 'middle_proximal', 'index_proximal', 'thumb_proximal_pitch', 'thumb_proximal_yaw')), - 'camera_link', 'lidar_link', ('L_hand_base_link', 'R_hand_base_link'), True) + 'camera_link', 'lidar_link', 'lidar_link', ('L_hand_base_link', 'R_hand_base_link'), True) raise ValueError(f'Unknown robot variant: {name}') diff --git a/tasks/common_config/robot_variant_config.py b/tasks/common_config/robot_variant_config.py index 7e358aad..0a74b2e0 100644 --- a/tasks/common_config/robot_variant_config.py +++ b/tasks/common_config/robot_variant_config.py @@ -47,5 +47,5 @@ def configure_robot_variant(cfg, name='magpie', fix_base=False): sensor.offset.pos = (0., 0., .045) sensor.offset.rot = (0., 0., 0., 1.) sensor.offset.convention = 'ros' - cfg.scene.imu.prim_path = f'{root}/{variant.lidar_link}' + cfg.scene.imu.prim_path = f'{root}/{variant.imu_link}' cfg.scene.lidar.prim_path = f'{root}/{variant.lidar_link}' From 739599845d16af6cbf2be3744c4dfb3a33bb1e65 Mon Sep 17 00:00:00 2001 From: maxconway Date: Wed, 28 Oct 2026 01:24:29 -0600 Subject: [PATCH 5/7] H1-2 rt/lowstate reports mode_machine 6, like the real robot H12RobotDDS leaves LowState_.mode_machine at the IDL default 0, while the real H1-2 reports 6 in every sample. golem_safety checks each /lowstate against the robot's mode_machine, so the sim must report the same value. Co-Authored-By: Claude Opus 5.5 (1M context) Claude-Session: https://claude.ai/code/session_01MezUeauFS9tKMgZKJxo1HC --- src/python/dds/specialized/h12_robot_dds.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/python/dds/specialized/h12_robot_dds.py b/src/python/dds/specialized/h12_robot_dds.py index 156d2285..7212bf30 100644 --- a/src/python/dds/specialized/h12_robot_dds.py +++ b/src/python/dds/specialized/h12_robot_dds.py @@ -27,6 +27,7 @@ def __init__(self,node_name:str="h1_2"): self.node_name = node_name self.crc = CRC() self.low_state = unitree_hg_msg_dds__LowState_() + self.low_state.mode_machine = 6 # what the real H1-2 reports; the IDL default is 0 self._initialized = True self._command_sequence = 0 From a4cc11bd99203b3ba38335ccfc19ab3f8ca8d627 Mon Sep 17 00:00:00 2001 From: maxconway Date: Sun, 15 Nov 2026 15:15:14 -0700 Subject: [PATCH 6/7] The H1-2 sim serves the estop box on a local socket src/python/sensors/estop_server.py serves the real box's lines (ESTOP,BOOT, then ESTOP,0|1 at 50 Hz) on 127.0.0.1:<15700 + ROS_DOMAIN_ID>; ENTER on the sim's terminal toggles the button. GOLEM's H1-2 bringups point estop_node there, so golem_safety runs the real robot's estop path in the sim. Co-Authored-By: Claude Opus 5.5 (1M context) Claude-Session: https://claude.ai/code/session_01MezUeauFS9tKMgZKJxo1HC --- sim_main.py | 4 + src/python/sensors/estop_server.py | 158 +++++++++++++++++++++++++++++ 2 files changed, 162 insertions(+) create mode 100644 src/python/sensors/estop_server.py diff --git a/sim_main.py b/sim_main.py index fdf7dd07..8ced516e 100644 --- a/sim_main.py +++ b/sim_main.py @@ -99,6 +99,10 @@ def report_exception(exc_type, exc, tb): env.magpie_controller = MagpieController(env.scene['robot'], magpie_bridge) dds = create_dds_objects(args, env) resources.callback(dds.stop_all_communication) + # The hardware estop box golem_bringup's estop_node reads (ENTER toggles it). + from src.python.sensors.estop_server import start_estop_server + estop = start_estop_server('isaac') + resources.callback(estop.shutdown) controller = RobotController(env, ControlConfig(step_hz=args.step_hz or 1 / env.step_dt)) resources.callback(controller.cleanup) controller.set_action_provider(create_action_provider(env, args)) diff --git a/src/python/sensors/estop_server.py b/src/python/sensors/estop_server.py new file mode 100644 index 00000000..b84a69ea --- /dev/null +++ b/src/python/sensors/estop_server.py @@ -0,0 +1,158 @@ +"""Serve the H1-2's hardware estop box over TCP, for the estop package's estop_node. + +The real box is an Arduino on USB serial (core_ws/src/estop/arduino/estop_serial.ino) +that prints "ESTOP,BOOT" once, then "ESTOP,0" (clear) or "ESTOP,1" (triggered) on every +loop. This server speaks the same lines to every client on 127.0.0.1:, which +estop_node opens as serial_port socket://127.0.0.1: (pyserial's serial_for_url), +so the sim bringup runs the real estop_node and golem_safety's estop path unchanged. +The RoboCasa sim serves the same box (GOLEM's golem_robocasa/estop_server.py); keep the +two and golem_bringup.sim_estop on one BASE_PORT. + +The port is estop_port() of the sim's ROS_DOMAIN_ID, so a sim on another domain never +answers this domain's estop_node. The ENTER key on the sim's terminal toggles the +button between clear and triggered. + +SAFETY: golem_safety latches its estop until restart on a triggered sample, on an +unplugged box and on a box silent for its estop.timeout_seconds; the server streams on +the wall clock from its own thread, so a stalled physics loop still reads as a live box, +and a stopped sim reads as an unplugged one. +""" +import os +import select +import socket +import sys +import threading + +# First port of the estop servers; the sim on ROS domain d listens on BASE_PORT + d. +BASE_PORT = 15700 +# Hz of lines sent to each client: estop_node treats a box silent for its +# line_stale_timeout_sec (0.1 s) as stale. +LINE_HZ = 50.0 + + +def estop_port(domain_id: str | None = None) -> int: + """Return the estop TCP port of a ROS domain (ROS_DOMAIN_ID when domain_id is None).""" + raw = os.environ.get("ROS_DOMAIN_ID", "") if domain_id is None else domain_id + return BASE_PORT + int(raw.strip() or "1") + + +class EstopServer: + """Stream the simulated estop box's lines to every connected estop_node.""" + + def __init__(self, port: int, tag: str): + """Bind the port; raise OSError when another process holds it. + + tag prefixes the log lines (e.g. "h12_mujoco"). + """ + self._tag = tag + self._lock = threading.Lock() + self._triggered = False + self._clients: list[socket.socket] = [] + self._stop = threading.Event() + self._listener = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self._listener.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + self._listener.bind(("127.0.0.1", port)) + self._listener.listen(4) + self._listener.settimeout(0.1) + self.port = port + self._threads = [ + threading.Thread(target=self._accept_loop, daemon=True, name="estop_accept"), + threading.Thread(target=self._send_loop, daemon=True, name="estop_send"), + ] + if sys.stdin is not None and sys.stdin.isatty(): + self._threads.append( + threading.Thread(target=self._stdin_loop, daemon=True, name="estop_stdin")) + + def start(self) -> None: + """Start serving, and reading ENTER from the terminal when it is one.""" + for thread in self._threads: + thread.start() + keys = "ENTER on this terminal toggles it" if len(self._threads) == 3 else "no terminal" + print(f"[{self._tag}] estop box on socket://127.0.0.1:{self.port}: clear; {keys}", + flush=True) + + @property + def triggered(self) -> bool: + """Return whether the button is pressed.""" + with self._lock: + return self._triggered + + def toggle(self) -> None: + """Press the button if it is clear, release it if it is pressed.""" + with self._lock: + self._triggered = not self._triggered + state = self._triggered + print(f"[{self._tag}] estop {'TRIGGERED' if state else 'released'}", flush=True) + + def shutdown(self) -> None: + """Stop serving and close every connection, which estop_node reads as unplugged.""" + self._stop.set() + for thread in self._threads: + if thread.name != "estop_stdin": + thread.join(timeout=1.0) + with self._lock: + clients, self._clients = self._clients, [] + for client in clients: + client.close() + self._listener.close() + + def _accept_loop(self) -> None: + """Accept estop_node's connections; each gets ESTOP,BOOT first, as at power-up.""" + while not self._stop.is_set(): + try: + client, _ = self._listener.accept() + except (socket.timeout, OSError): + continue + client.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) + try: + client.sendall(b"ESTOP,BOOT\r\n") + except OSError: + client.close() + continue + with self._lock: + self._clients.append(client) + + def _send_loop(self) -> None: + """Send the button's state to every client at LINE_HZ, on the wall clock.""" + period = 1.0 / LINE_HZ + while not self._stop.wait(period): + with self._lock: + line = b"ESTOP,1\r\n" if self._triggered else b"ESTOP,0\r\n" + clients = list(self._clients) + dead = [] + for client in clients: + try: + client.sendall(line) + except OSError: + dead.append(client) + if dead: + with self._lock: + self._clients = [c for c in self._clients if c not in dead] + for client in dead: + client.close() + + def _stdin_loop(self) -> None: + """Toggle the button on each line read from the terminal.""" + while not self._stop.is_set(): + ready, _, _ = select.select([sys.stdin], [], [], 0.2) + if not ready: + continue + if not sys.stdin.readline(): + return + self.toggle() + + +def start_estop_server(tag: str) -> EstopServer: + """Start the estop server on this domain's port; exit the sim when the port is taken.""" + port = estop_port() + try: + server = EstopServer(port, tag) + except OSError as exc: + raise SystemExit( + f"[{tag}] cannot serve the estop on 127.0.0.1:{port} ({exc}): another sim " + f"on ROS_DOMAIN_ID={os.environ.get('ROS_DOMAIN_ID', '')!r} holds it") from exc + server.start() + return server + + +__all__ = ["BASE_PORT", "EstopServer", "estop_port", "start_estop_server"] From 78c3f70ee28cb60bc46ab5dfe9d507dfe70b7143 Mon Sep 17 00:00:00 2001 From: max Date: Thu, 19 Nov 2026 20:13:17 -0700 Subject: [PATCH 7/7] Add an opt-in elastic band with /elastic_band/toggle (--elastic_band) --elastic_band hangs the floating H1-2 from the RoboCasa H1-2 sim's band: a spring-damper (1e4 N/m, 2e3 N*s/m) from torso_link to an anchor 4 cm below its spawn. UnitreeMotorAction applies it before every physics step, and /elastic_band/toggle (std_srvs/srv/Trigger), served on the sensor bridge's node and handled between steps, releases it or enables it again. It is refused with --fix_base; without the flag the robot floats free as before. The force is rotated into the torso's current frame before it is stored: Isaac Lab's wrench composer rotates a world-frame force with the orientation it cached at its first call, which sends a fallen robot flying when the band is re-enabled. Checked in the Isaac sim by GOLEM's tests/isaaclab/run_elastic_band.sh: held within 6 cm of the anchor for 40 s, released, and re-held from the fallen pose (4/4). Co-Authored-By: Claude Fable 5.2 Claude-Session: https://claude.ai/code/session_015EG7p9fHRyH9uB5cMqdm8P --- sim_main.py | 18 +++++ src/python/control/elastic_band.py | 90 ++++++++++++++++++++++ src/python/control/unitree_action.py | 5 ++ src/python/sensors/elastic_band_service.py | 40 ++++++++++ 4 files changed, 153 insertions(+) create mode 100644 src/python/control/elastic_band.py create mode 100644 src/python/sensors/elastic_band_service.py diff --git a/sim_main.py b/sim_main.py index 8ced516e..35555ed5 100644 --- a/sim_main.py +++ b/sim_main.py @@ -18,6 +18,9 @@ def main(): parser.add_argument('--enable_inspire_dds', action='store_true') parser.add_argument('--hand_type', choices=['magpie', 'inspire'], default='magpie') parser.add_argument('--fix_base', action='store_true', help='Fix the root for bench tests; Magpie normally floats') + parser.add_argument('--elastic_band', action='store_true', + help='Hang the floating robot from an elastic band on its torso until ' + '/elastic_band/toggle releases it, as the RoboCasa sims do; not with --fix_base') parser.add_argument('--step_hz', type=float, default=None, help='Wall-clock control frequency; default matches the environment step time') parser.add_argument('--physics_dt', type=float, default=None) @@ -34,6 +37,8 @@ def main(): if args.enable_inspire_dds: args.hand_type = 'inspire' args.enable_inspire_dds = args.hand_type == 'inspire' + if args.elastic_band and args.fix_base: + parser.error('--elastic_band holds a floating robot; it cannot be combined with --fix_base') if args.physics_dt is not None and not 0 < args.physics_dt < float('inf'): parser.error('--physics_dt must be finite and positive') if args.step_hz is not None and not 0 < args.step_hz < float('inf'): @@ -97,6 +102,17 @@ def report_exception(exc_type, exc, tb): magpie_bridge = MagpieBridge() resources.callback(magpie_bridge.close) env.magpie_controller = MagpieController(env.scene['robot'], magpie_bridge) + band_service = None + if args.elastic_band: + # The band holds the torso from the first physics step; UnitreeMotorAction applies it. + from src.python.control.elastic_band import ElasticBand + from src.python.sensors.elastic_band_service import ElasticBandService + env.elastic_band = ElasticBand(env.scene['robot']) + band_service = ElasticBandService(sensors, env.elastic_band) + resources.callback(band_service.close) + print(f'[isaac] elastic band on {env.elastic_band.body} anchored at ' + f'{[round(v, 3) for v in env.elastic_band.anchor[0].tolist()]}; ' + '/elastic_band/toggle releases it', flush=True) dds = create_dds_objects(args, env) resources.callback(dds.stop_all_communication) # The hardware estop box golem_bringup's estop_node reads (ENTER toggles it). @@ -123,6 +139,8 @@ def stop(signum, frame): sensors.publish(sim_time) if args.hand_type == 'magpie': env.magpie_controller.publish(sim_time) + if band_service is not None: + band_service.spin() if env.sim.is_stopped() or (args.max_steps and controller.step_count >= args.max_steps): break print(f'[isaac] completed {controller.step_count} steps', flush=True) diff --git a/src/python/control/elastic_band.py b/src/python/control/elastic_band.py new file mode 100644 index 00000000..14fd9d26 --- /dev/null +++ b/src/python/control/elastic_band.py @@ -0,0 +1,90 @@ +"""Model the elastic band the Isaac sim can hang the H1-2 from until a policy drives it. + +The band is the RoboCasa sims' (golem_robocasa/mujoco_env.py): a spring-damper pulling one +body (the torso) toward a fixed anchor near its spawn. sim_main.py makes one with +--elastic_band, UnitreeMotorAction.apply_actions applies it before every physics step, and +/elastic_band/toggle (sensors/elastic_band_service.py) releases it. This module imports only +torch, so the CPU tests run it without Kit. +""" +import torch + +# The RoboCasa H1-2 sim's band (h12_robot.make_band), so both sims hold the robot alike. +BAND_BODY = 'torso_link' +BAND_DROP = 0.04 # m, the anchor below the torso's spawn: the feet carry part of the weight +BAND_STIFFNESS = 1e4 # N/m +BAND_DAMPING = 2e3 # N*s/m, on the body's whole linear velocity + + +def band_force(position, velocity, anchor, stiffness, damping, length=0.0): + """Return the band's world-frame force (N) on a body, per row of (n, 3) tensors. + + position (m) and velocity (m/s) are the body's, anchor (m) the band's fixed end, all in + the world frame. The spring pulls toward the anchor beyond its rest length (m); the + damping acts on the whole velocity, since a zero-length spring pulls in every direction. + """ + offset = position - anchor + distance = offset.norm(dim=-1, keepdim=True) + direction = offset / distance.clamp_min(1e-9) + return -stiffness * (distance - length) * direction - damping * velocity + + +def world_to_body(quaternion, vector): + """Return world-frame vectors (n, 3) in the frames of bodies with world orientations quaternion (n, 4, WXYZ).""" + w, xyz = quaternion[:, :1], quaternion[:, 1:] + cross = torch.linalg.cross(xyz, vector) + return vector - 2. * w * cross + 2. * torch.linalg.cross(xyz, cross) + + +class ElasticBand: + """Hold one body of the robot on a spring-damper, and toggle it.""" + + def __init__(self, robot, body=BAND_BODY, drop=BAND_DROP, stiffness=BAND_STIFFNESS, + damping=BAND_DAMPING): + """Anchor the band drop m below body's current position, enabled. + + robot is the Isaac Lab articulation; call this once the scene is reset, with the robot + at its spawn. Raises ValueError, listing the robot's bodies, when it has no such body. + """ + names = list(robot.data.body_names) + if body not in names: + raise ValueError(f'the elastic band needs body {body!r}; the robot has {names}') + self._robot = robot + self._body_ids = torch.tensor([names.index(body)], device=robot.device) + self.body = body + self.stiffness = stiffness + self.damping = damping + self.anchor = robot.data.body_pos_w[:, self._body_ids[0]].clone() # m, world, (num_envs, 3) + self.anchor[:, 2] -= drop + self.enabled = True + self._applied = False # a nonzero force is stored in the robot's wrench buffer + + def toggle(self) -> bool: + """Enable or disable the band, log it, and return whether it is now enabled.""" + self.enabled = not self.enabled + print(f"[isaac] elastic band {'enabled' if self.enabled else 'disabled'}", flush=True) + return self.enabled + + def apply(self): + """Store the band's force on its body for the next physics step. + + Its pull while enabled; on the first call after a release, zero, since Isaac Lab keeps + a stored wrench until it is overwritten. Call it before every physics step. + """ + if self.enabled: + body = self._body_ids[0] + data = self._robot.data + force = band_force(data.body_pos_w[:, body], data.body_lin_vel_w[:, body], + self.anchor, self.stiffness, self.damping) + # Into the body's frame, at its orientation now: the wrench composer rotates a + # world-frame force with the orientation it cached at its first call, which is + # wrong once the torso has turned. + force = world_to_body(data.body_quat_w[:, body], force) + elif self._applied: + force = torch.zeros_like(self.anchor) + else: + return + self._applied = self.enabled + # Body-frame force on the body, no torque; (num_envs, 1 body, 3). + self._robot.permanent_wrench_composer.set_forces_and_torques( + forces=force.unsqueeze(1), torques=torch.zeros_like(force).unsqueeze(1), + body_ids=self._body_ids, is_global=False) diff --git a/src/python/control/unitree_action.py b/src/python/control/unitree_action.py index 51ef0892..9c59ae87 100644 --- a/src/python/control/unitree_action.py +++ b/src/python/control/unitree_action.py @@ -62,6 +62,11 @@ def process_actions(self, actions): actuator.damping.copy_(self._asset.data.joint_damping[:, actuator.joint_indices]) def apply_actions(self): + # Runs before every physics step, so the elastic band (sim_main.py --elastic_band) + # follows the torso at the physics rate. + band = getattr(self._env, 'elastic_band', None) + if band is not None: + band.apply() command = self._raw_actions[:, :BODY_ACTION_DIM].reshape(-1, 6, len(BODY_JOINT_NAMES)) self._asset.set_joint_position_target(command[:, 0], joint_ids=self._body_ids) self._asset.set_joint_velocity_target(command[:, 1], joint_ids=self._body_ids) diff --git a/src/python/sensors/elastic_band_service.py b/src/python/sensors/elastic_band_service.py new file mode 100644 index 00000000..3623217c --- /dev/null +++ b/src/python/sensors/elastic_band_service.py @@ -0,0 +1,40 @@ +"""Serve /elastic_band/toggle for the Isaac sim's elastic band, as the RoboCasa sims do.""" + + +class ElasticBandService: + """Answer /elastic_band/toggle (std_srvs/srv/Trigger) on the sensor bridge's ROS node. + + The requests are handled in spin(), which the sim's main loop calls between physics + steps: the band is toggled on the physics thread, never beside a step. + """ + + def __init__(self, bridge, band): + """Create the service on bridge's node (a RosSensorBridge) for band (an ElasticBand). + + Raises RuntimeError when the bridge has no ROS node. + """ + if bridge.node is None: + raise RuntimeError('the elastic band toggle needs the sensor bridge\'s ROS node') + from rclpy.executors import SingleThreadedExecutor + from std_srvs.srv import Trigger + self._band = band + self._node = bridge.node + self._executor = SingleThreadedExecutor(context=bridge.context) + self._executor.add_node(self._node) + self._service = self._node.create_service(Trigger, '/elastic_band/toggle', self._on_toggle) + + def _on_toggle(self, request, response): + """Toggle the band and answer with its new state, as RoboCasa's MeasurementBridge does.""" + response.success = True + response.message = 'enabled' if self._band.toggle() else 'disabled' + return response + + def spin(self): + """Handle the requests that have arrived, without waiting for more.""" + self._executor.spin_once(timeout_sec=0) + + def close(self): + """Remove the service and the executor; call it before the bridge closes its node.""" + self._executor.remove_node(self._node) + self._node.destroy_service(self._service) + self._executor.shutdown()