diff --git a/example/python/exported/policy.onnx b/example/python/exported/policy.onnx new file mode 100644 index 00000000..5b3e980a Binary files /dev/null and b/example/python/exported/policy.onnx differ diff --git a/example/python/exported/policy.pt b/example/python/exported/policy.pt new file mode 100644 index 00000000..f5a64c71 Binary files /dev/null and b/example/python/exported/policy.pt differ diff --git a/example/python/exported/read.py b/example/python/exported/read.py new file mode 100644 index 00000000..2b41db55 --- /dev/null +++ b/example/python/exported/read.py @@ -0,0 +1,7 @@ +import onnx + +# ONNX 모델 열기 +model = onnx.load("policy.onnx") + +# 모델 구조 출력 +print(onnx.helper.printable_graph(model.graph)) diff --git a/example/python/params/agent.pkl b/example/python/params/agent.pkl new file mode 100644 index 00000000..5779ff8d Binary files /dev/null and b/example/python/params/agent.pkl differ diff --git a/example/python/params/agent.yaml b/example/python/params/agent.yaml new file mode 100644 index 00000000..ab764dc8 --- /dev/null +++ b/example/python/params/agent.yaml @@ -0,0 +1,49 @@ +seed: 42 +device: cuda:0 +num_steps_per_env: 24 +max_iterations: 300 +empirical_normalization: null +obs_groups: {} +clip_actions: null +save_interval: 50 +experiment_name: unitree_go2_flat +run_name: '' +logger: tensorboard +neptune_project: isaaclab +wandb_project: isaaclab +resume: false +load_run: .* +load_checkpoint: model_.*.pt +class_name: OnPolicyRunner +policy: + class_name: ActorCritic + init_noise_std: 1.0 + noise_std_type: scalar + actor_obs_normalization: false + critic_obs_normalization: false + actor_hidden_dims: + - 128 + - 128 + - 128 + critic_hidden_dims: + - 128 + - 128 + - 128 + activation: elu +algorithm: + class_name: PPO + num_learning_epochs: 5 + num_mini_batches: 4 + learning_rate: 0.001 + schedule: adaptive + gamma: 0.99 + lam: 0.95 + entropy_coef: 0.01 + desired_kl: 0.01 + max_grad_norm: 1.0 + value_loss_coef: 1.0 + use_clipped_value_loss: true + clip_param: 0.2 + normalize_advantage_per_mini_batch: false + rnd_cfg: null + symmetry_cfg: null diff --git a/example/python/params/env.pkl b/example/python/params/env.pkl new file mode 100644 index 00000000..f577c6c0 Binary files /dev/null and b/example/python/params/env.pkl differ diff --git a/example/python/params/env.yaml b/example/python/params/env.yaml new file mode 100644 index 00000000..d7006eca --- /dev/null +++ b/example/python/params/env.yaml @@ -0,0 +1,796 @@ +viewer: + eye: !!python/tuple + - 7.5 + - 7.5 + - 7.5 + lookat: !!python/tuple + - 0.0 + - 0.0 + - 0.0 + cam_prim_path: /OmniverseKit_Persp + resolution: !!python/tuple + - 1280 + - 720 + origin_type: world + env_index: 0 + asset_name: null + body_name: null +sim: + physics_prim_path: /physicsScene + device: cuda:0 + dt: 0.005 + render_interval: 4 + gravity: !!python/tuple + - 0.0 + - 0.0 + - -9.81 + enable_scene_query_support: false + use_fabric: true + physx: + solver_type: 1 + min_position_iteration_count: 1 + max_position_iteration_count: 255 + min_velocity_iteration_count: 0 + max_velocity_iteration_count: 255 + enable_ccd: false + enable_stabilization: false + enable_enhanced_determinism: false + bounce_threshold_velocity: 0.5 + friction_offset_threshold: 0.04 + friction_correlation_distance: 0.025 + gpu_max_rigid_contact_count: 8388608 + gpu_max_rigid_patch_count: 327680 + gpu_found_lost_pairs_capacity: 2097152 + gpu_found_lost_aggregate_pairs_capacity: 33554432 + gpu_total_aggregate_pairs_capacity: 2097152 + gpu_collision_stack_size: 67108864 + gpu_heap_capacity: 67108864 + gpu_temp_buffer_capacity: 16777216 + gpu_max_num_partitions: 8 + gpu_max_soft_body_contacts: 1048576 + gpu_max_particle_contacts: 1048576 + physics_material: + func: isaaclab.sim.spawners.materials.physics_materials:spawn_rigid_body_material + static_friction: 1.0 + dynamic_friction: 1.0 + restitution: 0.0 + friction_combine_mode: multiply + restitution_combine_mode: multiply + compliant_contact_stiffness: 0.0 + compliant_contact_damping: 0.0 + render: + enable_translucency: null + enable_reflections: null + enable_global_illumination: null + antialiasing_mode: null + enable_dlssg: null + enable_dl_denoiser: null + dlss_mode: null + enable_direct_lighting: null + samples_per_pixel: null + enable_shadows: null + enable_ambient_occlusion: null + carb_settings: null + rendering_mode: null + create_stage_in_memory: false +ui_window_class_type: isaaclab.envs.ui.manager_based_rl_env_window:ManagerBasedRLEnvWindow +seed: 42 +decimation: 4 +scene: + num_envs: 4096 + env_spacing: 2.5 + lazy_sensor_update: true + replicate_physics: true + filter_collisions: true + clone_in_fabric: false + robot: + class_type: isaaclab.assets.articulation.articulation:Articulation + prim_path: /World/envs/env_.*/Robot + spawn: + func: isaaclab.sim.spawners.from_files.from_files:spawn_from_usd + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + deformable_props: null + rigid_props: + rigid_body_enabled: null + kinematic_enabled: null + disable_gravity: false + linear_damping: 0.0 + angular_damping: 0.0 + max_linear_velocity: 1000.0 + max_angular_velocity: 1000.0 + max_depenetration_velocity: 1.0 + max_contact_impulse: null + enable_gyroscopic_forces: null + retain_accelerations: false + solver_position_iteration_count: null + solver_velocity_iteration_count: null + sleep_threshold: null + stabilization_threshold: null + collision_props: null + activate_contact_sensors: true + scale: null + articulation_props: + articulation_enabled: null + enabled_self_collisions: false + solver_position_iteration_count: 4 + solver_velocity_iteration_count: 0 + sleep_threshold: null + stabilization_threshold: null + fix_root_link: null + fixed_tendons_props: null + spatial_tendons_props: null + joint_drive_props: null + visual_material_path: material + visual_material: null + usd_path: https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.0/Isaac/IsaacLab/Robots/Unitree/Go2/go2.usd + variants: null + init_state: + pos: !!python/tuple + - 0.0 + - 0.0 + - 0.4 + rot: !!python/tuple + - 1.0 + - 0.0 + - 0.0 + - 0.0 + lin_vel: !!python/tuple + - 0.0 + - 0.0 + - 0.0 + ang_vel: !!python/tuple + - 0.0 + - 0.0 + - 0.0 + joint_pos: + .*L_hip_joint: 0.1 + .*R_hip_joint: -0.1 + F[L,R]_thigh_joint: 0.8 + R[L,R]_thigh_joint: 1.0 + .*_calf_joint: -1.5 + joint_vel: + .*: 0.0 + collision_group: 0 + debug_vis: false + articulation_root_prim_path: null + soft_joint_pos_limit_factor: 0.9 + actuators: + base_legs: + class_type: isaaclab.actuators.actuator_pd:DCMotor + joint_names_expr: + - .*_hip_joint + - .*_thigh_joint + - .*_calf_joint + effort_limit: 23.5 + velocity_limit: 30.0 + effort_limit_sim: null + velocity_limit_sim: null + stiffness: 25.0 + damping: 0.5 + armature: null + friction: 0.0 + dynamic_friction: null + viscous_friction: null + saturation_effort: 23.5 + actuator_value_resolution_debug_print: false + terrain: + class_type: isaaclab.terrains.terrain_importer:TerrainImporter + collision_group: -1 + prim_path: /World/ground + num_envs: 4096 + terrain_type: plane + terrain_generator: null + usd_path: null + env_spacing: 2.5 + visual_material: + func: isaaclab.sim.spawners.materials.visual_materials:spawn_from_mdl_file + mdl_path: https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.0/Isaac/IsaacLab/Materials/TilesMarbleSpiderWhiteBrickBondHoned/TilesMarbleSpiderWhiteBrickBondHoned.mdl + project_uvw: true + albedo_brightness: null + texture_scale: !!python/tuple + - 0.25 + - 0.25 + physics_material: + func: isaaclab.sim.spawners.materials.physics_materials:spawn_rigid_body_material + static_friction: 1.0 + dynamic_friction: 1.0 + restitution: 0.0 + friction_combine_mode: multiply + restitution_combine_mode: multiply + compliant_contact_stiffness: 0.0 + compliant_contact_damping: 0.0 + max_init_terrain_level: 5 + debug_vis: false + height_scanner: null + contact_forces: + class_type: isaaclab.sensors.contact_sensor.contact_sensor:ContactSensor + prim_path: /World/envs/env_.*/Robot/.* + update_period: 0.005 + history_length: 3 + debug_vis: false + track_pose: false + track_contact_points: false + max_contact_data_count_per_prim: 4 + track_air_time: true + force_threshold: 1.0 + filter_prim_paths_expr: [] + visualizer_cfg: + prim_path: /Visuals/ContactSensor + markers: + contact: + func: isaaclab.sim.spawners.shapes.shapes:spawn_sphere + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + rigid_props: null + collision_props: null + activate_contact_sensors: false + visual_material_path: material + visual_material: + func: isaaclab.sim.spawners.materials.visual_materials:spawn_preview_surface + diffuse_color: !!python/tuple + - 1.0 + - 0.0 + - 0.0 + emissive_color: !!python/tuple + - 0.0 + - 0.0 + - 0.0 + roughness: 0.5 + metallic: 0.0 + opacity: 1.0 + physics_material_path: material + physics_material: null + radius: 0.02 + no_contact: + func: isaaclab.sim.spawners.shapes.shapes:spawn_sphere + visible: false + semantic_tags: null + copy_from_source: true + mass_props: null + rigid_props: null + collision_props: null + activate_contact_sensors: false + visual_material_path: material + visual_material: + func: isaaclab.sim.spawners.materials.visual_materials:spawn_preview_surface + diffuse_color: !!python/tuple + - 0.0 + - 1.0 + - 0.0 + emissive_color: !!python/tuple + - 0.0 + - 0.0 + - 0.0 + roughness: 0.5 + metallic: 0.0 + opacity: 1.0 + physics_material_path: material + physics_material: null + radius: 0.02 + sky_light: + class_type: null + prim_path: /World/skyLight + spawn: + func: isaaclab.sim.spawners.lights.lights:spawn_light + visible: true + semantic_tags: null + copy_from_source: true + prim_type: DomeLight + color: !!python/tuple + - 1.0 + - 1.0 + - 1.0 + enable_color_temperature: false + color_temperature: 6500.0 + normalize: false + exposure: 0.0 + intensity: 750.0 + texture_file: https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.0/Isaac/Materials/Textures/Skies/PolyHaven/kloofendal_43d_clear_puresky_4k.hdr + texture_format: automatic + visible_in_primary_ray: true + init_state: + pos: !!python/tuple + - 0.0 + - 0.0 + - 0.0 + rot: !!python/tuple + - 1.0 + - 0.0 + - 0.0 + - 0.0 + collision_group: 0 + debug_vis: false +recorders: + dataset_file_handler_class_type: isaaclab.utils.datasets.hdf5_dataset_file_handler:HDF5DatasetFileHandler + dataset_export_dir_path: /tmp/isaaclab/logs + dataset_filename: dataset + dataset_export_mode: + _value_: 1 + _name_: EXPORT_ALL + _sort_order_: 1 + export_in_record_pre_reset: true +observations: + policy: + concatenate_terms: true + concatenate_dim: -1 + enable_corruption: true + history_length: null + flatten_history_dim: true + base_lin_vel: + func: isaaclab.envs.mdp.observations:base_lin_vel + params: {} + modifiers: null + noise: + func: isaaclab.utils.noise.noise_model:uniform_noise + operation: add + n_min: -0.1 + n_max: 0.1 + clip: null + scale: null + history_length: 0 + flatten_history_dim: true + base_ang_vel: + func: isaaclab.envs.mdp.observations:base_ang_vel + params: {} + modifiers: null + noise: + func: isaaclab.utils.noise.noise_model:uniform_noise + operation: add + n_min: -0.2 + n_max: 0.2 + clip: null + scale: null + history_length: 0 + flatten_history_dim: true + projected_gravity: + func: isaaclab.envs.mdp.observations:projected_gravity + params: {} + modifiers: null + noise: + func: isaaclab.utils.noise.noise_model:uniform_noise + operation: add + n_min: -0.05 + n_max: 0.05 + clip: null + scale: null + history_length: 0 + flatten_history_dim: true + velocity_commands: + func: isaaclab.envs.mdp.observations:generated_commands + params: + command_name: base_velocity + modifiers: null + noise: null + clip: null + scale: null + history_length: 0 + flatten_history_dim: true + joint_pos: + func: isaaclab.envs.mdp.observations:joint_pos_rel + params: {} + modifiers: null + noise: + func: isaaclab.utils.noise.noise_model:uniform_noise + operation: add + n_min: -0.01 + n_max: 0.01 + clip: null + scale: null + history_length: 0 + flatten_history_dim: true + joint_vel: + func: isaaclab.envs.mdp.observations:joint_vel_rel + params: {} + modifiers: null + noise: + func: isaaclab.utils.noise.noise_model:uniform_noise + operation: add + n_min: -1.5 + n_max: 1.5 + clip: null + scale: null + history_length: 0 + flatten_history_dim: true + actions: + func: isaaclab.envs.mdp.observations:last_action + params: {} + modifiers: null + noise: null + clip: null + scale: null + history_length: 0 + flatten_history_dim: true + height_scan: null +actions: + joint_pos: + class_type: isaaclab.envs.mdp.actions.joint_actions:JointPositionAction + asset_name: robot + debug_vis: false + clip: null + joint_names: + - .* + scale: 0.25 + offset: 0.0 + preserve_order: false + use_default_offset: true +events: + physics_material: + func: isaaclab.envs.mdp.events:randomize_rigid_body_material + params: + asset_cfg: + name: robot + joint_names: null + joint_ids: !!python/object/apply:builtins.slice + - null + - null + - null + fixed_tendon_names: null + fixed_tendon_ids: !!python/object/apply:builtins.slice + - null + - null + - null + body_names: .* + body_ids: !!python/object/apply:builtins.slice + - null + - null + - null + object_collection_names: null + object_collection_ids: !!python/object/apply:builtins.slice + - null + - null + - null + preserve_order: false + static_friction_range: !!python/tuple + - 0.8 + - 0.8 + dynamic_friction_range: !!python/tuple + - 0.6 + - 0.6 + restitution_range: !!python/tuple + - 0.0 + - 0.0 + num_buckets: 64 + mode: startup + interval_range_s: null + is_global_time: false + min_step_count_between_reset: 0 + add_base_mass: + func: isaaclab.envs.mdp.events:randomize_rigid_body_mass + params: + asset_cfg: + name: robot + joint_names: null + joint_ids: !!python/object/apply:builtins.slice + - null + - null + - null + fixed_tendon_names: null + fixed_tendon_ids: !!python/object/apply:builtins.slice + - null + - null + - null + body_names: base + body_ids: !!python/object/apply:builtins.slice + - null + - null + - null + object_collection_names: null + object_collection_ids: !!python/object/apply:builtins.slice + - null + - null + - null + preserve_order: false + mass_distribution_params: !!python/tuple + - -1.0 + - 3.0 + operation: add + mode: startup + interval_range_s: null + is_global_time: false + min_step_count_between_reset: 0 + base_com: null + base_external_force_torque: + func: isaaclab.envs.mdp.events:apply_external_force_torque + params: + asset_cfg: + name: robot + joint_names: null + joint_ids: !!python/object/apply:builtins.slice + - null + - null + - null + fixed_tendon_names: null + fixed_tendon_ids: !!python/object/apply:builtins.slice + - null + - null + - null + body_names: base + body_ids: !!python/object/apply:builtins.slice + - null + - null + - null + object_collection_names: null + object_collection_ids: !!python/object/apply:builtins.slice + - null + - null + - null + preserve_order: false + force_range: !!python/tuple + - 0.0 + - 0.0 + torque_range: !!python/tuple + - -0.0 + - 0.0 + mode: reset + interval_range_s: null + is_global_time: false + min_step_count_between_reset: 0 + reset_base: + func: isaaclab.envs.mdp.events:reset_root_state_uniform + params: + pose_range: + x: !!python/tuple + - -0.5 + - 0.5 + y: !!python/tuple + - -0.5 + - 0.5 + yaw: !!python/tuple + - -3.14 + - 3.14 + velocity_range: + x: !!python/tuple + - 0.0 + - 0.0 + y: !!python/tuple + - 0.0 + - 0.0 + z: !!python/tuple + - 0.0 + - 0.0 + roll: !!python/tuple + - 0.0 + - 0.0 + pitch: !!python/tuple + - 0.0 + - 0.0 + yaw: !!python/tuple + - 0.0 + - 0.0 + mode: reset + interval_range_s: null + is_global_time: false + min_step_count_between_reset: 0 + reset_robot_joints: + func: isaaclab.envs.mdp.events:reset_joints_by_scale + params: + position_range: !!python/tuple + - 1.0 + - 1.0 + velocity_range: !!python/tuple + - 0.0 + - 0.0 + mode: reset + interval_range_s: null + is_global_time: false + min_step_count_between_reset: 0 + push_robot: null +rerender_on_reset: false +wait_for_textures: true +xr: null +teleop_devices: + devices: {} +export_io_descriptors: false +log_dir: /home/jeonchanwook/Documents/Github/IsaacLab/logs/rsl_rl/unitree_go2_flat/2025-10-18_16-20-26 +is_finite_horizon: false +episode_length_s: 20.0 +rewards: + track_lin_vel_xy_exp: + func: isaaclab.envs.mdp.rewards:track_lin_vel_xy_exp + params: + command_name: base_velocity + std: 0.5 + weight: 1.5 + track_ang_vel_z_exp: + func: isaaclab.envs.mdp.rewards:track_ang_vel_z_exp + params: + command_name: base_velocity + std: 0.5 + weight: 0.75 + lin_vel_z_l2: + func: isaaclab.envs.mdp.rewards:lin_vel_z_l2 + params: {} + weight: -2.0 + ang_vel_xy_l2: + func: isaaclab.envs.mdp.rewards:ang_vel_xy_l2 + params: {} + weight: -0.05 + dof_torques_l2: + func: isaaclab.envs.mdp.rewards:joint_torques_l2 + params: {} + weight: -0.0002 + dof_acc_l2: + func: isaaclab.envs.mdp.rewards:joint_acc_l2 + params: {} + weight: -2.5e-07 + action_rate_l2: + func: isaaclab.envs.mdp.rewards:action_rate_l2 + params: {} + weight: -0.01 + feet_air_time: + func: isaaclab_tasks.manager_based.locomotion.velocity.mdp.rewards:feet_air_time + params: + sensor_cfg: + name: contact_forces + joint_names: null + joint_ids: !!python/object/apply:builtins.slice + - null + - null + - null + fixed_tendon_names: null + fixed_tendon_ids: !!python/object/apply:builtins.slice + - null + - null + - null + body_names: .*_foot + body_ids: !!python/object/apply:builtins.slice + - null + - null + - null + object_collection_names: null + object_collection_ids: !!python/object/apply:builtins.slice + - null + - null + - null + preserve_order: false + command_name: base_velocity + threshold: 0.5 + weight: 0.25 + undesired_contacts: null + flat_orientation_l2: + func: isaaclab.envs.mdp.rewards:flat_orientation_l2 + params: {} + weight: -2.5 + dof_pos_limits: + func: isaaclab.envs.mdp.rewards:joint_pos_limits + params: {} + weight: 0.0 +terminations: + time_out: + func: isaaclab.envs.mdp.terminations:time_out + params: {} + time_out: true + base_contact: + func: isaaclab.envs.mdp.terminations:illegal_contact + params: + sensor_cfg: + name: contact_forces + joint_names: null + joint_ids: !!python/object/apply:builtins.slice + - null + - null + - null + fixed_tendon_names: null + fixed_tendon_ids: !!python/object/apply:builtins.slice + - null + - null + - null + body_names: base + body_ids: !!python/object/apply:builtins.slice + - null + - null + - null + object_collection_names: null + object_collection_ids: !!python/object/apply:builtins.slice + - null + - null + - null + preserve_order: false + threshold: 1.0 + time_out: false +curriculum: + terrain_levels: null +commands: + base_velocity: + class_type: isaaclab.envs.mdp.commands.velocity_command:UniformVelocityCommand + resampling_time_range: !!python/tuple + - 10.0 + - 10.0 + debug_vis: true + asset_name: robot + heading_command: true + heading_control_stiffness: 0.5 + rel_standing_envs: 0.02 + rel_heading_envs: 1.0 + ranges: + lin_vel_x: !!python/tuple + - -1.0 + - 1.0 + lin_vel_y: !!python/tuple + - -1.0 + - 1.0 + ang_vel_z: !!python/tuple + - -1.0 + - 1.0 + heading: !!python/tuple + - -3.141592653589793 + - 3.141592653589793 + goal_vel_visualizer_cfg: + prim_path: /Visuals/Command/velocity_goal + markers: + arrow: + func: isaaclab.sim.spawners.from_files.from_files:spawn_from_usd + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + deformable_props: null + rigid_props: null + collision_props: null + activate_contact_sensors: false + scale: !!python/tuple + - 0.5 + - 0.5 + - 0.5 + articulation_props: null + fixed_tendons_props: null + spatial_tendons_props: null + joint_drive_props: null + visual_material_path: material + visual_material: + func: isaaclab.sim.spawners.materials.visual_materials:spawn_preview_surface + diffuse_color: !!python/tuple + - 0.0 + - 1.0 + - 0.0 + emissive_color: !!python/tuple + - 0.0 + - 0.0 + - 0.0 + roughness: 0.5 + metallic: 0.0 + opacity: 1.0 + usd_path: https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.0/Isaac/Props/UIElements/arrow_x.usd + variants: null + current_vel_visualizer_cfg: + prim_path: /Visuals/Command/velocity_current + markers: + arrow: + func: isaaclab.sim.spawners.from_files.from_files:spawn_from_usd + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + deformable_props: null + rigid_props: null + collision_props: null + activate_contact_sensors: false + scale: !!python/tuple + - 0.5 + - 0.5 + - 0.5 + articulation_props: null + fixed_tendons_props: null + spatial_tendons_props: null + joint_drive_props: null + visual_material_path: material + visual_material: + func: isaaclab.sim.spawners.materials.visual_materials:spawn_preview_surface + diffuse_color: !!python/tuple + - 0.0 + - 0.0 + - 1.0 + emissive_color: !!python/tuple + - 0.0 + - 0.0 + - 0.0 + roughness: 0.5 + metallic: 0.0 + opacity: 1.0 + usd_path: https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.0/Isaac/Props/UIElements/arrow_x.usd + variants: null diff --git a/example/python/reordering_example.py b/example/python/reordering_example.py new file mode 100644 index 00000000..eba92564 --- /dev/null +++ b/example/python/reordering_example.py @@ -0,0 +1,136 @@ +import numpy as np + +""" +Unitree Go2 로봇의 모터 인덱스 순서 변환 튜토리얼 + +Unitree SDK와 IsaacLab에서 사용하는 모터 순서가 다르기 때문에 +데이터를 주고받을 때 인덱스 변환이 필요합니다. +""" + +print("=" * 60) +print("Unitree Go2 모터 인덱스 순서 변환 튜토리얼") +print("=" * 60) + +# 1. 각 시스템의 모터 순서 정의 +print("\n1. 각 시스템의 모터 순서:") +print("-" * 30) + +# Unitree SDK/Mujoco 순서 (실제 하드웨어/시뮬레이션 순서) +unitree_order = [ + "FR_hip", "FR_thigh", "FR_calf", # 0, 1, 2 + "FL_hip", "FL_thigh", "FL_calf", # 3, 4, 5 + "RR_hip", "RR_thigh", "RR_calf", # 6, 7, 8 + "RL_hip", "RL_thigh", "RL_calf" # 9, 10, 11 +] + +# IsaacLab 순서 (강화학습 정책에서 사용하는 순서) +isaaclab_order = [ + "FL_hip", "FR_hip", "RL_hip", "RR_hip", # 0, 1, 2, 3 + "FL_thigh", "FR_thigh", "RL_thigh", "RR_thigh", # 4, 5, 6, 7 + "FL_calf", "FR_calf", "RL_calf", "RR_calf" # 8, 9, 10, 11 +] + +print(f"Unitree SDK 순서: {unitree_order}") +print(f"IsaacLab 순서: {isaaclab_order}") + +# 2. 변환 인덱스 배열 생성 +print("\n2. 변환 인덱스 배열:") +print("-" * 30) + +# Unitree -> IsaacLab 변환: IsaacLab[i] = Unitree[UNITREE_TO_ISAACLAB[i]] +UNITREE_TO_ISAACLAB = np.array([ + 3, 0, 9, 6, + 4, 1, 10, 7, + 5, 2, 11, 8 +], dtype=np.int32) + +# IsaacLab -> Unitree 변환: Unitree[i] = IsaacLab[ISAACLAB_TO_UNITREE[i]] +ISAACLAB_TO_UNITREE = np.array([ + 1, 5, 9, + 0, 4, 8, + 3, 7, 11, + 2, 6, 10 +], dtype=np.int32) + +print(f"Unitree -> IsaacLab 인덱스: {UNITREE_TO_ISAACLAB}") +print(f"IsaacLab -> Unitree 인덱스: {ISAACLAB_TO_UNITREE}") + +# 3. 텍스트 변환 예시 (튜플/리스트 사용) +print("\n3. 텍스트 변환 예시:") +print("-" * 30) + +# 튜플을 사용한 텍스트 매핑 +def convert_unitree_to_isaaclab_names(unitree_names): + """Unitree 모터 이름을 IsaacLab 순서로 변환""" + UNITREE_TO_ISAACLAB = np.array([ + 3, 0, 9, 6, + 4, 1, 10, 7, + 5, 2, 11, 8 + ], dtype=np.int32) + return [unitree_names[i] for i in UNITREE_TO_ISAACLAB] + +def convert_isaaclab_to_unitree_names(isaaclab_names): + """IsaacLab 모터 이름을 Unitree 순서로 변환""" + ISAACLAB_TO_UNITREE = np.array([ + 1, 5, 9, + 0, 4, 8, + 3, 7, 11, + 2, 6, 10 + ], dtype=np.int32) + return [isaaclab_names[i] for i in ISAACLAB_TO_UNITREE] + +# Unitree -> IsaacLab 변환 +unitree_to_isaaclab_names = convert_unitree_to_isaaclab_names(unitree_order) +print(f"Unitree 순서를 IsaacLab 순서로 변환:") +print(f" 원본: {unitree_order}") +print(f" 결과: {unitree_to_isaaclab_names}") + +# IsaacLab -> Unitree 변환 +isaaclab_to_unitree_names = convert_isaaclab_to_unitree_names(isaaclab_order) +print(f"\nIsaacLab 순서를 Unitree 순서로 변환:") +print(f" 원본: {isaaclab_order}") +print(f" 결과: {isaaclab_to_unitree_names}") + +# 4. 숫자 인덱스 매핑 예시 (1~12) +print("\n4. 숫자 인덱스 매핑 예시 (1~12):") +print("-" * 30) + +# 1부터 12까지의 숫자로 모터 인덱스 표현 +unitree_indices = list(range(1, 13)) # [1, 2, 3, ..., 12] +isaaclab_indices = list(range(1, 13)) + +print(f"Unitree 인덱스 (1~12): {unitree_indices}") +print(f"IsaacLab 인덱스 (1~12): {isaaclab_indices}") + +# Unitree -> IsaacLab 변환 +unitree_to_isaaclab_indices = [unitree_indices[i] for i in UNITREE_TO_ISAACLAB] +print(f"\nUnitree 인덱스를 IsaacLab 순서로 재배열:") +print(f" 원본: {unitree_indices}") +print(f" 결과: {unitree_to_isaaclab_indices}") + +# IsaacLab -> Unitree 변환 +isaaclab_to_unitree_indices = [isaaclab_indices[i] for i in ISAACLAB_TO_UNITREE] +print(f"\nIsaacLab 인덱스를 Unitree 순서로 재배열:") +print(f" 원본: {isaaclab_indices}") +print(f" 결과: {isaaclab_to_unitree_indices}") + +# 5. 실제 데이터 변환 예시 (numpy 사용) +print("\n5. 실제 데이터 변환 예시 (numpy 사용):") +print("-" * 30) + +# 가상의 모터 위치 데이터 (Unitree SDK에서 받은 데이터라고 가정) +unitree_positions = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0, 1.1, 1.2]) +print(f"Unitree SDK 모터 위치: {unitree_positions}") + +# IsaacLab 순서로 변환 +isaaclab_positions = unitree_positions[UNITREE_TO_ISAACLAB] +print(f"IsaacLab 순서로 변환: {isaaclab_positions}") + +# 다시 Unitree 순서로 되돌리기 +restored_positions = isaaclab_positions[ISAACLAB_TO_UNITREE] +print(f"다시 Unitree 순서로: {restored_positions}") + +# 검증 +print(f"원본과 복원 데이터 일치: {np.allclose(unitree_positions, restored_positions)}") + +print("\n" + "=" * 60) \ No newline at end of file diff --git a/example/python/ros2_policy_runner_fixed.py b/example/python/ros2_policy_runner_fixed.py new file mode 100644 index 00000000..5204f8be --- /dev/null +++ b/example/python/ros2_policy_runner_fixed.py @@ -0,0 +1,857 @@ +#!/usr/bin/env python3 +""" +IsaacLab Policy Runner with ROS2 Diagnostics (with Joint Index Reordering) +========================================================================== + +This script executes a trained IsaacLab policy on the Unitree Go2 robot using MuJoCo simulation +and publishes detailed observation data to ROS2 topics for analysis with PlotJuggler. + +Key Features: +- ONNX-based policy execution +- ROS2 publishers for all observation components +- Real-time diagnostics data streaming +- Proper observation scaling matching training conditions +- Joint index reordering between Unitree SDK/MuJoCo and IsaacLab + +Joint Index Conversion: +- Unitree/MuJoCo order: FR_hip, FR_thigh, FR_calf, FL_hip, FL_thigh, FL_calf, + RR_hip, RR_thigh, RR_calf, RL_hip, RL_thigh, RL_calf +- IsaacLab order: FL_hip, FR_hip, RL_hip, RR_hip, + FL_thigh, FR_thigh, RL_thigh, RR_thigh, + FL_calf, FR_calf, RL_calf, RR_calf + +Author: Generated for IsaacLab policy integration with ROS2 diagnostics +""" + +import time +import sys +import numpy as np +import onnxruntime as ort +from typing import Optional, Tuple + +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float32MultiArray, Float32, Header +from geometry_msgs.msg import Vector3Stamped, TwistStamped +from sensor_msgs.msg import JointState + +from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber, ChannelFactoryInitialize +from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_, unitree_go_msg_dds__LowState_ +from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_, LowState_, SportModeState_ +from unitree_sdk2py.utils.crc import CRC + +# ============================================================================= +# Joint Index Reordering +# ============================================================================= + +# Unitree SDK/MuJoCo -> IsaacLab 변환 +# IsaacLab[i] = Unitree[UNITREE_TO_ISAACLAB[i]] +UNITREE_TO_ISAACLAB = np.array([ + 3, 0, 9, 6, # FL_hip, FR_hip, RL_hip, RR_hip + 4, 1, 10, 7, # FL_thigh, FR_thigh, RL_thigh, RR_thigh + 5, 2, 11, 8 # FL_calf, FR_calf, RL_calf, RR_calf +], dtype=np.int32) + +# IsaacLab -> Unitree SDK/MuJoCo 변환 +# Unitree[i] = IsaacLab[ISAACLAB_TO_UNITREE[i]] +ISAACLAB_TO_UNITREE = np.array([ + 1, 5, 9, # FR_hip, FR_thigh, FR_calf + 0, 4, 8, # FL_hip, FL_thigh, FL_calf + 3, 7, 11, # RR_hip, RR_thigh, RR_calf + 2, 6, 10 # RL_hip, RL_thigh, RL_calf +], dtype=np.int32) + +# ============================================================================= +# Configuration Parameters +# ============================================================================= + +class PolicyConfig: + """Configuration parameters matching IsaacLab training setup""" + + CONTROL_DT = 0.02 # 50Hz control frequency + + NOISE_LEVELS = { + 'base_lin_vel': (-0.1, 0.1), + 'base_ang_vel': (-0.2, 0.2), + 'projected_gravity': (-0.05, 0.05), + 'joint_pos': (-0.01, 0.01), + 'joint_vel': (-1.5, 1.5), + } + + ACTION_SCALE = 0.25 + + # DEFAULT_JOINT_POS는 Unitree/MuJoCo 순서로 정의됨. IsaacLab 순서로 변환 필요. + DEFAULT_JOINT_POS = np.array([ + -0.1, 0.8, -1.5, # FR_hip, FR_thigh, FR_calf + 0.1, 0.8, -1.5, # FL_hip, FL_thigh, FL_calf + -0.1, 1.0, -1.5, # RR_hip, RR_thigh, RR_calf + 0.1, 1.0, -1.5, # RL_hip, RL_thigh, RL_calf + ], dtype=np.float32) + + # Unitree/MuJoCo 순서 → IsaacLab 순서로 변환 + DEFAULT_JOINT_POS = DEFAULT_JOINT_POS[UNITREE_TO_ISAACLAB] + + KP = 25.0 + KD = 0.5 + + OBS_CLIP = 100.0 + ACTION_CLIP = 23.5 # effort limit + +# ============================================================================= +# ROS2 Diagnostics Node +# ============================================================================= + +class Go2DiagnosticsPublisher(Node): + """ROS2 node for publishing Go2 diagnostics data""" + + def __init__(self): + super().__init__('go2_diagnostics_publisher') + + # Publishers for observation components + self.pub_base_lin_vel = self.create_publisher( + Vector3Stamped, '/go2/obs/base_lin_vel', 10) + self.pub_base_ang_vel = self.create_publisher( + Vector3Stamped, '/go2/obs/base_ang_vel', 10) + self.pub_projected_gravity = self.create_publisher( + Vector3Stamped, '/go2/obs/projected_gravity', 10) + self.pub_velocity_commands = self.create_publisher( + Vector3Stamped, '/go2/obs/velocity_commands', 10) + + # Joint states publisher (IsaacLab order for visualization) + self.pub_joint_states = self.create_publisher( + JointState, '/go2/obs/joint_states', 10) + + # Actions publisher + self.pub_actions = self.create_publisher( + Float32MultiArray, '/go2/actions/raw', 10) + self.pub_actions_scaled = self.create_publisher( + Float32MultiArray, '/go2/actions/scaled', 10) + self.pub_target_positions = self.create_publisher( + Float32MultiArray, '/go2/actions/target_positions', 10) + self.pub_computed_torques = self.create_publisher( + Float32MultiArray, '/go2/actions/computed_torques', 10) + + # Full observation vector + self.pub_full_obs = self.create_publisher( + Float32MultiArray, '/go2/obs/full_vector', 10) + + # Policy diagnostics + self.pub_inference_time = self.create_publisher( + Float32, '/go2/diagnostics/inference_time_ms', 10) + self.pub_loop_time = self.create_publisher( + Float32, '/go2/diagnostics/loop_time_ms', 10) + + # Heading command diagnostics + self.pub_current_heading = self.create_publisher( + Float32, '/go2/diagnostics/current_heading_rad', 10) + self.pub_target_heading = self.create_publisher( + Float32, '/go2/diagnostics/target_heading_rad', 10) + self.pub_heading_error = self.create_publisher( + Float32, '/go2/diagnostics/heading_error_rad', 10) + + # Joint names (IsaacLab order for JointState message) + self.joint_names = [ + 'FL_hip', 'FL_thigh', 'FL_calf', + 'FR_hip', 'FR_thigh', 'FR_calf', + 'RL_hip', 'RL_thigh', 'RL_calf', + 'RR_hip', 'RR_thigh', 'RR_calf' + ] + + self.get_logger().info('Go2 Diagnostics Publisher initialized') + + def publish_observations(self, obs_dict: dict, timestamp: float): + """Publish all observation components""" + + header = Header() + header.stamp = self.get_clock().now().to_msg() + header.frame_id = 'base_link' + + # Base linear velocity + msg = Vector3Stamped() + msg.header = header + msg.vector.x = float(obs_dict['base_lin_vel'][0]) + msg.vector.y = float(obs_dict['base_lin_vel'][1]) + msg.vector.z = float(obs_dict['base_lin_vel'][2]) + self.pub_base_lin_vel.publish(msg) + + # Base angular velocity + msg = Vector3Stamped() + msg.header = header + msg.vector.x = float(obs_dict['base_ang_vel'][0]) + msg.vector.y = float(obs_dict['base_ang_vel'][1]) + msg.vector.z = float(obs_dict['base_ang_vel'][2]) + self.pub_base_ang_vel.publish(msg) + + # Projected gravity + msg = Vector3Stamped() + msg.header = header + msg.vector.x = float(obs_dict['projected_gravity'][0]) + msg.vector.y = float(obs_dict['projected_gravity'][1]) + msg.vector.z = float(obs_dict['projected_gravity'][2]) + self.pub_projected_gravity.publish(msg) + + # Velocity commands + msg = Vector3Stamped() + msg.header = header + msg.vector.x = float(obs_dict['velocity_commands'][0]) + msg.vector.y = float(obs_dict['velocity_commands'][1]) + msg.vector.z = float(obs_dict['velocity_commands'][2]) + self.pub_velocity_commands.publish(msg) + + # Joint states (already in IsaacLab order from obs_dict) + joint_msg = JointState() + joint_msg.header = header + joint_msg.name = self.joint_names + joint_msg.position = [float(p) for p in obs_dict['joint_pos']] + joint_msg.velocity = [float(v) for v in obs_dict['joint_vel']] + self.pub_joint_states.publish(joint_msg) + + # Full observation vector + full_obs_msg = Float32MultiArray() + full_obs_msg.data = [float(x) for x in obs_dict['full_obs']] + self.pub_full_obs.publish(full_obs_msg) + + def publish_actions(self, actions_raw: np.ndarray, actions_scaled: np.ndarray, + target_positions: np.ndarray, computed_torques: np.ndarray = None): + """Publish action data (all in IsaacLab order for consistency with observations)""" + + # Raw actions from policy (IsaacLab order) + msg = Float32MultiArray() + msg.data = [float(x) for x in actions_raw] + self.pub_actions.publish(msg) + + # Scaled actions (IsaacLab order) + msg = Float32MultiArray() + msg.data = [float(x) for x in actions_scaled] + self.pub_actions_scaled.publish(msg) + + # Target joint positions (IsaacLab order) + msg = Float32MultiArray() + msg.data = [float(x) for x in target_positions] + self.pub_target_positions.publish(msg) + + # Computed torques (IsaacLab order) + if computed_torques is not None: + msg = Float32MultiArray() + msg.data = [float(x) for x in computed_torques] + self.pub_computed_torques.publish(msg) + + def publish_diagnostics(self, inference_time_ms: float, loop_time_ms: float, + current_heading: float = 0.0, target_heading: float = 0.0, + heading_error: float = 0.0): + """Publish timing and heading diagnostics""" + + msg = Float32() + msg.data = inference_time_ms + self.pub_inference_time.publish(msg) + + msg = Float32() + msg.data = loop_time_ms + self.pub_loop_time.publish(msg) + + # Heading diagnostics + msg = Float32() + msg.data = current_heading + self.pub_current_heading.publish(msg) + + msg = Float32() + msg.data = target_heading + self.pub_target_heading.publish(msg) + + msg = Float32() + msg.data = heading_error + self.pub_heading_error.publish(msg) + +# ============================================================================= +# Observation Processing +# ============================================================================= + +class ObservationProcessor: + """Processes raw sensor data into policy observations with joint reordering""" + + def __init__(self, config: PolicyConfig): + self.config = config + # last_actions는 IsaacLab 순서로 저장됨 + self.last_actions = np.zeros(12, dtype=np.float32) + + # Store latest velocity from SportModeState + self.latest_base_velocity = np.zeros(3, dtype=np.float32) + + # Velocity command configuration (matching IsaacLab pattern) + self.heading_command = True # 학습 시 설정과 동일하게 (absolute heading mode) + self.heading_control_stiffness = 0.5 # IsaacLab default value + + # Input velocity commands [vx, vy, yaw] - what user provides + self.velocity_commands = np.zeros(3, dtype=np.float32) # [vx, vy, yaw_target or yaw_rate] + + # Processed velocity commands [vx, vy, wz] - what robot actually uses and goes to policy + self.vel_command_b = np.zeros(3, dtype=np.float32) # processed velocity commands + + # Heading control state (for absolute heading mode) + self.heading_target = 0.0 # target heading (radians) + + def update_base_velocity(self, velocity): + """Update base velocity from SportModeState""" + self.latest_base_velocity = np.array(velocity, dtype=np.float32) + + def process(self, lowstate: LowState_, add_noise: bool = True) -> Tuple[np.ndarray, dict]: + """Process raw sensor data and return observation + components dict + + CRITICAL: + - MuJoCo의 joint data를 받아서 IsaacLab 순서로 변환 + - Policy에 IsaacLab 순서의 observation 입력 + """ + + # Use base velocity from bridge (SportModeState) + base_lin_vel = self.latest_base_velocity.copy() + + # Get angular velocity from IMU + base_ang_vel = np.array(lowstate.imu_state.gyroscope, dtype=np.float32) + + # Projected gravity and current orientation + quat = np.array(lowstate.imu_state.quaternion, dtype=np.float32) + projected_gravity = self._compute_projected_gravity(quat) + + # Update processed velocity commands based on heading mode + self._update_vel_command_b(quat) + + # ===== CRITICAL: Joint Index Reordering ===== + # MuJoCo에서 받은 joint 데이터 (Unitree 순서) + joint_pos_unitree = np.array([ms.q for ms in lowstate.motor_state[:12]], dtype=np.float32) + joint_vel_unitree = np.array([ms.dq for ms in lowstate.motor_state[:12]], dtype=np.float32) + + # Unitree 순서 -> IsaacLab 순서로 변환 + joint_pos = joint_pos_unitree[UNITREE_TO_ISAACLAB] + joint_vel = joint_vel_unitree[UNITREE_TO_ISAACLAB] + + # Relative joint positions (IsaacLab 순서) + joint_pos_rel = joint_pos - self.config.DEFAULT_JOINT_POS + + # Store clean observations before noise (모두 IsaacLab 순서) + obs_clean = { + 'base_lin_vel': base_lin_vel.copy(), + 'base_ang_vel': base_ang_vel.copy(), + 'projected_gravity': projected_gravity.copy(), + 'velocity_commands': self.vel_command_b.copy(), + 'joint_pos': joint_pos_rel.copy(), + 'joint_vel': joint_vel.copy(), + 'last_actions': self.last_actions.copy() + } + + # Add noise if enabled + if add_noise: + base_lin_vel += self._add_noise(base_lin_vel, 'base_lin_vel') + base_ang_vel += self._add_noise(base_ang_vel, 'base_ang_vel') + projected_gravity += self._add_noise(projected_gravity, 'projected_gravity') + joint_pos_rel += self._add_noise(joint_pos_rel, 'joint_pos') + joint_vel += self._add_noise(joint_vel, 'joint_vel') + + # Concatenate observation (모두 IsaacLab 순서) + obs = np.concatenate([ + base_lin_vel, + base_ang_vel, + projected_gravity, + self.vel_command_b, + joint_pos_rel, + joint_vel, + self.last_actions + ]).astype(np.float32) + + # Clip observations + obs = np.clip(obs, -self.config.OBS_CLIP, self.config.OBS_CLIP) + + # Update components dict with noisy values + obs_clean['full_obs'] = obs + + return obs, obs_clean + + def update_last_actions(self, actions: np.ndarray): + """Update the last actions (IsaacLab 순서로 저장)""" + self.last_actions = actions.copy() + + def set_velocity_commands(self, vx: float, vy: float, yaw: float): + """Set velocity commands [vx, vy, yaw] + + Args: + vx: Forward velocity command (m/s) + vy: Lateral velocity command (m/s) + yaw: Target heading (rad) if heading_command=True, + or angular velocity (rad/s) if heading_command=False + """ + # Store input velocity commands + self.velocity_commands = np.array([vx, vy, yaw], dtype=np.float32) + + # Copy linear velocities to processed commands + self.vel_command_b[0] = vx # vx always direct + self.vel_command_b[1] = vy # vy always direct + + if self.heading_command: + # Store target heading for absolute heading control + self.heading_target = yaw + # Angular velocity will be computed in _update_vel_command_b() + else: + # Direct angular velocity command (relative mode) + self.vel_command_b[2] = yaw + + def _update_vel_command_b(self, quat: np.ndarray): + """Update vel_command_b based on heading mode (matching IsaacLab pattern exactly) + + Following IsaacLab code (simplified for single environment): + if self.cfg.heading_command: + heading_error = math_utils.wrap_to_pi(self.heading_target - self.robot.data.heading_w) + self.vel_command_b[2] = torch.clip( + self.cfg.heading_control_stiffness * heading_error, + min=self.cfg.ranges.ang_vel_z[0], + max=self.cfg.ranges.ang_vel_z[1], + ) + """ + if self.heading_command: + # Compute current heading from quaternion (equivalent to self.robot.data.heading_w) + current_heading = self._compute_heading_from_quat(quat) + + # Compute heading error (target - current, wrapped to [-pi, pi]) + heading_error = self._wrap_to_pi(self.heading_target - current_heading) + + # Compute angular velocity with stiffness control and clipping + # (equivalent to IsaacLab's torch.clip with cfg.ranges.ang_vel_z) + angular_velocity = self.heading_control_stiffness * heading_error + ang_vel_limit = 1.0 # equivalent to cfg.ranges.ang_vel_z + self.vel_command_b[2] = np.clip(angular_velocity, -ang_vel_limit, ang_vel_limit) + + # If heading_command=False (relative mode), vel_command_b[2] is already set + # directly in set_velocity_commands() as angular velocity command + + def _compute_projected_gravity(self, quat: np.ndarray) -> np.ndarray: + """Compute normalized gravity vector in body frame (matching Isaac Lab) + + Isaac Lab process: + 1. Get gravity from simulator: [0, 0, -9.81] + 2. Normalize to unit vector: [0, 0, -1] + 3. Transform to body frame using quaternion inverse + 4. Result: normalized direction vector in body frame + + Args: + quat: Quaternion [w, x, y, z] + + Returns: + Normalized gravity vector in body frame [-1, 1] + """ + w, x, y, z = quat + + # World-to-body rotation matrix (transpose of body-to-world) + R_T = np.array([ + [1-2*(y*y+z*z), 2*(x*y+w*z), 2*(x*z-w*y)], + [2*(x*y-w*z), 1-2*(x*x+z*z), 2*(y*z+w*x)], + [2*(x*z+w*y), 2*(y*z-w*x), 1-2*(x*x+y*y)] + ], dtype=np.float32) + + # Gravity in world frame (normalized direction vector) + # Isaac Lab normalizes: gravity_dir = normalize([0, 0, -9.81]) = [0, 0, -1] + gravity_world_normalized = np.array([0.0, 0.0, -1.0], dtype=np.float32) + + # Transform to body frame + gravity_body = R_T @ gravity_world_normalized + + # Ensure it's normalized (should already be unit vector from rotation) + gravity_body_norm = np.linalg.norm(gravity_body) + if gravity_body_norm > 0: + gravity_body = gravity_body / gravity_body_norm + + return gravity_body + + def _compute_heading_from_quat(self, quat: np.ndarray) -> float: + """Compute heading (yaw) angle from quaternion + + Args: + quat: Quaternion [w, x, y, z] + + Returns: + Heading angle in radians [-pi, pi] + """ + w, x, y, z = quat + + # Convert quaternion to yaw angle + # yaw = atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z)) + yaw = np.arctan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)) + + return float(yaw) + + def _wrap_to_pi(self, angle: float) -> float: + """Wrap angle to [-pi, pi] range (matching IsaacLab math_utils.wrap_to_pi)""" + return np.arctan2(np.sin(angle), np.cos(angle)) + + def _add_noise(self, data: np.ndarray, noise_type: str) -> np.ndarray: + """Add uniform noise""" + if noise_type not in self.config.NOISE_LEVELS: + return np.zeros_like(data) + + n_min, n_max = self.config.NOISE_LEVELS[noise_type] + noise = np.random.uniform(n_min, n_max, size=data.shape).astype(np.float32) + return noise + +# ============================================================================= +# Main Policy Runner with ROS2 +# ============================================================================= + +class IsaacLabPolicyRunnerROS2: + """Policy runner with ROS2 diagnostics publishing and joint reordering""" + + def __init__(self, policy_path: str, config: PolicyConfig, ros_node: Go2DiagnosticsPublisher): + self.config = config + self.crc = CRC() + self.ros_node = ros_node + + # Load ONNX policy + print(f"Loading ONNX policy from: {policy_path}") + self.ort_session = ort.InferenceSession(policy_path) + + self.input_name = self.ort_session.get_inputs()[0].name + self.output_name = self.ort_session.get_outputs()[0].name + + print(f"Policy input: {self.input_name}") + print(f"Policy output: {self.output_name}") + + # Initialize observation processor (no bridge dependency) + self.obs_processor = ObservationProcessor(config) + + # Communication setup + self.latest_lowstate: Optional[LowState_] = None + self.latest_sportmode: Optional[SportModeState_] = None + self._setup_communication() + + def _setup_communication(self): + """Setup Unitree SDK communication""" + if len(sys.argv) < 2: + ChannelFactoryInitialize(1, "lo") + else: + ChannelFactoryInitialize(0, sys.argv[1]) + + self.state_sub = ChannelSubscriber("rt/lowstate", LowState_) + self.state_sub.Init(self._lowstate_handler, 10) + + # Add SportModeState subscriber for velocity data + self.sportmode_sub = ChannelSubscriber("rt/sportmodestate", SportModeState_) + self.sportmode_sub.Init(self._sportmode_handler, 10) + + self.cmd_pub = ChannelPublisher("rt/lowcmd", LowCmd_) + self.cmd_pub.Init() + + print("Communication channels initialized (LowState + SportModeState)") + + def _lowstate_handler(self, msg: LowState_): + """Handle incoming robot state""" + self.latest_lowstate = msg + + def _sportmode_handler(self, msg: SportModeState_): + """Handle incoming SportModeState""" + self.latest_sportmode = msg + # Update observation processor with velocity data + velocity = [float(msg.velocity[0]), float(msg.velocity[1]), float(msg.velocity[2])] + self.obs_processor.update_base_velocity(velocity) + + def wait_for_robot_state(self, timeout: float = 5.0): + """Wait for first robot state""" + print("Waiting for robot state...") + start_time = time.time() + + while self.latest_lowstate is None: + if time.time() - start_time > timeout: + raise TimeoutError("Failed to receive robot state") + time.sleep(0.001) + + print("Robot state received!") + + def execute_policy(self, obs: np.ndarray) -> Tuple[np.ndarray, float]: + """Execute policy and return actions + inference time + + Returns: + actions_raw: Policy output in IsaacLab order + inference_time: Inference time in milliseconds + """ + inference_start = time.perf_counter() + + obs_batch = obs.reshape(1, -1) + outputs = self.ort_session.run([self.output_name], {self.input_name: obs_batch}) + actions_raw = outputs[0][0] # IsaacLab 순서의 action + + inference_time = (time.perf_counter() - inference_start) * 1000 # ms + + return actions_raw.astype(np.float32), inference_time + + + def _compute_torques(self, target_positions_unitree: np.ndarray, + current_positions_unitree: np.ndarray, + current_velocities_unitree: np.ndarray) -> np.ndarray: + """Compute torques using PD controller (matching IsaacLab pipeline) + + Args: + target_positions_unitree: Target joint positions in Unitree order + current_positions_unitree: Current joint positions in Unitree order + current_velocities_unitree: Current joint velocities in Unitree order + + Returns: + computed_torques: Computed torques in Unitree order + """ + # Position and velocity errors + position_error = target_positions_unitree - current_positions_unitree + velocity_error = np.zeros_like(current_velocities_unitree) # Target velocity = 0 + velocity_error = velocity_error - current_velocities_unitree + + # PD controller (matching IsaacLab DCMotor parameters) + kp = self.config.KP # 25.0 + kd = self.config.KD # 0.5 + + computed_torques = kp * position_error + kd * velocity_error + + # Apply torque limits (matching IsaacLab DCMotor clipping) + torque_limit = self.config.ACTION_CLIP # 23.5 N⋅m + computed_torques = np.clip(computed_torques, -torque_limit, torque_limit) + + return computed_torques + + def create_torque_command(self, target_positions_unitree: np.ndarray, + current_positions_unitree: np.ndarray, + current_velocities_unitree: np.ndarray) -> unitree_go_msg_dds__LowCmd_: + """Create torque-based motor command (matching IsaacLab behavior) + + Args: + target_positions_unitree: Target positions in Unitree/MuJoCo order + current_positions_unitree: Current positions in Unitree/MuJoCo order + current_velocities_unitree: Current velocities in Unitree/MuJoCo order + """ + # Compute torques using PD controller + torques = self._compute_torques(target_positions_unitree, + current_positions_unitree, + current_velocities_unitree) + + cmd = unitree_go_msg_dds__LowCmd_() + + cmd.head[0] = 0xFE + cmd.head[1] = 0xEF + cmd.level_flag = 0xFF + cmd.gpio = 0 + + # Set torque commands for leg joints (0-11) + for i in range(12): + cmd.motor_cmd[i].mode = 0x01 # Position mode but we'll use torque + cmd.motor_cmd[i].q = 0.0 # Position target (not used in torque mode) + cmd.motor_cmd[i].kp = 0.0 # Zero position gain (pure torque control) + cmd.motor_cmd[i].kd = 0.0 # Zero damping gain (pure torque control) + cmd.motor_cmd[i].dq = 0.0 # Target velocity + cmd.motor_cmd[i].tau = float(torques[i]) # Computed torque command + + # Set other joints to zero + for i in range(12, 20): + cmd.motor_cmd[i].mode = 0x01 + cmd.motor_cmd[i].q = 0.0 + cmd.motor_cmd[i].kp = 0.0 + cmd.motor_cmd[i].kd = 0.0 + cmd.motor_cmd[i].dq = 0.0 + cmd.motor_cmd[i].tau = 0.0 + + cmd.crc = self.crc.Crc(cmd) + + return cmd + + def _create_zero_command(self) -> unitree_go_msg_dds__LowCmd_: + """Create zero torque command to safely stop the robot""" + cmd = unitree_go_msg_dds__LowCmd_() + + cmd.head[0] = 0xFE + cmd.head[1] = 0xEF + cmd.level_flag = 0xFF + cmd.gpio = 0 + + # Set all motors to zero torque mode + for i in range(20): + cmd.motor_cmd[i].mode = 0x01 # Keep position mode but with zero gains + cmd.motor_cmd[i].q = 0.0 # Position doesn't matter with zero gains + cmd.motor_cmd[i].kp = 0.0 # Zero position gain = no position control + cmd.motor_cmd[i].kd = 0.0 # Zero damping gain = no velocity control + cmd.motor_cmd[i].dq = 0.0 # Zero target velocity + cmd.motor_cmd[i].tau = 0.0 # Zero torque command + + cmd.crc = self.crc.Crc(cmd) + + return cmd + + def run(self, duration: Optional[float] = None, enable_noise: bool = True): + """Main execution loop with ROS2 publishing and joint reordering""" + print("Starting policy execution with ROS2 diagnostics...") + print(f"Control frequency: {1.0/self.config.CONTROL_DT:.1f} Hz") + print(f"Observation noise: {'Enabled' if enable_noise else 'Disabled'}") + print("Joint index reordering: Enabled (Unitree/MuJoCo <-> IsaacLab)") + + self.wait_for_robot_state() + + # Set initial velocity commands (all zeros initially) + # For heading mode: (vx=0.0 m/s, vy=0.0, target_heading=0.0 rad) + # For relative mode: (vx=0.0 m/s, vy=0.0, wz=0.0 rad/s) + self.obs_processor.set_velocity_commands(0.0, 0.0, -1.0) + + start_time = time.time() + step_count = 0 + + try: + while rclpy.ok(): + loop_start = time.perf_counter() + + # Check duration + if duration and (time.time() - start_time) > duration: + break + + # Get robot state + if self.latest_lowstate is None: + time.sleep(0.001) + continue + + # Process observations (MuJoCo -> IsaacLab 순서 변환) + obs, obs_components = self.obs_processor.process( + self.latest_lowstate, add_noise=enable_noise) + + # Execute policy (IsaacLab 순서 입력 -> IsaacLab 순서 출력) + actions_raw_isaaclab, inference_time = self.execute_policy(obs) + + # Scale actions (IsaacLab 순서) + actions_scaled_isaaclab = actions_raw_isaaclab * self.config.ACTION_SCALE + target_positions_isaaclab = actions_scaled_isaaclab + self.config.DEFAULT_JOINT_POS + + # ===== CRITICAL: Convert actions to Unitree order for robot control ===== + actions_raw_unitree = actions_raw_isaaclab[ISAACLAB_TO_UNITREE] + actions_scaled_unitree = actions_scaled_isaaclab[ISAACLAB_TO_UNITREE] + target_positions_unitree = target_positions_isaaclab[ISAACLAB_TO_UNITREE] + + # Update last actions (IsaacLab 순서로 저장) + self.obs_processor.update_last_actions(actions_raw_isaaclab) + + # Get heading diagnostics for ROS2 publishing + current_heading = 0.0 + target_heading = 0.0 + heading_error = 0.0 + + if self.obs_processor.heading_command: + quat = np.array(self.latest_lowstate.imu_state.quaternion, dtype=np.float32) + current_heading = self.obs_processor._compute_heading_from_quat(quat) + target_heading = self.obs_processor.heading_target + heading_error = self.obs_processor._wrap_to_pi(target_heading - current_heading) + + # Get current joint states (Unitree 순서) + current_positions_unitree = np.array([ms.q for ms in self.latest_lowstate.motor_state[:12]], dtype=np.float32) + current_velocities_unitree = np.array([ms.dq for ms in self.latest_lowstate.motor_state[:12]], dtype=np.float32) + + # Compute torques for diagnostics (Unitree 순서) + computed_torques_unitree = self._compute_torques(target_positions_unitree, + current_positions_unitree, + current_velocities_unitree) + + # Convert torques to IsaacLab order for ROS2 publishing + computed_torques_isaaclab = computed_torques_unitree[UNITREE_TO_ISAACLAB] + + # Publish ROS2 diagnostics (obs_components는 IsaacLab 순서) + current_time = time.time() - start_time + self.ros_node.publish_observations(obs_components, current_time) + # actions는 IsaacLab 순서로 publish (obs의 previous_actions와 일치) + self.ros_node.publish_actions(actions_raw_isaaclab, actions_scaled_isaaclab, + target_positions_isaaclab, computed_torques_isaaclab) + + # Spin ROS2 node to process callbacks and publish messages + rclpy.spin_once(self.ros_node, timeout_sec=0.0) + + # Send torque-based motor command (Unitree 순서) + cmd = self.create_torque_command(target_positions_unitree, + current_positions_unitree, + current_velocities_unitree) + self.cmd_pub.Write(cmd) + + # Calculate loop time + loop_time = (time.perf_counter() - loop_start) * 1000 # ms + self.ros_node.publish_diagnostics(inference_time, loop_time, + current_heading, target_heading, heading_error) + + # Additional spin to ensure diagnostics are published + rclpy.spin_once(self.ros_node, timeout_sec=0.0) + + # Timing control + elapsed = time.perf_counter() - loop_start + sleep_time = self.config.CONTROL_DT - elapsed + + if sleep_time > 0: + time.sleep(sleep_time) + elif sleep_time < -0.001: + self.ros_node.get_logger().warning( + f'Loop running {-sleep_time*1000:.1f}ms behind') + + step_count += 1 + + # Status update + if step_count % (int(2.0 / self.config.CONTROL_DT)) == 0: + self.ros_node.get_logger().info( + f'Step {step_count}, Runtime: {current_time:.1f}s') + + except KeyboardInterrupt: + print("\nStopping...") + + finally: + # Stop robot with zero torque command + zero_cmd = self._create_zero_command() + for _ in range(10): + self.cmd_pub.Write(zero_cmd) + time.sleep(0.01) + + print(f"Completed. Total steps: {step_count}") + +# ============================================================================= +# Main Entry Point +# ============================================================================= + +def main(args=None): + """Main function""" + print("=" * 60) + print("IsaacLab Policy Runner with ROS2 Diagnostics") + print("(with Joint Index Reordering)") + print("=" * 60) + + # Initialize ROS2 + rclpy.init(args=args) + ros_node = Go2DiagnosticsPublisher() + + # Configuration + config = PolicyConfig() + policy_path = "/home/jeonchanwook/unitree_mujoco/example/python/exported/policy.onnx" + + print("\nWARNING: Ensure robot area is clear!") + print("ROS2 topics available for PlotJuggler:") + print(" Policy Observations:") + print(" - /go2/obs/base_lin_vel (from SportModeState)") + print(" - /go2/obs/base_ang_vel") + print(" - /go2/obs/projected_gravity") + print(" - /go2/obs/velocity_commands") + print(" - /go2/obs/joint_states (IsaacLab order)") + print(" Policy Actions:") + print(" - /go2/actions/raw (IsaacLab order)") + print(" - /go2/actions/scaled (IsaacLab order)") + print(" - /go2/actions/target_positions (IsaacLab order)") + print(" - /go2/actions/computed_torques (IsaacLab order)") + print(" Diagnostics:") + print(" - /go2/diagnostics/inference_time_ms") + print(" - /go2/diagnostics/loop_time_ms") + print("\nUse: ros2 run plotjuggler plotjuggler") + print("Note: Run ros2_bridge_sportmodestate.py separately if you need ROS2 bridge topics") + input("\nPress Enter to start...") + + try: + runner = IsaacLabPolicyRunnerROS2(policy_path, config, ros_node) + runner.run(duration=30.0, enable_noise=True) + + except Exception as e: + ros_node.get_logger().error(f'Error: {e}') + return 1 + + finally: + ros_node.destroy_node() + rclpy.shutdown() + + return 0 + +if __name__ == "__main__": + exit(main()) diff --git a/example/python/sportmode_full_bridge.py b/example/python/sportmode_full_bridge.py new file mode 100644 index 00000000..70ef5cda --- /dev/null +++ b/example/python/sportmode_full_bridge.py @@ -0,0 +1,438 @@ +#!/usr/bin/env python3 +""" +Unitree SportModeState Full Bridge to ROS2 +=========================================== + +This script publishes the COMPLETE SportModeState to ROS2 in a structured format. + +Usage: + python3 unitree_sportmode_full_bridge.py + +Then: + ros2 topic echo /unitree/sportmode/full + ros2 topic echo /unitree/sportmode/state + + +# /unitree/sportmode/full의 데이터 구조 +[ + # [0:5] Metadata + error_code, mode, progress, gait_type, foot_raise_height, + + # [5:9] Position & Height + pos_x, pos_y, pos_z, body_height, + + # [9:13] Velocity + vel_x, vel_y, vel_z, yaw_speed, + + # [13:26] IMU (quaternion[4] + gyro[3] + acc[3] + rpy[3]) + quat_w, quat_x, quat_y, quat_z, + gyro_x, gyro_y, gyro_z, + acc_x, acc_y, acc_z, + roll, pitch, yaw, + + # [26:30] Foot force + force_FL, force_FR, force_RL, force_RR, + + # [30:42] Foot position body (FL[3], FR[3], RL[3], RR[3]) + FL_x, FL_y, FL_z, + FR_x, FR_y, FR_z, + RL_x, RL_y, RL_z, + RR_x, RR_y, RR_z, + + # [42:54] Foot speed body (FL[3], FR[3], RL[3], RR[3]) + FL_vx, FL_vy, FL_vz, + FR_vx, FR_vy, FR_vz, + RL_vx, RL_vy, RL_vz, + RR_vx, RR_vy, RR_vz, + + # [54:58] Range obstacle + range_0, range_1, range_2, range_3 +] +""" + +import sys +import signal +import atexit +import numpy as np + +import rclpy +from rclpy.node import Node +from std_msgs.msg import Header, Float32MultiArray, UInt8, UInt32, Int16MultiArray, String +from geometry_msgs.msg import Vector3, Quaternion, Pose, Twist +from diagnostic_msgs.msg import KeyValue + +from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize +from unitree_sdk2py.idl.unitree_go.msg.dds_ import SportModeState_ + + +class SportModeStateFullPublisher(Node): + """Publish complete SportModeState to ROS2""" + + def __init__(self): + super().__init__('unitree_sportmode_full_bridge') + + # Publishers - Structured format + self.pub_full = self.create_publisher( + Float32MultiArray, '/unitree/sportmode/full', 10) + + self.pub_state_json = self.create_publisher( + String, '/unitree/sportmode/state_json', 10) + + # Publishers - Individual fields (for easy access) + self.pub_error_code = self.create_publisher( + UInt32, '/unitree/sportmode/error_code', 10) + + self.pub_mode = self.create_publisher( + UInt8, '/unitree/sportmode/mode', 10) + + self.pub_gait_type = self.create_publisher( + UInt8, '/unitree/sportmode/gait_type', 10) + + self.pub_position = self.create_publisher( + Vector3, '/unitree/sportmode/position', 10) + + self.pub_velocity = self.create_publisher( + Vector3, '/unitree/sportmode/velocity', 10) + + self.pub_imu_quat = self.create_publisher( + Quaternion, '/unitree/sportmode/imu/quaternion', 10) + + self.pub_imu_gyro = self.create_publisher( + Vector3, '/unitree/sportmode/imu/gyroscope', 10) + + self.pub_imu_acc = self.create_publisher( + Vector3, '/unitree/sportmode/imu/accelerometer', 10) + + self.pub_foot_force = self.create_publisher( + Int16MultiArray, '/unitree/sportmode/foot_force', 10) + + self.pub_foot_pos_body = self.create_publisher( + Float32MultiArray, '/unitree/sportmode/foot_position_body', 10) + + self.pub_foot_speed_body = self.create_publisher( + Float32MultiArray, '/unitree/sportmode/foot_speed_body', 10) + + # Statistics + self.msg_count = 0 + self.first_msg = True + + # Initialize Unitree DDS + self._setup_unitree_dds() + + # Timer for statistics + self.stat_timer = self.create_timer(5.0, self._print_statistics) + + self.get_logger().info('=' * 70) + self.get_logger().info('Unitree SportModeState FULL Bridge to ROS2') + self.get_logger().info('=' * 70) + self.get_logger().info('Publishing complete SportModeState to:') + self.get_logger().info(' - /unitree/sportmode/full (Float32MultiArray - all data)') + self.get_logger().info(' - /unitree/sportmode/state_json (String - JSON format)') + self.get_logger().info(' - /unitree/sportmode/* (Individual fields)') + self.get_logger().info('=' * 70) + + def _setup_unitree_dds(self): + """Setup Unitree DDS""" + if len(sys.argv) < 2: + ChannelFactoryInitialize(1, "lo") + self.get_logger().info('Connected to simulation (domain=1)') + else: + ChannelFactoryInitialize(0, sys.argv[1]) + self.get_logger().info(f'Connected to real robot: {sys.argv[1]}') + + self.sportmode_sub = ChannelSubscriber("rt/sportmodestate", SportModeState_) + self.sportmode_sub.Init(self._sportmode_callback, 10) + + self.get_logger().info('Waiting for SportModeState...') + + def _sportmode_callback(self, msg: SportModeState_): + """Handle SportModeState and publish everything""" + + timestamp = self.get_clock().now() + + # Print structure on first message + if self.first_msg: + self._print_message_structure(msg) + self.first_msg = False + + # Publish full state as structured array + self._publish_full_state(msg, timestamp) + + # Publish as JSON string + self._publish_json_state(msg, timestamp) + + # Publish individual fields + self._publish_individual_fields(msg, timestamp) + + self.msg_count += 1 + + def _print_message_structure(self, msg: SportModeState_): + """Print message structure on first receive""" + self.get_logger().info('') + self.get_logger().info('📋 SportModeState Structure:') + self.get_logger().info('-' * 70) + + try: + self.get_logger().info(f' error_code: {msg.error_code}') + self.get_logger().info(f' mode: {msg.mode}') + self.get_logger().info(f' progress: {msg.progress}') + self.get_logger().info(f' gait_type: {msg.gait_type}') + self.get_logger().info(f' foot_raise_height: {msg.foot_raise_height}') + self.get_logger().info(f' position: [{msg.position[0]:.3f}, {msg.position[1]:.3f}, {msg.position[2]:.3f}]') + self.get_logger().info(f' body_height: {msg.body_height}') + self.get_logger().info(f' velocity: [{msg.velocity[0]:.3f}, {msg.velocity[1]:.3f}, {msg.velocity[2]:.3f}]') + self.get_logger().info(f' yaw_speed: {msg.yaw_speed}') + + if hasattr(msg, 'imu_state') and msg.imu_state: + self.get_logger().info(f' imu_state.quaternion: [{msg.imu_state.quaternion[0]:.3f}, ...]') + self.get_logger().info(f' imu_state.gyroscope: [{msg.imu_state.gyroscope[0]:.3f}, ...]') + self.get_logger().info(f' imu_state.accelerometer: [{msg.imu_state.accelerometer[0]:.3f}, ...]') + + self.get_logger().info(f' foot_force: {list(msg.foot_force[:4])}') + self.get_logger().info(f' foot_position_body: [12 elements]') + self.get_logger().info(f' foot_speed_body: [12 elements]') + + except Exception as e: + self.get_logger().warn(f'Could not print structure: {e}') + + self.get_logger().info('-' * 70) + self.get_logger().info('') + + def _publish_full_state(self, msg: SportModeState_, timestamp): + """Publish complete state as Float32MultiArray""" + full_msg = Float32MultiArray() + + # Pack all data into one array + # Layout: [metadata(5), position(3), velocity(4), imu(13), feet(40)] + data = [] + + # Metadata (5) + data.append(float(msg.error_code)) + data.append(float(msg.mode)) + data.append(float(msg.progress)) + data.append(float(msg.gait_type)) + data.append(float(msg.foot_raise_height)) + + # Position & height (4) + data.extend([float(x) for x in msg.position]) + data.append(float(msg.body_height)) + + # Velocity (4: vx, vy, vz, yaw_speed) + data.extend([float(x) for x in msg.velocity]) + data.append(float(msg.yaw_speed)) + + # IMU (13: quat[4] + gyro[3] + acc[3] + rpy[3]) + if hasattr(msg, 'imu_state') and msg.imu_state: + data.extend([float(x) for x in msg.imu_state.quaternion]) + data.extend([float(x) for x in msg.imu_state.gyroscope]) + data.extend([float(x) for x in msg.imu_state.accelerometer]) + if hasattr(msg.imu_state, 'rpy'): + data.extend([float(x) for x in msg.imu_state.rpy]) + else: + data.extend([0.0, 0.0, 0.0]) + else: + data.extend([0.0] * 13) + + # Foot force (4) + data.extend([float(x) for x in msg.foot_force[:4]]) + + # Foot position body (12) + data.extend([float(x) for x in msg.foot_position_body[:12]]) + + # Foot speed body (12) + data.extend([float(x) for x in msg.foot_speed_body[:12]]) + + # Range obstacle (4) + if hasattr(msg, 'range_obstacle'): + data.extend([float(x) for x in msg.range_obstacle[:4]]) + else: + data.extend([0.0] * 4) + + full_msg.data = data + self.pub_full.publish(full_msg) + + def _publish_json_state(self, msg: SportModeState_, timestamp): + """Publish as JSON string for easy parsing""" + import json + + state_dict = { + 'timestamp_sec': timestamp.nanoseconds / 1e9, + 'error_code': int(msg.error_code), + 'mode': int(msg.mode), + 'progress': float(msg.progress), + 'gait_type': int(msg.gait_type), + 'foot_raise_height': float(msg.foot_raise_height), + 'position': [float(x) for x in msg.position], + 'body_height': float(msg.body_height), + 'velocity': [float(x) for x in msg.velocity], + 'yaw_speed': float(msg.yaw_speed), + 'foot_force': [int(x) for x in msg.foot_force[:4]], + 'foot_position_body': [float(x) for x in msg.foot_position_body[:12]], + 'foot_speed_body': [float(x) for x in msg.foot_speed_body[:12]], + } + + # Add IMU if available + if hasattr(msg, 'imu_state') and msg.imu_state: + state_dict['imu'] = { + 'quaternion': [float(x) for x in msg.imu_state.quaternion], + 'gyroscope': [float(x) for x in msg.imu_state.gyroscope], + 'accelerometer': [float(x) for x in msg.imu_state.accelerometer], + } + if hasattr(msg.imu_state, 'rpy'): + state_dict['imu']['rpy'] = [float(x) for x in msg.imu_state.rpy] + + json_msg = String() + json_msg.data = json.dumps(state_dict, indent=2) + self.pub_state_json.publish(json_msg) + + def _publish_individual_fields(self, msg: SportModeState_, timestamp): + """Publish individual fields for easy access""" + + # Error code + error_msg = UInt32() + error_msg.data = msg.error_code + self.pub_error_code.publish(error_msg) + + # Mode + mode_msg = UInt8() + mode_msg.data = msg.mode + self.pub_mode.publish(mode_msg) + + # Gait type + gait_msg = UInt8() + gait_msg.data = msg.gait_type + self.pub_gait_type.publish(gait_msg) + + # Position + pos_msg = Vector3() + pos_msg.x = float(msg.position[0]) + pos_msg.y = float(msg.position[1]) + pos_msg.z = float(msg.position[2]) + self.pub_position.publish(pos_msg) + + # Velocity + vel_msg = Vector3() + vel_msg.x = float(msg.velocity[0]) + vel_msg.y = float(msg.velocity[1]) + vel_msg.z = float(msg.velocity[2]) + self.pub_velocity.publish(vel_msg) + + # IMU + if hasattr(msg, 'imu_state') and msg.imu_state: + # Quaternion + quat_msg = Quaternion() + quat_msg.w = float(msg.imu_state.quaternion[0]) + quat_msg.x = float(msg.imu_state.quaternion[1]) + quat_msg.y = float(msg.imu_state.quaternion[2]) + quat_msg.z = float(msg.imu_state.quaternion[3]) + self.pub_imu_quat.publish(quat_msg) + + # Gyroscope + gyro_msg = Vector3() + gyro_msg.x = float(msg.imu_state.gyroscope[0]) + gyro_msg.y = float(msg.imu_state.gyroscope[1]) + gyro_msg.z = float(msg.imu_state.gyroscope[2]) + self.pub_imu_gyro.publish(gyro_msg) + + # Accelerometer + acc_msg = Vector3() + acc_msg.x = float(msg.imu_state.accelerometer[0]) + acc_msg.y = float(msg.imu_state.accelerometer[1]) + acc_msg.z = float(msg.imu_state.accelerometer[2]) + self.pub_imu_acc.publish(acc_msg) + + # Foot force + force_msg = Int16MultiArray() + force_msg.data = [int(x) for x in msg.foot_force[:4]] + self.pub_foot_force.publish(force_msg) + + # Foot position body + foot_pos_msg = Float32MultiArray() + foot_pos_msg.data = [float(x) for x in msg.foot_position_body[:12]] + self.pub_foot_pos_body.publish(foot_pos_msg) + + # Foot speed body + foot_speed_msg = Float32MultiArray() + foot_speed_msg.data = [float(x) for x in msg.foot_speed_body[:12]] + self.pub_foot_speed_body.publish(foot_speed_msg) + + def _print_statistics(self): + """Print statistics""" + if self.msg_count > 0: + self.get_logger().info( + f'📊 Received {self.msg_count} messages ' + f'(~{self.msg_count/5:.1f} Hz)' + ) + self.msg_count = 0 + else: + self.get_logger().warn('⚠️ No messages received') + + +# ============================================================================= +# Cleanup +# ============================================================================= + +_node_instance = None + +def cleanup_handler(signum=None, frame=None): + global _node_instance + + print("\n🧹 Shutting down SportModeState full bridge...") + + if _node_instance: + try: + _node_instance.destroy_node() + print("✓ Node destroyed") + except: + pass + + try: + if rclpy.ok(): + rclpy.shutdown() + print("✓ ROS2 shutdown") + except: + pass + + print("✓ Shutdown complete\n") + + if signum is not None: + sys.exit(0) + +signal.signal(signal.SIGINT, cleanup_handler) +signal.signal(signal.SIGTERM, cleanup_handler) +atexit.register(cleanup_handler) + + +# ============================================================================= +# Main +# ============================================================================= + +def main(args=None): + global _node_instance + + print("\n" + "=" * 70) + print("Unitree SportModeState FULL Bridge to ROS2") + print("=" * 70) + print("Press Ctrl+C to stop") + print("=" * 70 + "\n") + + try: + rclpy.init(args=args) + node = SportModeStateFullPublisher() + _node_instance = node + rclpy.spin(node) + return 0 + + except KeyboardInterrupt: + print("\n🛑 Interrupted") + return 0 + + except Exception as e: + print(f"\n❌ Error: {e}") + import traceback + traceback.print_exc() + return 1 + + +if __name__ == "__main__": + exit(main()) diff --git a/readme.md b/readme.md index 2fe5f88f..88e8f7fe 100644 --- a/readme.md +++ b/readme.md @@ -1,3 +1,34 @@ +### Summary +Implemented the motor command interface in MuJoCo to enable Go2 to walk correctly using the rsl_rl policy within the Unitree Mujoco environment. + +### Key Difference +- `compute_torque()` method converts the rsl_rl policy output into valid motor commands for MuJoCo. +- Added ROS2 PlotJuggler support to visualize joint states and command signals in real-time. +- Included helper scripts: + - `reordering_example.py` for clarifying joint index mapping + - `sportmode_full_bridge.py` for inspecting Sport Mode motor commands through PlotJuggler + +### How to Run +1. Launch the MuJoCo simulator +``` + cd unitree_mujoco/simulate_python + python3 unitree_mujoco.py +``` +2. Run the policy control script +``` + cd ../example/python + python3 ros2_policy_runner_fixed.py +``` + +### Expected Behavior +Go2 will walk properly using the trained rsl_rl policy. +Demo video: +https://youtu.be/uk0By2kI83g?si=a1glc6pgzfZDA3AB + + + +--- + # Introduction ## Unitree mujoco `unitree_mujoco` is a simulator developed based on `Unitree sdk2` and `mujoco`. Users can easily integrate the control programs developed with `Unitree_sdk2`, `unitree_ros2`, and `unitree_sdk2_python` into this simulator, enabling a seamless transition from simulation to physical development. The repository includes two versions of the simulator implemented in C++ and Python, with a structure as follows: @@ -306,4 +337,4 @@ export ROS_DOMAIN_ID=1 # Modify the domain id to match the simulation source ~/unitree_ros2/setup.sh # Use the network card connected to the robot export ROS_DOMAIN_ID=0 # Use the default domain id ./install/stand_go2/bin/stand_go2 # Run -``` \ No newline at end of file +``` diff --git a/simulate_python/unitree_sdk2py_bridge.py b/simulate_python/unitree_sdk2py_bridge.py index a9ca97ae..c5771813 100644 --- a/simulate_python/unitree_sdk2py_bridge.py +++ b/simulate_python/unitree_sdk2py_bridge.py @@ -301,7 +301,7 @@ def SetupJoystick(self, device_id=0, js_type="xbox"): self.joystick.init() else: print("No gamepad detected.") - sys.exit() + # sys.exit() if js_type == "xbox": self.axis_id = {