1524 lines
63 KiB
Python
Executable File
1524 lines
63 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
"""Local TS1P isomorphic-arm bridge for TianGong 3.0.
|
|
|
|
Arm targets use the vendor ``/encoder_identical_joint`` input, while guarded
|
|
HBWALK velocity commands use the vendor ``/hric/robot/cmd_vel`` input. This
|
|
bridge never changes the robot FSM; HBWALK must already be active and running.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import argparse
|
|
import json
|
|
import math
|
|
import os
|
|
import struct
|
|
import threading
|
|
import time
|
|
import tomllib
|
|
from dataclasses import dataclass
|
|
from pathlib import Path
|
|
from typing import Any
|
|
|
|
import rclpy
|
|
import zmq
|
|
from brainco_hand_msgs.msg import MotorStatus, SetMotorMulti
|
|
from diagnostic_msgs.msg import DiagnosticStatus
|
|
from geometry_msgs.msg import TwistStamped
|
|
from rclpy.node import Node
|
|
from ros2_bridge_msgs.msg import ArmStatus
|
|
from sensor_msgs.msg import JointState
|
|
from std_srvs.srv import Trigger
|
|
|
|
|
|
JOINT_NAMES = [
|
|
*(f"left_joints_{i}" for i in range(7)),
|
|
*(f"right_joints_{i}" for i in range(7)),
|
|
]
|
|
MOTOR_IDS = [*range(11, 18), *range(21, 28)]
|
|
HAND_SIDES = ("left", "right")
|
|
OMNI_MAGIC = b"TG3A"
|
|
OMNI_HEADER = struct.Struct("!4sQQI")
|
|
TELEOP_PROTOCOL_VERSION = 2
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class ArmSnapshot:
|
|
data: dict[str, Any]
|
|
received_at: float
|
|
|
|
|
|
class LatestArmData:
|
|
"""Receive the latest industrial-PC sample without buffering old motion."""
|
|
|
|
def __init__(self, config: dict[str, Any]) -> None:
|
|
self.cfg = config
|
|
self.transport = str(config.get("transport", "zmq")).lower()
|
|
if self.transport not in ("zmq", "omnisocket"):
|
|
raise ValueError(f"unsupported iarm transport: {self.transport}")
|
|
self.endpoint = str(config.get("iarm_endpoint", ""))
|
|
if self.transport == "omnisocket":
|
|
self.description = (
|
|
f"omnisocket://{config['omnisocket_server']}/"
|
|
f"{config['omnisocket_peer_id']}"
|
|
)
|
|
else:
|
|
self.description = self.endpoint
|
|
self._lock = threading.Lock()
|
|
self._latest: ArmSnapshot | None = None
|
|
self._last_error = f"waiting for the first {self.transport} sample"
|
|
self._metrics: dict[str, Any] = {
|
|
"transport": self.transport,
|
|
"connected": False,
|
|
"frames_received": 0,
|
|
"frames_accepted": 0,
|
|
"dropped_sender": 0,
|
|
"dropped_malformed": 0,
|
|
"dropped_stale": 0,
|
|
"dropped_sequence": 0,
|
|
"raw_packet_age_ms": None,
|
|
"packet_age_baseline_ms": None,
|
|
"effective_packet_age_ms": None,
|
|
"max_effective_packet_age_ms": None,
|
|
}
|
|
self._stop = threading.Event()
|
|
self._thread = threading.Thread(
|
|
target=self._run, name=f"iarm-{self.transport}", daemon=True
|
|
)
|
|
|
|
def start(self) -> None:
|
|
self._thread.start()
|
|
|
|
def close(self) -> None:
|
|
self._stop.set()
|
|
self._thread.join(timeout=2.0)
|
|
|
|
def get(self) -> tuple[ArmSnapshot | None, str]:
|
|
with self._lock:
|
|
return self._latest, self._last_error
|
|
|
|
def metrics(self) -> dict[str, Any]:
|
|
with self._lock:
|
|
return dict(self._metrics)
|
|
|
|
def _run(self) -> None:
|
|
if self.transport == "omnisocket":
|
|
self._run_omnisocket()
|
|
else:
|
|
self._run_zmq()
|
|
|
|
def _run_zmq(self) -> None:
|
|
context = zmq.Context.instance()
|
|
sock = context.socket(zmq.SUB)
|
|
sock.setsockopt(zmq.SUBSCRIBE, b"")
|
|
sock.setsockopt(zmq.CONFLATE, 1)
|
|
sock.setsockopt(zmq.RCVHWM, 1)
|
|
sock.setsockopt(zmq.LINGER, 0)
|
|
sock.connect(self.endpoint)
|
|
poller = zmq.Poller()
|
|
poller.register(sock, zmq.POLLIN)
|
|
try:
|
|
while not self._stop.is_set():
|
|
if sock not in dict(poller.poll(100)):
|
|
continue
|
|
try:
|
|
raw = sock.recv(zmq.NOBLOCK)
|
|
data = json.loads(raw)
|
|
self._validate_shape(data)
|
|
with self._lock:
|
|
self._latest = ArmSnapshot(
|
|
data=data, received_at=time.monotonic()
|
|
)
|
|
self._last_error = ""
|
|
self._metrics["connected"] = True
|
|
self._metrics["frames_received"] += 1
|
|
self._metrics["frames_accepted"] += 1
|
|
except Exception as exc: # Keep receiving after one malformed packet.
|
|
with self._lock:
|
|
self._last_error = f"invalid ZMQ sample: {exc}"
|
|
self._metrics["dropped_malformed"] += 1
|
|
finally:
|
|
sock.close()
|
|
|
|
def _run_omnisocket(self) -> None:
|
|
try:
|
|
from omnisocket import CONTROL_DEFAULTS, MSG_TYPE_BINARY, Session
|
|
except ImportError as exc:
|
|
with self._lock:
|
|
self._last_error = f"cannot import OmniSocket extension: {exc}"
|
|
return
|
|
|
|
expected_sender = str(self.cfg["omnisocket_expected_sender"])
|
|
max_age_ms = float(self.cfg["omnisocket_max_packet_age_ms"])
|
|
last_sequence = 0
|
|
while not self._stop.is_set():
|
|
session = Session()
|
|
baseline_ms: float | None = None
|
|
try:
|
|
session.connect(
|
|
server_addr=str(self.cfg["omnisocket_server"]),
|
|
peer_id=str(self.cfg["omnisocket_peer_id"]),
|
|
**CONTROL_DEFAULTS,
|
|
)
|
|
with self._lock:
|
|
self._metrics["connected"] = True
|
|
self._last_error = ""
|
|
|
|
while not self._stop.is_set():
|
|
message = session.recv(timeout_ms=100)
|
|
if message is None:
|
|
continue
|
|
messages = [message]
|
|
while True:
|
|
pending = session.recv(timeout_ms=0)
|
|
if pending is None:
|
|
break
|
|
messages.append(pending)
|
|
|
|
newest: tuple[int, bytes] | None = None
|
|
now_ns = time.time_ns()
|
|
for from_peer, msg_type, packet in messages:
|
|
with self._lock:
|
|
self._metrics["frames_received"] += 1
|
|
if from_peer != expected_sender:
|
|
with self._lock:
|
|
self._metrics["dropped_sender"] += 1
|
|
continue
|
|
decoded = self._decode_omni_packet(
|
|
msg_type,
|
|
MSG_TYPE_BINARY,
|
|
packet,
|
|
now_ns,
|
|
baseline_ms,
|
|
)
|
|
if decoded is None:
|
|
continue
|
|
sequence, payload, raw_age_ms, effective_age_ms = decoded
|
|
if baseline_ms is None or raw_age_ms < baseline_ms:
|
|
baseline_ms = raw_age_ms
|
|
effective_age_ms = 0.0
|
|
self._record_packet_age(
|
|
raw_age_ms, baseline_ms, effective_age_ms
|
|
)
|
|
if effective_age_ms > max_age_ms:
|
|
with self._lock:
|
|
self._metrics["dropped_stale"] += 1
|
|
continue
|
|
if sequence <= last_sequence:
|
|
with self._lock:
|
|
self._metrics["dropped_sequence"] += 1
|
|
continue
|
|
if newest is None or sequence > newest[0]:
|
|
newest = (sequence, payload)
|
|
|
|
if newest is None:
|
|
continue
|
|
sequence, payload = newest
|
|
try:
|
|
data = json.loads(payload)
|
|
self._validate_shape(data)
|
|
except Exception as exc:
|
|
with self._lock:
|
|
self._last_error = f"invalid OmniSocket sample: {exc}"
|
|
self._metrics["dropped_malformed"] += 1
|
|
continue
|
|
last_sequence = sequence
|
|
with self._lock:
|
|
self._latest = ArmSnapshot(
|
|
data=data, received_at=time.monotonic()
|
|
)
|
|
self._last_error = ""
|
|
self._metrics["frames_accepted"] += 1
|
|
except Exception as exc:
|
|
with self._lock:
|
|
self._metrics["connected"] = False
|
|
self._last_error = f"OmniSocket connection failed: {exc}"
|
|
finally:
|
|
try:
|
|
session.close()
|
|
except OSError:
|
|
pass
|
|
with self._lock:
|
|
self._metrics["connected"] = False
|
|
self._stop.wait(1.0)
|
|
|
|
def _decode_omni_packet(
|
|
self,
|
|
msg_type: int,
|
|
binary_type: int,
|
|
packet: bytes,
|
|
now_ns: int,
|
|
baseline_ms: float | None,
|
|
) -> tuple[int, bytes, float, float] | None:
|
|
if msg_type != binary_type or len(packet) < OMNI_HEADER.size:
|
|
with self._lock:
|
|
self._metrics["dropped_malformed"] += 1
|
|
return None
|
|
magic, sequence, sent_ns, payload_len = OMNI_HEADER.unpack_from(packet)
|
|
payload = packet[OMNI_HEADER.size :]
|
|
if magic != OMNI_MAGIC or payload_len != len(payload) or not payload:
|
|
with self._lock:
|
|
self._metrics["dropped_malformed"] += 1
|
|
return None
|
|
raw_age_ms = (now_ns - sent_ns) / 1_000_000.0
|
|
effective_age_ms = (
|
|
0.0 if baseline_ms is None else max(0.0, raw_age_ms - baseline_ms)
|
|
)
|
|
return sequence, payload, raw_age_ms, effective_age_ms
|
|
|
|
def _record_packet_age(
|
|
self, raw_age_ms: float, baseline_ms: float, effective_age_ms: float
|
|
) -> None:
|
|
with self._lock:
|
|
self._metrics["raw_packet_age_ms"] = round(raw_age_ms, 3)
|
|
self._metrics["packet_age_baseline_ms"] = round(baseline_ms, 3)
|
|
self._metrics["effective_packet_age_ms"] = round(effective_age_ms, 3)
|
|
previous_max = self._metrics["max_effective_packet_age_ms"]
|
|
if previous_max is None or effective_age_ms > previous_max:
|
|
self._metrics["max_effective_packet_age_ms"] = round(
|
|
effective_age_ms, 3
|
|
)
|
|
|
|
@staticmethod
|
|
def _validate_shape(data: dict[str, Any]) -> None:
|
|
position = data["arm"]["position"]
|
|
left = position["left"]
|
|
right = position["right"]
|
|
if len(left) != 7 or len(right) != 7:
|
|
raise ValueError("arm.position must contain left[7] and right[7]")
|
|
if not all(math.isfinite(float(v)) for v in [*left, *right]):
|
|
raise ValueError("arm.position contains a non-finite value")
|
|
|
|
|
|
class LocalTeleopBridge(Node):
|
|
FRAME_ID = "tg3_local_teleop"
|
|
|
|
def __init__(self, config: dict[str, Any], allow_publish: bool, status_file: Path) -> None:
|
|
super().__init__("tg3_local_teleop")
|
|
self.cfg = config
|
|
self.allow_publish = allow_publish
|
|
self.status_file = status_file
|
|
|
|
ros_cfg = config["ros"]
|
|
net_cfg = config["network"]
|
|
self.hands_cfg = config.get("hands", {})
|
|
self.hands_enabled = bool(self.hands_cfg.get("enabled", False))
|
|
self.locomotion_cfg = config.get("locomotion", {})
|
|
self.locomotion_enabled = bool(
|
|
self.locomotion_cfg.get("enabled", False)
|
|
)
|
|
self.hand_publishers: dict[str, Any] = {}
|
|
self.robot_hand_positions: dict[str, list[int] | None] = {
|
|
side: None for side in HAND_SIDES
|
|
}
|
|
self.robot_hand_states: dict[str, list[int]] = {
|
|
side: [] for side in HAND_SIDES
|
|
}
|
|
self.robot_hand_at: dict[str, float] = {side: 0.0 for side in HAND_SIDES}
|
|
self.last_hand_commands: dict[str, list[int] | None] = {
|
|
side: None for side in HAND_SIDES
|
|
}
|
|
self.expected_hand_messages: dict[
|
|
str, tuple[tuple[Any, ...], float] | None
|
|
] = {side: None for side in HAND_SIDES}
|
|
self.last_hand_publish_at = 0.0
|
|
self.hand_publish_count = 0
|
|
self.hand_output_ready = False
|
|
self.runtime_hand_output_reasons: list[str] = []
|
|
self.foreign_hand_source_seen = False
|
|
self.walk_combo_started_at: float | None = None
|
|
self.walk_active = False
|
|
self.walk_command = [0.0, 0.0]
|
|
self.walk_publish_count = 0
|
|
self.walk_zero_frames_remaining = 0
|
|
self.last_walk_publish_at = 0.0
|
|
|
|
self.publisher = self.create_publisher(JointState, ros_cfg["command_topic"], 10)
|
|
self.walk_publisher = None
|
|
if self.locomotion_enabled:
|
|
self.walk_publisher = self.create_publisher(
|
|
TwistStamped, self.locomotion_cfg["command_topic"], 10
|
|
)
|
|
self.create_subscription(
|
|
DiagnosticStatus, ros_cfg["rl_state_topic"], self._on_rl_state, 10
|
|
)
|
|
self.create_subscription(
|
|
ArmStatus, ros_cfg["arm_state_topic"], self._on_arm_state, 10
|
|
)
|
|
# A foreign sample means cloud teleoperation is active. Never mix two
|
|
# command sources on the vendor frequency-conversion input.
|
|
self.create_subscription(
|
|
JointState, ros_cfg["command_topic"], self._on_command_topic, 10
|
|
)
|
|
if self.hands_enabled:
|
|
for side in HAND_SIDES:
|
|
command_topic = str(self.hands_cfg[f"{side}_command_topic"])
|
|
status_topic = str(self.hands_cfg[f"{side}_status_topic"])
|
|
self.hand_publishers[side] = self.create_publisher(
|
|
SetMotorMulti, command_topic, 10
|
|
)
|
|
self.create_subscription(
|
|
MotorStatus,
|
|
status_topic,
|
|
lambda msg, hand_side=side: self._on_hand_status(hand_side, msg),
|
|
10,
|
|
)
|
|
self.create_subscription(
|
|
SetMotorMulti,
|
|
command_topic,
|
|
lambda msg, hand_side=side: self._on_hand_command(hand_side, msg),
|
|
10,
|
|
)
|
|
self.create_service(Trigger, ros_cfg["home_service"], self._on_home_request)
|
|
self.create_service(
|
|
Trigger, ros_cfg["cancel_home_service"], self._on_cancel_home_request
|
|
)
|
|
|
|
self.source = LatestArmData(net_cfg)
|
|
self.source.start()
|
|
|
|
self.rl_state: dict[str, str] = {}
|
|
self.rl_state_at = 0.0
|
|
self.robot_arm_positions: list[float] | None = None
|
|
self.robot_arm_errors: list[int] = []
|
|
self.robot_arm_at = 0.0
|
|
self.foreign_source_seen = False
|
|
self.armed = False
|
|
self.returning_home = False
|
|
self.home_command: list[float] | None = None
|
|
self.home_started_at = 0.0
|
|
self.home_settle_started_at: float | None = None
|
|
self.home_status = "idle"
|
|
self.combo_started_at: float | None = None
|
|
self.combo_latched = False
|
|
self.active_session_id: str | None = None
|
|
self.session_start_attempted_id: str | None = None
|
|
self.last_session_state = "inactive"
|
|
self.last_session_stop_reason = ""
|
|
self.last_command: list[float] | None = None
|
|
self.last_publish_at = 0.0
|
|
self.last_status_at = 0.0
|
|
self.last_log_signature: tuple[Any, ...] | None = None
|
|
self.started_at = time.monotonic()
|
|
|
|
rate = float(ros_cfg["publish_rate_hz"])
|
|
self.timer = self.create_timer(1.0 / rate, self._tick)
|
|
mode = "ACTIVE-CAPABLE" if allow_publish else "MONITOR-ONLY"
|
|
self.get_logger().info(
|
|
f"local bridge started in {mode}; source={self.source.description}; "
|
|
f"target={ros_cfg['command_topic']}; brainco_hands={self.hands_enabled}; "
|
|
f"locomotion={self.locomotion_enabled}"
|
|
)
|
|
|
|
def close(self) -> None:
|
|
self._disarm("bridge is shutting down")
|
|
self._finish_home("bridge is shutting down", success=False)
|
|
self.source.close()
|
|
|
|
def _on_rl_state(self, msg: DiagnosticStatus) -> None:
|
|
self.rl_state = {item.key: item.value for item in msg.values}
|
|
self.rl_state_at = time.monotonic()
|
|
|
|
def _on_arm_state(self, msg: ArmStatus) -> None:
|
|
by_id = {int(motor.name): motor for motor in msg.status}
|
|
if not all(motor_id in by_id for motor_id in MOTOR_IDS):
|
|
return
|
|
positions = [float(by_id[motor_id].pos) for motor_id in MOTOR_IDS]
|
|
if not all(math.isfinite(value) for value in positions):
|
|
return
|
|
self.robot_arm_positions = positions
|
|
self.robot_arm_errors = [int(by_id[motor_id].error) for motor_id in MOTOR_IDS]
|
|
self.robot_arm_at = time.monotonic()
|
|
|
|
def _on_hand_status(self, side: str, msg: MotorStatus) -> None:
|
|
positions = [int(value) for value in msg.positions]
|
|
states = [int(value) for value in msg.states]
|
|
if len(positions) != 6 or len(states) != 6:
|
|
return
|
|
self.robot_hand_positions[side] = positions
|
|
self.robot_hand_states[side] = states
|
|
self.robot_hand_at[side] = time.monotonic()
|
|
|
|
@staticmethod
|
|
def _hand_message_signature(msg: SetMotorMulti) -> tuple[Any, ...]:
|
|
return (
|
|
int(msg.mode),
|
|
tuple(int(value) for value in msg.positions),
|
|
tuple(int(value) for value in msg.speeds),
|
|
tuple(int(value) for value in msg.currents),
|
|
tuple(int(value) for value in msg.pwms),
|
|
tuple(int(value) for value in msg.durations),
|
|
)
|
|
|
|
def _on_hand_command(self, side: str, msg: SetMotorMulti) -> None:
|
|
now = time.monotonic()
|
|
signature = self._hand_message_signature(msg)
|
|
expected = self.expected_hand_messages[side]
|
|
if (
|
|
expected is not None
|
|
and now - expected[1] <= 0.25
|
|
and signature == expected[0]
|
|
):
|
|
return
|
|
if not self.foreign_hand_source_seen:
|
|
self.get_logger().error(
|
|
f"foreign {side} BrainCo hand command detected; local control is locked "
|
|
"until this bridge is restarted"
|
|
)
|
|
self.foreign_hand_source_seen = True
|
|
if self.armed:
|
|
self._disarm("foreign BrainCo hand command source detected")
|
|
if self.returning_home:
|
|
self._finish_home(
|
|
"foreign BrainCo hand command source detected", success=False
|
|
)
|
|
|
|
def _on_command_topic(self, msg: JointState) -> None:
|
|
if msg.header.frame_id != self.FRAME_ID and len(msg.position) in (14, 16):
|
|
if not self.foreign_source_seen:
|
|
self.get_logger().error(
|
|
"foreign /encoder_identical_joint data detected; local control is locked "
|
|
"until this bridge is restarted"
|
|
)
|
|
self.foreign_source_seen = True
|
|
if self.armed:
|
|
self._disarm("foreign/cloud arm command source detected")
|
|
if self.returning_home:
|
|
self._finish_home("foreign/cloud arm command source detected", success=False)
|
|
|
|
def _on_home_request(
|
|
self, _request: Trigger.Request, response: Trigger.Response
|
|
) -> Trigger.Response:
|
|
now = time.monotonic()
|
|
sample, source_error = self.source.get()
|
|
reasons = self._home_start_reasons(now, sample, source_error)
|
|
if self.armed:
|
|
reasons.append("manual isomorphic-arm control is armed")
|
|
if reasons:
|
|
response.success = False
|
|
response.message = "return-home rejected: " + "; ".join(reasons)
|
|
return response
|
|
|
|
self._start_home(now, "operator return-home service")
|
|
response.success = True
|
|
response.message = (
|
|
"limited-speed return-home accepted; use cancel service or hold left Z + "
|
|
"right C for 3s to stop"
|
|
)
|
|
return response
|
|
|
|
def _on_cancel_home_request(
|
|
self, _request: Trigger.Request, response: Trigger.Response
|
|
) -> Trigger.Response:
|
|
if not self.returning_home:
|
|
response.success = False
|
|
response.message = "return-home is not running"
|
|
return response
|
|
self._finish_home("operator cancel service", success=False, cancelled=True)
|
|
response.success = True
|
|
response.message = "return-home cancelled; arm command publication stopped"
|
|
return response
|
|
|
|
def _tick(self) -> None:
|
|
now = time.monotonic()
|
|
sample, source_error = self.source.get()
|
|
if self.source.transport == "omnisocket":
|
|
self._update_session_gate(now, sample, source_error)
|
|
else:
|
|
# Direct-LAN fallback retains the original local raw-button latch.
|
|
self._update_combo(now, sample)
|
|
# Custom joint-target limits are checked only when arming below. Once
|
|
# capture/following starts, the vendor node keeps its own unchanged
|
|
# limit protection while this bridge continues the non-limit gates.
|
|
startup_reasons = self._safety_reasons(now, sample, source_error)
|
|
armed_runtime_reasons = self._safety_reasons(
|
|
now,
|
|
sample,
|
|
source_error,
|
|
check_iarm_target=False,
|
|
check_hands=self.armed,
|
|
# Hand feedback is not an all-teleop teardown gate. A separate
|
|
# runtime gate below pauses only hand publication during a status
|
|
# gap. Foreign publishers and command-shape validation remain
|
|
# active through check_hands.
|
|
check_hand_feedback=False,
|
|
)
|
|
home_runtime_reasons = self._safety_reasons(
|
|
now,
|
|
sample,
|
|
source_error,
|
|
check_iarm=False,
|
|
check_iarm_target=False,
|
|
check_hands=False,
|
|
check_hand_feedback=False,
|
|
)
|
|
runtime_hand_reasons = (
|
|
self._hand_feedback_reasons(now)
|
|
if self.hands_enabled and self.armed
|
|
else []
|
|
)
|
|
self.runtime_hand_output_reasons = runtime_hand_reasons
|
|
|
|
if self.armed and armed_runtime_reasons:
|
|
self._disarm(
|
|
"runtime safety gate failed: "
|
|
+ "; ".join(armed_runtime_reasons)
|
|
)
|
|
if self.returning_home and home_runtime_reasons:
|
|
self._finish_home(
|
|
"runtime safety gate failed: "
|
|
+ "; ".join(home_runtime_reasons),
|
|
success=False,
|
|
)
|
|
|
|
if self.returning_home:
|
|
self._tick_home(now)
|
|
elif self.armed and sample is not None:
|
|
desired = self._positions(sample.data)
|
|
command = self._slew_limit(desired, now)
|
|
self._publish_target(command, now)
|
|
self.last_command = command
|
|
if self.hands_enabled:
|
|
if runtime_hand_reasons:
|
|
if self.hand_output_ready:
|
|
self.get_logger().warning(
|
|
"BRAINCO HAND OUTPUT PAUSED: "
|
|
+ "; ".join(runtime_hand_reasons)
|
|
)
|
|
self.hand_output_ready = False
|
|
else:
|
|
if not self.hand_output_ready:
|
|
# Resume the hand slew limiter at measured feedback,
|
|
# never at the last target from before the status gap.
|
|
self.last_hand_commands = {
|
|
side: list(self.robot_hand_positions[side] or [])
|
|
for side in HAND_SIDES
|
|
}
|
|
self.last_hand_publish_at = now
|
|
self.get_logger().info(
|
|
"BRAINCO HAND OUTPUT READY: feedback healthy"
|
|
)
|
|
self.hand_output_ready = True
|
|
desired_hands = self._hand_targets(sample.data)
|
|
commands = {
|
|
side: self._slew_hand(side, desired_hands[side], now)
|
|
for side in HAND_SIDES
|
|
}
|
|
self._publish_hands(commands, now)
|
|
self.last_hand_commands = commands
|
|
if self.locomotion_enabled:
|
|
self._tick_locomotion(now, sample)
|
|
elif self.locomotion_enabled:
|
|
self._tick_walk_zero_burst(now)
|
|
|
|
if self.returning_home:
|
|
reasons = home_runtime_reasons
|
|
elif self.armed:
|
|
reasons = armed_runtime_reasons
|
|
else:
|
|
reasons = startup_reasons
|
|
|
|
if now - self.last_status_at >= 0.2:
|
|
self._write_status(now, sample, reasons)
|
|
self.last_status_at = now
|
|
|
|
signature = (
|
|
self.armed,
|
|
self.returning_home,
|
|
bool(sample),
|
|
tuple(reasons),
|
|
self.rl_state.get("current_state"),
|
|
self.rl_state.get("status"),
|
|
)
|
|
if signature != self.last_log_signature:
|
|
if self.returning_home:
|
|
state = "RETURNING_HOME"
|
|
else:
|
|
state = "ARMED" if self.armed else "DISARMED"
|
|
detail = "all safety gates ready" if not reasons else "; ".join(reasons)
|
|
# rclpy associates severity with the Python call site, therefore
|
|
# INFO and WARN need separate call sites instead of a bound method.
|
|
if reasons:
|
|
self.get_logger().warning(f"{state}: {detail}")
|
|
else:
|
|
self.get_logger().info(f"{state}: {detail}")
|
|
self.last_log_signature = signature
|
|
|
|
restart_reason = self._source_restart_reason(now, sample)
|
|
if restart_reason is not None:
|
|
# A server outage can leave the native OmniSocket recv call stuck in
|
|
# an apparently connected session. Raising out of the ROS loop lets
|
|
# systemd replace the whole process (and native session) cleanly.
|
|
# The normal runtime gate above has already stopped publication.
|
|
self.get_logger().error(restart_reason)
|
|
raise RuntimeError(restart_reason)
|
|
|
|
def _source_restart_reason(
|
|
self, now: float, sample: ArmSnapshot | None
|
|
) -> str | None:
|
|
net_cfg = self.cfg["network"]
|
|
if str(net_cfg.get("transport", "zmq")).lower() != "omnisocket":
|
|
return None
|
|
restart_after = float(
|
|
net_cfg.get("omnisocket_restart_after_stale_s", 0.0)
|
|
)
|
|
if restart_after <= 0.0:
|
|
return None
|
|
# No xTELE business frame before START, and no frame after a matching
|
|
# STOP, are both normal idle states. Only a session that was last seen
|
|
# as START/ACTIVE may use business-frame staleness to rebuild a stuck
|
|
# native OmniSocket receiver.
|
|
if sample is None:
|
|
return None
|
|
info = self._teleop_session_info(sample.data)
|
|
if info is not None and info[2] == "stop":
|
|
return None
|
|
stale_for = now - sample.received_at
|
|
if stale_for < restart_after:
|
|
return None
|
|
return (
|
|
f"OmniSocket input stale for {stale_for:.3f}s; exiting so systemd "
|
|
"can establish a fresh session"
|
|
)
|
|
|
|
@staticmethod
|
|
def _teleop_session_info(
|
|
data: dict[str, Any]
|
|
) -> tuple[str, int, str, str] | None:
|
|
metadata = data.get("tg3_transport")
|
|
if not isinstance(metadata, dict):
|
|
return None
|
|
version = metadata.get("protocol_version")
|
|
session_id = metadata.get("session_id")
|
|
session_seq = metadata.get("session_seq")
|
|
session_state = metadata.get("session_state")
|
|
stop_reason = metadata.get("stop_reason", "")
|
|
if version != TELEOP_PROTOCOL_VERSION:
|
|
return None
|
|
if not (
|
|
isinstance(session_id, str)
|
|
and len(session_id) == 32
|
|
and all(character in "0123456789abcdef" for character in session_id)
|
|
):
|
|
return None
|
|
if (
|
|
isinstance(session_seq, bool)
|
|
or not isinstance(session_seq, int)
|
|
or session_seq <= 0
|
|
):
|
|
return None
|
|
if session_state not in ("start", "active", "stop"):
|
|
return None
|
|
if not isinstance(stop_reason, str):
|
|
return None
|
|
return session_id, session_seq, session_state, stop_reason
|
|
|
|
def _update_session_gate(
|
|
self,
|
|
now: float,
|
|
sample: ArmSnapshot | None,
|
|
source_error: str,
|
|
) -> None:
|
|
if sample is None:
|
|
return
|
|
info = self._teleop_session_info(sample.data)
|
|
if info is None:
|
|
self.last_session_state = "invalid"
|
|
if self.armed:
|
|
self._disarm("missing or invalid teleoperation session metadata")
|
|
self.active_session_id = None
|
|
return
|
|
|
|
session_id, _session_seq, session_state, stop_reason = info
|
|
self.last_session_state = session_state
|
|
self.last_session_stop_reason = stop_reason
|
|
|
|
if session_state == "active":
|
|
if self.armed and session_id != self.active_session_id:
|
|
self._disarm("teleoperation session ID changed without START")
|
|
self.active_session_id = None
|
|
return
|
|
|
|
if session_state == "stop":
|
|
if session_id != self.active_session_id:
|
|
return
|
|
was_armed = self.armed
|
|
self._disarm("matching teleoperation STOP received")
|
|
self.active_session_id = None
|
|
if (
|
|
was_armed
|
|
and stop_reason == "operator"
|
|
and bool(self.cfg["control"].get("auto_home_on_stop", True))
|
|
):
|
|
self._start_home_if_safe(
|
|
now,
|
|
sample,
|
|
source_error,
|
|
"operator teleoperation STOP",
|
|
)
|
|
return
|
|
|
|
# START is repeated for a short bounded window so the receiver's
|
|
# latest-frame conflation cannot hide the only arming event. A given
|
|
# session is attempted exactly once; a rejected or interrupted session
|
|
# cannot arm later without a new physical Z+C cycle and new ID.
|
|
if session_id == self.active_session_id and self.armed:
|
|
return
|
|
if session_id == self.session_start_attempted_id:
|
|
return
|
|
self.session_start_attempted_id = session_id
|
|
if self.armed:
|
|
self._disarm("new START arrived while another session was armed")
|
|
self.active_session_id = None
|
|
return
|
|
if not self.allow_publish:
|
|
self.get_logger().warning(
|
|
"teleoperation START received, but this process is MONITOR-ONLY"
|
|
)
|
|
return
|
|
|
|
reasons = self._safety_reasons(now, sample, source_error)
|
|
if reasons:
|
|
self.get_logger().error("cannot arm session: " + "; ".join(reasons))
|
|
return
|
|
if self.returning_home:
|
|
self._finish_home(
|
|
"new teleoperation START accepted", success=False, cancelled=True
|
|
)
|
|
self.active_session_id = session_id
|
|
self.armed = True
|
|
# Start both slew limiters at measured robot feedback, never at a
|
|
# potentially distant first network target.
|
|
self.last_command = list(self.robot_arm_positions or [])
|
|
self.last_publish_at = now
|
|
if self.hands_enabled:
|
|
self.last_hand_commands = {
|
|
side: list(self.robot_hand_positions[side] or [])
|
|
for side in HAND_SIDES
|
|
}
|
|
self.last_hand_publish_at = now
|
|
self.get_logger().warning(
|
|
"LOCAL DUAL-ARM + BRAINCO HAND CONTROL ARMED by validated EAI "
|
|
"teleoperation session; vendor drivers remain active"
|
|
)
|
|
|
|
def _update_combo(self, now: float, sample: ArmSnapshot | None) -> None:
|
|
pressed = False
|
|
if sample is not None:
|
|
buttons = sample.data.get("button", {})
|
|
left = buttons.get("left", [])
|
|
right = buttons.get("right", [])
|
|
# TS1P raw order is left X/Y/Z/... and right A/B/C/....
|
|
pressed = len(left) >= 3 and len(right) >= 3 and bool(left[2]) and bool(right[2])
|
|
|
|
if not pressed:
|
|
self.combo_started_at = None
|
|
self.combo_latched = False
|
|
return
|
|
|
|
if self.combo_started_at is None:
|
|
self.combo_started_at = now
|
|
hold_seconds = float(self.cfg["control"]["start_stop_hold_seconds"])
|
|
if self.combo_latched or now - self.combo_started_at < hold_seconds:
|
|
return
|
|
|
|
self.combo_latched = True
|
|
if self.returning_home:
|
|
self._finish_home(
|
|
"left Z + right C held: operator cancel", success=False, cancelled=True
|
|
)
|
|
return
|
|
if self.armed:
|
|
self._disarm("left Z + right C held: teleoperation ended")
|
|
if bool(self.cfg["control"].get("auto_home_on_stop", True)):
|
|
snapshot, source_error = self.source.get()
|
|
self._start_home_if_safe(
|
|
now,
|
|
snapshot,
|
|
source_error,
|
|
"teleoperation ended by left Z + right C",
|
|
)
|
|
return
|
|
if not self.allow_publish:
|
|
self.get_logger().warning(
|
|
"left Z + right C held, but this process is MONITOR-ONLY"
|
|
)
|
|
return
|
|
snapshot, source_error = self.source.get()
|
|
reasons = self._safety_reasons(now, snapshot, source_error)
|
|
if reasons:
|
|
self.get_logger().error("cannot arm: " + "; ".join(reasons))
|
|
return
|
|
self.armed = True
|
|
# Start the slew limiter at measured robot feedback. Using None here
|
|
# would make the first armed frame jump directly to the TS1P target.
|
|
self.last_command = list(self.robot_arm_positions or [])
|
|
self.last_publish_at = now
|
|
if self.hands_enabled:
|
|
self.last_hand_commands = {
|
|
side: list(self.robot_hand_positions[side] or [])
|
|
for side in HAND_SIDES
|
|
}
|
|
self.last_hand_publish_at = now
|
|
self.get_logger().warning(
|
|
"LOCAL DUAL-ARM + BRAINCO HAND CONTROL ARMED; vendor arm and hand "
|
|
"drivers remain active"
|
|
)
|
|
|
|
def _disarm(self, reason: str) -> None:
|
|
was_armed = self.armed
|
|
self._stop_locomotion(reason)
|
|
self.armed = False
|
|
self.hand_output_ready = False
|
|
self.runtime_hand_output_reasons = []
|
|
self.last_command = None
|
|
self.last_hand_commands = {side: None for side in HAND_SIDES}
|
|
if was_armed:
|
|
self.get_logger().warning(
|
|
f"LOCAL DUAL-ARM + BRAINCO HAND CONTROL DISARMED: {reason}"
|
|
)
|
|
|
|
@staticmethod
|
|
def _shape_joystick_axis(value: float, deadzone: float, expo: float) -> float:
|
|
"""Apply the xTELE deadzone and a normalized exponential response."""
|
|
|
|
if not math.isfinite(value):
|
|
raise ValueError("joystick axis is non-finite")
|
|
if not 0.0 <= deadzone < 1.0:
|
|
raise ValueError("joystick deadzone must be in [0, 1)")
|
|
if not math.isfinite(expo) or expo <= 0.0:
|
|
raise ValueError("joystick exponent must be positive")
|
|
value = max(-1.0, min(1.0, value))
|
|
magnitude = abs(value)
|
|
if magnitude <= deadzone:
|
|
return 0.0
|
|
normalized = (magnitude - deadzone) / (1.0 - deadzone)
|
|
return math.copysign(normalized**expo, value)
|
|
|
|
def _tick_locomotion(self, now: float, sample: ArmSnapshot) -> None:
|
|
cfg = self.locomotion_cfg
|
|
try:
|
|
buttons = sample.data["button"]["right"]
|
|
joystick = sample.data["joystick"]["left"]
|
|
right_c = len(buttons) >= 3 and bool(buttons[2])
|
|
if len(joystick) != 2:
|
|
raise ValueError("left joystick must contain x/y")
|
|
# TS1P reports the physical forward/back axis first and the
|
|
# left/right axis second. This was verified on the installed
|
|
# xTELE 0.1.2 stream; treating the pair as Cartesian x/y made a
|
|
# forward stick command become pure yaw.
|
|
raw_forward, raw_yaw = (float(joystick[0]), float(joystick[1]))
|
|
deadzone = float(cfg["joystick_deadzone"])
|
|
expo = float(cfg["joystick_expo"])
|
|
shaped_forward = self._shape_joystick_axis(raw_forward, deadzone, expo)
|
|
shaped_yaw = self._shape_joystick_axis(raw_yaw, deadzone, expo)
|
|
signed_forward = shaped_forward * float(
|
|
cfg.get("forward_axis_sign", 1.0)
|
|
)
|
|
max_forward = float(cfg["max_forward_m_s"])
|
|
max_reverse = float(cfg["max_reverse_m_s"])
|
|
max_angular = float(cfg["max_angular_rad_s"])
|
|
if not all(
|
|
math.isfinite(value) and value >= 0.0
|
|
for value in (max_forward, max_reverse, max_angular)
|
|
):
|
|
raise ValueError("locomotion limits must be finite and non-negative")
|
|
except (KeyError, TypeError, ValueError) as exc:
|
|
self._stop_locomotion(f"invalid locomotion input: {exc}")
|
|
return
|
|
|
|
joystick_active = shaped_forward != 0.0 or shaped_yaw != 0.0
|
|
if not right_c or not joystick_active:
|
|
self._stop_locomotion("right C released or left joystick returned to center")
|
|
self._tick_walk_zero_burst(now)
|
|
return
|
|
|
|
if self.walk_combo_started_at is None:
|
|
self.walk_combo_started_at = now
|
|
hold_seconds = float(cfg["hold_seconds"])
|
|
if now - self.walk_combo_started_at < hold_seconds:
|
|
self._tick_walk_zero_burst(now)
|
|
return
|
|
|
|
if not self.walk_active:
|
|
self.walk_active = True
|
|
self.walk_zero_frames_remaining = 0
|
|
self.get_logger().warning(
|
|
"LOCAL HBWALK VELOCITY STARTED: immediate right C + left "
|
|
"joystick input"
|
|
)
|
|
|
|
linear_limit = max_forward if signed_forward >= 0.0 else max_reverse
|
|
linear_x = signed_forward * linear_limit
|
|
angular_z = (
|
|
shaped_yaw
|
|
* float(cfg.get("yaw_axis_sign", -1.0))
|
|
* max_angular
|
|
)
|
|
self._publish_walk(linear_x, angular_z, now)
|
|
|
|
def _stop_locomotion(self, reason: str) -> None:
|
|
was_active = self.walk_active
|
|
had_nonzero_command = any(abs(value) > 1e-9 for value in self.walk_command)
|
|
self.walk_active = False
|
|
self.walk_combo_started_at = None
|
|
if was_active or had_nonzero_command:
|
|
now = time.monotonic()
|
|
self._publish_walk(0.0, 0.0, now)
|
|
self.walk_zero_frames_remaining = max(
|
|
0, int(self.locomotion_cfg.get("zero_burst_frames", 1)) - 1
|
|
)
|
|
self.get_logger().warning(f"LOCAL HBWALK LOCOMOTION STOPPED: {reason}")
|
|
|
|
def _tick_walk_zero_burst(self, now: float) -> None:
|
|
if self.walk_zero_frames_remaining <= 0:
|
|
return
|
|
self._publish_walk(0.0, 0.0, now)
|
|
self.walk_zero_frames_remaining -= 1
|
|
|
|
def _publish_walk(self, linear_x: float, angular_z: float, now: float) -> None:
|
|
if not self.allow_publish or self.walk_publisher is None:
|
|
return
|
|
msg = TwistStamped()
|
|
msg.header.stamp = self.get_clock().now().to_msg()
|
|
# The TG3 secondary-development topic specification uses "pelvis".
|
|
msg.header.frame_id = str(self.locomotion_cfg.get("frame_id", ""))
|
|
msg.twist.linear.x = float(linear_x)
|
|
msg.twist.linear.y = 0.0
|
|
msg.twist.linear.z = 0.0
|
|
msg.twist.angular.x = 0.0
|
|
msg.twist.angular.y = 0.0
|
|
msg.twist.angular.z = float(angular_z)
|
|
self.walk_publisher.publish(msg)
|
|
self.walk_command = [float(linear_x), float(angular_z)]
|
|
self.last_walk_publish_at = now
|
|
self.walk_publish_count += 1
|
|
|
|
def _home_start_reasons(
|
|
self,
|
|
now: float,
|
|
sample: ArmSnapshot | None,
|
|
source_error: str,
|
|
) -> list[str]:
|
|
reasons = self._safety_reasons(
|
|
now,
|
|
sample,
|
|
source_error,
|
|
check_iarm=False,
|
|
check_iarm_target=False,
|
|
check_hands=False,
|
|
)
|
|
if not self.allow_publish:
|
|
reasons.append("bridge is monitor-only")
|
|
if self.returning_home:
|
|
reasons.append("return-home is already running")
|
|
if self.robot_arm_positions is None:
|
|
reasons.append("robot arm position is unavailable")
|
|
reasons.extend(self._home_config_reasons())
|
|
return reasons
|
|
|
|
def _start_home(self, now: float, reason: str) -> None:
|
|
assert self.robot_arm_positions is not None
|
|
self.returning_home = True
|
|
self.home_command = list(self.robot_arm_positions)
|
|
self.home_started_at = now
|
|
self.home_settle_started_at = None
|
|
self.home_status = "running"
|
|
self.last_publish_at = now
|
|
self.get_logger().warning(
|
|
f"LIMITED-SPEED RETURN-HOME STARTED from measured robot arm state ({reason})"
|
|
)
|
|
|
|
def _start_home_if_safe(
|
|
self,
|
|
now: float,
|
|
sample: ArmSnapshot | None,
|
|
source_error: str,
|
|
reason: str,
|
|
) -> bool:
|
|
reasons = self._home_start_reasons(now, sample, source_error)
|
|
if reasons:
|
|
detail = "; ".join(reasons)
|
|
self.home_status = "failed: automatic return-home rejected: " + detail
|
|
self.get_logger().error(
|
|
f"AUTOMATIC RETURN-HOME REJECTED after {reason}: {detail}"
|
|
)
|
|
return False
|
|
self._start_home(now, reason)
|
|
return True
|
|
|
|
def _tick_home(self, now: float) -> None:
|
|
if self.home_command is None or self.robot_arm_positions is None:
|
|
self._finish_home("home trajectory state is unavailable", success=False)
|
|
return
|
|
home_cfg = self.cfg["home"]
|
|
if now - self.home_started_at > float(home_cfg["timeout_s"]):
|
|
self._finish_home("return-home timeout", success=False)
|
|
return
|
|
|
|
tracking_error = max(
|
|
abs(commanded - measured)
|
|
for commanded, measured in zip(
|
|
self.home_command, self.robot_arm_positions
|
|
)
|
|
)
|
|
if tracking_error > float(home_cfg["max_tracking_error_rad"]):
|
|
self._finish_home(
|
|
f"robot is not following the home trajectory "
|
|
f"(max error {tracking_error:.3f} rad)",
|
|
success=False,
|
|
)
|
|
return
|
|
|
|
dt = max(0.001, now - self.last_publish_at)
|
|
max_step = float(home_cfg["slew_rad_s"]) * dt
|
|
goal = [float(v) for v in home_cfg["joint_goal_rad"]]
|
|
if len(goal) != 14:
|
|
self._finish_home("configuration must define a 14-joint home goal", success=False)
|
|
return
|
|
command = [
|
|
current + max(-max_step, min(max_step, target - current))
|
|
for current, target in zip(self.home_command, goal)
|
|
]
|
|
self._publish_target(command, now)
|
|
self.home_command = command
|
|
|
|
command_tolerance = float(home_cfg["command_tolerance_rad"])
|
|
actual_tolerance = float(home_cfg["actual_tolerance_rad"])
|
|
command_at_goal = all(
|
|
abs(current - target) <= command_tolerance
|
|
for current, target in zip(command, goal)
|
|
)
|
|
robot_at_goal = all(
|
|
abs(current - target) <= actual_tolerance
|
|
for current, target in zip(self.robot_arm_positions, goal)
|
|
)
|
|
if command_at_goal and robot_at_goal:
|
|
if self.home_settle_started_at is None:
|
|
self.home_settle_started_at = now
|
|
elif now - self.home_settle_started_at >= float(home_cfg["settle_s"]):
|
|
self._finish_home("both arms reached the configured home pose", success=True)
|
|
else:
|
|
self.home_settle_started_at = None
|
|
|
|
def _finish_home(
|
|
self, reason: str, success: bool, cancelled: bool = False
|
|
) -> None:
|
|
was_running = self.returning_home
|
|
self.returning_home = False
|
|
self.home_command = None
|
|
self.home_settle_started_at = None
|
|
if not was_running:
|
|
return
|
|
if success:
|
|
self.home_status = "complete"
|
|
self.get_logger().warning(f"LIMITED-SPEED RETURN-HOME COMPLETE: {reason}")
|
|
elif cancelled:
|
|
self.home_status = "cancelled: " + reason
|
|
self.get_logger().warning(f"LIMITED-SPEED RETURN-HOME CANCELLED: {reason}")
|
|
else:
|
|
self.home_status = "failed: " + reason
|
|
self.get_logger().error(f"LIMITED-SPEED RETURN-HOME FAILED: {reason}")
|
|
|
|
def _publish_target(self, command: list[float], now: float) -> None:
|
|
msg = JointState()
|
|
msg.header.stamp = self.get_clock().now().to_msg()
|
|
msg.header.frame_id = self.FRAME_ID
|
|
msg.name = JOINT_NAMES
|
|
msg.position = command
|
|
# The vendor node checks both arrays for 14/16 DOF. Zero velocity
|
|
# preserves its position-control path and avoids malformed-DOF warnings.
|
|
msg.velocity = [0.0] * 14
|
|
self.publisher.publish(msg)
|
|
self.last_publish_at = now
|
|
|
|
def _publish_hands(self, commands: dict[str, list[int]], now: float) -> None:
|
|
mode = int(self.hands_cfg["mode"])
|
|
duration = int(self.hands_cfg["duration_ms"])
|
|
for side in HAND_SIDES:
|
|
msg = SetMotorMulti()
|
|
msg.mode = mode
|
|
msg.positions = commands[side]
|
|
msg.speeds = [0] * 6
|
|
msg.currents = [0] * 6
|
|
msg.pwms = [0] * 6
|
|
msg.durations = [duration] * 6
|
|
signature = self._hand_message_signature(msg)
|
|
self.expected_hand_messages[side] = (signature, now)
|
|
self.hand_publishers[side].publish(msg)
|
|
self.last_hand_publish_at = now
|
|
self.hand_publish_count += 1
|
|
|
|
def _safety_reasons(
|
|
self,
|
|
now: float,
|
|
sample: ArmSnapshot | None,
|
|
source_error: str,
|
|
check_iarm: bool = True,
|
|
check_iarm_target: bool = True,
|
|
check_hands: bool = True,
|
|
check_hand_feedback: bool | None = None,
|
|
) -> list[str]:
|
|
reasons: list[str] = []
|
|
net_cfg = self.cfg["network"]
|
|
robot_cfg = self.cfg["robot"]
|
|
control_cfg = self.cfg["control"]
|
|
if check_hand_feedback is None:
|
|
check_hand_feedback = check_hands
|
|
|
|
if check_iarm and sample is None:
|
|
reasons.append(source_error or "no isomorphic-arm data")
|
|
elif check_iarm:
|
|
age = now - sample.received_at
|
|
if age > float(net_cfg["source_timeout_s"]):
|
|
reasons.append(f"isomorphic-arm data stale ({age:.3f}s)")
|
|
data = sample.data
|
|
expected_id = str(net_cfg.get("expected_iarm_id", ""))
|
|
expected_type = str(net_cfg.get("expected_iarm_type", ""))
|
|
if expected_id and data.get("isomorphic_arm_id") != expected_id:
|
|
reasons.append("unexpected isomorphic-arm ID")
|
|
if expected_type and data.get("isomorphic_arm_type") != expected_type:
|
|
reasons.append("unexpected isomorphic-arm type")
|
|
|
|
errors = data.get("servo_error", {})
|
|
arm_errors = [*errors.get("left", []), *errors.get("right", [])]
|
|
if len(arm_errors) != 14 or any(int(v) != 0 for v in arm_errors):
|
|
reasons.append("arm servo error is non-zero or incomplete")
|
|
joy_errors = data.get("joycan_error", [])
|
|
if len(joy_errors) < 2 or any(int(v) != 0 for v in joy_errors[:2]):
|
|
reasons.append("TS1P controller/CAN error")
|
|
minimum_hz = float(net_cfg["minimum_arm_frequency_hz"])
|
|
freq = data.get("freq", {})
|
|
if float(freq.get("left", 0.0)) < minimum_hz or float(
|
|
freq.get("right", 0.0)
|
|
) < minimum_hz:
|
|
reasons.append("TS1P arm sampling frequency too low")
|
|
if check_iarm_target:
|
|
reasons.extend(self._limit_reasons(self._positions(data), control_cfg))
|
|
if self.hands_enabled and check_hands:
|
|
try:
|
|
self._hand_targets(data)
|
|
except (KeyError, TypeError, ValueError) as exc:
|
|
reasons.append(f"invalid isomorphic-hand target: {exc}")
|
|
|
|
arm_state_age = now - self.robot_arm_at
|
|
if self.robot_arm_at == 0.0 or arm_state_age > float(
|
|
robot_cfg["arm_state_timeout_s"]
|
|
):
|
|
reasons.append("robot arm state unavailable/stale")
|
|
elif len(self.robot_arm_errors) != 14 or any(self.robot_arm_errors):
|
|
reasons.append("robot arm motor error is non-zero or incomplete")
|
|
|
|
if self.hands_enabled and check_hand_feedback:
|
|
reasons.extend(self._hand_feedback_reasons(now))
|
|
|
|
rl_age = now - self.rl_state_at
|
|
if self.rl_state_at == 0.0 or rl_age > float(robot_cfg["state_timeout_s"]):
|
|
reasons.append("robot RL state unavailable/stale")
|
|
else:
|
|
required = str(robot_cfg["required_state"])
|
|
if self.rl_state.get("current_state") != required:
|
|
reasons.append(f"robot current_state is not {required}")
|
|
if self.rl_state.get("child_state") != required:
|
|
reasons.append(f"robot child_state is not {required}")
|
|
if self.rl_state.get("status") != str(robot_cfg["required_status"]):
|
|
reasons.append("robot RL status is not running")
|
|
if self.foreign_source_seen:
|
|
reasons.append("foreign/cloud arm command source was detected")
|
|
if self.hands_enabled and check_hands and self.foreign_hand_source_seen:
|
|
reasons.append("foreign BrainCo hand command source was detected")
|
|
return reasons
|
|
|
|
def _hand_feedback_reasons(self, now: float) -> list[str]:
|
|
"""Return reasons that should pause hand output, without stopping arms."""
|
|
|
|
reasons: list[str] = []
|
|
hand_timeout = float(self.hands_cfg["status_timeout_s"])
|
|
for side in HAND_SIDES:
|
|
age = now - self.robot_hand_at[side]
|
|
positions = self.robot_hand_positions[side]
|
|
states = self.robot_hand_states[side]
|
|
if self.robot_hand_at[side] == 0.0 or age > hand_timeout:
|
|
reasons.append(f"robot {side} hand state unavailable/stale")
|
|
elif positions is None or len(positions) != 6:
|
|
reasons.append(f"robot {side} hand position is incomplete")
|
|
elif len(states) != 6 or any(
|
|
state not in (0, 1, 2, 3) for state in states
|
|
):
|
|
# BrainCo MotorState: 0=idle, 1=running, 2=stall/contact or
|
|
# limit, 3=turbo/continuous force, 255=unknown. Running and
|
|
# contact are normal operating states; the vendor driver
|
|
# retains its own current, stall and limit protection.
|
|
reasons.append(
|
|
f"robot {side} hand state is unknown/invalid or incomplete"
|
|
)
|
|
return reasons
|
|
|
|
@staticmethod
|
|
def _positions(data: dict[str, Any]) -> list[float]:
|
|
position = data["arm"]["position"]
|
|
return [float(v) for v in [*position["left"], *position["right"]]]
|
|
|
|
def _hand_targets(self, data: dict[str, Any]) -> dict[str, list[int]]:
|
|
raw_positions = data["hand"]["position"]
|
|
open_pose = [float(value) for value in self.hands_cfg["open_normalized"]]
|
|
closed_pose = [float(value) for value in self.hands_cfg["closed_normalized"]]
|
|
if len(open_pose) != 6 or len(closed_pose) != 6:
|
|
raise ValueError("hand open/closed poses must each contain 6 values")
|
|
if not all(
|
|
math.isfinite(value) and 0.0 <= value <= 1.0
|
|
for value in [*open_pose, *closed_pose]
|
|
):
|
|
raise ValueError("hand open/closed poses must be finite values in [0, 1]")
|
|
|
|
minimum = int(self.hands_cfg["position_min"])
|
|
maximum = int(self.hands_cfg["position_max"])
|
|
if minimum < 0 or maximum <= minimum:
|
|
raise ValueError("invalid BrainCo position range")
|
|
targets: dict[str, list[int]] = {}
|
|
for side in HAND_SIDES:
|
|
raw = raw_positions[side]
|
|
if isinstance(raw, (int, float)) and not isinstance(raw, bool):
|
|
scalar = float(raw)
|
|
if not math.isfinite(scalar) or scalar < -0.05 or scalar > 1.05:
|
|
raise ValueError(f"{side} scalar must be in [0, 1]")
|
|
scalar = max(0.0, min(1.0, scalar))
|
|
if bool(self.hands_cfg.get("invert_scalar", False)):
|
|
scalar = 1.0 - scalar
|
|
normalized = [
|
|
opened + scalar * (closed - opened)
|
|
for opened, closed in zip(open_pose, closed_pose)
|
|
]
|
|
elif isinstance(raw, list) and len(raw) == 6:
|
|
normalized = [float(value) for value in raw]
|
|
if not all(
|
|
math.isfinite(value) and -0.05 <= value <= 1.05
|
|
for value in normalized
|
|
):
|
|
raise ValueError(f"{side} 6-D target must be in [0, 1]")
|
|
normalized = [max(0.0, min(1.0, value)) for value in normalized]
|
|
else:
|
|
raise ValueError(f"{side} target must be a scalar or 6-D list")
|
|
targets[side] = [
|
|
int(round(minimum + value * (maximum - minimum)))
|
|
for value in normalized
|
|
]
|
|
return targets
|
|
|
|
@staticmethod
|
|
def _limit_reasons(positions: list[float], cfg: dict[str, Any]) -> list[str]:
|
|
lower = [float(v) for v in cfg["joint_lower_rad"]]
|
|
upper = [float(v) for v in cfg["joint_upper_rad"]]
|
|
margin = float(cfg["joint_limit_margin_rad"])
|
|
if len(lower) != 14 or len(upper) != 14:
|
|
return ["configuration must define 14 joint limits"]
|
|
bad = [
|
|
JOINT_NAMES[i]
|
|
for i, value in enumerate(positions)
|
|
if value < lower[i] + margin or value > upper[i] - margin
|
|
]
|
|
return ["joint target outside safe limit: " + ",".join(bad)] if bad else []
|
|
|
|
def _home_config_reasons(self) -> list[str]:
|
|
home_cfg = self.cfg["home"]
|
|
try:
|
|
goal = [float(v) for v in home_cfg["joint_goal_rad"]]
|
|
values = [
|
|
float(home_cfg["slew_rad_s"]),
|
|
float(home_cfg["max_tracking_error_rad"]),
|
|
float(home_cfg["command_tolerance_rad"]),
|
|
float(home_cfg["actual_tolerance_rad"]),
|
|
float(home_cfg["settle_s"]),
|
|
float(home_cfg["timeout_s"]),
|
|
]
|
|
except (KeyError, TypeError, ValueError):
|
|
return ["invalid return-home configuration"]
|
|
if len(goal) != 14 or not all(math.isfinite(v) for v in [*goal, *values]):
|
|
return ["return-home configuration must contain finite values for 14 joints"]
|
|
if any(v <= 0.0 for v in values):
|
|
return ["return-home speed, tolerances and timeouts must be positive"]
|
|
return self._limit_reasons(goal, self.cfg["control"])
|
|
|
|
def _slew_limit(self, desired: list[float], now: float) -> list[float]:
|
|
if self.last_command is None:
|
|
return list(desired)
|
|
dt = max(0.001, now - self.last_publish_at)
|
|
max_step = float(self.cfg["control"]["max_slew_rad_s"]) * dt
|
|
return [
|
|
previous + max(-max_step, min(max_step, target - previous))
|
|
for previous, target in zip(self.last_command, desired)
|
|
]
|
|
|
|
def _slew_hand(self, side: str, desired: list[int], now: float) -> list[int]:
|
|
previous = self.last_hand_commands[side]
|
|
if previous is None or len(previous) != 6:
|
|
return list(desired)
|
|
dt = max(0.001, now - self.last_hand_publish_at)
|
|
max_step = float(self.hands_cfg["slew_units_per_s"]) * dt
|
|
return [
|
|
int(round(current + max(-max_step, min(max_step, target - current))))
|
|
for current, target in zip(previous, desired)
|
|
]
|
|
|
|
def _write_status(
|
|
self, now: float, sample: ArmSnapshot | None, reasons: list[str]
|
|
) -> None:
|
|
combo_elapsed = 0.0
|
|
if self.combo_started_at is not None:
|
|
combo_elapsed = now - self.combo_started_at
|
|
source_metrics = self.source.metrics()
|
|
status = {
|
|
"mode": "active-capable" if self.allow_publish else "monitor-only",
|
|
"armed": self.armed,
|
|
"returning_home": self.returning_home,
|
|
"home_status": self.home_status,
|
|
"home_speed_rad_s": self.cfg["home"]["slew_rad_s"],
|
|
"home_goal_rad": self.cfg["home"]["joint_goal_rad"],
|
|
"auto_home_on_stop": bool(
|
|
self.cfg["control"].get("auto_home_on_stop", True)
|
|
),
|
|
"custom_joint_limit_policy": "startup_only",
|
|
"custom_hand_feedback_policy": (
|
|
"startup_qualifies_teleop; runtime_gap_pauses_hands_only"
|
|
),
|
|
"runtime_hand_output_ready": self.hand_output_ready,
|
|
"runtime_hand_output_reasons": self.runtime_hand_output_reasons,
|
|
"safety_ready": not reasons,
|
|
"safety_reasons": reasons,
|
|
"iarm_transport": self.source.transport,
|
|
"iarm_endpoint": self.source.description,
|
|
"iarm_transport_status": source_metrics,
|
|
"iarm_watchdog_restart_after_s": self.cfg["network"].get(
|
|
"omnisocket_restart_after_stale_s"
|
|
),
|
|
"iarm_connected": sample is not None,
|
|
"iarm_stream_fresh": (
|
|
sample is not None
|
|
and now - sample.received_at
|
|
<= float(self.cfg["network"]["source_timeout_s"])
|
|
),
|
|
"iarm_age_s": None if sample is None else round(now - sample.received_at, 4),
|
|
"iarm_id": None if sample is None else sample.data.get("isomorphic_arm_id"),
|
|
"iarm_frequency_hz": None if sample is None else sample.data.get("freq"),
|
|
"rl_state": self.rl_state,
|
|
"rl_state_age_s": None
|
|
if self.rl_state_at == 0.0
|
|
else round(now - self.rl_state_at, 4),
|
|
"robot_arm_state_age_s": None
|
|
if self.robot_arm_at == 0.0
|
|
else round(now - self.robot_arm_at, 4),
|
|
"robot_arm_position_rad": self.robot_arm_positions,
|
|
"robot_arm_errors": self.robot_arm_errors,
|
|
"foreign_source_seen": self.foreign_source_seen,
|
|
"operator_session_state": self.last_session_state,
|
|
"operator_session_id": self.active_session_id,
|
|
"operator_session_start_attempted_id": self.session_start_attempted_id,
|
|
"operator_session_stop_reason": self.last_session_stop_reason,
|
|
"brainco_hands_enabled": self.hands_enabled,
|
|
"iarm_hand_position": None
|
|
if sample is None
|
|
else sample.data.get("hand", {}).get("position"),
|
|
"robot_hand_positions": self.robot_hand_positions,
|
|
"robot_hand_states": self.robot_hand_states,
|
|
"robot_hand_state_age_s": {
|
|
side: None
|
|
if self.robot_hand_at[side] == 0.0
|
|
else round(now - self.robot_hand_at[side], 4)
|
|
for side in HAND_SIDES
|
|
},
|
|
"last_hand_commands": self.last_hand_commands,
|
|
"last_hand_publish_age_s": None
|
|
if self.last_hand_publish_at == 0.0
|
|
else round(now - self.last_hand_publish_at, 4),
|
|
"hand_publish_count": self.hand_publish_count,
|
|
"foreign_hand_source_seen": self.foreign_hand_source_seen,
|
|
"locomotion_enabled": self.locomotion_enabled,
|
|
"locomotion_binding": "right_C + left_joystick, immediate",
|
|
"locomotion_active": self.walk_active,
|
|
"locomotion_hold_s": 0.0,
|
|
"locomotion_command": {
|
|
"linear_x_m_s": self.walk_command[0],
|
|
"angular_z_rad_s": self.walk_command[1],
|
|
},
|
|
"locomotion_limits": {
|
|
"forward_m_s": self.locomotion_cfg.get("max_forward_m_s"),
|
|
"reverse_m_s": self.locomotion_cfg.get("max_reverse_m_s"),
|
|
"angular_rad_s": self.locomotion_cfg.get("max_angular_rad_s"),
|
|
},
|
|
"locomotion_publish_count": self.walk_publish_count,
|
|
"locomotion_fsm_publish_enabled": False,
|
|
"last_locomotion_publish_age_s": None
|
|
if self.last_walk_publish_at == 0.0
|
|
else round(now - self.last_walk_publish_at, 4),
|
|
"unsupported_binding": (
|
|
"right_C + right_A (C+A) is not registered by xTELE 0.1.2"
|
|
),
|
|
"start_stop_combo": (
|
|
"EAI-gated left_Z + right_C, hold 3s; matching STOP triggers Home"
|
|
if self.source.transport == "omnisocket"
|
|
else "local left_Z + right_C, hold 3s; stop triggers Home"
|
|
),
|
|
"combo_hold_s": round(combo_elapsed, 2),
|
|
"last_publish_age_s": None
|
|
if self.last_publish_at == 0.0
|
|
else round(now - self.last_publish_at, 4),
|
|
"updated_unix_s": time.time(),
|
|
}
|
|
try:
|
|
self.status_file.parent.mkdir(parents=True, exist_ok=True)
|
|
temporary = self.status_file.with_suffix(self.status_file.suffix + ".tmp")
|
|
temporary.write_text(json.dumps(status, ensure_ascii=False, indent=2) + "\n")
|
|
os.replace(temporary, self.status_file)
|
|
except OSError as exc:
|
|
self.get_logger().error(f"cannot write status file: {exc}")
|
|
|
|
|
|
def load_config(path: Path) -> dict[str, Any]:
|
|
with path.open("rb") as handle:
|
|
return tomllib.load(handle)
|
|
|
|
|
|
def parse_args() -> argparse.Namespace:
|
|
parser = argparse.ArgumentParser(description=__doc__)
|
|
parser.add_argument("--config", type=Path, required=True)
|
|
parser.add_argument(
|
|
"--allow-publish",
|
|
action="store_true",
|
|
help=(
|
|
"allow a validated EAI START session (or direct-ZMQ fallback "
|
|
"combo) to arm ROS publication"
|
|
),
|
|
)
|
|
parser.add_argument(
|
|
"--status-file", type=Path, default=Path("/tmp/tg3_local_teleop_status.json")
|
|
)
|
|
parser.add_argument(
|
|
"--duration",
|
|
type=float,
|
|
default=0.0,
|
|
help="exit after this many seconds (used for monitor-only validation)",
|
|
)
|
|
return parser.parse_args()
|
|
|
|
|
|
def main() -> int:
|
|
args = parse_args()
|
|
config = load_config(args.config)
|
|
rclpy.init()
|
|
node = LocalTeleopBridge(config, args.allow_publish, args.status_file)
|
|
deadline = time.monotonic() + args.duration if args.duration > 0 else None
|
|
try:
|
|
while rclpy.ok() and (deadline is None or time.monotonic() < deadline):
|
|
rclpy.spin_once(node, timeout_sec=0.1)
|
|
except KeyboardInterrupt:
|
|
pass
|
|
finally:
|
|
node.close()
|
|
node.destroy_node()
|
|
rclpy.shutdown()
|
|
return 0
|
|
|
|
|
|
if __name__ == "__main__":
|
|
raise SystemExit(main())
|