diff --git a/source/mindbot/devices/openxr/retargeters/unitree/g1_brainco_dex_retargeting_utils.py b/source/mindbot/devices/openxr/retargeters/unitree/g1_brainco_dex_retargeting_utils.py new file mode 100644 index 0000000..756c141 --- /dev/null +++ b/source/mindbot/devices/openxr/retargeters/unitree/g1_brainco_dex_retargeting_utils.py @@ -0,0 +1,103 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import logging + +import numpy as np + +from isaaclab.devices.openxr.retargeters.humanoid.unitree.trihand.g1_dex_retargeting_utils import ( + G1TriHandDexRetargeting, +) + +logger = logging.getLogger(__name__) + +_LEFT_HAND_JOINT_NAMES = [ + "left_index_proximal_joint", + "left_middle_proximal_joint", + "left_ring_proximal_joint", + "left_pinky_proximal_joint", + "left_thumb_metacarpal_joint", + "left_thumb_proximal_joint", +] + +_RIGHT_HAND_JOINT_NAMES = [ + "right_index_proximal_joint", + "right_middle_proximal_joint", + "right_ring_proximal_joint", + "right_pinky_proximal_joint", + "right_thumb_metacarpal_joint", + "right_thumb_proximal_joint", +] + + +class G1BrainCoDexRetargeting: + """Wrap the stock G1 TriHand dex-retargeter and map its output to BrainCo joints.""" + + def __init__(self, hand_joint_names: list[str]): + self._base = G1TriHandDexRetargeting(hand_joint_names) + self.isaac_lab_hand_joint_names = hand_joint_names + self.left_dof_names = _LEFT_HAND_JOINT_NAMES + self.right_dof_names = _RIGHT_HAND_JOINT_NAMES + self.dof_names = self.left_dof_names + self.right_dof_names + + missing = [name for name in self.dof_names if name not in hand_joint_names] + if missing: + raise ValueError(f"BrainCo hand_joint_names missing joints: {missing}") + + def _map_one_hand(self, base_q: np.ndarray) -> np.ndarray: + """Map old 7-DoF TriHand output to 6 BrainCo joints. + + Old order: + [thumb_0, thumb_1, thumb_2, index_0, index_1, middle_0, middle_1] + + New order: + [index_prox, middle_prox, ring_prox, pinky_prox, thumb_meta, thumb_prox] + """ + q = np.asarray(base_q, dtype=np.float32) + if q.shape[0] != 7: + raise ValueError(f"Expected 7 TriHand joints, got shape {q.shape}") + + index_prox = q[3] + middle_prox = q[5] + + # BrainCo has extra ring/pinky proximal joints; tie them to middle for a simple grasp prior. + ring_prox = middle_prox + pinky_prox = middle_prox + + thumb_meta = q[0] + thumb_prox = q[1] + + return np.array( + [ + index_prox, + middle_prox, + ring_prox, + pinky_prox, + thumb_meta, + thumb_prox, + ], + dtype=np.float32, + ) + + def compute_left(self, left_hand_poses: dict[str, np.ndarray]) -> np.ndarray: + if left_hand_poses is None: + return np.zeros(len(_LEFT_HAND_JOINT_NAMES), dtype=np.float32) + return self._map_one_hand(self._base.compute_left(left_hand_poses)) + + def compute_right(self, right_hand_poses: dict[str, np.ndarray]) -> np.ndarray: + if right_hand_poses is None: + return np.zeros(len(_RIGHT_HAND_JOINT_NAMES), dtype=np.float32) + return self._map_one_hand(self._base.compute_right(right_hand_poses)) + + def get_joint_names(self) -> list[str]: + return self.dof_names + + def get_left_joint_names(self) -> list[str]: + return self.left_dof_names + + def get_right_joint_names(self) -> list[str]: + return self.right_dof_names diff --git a/source/mindbot/devices/openxr/retargeters/unitree/g1_upper_body_brainco_motion_ctrl_retargeter.py b/source/mindbot/devices/openxr/retargeters/unitree/g1_upper_body_brainco_motion_ctrl_retargeter.py new file mode 100644 index 0000000..7cb4917 --- /dev/null +++ b/source/mindbot/devices/openxr/retargeters/unitree/g1_upper_body_brainco_motion_ctrl_retargeter.py @@ -0,0 +1,130 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np +import torch + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as PoseUtils +from isaaclab.devices.device_base import DeviceBase +from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg +from isaaclab.markers import VisualizationMarkers, VisualizationMarkersCfg + + +class G1BrainCoUpperBodyMotionControllerRetargeter(RetargeterBase): + """Map motion-controller inputs to G1 BrainCo wrist and hand joints.""" + + def __init__(self, cfg: "G1BrainCoUpperBodyMotionControllerRetargeterCfg"): + super().__init__(cfg) + self._sim_device = cfg.sim_device + self._hand_joint_names = cfg.hand_joint_names + self._enable_visualization = cfg.enable_visualization + + if cfg.hand_joint_names is None: + raise ValueError("hand_joint_names must be provided") + + if self._enable_visualization: + marker_cfg = VisualizationMarkersCfg( + prim_path="/Visuals/g1_brainco_controller_markers", + markers={ + "joint": sim_utils.SphereCfg( + radius=0.01, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0)), + ), + }, + ) + self._markers = VisualizationMarkers(marker_cfg) + + def retarget(self, data: dict) -> torch.Tensor: + left_controller_data = data.get(DeviceBase.TrackingTarget.CONTROLLER_LEFT, np.array([])) + right_controller_data = data.get(DeviceBase.TrackingTarget.CONTROLLER_RIGHT, np.array([])) + + default_wrist = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], dtype=np.float32) + + left_wrist = self._extract_wrist_pose(left_controller_data, default_wrist) + right_wrist = self._extract_wrist_pose(right_controller_data, default_wrist) + + left_hand_joints = self._map_to_hand_joints(left_controller_data, is_left=True) + right_hand_joints = self._map_to_hand_joints(right_controller_data, is_left=False) + + all_hand_joints = np.concatenate([left_hand_joints, right_hand_joints]).astype(np.float32) + + left_wrist_tensor = torch.tensor( + self._retarget_abs(left_wrist, is_left=True), dtype=torch.float32, device=self._sim_device + ) + right_wrist_tensor = torch.tensor( + self._retarget_abs(right_wrist, is_left=False), dtype=torch.float32, device=self._sim_device + ) + hand_joints_tensor = torch.tensor(all_hand_joints, dtype=torch.float32, device=self._sim_device) + + return torch.cat([left_wrist_tensor, right_wrist_tensor, hand_joints_tensor]) + + def get_requirements(self) -> list[RetargeterBase.Requirement]: + return [RetargeterBase.Requirement.MOTION_CONTROLLER] + + def _extract_wrist_pose(self, controller_data: np.ndarray, default_pose: np.ndarray) -> np.ndarray: + if len(controller_data) > DeviceBase.MotionControllerDataRowIndex.POSE.value: + return controller_data[DeviceBase.MotionControllerDataRowIndex.POSE.value] + return default_pose + + def _map_to_hand_joints(self, controller_data: np.ndarray, is_left: bool) -> np.ndarray: + joints = np.zeros(6, dtype=np.float32) + + if len(controller_data) <= DeviceBase.MotionControllerDataRowIndex.INPUTS.value: + return joints + + inputs = controller_data[DeviceBase.MotionControllerDataRowIndex.INPUTS.value] + if len(inputs) < len(DeviceBase.MotionControllerInputIndex): + return joints + + trigger = float(inputs[DeviceBase.MotionControllerInputIndex.TRIGGER.value]) + squeeze = float(inputs[DeviceBase.MotionControllerInputIndex.SQUEEZE.value]) + + index_prox = trigger * 1.0 + middle_prox = squeeze * 1.0 + ring_prox = squeeze * 1.0 + pinky_prox = squeeze * 1.0 + + thumb_meta = 0.5 * trigger - 0.5 * squeeze + thumb_prox = -max(trigger, squeeze) * 0.4 + + if is_left: + thumb_meta = -thumb_meta + + joints[:] = [ + index_prox, + middle_prox, + ring_prox, + pinky_prox, + thumb_meta, + thumb_prox, + ] + return joints + + def _retarget_abs(self, wrist: np.ndarray, is_left: bool) -> np.ndarray: + wrist_pos = torch.tensor(wrist[:3], dtype=torch.float32) + wrist_quat = torch.tensor(wrist[3:], dtype=torch.float32) + + combined_quat = torch.tensor([0.5358, -0.4619, 0.5358, 0.4619], dtype=torch.float32) + + openxr_pose = PoseUtils.make_pose(wrist_pos, PoseUtils.matrix_from_quat(wrist_quat)) + transform_pose = PoseUtils.make_pose(torch.zeros(3), PoseUtils.matrix_from_quat(combined_quat)) + + result_pose = PoseUtils.pose_in_A_to_pose_in_B(transform_pose, openxr_pose) + pos, rot_mat = PoseUtils.unmake_pose(result_pose) + quat = PoseUtils.quat_from_matrix(rot_mat) + + return np.concatenate([pos.numpy(), quat.numpy()]) + + +@dataclass +class G1BrainCoUpperBodyMotionControllerRetargeterCfg(RetargeterCfg): + enable_visualization: bool = False + hand_joint_names: list[str] | None = None + retargeter_type: type[RetargeterBase] = G1BrainCoUpperBodyMotionControllerRetargeter diff --git a/source/mindbot/devices/openxr/retargeters/unitree/g1_upper_body_brainco_retargeter.py b/source/mindbot/devices/openxr/retargeters/unitree/g1_upper_body_brainco_retargeter.py new file mode 100644 index 0000000..5570932 --- /dev/null +++ b/source/mindbot/devices/openxr/retargeters/unitree/g1_upper_body_brainco_retargeter.py @@ -0,0 +1,117 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np +import torch + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as PoseUtils +from isaaclab.devices.device_base import DeviceBase +from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg +from isaaclab.markers import VisualizationMarkers, VisualizationMarkersCfg + +from .g1_brainco_dex_retargeting_utils import G1BrainCoDexRetargeting + + +class G1BrainCoUpperBodyRetargeter(RetargeterBase): + """Retarget OpenXR hand tracking to G1 BrainCo upper-body commands.""" + + def __init__(self, cfg: "G1BrainCoUpperBodyRetargeterCfg"): + super().__init__(cfg) + self._sim_device = cfg.sim_device + self._hand_joint_names = cfg.hand_joint_names + + if cfg.hand_joint_names is None: + raise ValueError("hand_joint_names must be provided in configuration") + + self._hands_controller = G1BrainCoDexRetargeting(cfg.hand_joint_names) + self._enable_visualization = cfg.enable_visualization + self._num_open_xr_hand_joints = cfg.num_open_xr_hand_joints + + if self._enable_visualization: + marker_cfg = VisualizationMarkersCfg( + prim_path="/Visuals/g1_brainco_hand_markers", + markers={ + "joint": sim_utils.SphereCfg( + radius=0.005, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)), + ), + }, + ) + self._markers = VisualizationMarkers(marker_cfg) + + def retarget(self, data: dict) -> torch.Tensor: + left_hand_poses = data[DeviceBase.TrackingTarget.HAND_LEFT] + right_hand_poses = data[DeviceBase.TrackingTarget.HAND_RIGHT] + + left_wrist = left_hand_poses.get("wrist") + right_wrist = right_hand_poses.get("wrist") + + default_pose = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], dtype=np.float32) + if left_wrist is None: + left_wrist = default_pose + if right_wrist is None: + right_wrist = default_pose + + if self._enable_visualization: + joints_position = np.zeros((self._num_open_xr_hand_joints, 3), dtype=np.float32) + joints_position[::2] = np.array([pose[:3] for pose in left_hand_poses.values()], dtype=np.float32) + joints_position[1::2] = np.array([pose[:3] for pose in right_hand_poses.values()], dtype=np.float32) + self._markers.visualize(translations=torch.tensor(joints_position, device=self._sim_device)) + + left_hand_pos = self._hands_controller.compute_left(left_hand_poses) + left_indexes = [self._hand_joint_names.index(name) for name in self._hands_controller.get_left_joint_names()] + left_retargeted = np.zeros(len(self._hand_joint_names), dtype=np.float32) + left_retargeted[left_indexes] = left_hand_pos + + right_hand_pos = self._hands_controller.compute_right(right_hand_poses) + right_indexes = [self._hand_joint_names.index(name) for name in self._hands_controller.get_right_joint_names()] + right_retargeted = np.zeros(len(self._hand_joint_names), dtype=np.float32) + right_retargeted[right_indexes] = right_hand_pos + + retargeted_hand_joints = left_retargeted + right_retargeted + + left_wrist_tensor = torch.tensor( + self._retarget_abs(left_wrist, is_left=True), dtype=torch.float32, device=self._sim_device + ) + right_wrist_tensor = torch.tensor( + self._retarget_abs(right_wrist, is_left=False), dtype=torch.float32, device=self._sim_device + ) + hand_joints_tensor = torch.tensor(retargeted_hand_joints, dtype=torch.float32, device=self._sim_device) + + return torch.cat([left_wrist_tensor, right_wrist_tensor, hand_joints_tensor]) + + def get_requirements(self) -> list[RetargeterBase.Requirement]: + return [RetargeterBase.Requirement.HAND_TRACKING] + + def _retarget_abs(self, wrist: np.ndarray, is_left: bool) -> np.ndarray: + wrist_pos = torch.tensor(wrist[:3], dtype=torch.float32) + wrist_quat = torch.tensor(wrist[3:], dtype=torch.float32) + + if is_left: + combined_quat = torch.tensor([0.7071, 0.0, 0.7071, 0.0], dtype=torch.float32) + else: + combined_quat = torch.tensor([0.0, -0.7071, 0.0, 0.7071], dtype=torch.float32) + + openxr_pose = PoseUtils.make_pose(wrist_pos, PoseUtils.matrix_from_quat(wrist_quat)) + transform_pose = PoseUtils.make_pose(torch.zeros(3), PoseUtils.matrix_from_quat(combined_quat)) + + result_pose = PoseUtils.pose_in_A_to_pose_in_B(transform_pose, openxr_pose) + pos, rot_mat = PoseUtils.unmake_pose(result_pose) + quat = PoseUtils.quat_from_matrix(rot_mat) + + return np.concatenate([pos.numpy(), quat.numpy()]) + + +@dataclass +class G1BrainCoUpperBodyRetargeterCfg(RetargeterCfg): + enable_visualization: bool = False + num_open_xr_hand_joints: int = 100 + hand_joint_names: list[str] | None = None + retargeter_type: type[RetargeterBase] = G1BrainCoUpperBodyRetargeter diff --git a/source/mindbot/mindbot/devices/openxr/retargeters/unitree/g1_brainco_dex_retargeting_utils.py b/source/mindbot/mindbot/devices/openxr/retargeters/unitree/g1_brainco_dex_retargeting_utils.py new file mode 100644 index 0000000..756c141 --- /dev/null +++ b/source/mindbot/mindbot/devices/openxr/retargeters/unitree/g1_brainco_dex_retargeting_utils.py @@ -0,0 +1,103 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import logging + +import numpy as np + +from isaaclab.devices.openxr.retargeters.humanoid.unitree.trihand.g1_dex_retargeting_utils import ( + G1TriHandDexRetargeting, +) + +logger = logging.getLogger(__name__) + +_LEFT_HAND_JOINT_NAMES = [ + "left_index_proximal_joint", + "left_middle_proximal_joint", + "left_ring_proximal_joint", + "left_pinky_proximal_joint", + "left_thumb_metacarpal_joint", + "left_thumb_proximal_joint", +] + +_RIGHT_HAND_JOINT_NAMES = [ + "right_index_proximal_joint", + "right_middle_proximal_joint", + "right_ring_proximal_joint", + "right_pinky_proximal_joint", + "right_thumb_metacarpal_joint", + "right_thumb_proximal_joint", +] + + +class G1BrainCoDexRetargeting: + """Wrap the stock G1 TriHand dex-retargeter and map its output to BrainCo joints.""" + + def __init__(self, hand_joint_names: list[str]): + self._base = G1TriHandDexRetargeting(hand_joint_names) + self.isaac_lab_hand_joint_names = hand_joint_names + self.left_dof_names = _LEFT_HAND_JOINT_NAMES + self.right_dof_names = _RIGHT_HAND_JOINT_NAMES + self.dof_names = self.left_dof_names + self.right_dof_names + + missing = [name for name in self.dof_names if name not in hand_joint_names] + if missing: + raise ValueError(f"BrainCo hand_joint_names missing joints: {missing}") + + def _map_one_hand(self, base_q: np.ndarray) -> np.ndarray: + """Map old 7-DoF TriHand output to 6 BrainCo joints. + + Old order: + [thumb_0, thumb_1, thumb_2, index_0, index_1, middle_0, middle_1] + + New order: + [index_prox, middle_prox, ring_prox, pinky_prox, thumb_meta, thumb_prox] + """ + q = np.asarray(base_q, dtype=np.float32) + if q.shape[0] != 7: + raise ValueError(f"Expected 7 TriHand joints, got shape {q.shape}") + + index_prox = q[3] + middle_prox = q[5] + + # BrainCo has extra ring/pinky proximal joints; tie them to middle for a simple grasp prior. + ring_prox = middle_prox + pinky_prox = middle_prox + + thumb_meta = q[0] + thumb_prox = q[1] + + return np.array( + [ + index_prox, + middle_prox, + ring_prox, + pinky_prox, + thumb_meta, + thumb_prox, + ], + dtype=np.float32, + ) + + def compute_left(self, left_hand_poses: dict[str, np.ndarray]) -> np.ndarray: + if left_hand_poses is None: + return np.zeros(len(_LEFT_HAND_JOINT_NAMES), dtype=np.float32) + return self._map_one_hand(self._base.compute_left(left_hand_poses)) + + def compute_right(self, right_hand_poses: dict[str, np.ndarray]) -> np.ndarray: + if right_hand_poses is None: + return np.zeros(len(_RIGHT_HAND_JOINT_NAMES), dtype=np.float32) + return self._map_one_hand(self._base.compute_right(right_hand_poses)) + + def get_joint_names(self) -> list[str]: + return self.dof_names + + def get_left_joint_names(self) -> list[str]: + return self.left_dof_names + + def get_right_joint_names(self) -> list[str]: + return self.right_dof_names diff --git a/source/mindbot/mindbot/devices/openxr/retargeters/unitree/g1_upper_body_brainco_motion_ctrl_retargeter.py b/source/mindbot/mindbot/devices/openxr/retargeters/unitree/g1_upper_body_brainco_motion_ctrl_retargeter.py new file mode 100644 index 0000000..7cb4917 --- /dev/null +++ b/source/mindbot/mindbot/devices/openxr/retargeters/unitree/g1_upper_body_brainco_motion_ctrl_retargeter.py @@ -0,0 +1,130 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np +import torch + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as PoseUtils +from isaaclab.devices.device_base import DeviceBase +from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg +from isaaclab.markers import VisualizationMarkers, VisualizationMarkersCfg + + +class G1BrainCoUpperBodyMotionControllerRetargeter(RetargeterBase): + """Map motion-controller inputs to G1 BrainCo wrist and hand joints.""" + + def __init__(self, cfg: "G1BrainCoUpperBodyMotionControllerRetargeterCfg"): + super().__init__(cfg) + self._sim_device = cfg.sim_device + self._hand_joint_names = cfg.hand_joint_names + self._enable_visualization = cfg.enable_visualization + + if cfg.hand_joint_names is None: + raise ValueError("hand_joint_names must be provided") + + if self._enable_visualization: + marker_cfg = VisualizationMarkersCfg( + prim_path="/Visuals/g1_brainco_controller_markers", + markers={ + "joint": sim_utils.SphereCfg( + radius=0.01, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0)), + ), + }, + ) + self._markers = VisualizationMarkers(marker_cfg) + + def retarget(self, data: dict) -> torch.Tensor: + left_controller_data = data.get(DeviceBase.TrackingTarget.CONTROLLER_LEFT, np.array([])) + right_controller_data = data.get(DeviceBase.TrackingTarget.CONTROLLER_RIGHT, np.array([])) + + default_wrist = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], dtype=np.float32) + + left_wrist = self._extract_wrist_pose(left_controller_data, default_wrist) + right_wrist = self._extract_wrist_pose(right_controller_data, default_wrist) + + left_hand_joints = self._map_to_hand_joints(left_controller_data, is_left=True) + right_hand_joints = self._map_to_hand_joints(right_controller_data, is_left=False) + + all_hand_joints = np.concatenate([left_hand_joints, right_hand_joints]).astype(np.float32) + + left_wrist_tensor = torch.tensor( + self._retarget_abs(left_wrist, is_left=True), dtype=torch.float32, device=self._sim_device + ) + right_wrist_tensor = torch.tensor( + self._retarget_abs(right_wrist, is_left=False), dtype=torch.float32, device=self._sim_device + ) + hand_joints_tensor = torch.tensor(all_hand_joints, dtype=torch.float32, device=self._sim_device) + + return torch.cat([left_wrist_tensor, right_wrist_tensor, hand_joints_tensor]) + + def get_requirements(self) -> list[RetargeterBase.Requirement]: + return [RetargeterBase.Requirement.MOTION_CONTROLLER] + + def _extract_wrist_pose(self, controller_data: np.ndarray, default_pose: np.ndarray) -> np.ndarray: + if len(controller_data) > DeviceBase.MotionControllerDataRowIndex.POSE.value: + return controller_data[DeviceBase.MotionControllerDataRowIndex.POSE.value] + return default_pose + + def _map_to_hand_joints(self, controller_data: np.ndarray, is_left: bool) -> np.ndarray: + joints = np.zeros(6, dtype=np.float32) + + if len(controller_data) <= DeviceBase.MotionControllerDataRowIndex.INPUTS.value: + return joints + + inputs = controller_data[DeviceBase.MotionControllerDataRowIndex.INPUTS.value] + if len(inputs) < len(DeviceBase.MotionControllerInputIndex): + return joints + + trigger = float(inputs[DeviceBase.MotionControllerInputIndex.TRIGGER.value]) + squeeze = float(inputs[DeviceBase.MotionControllerInputIndex.SQUEEZE.value]) + + index_prox = trigger * 1.0 + middle_prox = squeeze * 1.0 + ring_prox = squeeze * 1.0 + pinky_prox = squeeze * 1.0 + + thumb_meta = 0.5 * trigger - 0.5 * squeeze + thumb_prox = -max(trigger, squeeze) * 0.4 + + if is_left: + thumb_meta = -thumb_meta + + joints[:] = [ + index_prox, + middle_prox, + ring_prox, + pinky_prox, + thumb_meta, + thumb_prox, + ] + return joints + + def _retarget_abs(self, wrist: np.ndarray, is_left: bool) -> np.ndarray: + wrist_pos = torch.tensor(wrist[:3], dtype=torch.float32) + wrist_quat = torch.tensor(wrist[3:], dtype=torch.float32) + + combined_quat = torch.tensor([0.5358, -0.4619, 0.5358, 0.4619], dtype=torch.float32) + + openxr_pose = PoseUtils.make_pose(wrist_pos, PoseUtils.matrix_from_quat(wrist_quat)) + transform_pose = PoseUtils.make_pose(torch.zeros(3), PoseUtils.matrix_from_quat(combined_quat)) + + result_pose = PoseUtils.pose_in_A_to_pose_in_B(transform_pose, openxr_pose) + pos, rot_mat = PoseUtils.unmake_pose(result_pose) + quat = PoseUtils.quat_from_matrix(rot_mat) + + return np.concatenate([pos.numpy(), quat.numpy()]) + + +@dataclass +class G1BrainCoUpperBodyMotionControllerRetargeterCfg(RetargeterCfg): + enable_visualization: bool = False + hand_joint_names: list[str] | None = None + retargeter_type: type[RetargeterBase] = G1BrainCoUpperBodyMotionControllerRetargeter diff --git a/source/mindbot/mindbot/devices/openxr/retargeters/unitree/g1_upper_body_brainco_retargeter.py b/source/mindbot/mindbot/devices/openxr/retargeters/unitree/g1_upper_body_brainco_retargeter.py new file mode 100644 index 0000000..5570932 --- /dev/null +++ b/source/mindbot/mindbot/devices/openxr/retargeters/unitree/g1_upper_body_brainco_retargeter.py @@ -0,0 +1,117 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np +import torch + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as PoseUtils +from isaaclab.devices.device_base import DeviceBase +from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg +from isaaclab.markers import VisualizationMarkers, VisualizationMarkersCfg + +from .g1_brainco_dex_retargeting_utils import G1BrainCoDexRetargeting + + +class G1BrainCoUpperBodyRetargeter(RetargeterBase): + """Retarget OpenXR hand tracking to G1 BrainCo upper-body commands.""" + + def __init__(self, cfg: "G1BrainCoUpperBodyRetargeterCfg"): + super().__init__(cfg) + self._sim_device = cfg.sim_device + self._hand_joint_names = cfg.hand_joint_names + + if cfg.hand_joint_names is None: + raise ValueError("hand_joint_names must be provided in configuration") + + self._hands_controller = G1BrainCoDexRetargeting(cfg.hand_joint_names) + self._enable_visualization = cfg.enable_visualization + self._num_open_xr_hand_joints = cfg.num_open_xr_hand_joints + + if self._enable_visualization: + marker_cfg = VisualizationMarkersCfg( + prim_path="/Visuals/g1_brainco_hand_markers", + markers={ + "joint": sim_utils.SphereCfg( + radius=0.005, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)), + ), + }, + ) + self._markers = VisualizationMarkers(marker_cfg) + + def retarget(self, data: dict) -> torch.Tensor: + left_hand_poses = data[DeviceBase.TrackingTarget.HAND_LEFT] + right_hand_poses = data[DeviceBase.TrackingTarget.HAND_RIGHT] + + left_wrist = left_hand_poses.get("wrist") + right_wrist = right_hand_poses.get("wrist") + + default_pose = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], dtype=np.float32) + if left_wrist is None: + left_wrist = default_pose + if right_wrist is None: + right_wrist = default_pose + + if self._enable_visualization: + joints_position = np.zeros((self._num_open_xr_hand_joints, 3), dtype=np.float32) + joints_position[::2] = np.array([pose[:3] for pose in left_hand_poses.values()], dtype=np.float32) + joints_position[1::2] = np.array([pose[:3] for pose in right_hand_poses.values()], dtype=np.float32) + self._markers.visualize(translations=torch.tensor(joints_position, device=self._sim_device)) + + left_hand_pos = self._hands_controller.compute_left(left_hand_poses) + left_indexes = [self._hand_joint_names.index(name) for name in self._hands_controller.get_left_joint_names()] + left_retargeted = np.zeros(len(self._hand_joint_names), dtype=np.float32) + left_retargeted[left_indexes] = left_hand_pos + + right_hand_pos = self._hands_controller.compute_right(right_hand_poses) + right_indexes = [self._hand_joint_names.index(name) for name in self._hands_controller.get_right_joint_names()] + right_retargeted = np.zeros(len(self._hand_joint_names), dtype=np.float32) + right_retargeted[right_indexes] = right_hand_pos + + retargeted_hand_joints = left_retargeted + right_retargeted + + left_wrist_tensor = torch.tensor( + self._retarget_abs(left_wrist, is_left=True), dtype=torch.float32, device=self._sim_device + ) + right_wrist_tensor = torch.tensor( + self._retarget_abs(right_wrist, is_left=False), dtype=torch.float32, device=self._sim_device + ) + hand_joints_tensor = torch.tensor(retargeted_hand_joints, dtype=torch.float32, device=self._sim_device) + + return torch.cat([left_wrist_tensor, right_wrist_tensor, hand_joints_tensor]) + + def get_requirements(self) -> list[RetargeterBase.Requirement]: + return [RetargeterBase.Requirement.HAND_TRACKING] + + def _retarget_abs(self, wrist: np.ndarray, is_left: bool) -> np.ndarray: + wrist_pos = torch.tensor(wrist[:3], dtype=torch.float32) + wrist_quat = torch.tensor(wrist[3:], dtype=torch.float32) + + if is_left: + combined_quat = torch.tensor([0.7071, 0.0, 0.7071, 0.0], dtype=torch.float32) + else: + combined_quat = torch.tensor([0.0, -0.7071, 0.0, 0.7071], dtype=torch.float32) + + openxr_pose = PoseUtils.make_pose(wrist_pos, PoseUtils.matrix_from_quat(wrist_quat)) + transform_pose = PoseUtils.make_pose(torch.zeros(3), PoseUtils.matrix_from_quat(combined_quat)) + + result_pose = PoseUtils.pose_in_A_to_pose_in_B(transform_pose, openxr_pose) + pos, rot_mat = PoseUtils.unmake_pose(result_pose) + quat = PoseUtils.quat_from_matrix(rot_mat) + + return np.concatenate([pos.numpy(), quat.numpy()]) + + +@dataclass +class G1BrainCoUpperBodyRetargeterCfg(RetargeterCfg): + enable_visualization: bool = False + num_open_xr_hand_joints: int = 100 + hand_joint_names: list[str] | None = None + retargeter_type: type[RetargeterBase] = G1BrainCoUpperBodyRetargeter diff --git a/source/mindbot/mindbot/robot/test.py b/source/mindbot/mindbot/robot/test.py new file mode 100644 index 0000000..872f667 --- /dev/null +++ b/source/mindbot/mindbot/robot/test.py @@ -0,0 +1,153 @@ +#!/usr/bin/env python3 +""" +修复 USD 文件： +1. 将 ArticulationRootAPI 从 root_joint 移动到根 Xform Prim +用法: ./isaaclab.sh -p fix_usd.py +""" + +import argparse +from isaaclab.app import AppLauncher + +parser = argparse.ArgumentParser(description="修复 USD ArticulationRootAPI 位置") +parser.add_argument( + "--usd_path", + type=str, + default="/home/maic/xh/maic_usd_assets_moudle/robots/g1_with_brainco_hand/g1_29dof_mode_15_brainco_hand/g1_brainco.usd", +) +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() +args_cli.headless = True +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +# ============================================================ +import os +import shutil +from pxr import Usd, UsdPhysics, PhysxSchema + + +def fix_articulation_root(usd_path: str): + print("\n" + "="*60) + print("【修复】ArticulationRootAPI 位置修正") + print("="*60) + + # 1. 备份 + backup_path = usd_path + ".bak" + if not os.path.exists(backup_path): + shutil.copy2(usd_path, backup_path) + print(f" ✅ 已备份到: {backup_path}") + else: + print(f" ℹ️ 备份已存在: {backup_path}") + + # 2. 打开 Stage + stage = Usd.Stage.Open(usd_path) + default_prim = stage.GetDefaultPrim() + print(f"\n defaultPrim: {default_prim.GetPath()} [{default_prim.GetTypeName()}]") + + # 3. 找到并清理 root_joint 上的 ArticulationRootAPI + print(f"\n[Step 1] 查找错误位置的 ArticulationRootAPI ...") + fixed_remove = False + for prim in stage.Traverse(): + schemas = prim.GetAppliedSchemas() + if "PhysicsArticulationRootAPI" in schemas: + print(f" 发现 ArticulationRootAPI @ {prim.GetPath()} [{prim.GetTypeName()}]") + # 如果不是根 Xform，则移除 + if prim.GetPath() != default_prim.GetPath(): + print(f" ⚠️ 这不是根 Xform，需要移除...") + try: + # 移除 ArticulationRootAPI + prim.RemoveAPI(UsdPhysics.ArticulationRootAPI) + print(f" ✅ 已从 {prim.GetPath()} 移除 ArticulationRootAPI") + fixed_remove = True + except Exception as e: + # 某些版本用不同方式移除 + print(f" ⚠️ RemoveAPI 失败，尝试其他方式: {e}") + try: + prim.ClearAPI(UsdPhysics.ArticulationRootAPI) + print(f" ✅ 已用 ClearAPI 从 {prim.GetPath()} 移除") + fixed_remove = True + except Exception as e2: + print(f" ❌ 移除失败: {e2}") + else: + print(f" ✅ 位置正确，无需修改") + return + + # 4. 在根 Xform Prim 上添加 ArticulationRootAPI + print(f"\n[Step 2] 在根 Prim 上添加 ArticulationRootAPI ...") + if not default_prim.IsValid(): + print(f" ❌ defaultPrim 无效！") + return + + try: + UsdPhysics.ArticulationRootAPI.Apply(default_prim) + print(f" ✅ 已在 {default_prim.GetPath()} 上添加 ArticulationRootAPI") + except Exception as e: + print(f" ❌ 添加失败: {e}") + return + + # 5. 同时添加 PhysxArticulationAPI（Isaac Sim 需要） + print(f"\n[Step 3] 添加 PhysxArticulationAPI ...") + try: + PhysxSchema.PhysxArticulationAPI.Apply(default_prim) + print(f" ✅ 已在 {default_prim.GetPath()} 上添加 PhysxArticulationAPI") + except Exception as e: + print(f" ⚠️ PhysxArticulationAPI 添加失败（非必须）: {e}") + + # 6. 保存 + stage.Save() + print(f"\n ✅ 文件已保存: {usd_path}") + + # 7. 验证 + print(f"\n[验证] 重新检查 ...") + stage2 = Usd.Stage.Open(usd_path) + for prim in stage2.Traverse(): + if "PhysicsArticulationRootAPI" in prim.GetAppliedSchemas(): + print(f" ✅ ArticulationRootAPI @ {prim.GetPath()} [{prim.GetTypeName()}]") + + +def check_mimic_joints(usd_path: str): + """检查 MimicJoint 配置""" + print("\n" + "="*60) + print("【检查】PhysxMimicJointAPI 配置") + print("="*60) + + stage = Usd.Stage.Open(usd_path) + mimic_joints = [] + + for prim in stage.Traverse(): + schemas = prim.GetAppliedSchemas() + mimic_schemas = [s for s in schemas if "MimicJoint" in s] + if mimic_schemas: + mimic_joints.append((prim.GetPath(), prim.GetTypeName(), mimic_schemas)) + + if mimic_joints: + print(f"\n 找到 {len(mimic_joints)} 个 MimicJoint:") + for path, type_name, schemas in mimic_joints: + print(f" - {path} [{type_name}] {schemas}") + print(f"\n ℹ️ MimicJoint 需要 Articulation 正确建立才能工作") + print(f" ℹ️ 修复 ArticulationRootAPI 位置后，MimicJoint 错误应自动消失") + else: + print(f" ℹ️ 未找到 MimicJoint") + + +def main(): + usd_path = args_cli.usd_path + print(f"\n{'='*60}") + print(f" USD 修复工具") + print(f" 目标: {usd_path}") + print(f"{'='*60}") + + # 检查 MimicJoint + check_mimic_joints(usd_path) + + # 修复 ArticulationRootAPI + fix_articulation_root(usd_path) + + print(f"\n{'='*60}") + print(f" 修复完成！请重新运行任务验证") + print(f"{'='*60}\n") + + +if __name__ == "__main__": + main() + simulation_app.close() \ No newline at end of file diff --git a/source/mindbot/mindbot/robot/unitree_brainco.py b/source/mindbot/mindbot/robot/unitree_brainco.py index 048d33d..96f581f 100644 --- a/source/mindbot/mindbot/robot/unitree_brainco.py +++ b/source/mindbot/mindbot/robot/unitree_brainco.py @@ -20,10 +20,10 @@ from mindbot.utils.assets import MINDBOT_ASSETS_DIR G1_BRAINCO_ASSET_DIR = f"{MINDBOT_ASSETS_DIR}/robots/g1_with_brainco_hand" -"""Asset directory for the rewritten BrainCo-hand G1 robot.""" +"""Asset directory for the BrainCo-hand G1 robot.""" -G1_BRAINCO_USD_PATH = f"{G1_BRAINCO_ASSET_DIR}/g1_29dof_mode_15_brainco_hand.usd" +G1_BRAINCO_USD_PATH = f"{G1_BRAINCO_ASSET_DIR}/g1_29dof_mode_15_brainco_hand/g1_brainco.usd" """Default USD path for the BrainCo-hand G1 asset.""" @@ -43,7 +43,7 @@ G1_BRAINCO_LEFT_HAND_JOINT_NAMES = [ "left_thumb_metacarpal_joint", "left_thumb_proximal_joint", ] -"""Actuated left-hand joints in the rewritten BrainCo asset.""" +"""Actuated left-hand joints in the BrainCo asset.""" G1_BRAINCO_RIGHT_HAND_JOINT_NAMES = [ @@ -54,11 +54,11 @@ G1_BRAINCO_RIGHT_HAND_JOINT_NAMES = [ "right_thumb_metacarpal_joint", "right_thumb_proximal_joint", ] -"""Actuated right-hand joints in the rewritten BrainCo asset.""" +"""Actuated right-hand joints in the BrainCo asset.""" G1_BRAINCO_HAND_JOINT_NAMES = G1_BRAINCO_LEFT_HAND_JOINT_NAMES + G1_BRAINCO_RIGHT_HAND_JOINT_NAMES -"""Ordered actuated hand joints used by teleoperation and Pink IK action terms.""" +"""Ordered actuated hand joints used by teleoperation and retargeting.""" G1_BRAINCO_HAND_JOINT_PATTERNS = [ @@ -69,13 +69,13 @@ G1_BRAINCO_HAND_JOINT_PATTERNS = [ ".*_thumb_metacarpal_joint", ".*_thumb_proximal_joint", ] -"""Regex patterns that match actuated BrainCo finger joints in the rewritten USD.""" +"""Regex patterns that match actuated BrainCo finger joints in the USD.""" G1_BRAINCO_29DOF_CFG = ArticulationCfg( spawn=sim_utils.UsdFileCfg( usd_path=G1_BRAINCO_USD_PATH, - activate_contact_sensors=True, + activate_contact_sensors=False, rigid_props=sim_utils.RigidBodyPropertiesCfg( disable_gravity=False, retain_accelerations=False, @@ -103,7 +103,11 @@ G1_BRAINCO_29DOF_CFG = ArticulationCfg( "left_shoulder_roll_joint": 0.16, "right_shoulder_roll_joint": -0.16, ".*_elbow_joint": 0.50, - # ".*": 0.0, + # "left_index_tip_joint": 1.0, + # "left_middle_tip_joint": 1.0, + # "left_ring_tip_joint": 1.0, + # "left_pinky_tip_joint": 1.0, + }, joint_vel={".*": 0.0}, ), @@ -119,7 +123,7 @@ G1_BRAINCO_29DOF_CFG = ArticulationCfg( effort_limit={ ".*_hip_yaw_joint": 88.0, ".*_hip_roll_joint": 139.0, - ".*_hip_pitch_joint": 88.0, + ".*_hip_pitch_joint": 139.0, ".*_knee_joint": 139.0, }, velocity_limit={ @@ -149,8 +153,8 @@ G1_BRAINCO_29DOF_CFG = ArticulationCfg( "feet": DCMotorCfg( joint_names_expr=[".*_ankle_pitch_joint", ".*_ankle_roll_joint"], effort_limit={ - ".*_ankle_pitch_joint": 50.0, - ".*_ankle_roll_joint": 50.0, + ".*_ankle_pitch_joint": 35.0, + ".*_ankle_roll_joint": 35.0, }, velocity_limit={ ".*_ankle_pitch_joint": 37.0, @@ -171,8 +175,8 @@ G1_BRAINCO_29DOF_CFG = ArticulationCfg( joint_names_expr=["waist_.*_joint"], effort_limit={ "waist_yaw_joint": 88.0, - "waist_roll_joint": 50.0, - "waist_pitch_joint": 50.0, + "waist_roll_joint": 35.0, + "waist_pitch_joint": 35.0, }, velocity_limit={ "waist_yaw_joint": 32.0, @@ -180,14 +184,14 @@ G1_BRAINCO_29DOF_CFG = ArticulationCfg( "waist_pitch_joint": 37.0, }, stiffness={ - "waist_yaw_joint": 27.7928, - "waist_roll_joint": 54.2666, - "waist_pitch_joint": 53.3254, + "waist_yaw_joint": 27.7489, + "waist_roll_joint": 54.0995, + "waist_pitch_joint": 53.0524, }, damping={ "waist_yaw_joint": 0.0111, - "waist_roll_joint": 0.0217, - "waist_pitch_joint": 0.0213, + "waist_roll_joint": 0.0216, + "waist_pitch_joint": 0.0212, }, armature=0.001, ), @@ -210,22 +214,22 @@ G1_BRAINCO_29DOF_CFG = ArticulationCfg( }, velocity_limit=100.0, stiffness={ - ".*_shoulder_pitch_joint": 168.7152, - ".*_shoulder_roll_joint": 151.6808, - ".*_shoulder_yaw_joint": 141.6967, - ".*_elbow_joint": 88.9802, - ".*_wrist_roll_joint": 66.6865, - ".*_wrist_pitch_joint": 26.5734, - ".*_wrist_yaw_joint": 16.3066, + ".*_shoulder_pitch_joint": 149.8304, + ".*_shoulder_roll_joint": 124.0138, + ".*_shoulder_yaw_joint": 114.5362, + ".*_elbow_joint": 65.6598, + ".*_wrist_roll_joint": 46.9630, + ".*_wrist_pitch_joint": 17.4997, + ".*_wrist_yaw_joint": 10.2527, }, damping={ - ".*_shoulder_pitch_joint": 0.0675, - ".*_shoulder_roll_joint": 0.0607, - ".*_shoulder_yaw_joint": 0.0567, - ".*_elbow_joint": 0.0356, - ".*_wrist_roll_joint": 0.0267, - ".*_wrist_pitch_joint": 0.0106, - ".*_wrist_yaw_joint": 0.0065, + ".*_shoulder_pitch_joint": 0.0599, + ".*_shoulder_roll_joint": 0.0496, + ".*_shoulder_yaw_joint": 0.0458, + ".*_elbow_joint": 0.0263, + ".*_wrist_roll_joint": 0.0188, + ".*_wrist_pitch_joint": 0.0070, + ".*_wrist_yaw_joint": 0.0041, }, armature={ ".*_shoulder_.*": 0.001, @@ -261,19 +265,11 @@ G1_BRAINCO_CFG = G1_BRAINCO_29DOF_CFG G1_BRAINCO_FTP_CFG = G1_BRAINCO_29DOF_CFG.copy() G1_BRAINCO_FTP_CFG.spawn.activate_contact_sensors = True +G1_BRAINCO_FTP_CFG.spawn.rigid_props.disable_gravity = True G1_BRAINCO_FTP_CFG.spawn.articulation_props.fix_root_link = True G1_BRAINCO_FTP_CFG.init_state = ArticulationCfg.InitialStateCfg( pos=(0.0, 0.0, 1.0), - joint_pos={ - ".*_hip_pitch_joint": -0.10, - ".*_knee_joint": 0.30, - ".*_ankle_pitch_joint": -0.20, - ".*_shoulder_pitch_joint": 0.35, - "left_shoulder_roll_joint": 0.16, - "right_shoulder_roll_joint": -0.16, - ".*_elbow_joint": 0.50, - ".*": 0.0, - }, + joint_pos={".*": 0.0}, joint_vel={".*": 0.0}, ) G1_BRAINCO_FTP_CFG.actuators["arms"] = ImplicitActuatorCfg( diff --git a/source/mindbot/mindbot/robot/unitree_brainco_joint.txt b/source/mindbot/mindbot/robot/unitree_brainco_joint.txt new file mode 100644 index 0000000..50f6928 --- /dev/null +++ b/source/mindbot/mindbot/robot/unitree_brainco_joint.txt @@ -0,0 +1,742 @@ +============================================================ +开始扫描场景中的关节信息... +============================================================ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_hip_pitch_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/pelvis + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_hip_pitch_link + ├─ 限位 : [-144.9984 deg, 165.0004 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 24.844929 + ├─ 阻尼 : 0.009938 + ├─ 最大力 : 139.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_hip_roll_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_hip_pitch_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_hip_roll_link + ├─ 限位 : [-30.0001 deg, 170.0023 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 62.141331 + ├─ 阻尼 : 0.024857 + ├─ 最大力 : 139.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_hip_yaw_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_hip_roll_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_hip_yaw_link + ├─ 限位 : [-157.9988 deg, 157.9988 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 150.476929 + ├─ 阻尼 : 0.060191 + ├─ 最大力 : 88.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_knee_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_hip_yaw_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_knee_link + ├─ 限位 : [-5.0000 deg, 165.0004 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 230.089951 + ├─ 阻尼 : 0.092036 + ├─ 最大力 : 139.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_ankle_pitch_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_knee_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_ankle_pitch_link + ├─ 限位 : [-50.0003 deg, 30.0001 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 64.936783 + ├─ 阻尼 : 0.025975 + ├─ 最大力 : 35.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_ankle_roll_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_ankle_pitch_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_ankle_roll_link + ├─ 限位 : [-15.0000 deg, 15.0000 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 2.301954 + ├─ 阻尼 : 0.000921 + ├─ 最大力 : 35.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/pelvis_contour_joint + ├─ 类型 : PhysicsFixedJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/pelvis + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/pelvis_contour_link + ├─ 限位 : [无限制/非旋转关节, 无限制/非旋转关节] + └─ Drive : 未找到驱动器（被动 / 固定关节） +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_hip_pitch_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/pelvis + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_hip_pitch_link + ├─ 限位 : [-144.9984 deg, 165.0004 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 24.844933 + ├─ 阻尼 : 0.009938 + ├─ 最大力 : 139.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_hip_roll_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_hip_pitch_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_hip_roll_link + ├─ 限位 : [-170.0023 deg, 30.0001 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 62.141346 + ├─ 阻尼 : 0.024857 + ├─ 最大力 : 139.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_hip_yaw_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_hip_roll_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_hip_yaw_link + ├─ 限位 : [-157.9988 deg, 157.9988 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 150.476379 + ├─ 阻尼 : 0.060191 + ├─ 最大力 : 88.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_knee_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_hip_yaw_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_knee_link + ├─ 限位 : [-5.0000 deg, 165.0004 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 230.081039 + ├─ 阻尼 : 0.092032 + ├─ 最大力 : 139.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_ankle_pitch_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_knee_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_ankle_pitch_link + ├─ 限位 : [-50.0003 deg, 30.0001 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 64.936783 + ├─ 阻尼 : 0.025975 + ├─ 最大力 : 35.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_ankle_roll_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_ankle_pitch_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_ankle_roll_link + ├─ 限位 : [-15.0000 deg, 15.0000 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 2.301954 + ├─ 阻尼 : 0.000921 + ├─ 最大力 : 35.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/waist_yaw_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/pelvis + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/waist_yaw_link + ├─ 限位 : [-150.0004 deg, 150.0004 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 27.748882 + ├─ 阻尼 : 0.011100 + ├─ 最大力 : 88.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/waist_roll_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/waist_yaw_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/waist_roll_link + ├─ 限位 : [-29.7938 deg, 29.7938 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 54.099495 + ├─ 阻尼 : 0.021640 + ├─ 最大力 : 35.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/waist_pitch_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/waist_roll_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/torso_link + ├─ 限位 : [-29.7938 deg, 29.7938 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 53.052383 + ├─ 阻尼 : 0.021221 + ├─ 最大力 : 35.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/head_joint + ├─ 类型 : PhysicsFixedJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/torso_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/head_link + ├─ 限位 : [无限制/非旋转关节, 无限制/非旋转关节] + └─ Drive : 未找到驱动器（被动 / 固定关节） +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_shoulder_pitch_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/torso_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_shoulder_pitch_link + ├─ 限位 : [-176.9981 deg, 153.0026 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 149.831284 + ├─ 阻尼 : 0.059933 + ├─ 最大力 : 25.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_shoulder_roll_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_shoulder_pitch_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_shoulder_roll_link + ├─ 限位 : [-90.9972 deg, 129.0014 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 124.015877 + ├─ 阻尼 : 0.049606 + ├─ 最大力 : 25.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_shoulder_yaw_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_shoulder_roll_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_shoulder_yaw_link + ├─ 限位 : [-150.0004 deg, 150.0004 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 114.533615 + ├─ 阻尼 : 0.045813 + ├─ 最大力 : 25.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_elbow_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_shoulder_yaw_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_elbow_link + ├─ 限位 : [-60.0001 deg, 120.0003 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 65.659897 + ├─ 阻尼 : 0.026264 + ├─ 最大力 : 25.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_wrist_roll_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_elbow_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_wrist_roll_link + ├─ 限位 : [-113.0000 deg, 113.0000 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 46.960934 + ├─ 阻尼 : 0.018784 + ├─ 最大力 : 25.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_wrist_pitch_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_wrist_roll_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_wrist_pitch_link + ├─ 限位 : [-92.5000 deg, 92.5000 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 17.497646 + ├─ 阻尼 : 0.006999 + ├─ 最大力 : 5.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_wrist_yaw_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_wrist_pitch_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_wrist_yaw_link + ├─ 限位 : [-92.5000 deg, 92.5000 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 10.250670 + ├─ 阻尼 : 0.004100 + ├─ 最大力 : 5.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_base2_joint + ├─ 类型 : PhysicsFixedJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_wrist_yaw_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_base2_link + ├─ 限位 : [无限制/非旋转关节, 无限制/非旋转关节] + └─ Drive : 未找到驱动器（被动 / 固定关节） +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_base_joint + ├─ 类型 : PhysicsFixedJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_wrist_yaw_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_base_link + ├─ 限位 : [无限制/非旋转关节, 无限制/非旋转关节] + └─ Drive : 未找到驱动器（被动 / 固定关节） +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_index_proximal_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_base_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_index_proximal_Link + ├─ 限位 : [0.0000 deg, 84.0013 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 0.242971 + ├─ 阻尼 : 0.000097 + ├─ 最大力 : 2.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_index_distal_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_index_proximal_Link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_index_distal_Link + ├─ 限位 : [-19.4043 deg, 116.4258 deg] + └─ Drive : 未找到驱动器（被动 / 固定关节） +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_index_tip_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_index_distal_Link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_index_tip_Link + ├─ 限位 : [57.2958 deg, 57.2958 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 0.001386 + ├─ 阻尼 : 0.000001 + ├─ 最大力 : 1.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_middle_proximal_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_base_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_middle_proximal_Link + ├─ 限位 : [0.0000 deg, 84.0013 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 0.267033 + ├─ 阻尼 : 0.000107 + ├─ 最大力 : 2.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_middle_distal_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_middle_proximal_Link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_middle_distal_Link + ├─ 限位 : [-19.4043 deg, 116.4258 deg] + └─ Drive : 未找到驱动器（被动 / 固定关节） +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_middle_tip_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_middle_distal_Link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_middle_tip_Link + ├─ 限位 : [57.2958 deg, 57.2958 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 0.001733 + ├─ 阻尼 : 0.000001 + ├─ 最大力 : 1.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_pinky_proximal_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_base_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_pinky_proximal_Link + ├─ 限位 : [0.0000 deg, 84.0013 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 0.222033 + ├─ 阻尼 : 0.000089 + ├─ 最大力 : 2.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_pinky_distal_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_pinky_proximal_Link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_pinky_distal_Link + ├─ 限位 : [-19.4043 deg, 116.4258 deg] + └─ Drive : 未找到驱动器（被动 / 固定关节） +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_pinky_tip_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_pinky_distal_Link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_pinky_tip_Link + ├─ 限位 : [57.2958 deg, 57.2958 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 0.001199 + ├─ 阻尼 : 0.000000 + ├─ 最大力 : 1.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_ring_proximal_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_base_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_ring_proximal_Link + ├─ 限位 : [0.0000 deg, 84.0013 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 0.254636 + ├─ 阻尼 : 0.000102 + ├─ 最大力 : 2.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_ring_distal_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_ring_proximal_Link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_ring_distal_Link + ├─ 限位 : [-19.4043 deg, 116.4258 deg] + └─ Drive : 未找到驱动器（被动 / 固定关节） +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_ring_tip_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_ring_distal_Link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_ring_tip_Link + ├─ 限位 : [57.2958 deg, 57.2958 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 0.001660 + ├─ 阻尼 : 0.000001 + ├─ 最大力 : 1.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_thumb_metacarpal_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_base_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_thumb_metacarpal_Link + ├─ 限位 : [0.0000 deg, 86.9979 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 0.312518 + ├─ 阻尼 : 0.000125 + ├─ 最大力 : 0.500000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_thumb_proximal_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_thumb_metacarpal_Link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_thumb_proximal_Link + ├─ 限位 : [0.0000 deg, 60.0001 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 0.131862 + ├─ 阻尼 : 0.000053 + ├─ 最大力 : 1.100000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_thumb_distal_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_thumb_proximal_Link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_thumb_distal_Link + ├─ 限位 : [-12.0000 deg, 72.0002 deg] + └─ Drive : 未找到驱动器（被动 / 固定关节） +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/left_thumb_tip_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/left_thumb_distal_Link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/left_thumb_tip_Link + ├─ 限位 : [0.0000 deg, 0.0000 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 0.000776 + ├─ 阻尼 : 0.000000 + ├─ 最大力 : 340282346638528859811704183484516925440.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/logo_joint + ├─ 类型 : PhysicsFixedJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/torso_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/logo_link + ├─ 限位 : [无限制/非旋转关节, 无限制/非旋转关节] + └─ Drive : 未找到驱动器（被动 / 固定关节） +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_shoulder_pitch_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/torso_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_shoulder_pitch_link + ├─ 限位 : [-176.9981 deg, 153.0026 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 149.829498 + ├─ 阻尼 : 0.059932 + ├─ 最大力 : 25.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_shoulder_roll_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_shoulder_pitch_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_shoulder_roll_link + ├─ 限位 : [-129.0014 deg, 90.9972 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 124.011757 + ├─ 阻尼 : 0.049605 + ├─ 最大力 : 25.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_shoulder_yaw_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_shoulder_roll_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_shoulder_yaw_link + ├─ 限位 : [-150.0004 deg, 150.0004 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 114.538696 + ├─ 阻尼 : 0.045815 + ├─ 最大力 : 25.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_elbow_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_shoulder_yaw_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_elbow_link + ├─ 限位 : [-60.0001 deg, 120.0003 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 65.659744 + ├─ 阻尼 : 0.026264 + ├─ 最大力 : 25.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_wrist_roll_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_elbow_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_wrist_roll_link + ├─ 限位 : [-113.0000 deg, 113.0000 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 46.965149 + ├─ 阻尼 : 0.018786 + ├─ 最大力 : 25.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_wrist_pitch_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_wrist_roll_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_wrist_pitch_link + ├─ 限位 : [-92.5000 deg, 92.5000 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 17.501703 + ├─ 阻尼 : 0.007001 + ├─ 最大力 : 5.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_wrist_yaw_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_wrist_pitch_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_wrist_yaw_link + ├─ 限位 : [-92.5000 deg, 92.5000 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 10.254728 + ├─ 阻尼 : 0.004102 + ├─ 最大力 : 5.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_base2_joint + ├─ 类型 : PhysicsFixedJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_wrist_yaw_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_base2_link + ├─ 限位 : [无限制/非旋转关节, 无限制/非旋转关节] + └─ Drive : 未找到驱动器（被动 / 固定关节） +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_base_joint + ├─ 类型 : PhysicsFixedJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_wrist_yaw_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_base_link + ├─ 限位 : [无限制/非旋转关节, 无限制/非旋转关节] + └─ Drive : 未找到驱动器（被动 / 固定关节） +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_index_proximal_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_base_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_index_proximal_link + ├─ 限位 : [0.0000 deg, 84.0013 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 0.242957 + ├─ 阻尼 : 0.000097 + ├─ 最大力 : 2.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_index_distal_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_index_proximal_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_index_distal_link + ├─ 限位 : [-19.4043 deg, 116.4258 deg] + └─ Drive : 未找到驱动器（被动 / 固定关节） +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_index_tip_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_index_distal_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_index_tip + ├─ 限位 : [0.0000 deg, 0.0000 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 0.001386 + ├─ 阻尼 : 0.000001 + ├─ 最大力 : 340282346638528859811704183484516925440.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_middle_proximal_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_base_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_middle_proximal_link + ├─ 限位 : [0.0000 deg, 84.0013 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 0.267012 + ├─ 阻尼 : 0.000107 + ├─ 最大力 : 2.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_middle_distal_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_middle_proximal_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_middle_distal_link + ├─ 限位 : [-19.4043 deg, 116.4258 deg] + └─ Drive : 未找到驱动器（被动 / 固定关节） +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_middle_tip_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_middle_distal_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_middle_tip + ├─ 限位 : [0.0000 deg, 0.0000 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 0.001733 + ├─ 阻尼 : 0.000001 + ├─ 最大力 : 340282346638528859811704183484516925440.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_pinky_proximal_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_base_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_pinky_proximal_link + ├─ 限位 : [0.0000 deg, 84.0013 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 0.222046 + ├─ 阻尼 : 0.000089 + ├─ 最大力 : 2.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_pinky_distal_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_pinky_proximal_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_pinky_distal_link + ├─ 限位 : [-19.4043 deg, 116.4258 deg] + └─ Drive : 未找到驱动器（被动 / 固定关节） +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_pinky_tip_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_pinky_distal_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_pinky_tip + ├─ 限位 : [0.0000 deg, 0.0000 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 0.001199 + ├─ 阻尼 : 0.000000 + ├─ 最大力 : 340282346638528859811704183484516925440.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_ring_proximal_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_base_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_ring_proximal_link + ├─ 限位 : [0.0000 deg, 84.0013 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 0.254628 + ├─ 阻尼 : 0.000102 + ├─ 最大力 : 2.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_ring_distal_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_ring_proximal_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_ring_distal_link + ├─ 限位 : [-19.4043 deg, 116.4258 deg] + └─ Drive : 未找到驱动器（被动 / 固定关节） +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_ring_tip_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_ring_distal_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_ring_tip + ├─ 限位 : [0.0000 deg, 0.0000 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 0.001660 + ├─ 阻尼 : 0.000001 + ├─ 最大力 : 340282346638528859811704183484516925440.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_thumb_metacarpal_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_base_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_thumb_metacarpal_link + ├─ 限位 : [0.0000 deg, 86.9979 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 0.320300 + ├─ 阻尼 : 0.000128 + ├─ 最大力 : 0.500000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_thumb_proximal_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_thumb_metacarpal_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_thumb_proximal_link + ├─ 限位 : [0.0000 deg, 60.0001 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 0.131847 + ├─ 阻尼 : 0.000053 + ├─ 最大力 : 1.100000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_thumb_distal_joint + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_thumb_proximal_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_thumb_distal_link + ├─ 限位 : [-12.0000 deg, 72.0002 deg] + └─ Drive : 未找到驱动器（被动 / 固定关节） +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/joints/right_thumb_tip + ├─ 类型 : PhysicsRevoluteJoint + ├─ 父节点 : /g1_29dof_mode_15_brainco_hand/right_thumb_distal_link + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/right_thumb_tip + ├─ 限位 : [0.0000 deg, 0.0000 deg] + ├─ Drive : token = 'angular' + ├─ 刚度 : 0.000772 + ├─ 阻尼 : 0.000000 + ├─ 最大力 : 340282346638528859811704183484516925440.000000 N·m + ├─ 目标位置: 0.000000 deg + └─ 目标速度: 0.000000 +------------------------------------------------------------ +关节路径: /g1_29dof_mode_15_brainco_hand/root_joint + ├─ 类型 : PhysicsFixedJoint + ├─ 父节点 : World (None) + ├─ 子节点 : /g1_29dof_mode_15_brainco_hand/pelvis + ├─ 限位 : [无限制/非旋转关节, 无限制/非旋转关节] + └─ Drive : 未找到驱动器（被动 / 固定关节） +------------------------------------------------------------ +扫描完毕！共找到 69 个关节。 \ No newline at end of file diff --git a/source/mindbot/mindbot/tasks/manager_based/il/locomanipulation/pick_place/__init__.py b/source/mindbot/mindbot/tasks/manager_based/il/locomanipulation/pick_place/__init__.py index ff7e927..d874d54 100644 --- a/source/mindbot/mindbot/tasks/manager_based/il/locomanipulation/pick_place/__init__.py +++ b/source/mindbot/mindbot/tasks/manager_based/il/locomanipulation/pick_place/__init__.py @@ -9,7 +9,14 @@ import gymnasium as gym import os -from . import agents, fixed_base_upper_body_ik_g1_env_cfg, locomanipulation_g1_env_cfg +from . import ( + agents, + fixed_base_upper_body_ik_g1_env_cfg, + fixed_base_upper_body_ik_g1_brainco_env_cfg, + locomanipulation_g1_env_cfg, + locomanipulation_g1_brainco_env_cfg, +) + gym.register( id="Isaac-PickPlace-Locomanipulation-G1-Abs-v0", @@ -21,6 +28,17 @@ gym.register( disable_env_checker=True, ) +gym.register( + id="Isaac-PickPlace-Locomanipulation-G1-BrainCo-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": locomanipulation_g1_brainco_env_cfg.LocomanipulationG1EnvCfg, + "robomimic_bc_cfg_entry_point": os.path.join(agents.__path__[0], "robomimic/bc_rnn_low_dim.json"), + }, + disable_env_checker=True, +) + + gym.register( id="Isaac-PickPlace-FixedBaseUpperBodyIK-G1-Abs-v0", entry_point="isaaclab.envs:ManagerBasedRLEnv", diff --git a/source/mindbot/mindbot/tasks/manager_based/il/locomanipulation/pick_place/configs/pink_controller_brainco_cfg.py b/source/mindbot/mindbot/tasks/manager_based/il/locomanipulation/pick_place/configs/pink_controller_brainco_cfg.py new file mode 100644 index 0000000..f905927 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/locomanipulation/pick_place/configs/pink_controller_brainco_cfg.py @@ -0,0 +1,103 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""BrainCo-specific configuration for the G1 pink controller. + +This module mirrors the generic G1 pink IK configuration but swaps in the +BrainCo-hand URDF frame names and the BrainCo hand joint list. +""" + +from isaaclab.controllers.pink_ik.local_frame_task import LocalFrameTask +from isaaclab.controllers.pink_ik.null_space_posture_task import NullSpacePostureTask +from isaaclab.controllers.pink_ik.pink_ik_cfg import PinkIKControllerCfg +from isaaclab.envs.mdp.actions.pink_actions_cfg import PinkInverseKinematicsActionCfg + + +G1_BRAINCO_URDF_FRAME_PREFIX = "g1_29dof_mode_15_brainco_hand" +"""Frame prefix generated from the BrainCo G1 URDF robot name.""" + + +G1_BRAINCO_UPPER_BODY_IK_CONTROLLER_CFG = PinkIKControllerCfg( + articulation_name="robot", + base_link_name="pelvis", + num_hand_joints=12, + show_ik_warnings=True, + fail_on_joint_limit_violation=False, + variable_input_tasks=[ + LocalFrameTask( + f"{G1_BRAINCO_URDF_FRAME_PREFIX}_left_wrist_yaw_link", + base_link_frame_name=f"{G1_BRAINCO_URDF_FRAME_PREFIX}_pelvis", + position_cost=8.0, + orientation_cost=2.0, + lm_damping=10, + gain=0.5, + ), + LocalFrameTask( + f"{G1_BRAINCO_URDF_FRAME_PREFIX}_right_wrist_yaw_link", + base_link_frame_name=f"{G1_BRAINCO_URDF_FRAME_PREFIX}_pelvis", + position_cost=8.0, + orientation_cost=2.0, + lm_damping=10, + gain=0.5, + ), + NullSpacePostureTask( + cost=0.5, + lm_damping=1, + controlled_frames=[ + f"{G1_BRAINCO_URDF_FRAME_PREFIX}_left_wrist_yaw_link", + f"{G1_BRAINCO_URDF_FRAME_PREFIX}_right_wrist_yaw_link", + ], + controlled_joints=[ + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "waist_yaw_joint", + "waist_pitch_joint", + "waist_roll_joint", + ], + gain=0.3, + ), + ], + fixed_input_tasks=[], +) +"""Pink IK controller configuration for the BrainCo-hand G1 upper body.""" + + +G1_BRAINCO_UPPER_BODY_IK_ACTION_CFG = PinkInverseKinematicsActionCfg( + pink_controlled_joint_names=[ + ".*_shoulder_pitch_joint", + ".*_shoulder_roll_joint", + ".*_shoulder_yaw_joint", + ".*_elbow_joint", + ".*_wrist_pitch_joint", + ".*_wrist_roll_joint", + ".*_wrist_yaw_joint", + "waist_.*_joint", + ], + hand_joint_names=[ + "left_index_proximal_joint", + "left_middle_proximal_joint", + "left_ring_proximal_joint", + "left_pinky_proximal_joint", + "left_thumb_metacarpal_joint", + "left_thumb_proximal_joint", + "right_index_proximal_joint", + "right_middle_proximal_joint", + "right_ring_proximal_joint", + "right_pinky_proximal_joint", + "right_thumb_metacarpal_joint", + "right_thumb_proximal_joint", + ], + target_eef_link_names={ + "left_wrist": "left_wrist_yaw_link", + "right_wrist": "right_wrist_yaw_link", + }, + asset_name="robot", + controller=G1_BRAINCO_UPPER_BODY_IK_CONTROLLER_CFG, +) +"""Pink IK action configuration for the BrainCo-hand G1 upper body.""" diff --git a/source/mindbot/mindbot/tasks/manager_based/il/locomanipulation/pick_place/configs/pink_controller_cfg.py b/source/mindbot/mindbot/tasks/manager_based/il/locomanipulation/pick_place/configs/pink_controller_cfg.py index 488d22c..1e5a11f 100644 --- a/source/mindbot/mindbot/tasks/manager_based/il/locomanipulation/pick_place/configs/pink_controller_cfg.py +++ b/source/mindbot/mindbot/tasks/manager_based/il/locomanipulation/pick_place/configs/pink_controller_cfg.py @@ -124,3 +124,142 @@ The configuration includes: - Hand joint names for additional control - Reference to the pink IK controller configuration """ + + +## +# Pink IK Controller Configuration for BrainCo G1 +## + +G1_BRAINCO_URDF_FRAME_PREFIX = "g1_29dof_mode_15_brainco_hand" +"""Frame prefix generated from the BrainCo G1 URDF robot name.""" + + +G1_BRAINCO_UPPER_BODY_IK_CONTROLLER_CFG = PinkIKControllerCfg( + articulation_name="robot", + base_link_name="pelvis", + num_hand_joints=12, + show_ik_warnings=True, + fail_on_joint_limit_violation=False, + variable_input_tasks=[ + LocalFrameTask( + "left_wrist_yaw_link", + base_link_frame_name="pelvis", + position_cost=8.0, + orientation_cost=2.0, + lm_damping=10, + gain=0.5, + ), + LocalFrameTask( + "right_wrist_yaw_link", + base_link_frame_name="pelvis", + position_cost=8.0, + orientation_cost=2.0, + lm_damping=10, + gain=0.5, + ), + NullSpacePostureTask( + cost=0.5, + lm_damping=1, + controlled_frames=[ + "left_wrist_yaw_link", + "right_wrist_yaw_link", + ], + controlled_joints=[ + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "waist_yaw_joint", + "waist_pitch_joint", + "waist_roll_joint", + ], + gain=0.3, + ), + ], + fixed_input_tasks=[], +) + +# G1_BRAINCO_UPPER_BODY_IK_CONTROLLER_CFG = PinkIKControllerCfg( +# articulation_name="robot", +# base_link_name="pelvis", +# num_hand_joints=12, +# show_ik_warnings=True, +# fail_on_joint_limit_violation=False, +# variable_input_tasks=[ +# LocalFrameTask( +# f"{G1_BRAINCO_URDF_FRAME_PREFIX}_left_wrist_yaw_link", +# base_link_frame_name=f"{G1_BRAINCO_URDF_FRAME_PREFIX}_pelvis", +# position_cost=8.0, # [cost] / [m] +# orientation_cost=2.0, # [cost] / [rad] +# lm_damping=10, # dampening for solver for step jumps +# gain=0.5, +# ), +# LocalFrameTask( +# f"{G1_BRAINCO_URDF_FRAME_PREFIX}_right_wrist_yaw_link", +# base_link_frame_name=f"{G1_BRAINCO_URDF_FRAME_PREFIX}_pelvis", +# position_cost=8.0, # [cost] / [m] +# orientation_cost=2.0, # [cost] / [rad] +# lm_damping=10, # dampening for solver for step jumps +# gain=0.5, +# ), +# NullSpacePostureTask( +# cost=0.5, +# lm_damping=1, +# controlled_frames=[ +# f"{G1_BRAINCO_URDF_FRAME_PREFIX}_left_wrist_yaw_link", +# f"{G1_BRAINCO_URDF_FRAME_PREFIX}_right_wrist_yaw_link", +# ], +# controlled_joints=[ +# "left_shoulder_pitch_joint", +# "left_shoulder_roll_joint", +# "left_shoulder_yaw_joint", +# "right_shoulder_pitch_joint", +# "right_shoulder_roll_joint", +# "right_shoulder_yaw_joint", +# "waist_yaw_joint", +# "waist_pitch_joint", +# "waist_roll_joint", +# ], +# gain=0.3, +# ), +# ], +# fixed_input_tasks=[], +# ) +"""Configuration for the BrainCo G1 pink IK controller.""" + + +G1_BRAINCO_UPPER_BODY_IK_ACTION_CFG = PinkInverseKinematicsActionCfg( + pink_controlled_joint_names=[ + ".*_shoulder_pitch_joint", + ".*_shoulder_roll_joint", + ".*_shoulder_yaw_joint", + ".*_elbow_joint", + ".*_wrist_pitch_joint", + ".*_wrist_roll_joint", + ".*_wrist_yaw_joint", + "waist_.*_joint", + ], + hand_joint_names=[ + "left_index_proximal_joint", + "left_middle_proximal_joint", + "left_ring_proximal_joint", + "left_pinky_proximal_joint", + "left_thumb_metacarpal_joint", + "left_thumb_proximal_joint", + "right_index_proximal_joint", + "right_middle_proximal_joint", + "right_ring_proximal_joint", + "right_pinky_proximal_joint", + "right_thumb_metacarpal_joint", + "right_thumb_proximal_joint", + ], + target_eef_link_names={ + "left_wrist": "left_wrist_yaw_link", + "right_wrist": "right_wrist_yaw_link", + }, + asset_name="robot", + controller=G1_BRAINCO_UPPER_BODY_IK_CONTROLLER_CFG, +) +"""Configuration for the BrainCo G1 pink IK action.""" diff --git a/source/mindbot/mindbot/tasks/manager_based/il/locomanipulation/pick_place/fixed_base_upper_body_ik_g1_brainco_env_cfg.py b/source/mindbot/mindbot/tasks/manager_based/il/locomanipulation/pick_place/fixed_base_upper_body_ik_g1_brainco_env_cfg.py new file mode 100644 index 0000000..eb88a58 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/locomanipulation/pick_place/fixed_base_upper_body_ik_g1_brainco_env_cfg.py @@ -0,0 +1,217 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +import isaaclab.envs.mdp as base_mdp +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.devices.device_base import DevicesCfg +from isaaclab.devices.openxr import OpenXRDeviceCfg, XrCfg +from isaaclab.devices.openxr.retargeters.humanoid.unitree.trihand.g1_upper_body_retargeter import ( + G1TriHandUpperBodyRetargeterCfg, +) +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR, retrieve_file_path + +from mindbot.tasks.manager_based.il.locomanipulation.pick_place import mdp as locomanip_mdp +from mindbot.tasks.manager_based.il.manipulation.pick_place import mdp as manip_mdp + +from isaaclab_assets.robots.unitree import G1_29DOF_CFG +from mindbot.robot.unitree_brainco import G1_BRAINCO_29DOF_CFG + +from mindbot.tasks.manager_based.il.locomanipulation.pick_place.configs.pink_controller_cfg import ( # isort: skip + G1_UPPER_BODY_IK_ACTION_CFG, +) + +# from mindbot.tasks.manager_based.il. + +## +# Scene definition +## +@configclass +class FixedBaseUpperBodyIKG1SceneCfg(InteractiveSceneCfg): + """Scene configuration for fixed base upper body IK environment with G1 robot. + + This configuration sets up the G1 humanoid robot with fixed pelvis and legs, + allowing only arm manipulation while the base remains stationary. The robot is + controlled using upper body IK. + """ + + # Table + packing_table = AssetBaseCfg( + prim_path="/World/envs/env_.*/PackingTable", + init_state=AssetBaseCfg.InitialStateCfg(pos=[0.0, 0.55, -0.3], rot=[1.0, 0.0, 0.0, 0.0]), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/PackingTable/packing_table.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + ), + ) + + object = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Object", + init_state=RigidObjectCfg.InitialStateCfg(pos=[-0.35, 0.45, 0.6996], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/pick_place_task/pick_place_assets/steering_wheel.usd", + scale=(0.75, 0.75, 0.75), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + ), + ) + + # Unitree G1 Humanoid robot - fixed base configuration + robot: ArticulationCfg = G1_BRAINCO_29DOF_CFG + + # Ground plane + ground = AssetBaseCfg( + prim_path="/World/GroundPlane", + spawn=GroundPlaneCfg(), + ) + + # Lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), + ) + + def __post_init__(self): + """Post initialization.""" + # Set the robot to fixed base + self.robot.spawn.articulation_props.fix_root_link = True + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + upper_body_ik = G1_UPPER_BODY_IK_ACTION_CFG + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP. + This class is required by the environment configuration but not used in this implementation + """ + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group with state values.""" + + actions = ObsTerm(func=manip_mdp.last_action) + robot_joint_pos = ObsTerm( + func=base_mdp.joint_pos, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + robot_root_pos = ObsTerm(func=base_mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("robot")}) + robot_root_rot = ObsTerm(func=base_mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("robot")}) + object_pos = ObsTerm(func=base_mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("object")}) + object_rot = ObsTerm(func=base_mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("object")}) + robot_links_state = ObsTerm(func=manip_mdp.get_all_robot_link_state) + + left_eef_pos = ObsTerm(func=manip_mdp.get_eef_pos, params={"link_name": "left_wrist_yaw_link"}) + left_eef_quat = ObsTerm(func=manip_mdp.get_eef_quat, params={"link_name": "left_wrist_yaw_link"}) + right_eef_pos = ObsTerm(func=manip_mdp.get_eef_pos, params={"link_name": "right_wrist_yaw_link"}) + right_eef_quat = ObsTerm(func=manip_mdp.get_eef_quat, params={"link_name": "right_wrist_yaw_link"}) + + hand_joint_state = ObsTerm(func=manip_mdp.get_robot_joint_state, params={"joint_names": [".*_hand.*"]}) + head_joint_state = ObsTerm(func=manip_mdp.get_robot_joint_state, params={"joint_names": []}) + + object = ObsTerm( + func=manip_mdp.object_obs, + params={"left_eef_link_name": "left_wrist_yaw_link", "right_eef_link_name": "right_wrist_yaw_link"}, + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=locomanip_mdp.time_out, time_out=True) + + object_dropping = DoneTerm( + func=base_mdp.root_height_below_minimum, params={"minimum_height": 0.5, "asset_cfg": SceneEntityCfg("object")} + ) + + success = DoneTerm(func=manip_mdp.task_done_pick_place, params={"task_link_name": "right_wrist_yaw_link"}) + + +## +# MDP settings +## + + +@configclass +class FixedBaseUpperBodyIKG1EnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the G1 fixed base upper body IK environment. + + This environment is designed for manipulation tasks where the G1 humanoid robot + has a fixed pelvis and legs, allowing only arm and hand movements for manipulation. The robot is + controlled using upper body IK. + """ + + # Scene settings + scene: FixedBaseUpperBodyIKG1SceneCfg = FixedBaseUpperBodyIKG1SceneCfg( + num_envs=1, env_spacing=2.5, replicate_physics=True + ) + # MDP settings + terminations: TerminationsCfg = TerminationsCfg() + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + + # Unused managers + commands = None + rewards = None + curriculum = None + + # Position of the XR anchor in the world frame + xr: XrCfg = XrCfg( + anchor_pos=(0.0, 0.0, -0.45), + anchor_rot=(1.0, 0.0, 0.0, 0.0), + ) + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 4 + self.episode_length_s = 20.0 + # simulation settings + self.sim.dt = 1 / 200 # 200Hz + self.sim.render_interval = 2 + + # Set the URDF and mesh paths for the IK controller + urdf_omniverse_path = f"{ISAACLAB_NUCLEUS_DIR}/Controllers/LocomanipulationAssets/unitree_g1_kinematics_asset/g1_29dof_with_hand_only_kinematics.urdf" # noqa: E501 + + # Retrieve local paths for the URDF and mesh files. Will be cached for call after the first time. + self.actions.upper_body_ik.controller.urdf_path = retrieve_file_path(urdf_omniverse_path) + + self.teleop_devices = DevicesCfg( + devices={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + G1TriHandUpperBodyRetargeterCfg( + enable_visualization=True, + # OpenXR hand tracking has 26 joints per hand + num_open_xr_hand_joints=2 * 26, + sim_device=self.sim.device, + hand_joint_names=self.actions.upper_body_ik.hand_joint_names, + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + } + ) diff --git a/source/mindbot/mindbot/tasks/manager_based/il/locomanipulation/pick_place/locomanipulation_g1_brainco_env_cfg.py b/source/mindbot/mindbot/tasks/manager_based/il/locomanipulation/pick_place/locomanipulation_g1_brainco_env_cfg.py new file mode 100644 index 0000000..e648a62 --- /dev/null +++ b/source/mindbot/mindbot/tasks/manager_based/il/locomanipulation/pick_place/locomanipulation_g1_brainco_env_cfg.py @@ -0,0 +1,339 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import isaaclab.envs.mdp as base_mdp +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.devices.device_base import DevicesCfg +from isaaclab.devices.openxr import OpenXRDeviceCfg, XrCfg +from isaaclab.devices.openxr.retargeters.humanoid.unitree.g1_lower_body_standing import G1LowerBodyStandingRetargeterCfg +from isaaclab.devices.openxr.retargeters.humanoid.unitree.g1_motion_controller_locomotion import ( + G1LowerBodyStandingMotionControllerRetargeterCfg, +) +from isaaclab.devices.openxr.retargeters.humanoid.unitree.trihand.g1_upper_body_motion_ctrl_retargeter import ( + G1TriHandUpperBodyMotionControllerRetargeterCfg, +) +from isaaclab.devices.openxr.retargeters.humanoid.unitree.trihand.g1_upper_body_retargeter import ( + G1TriHandUpperBodyRetargeterCfg, +) +from mindbot.devices.openxr.retargeters.unitree.g1_upper_body_brainco_retargeter import ( + G1BrainCoUpperBodyRetargeterCfg, +) +from mindbot.devices.openxr.retargeters.unitree.g1_upper_body_brainco_motion_ctrl_retargeter import ( + G1BrainCoUpperBodyMotionControllerRetargeterCfg, +) + + +from isaaclab.devices.openxr.xr_cfg import XrAnchorRotationMode +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR, retrieve_file_path + +from mindbot.tasks.manager_based.il.locomanipulation.pick_place import mdp as locomanip_mdp +from mindbot.tasks.manager_based.il.locomanipulation.pick_place.configs.action_cfg import AgileBasedLowerBodyActionCfg +from mindbot.tasks.manager_based.il.locomanipulation.pick_place.configs.agile_locomotion_observation_cfg import ( + AgileTeacherPolicyObservationsCfg, +) +from mindbot.tasks.manager_based.il.manipulation.pick_place import mdp as manip_mdp + +# from isaaclab_assets.robots.unitree import G1_29DOF_CFG +from mindbot.robot.unitree_brainco import G1_BRAINCO_29DOF_CFG + +from mindbot.tasks.manager_based.il.locomanipulation.pick_place.configs.pink_controller_cfg import ( # isort: skip + G1_UPPER_BODY_IK_ACTION_CFG, G1_BRAINCO_UPPER_BODY_IK_ACTION_CFG +) +# from mindbot.tasks.manager_based.il.locomanipulation.pick_place.configs.pink_controller_brainco_cfg import ( +# G1_BRAINCO_UPPER_BODY_IK_CONTROLLER_CFG, +# ) + + +from mindbot.utils.assets import MINDBOT_ASSETS_DIR + +## +# Scene definition +## +@configclass +class LocomanipulationG1SceneCfg(InteractiveSceneCfg): + """Scene configuration for locomanipulation environment with G1 robot. + + This configuration sets up the G1 humanoid robot for locomanipulation tasks, + allowing both locomotion and manipulation capabilities. The robot can move its + base and use its arms for manipulation tasks. + """ + + # Table + packing_table = AssetBaseCfg( + prim_path="/World/envs/env_.*/PackingTable", + init_state=AssetBaseCfg.InitialStateCfg(pos=[0.0, 0.55, -0.3], rot=[1.0, 0.0, 0.0, 0.0]), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/PackingTable/packing_table.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + ), + ) + + object = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Object", + init_state=RigidObjectCfg.InitialStateCfg(pos=[-0.35, 0.45, 0.6996], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/pick_place_task/pick_place_assets/steering_wheel.usd", + scale=(0.75, 0.75, 0.75), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + ), + ) + + # Humanoid robot w/ arms higher + robot: ArticulationCfg = G1_BRAINCO_29DOF_CFG + + # Ground plane + ground = AssetBaseCfg( + prim_path="/World/GroundPlane", + spawn=GroundPlaneCfg(), + ) + + # Lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), + ) + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + upper_body_ik = G1_BRAINCO_UPPER_BODY_IK_ACTION_CFG + + lower_body_joint_pos = AgileBasedLowerBodyActionCfg( + asset_name="robot", + joint_names=[ + ".*_hip_.*_joint", + ".*_knee_joint", + ".*_ankle_.*_joint", + ], + policy_output_scale=0.25, + obs_group_name="lower_body_policy", # need to be the same name as the on in ObservationCfg + policy_path=f"{ISAACLAB_NUCLEUS_DIR}/Policies/Agile/agile_locomotion.pt", + ) + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP. + This class is required by the environment configuration but not used in this implementation + """ + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group with state values.""" + + actions = ObsTerm(func=manip_mdp.last_action) + robot_joint_pos = ObsTerm( + func=base_mdp.joint_pos, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + robot_root_pos = ObsTerm(func=base_mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("robot")}) + robot_root_rot = ObsTerm(func=base_mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("robot")}) + object_pos = ObsTerm(func=base_mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("object")}) + object_rot = ObsTerm(func=base_mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("object")}) + robot_links_state = ObsTerm(func=manip_mdp.get_all_robot_link_state) + + left_eef_pos = ObsTerm(func=manip_mdp.get_eef_pos, params={"link_name": "left_wrist_yaw_link"}) + left_eef_quat = ObsTerm(func=manip_mdp.get_eef_quat, params={"link_name": "left_wrist_yaw_link"}) + right_eef_pos = ObsTerm(func=manip_mdp.get_eef_pos, params={"link_name": "right_wrist_yaw_link"}) + right_eef_quat = ObsTerm(func=manip_mdp.get_eef_quat, params={"link_name": "right_wrist_yaw_link"}) + + # hand_joint_state = ObsTerm(func=manip_mdp.get_robot_joint_state, params={"joint_names": [".*_hand.*"]}) + hand_joint_state = ObsTerm( + func=manip_mdp.get_robot_joint_state, + params={ + "joint_names": [ + ".*_index_proximal_joint", + ".*_middle_proximal_joint", + ".*_ring_proximal_joint", + ".*_pinky_proximal_joint", + ".*_thumb_metacarpal_joint", + ".*_thumb_proximal_joint", + ] + }, + ) + + + object = ObsTerm( + func=manip_mdp.object_obs, + params={"left_eef_link_name": "left_wrist_yaw_link", "right_eef_link_name": "right_wrist_yaw_link"}, + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + # observation groups + policy: PolicyCfg = PolicyCfg() + lower_body_policy: AgileTeacherPolicyObservationsCfg = AgileTeacherPolicyObservationsCfg() + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=locomanip_mdp.time_out, time_out=True) + + object_dropping = DoneTerm( + func=base_mdp.root_height_below_minimum, params={"minimum_height": 0.5, "asset_cfg": SceneEntityCfg("object")} + ) + + success = DoneTerm(func=manip_mdp.task_done_pick_place, params={"task_link_name": "right_wrist_yaw_link"}) + + +## +# MDP settings +## + + +@configclass +class LocomanipulationG1EnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the G1 locomanipulation environment. + + This environment is designed for locomanipulation tasks where the G1 humanoid robot + can perform both locomotion and manipulation simultaneously. The robot can move its + base and use its arms for manipulation tasks, enabling complex mobile manipulation + behaviors. + """ + + # Scene settings + scene: LocomanipulationG1SceneCfg = LocomanipulationG1SceneCfg(num_envs=1, env_spacing=2.5, replicate_physics=True) + # MDP settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + commands = None + terminations: TerminationsCfg = TerminationsCfg() + + # Unused managers + rewards = None + curriculum = None + + # Position of the XR anchor in the world frame + xr: XrCfg = XrCfg( + anchor_pos=(0.0, 0.0, -0.35), + anchor_rot=(1.0, 0.0, 0.0, 0.0), + ) + + def __post_init__(self): + """Post initialization.""" + self.decimation = 4 + self.episode_length_s = 20.0 + self.sim.dt = 1 / 200 + self.sim.render_interval = 2 + + urdf_omniverse_path = f"{MINDBOT_ASSETS_DIR}/robots/g1_with_brainco_hand/g1_29dof_mode_15_brainco_hand.urdf" + mesh_root_path = f"{MINDBOT_ASSETS_DIR}/robots/g1_with_brainco_hand" + + self.actions.upper_body_ik.controller.urdf_path = retrieve_file_path(urdf_omniverse_path) + self.actions.upper_body_ik.controller.mesh_path = mesh_root_path + + # urdf_omniverse_path = f"{MINDBOT_ASSETS_DIR}/robots/g1_with_brainco_hand/g1_29dof_mode_15_brainco_hand.urdf" + + # self.actions.upper_body_ik.controller.urdf_path = retrieve_file_path(urdf_omniverse_path) + + self.xr.anchor_prim_path = "/World/envs/env_0/Robot/pelvis" + self.xr.fixed_anchor_height = True + self.xr.anchor_rotation_mode = XrAnchorRotationMode.FOLLOW_PRIM_SMOOTHED + + self.teleop_devices = DevicesCfg( + devices={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + G1BrainCoUpperBodyRetargeterCfg( + enable_visualization=True, + num_open_xr_hand_joints=2 * 26, + sim_device=self.sim.device, + hand_joint_names=self.actions.upper_body_ik.hand_joint_names, + ), + G1LowerBodyStandingRetargeterCfg( + sim_device=self.sim.device, + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + "motion_controllers": OpenXRDeviceCfg( + retargeters=[ + G1BrainCoUpperBodyMotionControllerRetargeterCfg( + enable_visualization=True, + sim_device=self.sim.device, + hand_joint_names=self.actions.upper_body_ik.hand_joint_names, + ), + G1LowerBodyStandingMotionControllerRetargeterCfg( + sim_device=self.sim.device, + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + } + ) + + # def __post_init__(self): + # """Post initialization.""" + # # general settings + # self.decimation = 4 + # self.episode_length_s = 20.0 + # # simulation settings + # self.sim.dt = 1 / 200 # 200Hz + # self.sim.render_interval = 2 + + # # Set the URDF and mesh paths for the IK controller + # # urdf_omniverse_path = f"{ISAACLAB_NUCLEUS_DIR}/Controllers/LocomanipulationAssets/unitree_g1_kinematics_asset/g1_29dof_with_hand_only_kinematics.urdf" # noqa: E501 + # urdf_omniverse_path = f"{MINDBOT_ASSETS_DIR}/robots/g1_with_brainco_hand/g1_29dof_mode_15_brainco_hand.urdf" + + + # # Retrieve local paths for the URDF and mesh files. Will be cached for call after the first time. + # self.actions.upper_body_ik.controller.urdf_path = retrieve_file_path(urdf_omniverse_path) + + # self.xr.anchor_prim_path = "/World/envs/env_0/Robot/pelvis" + # self.xr.fixed_anchor_height = True + # # Ensure XR anchor rotation follows the robot pelvis (yaw only), with smoothing for comfort + # self.xr.anchor_rotation_mode = XrAnchorRotationMode.FOLLOW_PRIM_SMOOTHED + + # self.teleop_devices = DevicesCfg( + # devices={ + # "handtracking": OpenXRDeviceCfg( + # retargeters=[ + # G1TriHandUpperBodyRetargeterCfg( + # enable_visualization=True, + # # OpenXR hand tracking has 26 joints per hand + # num_open_xr_hand_joints=2 * 26, + # sim_device=self.sim.device, + # hand_joint_names=self.actions.upper_body_ik.hand_joint_names, + # ), + # G1LowerBodyStandingRetargeterCfg( + # sim_device=self.sim.device, + # ), + # ], + # sim_device=self.sim.device, + # xr_cfg=self.xr, + # ), + # "motion_controllers": OpenXRDeviceCfg( + # retargeters=[ + # G1TriHandUpperBodyMotionControllerRetargeterCfg( + # enable_visualization=True, + # sim_device=self.sim.device, + # hand_joint_names=self.actions.upper_body_ik.hand_joint_names, + # ), + # G1LowerBodyStandingMotionControllerRetargeterCfg( + # sim_device=self.sim.device, + # ), + # ], + # sim_device=self.sim.device, + # xr_cfg=self.xr, + # ), + # } + # ) diff --git a/source/mindbot/mindbot/utils/assets.py b/source/mindbot/mindbot/utils/assets.py index c46934c..c68fc90 100644 --- a/source/mindbot/mindbot/utils/assets.py +++ b/source/mindbot/mindbot/utils/assets.py @@ -21,7 +21,7 @@ import os MINDBOT_ASSETS_DIR: str = os.environ.get( "MINDBOT_ASSETS_DIR", # "/home/tangger/LYT/maic_usd_assets_moudle", - "/home/maic/xh/maic_usd_assets_moudle" - # "/workspace/maic_usd_assets_moudle" + # "/home/maic/xh/maic_usd_assets_moudle" + "/workspace/maic_usd_assets_moudle" # "/home/maic/LYT/maic_usd_assets_moudle", )