Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
789 changes: 138 additions & 651 deletions sim_main.py

Large diffs are not rendered by default.

13 changes: 5 additions & 8 deletions src/python/control/RobotController.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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)
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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
Expand Down
282 changes: 61 additions & 221 deletions src/python/control/action_provider_dds.py
Original file line number Diff line number Diff line change
@@ -1,229 +1,69 @@
# 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)
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

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()
Loading