重构xr teleop
This commit is contained in:
@@ -13,7 +13,7 @@ The following configurations are available:
|
||||
"""
|
||||
|
||||
import isaaclab.sim as sim_utils
|
||||
from isaaclab.actuators import ImplicitActuatorCfg
|
||||
from isaaclab.actuators import IdealPDActuatorCfg, ImplicitActuatorCfg
|
||||
from isaaclab.assets import ArticulationCfg
|
||||
|
||||
from mindbot.utils.assets import MINDBOT_ASSETS_DIR
|
||||
@@ -21,7 +21,7 @@ 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",
|
||||
usd_path=f"{MINDBOT_ASSETS_DIR}/robots/mindrobot_2i/contact_sensor.usd",
|
||||
usd_path=f"{MINDBOT_ASSETS_DIR}/robots/mindbot_zed/mindbot.usd",
|
||||
activate_contact_sensors=True,
|
||||
rigid_props=sim_utils.RigidBodyPropertiesCfg(
|
||||
disable_gravity=False,
|
||||
@@ -29,7 +29,7 @@ MINDBOT_CFG = ArticulationCfg(
|
||||
),
|
||||
articulation_props=sim_utils.ArticulationRootPropertiesCfg(
|
||||
fix_root_link=False,
|
||||
enabled_self_collisions=True,
|
||||
enabled_self_collisions=False, # TODO: re-enable after fixing trunk self-collision
|
||||
solver_position_iteration_count=8,
|
||||
solver_velocity_iteration_count=0,
|
||||
),
|
||||
@@ -58,11 +58,14 @@ MINDBOT_CFG = ArticulationCfg(
|
||||
"right_hand_joint_left": 0.0,
|
||||
"right_hand_joint_right": 0.0,
|
||||
# ====== Trunk & Head ======
|
||||
# NOTE: pos z = 0.44 (base clearance) + PrismaticJoint value.
|
||||
# If you change PrismaticJoint, update pos z accordingly to avoid
|
||||
# geometry clipping that causes the robot to fall on spawn.
|
||||
"PrismaticJoint": 0.26,
|
||||
"head_pitch_Joint": 0.0,
|
||||
"head_yaw_Joint": 0.0,
|
||||
},
|
||||
pos=(0.0, 0.0, 0.7),
|
||||
pos=(0.001, 0.001, 0.74), # z = 0.44 (base clearance) + 0.26 (PrismaticJoint)
|
||||
),
|
||||
actuators={
|
||||
# RM-65 J1-J3: shoulder/elbow — max 180°/s ≈ 3.14 rad/s
|
||||
@@ -104,12 +107,16 @@ MINDBOT_CFG = ArticulationCfg(
|
||||
stiffness=80.0,
|
||||
damping=4.0,
|
||||
),
|
||||
# IdealPDActuator: explicitly computes F = k_p*(target-pos) + k_d*(target_vel-vel)
|
||||
# and sends via set_dof_actuation_forces(), bypassing PhysX drive which
|
||||
# doesn't work correctly for prismatic joints with set_dof_position_targets().
|
||||
# NOTE: trunk gravity is disabled in USD, so moderate stiffness is sufficient.
|
||||
"trunk": ImplicitActuatorCfg(
|
||||
joint_names_expr=["PrismaticJoint"],
|
||||
effort_limit_sim=200.0,
|
||||
velocity_limit_sim=0.2,
|
||||
stiffness=1000.0,
|
||||
damping=100.0,
|
||||
effort_limit=100.0,
|
||||
velocity_limit=0.2,
|
||||
stiffness=10000.0,
|
||||
damping=1000.0,
|
||||
),
|
||||
"wheels": ImplicitActuatorCfg(
|
||||
# joint_names_expr=[".*_revolute_Joint"],
|
||||
@@ -168,6 +175,8 @@ MINDBOT_HEAD_JOIONTS =[
|
||||
"head_pitch_Joint"
|
||||
]
|
||||
|
||||
MINDBOT_TRUNK_JOINT = "PrismaticJoint"
|
||||
|
||||
# EEF body names (as defined in the USD asset)
|
||||
MINDBOT_LEFT_EEF_BODY = "left_hand_body"
|
||||
MINDBOT_RIGHT_EEF_BODY = "right_hand_body"
|
||||
|
||||
@@ -30,6 +30,7 @@ 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.sensors import CameraCfg
|
||||
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
|
||||
@@ -48,6 +49,7 @@ from .mindrobot_2i_cfg import (
|
||||
MINDBOT_RIGHT_GRIPPER_JOINTS,
|
||||
MINDBOT_WHEEL_JOINTS,
|
||||
MINDBOT_HEAD_JOIONTS,
|
||||
MINDBOT_TRUNK_JOINT,
|
||||
MINDBOT_LEFT_ARM_PRIM_ROOT,
|
||||
MINDBOT_RIGHT_ARM_PRIM_ROOT,
|
||||
MINDBOT_LEFT_EEF_PRIM,
|
||||
@@ -96,6 +98,28 @@ class MindRobotDualArmSceneCfg(InteractiveSceneCfg):
|
||||
ee_frame: FrameTransformerCfg = None
|
||||
ee_frame_right: FrameTransformerCfg = None
|
||||
|
||||
# VR stereo cameras — reuse existing ZED_X CameraLeft/CameraRight prims from USD
|
||||
# (spawn=None means use existing prim, don't create a new one)
|
||||
# The ZED_X baseline in USD is 120mm; for perfect VR IPD we'd ideally set 65mm,
|
||||
# but reusing existing prims avoids spawn issues. stereoOffset=0 in video_source.yml
|
||||
# since sim rendering gives correct perspective regardless of physical baseline.
|
||||
vr_left_eye: CameraCfg = CameraCfg(
|
||||
prim_path="{ENV_REGEX_NS}/Robot/robot_head/ZED_X/base_link/ZED_X/CameraLeft",
|
||||
update_period=1 / 30,
|
||||
height=1440, # 2560x1440 per eye → SBS 5120x1440; effective ~1920x1440 after shader crop
|
||||
width=2560,
|
||||
data_types=["rgb"],
|
||||
spawn=None,
|
||||
)
|
||||
vr_right_eye: CameraCfg = CameraCfg(
|
||||
prim_path="{ENV_REGEX_NS}/Robot/robot_head/ZED_X/base_link/ZED_X/CameraRight",
|
||||
update_period=1 / 30,
|
||||
height=1440,
|
||||
width=2560,
|
||||
data_types=["rgb"],
|
||||
spawn=None,
|
||||
)
|
||||
|
||||
|
||||
# =====================================================================
|
||||
# Actions Configuration
|
||||
@@ -166,6 +190,13 @@ class MindRobotDualArmActionsCfg:
|
||||
scale=1.0,
|
||||
)
|
||||
|
||||
trunk_action = JointPositionActionCfg(
|
||||
asset_name="robot",
|
||||
joint_names=[MINDBOT_TRUNK_JOINT],
|
||||
scale=1.0,
|
||||
use_default_offset=False, # action = absolute target, not offset from default
|
||||
)
|
||||
|
||||
|
||||
# =====================================================================
|
||||
# Observations Configuration
|
||||
@@ -236,12 +267,29 @@ def _disable_arm_gravity(env, env_ids: torch.Tensor):
|
||||
schemas.modify_rigid_body_properties(arm_path, arm_cfg)
|
||||
|
||||
|
||||
def _disable_trunk_head_gravity(env, env_ids: torch.Tensor):
|
||||
"""Disable gravity for trunk and head at startup."""
|
||||
import isaaclab.sim.schemas as schemas
|
||||
|
||||
cfg = RigidBodyPropertiesCfg(disable_gravity=True)
|
||||
for env_id in range(env.num_envs):
|
||||
for prim_path in [
|
||||
f"/World/envs/env_{env_id}/Robot/robot_trunk",
|
||||
f"/World/envs/env_{env_id}/Robot/robot_head",
|
||||
]:
|
||||
schemas.modify_rigid_body_properties(prim_path, cfg)
|
||||
|
||||
|
||||
@configclass
|
||||
class MindRobotDualArmEventsCfg:
|
||||
disable_arm_gravity = EventTerm(
|
||||
func=_disable_arm_gravity,
|
||||
mode="startup",
|
||||
)
|
||||
disable_trunk_head_gravity = EventTerm(
|
||||
func=_disable_trunk_head_gravity,
|
||||
mode="startup",
|
||||
)
|
||||
reset_scene = EventTerm(
|
||||
func=mdp.reset_scene_to_default,
|
||||
mode="reset",
|
||||
@@ -292,7 +340,7 @@ class MindRobot2iDualArmIKAbsEnvCfg(ManagerBasedRLEnvCfg):
|
||||
lookat=(0.0, 0.0, 0.8), # 瞄准点（机器人腰部高度附近）
|
||||
origin_type="world",
|
||||
)
|
||||
self.decimation = 2
|
||||
self.decimation = 2 # 25 Hz control (was 2 = 50 Hz, unnecessary for teleop)
|
||||
self.episode_length_s = 50.0
|
||||
self.sim.dt = 0.01 # 100 Hz
|
||||
self.sim.render_interval = 2
|
||||
|
||||
@@ -56,10 +56,13 @@ MINDBOT_CFG = ArticulationCfg(
|
||||
"right_hand_joint_left": 0.0,
|
||||
"right_hand_joint_right": 0.0,
|
||||
# ====== Trunk & Head ======
|
||||
# NOTE: pos z = 0.44 (base clearance) + PrismaticJoint value.
|
||||
# If you change PrismaticJoint, update pos z accordingly to avoid
|
||||
# geometry clipping that causes the robot to fall on spawn.
|
||||
"PrismaticJoint": 0.26,
|
||||
"head_revoluteJoint": 0.0,
|
||||
},
|
||||
pos=(0.0, 0.0, 0.7),
|
||||
pos=(0.0, 0.0, 0.7), # z = 0.44 (base clearance) + 0.26 (PrismaticJoint)
|
||||
),
|
||||
actuators={
|
||||
# RM-65 J1-J3: shoulder/elbow — max 180°/s ≈ 3.14 rad/s
|
||||
|
||||
@@ -283,7 +283,7 @@ class MindRobotDualArmIKAbsEnvCfg(ManagerBasedRLEnvCfg):
|
||||
lookat=(0.0, 0.0, 0.8), # 瞄准点（机器人腰部高度附近）
|
||||
origin_type="world",
|
||||
)
|
||||
self.decimation = 2
|
||||
self.decimation = 2 # 25 Hz control (was 2 = 50 Hz, unnecessary for teleop)
|
||||
self.episode_length_s = 50.0
|
||||
self.sim.dt = 0.01 # 100 Hz
|
||||
self.sim.render_interval = 2
|
||||
|
||||
@@ -20,7 +20,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/tangger/LYT/maic_usd_assets_moudle",
|
||||
# "/home/maic/xh/maic_usd_assets_moudle"
|
||||
# "/home/maic/LYT/maic_usd_assets_moudle",
|
||||
)
|
||||
|
||||
238
source/mindbot/mindbot/utils/sim_video_streamer.py
Normal file
238
source/mindbot/mindbot/utils/sim_video_streamer.py
Normal file
@@ -0,0 +1,238 @@
|
||||
# SPDX-FileCopyrightText: Copyright (c) 2022-2025, The Isaac Lab Project Developers.
|
||||
# SPDX-License-Identifier: BSD-3-Clause
|
||||
|
||||
"""Simulation stereo video streamer for VR headset.
|
||||
|
||||
Encodes stereo RGB frames (left + right eye) as H264 side-by-side video
|
||||
and streams to the Unity VR client over TCP, using the same 4-byte
|
||||
big-endian length-prefixed NALU protocol as RobotVisionConsole.
|
||||
|
||||
Usage:
|
||||
streamer = SimVideoStreamer("172.20.103.171", 12345, width=960, height=540)
|
||||
streamer.connect()
|
||||
# In the sim loop:
|
||||
streamer.send_frame(left_rgb_np, right_rgb_np)
|
||||
# Cleanup:
|
||||
streamer.close()
|
||||
|
||||
The Unity client (PICO) acts as TCP SERVER on the specified port.
|
||||
This streamer connects as TCP CLIENT — same role as RobotVisionConsole.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import logging
|
||||
import socket
|
||||
import struct
|
||||
import threading
|
||||
import time
|
||||
from typing import Optional
|
||||
|
||||
import av
|
||||
import numpy as np
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
class SimVideoStreamer:
|
||||
"""H264 stereo video streamer over TCP for VR headset display.
|
||||
|
||||
Accepts left/right RGB numpy arrays, assembles a side-by-side frame,
|
||||
encodes to H264 with PyAV, and sends each NALU packet prefixed with
|
||||
a 4-byte big-endian length header over a TCP socket.
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
host: str,
|
||||
port: int = 12345,
|
||||
width: int = 960,
|
||||
height: int = 540,
|
||||
fps: int = 30,
|
||||
bitrate: int = 20_000_000,
|
||||
):
|
||||
"""Initialize the streamer.
|
||||
|
||||
Args:
|
||||
host: IP address of the PICO VR headset (TCP server).
|
||||
port: TCP port the PICO is listening on.
|
||||
width: Width of EACH eye image (SBS output = width*2 x height).
|
||||
height: Height of each eye image.
|
||||
fps: Target frame rate for H264 encoding.
|
||||
bitrate: H264 encoding bitrate in bits per second.
|
||||
"""
|
||||
self.host = host
|
||||
self.port = port
|
||||
self.eye_width = width
|
||||
self.eye_height = height
|
||||
self.sbs_width = width * 2
|
||||
self.sbs_height = height
|
||||
self.fps = fps
|
||||
self.bitrate = bitrate
|
||||
|
||||
self._sock: Optional[socket.socket] = None
|
||||
self._codec_ctx: Optional[av.CodecContext] = None
|
||||
self._frame_idx = 0
|
||||
self._last_frame_time = 0.0
|
||||
self._min_frame_interval = 1.0 / fps
|
||||
|
||||
# Async encoding: background thread picks up frames from _pending_frame
|
||||
self._pending_frame: Optional[np.ndarray] = None
|
||||
self._frame_lock = threading.Lock()
|
||||
self._encode_thread: Optional[threading.Thread] = None
|
||||
self._running = False
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Public API
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def connect(self) -> bool:
|
||||
"""Connect to the PICO VR headset TCP server.
|
||||
|
||||
Returns:
|
||||
True if connection succeeded.
|
||||
"""
|
||||
try:
|
||||
self._sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
self._sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
|
||||
self._sock.connect((self.host, self.port))
|
||||
logger.info(f"[SimVideoStreamer] Connected to {self.host}:{self.port}")
|
||||
except OSError as e:
|
||||
logger.error(f"[SimVideoStreamer] TCP connect failed: {e}")
|
||||
self._sock = None
|
||||
return False
|
||||
|
||||
self._init_encoder()
|
||||
self._frame_idx = 0
|
||||
self._last_frame_time = 0.0
|
||||
|
||||
# Start background encode+send thread
|
||||
self._running = True
|
||||
self._encode_thread = threading.Thread(target=self._encode_loop, daemon=True)
|
||||
self._encode_thread.start()
|
||||
return True
|
||||
|
||||
def send_frame(self, left_rgb: np.ndarray, right_rgb: np.ndarray) -> bool:
|
||||
"""Submit a stereo frame for async encoding and sending.
|
||||
|
||||
This method is NON-BLOCKING: it copies the frame data and returns
|
||||
immediately. The background thread handles encoding and TCP send.
|
||||
If the previous frame hasn't been encoded yet, this frame replaces it
|
||||
(latest-frame-wins, no queue buildup).
|
||||
|
||||
Args:
|
||||
left_rgb: Left eye image, shape (H, W, 3), dtype uint8, RGB order.
|
||||
right_rgb: Right eye image, shape (H, W, 3), dtype uint8, RGB order.
|
||||
|
||||
Returns:
|
||||
True if frame was accepted. False if connection is lost.
|
||||
"""
|
||||
if not self._running:
|
||||
return False
|
||||
|
||||
# Assemble SBS and hand off to background thread (latest-frame-wins)
|
||||
sbs = np.concatenate([left_rgb, right_rgb], axis=1) # (H, W*2, 3)
|
||||
with self._frame_lock:
|
||||
self._pending_frame = sbs
|
||||
|
||||
return True
|
||||
|
||||
def close(self):
|
||||
"""Stop background thread, flush encoder, and close connection."""
|
||||
self._running = False
|
||||
if self._encode_thread is not None:
|
||||
self._encode_thread.join(timeout=2.0)
|
||||
self._encode_thread = None
|
||||
|
||||
if self._codec_ctx is not None:
|
||||
try:
|
||||
packets = self._codec_ctx.encode(None)
|
||||
if self._sock is not None:
|
||||
for pkt in packets:
|
||||
nalu_data = bytes(pkt)
|
||||
header = struct.pack(">I", len(nalu_data))
|
||||
self._sock.sendall(header + nalu_data)
|
||||
except Exception:
|
||||
pass
|
||||
self._codec_ctx = None
|
||||
|
||||
if self._sock is not None:
|
||||
try:
|
||||
self._sock.close()
|
||||
except Exception:
|
||||
pass
|
||||
self._sock = None
|
||||
logger.info("[SimVideoStreamer] Connection closed.")
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Private
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def _init_encoder(self):
|
||||
"""Initialize H264 encoder via PyAV."""
|
||||
codec = av.Codec("libx264", "w")
|
||||
self._codec_ctx = codec.create()
|
||||
self._codec_ctx.width = self.sbs_width
|
||||
self._codec_ctx.height = self.sbs_height
|
||||
self._codec_ctx.pix_fmt = "yuv420p"
|
||||
from fractions import Fraction
|
||||
self._codec_ctx.time_base = Fraction(1, self.fps)
|
||||
self._codec_ctx.framerate = Fraction(self.fps, 1)
|
||||
self._codec_ctx.bit_rate = self.bitrate
|
||||
self._codec_ctx.gop_size = self.fps # keyframe every 1 second
|
||||
self._codec_ctx.max_b_frames = 0 # no B-frames for low latency
|
||||
self._codec_ctx.thread_count = 2
|
||||
|
||||
# Low-latency encoding options
|
||||
self._codec_ctx.options = {
|
||||
"preset": "ultrafast",
|
||||
"tune": "zerolatency",
|
||||
"profile": "baseline", # widest decoder compatibility
|
||||
}
|
||||
|
||||
self._codec_ctx.open()
|
||||
logger.info(
|
||||
f"[SimVideoStreamer] H264 encoder initialized: "
|
||||
f"{self.sbs_width}x{self.sbs_height} @ {self.fps}fps, "
|
||||
f"bitrate={self.bitrate/1e6:.1f}Mbps"
|
||||
)
|
||||
|
||||
def _encode_loop(self):
|
||||
"""Background thread: encode and send frames at target fps."""
|
||||
while self._running:
|
||||
# Grab the latest pending frame (if any)
|
||||
with self._frame_lock:
|
||||
sbs = self._pending_frame
|
||||
self._pending_frame = None
|
||||
|
||||
if sbs is None:
|
||||
time.sleep(0.001) # 1ms idle wait
|
||||
continue
|
||||
|
||||
# Rate limit encoding to target fps
|
||||
now = time.monotonic()
|
||||
if now - self._last_frame_time < self._min_frame_interval:
|
||||
continue
|
||||
self._last_frame_time = now
|
||||
|
||||
# Encode
|
||||
frame = av.VideoFrame.from_ndarray(sbs, format="rgb24")
|
||||
frame.pts = self._frame_idx
|
||||
self._frame_idx += 1
|
||||
|
||||
try:
|
||||
packets = self._codec_ctx.encode(frame)
|
||||
except av.error.FFmpegError as e:
|
||||
logger.warning(f"[SimVideoStreamer] Encode error: {e}")
|
||||
continue
|
||||
|
||||
# Send each packet
|
||||
for pkt in packets:
|
||||
nalu_data = bytes(pkt)
|
||||
header = struct.pack(">I", len(nalu_data))
|
||||
try:
|
||||
self._sock.sendall(header + nalu_data)
|
||||
except (BrokenPipeError, ConnectionResetError, OSError) as e:
|
||||
logger.error(f"[SimVideoStreamer] Send failed: {e}")
|
||||
self._running = False
|
||||
return
|
||||
Reference in New Issue
Block a user