hangX/mindbot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

双臂头部遥操作控制

Browse Source
This commit is contained in:
hangX
2026-03-19 21:33:39 +08:00
parent 1105d505e6
commit d23898c3cb
4 changed files with 598 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
source/mindbot/mindbot/tasks/manager_based/il/open_drybox/__init__.py
View File

@@ -56,4 +56,14 @@ gym.register(
"robomimic_bc_cfg_entry_point": f"{__name__}.agents:robomimic/bc_rnn_low_dim.json",
},
disable_env_checker=True,
)
gym.register(
id="Isaac-MindRobot-2i-DualArm-IK-Abs-v0",
entry_point="isaaclab.envs:ManagerBasedRLEnv",
kwargs={
"env_cfg_entry_point": f"{__name__}.mindrobot_2i_dual_arm_ik_env_cfg:MindRobot2iDualArmIKAbsEnvCfg",
"robomimic_bc_cfg_entry_point": f"{__name__}.agents:robomimic/bc_rnn_low_dim.json",
},
disable_env_checker=True,
)

177
source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_2i_cfg.py Normal file
View File

@@ -0,0 +1,177 @@
# Copyright (c) 2024, MindRobot Project
# SPDX-License-Identifier: BSD-3-Clause
"""Configuration for MindRobot dual-arm robot.
Arms are RealMan RM-65 (6-DOF, 5kg payload, 7.2kg self-weight, 610mm reach).
Reference: https://develop.realman-robotics.com/robot4th/robotParameter/RM65OntologyParameters/
The following configurations are available:
* :obj:`MINDBOT_CFG`: Base configuration with gravity enabled (general purpose).
* :obj:`MINDBOT_HIGH_PD_CFG`: Gravity disabled, stiffer PD for IK task-space control.
"""
import isaaclab.sim as sim_utils
from isaaclab.actuators import ImplicitActuatorCfg
from isaaclab.assets import ArticulationCfg
from mindbot.utils.assets import MINDBOT_ASSETS_DIR
MINDBOT_CFG = ArticulationCfg(
spawn=sim_utils.UsdFileCfg(
usd_path=f"{MINDBOT_ASSETS_DIR}/robots/mindrobot_2i/mindrobot_2i.usd",
rigid_props=sim_utils.RigidBodyPropertiesCfg(
disable_gravity=False,
max_depenetration_velocity=5.0,
),
articulation_props=sim_utils.ArticulationRootPropertiesCfg(
fix_root_link=False,
enabled_self_collisions=True,
solver_position_iteration_count=8,
solver_velocity_iteration_count=0,
),
),
init_state=ArticulationCfg.InitialStateCfg(
joint_pos={
# ====== Left arm (RM-65) — away from singularities ======
# Elbow singularity: q3=0; Wrist singularity: q5=0.
# Small offsets on q2/q4/q6 to avoid exact 90° Jacobian symmetry.
"l_joint1": 1.5708, # 90°
"l_joint2": -1.2217, # -70° (offset from -90° for better elbow workspace)
"l_joint3": -1.5708, # -90° (away from elbow singularity q3=0)
"l_joint4": 1.3963, # 80° (offset from 90° to break wrist symmetry)
"l_joint5": 1.5708, # 90° (away from wrist singularity q5=0)
"l_joint6": -1.3963, # -80° (offset from -90° to break wrist symmetry)
# ====== Right arm (RM-65) — same joint values as left (verified visually) ======
"r_joint1": 1.5708, # 90°
"r_joint2": -1.2217, # -70°
"r_joint3": -1.5708, # -90° (away from elbow singularity q3=0)
"r_joint4": 1.3963, # 80°
"r_joint5": 1.5708, # 90° (away from wrist singularity q5=0)
"r_joint6": 1.3963, # -80°
# ====== Grippers (0=open) ======
"left_hand_joint_left": 0.0,
"left_hand_joint_right": 0.0,
"right_hand_joint_left": 0.0,
"right_hand_joint_right": 0.0,
# ====== Trunk & Head ======
"PrismaticJoint": 0.26,
"head_pitch_Joint": 0.0,
"head_yaw_Joint": 0.0,
},
pos=(0.0, 0.0, 0.7),
),
actuators={
# RM-65 J1-J3: shoulder/elbow — max 180°/s ≈ 3.14 rad/s
"left_arm_shoulder": ImplicitActuatorCfg(
joint_names_expr=["l_joint[1-3]"],
effort_limit_sim=50.0,
velocity_limit_sim=3.14,
stiffness=80.0,
damping=4.0,
),
# RM-65 J4-J6: wrist — max 225°/s ≈ 3.93 rad/s
"left_arm_wrist": ImplicitActuatorCfg(
joint_names_expr=["l_joint[4-6]"],
effort_limit_sim=20.0,
velocity_limit_sim=3.93,
stiffness=80.0,
damping=4.0,
),
"right_arm_shoulder": ImplicitActuatorCfg(
joint_names_expr=["r_joint[1-3]"],
effort_limit_sim=50.0,
velocity_limit_sim=3.14,
stiffness=80.0,
damping=4.0,
),
"right_arm_wrist": ImplicitActuatorCfg(
joint_names_expr=["r_joint[4-6]"],
effort_limit_sim=20.0,
velocity_limit_sim=3.93,
stiffness=80.0,
damping=4.0,
),
"head": ImplicitActuatorCfg(
joint_names_expr=[
"head_yaw_Joint",
"head_pitch_Joint"
],
effort_limit_sim=12.0,
stiffness=80.0,
damping=4.0,
),
"trunk": ImplicitActuatorCfg(
joint_names_expr=["PrismaticJoint"],
effort_limit_sim=200.0,
velocity_limit_sim=0.2,
stiffness=1000.0,
damping=100.0,
),
"wheels": ImplicitActuatorCfg(
# joint_names_expr=[".*_revolute_Joint"],
joint_names_expr=[
"right_b_revolute_Joint",
"left_b_revolute_Joint",
"left_f_revolute_Joint",
"right_f_revolute_Joint",
],
effort_limit_sim=200.0,
stiffness=0.0,
damping=100.0,
),
"grippers": ImplicitActuatorCfg(
joint_names_expr=[".*_hand_joint.*"],
effort_limit_sim=200.0,
stiffness=2e3,
damping=1e2,
),
},
soft_joint_pos_limit_factor=1.0,
)
"""Base configuration for MindRobot. Gravity enabled, low PD gains."""
MINDBOT_HIGH_PD_CFG = MINDBOT_CFG.copy()
MINDBOT_HIGH_PD_CFG.actuators["left_arm_shoulder"].stiffness = 400.0
MINDBOT_HIGH_PD_CFG.actuators["left_arm_shoulder"].damping = 80.0
MINDBOT_HIGH_PD_CFG.actuators["left_arm_wrist"].stiffness = 400.0
MINDBOT_HIGH_PD_CFG.actuators["left_arm_wrist"].damping = 80.0
MINDBOT_HIGH_PD_CFG.actuators["right_arm_shoulder"].stiffness = 400.0
MINDBOT_HIGH_PD_CFG.actuators["right_arm_shoulder"].damping = 80.0
MINDBOT_HIGH_PD_CFG.actuators["right_arm_wrist"].stiffness = 400.0
MINDBOT_HIGH_PD_CFG.actuators["right_arm_wrist"].damping = 80.0
"""IK task-space control configuration. Gravity disabled, stiffer PD for tracking."""
# =====================================================================
# Robot Body / Joint Name Constants
# Keep all robot-specific strings here so env cfgs stay robot-agnostic.
# =====================================================================
# Joint name patterns (regex, for use in action/actuator configs)
MINDBOT_LEFT_ARM_JOINTS = "l_joint[1-6]"
MINDBOT_RIGHT_ARM_JOINTS = "r_joint[1-6]"
MINDBOT_LEFT_GRIPPER_JOINTS = ["left_hand_joint_left", "left_hand_joint_right"]
MINDBOT_RIGHT_GRIPPER_JOINTS = ["right_hand_joint_left", "right_hand_joint_right"]
MINDBOT_WHEEL_JOINTS = [
"right_b_revolute_Joint",
"left_b_revolute_Joint",
"left_f_revolute_Joint",
"right_f_revolute_Joint",
]
MINDBOT_HEAD_JOIONTS =[
"head_yaw_Joint", #
"head_pitch_Joint"
]
# EEF body names (as defined in the USD asset)
MINDBOT_LEFT_EEF_BODY = "left_hand_body"
MINDBOT_RIGHT_EEF_BODY = "right_hand_body"
# Prim paths relative to the Robot prim (i.e. after "{ENV_REGEX_NS}/Robot/")
MINDBOT_LEFT_ARM_PRIM_ROOT = "rm_65_fb_left"
MINDBOT_RIGHT_ARM_PRIM_ROOT = "rm_65_b_right"
MINDBOT_LEFT_EEF_PRIM = "rm_65_fb_left/l_hand_01/left_hand_body"
MINDBOT_RIGHT_EEF_PRIM = "rm_65_b_right/r_hand/right_hand_body"

363
source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_2i_dual_arm_ik_env_cfg.py Normal file
View File

@@ -0,0 +1,363 @@
# Copyright (c) 2024, MindRobot Project
# SPDX-License-Identifier: BSD-3-Clause
"""MindRobot dual-arm IK absolute environment config for XR teleoperation."""
from __future__ import annotations
import torch
import isaaclab.sim as sim_utils
from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg
from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg
from isaaclab.devices import DevicesCfg
from isaaclab.devices.keyboard import Se3KeyboardCfg
from isaaclab.devices.spacemouse import Se3SpaceMouseCfg
from isaaclab.devices.gamepad import Se3GamepadCfg
from isaaclab.devices.openxr import XrCfg
from isaaclab.envs import ManagerBasedRLEnvCfg
from isaaclab.envs.mdp.actions.actions_cfg import (
BinaryJointPositionActionCfg,
DifferentialInverseKinematicsActionCfg,
JointPositionActionCfg,
JointVelocityActionCfg,
)
from isaaclab.managers import EventTermCfg as EventTerm
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.markers.config import FRAME_MARKER_CFG
from isaaclab.scene import InteractiveSceneCfg
from isaaclab.sensors.frame_transformer.frame_transformer_cfg import FrameTransformerCfg, OffsetCfg
from isaaclab.sim.schemas.schemas_cfg import RigidBodyPropertiesCfg
from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg
from isaaclab.sim.spawners.materials import RigidBodyMaterialCfg
from isaaclab.utils import configclass
from isaaclab.envs import ViewerCfg
from . import mdp
from .mindrobot_2i_cfg import (
MINDBOT_HIGH_PD_CFG,
MINDBOT_LEFT_ARM_JOINTS,
MINDBOT_RIGHT_ARM_JOINTS,
MINDBOT_LEFT_EEF_BODY,
MINDBOT_RIGHT_EEF_BODY,
MINDBOT_LEFT_GRIPPER_JOINTS,
MINDBOT_RIGHT_GRIPPER_JOINTS,
MINDBOT_WHEEL_JOINTS,
MINDBOT_HEAD_JOIONTS,
MINDBOT_LEFT_ARM_PRIM_ROOT,
MINDBOT_RIGHT_ARM_PRIM_ROOT,
MINDBOT_LEFT_EEF_PRIM,
MINDBOT_RIGHT_EEF_PRIM,
)
from mindbot.assets.lab import ROOM_CFG
from mindbot.assets.table import TABLE_CFG
from mindbot.assets.dryingbox import DRYINGBOX_CFG
# =====================================================================
# Scene Configuration
# =====================================================================
@configclass
class MindRobotDualArmSceneCfg(InteractiveSceneCfg):
"""Scene for MindRobot dual-arm teleoperation."""
plane = AssetBaseCfg(
prim_path="/World/GroundPlane",
init_state=AssetBaseCfg.InitialStateCfg(pos=[0.0, 0.0, 0.0]),
spawn=GroundPlaneCfg(
# Moderate friction: enough traction to move forward, low enough to
# allow skid-steer lateral wheel slip for turning.
# Default (1.0/1.0) moves but can't turn; 0.4/0.3 spins in place.
physics_material=RigidBodyMaterialCfg(
static_friction=0.7,
dynamic_friction=0.5,
restitution=0.0,
)
),
)
room = ROOM_CFG.replace(prim_path="{ENV_REGEX_NS}/Room")
table = TABLE_CFG.replace(prim_path="{ENV_REGEX_NS}/Table")
drying_box = DRYINGBOX_CFG.replace(prim_path="{ENV_REGEX_NS}/DryingBox")
robot: ArticulationCfg = MINDBOT_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
light = AssetBaseCfg(
prim_path="/World/light",
spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0),
)
# EEF frame transformers — set in __post_init__
ee_frame: FrameTransformerCfg = None
ee_frame_right: FrameTransformerCfg = None
# =====================================================================
# Actions Configuration
# =====================================================================
@configclass
class MindRobotDualArmActionsCfg:
"""Actions for MindRobot dual-arm IK teleoperation (absolute mode).
Action vector layout (total 22D):
left_arm_action (7D): [x, y, z, qw, qx, qy, qz] absolute EEF pose, root frame
wheel_action (4D): [right_b, left_b, left_f, right_f] wheel vel (rad/s)
left_gripper_action (1D): +1=open, -1=close
right_arm_action (7D): [x, y, z, qw, qx, qy, qz] absolute EEF pose, root frame
right_gripper_action(1D): +1=open, -1=close
head_action (2D): [yaw_rad, pitch_rad] absolute joint position
"""
left_arm_action = DifferentialInverseKinematicsActionCfg(
asset_name="robot",
joint_names=[MINDBOT_LEFT_ARM_JOINTS],
body_name=MINDBOT_LEFT_EEF_BODY,
controller=DifferentialIKControllerCfg(
command_type="pose",
use_relative_mode=False,
ik_method="dls",
),
scale=1,
body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.0]),
)
wheel_action = JointVelocityActionCfg(
asset_name="robot",
joint_names=MINDBOT_WHEEL_JOINTS,
scale=1.0,
)
left_gripper_action = BinaryJointPositionActionCfg(
asset_name="robot",
joint_names=MINDBOT_LEFT_GRIPPER_JOINTS,
open_command_expr={"left_hand_joint_left": 0.0, "left_hand_joint_right": 0.0},
close_command_expr={"left_hand_joint_left": -0.03, "left_hand_joint_right": 0.03},
)
right_arm_action = DifferentialInverseKinematicsActionCfg(
asset_name="robot",
joint_names=[MINDBOT_RIGHT_ARM_JOINTS],
body_name=MINDBOT_RIGHT_EEF_BODY,
controller=DifferentialIKControllerCfg(
command_type="pose",
use_relative_mode=False,
ik_method="dls",
),
scale=1,
body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.0]),
)
right_gripper_action = BinaryJointPositionActionCfg(
asset_name="robot",
joint_names=MINDBOT_RIGHT_GRIPPER_JOINTS,
open_command_expr={"right_hand_joint_left": 0.0, "right_hand_joint_right": 0.0},
close_command_expr={"right_hand_joint_left": -0.03, "right_hand_joint_right": 0.03},
)
head_action = JointPositionActionCfg(
asset_name="robot",
joint_names=MINDBOT_HEAD_JOIONTS,
scale=1.0,
)
# =====================================================================
# Observations Configuration
# =====================================================================
@configclass
class MindRobotDualArmObsCfg:
"""Observations for dual-arm teleoperation."""
@configclass
class PolicyCfg(ObsGroup):
actions = ObsTerm(func=mdp.last_action)
joint_pos = ObsTerm(func=mdp.joint_pos_rel)
joint_vel = ObsTerm(func=mdp.joint_vel_rel)
# Left arm EEF
eef_pos_left = ObsTerm(func=mdp.ee_frame_pos)
eef_quat_left = ObsTerm(func=mdp.ee_frame_quat)
# Left gripper (2 joints)
left_gripper_pos = ObsTerm(
func=mdp.gripper_pos,
params={"joint_names": MINDBOT_LEFT_GRIPPER_JOINTS},
)
# Right arm EEF
eef_pos_right = ObsTerm(
func=mdp.ee_frame_pos,
params={"ee_frame_cfg": SceneEntityCfg("ee_frame_right")},
)
eef_quat_right = ObsTerm(
func=mdp.ee_frame_quat,
params={"ee_frame_cfg": SceneEntityCfg("ee_frame_right")},
)
# Right gripper (2 joints)
right_gripper_pos = ObsTerm(
func=mdp.gripper_pos,
params={"joint_names": MINDBOT_RIGHT_GRIPPER_JOINTS},
)
def __post_init__(self):
self.enable_corruption = False
self.concatenate_terms = False
policy: PolicyCfg = PolicyCfg()
# =====================================================================
# Terminations Configuration
# =====================================================================
@configclass
class MindRobotDualArmTerminationsCfg:
time_out = DoneTerm(func=mdp.time_out, time_out=True)
# =====================================================================
# Events Configuration
# =====================================================================
def _disable_arm_gravity(env, env_ids: torch.Tensor):
"""Disable gravity for both arm subtrees at startup."""
import isaaclab.sim.schemas as schemas
arm_cfg = RigidBodyPropertiesCfg(disable_gravity=True)
for env_id in range(env.num_envs):
for arm_path in [
f"/World/envs/env_{env_id}/Robot/{MINDBOT_LEFT_ARM_PRIM_ROOT}",
f"/World/envs/env_{env_id}/Robot/{MINDBOT_RIGHT_ARM_PRIM_ROOT}",
]:
schemas.modify_rigid_body_properties(arm_path, arm_cfg)
@configclass
class MindRobotDualArmEventsCfg:
disable_arm_gravity = EventTerm(
func=_disable_arm_gravity,
mode="startup",
)
reset_scene = EventTerm(
func=mdp.reset_scene_to_default,
mode="reset",
)
# =====================================================================
# Empty placeholder configs
# =====================================================================
@configclass
class EmptyCfg:
pass
# =====================================================================
# Main Environment Configuration
# =====================================================================
@configclass
class MindRobot2iDualArmIKAbsEnvCfg(ManagerBasedRLEnvCfg):
"""MindRobot 双臂 IK-Abs 遥操作环境配置（XR 控制器）。"""
scene: MindRobotDualArmSceneCfg = MindRobotDualArmSceneCfg(
num_envs=1,
env_spacing=2.5,
)
observations: MindRobotDualArmObsCfg = MindRobotDualArmObsCfg()
actions: MindRobotDualArmActionsCfg = MindRobotDualArmActionsCfg()
terminations: MindRobotDualArmTerminationsCfg = MindRobotDualArmTerminationsCfg()
events: MindRobotDualArmEventsCfg = MindRobotDualArmEventsCfg()
commands: EmptyCfg = EmptyCfg()
rewards: EmptyCfg = EmptyCfg()
curriculum: EmptyCfg = EmptyCfg()
xr: XrCfg = XrCfg(
anchor_pos=(-0.1, -0.5, -1.05),
anchor_rot=(0.866, 0, 0, -0.5),
)
def __post_init__(self):
super().__post_init__()
self.viewer = ViewerCfg(
eye=(2.0, -1.5, 1.8), # 相机位置
lookat=(0.0, 0.0, 0.8), # 瞄准点（机器人腰部高度附近）
origin_type="world",
)
self.decimation = 2
self.episode_length_s = 50.0
self.sim.dt = 0.01 # 100 Hz
self.sim.render_interval = 2
# Mobile base — root link not fixed
robot_cfg = MINDBOT_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
robot_cfg.spawn.articulation_props.fix_root_link = False
self.scene.robot = robot_cfg
# Left arm EEF frame transformer
marker_cfg = FRAME_MARKER_CFG.copy()
marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1)
marker_cfg.prim_path = "/Visuals/FrameTransformerLeft"
self.scene.ee_frame = FrameTransformerCfg(
prim_path=f"{{ENV_REGEX_NS}}/Robot/{MINDBOT_LEFT_ARM_PRIM_ROOT}/base_link",
debug_vis=False,
visualizer_cfg=marker_cfg,
target_frames=[
FrameTransformerCfg.FrameCfg(
prim_path=f"{{ENV_REGEX_NS}}/Robot/{MINDBOT_LEFT_EEF_PRIM}",
name="end_effector",
offset=OffsetCfg(pos=[0.0, 0.0, 0.0]),
),
],
)
# Right arm EEF frame transformer
marker_cfg_right = FRAME_MARKER_CFG.copy()
marker_cfg_right.markers["frame"].scale = (0.1, 0.1, 0.1)
marker_cfg_right.prim_path = "/Visuals/FrameTransformerRight"
self.scene.ee_frame_right = FrameTransformerCfg(
prim_path=f"{{ENV_REGEX_NS}}/Robot/{MINDBOT_RIGHT_ARM_PRIM_ROOT}/base_link",
debug_vis=False,
visualizer_cfg=marker_cfg_right,
target_frames=[
FrameTransformerCfg.FrameCfg(
prim_path=f"{{ENV_REGEX_NS}}/Robot/{MINDBOT_RIGHT_EEF_PRIM}",
name="end_effector",
offset=OffsetCfg(pos=[0.0, 0.0, 0.0]),
),
],
)
# Teleoperation devices
self.teleop_devices = DevicesCfg(
devices={
"keyboard": Se3KeyboardCfg(
pos_sensitivity=0.05,
rot_sensitivity=0.05,
sim_device=self.sim.device,
),
"spacemouse": Se3SpaceMouseCfg(
pos_sensitivity=0.05,
rot_sensitivity=0.05,
sim_device=self.sim.device,
),
"gamepad": Se3GamepadCfg(
pos_sensitivity=0.1,
rot_sensitivity=0.1,
sim_device=self.sim.device,
),
}
)
# Gripper params (left gripper for single-arm compat; dual-arm uses obs directly)
self.gripper_joint_names = MINDBOT_LEFT_GRIPPER_JOINTS
self.gripper_open_val = 0.0
self.gripper_threshold = 0.005