用手机遥操机械臂的坐标变换方法

为了手机任意初始化都能合乎直觉地能操作任意初始化的机械臂，我们在 t0 时刻创建一个与手机坐标系固连的 x 前 z 上的附加坐标系，以及与机械臂末端固连的 x 前 z 上的附加坐标系，随后让这两个坐标系永远同步变化.

这里的前就是机械臂桌的前方.

实验表明，这非常 work. 下面是给 codex 的 prompt（与实验实际略有不同，实验在 iphone 端直接发送 a_t_to_a0 感觉没啥必要.

在仓库中，scripts/ 下，新建脚本。需要的 API 可以多参考 consumer/ 下的文件

就像附录 server 代码那样，开一个服务器，接收 iphone 发来的变换.

收到第一帧 iPhone 变换时，这个变换变量叫做 a0_to_iphone_base（用 class 管理状态，手机端有处理，其实这个大概率是单位矩阵）

在这个瞬间，读取机械臂 TCP 位姿，存为变量 tcp0_to_base，（TCP 第 0 帧在机械臂 base 下的变换）

后续任意时刻，我们会收到 iPhone 的变换，变量叫做 a_t_to_iphone_base

我们要求的就是 tcp_t_to_base （TCP 第 t 帧在机械臂 base 下的变换），这个直接发送给机械臂作为 target pose

怎么算？

在存储 a0 和 tcp0_to_base 的时候，我们定义变换 b0_to_base，其 position = tcp0_to_base.position，其 rotation 为单位矩阵.

计算 b_to_tcp = tcp0_to_base.inverse() * b0_to_base，这是一个纯旋转矩阵. a_t_to_a0 也可以计算.

然后 tcp_t_to_base = tcp0_to_base.inverse() * b_to_tcp * a_t_to_a0 * b_to_tcp.inverse()，算出来了后参考合适的非夕机械臂 API，让机械臂运动。

发过来的 throttle，计算 width = 1 - throttle，然后给夹爪发送 width 相对宽度（我记得 APi 好像只能发绝对值，那就以 0.0953 为最大值）.

停顿规则

iphone 有时候过程中停止发送，也可能网络卡顿，如果超过 1 秒，接下来收到时视为新的第一帧。如有不确定信息，问我

给 Ai 的附录：

server 代码参考如下：

1
# /// script
2
# dependencies = ["fastapi", "uvicorn", "pydantic"]
3
# ///
4
from __future__ import annotations
5

6
import math
7
import time
8
from typing import Annotated
9

10
from fastapi import FastAPI, Response, status
11
from pydantic import BaseModel, ConfigDict, Field, model_validator
12
import uvicorn
13

14

15
Finite = Annotated[float, Field(allow_inf_nan=False)]
87 collapsed lines
16
UnitInterval = Annotated[float, Field(ge=0.0, le=1.0, allow_inf_nan=False)]
17

18

19
class Translation(BaseModel):
20
    model_config = ConfigDict(extra="forbid")
21
    x: Finite
22
    y: Finite
23
    z: Finite
24

25

26
class Rotation(BaseModel):
27
    model_config = ConfigDict(extra="forbid")
28
    w: Finite
29
    x: Finite
30
    y: Finite
31
    z: Finite
32

33
    @model_validator(mode="after")
34
    def require_unit_quaternion(self) -> Rotation:
35
        norm = math.sqrt(self.w**2 + self.x**2 + self.y**2 + self.z**2)
36
        if not math.isclose(norm, 1.0, abs_tol=1e-3):
37
            raise ValueError("rotation must be a unit quaternion")
38
        return self
39

40

41
class Transform(BaseModel):
42
    model_config = ConfigDict(extra="forbid")
43
    translation: Translation
44
    rotation: Rotation
45

46
    def __repr__(self) -> str:
47
        """Format one transform compactly with three decimal places."""
48
        translation = self.translation
49
        rotation = self.rotation
50
        return (
51
            "Transform("
52
            f"translation=({translation.x:.3f}, {translation.y:.3f}, {translation.z:.3f}), "
53
            f"rotation=({rotation.w:.3f}, {rotation.x:.3f}, {rotation.y:.3f}, {rotation.z:.3f})"
54
            ")"
55
        )
56

57

58
class TeleopCommand(BaseModel):
59
    model_config = ConfigDict(extra="forbid")
60
    timestamp: Finite
61
    throttle: UnitInterval
62
    transform: Transform
63

64

65
class ServerState(BaseModel):
66
    received: int
67
    receive_rate_hz: float
68
    latest: TeleopCommand | None
69

70

71
app = FastAPI(title="Ardc2 Teleop", version="1.0.0")
72
latest_command: TeleopCommand | None = None
73
received = 0
74
started_at = time.monotonic()
75

76

77
@app.get("/healthz")
78
async def health() -> dict[str, bool]:
79
    return {"ok": True}
80

81

82
@app.get("/status", response_model=ServerState)
83
async def teleop_status() -> ServerState:
84
    elapsed = max(time.monotonic() - started_at, 1e-6)
85
    return ServerState(
86
        received=received,
87
        receive_rate_hz=received / elapsed,
88
        latest=latest_command,
89
    )
90

91

92
@app.post("/teleop", status_code=status.HTTP_204_NO_CONTENT)
93
async def receive_command(command: TeleopCommand) -> Response:
94
    global latest_command, received
95
    latest_command = command
96
    received += 1
97
    print(f"throttle={command.throttle:.3f} transform={command.transform!r}")
98
    return Response(status_code=status.HTTP_204_NO_CONTENT)
99

100

101
if __name__ == "__main__":
102
    uvicorn.run(app, host="0.0.0.0", port=4050)