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

轮子

Browse Source
This commit is contained in:
yt lee
2026-03-06 17:31:00 +08:00
parent 36b4273582
commit 2b290d805a
3 changed files with 667 additions and 492 deletions
Download Patch File Download Diff File Expand all files Collapse all files

216
scripts/environments/teleoperation/teleop_xr_agent.py
View File

@@ -45,6 +45,14 @@ parser.add_argument(
choices=["left", "right"], choices=["left", "right"],
help="Which arm/controller to use.", help="Which arm/controller to use.",
) )
parser.add_argument(
"--base_speed", type=float, default=3.0,
help="Max wheel speed (rad/s) for joystick full forward.",
)
parser.add_argument(
"--base_turn", type=float, default=2.0,
help="Max wheel differential (rad/s) for joystick full left/right.",
)
# append AppLauncher cli args # append AppLauncher cli args
AppLauncher.add_app_launcher_args(parser) AppLauncher.add_app_launcher_args(parser)
args_cli = parser.parse_args() args_cli = parser.parse_args()
@@ -78,6 +86,31 @@ from xr_utils.geometry import R_HEADSET_TO_WORLD
# Teleoperation Interface for XR # Teleoperation Interface for XR
# ===================================================================== # =====================================================================
def _quat_wxyz_to_rotation(quat_wxyz: np.ndarray) -> R:
"""Convert Isaac-style wxyz quaternion to scipy Rotation."""
return R.from_quat([quat_wxyz[1], quat_wxyz[2], quat_wxyz[3], quat_wxyz[0]])
def _rotation_to_quat_wxyz(rot: R) -> np.ndarray:
"""Convert scipy Rotation quaternion to Isaac-style wxyz."""
quat_xyzw = rot.as_quat()
return np.array([quat_xyzw[3], quat_xyzw[0], quat_xyzw[1], quat_xyzw[2]])
def world_pose_to_root_frame(
pos_w: np.ndarray,
quat_wxyz: np.ndarray,
root_pos_w: np.ndarray,
root_quat_wxyz: np.ndarray,
) -> tuple[np.ndarray, np.ndarray]:
"""Express a world-frame pose in the robot root frame."""
root_rot = _quat_wxyz_to_rotation(root_quat_wxyz)
pose_rot = _quat_wxyz_to_rotation(quat_wxyz)
pos_root = root_rot.inv().apply(pos_w - root_pos_w)
quat_root = _rotation_to_quat_wxyz(root_rot.inv() * pose_rot)
return pos_root, quat_root
class XrTeleopController: class XrTeleopController:
"""Teleop controller for PICO XR headset.""" """Teleop controller for PICO XR headset."""
@@ -105,6 +138,10 @@ class XrTeleopController:
self.grip_active = False self.grip_active = False
self.frame_count = 0 self.frame_count = 0
self.reset_button_latched = False
self.require_grip_reengage = False
self.grip_engage_threshold = 0.8
self.grip_release_threshold = 0.2
# Callbacks (like reset, etc) # Callbacks (like reset, etc)
self.callbacks = {} self.callbacks = {}
@@ -117,6 +154,11 @@ class XrTeleopController:
self.prev_xr_quat = None self.prev_xr_quat = None
self.grip_active = False self.grip_active = False
self.frame_count = 0 self.frame_count = 0
self.target_eef_pos = None
self.target_eef_quat = None
# Require one grip release after reset so stale controller motion
# cannot immediately drive the robot back toward the previous pose.
self.require_grip_reengage = True
def close(self): def close(self):
self.xr_client.close() self.xr_client.close()
@@ -144,14 +186,17 @@ class XrTeleopController:
Reads the XR controller. Reads the XR controller.
Relative bounds return 7D action tensor: [dx, dy, dz, drx, dry, drz, gripper] Relative bounds return 7D action tensor: [dx, dy, dz, drx, dry, drz, gripper]
Absolute bounds return 8D action tensor: [x, y, z, qw, qx, qy, qz, gripper] Absolute bounds return 8D action tensor: [x, y, z, qw, qx, qy, qz, gripper]
Note: current_eef_quat expects wxyz. Note: in absolute mode current_eef_* and the returned target are in WORLD frame.
The caller is responsible for converting to root frame before sending to IK.
""" """
# XR buttons check (e.g. A or B for reset) # XR buttons check (e.g. A or B for reset)
try: try:
if self.xr_client.get_button("B") or self.xr_client.get_button("Y"): reset_pressed = self.xr_client.get_button("B") or self.xr_client.get_button("Y")
if reset_pressed and not self.reset_button_latched:
if "RESET" in self.callbacks: if "RESET" in self.callbacks:
self.callbacks["RESET"]() self.callbacks["RESET"]()
except: self.reset_button_latched = reset_pressed
except Exception:
pass pass
try: try:
@@ -165,11 +210,23 @@ class XrTeleopController:
if not is_valid_quaternion(raw_pose[3:]): if not is_valid_quaternion(raw_pose[3:]):
return self.get_zero_action(trigger, current_eef_pos, current_eef_quat) return self.get_zero_action(trigger, current_eef_pos, current_eef_quat)
# Transform XR pose pos directly via Matrix mapping for safety check (though we can map diffs next) if self.require_grip_reengage:
pos_w = self.R_headset_world @ raw_pose[:3] if grip <= self.grip_release_threshold:
self.require_grip_reengage = False
else:
if self.is_absolute and current_eef_pos is not None and current_eef_quat is not None:
self.target_eef_pos = current_eef_pos.copy()
self.target_eef_quat = current_eef_quat.copy()
return self.get_zero_action(trigger, current_eef_pos, current_eef_quat)
# Use hysteresis so noisy analog grip values do not accidentally re-enable teleop.
if self.grip_active:
grip_pressed = grip > self.grip_release_threshold
else:
grip_pressed = grip >= self.grip_engage_threshold
# 握持键作为离合器 (Clutch) # 握持键作为离合器 (Clutch)
if grip < 0.5: if not grip_pressed:
self.prev_xr_pos = None self.prev_xr_pos = None
self.prev_xr_quat = None self.prev_xr_quat = None
self.grip_active = False self.grip_active = False
@@ -197,11 +254,9 @@ class XrTeleopController:
# 2. Extract Delta POS in World frame # 2. Extract Delta POS in World frame
world_delta_pos = (xr_world_pos - prev_xr_world_pos) * self.pos_sensitivity world_delta_pos = (xr_world_pos - prev_xr_world_pos) * self.pos_sensitivity
pos_norm = np.linalg.norm((raw_pose[:3] - self.prev_xr_pos))
# 3. Extract Delta ROT in World frame # 3. Extract Delta ROT in World frame
world_delta_rot = quat_diff_as_rotvec_xyzw(prev_xr_world_quat_xyzw, xr_world_quat_xyzw) * self.rot_sensitivity world_delta_rot = quat_diff_as_rotvec_xyzw(prev_xr_world_quat_xyzw, xr_world_quat_xyzw) * self.rot_sensitivity
rot_norm = np.linalg.norm(quat_diff_as_rotvec_xyzw(self.prev_xr_quat, raw_pose[3:]))
# 4. Gripper # 4. Gripper
gripper_action = 1.0 if trigger > 0.5 else -1.0 gripper_action = 1.0 if trigger > 0.5 else -1.0
@@ -211,16 +266,12 @@ class XrTeleopController:
self.target_eef_pos = np.zeros(3) self.target_eef_pos = np.zeros(3)
self.target_eef_quat = np.array([1.0, 0.0, 0.0, 0.0]) self.target_eef_quat = np.array([1.0, 0.0, 0.0, 0.0])
# Position update (simple translation in world frame) # Accumulate in world frame so VR direction always matches sim direction.
self.target_eef_pos += world_delta_pos self.target_eef_pos += world_delta_pos
# Rotation update: apply delta_R (in world frame) to target_R (in world frame) target_r = _quat_wxyz_to_rotation(self.target_eef_quat)
# R_new = R_delta @ R_target
target_r = R.from_quat([self.target_eef_quat[1], self.target_eef_quat[2], self.target_eef_quat[3], self.target_eef_quat[0]])
delta_r = R.from_rotvec(world_delta_rot) delta_r = R.from_rotvec(world_delta_rot)
new_r = delta_r * target_r self.target_eef_quat = _rotation_to_quat_wxyz(delta_r * target_r)
new_rot_xyzw = new_r.as_quat()
self.target_eef_quat = np.array([new_rot_xyzw[3], new_rot_xyzw[0], new_rot_xyzw[1], new_rot_xyzw[2]])
action = torch.tensor([ action = torch.tensor([
self.target_eef_pos[0], self.target_eef_pos[1], self.target_eef_pos[2], self.target_eef_pos[0], self.target_eef_pos[1], self.target_eef_pos[2],
@@ -231,14 +282,15 @@ class XrTeleopController:
self.prev_xr_quat = raw_pose[3:].copy() self.prev_xr_quat = raw_pose[3:].copy()
else: else:
# Relative clamping limits (used by relative mode to avoid divergence) max_pos_delta = 0.05
max_pos_delta = 0.04 world_pos_norm = np.linalg.norm(world_delta_pos)
if pos_norm > max_pos_delta: if world_pos_norm > max_pos_delta:
world_delta_pos = (world_delta_pos * max_pos_delta / pos_norm) world_delta_pos = world_delta_pos * (max_pos_delta / world_pos_norm)
max_rot_delta = 0.02 max_rot_delta = 0.15
if rot_norm > max_rot_delta: world_rot_norm = np.linalg.norm(world_delta_rot)
world_delta_rot = (world_delta_rot * max_rot_delta / rot_norm) if world_rot_norm > max_rot_delta:
world_delta_rot = world_delta_rot * (max_rot_delta / world_rot_norm)
action = torch.tensor([ action = torch.tensor([
world_delta_pos[0], world_delta_pos[1], world_delta_pos[2], world_delta_pos[0], world_delta_pos[1], world_delta_pos[2],
@@ -256,11 +308,11 @@ class XrTeleopController:
print(f"| Task Mode: {'ABSOLUTE' if self.is_absolute else 'RELATIVE'}") print(f"| Task Mode: {'ABSOLUTE' if self.is_absolute else 'RELATIVE'}")
print(f"| Raw VR Pos (OpenXR): {np.array(raw_pose[:3])}") print(f"| Raw VR Pos (OpenXR): {np.array(raw_pose[:3])}")
print(f"| Raw VR Quat (xyzw): {np.array(raw_pose[3:])}") print(f"| Raw VR Quat (xyzw): {np.array(raw_pose[3:])}")
print(f"| XR Delta Pos (world): {world_delta_pos} (norm={pos_norm:.4f})") print(f"| XR Delta Pos (world): {world_delta_pos} (norm={np.linalg.norm(world_delta_pos):.4f})")
print(f"| XR Delta Rot (world): {world_delta_rot} (norm={rot_norm:.4f})") print(f"| XR Delta Rot (world): {world_delta_rot} (norm={np.linalg.norm(world_delta_rot):.4f})")
if self.is_absolute: if self.is_absolute:
print(f"| Targ Pos (dx,dy,dz): {action[:3].numpy()}") print(f"| Targ Pos (world): {action[:3].numpy()}")
print(f"| Targ Quat (wxyz): {action[3:7].numpy()}") print(f"| Targ Quat (world, wxyz): {action[3:7].numpy()}")
else: else:
print(f"| Sent Action Pos (dx,dy,dz): {action[:3].numpy()}") print(f"| Sent Action Pos (dx,dy,dz): {action[:3].numpy()}")
print(f"| Sent Action Rot (rx,ry,rz): {action[3:6].numpy()}") print(f"| Sent Action Rot (rx,ry,rz): {action[3:6].numpy()}")
@@ -313,30 +365,114 @@ def main() -> None:
teleop_interface.add_callback("RESET", request_reset) teleop_interface.add_callback("RESET", request_reset)
def get_arm_action_term():
return env.action_manager._terms["arm_action"]
def clear_ik_target_state():
"""Clear the internal IK target so reset does not reuse the previous pose command."""
if not is_abs_mode:
return
arm_action_term = get_arm_action_term()
ee_pos_b, ee_quat_b = arm_action_term._compute_frame_pose()
arm_action_term._raw_actions.zero_()
arm_action_term._processed_actions.zero_()
arm_action_term._ik_controller._command.zero_()
arm_action_term._ik_controller.ee_pos_des[:] = ee_pos_b
arm_action_term._ik_controller.ee_quat_des[:] = ee_quat_b
def convert_action_world_to_root(action_tensor: torch.Tensor) -> torch.Tensor:
"""Convert an absolute IK action from world frame to robot root frame."""
robot = env.unwrapped.scene["robot"]
root_pos_w = robot.data.root_pos_w[0].detach().cpu().numpy()
root_quat_w = robot.data.root_quat_w[0].detach().cpu().numpy() # wxyz
target_pos_w = action_tensor[:3].numpy()
target_quat_w = action_tensor[3:7].numpy()
pos_root, quat_root = world_pose_to_root_frame(
target_pos_w, target_quat_w, root_pos_w, root_quat_w,
)
out = action_tensor.clone()
out[:3] = torch.tensor(pos_root, dtype=torch.float32)
out[3:7] = torch.tensor(quat_root, dtype=torch.float32)
return out
def get_wheel_action() -> torch.Tensor:
"""Read left joystick and return 4-DOF wheel velocity command.
Skid-steer differential drive.
Joystick: Y-axis (+1 = forward), X-axis (+1 = right turn).
Joint order from articulation (terminal log):
[right_b, left_b, left_f, right_f]
Right/left sign convention assumes both sides' joints have the same
axis direction (positive velocity = forward). If the robot drives
backward when pushing forward, negate base_speed in the launch command.
"""
try:
joy = teleop_interface.xr_client.get_joystick("left")
jy = float(joy[1]) # forward / backward
jx = float(joy[0]) # right / left
except Exception:
return torch.zeros(4)
v = jy * args_cli.base_speed
omega = jx * args_cli.base_turn
# Positive omega = turn right → left wheels faster, right wheels slower
right_vel = v - omega
left_vel = v + omega
return torch.tensor(
[right_vel, left_vel, left_vel, right_vel], dtype=torch.float32
)
env.reset() env.reset()
clear_ik_target_state()
teleop_interface.reset() teleop_interface.reset()
print("\n" + "=" * 50) print("\n" + "=" * 50)
print(" 🚀 Teleoperation Started!") print(" 🚀 Teleoperation Started!")
print(" 🎮 Use the TRIGGER to open/close gripper.") print(" 🎮 Use the TRIGGER to open/close gripper.")
print(" ✊ Hold GRIP button and move the controller to move the arm.") print(" ✊ Hold GRIP and move the controller to move the arm.")
print(" 🕹️ Press B or Y to reset the environment.") print(" 🕹️ Left joystick: Y=forward/back, X=turn left/right.")
print(" 🔄 Press B or Y to reset the environment.")
print("=" * 50 + "\n") print("=" * 50 + "\n")
device = env.unwrapped.device device = env.unwrapped.device
sim_frame = 0 sim_frame = 0
obs, _ = env.reset() obs, _ = env.reset()
clear_ik_target_state()
while simulation_app.is_running(): while simulation_app.is_running():
try: try:
with torch.inference_mode(): with torch.inference_mode():
# Extract tracking pose to seed absolute IK if should_reset:
obs, _ = env.reset()
clear_ik_target_state()
teleop_interface.reset()
should_reset = False
sim_frame = 0
continue
# Read current EEF in world frame from observations.
policy_obs = obs["policy"] policy_obs = obs["policy"]
eef_pos = policy_obs["eef_pos"][0].cpu().numpy() eef_pos = policy_obs["eef_pos"][0].cpu().numpy()
eef_quat = policy_obs["eef_quat"][0].cpu().numpy() eef_quat = policy_obs["eef_quat"][0].cpu().numpy()
# Get action from XR Controller # Get action from XR Controller (world frame for absolute mode).
action_np = teleop_interface.advance(current_eef_pos=eef_pos, current_eef_quat=eef_quat) action_np = teleop_interface.advance(
current_eef_pos=eef_pos, current_eef_quat=eef_quat,
)
# IK expects root-frame commands; convert just before sending.
if is_abs_mode:
action_np = convert_action_world_to_root(action_np)
# Action manager order: arm_action | wheel_action | gripper_action
# arm=7, wheel=4, gripper=1 → total 12 dims.
wheel_np = get_wheel_action()
action_np = torch.cat([action_np[:7], wheel_np, action_np[7:]])
actions = action_np.unsqueeze(0).repeat(env.num_envs, 1).to(device) actions = action_np.unsqueeze(0).repeat(env.num_envs, 1).to(device)
# Step environment # Step environment
@@ -363,30 +499,32 @@ def main() -> None:
print(f" [{i:2d}] {name:30s} = {joint_pos[i]:+.4f}") print(f" [{i:2d}] {name:30s} = {joint_pos[i]:+.4f}")
print(f"{'='*70}") print(f"{'='*70}")
# Find arm joint indices dynamically by looking at the first 6-7 joints that aren't fingers or hands # Find arm joint indices dynamically by looking at the first 6-7 joints that aren't fingers or hands
arm_idx = [i for i, n in enumerate(jnames) if "finger" not in n and "hand" not in n][:7] arm_idx = [i for i, n in enumerate(jnames) if n.startswith("l_joint")]
print(f" Deduced arm indices: {arm_idx}") print(f" Deduced left arm indices: {arm_idx}")
print(f"{'='*70}\n") print(f"{'='*70}\n")
# Get arm indices (cache-friendly: find once) # Get arm indices (cache-friendly: find once)
if not hasattr(env, '_arm_idx_cache'): if not hasattr(env, '_arm_idx_cache'):
robot = env.unwrapped.scene["robot"] robot = env.unwrapped.scene["robot"]
jnames = robot.joint_names jnames = robot.joint_names
env._arm_idx_cache = [i for i, n in enumerate(jnames) if "finger" not in n and "hand" not in n][:7] env._arm_idx_cache = [i for i, n in enumerate(jnames) if n.startswith("l_joint")]
arm_idx = env._arm_idx_cache arm_idx = env._arm_idx_cache
arm_joints = joint_pos[arm_idx] arm_joints = joint_pos[arm_idx]
try:
joy_dbg = teleop_interface.xr_client.get_joystick("left")
joy_str = f"[{joy_dbg[0]:+.2f}, {joy_dbg[1]:+.2f}]"
except Exception:
joy_str = "N/A"
print(f"\n---------------- [ROBOT STATE frame={sim_frame}] ----------------") print(f"\n---------------- [ROBOT STATE frame={sim_frame}] ----------------")
print(f"| Left Arm Joints (rad): {arm_joints}") print(f"| Left Arm Joints (rad): {arm_joints}")
print(f"| EEF Pos (world): {eef_pos}") print(f"| EEF Pos (world): {eef_pos}")
print(f"| EEF Quat (world, wxyz): {eef_quat}") print(f"| EEF Quat (world, wxyz): {eef_quat}")
print(f"| Last Action Sent: {last_act}") print(f"| Last Action Sent: {last_act}")
print(f"| Joystick (x=turn,y=fwd): {joy_str}")
print(f"----------------------------------------------------------------") print(f"----------------------------------------------------------------")
if should_reset:
env.reset()
teleop_interface.reset()
should_reset = False
except Exception as e: except Exception as e:
logger.error(f"Error during simulation step: {e}") logger.error(f"Error during simulation step: {e}")
break break

271
source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_cfg.py
View File

@@ -1,136 +1,135 @@
# Copyright (c) 2024, MindRobot Project # Copyright (c) 2024, MindRobot Project
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
"""Configuration for MindRobot dual-arm robot. """Configuration for MindRobot dual-arm robot.
Arms are RealMan RM-65 (6-DOF, 5kg payload, 7.2kg self-weight, 610mm reach). Arms are RealMan RM-65 (6-DOF, 5kg payload, 7.2kg self-weight, 610mm reach).
Reference: https://develop.realman-robotics.com/robot4th/robotParameter/RM65OntologyParameters/ Reference: https://develop.realman-robotics.com/robot4th/robotParameter/RM65OntologyParameters/
The following configurations are available: The following configurations are available:
* :obj:`MINDBOT_CFG`: Base configuration with gravity enabled (general purpose). * :obj:`MINDBOT_CFG`: Base configuration with gravity enabled (general purpose).
* :obj:`MINDBOT_HIGH_PD_CFG`: Gravity disabled, stiffer PD for IK task-space control. * :obj:`MINDBOT_HIGH_PD_CFG`: Gravity disabled, stiffer PD for IK task-space control.
""" """
import isaaclab.sim as sim_utils import isaaclab.sim as sim_utils
from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.actuators import ImplicitActuatorCfg
from isaaclab.assets import ArticulationCfg from isaaclab.assets import ArticulationCfg
from mindbot.utils.assets import MINDBOT_ASSETS_DIR from mindbot.utils.assets import MINDBOT_ASSETS_DIR
MINDBOT_CFG = ArticulationCfg( MINDBOT_CFG = ArticulationCfg(
spawn=sim_utils.UsdFileCfg( spawn=sim_utils.UsdFileCfg(
usd_path=f"{MINDBOT_ASSETS_DIR}/robots/mindbot/mindbot_cd2.usd", usd_path=f"{MINDBOT_ASSETS_DIR}/robots/mindbot/mindbot_cd2.usd",
rigid_props=sim_utils.RigidBodyPropertiesCfg( rigid_props=sim_utils.RigidBodyPropertiesCfg(
disable_gravity=False, disable_gravity=False,
max_depenetration_velocity=5.0, max_depenetration_velocity=5.0,
), ),
articulation_props=sim_utils.ArticulationRootPropertiesCfg( articulation_props=sim_utils.ArticulationRootPropertiesCfg(
fix_root_link=False, fix_root_link=False,
enabled_self_collisions=True, enabled_self_collisions=True,
solver_position_iteration_count=8, solver_position_iteration_count=8,
solver_velocity_iteration_count=0, solver_velocity_iteration_count=0,
), ),
), ),
init_state=ArticulationCfg.InitialStateCfg( init_state=ArticulationCfg.InitialStateCfg(
joint_pos={ joint_pos={
# ====== Left arm (RM-65) — away from singularities ====== # ====== Left arm (RM-65) — away from singularities ======
# Elbow singularity: q3=0; Wrist singularity: q5=0. # Elbow singularity: q3=0; Wrist singularity: q5=0.
# The pose below keeps q3≠0 and q5≠0. # The pose below keeps q3≠0 and q5≠0.
"l_joint1": 1.5708, # 135° "l_joint1": 1.5708, # 135°
"l_joint2": -1.5708, # -70° "l_joint2": -1.5708, # -70°
"l_joint3": -1.5708, # -90° (away from elbow singularity q3=0) "l_joint3": -1.5708, # -90° (away from elbow singularity q3=0)
"l_joint4": 1.5708, # 90° "l_joint4": 1.5708, # 90°
"l_joint5": 1.5708, # 90° (away from wrist singularity q5=0) "l_joint5": 1.5708, # 90° (away from wrist singularity q5=0)
"l_joint6": -1.5708, # -70° "l_joint6": -1.5708, # -70°
# ====== Right arm (RM-65) ====== # ====== Right arm (RM-65) ======
"r_joint1": -2.3562, "r_joint1": -2.3562,
"r_joint2": -1.2217, "r_joint2": -1.2217,
"r_joint3": 1.5708, "r_joint3": 1.5708,
"r_joint4": -1.5708, "r_joint4": -1.5708,
"r_joint5": -1.5708, "r_joint5": -1.5708,
"r_joint6": 1.2217, "r_joint6": 1.2217,
# ====== Grippers (0=open) ====== # ====== Grippers (0=open) ======
"left_hand_joint_left": 0.0, "left_hand_joint_left": 0.0,
"left_hand_joint_right": 0.0, "left_hand_joint_right": 0.0,
"right_hand_joint_left": 0.0, "right_hand_joint_left": 0.0,
"right_hand_joint_right": 0.0, "right_hand_joint_right": 0.0,
# ====== Trunk & Head ====== # ====== Trunk & Head ======
"PrismaticJoint": 0.26, "PrismaticJoint": 0.26,
"head_revoluteJoint": 0.0, "head_revoluteJoint": 0.0,
}, },
pos=(0.0, 0.0, 0.0), pos=(0.0, 0.0, 0.7),
), ),
actuators={ actuators={
# RM-65 J1-J3: shoulder/elbow — max 180°/s ≈ 3.14 rad/s # RM-65 J1-J3: shoulder/elbow — max 180°/s ≈ 3.14 rad/s
"left_arm_shoulder": ImplicitActuatorCfg( "left_arm_shoulder": ImplicitActuatorCfg(
joint_names_expr=["l_joint[1-3]"], joint_names_expr=["l_joint[1-3]"],
effort_limit_sim=50.0, effort_limit_sim=50.0,
velocity_limit_sim=3.14, velocity_limit_sim=3.14,
stiffness=80.0, stiffness=80.0,
damping=4.0, damping=4.0,
), ),
# RM-65 J4-J6: wrist — max 225°/s ≈ 3.93 rad/s # RM-65 J4-J6: wrist — max 225°/s ≈ 3.93 rad/s
"left_arm_wrist": ImplicitActuatorCfg( "left_arm_wrist": ImplicitActuatorCfg(
joint_names_expr=["l_joint[4-6]"], joint_names_expr=["l_joint[4-6]"],
effort_limit_sim=20.0, effort_limit_sim=20.0,
velocity_limit_sim=3.93, velocity_limit_sim=3.93,
stiffness=80.0, stiffness=80.0,
damping=4.0, damping=4.0,
), ),
"right_arm_shoulder": ImplicitActuatorCfg( "right_arm_shoulder": ImplicitActuatorCfg(
joint_names_expr=["r_joint[1-3]"], joint_names_expr=["r_joint[1-3]"],
effort_limit_sim=50.0, effort_limit_sim=50.0,
velocity_limit_sim=3.14, velocity_limit_sim=3.14,
stiffness=80.0, stiffness=80.0,
damping=4.0, damping=4.0,
), ),
"right_arm_wrist": ImplicitActuatorCfg( "right_arm_wrist": ImplicitActuatorCfg(
joint_names_expr=["r_joint[4-6]"], joint_names_expr=["r_joint[4-6]"],
effort_limit_sim=20.0, effort_limit_sim=20.0,
velocity_limit_sim=3.93, velocity_limit_sim=3.93,
stiffness=80.0, stiffness=80.0,
damping=4.0, damping=4.0,
), ),
"head": ImplicitActuatorCfg( "head": ImplicitActuatorCfg(
joint_names_expr=["head_revoluteJoint"], joint_names_expr=["head_revoluteJoint"],
effort_limit_sim=12.0, effort_limit_sim=12.0,
stiffness=80.0, stiffness=80.0,
damping=4.0, damping=4.0,
), ),
"trunk": ImplicitActuatorCfg( "trunk": ImplicitActuatorCfg(
joint_names_expr=["PrismaticJoint"], joint_names_expr=["PrismaticJoint"],
effort_limit_sim=200.0, effort_limit_sim=200.0,
velocity_limit_sim=0.2, velocity_limit_sim=0.2,
stiffness=1000.0, stiffness=1000.0,
damping=100.0, damping=100.0,
), ),
"wheels": ImplicitActuatorCfg( "wheels": ImplicitActuatorCfg(
joint_names_expr=[".*_revolute_Joint"], joint_names_expr=[".*_revolute_Joint"],
effort_limit_sim=200.0, effort_limit_sim=200.0,
stiffness=0.0, stiffness=0.0,
damping=100.0, damping=100.0,
), ),
"grippers": ImplicitActuatorCfg( "grippers": ImplicitActuatorCfg(
joint_names_expr=[".*_hand_joint.*"], joint_names_expr=[".*_hand_joint.*"],
effort_limit_sim=200.0, effort_limit_sim=200.0,
stiffness=2e3, stiffness=2e3,
damping=1e2, damping=1e2,
), ),
}, },
soft_joint_pos_limit_factor=1.0, soft_joint_pos_limit_factor=1.0,
) )
"""Base configuration for MindRobot. Gravity enabled, low PD gains.""" """Base configuration for MindRobot. Gravity enabled, low PD gains."""
MINDBOT_HIGH_PD_CFG = MINDBOT_CFG.copy() MINDBOT_HIGH_PD_CFG = MINDBOT_CFG.copy()
MINDBOT_HIGH_PD_CFG.spawn.rigid_props.disable_gravity = True MINDBOT_HIGH_PD_CFG.actuators["left_arm_shoulder"].stiffness = 400.0
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_shoulder"].damping = 80.0 MINDBOT_HIGH_PD_CFG.actuators["left_arm_wrist"].stiffness = 400.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["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"].stiffness = 400.0 MINDBOT_HIGH_PD_CFG.actuators["right_arm_shoulder"].damping = 80.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"].stiffness = 400.0 MINDBOT_HIGH_PD_CFG.actuators["right_arm_wrist"].damping = 80.0
MINDBOT_HIGH_PD_CFG.actuators["right_arm_wrist"].damping = 80.0 """IK task-space control configuration. Gravity disabled, stiffer PD for tracking."""
"""IK task-space control configuration. Gravity disabled, stiffer PD for tracking."""

672
source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_left_arm_ik_env_cfg.py
View File

@@ -1,317 +1,355 @@
# Copyright (c) 2024, MindRobot Project # Copyright (c) 2024, MindRobot Project
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
"""MindRobot left arm IK-Rel environment config for teleoperation.""" """MindRobot left arm IK-Rel environment config for teleoperation."""
from __future__ import annotations from __future__ import annotations
from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR
import torch import torch
import isaaclab.sim as sim_utils import isaaclab.sim as sim_utils
from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg
from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg
from isaaclab.devices import DevicesCfg from isaaclab.devices import DevicesCfg
from isaaclab.devices.keyboard import Se3KeyboardCfg from isaaclab.devices.keyboard import Se3KeyboardCfg
from isaaclab.devices.spacemouse import Se3SpaceMouseCfg from isaaclab.devices.spacemouse import Se3SpaceMouseCfg
from isaaclab.devices.gamepad import Se3GamepadCfg from isaaclab.devices.gamepad import Se3GamepadCfg
from isaaclab.envs import ManagerBasedRLEnvCfg from isaaclab.envs import ManagerBasedRLEnvCfg
from isaaclab.envs.mdp.actions.actions_cfg import ( from isaaclab.envs.mdp.actions.actions_cfg import (
BinaryJointPositionActionCfg, BinaryJointPositionActionCfg,
DifferentialInverseKinematicsActionCfg, DifferentialInverseKinematicsActionCfg,
) JointVelocityActionCfg,
from isaaclab.managers import EventTermCfg as EventTerm )
from isaaclab.managers import ObservationGroupCfg as ObsGroup from isaaclab.managers import EventTermCfg as EventTerm
from isaaclab.managers import ObservationTermCfg as ObsTerm from isaaclab.managers import ObservationGroupCfg as ObsGroup
from isaaclab.managers import TerminationTermCfg as DoneTerm from isaaclab.managers import ObservationTermCfg as ObsTerm
from isaaclab.scene import InteractiveSceneCfg from isaaclab.managers import TerminationTermCfg as DoneTerm
from isaaclab.sensors.frame_transformer.frame_transformer_cfg import FrameTransformerCfg from isaaclab.scene import InteractiveSceneCfg
from isaaclab.sensors.frame_transformer.frame_transformer_cfg import OffsetCfg from isaaclab.sensors.frame_transformer.frame_transformer_cfg import FrameTransformerCfg
from isaaclab.sim.schemas.schemas_cfg import RigidBodyPropertiesCfg from isaaclab.sensors.frame_transformer.frame_transformer_cfg import OffsetCfg
from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg from isaaclab.sim.schemas.schemas_cfg import RigidBodyPropertiesCfg
from isaaclab.utils import configclass from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg
from isaaclab.markers.config import FRAME_MARKER_CFG from isaaclab.utils import configclass
from isaaclab.devices.openxr import XrCfg from isaaclab.markers.config import FRAME_MARKER_CFG
from isaaclab.devices.openxr import XrCfg
# 导入基础配置和MDP函数
from isaaclab_tasks.manager_based.manipulation.stack import mdp # 导入基础配置和MDP函数
from isaaclab_tasks.manager_based.manipulation.stack import mdp
# 导入 MindRobot 资产
from .mindrobot_cfg import MINDBOT_HIGH_PD_CFG # 导入 MindRobot 资产
from .mindrobot_cfg import MINDBOT_HIGH_PD_CFG
# 在文件开头添加
import isaaclab.utils.assets as assets_utils # 在文件开头添加
from mindbot.utils.assets import MINDBOT_ASSETS_DIR import isaaclab.utils.assets as assets_utils
from mindbot.utils.assets import MINDBOT_ASSETS_DIR
# # 然后在 scene 配置中使用
# spawn=sim_utils.UsdFileCfg( # # 然后在 scene 配置中使用
# usd_path=f"{assets_utils.ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd" # spawn=sim_utils.UsdFileCfg(
# ), # usd_path=f"{assets_utils.ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd"
# ===================================================================== # ),
# Scene Configuration # =====================================================================
# ===================================================================== # Scene Configuration
# =====================================================================
@configclass
class MindRobotTeleopSceneCfg(InteractiveSceneCfg): @configclass
"""Minimal scene for MindRobot teleoperation: robot + table + optional cube.""" class MindRobotTeleopSceneCfg(InteractiveSceneCfg):
"""Minimal scene for MindRobot teleoperation: robot + table + optional cube."""
# Ground plane
plane = AssetBaseCfg( # Ground plane
prim_path="/World/GroundPlane", plane = AssetBaseCfg(
init_state=AssetBaseCfg.InitialStateCfg(pos=[0.0, 0.0, 0.0]), prim_path="/World/GroundPlane",
spawn=GroundPlaneCfg(), init_state=AssetBaseCfg.InitialStateCfg(pos=[0.0, 0.0, 0.0]),
) spawn=GroundPlaneCfg(),
)
# # Table
# table = AssetBaseCfg( # # Table
# prim_path="{ENV_REGEX_NS}/Table", # table = AssetBaseCfg(
# init_state=AssetBaseCfg.InitialStateCfg(pos=[0.5, 0, 0], rot=[0.707, 0, 0, 0.707]), # prim_path="{ENV_REGEX_NS}/Table",
# spawn=sim_utils.UsdFileCfg( # init_state=AssetBaseCfg.InitialStateCfg(pos=[0.5, 0, 0], rot=[0.707, 0, 0, 0.707]),
# usd_path=f"{assets_utils.ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd" # spawn=sim_utils.UsdFileCfg(
# ), # usd_path=f"{assets_utils.ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd"
# ) # ),
# )
# Optional: Single cube for testing (can be removed if not needed)
cube = RigidObjectCfg( # Optional: Single cube for testing (can be removed if not needed)
prim_path="{ENV_REGEX_NS}/Cube", cube = RigidObjectCfg(
spawn=sim_utils.CuboidCfg( prim_path="{ENV_REGEX_NS}/Cube",
size=(0.04, 0.04, 0.04), spawn=sim_utils.CuboidCfg(
rigid_props=RigidBodyPropertiesCfg(), size=(0.04, 0.04, 0.04),
mass_props=sim_utils.MassPropertiesCfg(mass=0.1), rigid_props=RigidBodyPropertiesCfg(),
collision_props=sim_utils.CollisionPropertiesCfg(), mass_props=sim_utils.MassPropertiesCfg(mass=0.1),
visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)), collision_props=sim_utils.CollisionPropertiesCfg(),
), visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)),
init_state=RigidObjectCfg.InitialStateCfg(pos=(0.5, 0.0, 0.45)), ),
) init_state=RigidObjectCfg.InitialStateCfg(pos=(0.5, 0.0, 0.45)),
)
# room = AssetBaseCfg(
# prim_path="{ENV_REGEX_NS}/Room", # room = AssetBaseCfg(
# spawn=sim_utils.UsdFileCfg( # prim_path="{ENV_REGEX_NS}/Room",
# usd_path=f"{MINDBOT_ASSETS_DIR}/twinlab/Collected_lab2/lab.usd", # spawn=sim_utils.UsdFileCfg(
# ), # usd_path=f"{MINDBOT_ASSETS_DIR}/twinlab/Collected_lab2/lab.usd",
# ) # ),
# )
# MindRobot
robot: ArticulationCfg = MINDBOT_HIGH_PD_CFG.replace( # MindRobot
prim_path="{ENV_REGEX_NS}/Robot" robot: ArticulationCfg = MINDBOT_HIGH_PD_CFG.replace(
) prim_path="{ENV_REGEX_NS}/Robot"
)
# Lighting
light = AssetBaseCfg( # Lighting
prim_path="/World/light", light = AssetBaseCfg(
spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), prim_path="/World/light",
) spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0),
)
# End-effector frame transformer
ee_frame: FrameTransformerCfg = None # Will be set in __post_init__ # End-effector frame transformer
ee_frame: FrameTransformerCfg = None # Will be set in __post_init__
# =====================================================================
# Actions Configuration # =====================================================================
# ===================================================================== # Actions Configuration
# =====================================================================
@configclass
class MindRobotTeleopActionsCfg: @configclass
"""Actions for MindRobot left arm IK teleoperation.""" class MindRobotTeleopActionsCfg:
"""Actions for MindRobot left arm IK teleoperation."""
# Left arm IK control
arm_action = DifferentialInverseKinematicsActionCfg( # Left arm IK control
asset_name="robot", arm_action = DifferentialInverseKinematicsActionCfg(
joint_names=["l_joint[1-6]"], asset_name="robot",
body_name="left_hand_body", joint_names=["l_joint[1-6]"],
controller=DifferentialIKControllerCfg( body_name="left_hand_body",
command_type="pose", controller=DifferentialIKControllerCfg(
use_relative_mode=True, command_type="pose",
ik_method="dls", use_relative_mode=True,
), ik_method="dls",
scale=1, ),
body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg( scale=1,
pos=[0.0, 0.0, 0.0], body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(
), pos=[0.0, 0.0, 0.0],
) ),
)
# Left gripper control (binary: open/close)
gripper_action = BinaryJointPositionActionCfg( # Wheel velocity control for differential drive (skid-steer).
asset_name="robot", # Joint order in articulation: right_b, left_b, left_f, right_f
joint_names=["left_hand_joint_left", "left_hand_joint_right"], # (from terminal joint index listing: [2],[3],[4],[5]).
open_command_expr={ # Action vector: [right_b_vel, left_b_vel, left_f_vel, right_f_vel] in rad/s.
"left_hand_joint_left": 0.0, wheel_action = JointVelocityActionCfg(
"left_hand_joint_right": 0.0, asset_name="robot",
}, joint_names=[".*_revolute_Joint"],
close_command_expr={ scale=1.0,
"left_hand_joint_left": -0.03, )
"left_hand_joint_right": 0.03,
}, # Left gripper control (binary: open/close)
) gripper_action = BinaryJointPositionActionCfg(
asset_name="robot",
joint_names=["left_hand_joint_left", "left_hand_joint_right"],
# ===================================================================== open_command_expr={
# Observations Configuration "left_hand_joint_left": 0.0,
# ===================================================================== "left_hand_joint_right": 0.0,
},
close_command_expr={
@configclass "left_hand_joint_left": -0.03,
class MindRobotTeleopObsCfg: "left_hand_joint_right": 0.03,
"""Minimal observations for MindRobot teleoperation.""" },
)
@configclass
class PolicyCfg(ObsGroup):
"""Observations for policy group - only robot state, no cube dependencies.""" # =====================================================================
# Observations Configuration
actions = ObsTerm(func=mdp.last_action) # =====================================================================
joint_pos = ObsTerm(func=mdp.joint_pos_rel)
joint_vel = ObsTerm(func=mdp.joint_vel_rel)
eef_pos = ObsTerm(func=mdp.ee_frame_pos) @configclass
eef_quat = ObsTerm(func=mdp.ee_frame_quat) class MindRobotTeleopObsCfg:
gripper_pos = ObsTerm(func=mdp.gripper_pos) """Minimal observations for MindRobot teleoperation."""
def __post_init__(self): @configclass
self.enable_corruption = False class PolicyCfg(ObsGroup):
self.concatenate_terms = False """Observations for policy group - only robot state, no cube dependencies."""
# observation groups actions = ObsTerm(func=mdp.last_action)
policy: PolicyCfg = PolicyCfg() joint_pos = ObsTerm(func=mdp.joint_pos_rel)
joint_vel = ObsTerm(func=mdp.joint_vel_rel)
eef_pos = ObsTerm(func=mdp.ee_frame_pos)
# ===================================================================== eef_quat = ObsTerm(func=mdp.ee_frame_quat)
# Terminations Configuration gripper_pos = ObsTerm(func=mdp.gripper_pos)
# =====================================================================
def __post_init__(self):
self.enable_corruption = False
@configclass self.concatenate_terms = False
class MindRobotTeleopTerminationsCfg:
"""Minimal terminations for teleoperation - only time_out or none.""" # observation groups
policy: PolicyCfg = PolicyCfg()
# Optional: Keep time_out for safety, or remove entirely
time_out = DoneTerm(func=mdp.time_out, time_out=True)
# =====================================================================
# Terminations Configuration
# ===================================================================== # =====================================================================
# Events Configuration
# =====================================================================
@configclass
@configclass class MindRobotTeleopTerminationsCfg:
class MindRobotTeleopEventsCfg: """Minimal terminations for teleoperation - only time_out or none."""
"""Reset events for teleoperation: R键重置时将场景恢复到初始状态。"""
# Optional: Keep time_out for safety, or remove entirely
reset_scene = EventTerm( time_out = DoneTerm(func=mdp.time_out, time_out=True)
func=mdp.reset_scene_to_default,
mode="reset",
) # =====================================================================
# Events Configuration
# =====================================================================
# =====================================================================
# Main Environment Configuration
# ===================================================================== def _disable_arm_gravity(env, env_ids: torch.Tensor):
"""Disable gravity for both arm subtrees; chassis/wheels/trunk keep gravity.
@configclass Called once at startup. The @apply_nested decorator on
class EmptyCfg: modify_rigid_body_properties recurses into all rigid-body children under
pass the given prim path, so every link of the arm (including gripper) is covered.
Arm prim roots (per env):
@configclass Robot/rm_65_fb_left — left RM-65 arm + gripper
class MindRobotLeftArmIKEnvCfg(ManagerBasedRLEnvCfg): Robot/rm_65_fb_right — right RM-65 arm + gripper
"""MindRobot 左臂 IK-Rel 遥操作环境配置（最小化版本）。""" """
import isaaclab.sim.schemas as schemas
# Scene settings
scene: MindRobotTeleopSceneCfg = MindRobotTeleopSceneCfg( arm_cfg = RigidBodyPropertiesCfg(disable_gravity=True)
num_envs=1, for env_id in range(env.num_envs):
env_spacing=2.5, for arm_path in [
) f"/World/envs/env_{env_id}/Robot/rm_65_fb_left",
f"/World/envs/env_{env_id}/Robot/rm_65_b_right",
# Basic settings ]:
observations: MindRobotTeleopObsCfg = MindRobotTeleopObsCfg() schemas.modify_rigid_body_properties(arm_path, arm_cfg)
actions: MindRobotTeleopActionsCfg = MindRobotTeleopActionsCfg()
# MDP settings @configclass
terminations: MindRobotTeleopTerminationsCfg = MindRobotTeleopTerminationsCfg() class MindRobotTeleopEventsCfg:
events: MindRobotTeleopEventsCfg = MindRobotTeleopEventsCfg() """Reset events for teleoperation: R键重置时将场景恢复到初始状态。"""
# Unused managers disable_arm_gravity = EventTerm(
commands: EmptyCfg = EmptyCfg() func=_disable_arm_gravity,
rewards: EmptyCfg = EmptyCfg() mode="startup",
curriculum: EmptyCfg = EmptyCfg() )
# XR configuration for hand tracking (if needed) reset_scene = EventTerm(
xr: XrCfg = XrCfg( func=mdp.reset_scene_to_default,
anchor_pos=(-0.1, -0.5, -1.05), mode="reset",
anchor_rot=(0.866, 0, 0, -0.5), )
)
def __post_init__(self): # =====================================================================
"""Post initialization.""" # Main Environment Configuration
super().__post_init__() # =====================================================================
# General settings
self.decimation = 2 @configclass
self.episode_length_s = 50.0 class EmptyCfg:
pass
# Simulation settings
self.sim.dt = 0.01 # 100Hz
self.sim.render_interval = 2 @configclass
class MindRobotLeftArmIKEnvCfg(ManagerBasedRLEnvCfg):
# Set MindRobot with FIXED root for teleoperation """MindRobot 左臂 IK-Rel 遥操作环境配置（最小化版本）。"""
# The original MINDBOT_HIGH_PD_CFG has fix_root_link=False (mobile base).
# For teleoperation, the base MUST be fixed to prevent the whole robot # Scene settings
# from sliding/tipping when IK applies torques to the arm joints. scene: MindRobotTeleopSceneCfg = MindRobotTeleopSceneCfg(
robot_cfg = MINDBOT_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") num_envs=1,
robot_cfg.spawn.articulation_props.fix_root_link = True env_spacing=2.5,
self.scene.robot = robot_cfg )
# Configure end-effector frame transformer # Basic settings
marker_cfg = FRAME_MARKER_CFG.copy() observations: MindRobotTeleopObsCfg = MindRobotTeleopObsCfg()
marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) actions: MindRobotTeleopActionsCfg = MindRobotTeleopActionsCfg()
marker_cfg.prim_path = "/Visuals/FrameTransformer"
self.scene.ee_frame = FrameTransformerCfg( # MDP settings
prim_path="{ENV_REGEX_NS}/Robot/rm_65_fb_left/base_link", terminations: MindRobotTeleopTerminationsCfg = MindRobotTeleopTerminationsCfg()
debug_vis=False, events: MindRobotTeleopEventsCfg = MindRobotTeleopEventsCfg()
visualizer_cfg=marker_cfg,
target_frames=[ # Unused managers
FrameTransformerCfg.FrameCfg( commands: EmptyCfg = EmptyCfg()
prim_path="{ENV_REGEX_NS}/Robot/rm_65_fb_left/l_hand_01/left_hand_body", rewards: EmptyCfg = EmptyCfg()
name="end_effector", curriculum: EmptyCfg = EmptyCfg()
offset=OffsetCfg(
pos=[0.0, 0.0, 0.0], # XR configuration for hand tracking (if needed)
), xr: XrCfg = XrCfg(
), anchor_pos=(-0.1, -0.5, -1.05),
], anchor_rot=(0.866, 0, 0, -0.5),
) )
def __post_init__(self):
# Configure teleoperation devices """Post initialization."""
self.teleop_devices = DevicesCfg( super().__post_init__()
devices={
"keyboard": Se3KeyboardCfg( # General settings
pos_sensitivity=0.05, self.decimation = 2
rot_sensitivity=0.05, self.episode_length_s = 50.0
sim_device=self.sim.device,
), # Simulation settings
"spacemouse": Se3SpaceMouseCfg( self.sim.dt = 0.01 # 100Hz
pos_sensitivity=0.05, self.sim.render_interval = 2
rot_sensitivity=0.05,
sim_device=self.sim.device, # Keep the base mobile for teleoperation.
), # The absolute arm IK command must therefore be expressed in the robot
"gamepad": Se3GamepadCfg( # root frame instead of assuming a fixed world-aligned base.
pos_sensitivity=0.1, robot_cfg = MINDBOT_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
rot_sensitivity=0.1, robot_cfg.spawn.articulation_props.fix_root_link = False
sim_device=self.sim.device, self.scene.robot = robot_cfg
),
} # Configure end-effector frame transformer
) marker_cfg = FRAME_MARKER_CFG.copy()
marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1)
# Gripper parameters for status checking marker_cfg.prim_path = "/Visuals/FrameTransformer"
self.gripper_joint_names = ["left_hand_joint_left", "left_hand_joint_right"] self.scene.ee_frame = FrameTransformerCfg(
self.gripper_open_val = 0.0 prim_path="{ENV_REGEX_NS}/Robot/rm_65_fb_left/base_link",
self.gripper_threshold = 0.005 debug_vis=False,
visualizer_cfg=marker_cfg,
target_frames=[
@configclass FrameTransformerCfg.FrameCfg(
class MindRobotLeftArmIKAbsEnvCfg(MindRobotLeftArmIKEnvCfg): prim_path="{ENV_REGEX_NS}/Robot/rm_65_fb_left/l_hand_01/left_hand_body",
"""MindRobot 左臂 IK-Abs 遥操作环境配置（绝对坐标系，适合 VR 控制器）。""" name="end_effector",
offset=OffsetCfg(
def __post_init__(self): pos=[0.0, 0.0, 0.0],
super().__post_init__() ),
# Switch controller to absolute mode ),
self.actions.arm_action.controller.use_relative_mode = False ],
)
# Configure 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 parameters for status checking
self.gripper_joint_names = ["left_hand_joint_left", "left_hand_joint_right"]
self.gripper_open_val = 0.0
self.gripper_threshold = 0.005
@configclass
class MindRobotLeftArmIKAbsEnvCfg(MindRobotLeftArmIKEnvCfg):
"""MindRobot 左臂 IK-Abs 遥操作环境配置（绝对坐标系，适合 VR 控制器）。"""
def __post_init__(self):
super().__post_init__()
# Switch controller to absolute mode
self.actions.arm_action.controller.use_relative_mode = False