双臂头部遥操作控制
This commit is contained in:
@@ -83,7 +83,7 @@ from xr_utils.geometry import R_HEADSET_TO_WORLD
|
||||
_ROBOT_CAM_PRIMS: dict[str, str] = {
|
||||
"Left Hand": "/World/envs/env_0/Robot/rm_65_fb_left/Link_6/robot___left_8_02/cam_left_hand",
|
||||
"Right Hand": "/World/envs/env_0/Robot/rm_65_b_right/Link6/robot___right_8_2/cam_right_hand",
|
||||
"Head": "/World/envs/env_0/Robot/robot_head/cam_head",
|
||||
"Head": "/World/envs/env_0/Robot/robot_head/ZED_X/base_link/ZED_X/CameraLeft",
|
||||
"Chest": "/World/envs/env_0/Robot/robot_trunk/cam_chest",
|
||||
}
|
||||
|
||||
@@ -339,6 +339,43 @@ class XrTeleopController:
|
||||
return action
|
||||
|
||||
|
||||
# =====================================================================
|
||||
# Head Tracking Utility
|
||||
# =====================================================================
|
||||
|
||||
def get_head_joint_targets(
|
||||
xr_client, neutral_rot: R | None
|
||||
) -> tuple[np.ndarray, R | None]:
|
||||
"""Extract head joint targets [yaw, pitch] from HMD pose.
|
||||
|
||||
At first call (neutral_rot is None) the current headset orientation
|
||||
is recorded as the neutral reference and zeros are returned.
|
||||
Subsequent calls return yaw/pitch relative to that neutral pose.
|
||||
|
||||
Returns:
|
||||
(joint_targets, neutral_rot) where joint_targets is shape (2,)
|
||||
float32 [head_yaw_Joint_rad, head_pitch_Joint_rad].
|
||||
"""
|
||||
try:
|
||||
raw_pose = xr_client.get_pose("headset")
|
||||
if not is_valid_quaternion(raw_pose[3:]):
|
||||
return np.zeros(2, dtype=np.float32), neutral_rot
|
||||
_, head_world_quat_xyzw = transform_xr_pose(raw_pose[:3], raw_pose[3:])
|
||||
head_rot = R.from_quat(head_world_quat_xyzw)
|
||||
if neutral_rot is None:
|
||||
# First call — record neutral and hold zero
|
||||
return np.zeros(2, dtype=np.float32), head_rot
|
||||
# Relative rotation from neutral heading
|
||||
rel_rot = head_rot * neutral_rot.inv()
|
||||
# ZYX Euler: [0]=yaw (Z), [1]=pitch (Y)
|
||||
euler_zyx = rel_rot.as_euler("ZYX")
|
||||
yaw = float(np.clip(euler_zyx[0], -1.57, 1.57)) # ±90°
|
||||
pitch = float(np.clip(euler_zyx[1], -0.52, 0.52)) # ±30°
|
||||
return np.array([yaw, pitch], dtype=np.float32), neutral_rot
|
||||
except Exception:
|
||||
return np.zeros(2, dtype=np.float32), neutral_rot
|
||||
|
||||
|
||||
# =====================================================================
|
||||
# Main Execution Loop
|
||||
# =====================================================================
|
||||
@@ -519,6 +556,7 @@ def main() -> None:
|
||||
clear_ik_target_state()
|
||||
last_root_left = None
|
||||
last_root_right = None
|
||||
neutral_head_rot = None # calibrated on first headset sample after reset
|
||||
|
||||
while simulation_app.is_running():
|
||||
try:
|
||||
@@ -534,11 +572,18 @@ def main() -> None:
|
||||
sim_frame = 0
|
||||
last_root_left = None
|
||||
last_root_right = None
|
||||
neutral_head_rot = None # re-calibrate on next headset sample
|
||||
continue
|
||||
|
||||
policy_obs = obs["policy"]
|
||||
wheel_np, v_fwd, omega_base = get_chassis_commands()
|
||||
|
||||
# Head tracking: HMD yaw/pitch relative to neutral pose
|
||||
head_cmd_np, neutral_head_rot = get_head_joint_targets(
|
||||
teleop_interface.xr_client, neutral_head_rot
|
||||
)
|
||||
head_cmd = torch.tensor(head_cmd_np, dtype=torch.float32)
|
||||
|
||||
if is_dual_arm:
|
||||
eef_pos_left = policy_obs["eef_pos_left"][0].cpu().numpy()
|
||||
eef_quat_left = policy_obs["eef_quat_left"][0].cpu().numpy()
|
||||
@@ -554,10 +599,11 @@ def main() -> None:
|
||||
if teleop_right_ref.grip_active or last_root_right is None:
|
||||
last_root_right = convert_action_world_to_root(right_action)[:7].clone()
|
||||
|
||||
# Action layout: left_arm(7) | wheel(4) | left_gripper(1) | right_arm(7) | right_gripper(1)
|
||||
# Action layout: left_arm(7) | wheel(4) | left_gripper(1) | right_arm(7) | right_gripper(1) | head(2)
|
||||
action_np = torch.cat([
|
||||
last_root_left, wheel_np, left_action[7:8],
|
||||
last_root_right, right_action[7:8],
|
||||
head_cmd,
|
||||
])
|
||||
else:
|
||||
eef_pos = policy_obs["eef_pos"][0].cpu().numpy()
|
||||
|
||||
@@ -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,
|
||||
)
|
||||
@@ -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"
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user