使用水星Mecury人形機(jī)器人搭建VR遙操作控制平臺(tái)！

VR遙操作機(jī)械臂是一種將虛擬現(xiàn)實(shí)技術(shù)與機(jī)械臂控制相結(jié)合的系統(tǒng)，使用戶(hù)可以通過(guò)虛擬現(xiàn)實(shí)設(shè)備操控和交互實(shí)際的機(jī)械臂。這種技術(shù)可以應(yīng)用于多個(gè)領(lǐng)域，包括遠(yuǎn)程操作、培訓(xùn)、危險(xiǎn)環(huán)境中的工作等。

雙臂人形機(jī)器人是一種模擬人體上半身結(jié)構(gòu)，包括頭部、軀干和雙臂的機(jī)器人。這種機(jī)器人設(shè)計(jì)的目標(biāo)是模仿人類(lèi)的上半身動(dòng)作和功能，以執(zhí)行各種任務(wù)，從而在虛擬現(xiàn)實(shí)（VR）領(lǐng)域中有廣泛的應(yīng)用。

合虛擬現(xiàn)實(shí)和人型機(jī)器人可以為許多領(lǐng)域帶來(lái)創(chuàng)新和改進(jìn)，提高用戶(hù)體驗(yàn)，擴(kuò)展應(yīng)用領(lǐng)域，促進(jìn)技術(shù)的發(fā)展。這種結(jié)合使得虛擬和現(xiàn)實(shí)之間的交界更加模糊，為未來(lái)創(chuàng)造了令人興奮的可能性。

硬件介紹
Mecury X1
這是一款名為“Mercury X1”的人形機(jī)器臂，由Elephant Robotics 精心研發(fā)。17個(gè)自由度（DOF），使其具有極高的靈活性和適應(yīng)性。工作電壓為24V，配備了一個(gè)9英寸的量子點(diǎn)觸控屏，既現(xiàn)代又高效。Mercury X1最大工作半徑為450毫米，最大負(fù)載能力為1公斤，凈重8公斤，非常適合輕量級(jí)的操作任務(wù)。

它的重復(fù)定位精度高達(dá)±0.05毫米，意味著它在進(jìn)行精密作業(yè)時(shí)非?？煽?。機(jī)器人的壽命預(yù)計(jì)為5000小時(shí)，表明了其出色的耐用性。主控制單元采用了Jetson Xavier，輔助控制則由ESP32*1負(fù)責(zé)，這樣的配置保證了其強(qiáng)大的數(shù)據(jù)處理能力和穩(wěn)定的控制性能。還包含了碳纖維外殼材料，使得機(jī)器臂不僅堅(jiān)固耐用，同時(shí)也輕便。它裝備了Orbbec Deeyea 3D攝像頭和一個(gè)具有4個(gè)麥克風(fēng)的線(xiàn)性陣列聲音模塊，能夠進(jìn)行高效的視覺(jué)和聲音處理。通信方面，支持WIFI、Ethernet、藍(lán)牙、USB端口和RS485，兼容ROS1/ROS2，這意味著它能夠輕松集成到各種智能制造和研究環(huán)境中。

VR Oculus Quest 2
是由Facebook的子公司Oculus VR開(kāi)發(fā)的一款虛擬現(xiàn)實(shí)（VR）頭戴設(shè)備。Quest 2有開(kāi)發(fā)者模式，它允許開(kāi)發(fā)者直接在設(shè)備上安裝和測(cè)試自己開(kāi)發(fā)的應(yīng)用。主要是由游戲引擎unity和unreal engine平臺(tái)支持。

VR控制機(jī)器人項(xiàng)目
軟件架構(gòu)和交互設(shè)計(jì)
VR遙操作首先要解決的問(wèn)題就是操作者和機(jī)器人的通信問(wèn)題，在這方面我選擇的是基于HTTP協(xié)議的通信，服務(wù)器選擇Flask。雖然選擇Python可能會(huì)對(duì)性能產(chǎn)生不利影響，但是因?yàn)镸ercury機(jī)器人提供的Python SDK——pymycobot是Python的庫(kù),因此Python作為開(kāi)發(fā)語(yǔ)言是最方便與機(jī)器人集成的選擇，非常適合驗(yàn)證項(xiàng)目的可行性。

VR端我們選擇了Unity3D,豐富的社區(qū)資源使得這個(gè)項(xiàng)目成為可能。

服務(wù)模型為經(jīng)典的C/S結(jié)構(gòu)，VR端通過(guò)訪問(wèn)特定的URL向機(jī)器人發(fā)送不同的命令，再通過(guò)服務(wù)器轉(zhuǎn)發(fā)到pymycobot，執(zhí)行實(shí)際的動(dòng)作。

另一方面是交互方面的設(shè)計(jì)，我們采用相對(duì)移動(dòng)的方式，用戶(hù)按下手柄的A鍵標(biāo)志著移動(dòng)開(kāi)始，松開(kāi)標(biāo)志著移動(dòng)結(jié)束。

下面是VR遙控操作的通信流程：

實(shí)時(shí)視頻流
在克服VR遙操作技術(shù)難題的過(guò)程中，確保獲取低延遲的視頻流一直是關(guān)鍵挑戰(zhàn)之一。我們采用了一項(xiàng)創(chuàng)新性的解決方案，通過(guò)利用NVIDIA Jetson Xavier平臺(tái)所提供的Accelerated GStreamer插件，成功實(shí)現(xiàn)了GPU加速的視頻編解碼，旨在在保障實(shí)時(shí)性的同時(shí)，最大程度地優(yōu)化帶寬利用率。

Jetson Xavier平臺(tái)的Accelerated GStreamer插件是一種強(qiáng)大的工具，通過(guò)充分利用GPU的計(jì)算能力，對(duì)視頻數(shù)據(jù)進(jìn)行加速處理。這一創(chuàng)新性的技術(shù)手段不僅提高了視頻編解碼的效率，同時(shí)在大幅度減少延遲的同時(shí)，優(yōu)化了網(wǎng)絡(luò)帶寬的利用效率。

Accelerated GStreamer是NVIDIA為其Jetson平臺(tái)提供的一組GStreamer插件，旨在通過(guò)使用GPU（圖形處理單元）加速多媒體處理任務(wù)，提高性能并降低延遲。這一套插件專(zhuān)為Jetson平臺(tái)設(shè)計(jì)，充分利用了其強(qiáng)大的GPU資源，適用于視頻編解碼、圖像處理、深度學(xué)習(xí)推理等多媒體應(yīng)用。

Accelerated GStreamer — Jetson Linux Developer Guide documentation

實(shí)現(xiàn)過(guò)程
首先，下載編譯并編譯GStreamer官方提供的rtsp server源代碼https://github.com/GStreamer/gst-rtsp-server/blob/1.14.5/examples/test-launch.c

編譯好后會(huì)有一個(gè)test-launch的文件

通過(guò)GStreamer命令構(gòu)造推流管線(xiàn)，這里測(cè)試采用的是

```bash
nvarguscamerasrc sensor-id=0 ! video/x-raw(memory:NVMM), format=NV12, width=3264, height=2464, framerate=21/1 ! nvv4l2h264enc insert-sps-pps=true ! h264parse ! rtph264pay name=pay0 pt=96
```

`提示：! 用于分隔不同的 GStreamer 元素`

nvarguscamerasrc sensor-id=0：

nvarguscamerasrc是 NVIDIA 提供的用于處理 Argus 相機(jī)的 GStreamer 插件。

sensor-id=0 指定使用 ID 為 0 的相機(jī)傳感器。在多攝像頭系統(tǒng)中，可以選擇不同的相機(jī)傳感器。

video/x-raw(memory:NVMM), format=NV12, width=3264, height=2464, framerate=21/1：

video/x-raw(memory:NVMM) 表示輸出的是 NVIDIA 內(nèi)存管理的原始視頻數(shù)據(jù)。

format=NV12 指定了視頻幀的格式為 NV12，這是一種 YUV 格式。

width=3264, height=2464 指定了視頻幀的寬度和高度。

framerate=21/1 指定了視頻的幀率為 21 幀每秒。

nvv4l2h264enc insert-sps-pps=true：

nvv4l2h264enc 是 NVIDIA 提供的 H.264 編碼插件。

insert-sps-pps=true 表示在輸出流中插入 SPS（序列參數(shù)集）和 PPS（圖像參數(shù)集），這對(duì)于 H.264 視頻流的解碼是必需的。

h264parse：

h264parse 插件用于解析 H.264 數(shù)據(jù)流。

rtph264pay name=pay0 pt=96：

rtph264pay 插件用于封裝 H.264 數(shù)據(jù)流為 RTP 包。

name=pay0 為該 RTP Payloader 指定了名稱(chēng)。

pt=96 指定了 RTP 負(fù)載類(lèi)型（Payload Type），這里設(shè)置為 96。

將GST命令與RTSP Server聯(lián)合使用，輸入命令

```bash
./test-launch "nvarguscamerasrc sensor-id=0 ! video/x-raw(memory:NVMM), format=NV12, width=3264, height=2464, framerate=21/1 ! nvv4l2h264enc insert-sps-pps=true ! h264parse ! rtph264pay name=pay0 pt=96"
```

可以在同局域網(wǎng)下的另一臺(tái)主機(jī)上通過(guò)RTSP協(xié)議訪問(wèn)當(dāng)前主機(jī)來(lái)測(cè)試結(jié)果（VLC播放器可以接入RTSP協(xié)議流直接進(jìn)行播放）。

最后，將test-launch.c內(nèi)綁定的URL重新命名為left和right。并將開(kāi)啟和關(guān)閉寫(xiě)入腳本方便執(zhí)行。

```bash
# launch-rtsp.sh
width=1920
height=1080
framerate=28
./left-rtsp-server "nvarguscamerasrc sensor-id=1 ! video/x-raw(memory:NVMM), format=NV12, width=$width, height=$height, framerate=$framerate/1 ! nvv4l2h264enc insert-sps-pps=true maxperf-enable=1 bitrate=8000000 ! h264parse ! rtph264pay name=pay0 pt=96" &
./right-rtsp-server "nvarguscamerasrc sensor-id=0 ! video/x-raw(memory:NVMM), format=NV12, width=$width, height=$height, framerate=$framerate/1 ! nvv4l2h264enc insert-sps-pps=true maxperf-enable=1 bitrate=8000000 ! h264parse ! rtph264pay name=pay0 pt=96" &
```

```bash
# stop-rtsp.sh

ps -al | grep "left|right" | awk '{print $4}' | xargs kill
```

遙操作系統(tǒng)
VR遙操作系統(tǒng)面臨的主要挑戰(zhàn)在于要協(xié)調(diào)處理網(wǎng)絡(luò)的不穩(wěn)定性與對(duì)機(jī)械臂等硬件需要穩(wěn)定輸入的矛盾。這兩者之間的平衡是確保遠(yuǎn)程遙操作系統(tǒng)有效運(yùn)行的關(guān)鍵因素之一。

首先，網(wǎng)絡(luò)的不穩(wěn)定性可能導(dǎo)致延遲、數(shù)據(jù)包丟失或者不確定性的帶寬情況。這種情況會(huì)直接影響到遠(yuǎn)程用戶(hù)與機(jī)械臂之間的實(shí)時(shí)交互。在VR遙操作系統(tǒng)中，用戶(hù)需要感受到虛擬環(huán)境中的實(shí)時(shí)變化，并對(duì)機(jī)械臂的運(yùn)動(dòng)進(jìn)行即時(shí)響應(yīng)。網(wǎng)絡(luò)延遲可能導(dǎo)致用戶(hù)的指令與機(jī)械臂的實(shí)際動(dòng)作之間存在明顯的滯后，降低了操作的準(zhǔn)確性和流暢性。

另一方面，機(jī)械臂等硬件設(shè)備對(duì)輸入的穩(wěn)定性要求較高。由于遙操作系統(tǒng)需要實(shí)時(shí)傳輸用戶(hù)輸入到機(jī)械臂，輸入信號(hào)的不穩(wěn)定性可能導(dǎo)致機(jī)械臂的運(yùn)動(dòng)異?；蚴Э?。這對(duì)于需要高精度和可靠性的任務(wù)，如手術(shù)操作、工業(yè)維護(hù)等，可能帶來(lái)嚴(yán)重的問(wèn)題。

VR遙操作的困局(dilemma)
為了幫助你理解這個(gè)問(wèn)題，首先要了解機(jī)械臂的運(yùn)動(dòng)方式。在傳統(tǒng)機(jī)械臂運(yùn)動(dòng)中，發(fā)給機(jī)械臂一個(gè)點(diǎn)位，機(jī)械臂會(huì)自動(dòng)規(guī)劃到達(dá)目的地的加速與減速，以達(dá)到流暢順滑的移動(dòng)；但是由于機(jī)械臂會(huì)自動(dòng)進(jìn)行加速減速的規(guī)劃，如果用這個(gè)方法去做遙操作，就會(huì)遇到機(jī)械臂在每?jī)蓚€(gè)采樣點(diǎn)上頻繁加速減速的過(guò)程，導(dǎo)致機(jī)械臂無(wú)法連續(xù)運(yùn)動(dòng)。而在遙操作情況下，加速減速應(yīng)該由操作者的手運(yùn)動(dòng)來(lái)控制，因此理論上如果想要實(shí)現(xiàn)順暢的遙操作，則需要機(jī)械臂有一個(gè)可以放棄自動(dòng)規(guī)劃，完全使用采樣點(diǎn)來(lái)進(jìn)行插值運(yùn)動(dòng)的接口。我們將這個(gè)接口命名為“速度融合接口”。

但是機(jī)械臂的底層插值運(yùn)動(dòng)接口通常對(duì)信號(hào)的穩(wěn)定性有著極高的要求，必須要有毫秒級(jí)的穩(wěn)定性才能保證機(jī)械臂穩(wěn)定運(yùn)行；這種完全采用采樣點(diǎn)的插值底層運(yùn)動(dòng)接口，在Mercury機(jī)器人上的VR控制方式為在給定時(shí)間內(nèi)，向指定目標(biāo)點(diǎn)移動(dòng)，舉個(gè)例子，你輸入了一個(gè)坐標(biāo)，還有一個(gè)時(shí)間50ms，那么機(jī)械臂就會(huì)在這50ms內(nèi)向著你發(fā)的坐標(biāo)移動(dòng)，如果到了就會(huì)停止（這也帶來(lái)一個(gè)問(wèn)題，后面會(huì)細(xì)說(shuō)），如果機(jī)械臂沒(méi)到目標(biāo)位置，但是時(shí)間到了的話(huà)也會(huì)停止。

理想狀態(tài)下，如果你持續(xù)且穩(wěn)定地用速度融合直接給機(jī)械臂輸入移動(dòng)點(diǎn)位的命令，且你發(fā)送的間隔，剛好和你給定的運(yùn)動(dòng)時(shí)間片是完全匹配的，那么理論上機(jī)械臂此時(shí)在速度不超過(guò)限速的情況下，能夠完全跟隨人手。

但是此時(shí)如果下一條指令沒(méi)有及時(shí)到來(lái)（可能因?yàn)榫W(wǎng)絡(luò)帶來(lái)的延遲波動(dòng)），機(jī)械臂此時(shí)就會(huì)立刻急停，但是又因?yàn)闆](méi)有減速的過(guò)程，此時(shí)機(jī)械臂就會(huì)因?yàn)榧蓖６a(chǎn)生抖動(dòng)。

一個(gè)合理的改進(jìn)方案是加入緩存，緩存會(huì)平滑網(wǎng)絡(luò)延遲帶來(lái)的誤差，給機(jī)械臂一個(gè)相對(duì)穩(wěn)定的指令流，但是隨之而來(lái)的問(wèn)題就是實(shí)時(shí)性的降低。因?yàn)槿绻彺姘l(fā)揮作用，那么實(shí)際下發(fā)的速度融合參數(shù)內(nèi)給定的時(shí)間片就必須要小于實(shí)際的發(fā)送間隔，因?yàn)橹挥邢M(fèi)的指令比生產(chǎn)的指令更慢，才能保證緩存區(qū)內(nèi)始終有一定量的指令可以下發(fā)，避免產(chǎn)生沒(méi)有指令導(dǎo)致急停的尷尬。但是這樣就會(huì)導(dǎo)致在網(wǎng)絡(luò)穩(wěn)定地情況下，緩沖區(qū)一直是滿(mǎn)的，給整個(gè)控制系統(tǒng)加上了一個(gè)固定的時(shí)間片x緩沖區(qū)大小的延遲。

機(jī)械臂需要穩(wěn)定地控制信號(hào)，因?yàn)樗倪\(yùn)動(dòng)本質(zhì)上由電機(jī)驅(qū)動(dòng)。在常規(guī)的點(diǎn)位移動(dòng)中，機(jī)械臂會(huì)通過(guò)內(nèi)部自動(dòng)規(guī)劃加速和減速，從而實(shí)現(xiàn)相對(duì)較為穩(wěn)定的運(yùn)動(dòng)。然而，在遙操作領(lǐng)域，實(shí)時(shí)性是至關(guān)重要的，因此通常需要設(shè)計(jì)速度融合接口，將由VR端采樣得到的點(diǎn)位直接下發(fā)給機(jī)械臂。這樣的設(shè)計(jì)是為了確保實(shí)時(shí)性，但同時(shí)也帶來(lái)了延遲和穩(wěn)定性難以兼得的問(wèn)題。

如果我們追求實(shí)時(shí)性，從網(wǎng)絡(luò)傳來(lái)一個(gè)點(diǎn)位就立刻發(fā)送，由于我們無(wú)法完全預(yù)測(cè)下一個(gè)控制信號(hào)何時(shí)到來(lái)，則提供的時(shí)間參數(shù)可能就會(huì)和實(shí)際產(chǎn)生偏移，如果給的太少就會(huì)造成機(jī)械臂速度連不起來(lái)，無(wú)法運(yùn)動(dòng)；給的太多就會(huì)造成機(jī)械臂還沒(méi)運(yùn)動(dòng)完，下一個(gè)指令就發(fā)來(lái)了，造成指令堆積，降低實(shí)時(shí)性。如果追求穩(wěn)定性，那就需要增加緩沖隊(duì)列，用緩沖隊(duì)列來(lái)平滑通信方式帶來(lái)的延遲波動(dòng)。但是同樣的，這樣就會(huì)增加控制延遲。這也就是遙操作系統(tǒng)的困局：延遲和穩(wěn)定性不可兼得

VR端實(shí)現(xiàn)
VR端的實(shí)現(xiàn)采用了Unity3D + XR Interactive Toolkit, 選擇Unity3D主要是因?yàn)槠浜?jiǎn)單性，可以快速上手，且能支持多種VR平臺(tái)。如果采用虛幻引擎可能需要花費(fèi)大量時(shí)間在此工程之外的問(wèn)題上。XR Interactive Toolkit是Unity3D的官方VR框架，使用這套系統(tǒng)而不是Oculus插件能夠使項(xiàng)目方便的移植到其他VR設(shè)備上。

開(kāi)發(fā)環(huán)境方面，除了使用Unity3D自帶的Editor以外，C#編輯器使用Visual Studio 2022社區(qū)版。導(dǎo)入了UMP插件作為RTSP Player，以及Oculus官方的基本VR素材。

最開(kāi)始的時(shí)候，我是打算采用絕對(duì)坐標(biāo)一比一的轉(zhuǎn)發(fā)手柄的動(dòng)作，但是我發(fā)現(xiàn)這樣操作很不方便。首先，Mercury機(jī)器人的臂展大概只相當(dāng)于十幾歲的小孩，如果使用完全絕對(duì)坐標(biāo)1：1控制很容易導(dǎo)致機(jī)械臂撞到關(guān)節(jié)限位。而且手柄的初始位置也是個(gè)問(wèn)題。因此最后我決定采用相對(duì)的控制方式，只有在按住特定按鍵的時(shí)候才允許機(jī)械臂移動(dòng)，這樣做的好處就是：在沒(méi)有按下按鈕的時(shí)候，你可以隨意調(diào)整姿勢(shì)；而在你開(kāi)始運(yùn)動(dòng)的時(shí)候，機(jī)械臂永遠(yuǎn)是以你當(dāng)前點(diǎn)為基準(zhǔn)點(diǎn)進(jìn)行相對(duì)運(yùn)動(dòng)，這樣控制起來(lái)就容易得多了。

圖傳方面最開(kāi)始的做法是，用服務(wù)器轉(zhuǎn)發(fā)MJPG圖片到VR端，然后以texture的方式渲染到屏幕上，這種方式的好處就是實(shí)現(xiàn)簡(jiǎn)單。缺點(diǎn)就太多了，首先就是延遲和CPU負(fù)載的問(wèn)題；如果直接使用服務(wù)器轉(zhuǎn)發(fā)圖片，不但要至少多一次拷貝，還很難調(diào)用Nvidia自帶的編解碼器。而且在VR端也需要時(shí)間進(jìn)行解碼拷貝，整體延遲和CPU負(fù)載都很高。后來(lái)我想到了使用GStreamer+NV加速插件的方案，也就是上面說(shuō)到的，利用了NV硬件加速以后，延遲和負(fù)載都得到了大幅度的改善。

除此之外，在開(kāi)發(fā)過(guò)程中，我也對(duì)Unity3D+Quest 3作為遙操作平臺(tái)有了更深的了解。首先在Unity3D中，幾乎所有的運(yùn)算都是和幀對(duì)齊的，雖然你可以開(kāi)線(xiàn)程，但是游戲引擎給你提供的資源幾乎都是按幀進(jìn)行刷新的。比如我能獲取到的手柄坐標(biāo)，我能獲取到的最大刷新率就是等于游戲幀率。因此在這個(gè)平臺(tái)上，不考慮插值等操作，遙操作控制頻率采樣的上限其實(shí)就是幀率,這個(gè)數(shù)字通常是70-90hz每秒。因此我沒(méi)有采用多線(xiàn)程來(lái)發(fā)送信息，而是使用了Unity3D中最普遍的做法：協(xié)程。使用協(xié)程能夠保證你的操作和幀是對(duì)齊的，能夠避免很多因?yàn)椴煌綄?dǎo)致的奇怪問(wèn)題。

```c#
// One example of coroutine function
IEnumerator sendUpdate()
{
   if (!flag_origin_initialized) yield break;
   Vector3 pos = gameObject.transform.position;
   pos -= origin_pos;
   pos *= 1000;
   
   Vector3 rotation = gameObject.transform.rotation.eulerAngles;
   if (isZero(pos) || isZero(rotation))
       yield break;
   rotation = angleConvert(rotation);
   string content = $"[{pos.x:F2},{pos.y:F2},{pos.z:F2},{rotation.x:F2},{rotation.y:F2},{rotation.z:F2},{System.DateTime.UtcNow.Ticks / TimeSpan.TicksPerMillisecond}]";
   Debug.Log(content);
   using (UnityWebRequest www = UnityWebRequest.Post(updateURL, content, "application/json"))
   {
       www.timeout = 1;
       yield return www.SendWebRequest();
       Debug.Log(www.result);
       if (www.result != UnityWebRequest.Result.Success)
       {
           Debug.Log(www.error);
       }
   }
}

// Use this in main thread (eg. inside Update() to sync with main loop)
StartCoroutine(sendUpdate());

```

服務(wù)器端實(shí)現(xiàn)
服務(wù)器方面我使用的是簡(jiǎn)單可靠的Flask作為框架，因?yàn)槲倚枰膬H僅只是作為RPC用途的控制框架。精簡(jiǎn)的Flask就能可以很好的完成這個(gè)任務(wù)。

在控制系統(tǒng)方面，Unity3D和機(jī)械臂平臺(tái)的對(duì)齊也是一大難點(diǎn)，即如何將VR世界的坐標(biāo)，翻譯為機(jī)械臂能聽(tīng)懂的坐標(biāo)。因?yàn)镸ercury機(jī)器人的手臂，本質(zhì)上是由兩個(gè)Mercury單臂組成的，在單臂自己的視角下，它的坐標(biāo)實(shí)際上是以它自己的底座，也就是胳膊大臂關(guān)節(jié)的位置為原點(diǎn)的，而不是以我們期望的——腰部為原點(diǎn)。因此需要設(shè)計(jì)一套坐標(biāo)轉(zhuǎn)換工具來(lái)實(shí)現(xiàn)從VR世界，到Mercury的基坐標(biāo)系，再到單臂的坐標(biāo)的轉(zhuǎn)換。這個(gè)過(guò)程使用到了大量的線(xiàn)性代數(shù)工具和機(jī)器人學(xué)理論。

比如下面這段代碼實(shí)現(xiàn)了VR坐標(biāo)到基坐標(biāo)系的轉(zhuǎn)換

```python
def vr_to_base(posture):
   position = np.array(posture[:3])
   rotation = np.array(posture[3:])
   matrix = cvt_euler_angle_to_rotation_matrix(rotation)
   T = np.vstack((np.hstack((matrix, position.reshape(3, 1))), np.array([0, 0, 0, 1])))

   TRB = np.array([[0, 0, 1, 0], [-1, 0, 0, 0], [0, 1, 0, 310], [0, 0, 0, 1]])

   Tp = np.dot(TRB, T)

   rotation_matrix = Tp[:3, :3]
   position = Tp[:3, 3]
   rotation = cvt_rotation_matrix_to_euler_angle(rotation_matrix)

   return np.hstack((position, rotation))
```

一個(gè)有趣的問(wèn)題就是左右臂互為鏡像的問(wèn)題，我和負(fù)責(zé)機(jī)器人算法的工程師不得不分別給兩條臂使用不同的固件以兼容使用同一套轉(zhuǎn)換算法。

前文提到，機(jī)械臂期望穩(wěn)定且固定間隔的控制信號(hào)，而實(shí)現(xiàn)固定間隔就需要緩存系統(tǒng)和高精度的計(jì)時(shí)的下發(fā)系統(tǒng)。

首先是緩存問(wèn)題，緩存主要考慮的一個(gè)問(wèn)題就是線(xiàn)程安全。因?yàn)榉?wù)器本身是多線(xiàn)程運(yùn)行的，兩個(gè)網(wǎng)絡(luò)包可能會(huì)同時(shí)到達(dá)，或者網(wǎng)絡(luò)包到達(dá)時(shí)剛好碰上下發(fā)，因此設(shè)計(jì)一個(gè)線(xiàn)程安全的雙端隊(duì)列作為緩存區(qū)是首要任務(wù)。其次就是緩沖區(qū)的大小，下發(fā)時(shí)間的間隔，以及時(shí)間片參數(shù)的問(wèn)題；這些參數(shù)也是下發(fā)系統(tǒng)延遲表現(xiàn)和穩(wěn)定表現(xiàn)的均衡器，這些參數(shù)需要大量的實(shí)驗(yàn)來(lái)調(diào)試，以達(dá)到最佳表現(xiàn)。其中尤為難以平衡的是下發(fā)時(shí)間的間隔和時(shí)間片，因?yàn)榫W(wǎng)絡(luò)延遲是不確定的，機(jī)器處理運(yùn)算也需要時(shí)間，因此實(shí)際需要的時(shí)間是要比單純下發(fā)的間隔要長(zhǎng)的，具體長(zhǎng)多少也是不固定的。因?yàn)檫@一層原因，實(shí)際上參數(shù)的設(shè)置需要更加保守才能保證系統(tǒng)運(yùn)行穩(wěn)定。

Python標(biāo)準(zhǔn)庫(kù)中自帶的time精度并不理想。因此單純靠sleep來(lái)實(shí)現(xiàn)計(jì)時(shí)肯定是不可行的。我目前的做法是單獨(dú)開(kāi)一個(gè)線(xiàn)程，以大量占用CPU為代價(jià)高速輪詢(xún)來(lái)實(shí)現(xiàn)低延遲下發(fā)。實(shí)測(cè)證明這種方式的延遲是要比直接給一個(gè)完整的sleep要低的。

class MyThread(Thread):
   ...

   def run(self) -> None:
       while running:
           if len(self.bucket)!= 0 and time.time_ns() - self.last_time > self.time_slice:
               # do something
               self.last_time = time.time_ns()
           time.sleep(0.00001)

優(yōu)化
在基本框架完成以后，還可以對(duì)下發(fā)點(diǎn)位進(jìn)行簡(jiǎn)單的濾波，以進(jìn)一步去除抖動(dòng)。人手在使用VR的時(shí)候不可能確保完美的停在一個(gè)點(diǎn)，在做直線(xiàn)運(yùn)動(dòng)的時(shí)候也不可能保證是完美的直線(xiàn)。我們可以單獨(dú)對(duì)每個(gè)分量進(jìn)行控制，過(guò)濾掉較低的分量，以實(shí)現(xiàn)更穩(wěn)定地運(yùn)動(dòng)軌跡。

a = final_base_coords[:3]
b = self.last_arm_base_coords[:3]
if self.last_arm_base_coords is not None:
   diff = a - b
final_base_coords[:3] = np.where(abs(diff) > 5, a, b)

另一個(gè)可能的優(yōu)化是對(duì)軌跡進(jìn)行平均化操作，這樣可以使得軌跡更加平滑，但是可能的trade off是要進(jìn)一步增加延遲。以下是一個(gè)基于雙端隊(duì)列的滑動(dòng)窗口的實(shí)現(xiàn)。

class SlidingWindow:
   def __init__(self, maxlen=5) -> None:
       self.lock = Lock()
       self.store = deque(maxlen=maxlen)
       for i in range(maxlen):
           self.store.append(1)
       self.maxsize = maxlen

   def append(self, obj):
       with self.lock:
           self.store.append(obj)

   def mean(self):
       with self.lock:
           return np.mean(np.array(self.store), axis=0)

   def clear(self):
       with self.lock:
           self.store.clear()

   def size(self):
       with self.lock:
           return len(self.store)

機(jī)械臂運(yùn)動(dòng)控制
1. 動(dòng)作捕捉：佩戴VR設(shè)備（如頭盔和手套）進(jìn)行實(shí)際動(dòng)作。這些設(shè)備通過(guò)內(nèi)置傳感器捕捉用戶(hù)的動(dòng)作數(shù)據(jù)（如頭部方向、手部位置和手勢(shì)），并將這些數(shù)據(jù)實(shí)時(shí)傳輸?shù)娇刂葡到y(tǒng)。

2. 虛擬現(xiàn)實(shí)界面：用戶(hù)在VR環(huán)境中可以看到虛擬的機(jī)械臂模型，并通過(guò)VR設(shè)備進(jìn)行操作。用戶(hù)的動(dòng)作被映射到虛擬模型上，實(shí)時(shí)轉(zhuǎn)化為機(jī)械臂的運(yùn)動(dòng)指令。

3. 數(shù)據(jù)處理和傳輸：捕捉到的動(dòng)作數(shù)據(jù)需要通過(guò)軟件處理，轉(zhuǎn)化為機(jī)械臂可以理解的指令。這些指令通常涉及關(guān)節(jié)角度、速度或位置信息的計(jì)算。然后，這些指令通過(guò)網(wǎng)絡(luò)發(fā)送給機(jī)械臂控制器。

4. 機(jī)械臂執(zhí)行：機(jī)械臂接收到指令后，通過(guò)其內(nèi)部控制系統(tǒng)執(zhí)行相應(yīng)的動(dòng)作。這可能包括移動(dòng)到特定的位置、按照特定的路徑移動(dòng)或執(zhí)行復(fù)雜的手部動(dòng)作。

5. 反饋機(jī)制：為了提高操作的精確性和用戶(hù)體驗(yàn)，系統(tǒng)可能包括反饋機(jī)制，如觸覺(jué)反饋或視覺(jué)反饋，將機(jī)械臂的狀態(tài)實(shí)時(shí)反映到VR環(huán)境中，以便用戶(hù)調(diào)整其操作。

案例展示
https://youtu.be/mvpHFXcadNk

Summary
VR技術(shù)的進(jìn)步正在打破物理空間的限制，使人們能夠在虛擬環(huán)境中實(shí)現(xiàn)復(fù)雜的操作和互動(dòng)，這對(duì)于遠(yuǎn)程控制、教育、醫(yī)療等領(lǐng)域具有重大意義。未來(lái)，隨著VR技術(shù)的持續(xù)發(fā)展，我們可以預(yù)見(jiàn)更多創(chuàng)新的應(yīng)用出現(xiàn)。例如，VR在模擬復(fù)雜手術(shù)、遠(yuǎn)程教育、災(zāi)難響應(yīng)訓(xùn)練等領(lǐng)域的應(yīng)用將更加廣泛。

如果是你，你會(huì)怎樣來(lái)使用Mercury X1呢？

大象機(jī)器人今天正式發(fā)布Mercury系列機(jī)器人，目前水星系列機(jī)器人共有三款產(chǎn)品，Mercury-A1七軸協(xié)作機(jī)械臂,Mercury-B1 半人形雙臂機(jī)器人,Mercury-X1通用輪式人形機(jī)器人。三款產(chǎn)品的工業(yè)設(shè)計(jì)皆由瑞典團(tuán)隊(duì)精心設(shè)計(jì)而成，集成七大機(jī)器人核心算法，多種使用與開(kāi)發(fā)方式。

審核編輯 黃宇