unitree G1 强脑智能

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

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

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

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

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

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

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

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_pitch_joint": 35.0,
-                ".*_ankle_roll_joint": 50.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_roll_joint": 35.0,
-                "waist_pitch_joint": 50.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_yaw_joint": 27.7489,
-                "waist_roll_joint": 54.2666,
+                "waist_roll_joint": 54.0995,
-                "waist_pitch_joint": 53.3254,
+                "waist_pitch_joint": 53.0524,
             },
             damping={
                 "waist_yaw_joint": 0.0111,
-                "waist_roll_joint": 0.0217,
+                "waist_roll_joint": 0.0216,
-                "waist_pitch_joint": 0.0213,
+                "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_pitch_joint": 149.8304,
-                ".*_shoulder_roll_joint": 151.6808,
+                ".*_shoulder_roll_joint": 124.0138,
-                ".*_shoulder_yaw_joint": 141.6967,
+                ".*_shoulder_yaw_joint": 114.5362,
-                ".*_elbow_joint": 88.9802,
+                ".*_elbow_joint": 65.6598,
-                ".*_wrist_roll_joint": 66.6865,
+                ".*_wrist_roll_joint": 46.9630,
-                ".*_wrist_pitch_joint": 26.5734,
+                ".*_wrist_pitch_joint": 17.4997,
-                ".*_wrist_yaw_joint": 16.3066,
+                ".*_wrist_yaw_joint": 10.2527,
             },
             damping={
-                ".*_shoulder_pitch_joint": 0.0675,
+                ".*_shoulder_pitch_joint": 0.0599,
-                ".*_shoulder_roll_joint": 0.0607,
+                ".*_shoulder_roll_joint": 0.0496,
-                ".*_shoulder_yaw_joint": 0.0567,
+                ".*_shoulder_yaw_joint": 0.0458,
-                ".*_elbow_joint": 0.0356,
+                ".*_elbow_joint": 0.0263,
-                ".*_wrist_roll_joint": 0.0267,
+                ".*_wrist_roll_joint": 0.0188,
-                ".*_wrist_pitch_joint": 0.0106,
+                ".*_wrist_pitch_joint": 0.0070,
-                ".*_wrist_yaw_joint": 0.0065,
+                ".*_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={
+    joint_pos={".*": 0.0},
-        ".*_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_vel={".*": 0.0},
 )
 G1_BRAINCO_FTP_CFG.actuators["arms"] = ImplicitActuatorCfg(

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 个关节。

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",

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."""

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."""

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,
+                ),
+            }
+        )

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,
+    #             ),
+    #         }
+    #     )

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"
+    # "/home/maic/xh/maic_usd_assets_moudle"
-    # "/workspace/maic_usd_assets_moudle"
+    "/workspace/maic_usd_assets_moudle"
     # "/home/maic/LYT/maic_usd_assets_moudle",
 )