From 681cba7390236fb217cbc71cbf08f2a096269d01 Mon Sep 17 00:00:00 2001 From: hjoy008 Date: Tue, 14 Apr 2026 17:07:54 -0400 Subject: [PATCH 1/3] feat: arm stow mode --- .../backend/models_pydantic.py | 7 + .../basestation_gui/backend/ra_controls.py | 139 ++++++++++- .../basestation_gui/backend/routes/arm.py | 65 ++++- .../basestation_gui/backend/ws/arm_ws.py | 32 ++- .../frontend/src/components/ArmControls.vue | 231 +++++++++++++++--- .../src/components/StowConfigModal.vue | 150 ++++++++++++ .../frontend/src/types/websocket.d.ts | 9 + .../frontend/src/utils/apiTypes.ts | 21 ++ .../frontend/src/utils/armAPI.ts | 33 ++- teleoperation/basestation_gui/server.py | 3 + 10 files changed, 638 insertions(+), 52 deletions(-) create mode 100644 teleoperation/basestation_gui/frontend/src/components/StowConfigModal.vue diff --git a/teleoperation/basestation_gui/backend/models_pydantic.py b/teleoperation/basestation_gui/backend/models_pydantic.py index c1b1a1b98..b28d2fa7b 100644 --- a/teleoperation/basestation_gui/backend/models_pydantic.py +++ b/teleoperation/basestation_gui/backend/models_pydantic.py @@ -84,6 +84,13 @@ class RecordingWaypointRequest(BaseModel): class RAModeRequest(BaseModel): mode: str +class StowPositionRequest(BaseModel): + x: float + y: float + z: float + pitch: float + roll: float + class ServoPositionRequest(BaseModel): names: List[str] positions: List[float] diff --git a/teleoperation/basestation_gui/backend/ra_controls.py b/teleoperation/basestation_gui/backend/ra_controls.py index 8fb269207..0d343fc03 100644 --- a/teleoperation/basestation_gui/backend/ra_controls.py +++ b/teleoperation/basestation_gui/backend/ra_controls.py @@ -1,37 +1,82 @@ +import asyncio +import json +import math from enum import Enum +from pathlib import Path import threading +import rclpy.time +import tf2_ros +from tf2_ros import LookupException, ConnectivityException, ExtrapolationException from rclpy.publisher import Publisher from backend.input import filter_input, simulated_axis, safe_index, DeviceInputs from backend.mappings import ControllerAxis, ControllerButton from backend.managers.ros import get_service_client from backend.utils.ros_service import call_service_async +from lie import SE3 from mrover.msg import Throttle, IK from mrover.srv import IkMode from geometry_msgs.msg import Twist +STOW_CONFIG_PATH = Path(__file__).parent / '../../stow_config.json' + ra_mode = "disabled" ra_mode_lock = threading.Lock() +ik_pos_pub: Publisher | None = None +stow_task: asyncio.Task | None = None +tf_buffer: tf2_ros.Buffer | None = None + + +async def stow_publish_loop() -> None: + try: + while get_ra_mode() == "stow" and ik_pos_pub is not None: + ik_pos_pub.publish(STOW_POSITION) + await asyncio.sleep(0.1) + except asyncio.CancelledError: + pass + + +def register_ik_pos_pub(pub: Publisher) -> None: + global ik_pos_pub + ik_pos_pub = pub + + +def register_tf_buffer(buffer: tf2_ros.Buffer) -> None: + global tf_buffer + tf_buffer = buffer + def get_ra_mode() -> str: with ra_mode_lock: return ra_mode -async def set_ra_mode(new_ra_mode: str): - global ra_mode - if new_ra_mode == "ik-pos": +async def set_ra_mode(new_ra_mode: str) -> bool: + global ra_mode, stow_task + if new_ra_mode in ("ik-pos", "stow"): if not await call_ik_mode_service(IK_MODE_POSITION_CONTROL): - return + return False elif new_ra_mode == "ik-vel": if not await call_ik_mode_service(IK_MODE_VELOCITY_CONTROL): - return + return False with ra_mode_lock: ra_mode = new_ra_mode + if new_ra_mode == "stow": + if ik_pos_pub is not None: + ik_pos_pub.publish(STOW_POSITION) + if stow_task is None or stow_task.done(): + stow_task = asyncio.create_task(stow_publish_loop()) + else: + if stow_task is not None and not stow_task.done(): + stow_task.cancel() + stow_task = None + + return True + async def call_ik_mode_service(mode: int) -> bool: client = get_service_client(IkMode, "/ik_mode") @@ -45,6 +90,86 @@ async def call_ik_mode_service(mode: int) -> bool: IK_MODE_VELOCITY_CONTROL = 1 IK_MODE_TYPING = 2 +STOW_POSITION_DEFAULTS = { + "x": 1.124319, + "y": 0.0, + "z": 0.042229, + "pitch": 0.072694, + "roll": 0.0, +} +STOW_POSITION = IK() + + +def _quat_to_pitch_roll(qx: float, qy: float, qz: float, qw: float) -> tuple[float, float]: + sin_pitch = 2.0 * (qw * qy - qz * qx) + sin_pitch = max(-1.0, min(1.0, sin_pitch)) + pitch = math.asin(sin_pitch) + roll = math.atan2(2.0 * (qw * qx + qy * qz), 1.0 - 2.0 * (qx * qx + qy * qy)) + return pitch, roll + + +def _apply_stow_fields(values: dict) -> None: + STOW_POSITION.pos.x = float(values["x"]) + STOW_POSITION.pos.y = float(values["y"]) + STOW_POSITION.pos.z = float(values["z"]) + STOW_POSITION.pitch = float(values.get("pitch", STOW_POSITION_DEFAULTS["pitch"])) + STOW_POSITION.roll = float(values.get("roll", STOW_POSITION_DEFAULTS["roll"])) + + +def stow_position_dict() -> dict: + return { + "x": STOW_POSITION.pos.x, + "y": STOW_POSITION.pos.y, + "z": STOW_POSITION.pos.z, + "pitch": STOW_POSITION.pitch, + "roll": STOW_POSITION.roll, + } + + +def load_stow_position() -> None: + try: + stored = json.loads(STOW_CONFIG_PATH.read_text()) + except (FileNotFoundError, json.JSONDecodeError): + stored = STOW_POSITION_DEFAULTS + _apply_stow_fields(stored) + + +def update_stow_position(x: float, y: float, z: float, pitch: float, roll: float) -> dict: + values = {"x": x, "y": y, "z": z, "pitch": pitch, "roll": roll} + _apply_stow_fields(values) + STOW_CONFIG_PATH.write_text(json.dumps(values, indent=2)) + return stow_position_dict() + + +def reset_stow_position() -> dict: + _apply_stow_fields(STOW_POSITION_DEFAULTS) + STOW_CONFIG_PATH.unlink(missing_ok=True) + return stow_position_dict() + + +def capture_current_arm_pose() -> dict | None: + if tf_buffer is None: + return None + try: + if not tf_buffer.can_transform("arm_base_link", "arm_fk", rclpy.time.Time()): + return None + arm_in_base = SE3.from_tf_tree(tf_buffer, "arm_fk", "arm_base_link") + except (LookupException, ConnectivityException, ExtrapolationException): + return None + tx, ty, tz = arm_in_base.translation() + qx, qy, qz, qw = arm_in_base.quat() + pitch, roll = _quat_to_pitch_roll(float(qx), float(qy), float(qz), float(qw)) + return { + "x": float(tx), + "y": float(ty), + "z": float(tz), + "pitch": pitch, + "roll": roll, + } + + +_apply_stow_fields(STOW_POSITION_DEFAULTS) + class Joint(Enum): A = 0 @@ -158,8 +283,8 @@ def send_ra_controls( ik_vel_msg.linear.x = (-1.0) * filter_input(safe_index(inputs.axes, ControllerAxis.LEFT_Y), deadzone=CONTROLLER_STICK_DEADZONE) ik_vel_msg.linear.y = (-1.0) * filter_input(safe_index(inputs.axes, ControllerAxis.LEFT_X), deadzone=CONTROLLER_STICK_DEADZONE) ik_vel_msg.linear.z = (-1.0) * filter_input(safe_index(inputs.axes, ControllerAxis.RIGHT_Y), deadzone=CONTROLLER_STICK_DEADZONE) - ik_vel_msg.angular.y = 1.0 * simulated_axis(inputs.buttons, ControllerButton.RIGHT_BUMPER, ControllerButton.LEFT_BUMPER) - ik_vel_msg.angular.x = 1.0 * simulated_axis(inputs.buttons, ControllerButton.RIGHT_TRIGGER, ControllerButton.LEFT_TRIGGER) + ik_vel_msg.angular.y = 1.0 * simulated_axis(inputs.buttons, ControllerButton.RIGHT_TRIGGER, ControllerButton.LEFT_TRIGGER) + ik_vel_msg.angular.x = 1.0 * simulated_axis(inputs.buttons, ControllerButton.RIGHT_BUMPER, ControllerButton.LEFT_BUMPER) ee_vel_pub.publish(ik_vel_msg) cam_throttle = filter_input( diff --git a/teleoperation/basestation_gui/backend/routes/arm.py b/teleoperation/basestation_gui/backend/routes/arm.py index 643a0613c..773703c8b 100644 --- a/teleoperation/basestation_gui/backend/routes/arm.py +++ b/teleoperation/basestation_gui/backend/routes/arm.py @@ -1,10 +1,17 @@ from fastapi import APIRouter, HTTPException -from backend.ra_controls import set_ra_mode as update_ra_mode -from backend.models_pydantic import RAModeRequest +from backend.ra_controls import ( + set_ra_mode as update_ra_mode, + STOW_POSITION, + stow_position_dict, + update_stow_position, + reset_stow_position, + capture_current_arm_pose, +) +from backend.models_pydantic import RAModeRequest, StowPositionRequest router = APIRouter(prefix="/api/arm", tags=["arm"]) -VALID_RA_MODES = ["disabled", "throttle", "ik-pos", "ik-vel"] +VALID_RA_MODES = ["disabled", "throttle", "ik-pos", "ik-vel", "stow"] @router.post("/ra_mode/") @@ -15,5 +22,55 @@ async def change_ra_mode(data: RAModeRequest): status_code=400, detail=f"Invalid mode '{mode}'. Must be one of: {', '.join(VALID_RA_MODES)}" ) - await update_ra_mode(mode) + if not await update_ra_mode(mode): + raise HTTPException(status_code=503, detail="Failed to set RA mode (ROS service might be unavailable)") + return {"status": "success", "mode": mode} + + +@router.post("/stow/") +async def change_to_stow(): + mode = "stow" + if not await update_ra_mode(mode): + raise HTTPException(status_code=503, detail="Failed to start stow sequence (ROS service might be unavailable)") + + return { + "status": "success", + "mode": mode, + "stow_target": { + "pos": { + "x": STOW_POSITION.pos.x, + "y": STOW_POSITION.pos.y, + "z": STOW_POSITION.pos.z, + }, + "pitch": STOW_POSITION.pitch, + "roll": STOW_POSITION.roll, + }, + } + + +@router.get("/stow/config/") +async def get_stow_config(): + return {"status": "success", "stow_position": stow_position_dict()} + + +@router.post("/stow/capture/") +async def capture_stow_pose(): + pose = capture_current_arm_pose() + if pose is None: + raise HTTPException( + status_code=503, + detail="TF transform arm_base_link -> arm_fk is not available yet.", + ) + return {"status": "success", "stow_position": pose} + + +@router.post("/stow/config/") +async def save_stow_config(data: StowPositionRequest): + saved = update_stow_position(data.x, data.y, data.z, data.pitch, data.roll) + return {"status": "success", "stow_position": saved} + + +@router.post("/stow/config/reset/") +async def reset_stow_config(): + return {"status": "success", "stow_position": reset_stow_position()} diff --git a/teleoperation/basestation_gui/backend/ws/arm_ws.py b/teleoperation/basestation_gui/backend/ws/arm_ws.py index f12d9dbfc..eb1853334 100644 --- a/teleoperation/basestation_gui/backend/ws/arm_ws.py +++ b/teleoperation/basestation_gui/backend/ws/arm_ws.py @@ -1,11 +1,16 @@ +import rclpy.time +import tf2_ros +from tf2_ros import LookupException, ConnectivityException, ExtrapolationException +from lie import SE3 from backend.ws.base_ws import WebSocketHandler from backend.managers.ros import get_logger from backend.input import DeviceInputs -from backend.ra_controls import send_ra_controls +from backend.ra_controls import send_ra_controls, register_ik_pos_pub, register_tf_buffer from mrover.msg import Throttle, IK, ControllerState from geometry_msgs.msg import Twist from rclpy.publisher import Publisher + class ArmHandler(WebSocketHandler): arm_thr_pub: Publisher ik_pos_pub: Publisher @@ -13,16 +18,41 @@ class ArmHandler(WebSocketHandler): def __init__(self, websocket): super().__init__(websocket, 'arm') + self.buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.buffer, self.node, spin_thread=False) async def setup(self): self.arm_thr_pub = self.node.create_publisher(Throttle, "/arm_thr_cmd", 1) self.ik_pos_pub = self.node.create_publisher(IK, "/ik_pos_cmd", 1) self.ik_vel_pub = self.node.create_publisher(Twist, "/ik_vel_cmd", 1) self.publishers.extend([self.arm_thr_pub, self.ik_pos_pub, self.ik_vel_pub]) + register_ik_pos_pub(self.ik_pos_pub) + register_tf_buffer(self.buffer) self.forward_ros_topic("/arm_controller_state", ControllerState, "arm_state") self.forward_ros_topic("/arm_ik", IK, "ik_target") + self.timers.append(self.node.create_timer(0.1, self.send_arm_feedback_callback)) + + def send_arm_feedback_callback(self): + try: + if not self.buffer.can_transform("arm_base_link", "arm_fk", rclpy.time.Time()): + return + arm_in_base = SE3.from_tf_tree(self.buffer, "arm_fk", "arm_base_link") + pos = arm_in_base.translation() + self.schedule_send({ + "type": "ik_feedback", + "pos": { + "x": float(pos[0]), + "y": float(pos[1]), + "z": float(pos[2]), + }, + }) + except (LookupException, ConnectivityException, ExtrapolationException): + pass + except Exception as e: + get_logger().error(f"ArmHandler feedback error: {e}") + async def handle_message(self, data): msg_type = data.get('type') diff --git a/teleoperation/basestation_gui/frontend/src/components/ArmControls.vue b/teleoperation/basestation_gui/frontend/src/components/ArmControls.vue index 0bd1abfff..e03f53d12 100644 --- a/teleoperation/basestation_gui/frontend/src/components/ArmControls.vue +++ b/teleoperation/basestation_gui/frontend/src/components/ArmControls.vue @@ -1,59 +1,113 @@ - diff --git a/teleoperation/basestation_gui/frontend/src/components/StowConfigModal.vue b/teleoperation/basestation_gui/frontend/src/components/StowConfigModal.vue new file mode 100644 index 000000000..b103c3ab2 --- /dev/null +++ b/teleoperation/basestation_gui/frontend/src/components/StowConfigModal.vue @@ -0,0 +1,150 @@ + + + diff --git a/teleoperation/basestation_gui/frontend/src/types/websocket.d.ts b/teleoperation/basestation_gui/frontend/src/types/websocket.d.ts index 320163dda..125ce6abd 100644 --- a/teleoperation/basestation_gui/frontend/src/types/websocket.d.ts +++ b/teleoperation/basestation_gui/frontend/src/types/websocket.d.ts @@ -114,3 +114,12 @@ export interface SPPressureMessage { } export type ScienceMessage = OxygenMessage | UVMessage | TemperatureMessage | HumidityMessage | SPHumidityMessage | SPTemperatureMessage | SPOxygenMessage | SPUVMessage | SPOzoneMessage | SPCO2Message | SPPressureMessage; + +export interface IkFeedbackMessage { + type: 'ik_feedback'; + pos: { + x: number; + y: number; + z: number; + }; +} diff --git a/teleoperation/basestation_gui/frontend/src/utils/apiTypes.ts b/teleoperation/basestation_gui/frontend/src/utils/apiTypes.ts index 815636010..8271a1fda 100644 --- a/teleoperation/basestation_gui/frontend/src/utils/apiTypes.ts +++ b/teleoperation/basestation_gui/frontend/src/utils/apiTypes.ts @@ -80,3 +80,24 @@ export interface RecordingWaypointsResponse extends APIResponse { export interface RAModeResponse extends APIResponse { mode?: string } + +export interface StowResponse extends APIResponse { + mode?: string + stow_target: { + pos: { x: number; y: number; z: number } + pitch: number + roll: number + } +} + +export interface StowPosition { + x: number + y: number + z: number + pitch: number + roll: number +} + +export interface StowConfigResponse extends APIResponse { + stow_position: StowPosition +} diff --git a/teleoperation/basestation_gui/frontend/src/utils/armAPI.ts b/teleoperation/basestation_gui/frontend/src/utils/armAPI.ts index 0968747bd..b0f664a88 100644 --- a/teleoperation/basestation_gui/frontend/src/utils/armAPI.ts +++ b/teleoperation/basestation_gui/frontend/src/utils/armAPI.ts @@ -1,4 +1,4 @@ -import type { RAModeResponse } from './apiTypes' +import type { RAModeResponse, StowResponse, StowPosition, StowConfigResponse } from './apiTypes' import { apiFetch } from './apiFetch' export const armAPI = { @@ -7,5 +7,36 @@ export const armAPI = { method: 'POST', body: JSON.stringify({ mode }) }) + }, + + stowArm(): Promise { + return apiFetch('/arm/stow/', { + method: 'POST' + }) + }, + + getStowConfig(): Promise { + return apiFetch('/arm/stow/config/', { + method: 'GET' + }) + }, + + captureStowPose(): Promise { + return apiFetch('/arm/stow/capture/', { + method: 'POST' + }) + }, + + saveStowConfig(data: StowPosition): Promise { + return apiFetch('/arm/stow/config/', { + method: 'POST', + body: JSON.stringify(data) + }) + }, + + resetStowConfig(): Promise { + return apiFetch('/arm/stow/config/reset/', { + method: 'POST' + }) } } diff --git a/teleoperation/basestation_gui/server.py b/teleoperation/basestation_gui/server.py index a59255afe..1536cfaf9 100644 --- a/teleoperation/basestation_gui/server.py +++ b/teleoperation/basestation_gui/server.py @@ -110,6 +110,9 @@ async def ws_auton(websocket: WebSocket): from backend.database import ensure_initialized ensure_initialized() + from backend.ra_controls import load_stow_position + load_stow_position() + if args.serve_static: dist = Path(__file__).resolve().parent / "frontend" / "dist" index = dist / "index.html" From fc1d12dd04bd730fe53d0542348185587e7aa417 Mon Sep 17 00:00:00 2001 From: kevinjin420 Date: Tue, 14 Apr 2026 17:14:20 -0400 Subject: [PATCH 2/3] track stow config --- teleoperation/stow_config.json | 7 +++++++ 1 file changed, 7 insertions(+) create mode 100644 teleoperation/stow_config.json diff --git a/teleoperation/stow_config.json b/teleoperation/stow_config.json new file mode 100644 index 000000000..0b03ea095 --- /dev/null +++ b/teleoperation/stow_config.json @@ -0,0 +1,7 @@ +{ + "x": 0.292769027014808, + "y": 4.902924956695642e-06, + "z": -0.33109657146623206, + "pitch": 0.9705731868743901, + "roll": -3.337724410812374e-05 +} \ No newline at end of file From 5626fbf773fe7ccd9d1595050d5a2925d11e81b4 Mon Sep 17 00:00:00 2001 From: kevinjin420 Date: Tue, 14 Apr 2026 17:16:41 -0400 Subject: [PATCH 3/3] lint armcontrols --- .../frontend/src/components/AutonControls.vue | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/teleoperation/basestation_gui/frontend/src/components/AutonControls.vue b/teleoperation/basestation_gui/frontend/src/components/AutonControls.vue index fc5f99479..a6f533753 100644 --- a/teleoperation/basestation_gui/frontend/src/components/AutonControls.vue +++ b/teleoperation/basestation_gui/frontend/src/components/AutonControls.vue @@ -1,6 +1,6 @@