1. 為什么選擇Pico手柄來做Mujoco遙操作先說項(xiàng)目背景。我最近在做一套基于Mujoco的機(jī)器人仿真遙操作系統(tǒng)目標(biāo)很直接戴上Pico VR頭顯雙手各拿一支手柄在現(xiàn)實(shí)空間中自然移動手臂仿真環(huán)境里的機(jī)械臂末端就跟著我的手走。聽起來像是一個(gè)標(biāo)準(zhǔn)玩法但真正跑起來之后我才意識到整個(gè)鏈路里最花時(shí)間的不是環(huán)境搭建、不是手柄協(xié)議而是坐標(biāo)系轉(zhuǎn)換。先聊聊方案選型。目前做仿真遙操作主流輸入設(shè)備無非這么幾類輸入設(shè)備自由度成本上手難度適用場景鍵盤 鼠標(biāo)低極低低快速調(diào)試、離散點(diǎn)位控制SpaceMouse 3D鼠標(biāo)中中等中連續(xù)軌跡拖拽、示教數(shù)據(jù)手套高很高高靈巧手操作、精細(xì)抓取VR手柄Pico/Quest高較低中全身位姿映射、沉浸式遙操作我最終選了Pico手柄理由其實(shí)很樸素它自帶完整的6DoF六自由度追蹤位置三自由度、姿態(tài)三自由度一套拿到手就能用內(nèi)部有IMU和光學(xué)追蹤融合幾十毫秒內(nèi)能給出穩(wěn)定的手柄位姿不需要額外搭動捕系統(tǒng)。對比SpaceMouse那種搖桿式的輸入VR手柄最大的優(yōu)勢是你不需要學(xué)習(xí)映射邏輯——手怎么動機(jī)械臂末端就怎么動操作直覺幾乎為零門檻。對比數(shù)據(jù)手套成本又友好太多Pico Neo 3或Pico 4的一對手柄在閑魚或促銷時(shí)幾百塊就能收到而一套商用數(shù)據(jù)手套動輒上萬。實(shí)測下來對于末端位姿連續(xù)控制這個(gè)需求VR手柄是性價(jià)比最高的方案。順帶提一句我為什么沒有用WebXR或者瀏覽器方案。雖然Pico瀏覽器支持WebXR串流延遲和幀率穩(wěn)定性在復(fù)雜場景下不夠可靠遙操作最怕的就是手柄位姿抖動或者延遲突刺一旦出現(xiàn)機(jī)械臂在仿真里就是一頓亂甩。所以我選擇了更穩(wěn)的PC端鏈路Pico串流 - OpenVR/OpenXR運(yùn)行時(shí) - Python - Mujoco。整套架構(gòu)從硬件到仿真每一環(huán)都能拿到確定的位姿數(shù)據(jù)和控制頻率排錯也方便。整個(gè)項(xiàng)目的技術(shù)鏈路如下Pico手柄物理位姿 ↓ 無線串流 PC端OpenVR運(yùn)行時(shí)SteamVR ↓ Python openvr庫 6DoF位姿矩陣OpenVR世界坐標(biāo)系Y-up ↓ 坐標(biāo)轉(zhuǎn)換Y-up → Z-up縮放偏置 Mujoco世界坐標(biāo)系下的目標(biāo)末端位姿 ↓ 逆運(yùn)動學(xué)求解 機(jī)械臂各關(guān)節(jié)角目標(biāo)值 ↓ Mujoco物理步進(jìn) 仿真機(jī)械臂末端跟隨手柄運(yùn)動這篇文章的重點(diǎn)放在坐標(biāo)轉(zhuǎn)換上因?yàn)檫@是我實(shí)際花費(fèi)時(shí)間最多、也最容易被后續(xù)項(xiàng)目復(fù)用的一段經(jīng)驗(yàn)。但為了讓整個(gè)流程完整可跑我會把環(huán)境搭建、手柄數(shù)據(jù)讀取、控制環(huán)代碼一并講清楚。2. Mujoco仿真環(huán)境搭建從空環(huán)境到可動機(jī)械臂2.1 Windows 11上安裝Mujoco的完整步驟Mujoco目前已經(jīng)更新到3.x版本Python包安裝非常簡單本質(zhì)就是一條pip命令pip install mujoco它會自動安裝預(yù)編譯的wheel包里面已經(jīng)帶了物理引擎本體和離屏渲染器不需要自己編譯C代碼。但很多人在這一步就卡住了因?yàn)榘惭b完跑一個(gè)簡單demo會報(bào)glfw相關(guān)錯誤或者窗口閃退。原因通常是Windows 11缺少Visual C運(yùn)行庫或者顯卡驅(qū)動太老。建議先把這兩件套補(bǔ)上安裝Microsoft Visual C Redistributablex64版本這是Mujoco在Windows上跑起來的硬性依賴。去NVIDIA或AMD官網(wǎng)更新顯卡驅(qū)動Mujoco 3.x的渲染后端對OpenGL版本有要求老驅(qū)動會直接報(bào)錯。裝完后跑一下官方自帶的demo驗(yàn)證環(huán)境import mujoco import mujoco.viewer xml mujoco modeltest_scene worldbody light pos0 0 2 directionaltrue/ geom typeplane size1 1 0.1/ body pos0 0 0.1 freejoint/ geom typesphere size0.1 rgba1 0 0 1/ /body /worldbody /mujoco model mujoco.MjModel.from_xml_string(xml) data mujoco.MjData(model) with mujoco.viewer.launch_passive(model, data) as viewer: for _ in range(1000): mujoco.mj_step(model, data) viewer.sync()如果能看到一個(gè)紅色小球自由落體并停在平面上環(huán)境就算通了。2.2 幾個(gè)高頻安裝問題的排查鏈路結(jié)合熱搜詞里大家都在搜的Mujoco安裝常見問題我把幾個(gè)真實(shí)出現(xiàn)過的坑和排查步驟整理出來第一個(gè)坑ImportError: DLL load failed while importing mujoco這個(gè)問題幾乎都出在mujoco.dll找不到依賴庫上。排查順序打開cmd輸入where python確認(rèn)當(dāng)前Python環(huán)境。確認(rèn)pip list里有mujoco且版本在2.3以上。運(yùn)行python -c import mujoco看具體報(bào)錯。如果依賴缺失重裝VC運(yùn)行庫重啟終端再試。第二個(gè)坑渲染窗口初始化失敗報(bào)GLFW error這個(gè)大概率是環(huán)境變量問題。Mujoco在Windows下會嘗試加載GLFW和OpenGL如果系統(tǒng)里裝了多個(gè)OpenGL驅(qū)動或者缺少M(fèi)ESA環(huán)境就會初始化失敗。我在Windows 11上遇到過一次最后是卸載了機(jī)器上殘留的舊版OpenGL驅(qū)動再重裝顯卡驅(qū)動解決的。如果你用的是虛擬機(jī)或遠(yuǎn)程桌面建議直接放棄GUI viewer改用mujoco.Renderer做離屏渲染再把圖像傳回來顯示。第三個(gè)坑加載自己訓(xùn)練的MJCF模型時(shí)報(bào)Unknown function in ...這通常是Mujoco版本升級后XML里的舊字段不再被支持。比如某些網(wǎng)頁上的教程還在用default class...而新版語法已經(jīng)變了。遇到這種問題不要慌直接去官方mujoco_menagerie倉庫找對應(yīng)機(jī)器人的最新MJCF文件比自己手寫XML省事得多。我用的是Franka Panda機(jī)械臂直接從官方模型庫里拉的模型。2.3 MJCF模型的加載與基本控制接口我這邊用的機(jī)械臂模型是Franka PandaMujoco官方mujoco_menagerie里就有。加載方式import mujoco model mujoco.MjModel.from_xml_path(path/to/franka_emika_panda/scene.xml) data mujoco.MjData(model) # 查看關(guān)節(jié)列表 for i in range(model.nu): print(model.actuator_names[i])Mujoco里的控制邏輯很簡單data.ctrl是一個(gè)數(shù)組每個(gè)元素對應(yīng)一個(gè)執(zhí)行器關(guān)節(jié)電機(jī)的目標(biāo)值。你設(shè)置了ctrl然后調(diào)用mujoco.mj_step(model, data)仿真就推進(jìn)一個(gè)物理步。后面做遙操作本質(zhì)上就是不斷把手柄解算出的關(guān)節(jié)目標(biāo)灌進(jìn)data.ctrl然后步進(jìn)仿真。需要記住的一個(gè)點(diǎn)Mujoco里運(yùn)動學(xué)計(jì)算依賴data.qpos廣義坐標(biāo)和data.qvel廣義速度而控制接口是data.ctrl。你做逆運(yùn)動學(xué)時(shí)通常是給一個(gè)期望末端位姿通過IK求出期望關(guān)節(jié)角然后設(shè)置到ctrl上。如果機(jī)械臂沒有PD控制器直接設(shè)置ctrl會導(dǎo)致關(guān)節(jié)位置是開環(huán)的機(jī)械臂不會乖乖停在目標(biāo)位置。所以MJCF模型里自帶的motor執(zhí)行器通常需要配合一個(gè)簡單的關(guān)節(jié)PD控制器或者用官方模型里已經(jīng)配好的position執(zhí)行器。3. Pico手柄數(shù)據(jù)讀取從硬件到Python的完整鏈路3.1 通信方案為什么走OpenVR而不是走Pico原生SDKPico手柄要拿到PC上現(xiàn)在主流有幾條路Pico自家SDK、OpenXR、OpenVRSteamVR。我最后用的是Pico串流助手 SteamVR Python openvr庫的組合。原因有幾個(gè)Pico串流助手是官方工具穩(wěn)定性和壓縮延遲控制得不錯。手柄位姿的精度很大程度依賴追蹤算法Pico的inside-out追蹤在手部快速移動時(shí)偶爾會有抖動但串流鏈路本身不會丟數(shù)據(jù)。OpenVR是PC端VR生態(tài)的事實(shí)標(biāo)準(zhǔn)。不管手柄是Pico還是Quest插上SteamVR都能被統(tǒng)一抽象為左右控制器設(shè)備接口固定Python有現(xiàn)成的openvr綁定不用自己寫底層USB協(xié)議。OpenXR雖然更現(xiàn)代但Python綁定少調(diào)試工具也少。如果只是做研究驗(yàn)證OpenVR這條路更省事。3.2 通過openvr讀取手柄6DoF位姿安裝依賴pip install openvr讀取手柄位姿的完整邏輯如下import openvr import numpy as np # 初始化OpenVR openvr.VR_Init() poses None def get_controller_pose(): # 獲取所有設(shè)備的位姿 poses, _ openvr.VRCompositor().waitGetPoses() left_pose None right_pose None for device_index in range(openvr.k_unMaxTrackedDeviceCount): device_class openvr.VRSystem().getTrackedDeviceClass(device_index) if device_class openvr.TrackedDeviceClass_Controller: # 判斷左右手 role openvr.VRSystem().getControllerRoleForTrackedDeviceIndex(device_index) pose poses[device_index] # 位姿矩陣是3x4行主序 mat np.array(pose.mDeviceToAbsoluteTracking).reshape(3, 4) if role openvr.TrackedControllerRole_LeftHand: left_pose mat elif role openvr.TrackedControllerRole_RightHand: right_pose mat return left_pose, right_pose這里拿到的mat是3x4矩陣前三列是旋轉(zhuǎn)矩陣相對于OpenVR的世界坐標(biāo)系最后一列是位置。注意OpenVR的世界坐標(biāo)系是Y-up的右手坐標(biāo)系這一點(diǎn)非常關(guān)鍵后面坐標(biāo)轉(zhuǎn)換全靠它。3.3 數(shù)據(jù)形態(tài)與頻率說明OpenVR的數(shù)據(jù)刷新率通常跟頭顯的追蹤頻率一致Pico在串流SteamVR模式下一般是60Hz或者90Hz。對于遙操作來說這個(gè)頻率足夠了。但要注意waitGetPoses是阻塞式的如果串流畫面掉幀這個(gè)函數(shù)也會跟著掉幀。所以我在代碼里用的是getDeviceToAbsoluteTrackingPose配合一個(gè)獨(dú)立線程來讀數(shù)據(jù)避免畫面掉幀影響控制。還有一個(gè)容易忽略的點(diǎn)剛拿到手柄位姿時(shí)不要直接當(dāng)世界坐標(biāo)用。雙手拿手柄站在不同位置手柄位姿差異很大。遙操作時(shí)通常要定義一個(gè)初始零點(diǎn)——比如按下扳機(jī)鍵的瞬間記錄當(dāng)前手柄位姿作為參考基準(zhǔn)之后的手柄相對運(yùn)動都相對于這個(gè)零點(diǎn)來計(jì)算。這樣操作者不管站在哪里都能以一個(gè)舒服的姿勢開始控制。reference_pose None def on_trigger_pressed(): global reference_pose left, right get_controller_pose() reference_pose left # 比如以左手為基準(zhǔn)這個(gè)相對位姿的思路后面會反復(fù)用到也是避免操作者手酸的關(guān)鍵。4. 坐標(biāo)轉(zhuǎn)換這個(gè)項(xiàng)目真正的攔路虎4.1 為什么非轉(zhuǎn)不可回到標(biāo)題里的坐標(biāo)轉(zhuǎn)換。很多人覺得Mujoco環(huán)境搭好、手柄數(shù)據(jù)能讀到了接下來不就是把手柄位置賦給機(jī)械臂末端嗎其實(shí)根本不是。原因有三層坐標(biāo)系軸方向不同。OpenVR世界坐標(biāo)系是Y-upY軸朝上而Mujoco里常見的機(jī)械臂模型約定Z軸朝上機(jī)器人學(xué)慣例也貼合重力方向。如果直接把OpenVR的位置塞給Mujoco你會發(fā)現(xiàn)手柄往上抬機(jī)械臂往屏幕里走完全錯亂。物理尺寸映射需要縮放?，F(xiàn)實(shí)中你手臂的移動范圍可能只有0.5米左右但仿真里的機(jī)械臂工作空間可能是1米、2米。你需要一個(gè)比例因子把真實(shí)操作空間映射到仿真工作空間。初始對齊問題。手柄在世界坐標(biāo)系里的原點(diǎn)和Mujoco世界坐標(biāo)系里的機(jī)械臂基座原點(diǎn)并不重合。必須定義一個(gè)偏移讓手柄移動到某個(gè)區(qū)域時(shí)機(jī)械臂末端正好在初始位置。4.2 Y-up到Z-up的旋轉(zhuǎn)映射這是坐標(biāo)轉(zhuǎn)換的第一步。我需要一個(gè)旋轉(zhuǎn)矩陣把OpenVR的Y-up坐標(biāo)變成Mujoco的Z-up坐標(biāo)。繞X軸旋轉(zhuǎn)-90度即可實(shí)現(xiàn)Y-up到Z-up的映射。旋轉(zhuǎn)矩陣R [1 0 0] [0 0 1] [0 -1 0]驗(yàn)證一下OpenVR坐標(biāo)系中的Y軸單位向量(0, 1, 0)左乘R得到(0, 0, -1)。這表示OpenVR中的向上在Mujoco中變成了-Z方向等等不對應(yīng)該驗(yàn)證實(shí)際上OpenVR的向上是YMujoco的向上是Z。繞X軸旋轉(zhuǎn)-90度Y軸單位向量 (0,1,0) → 旋轉(zhuǎn)-90°繞X軸Y → -Z還是 Z繞X軸旋轉(zhuǎn)角度θ標(biāo)準(zhǔn)矩陣[1 0 0] [0 cosθ -sinθ] [0 sinθ cosθ]θ -90°時(shí)cos 0, sin -1[1 0 0] [0 0 1] [0 -1 0](0,1,0) → (0, 0, -1)。所以O(shè)penVR的Y映射到Mujoco的-Z。這不對我想要的應(yīng)該是Y→Z。那應(yīng)該用繞X軸旋轉(zhuǎn)90度[1 0 0] [0 0 -1] [0 1 0](0,1,0) → (0, 0, 1)即Y → Z。好這個(gè)才對。所以正確的旋轉(zhuǎn)矩陣應(yīng)該是繞X軸旋轉(zhuǎn)**90度**R [1 0 0] [0 0 -1] [0 1 0]同時(shí)-Z軸會映射到Y(jié)。通常OpenVR的-Z是朝向屏幕內(nèi)側(cè)或者說用戶前方映射到Mujoco的Y這通常沒什么問題因?yàn)闄C(jī)器人正面朝向可以人為約定。四元數(shù)的轉(zhuǎn)換也可以用同樣的思路但更穩(wěn)妥的做法是把OpenVR給的旋轉(zhuǎn)矩陣左乘R得到新的旋轉(zhuǎn)矩陣再轉(zhuǎn)成四元數(shù)。不要試圖在四元數(shù)層面直接變換容易在wxyz/xyzw的順序上翻車。def yup_to_zup(mat_3x4): R_yup mat_3x4[:, :3] # 3x3旋轉(zhuǎn)矩陣 t_yup mat_3x4[:, 3] # 位置向量 R_convert np.array([ [1, 0, 0], [0, 0, -1], [0, 1, 0] ]) R_zup R_convert R_yup t_zup R_convert t_yup mat_zup np.hstack([R_zup, t_zup.reshape(-1, 1)]) return mat_zup這一步做完手柄的手勢方向已經(jīng)和Mujoco對齊了但位置還是OpenVR原點(diǎn)下的絕對位置需要繼續(xù)平移和縮放。4.3 位置偏移與操作空間縮放接下來處理位置。假設(shè)操作者在初始化時(shí)按下扳機(jī)記錄一個(gè)參考位姿mat_ref_zup。之后每一幀的手柄位姿mat_cur_zup計(jì)算相對位移delta_pos mat_cur_zup[:, 3] - mat_ref_zup[:, 3]這個(gè)delta_pos就是操作者手部相對于初始位置的空間位移。然后乘一個(gè)縮放因子scale 0.6 # 根據(jù)實(shí)際工作空間調(diào)整 target_pos base_pos scale * delta_pos其中base_pos是Mujoco中機(jī)械臂末端的期望初始位置。你可以根據(jù)機(jī)械臂的零位姿態(tài)算出來比如Franka Panda的末端默認(rèn)在基座前方約0.5米處那么base_pos就設(shè)成(0.3, 0.0, 0.5)之類的值?？s放因子的選擇有講究。值太大手柄微動一下機(jī)械臂就飛出工作空間值太小手部大幅移動機(jī)械臂才挪一點(diǎn)點(diǎn)操作很遲鈍。我這邊實(shí)測的經(jīng)驗(yàn)是對于Franka Panda這種臂長1米左右的操作臂縮放0.5到0.8比較合適。對于小型的桌面機(jī)械臂工作空間20厘米量級縮放0.1到0.2。如果希望精細(xì)操作可以在手柄上某個(gè)觸摸板上加一個(gè)變速檔按住觸摸板時(shí)縮放系數(shù)降到原來的1/5做微調(diào)。4.4 四元數(shù)順序的坑Mujoco的wxyz與常見庫的xyzw這是整個(gè)坐標(biāo)轉(zhuǎn)換過程中最容易陰溝翻船的地方。Mujoco內(nèi)部使用四元數(shù)表示姿態(tài)時(shí)順序是w, x, y, z也就是實(shí)部在前。而很多庫比如scipy的Rotation.from_quat默認(rèn)接受x, y, z, w。如果你從OpenVR拿到的旋轉(zhuǎn)矩陣轉(zhuǎn)四元數(shù)默認(rèn)轉(zhuǎn)了xyzw順序直接塞給Mujoco的qpos機(jī)械臂的姿態(tài)會完全亂掉。我在寫代碼時(shí)統(tǒng)一封裝了一個(gè)轉(zhuǎn)換函數(shù)from scipy.spatial.transform import Rotation as R def mat_to_mujoco_quat(mat_3x4): # 輸入: 經(jīng)過yup_to_zup轉(zhuǎn)換后的3x4矩陣 rot_matrix mat_3x4[:, :3] quat_xyzw R.from_matrix(rot_matrix).as_quat() # 拿到xyzw quat_wxyz np.roll(quat_xyzw, 1) # 移到wxyz return quat_wxyz這個(gè)坑我必須強(qiáng)調(diào)凡是涉及四元數(shù)和Mujoco交互的統(tǒng)一走這一個(gè)函數(shù)別在調(diào)用處再自己轉(zhuǎn)一次。我剛開始時(shí)就是沒注意在IK解算里用scipy轉(zhuǎn)了一次又在賦值給qpos前用Mujoco的內(nèi)部函數(shù)轉(zhuǎn)了一次兩次轉(zhuǎn)換互相抵消姿態(tài)穩(wěn)定地偏了180度排查了整整一個(gè)晚上。5. 從手柄末端位姿到機(jī)械臂關(guān)節(jié)角完整控制環(huán)5.1 控制頻率與數(shù)據(jù)流設(shè)計(jì)手柄數(shù)據(jù)是60~90HzMujoco物理步進(jìn)可以跑得非常快我這里設(shè)定的是200Hz步進(jìn)。兩者頻率不一致不能每讀一次手柄就步進(jìn)一步也不能每個(gè)控制周期都去讀手柄否則控制會抖。我的做法是開兩個(gè)線程讀取線程以O(shè)penVR的頻率不斷更新當(dāng)前目標(biāo)末端位姿這個(gè)共享變量。控制線程以200Hz頻率運(yùn)行從共享變量里拿最新的目標(biāo)位姿做IK設(shè)置關(guān)節(jié)目標(biāo)步進(jìn)仿真。這兩個(gè)線程之間用threading.Lock保護(hù)共享變量避免讀到半寫狀態(tài)。5.2 末端位姿平滑低通濾波是必須的手柄追蹤在快速移動時(shí)會有微小的抖動直接用于控制會讓機(jī)械臂末端出現(xiàn)高頻顫振。解決方案是一個(gè)簡單的一階低通濾波alpha 0.3 smoothed_pos alpha * target_pos (1 - alpha) * smoothed_pos姿態(tài)也可以用球面線性插值slerp做平滑但我在實(shí)際項(xiàng)目中只對位置做了低通姿態(tài)平滑用了更簡單的nlerp歸一化線性插值效果夠用計(jì)算還便宜。注意alpha值不要太小否則滯后感明顯操作者會覺得跟不上手。我實(shí)測0.2到0.4是比較舒服的區(qū)間。5.3 逆運(yùn)動學(xué)求解阻尼最小二乘法從末端位姿求關(guān)節(jié)角最常用的方法是基于雅可比矩陣的數(shù)值迭代。Mujoco自帶mj_jac接口可以拿到雅可比矩陣所以我直接在控制循環(huán)里做阻尼最小二乘IKimport mujoco def ik_solve(model, data, target_pos, target_quat_wxyz, init_q, max_iter30): # 把當(dāng)前關(guān)節(jié)角作為初始猜測 data.qpos[:7] init_q mujoco.mj_forward(model, data) # 找到末端body的id ee_body_id mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, ee_link) for _ in range(max_iter): # 當(dāng)前末端位姿 cur_pos data.body(ee_body_id).xpos cur_quat data.body(ee_body_id).xquat # 位置誤差 pos_err target_pos - cur_pos # 姿態(tài)誤差四元數(shù)轉(zhuǎn)旋轉(zhuǎn)向量 err_quat quat_error(cur_quat, target_quat_wxyz) rot_err quat_to_rotvec(err_quat) err np.hstack([pos_err, rot_err]) # 6維誤差向量 if np.linalg.norm(err) 1e-4: break # 雅可比矩陣3xN位置 3xN姿態(tài) jacp np.zeros((3, model.nv)) jacr np.zeros((3, model.nv)) mujoco.mj_jac(model, data, jacp, jacr, target_pos, ee_body_id) J np.vstack([jacp[:3, :7], jacr[:3, :7]]) # 阻尼最小二乘 lambda_reg 0.05 dq J.T np.linalg.solve(J J.T lambda_reg**2 * np.eye(6), err) data.qpos[:7] dq # 關(guān)節(jié)限位 data.qpos[:7] np.clip(data.qpos[:7], model.jnt_range[:, 0], model.jnt_range[:, 1]) mujoco.mj_forward(model, data) return data.qpos[:7].copy()這個(gè)IK不是全局最優(yōu)解但對連續(xù)控制場景足夠。每次迭代時(shí)以上一幀的關(guān)節(jié)角為初始值收斂非?？焱ǔ?到5次迭代就到達(dá)目標(biāo)精度。阻尼系數(shù)lambda_reg不能去掉否則在奇異位型附近雅可比矩陣奇異解會爆炸。5.4 完整控制循環(huán)的代碼骨架把所有環(huán)節(jié)串起來主控循環(huán)如下def control_loop(): while running: with lock: target_pos smoothed_target_pos target_quat smoothed_target_quat # IK求解 q_target ik_solve(model, data, target_pos, target_quat, current_q) # 或者直接用關(guān)節(jié)PD設(shè)置目標(biāo)位置 data.ctrl[:7] q_target # 步進(jìn)仿真 mujoco.mj_step(model, data) # 更新平滑后的目標(biāo) update_smoothed_target()有一點(diǎn)需要提醒data.ctrl設(shè)置的是關(guān)節(jié)電機(jī)的目標(biāo)值如果你的MJCF模型里是純力矩電機(jī)這種開環(huán)控制會有靜態(tài)誤差。最省事的做法是在MJCF里給每個(gè)關(guān)節(jié)配一個(gè)position執(zhí)行器Mujoco內(nèi)置的position actuator會自動做關(guān)節(jié)PD控制量直接就是期望關(guān)節(jié)角省去自己調(diào)PD參數(shù)的麻煩。6. 實(shí)測效果與高頻踩坑記錄6.1 坐標(biāo)轉(zhuǎn)換沒做好時(shí)的典型病征這部分是我最想分享的因?yàn)椴瓤訒r(shí)的現(xiàn)象和最終原因之間往往隔著一層窗戶紙。我把幾個(gè)典型表現(xiàn)列出來如果你也在調(diào)同類系統(tǒng)可以對照排查病征一手柄往右動機(jī)械臂往左動。這說明坐標(biāo)系發(fā)生了鏡像通常是繞某個(gè)軸的旋轉(zhuǎn)方向反了。檢查你自己的轉(zhuǎn)換矩陣看看是不是把繞X軸的90度和-90度搞反了。病征二手柄往上抬機(jī)械臂沿著水平方向亂飛。這是典型的Y-up/Z-up沒轉(zhuǎn)換OpenVR的Y軸位移被當(dāng)成了Mujoco的X或Z軸位移。我之前第一次跑起來就是這個(gè)現(xiàn)象整個(gè)機(jī)械臂像喝醉了一樣在水平面上亂竄。病征三機(jī)械臂末端位置對但姿態(tài)完全是擰的。姿態(tài)問題優(yōu)先檢查四元數(shù)順序。Mujoco要wxyzscipy給xyzw差一個(gè)np.roll就天翻地覆。另外檢查旋轉(zhuǎn)矩陣左乘的順序是R_convert R_hand還是R_hand R_convert這個(gè)順序錯了姿態(tài)同樣會擰。病征四一切正常但機(jī)械臂運(yùn)動有可感知的延遲和拖尾感。這種多半是平滑系數(shù)設(shè)得太小低通濾波滯后嚴(yán)重。把a(bǔ)lpha調(diào)大到0.3以上再看看是不是IK迭代次數(shù)太少導(dǎo)致每幀只能走一部分。還有一種可能是控制線程頻率太低試著手柄讀取線程頻率對齊。6.2 幾個(gè)容易被忽略的細(xì)節(jié)手柄丟失追蹤的容錯處理。Pico手柄快速甩動或者被身體擋住時(shí)追蹤會瞬間丟失OpenVR會返回上一次有效的位姿或者一個(gè)無效的pose。如果不做處理機(jī)械臂會突然停在原地然后等手柄恢復(fù)后猛跳一下。我的做法是拿到一幀位姿后檢查旋轉(zhuǎn)矩陣是否包含NaN以及位置是否發(fā)生突變位移超過5厘米就認(rèn)為是異常幀異常幀直接丟棄用上一幀值頂替。初始參考位姿的坐標(biāo)系基準(zhǔn)。記錄初始化基準(zhǔn)時(shí)建議讓操作者把雙手放在一個(gè)舒適的自然位置然后按下扳機(jī)。如果操作者身高不同、站位不同都要重新校準(zhǔn)。我寫了個(gè)簡單邏輯每次按下手柄的A鍵都重新記錄參考位姿方便隨時(shí)重新對齊。Mujoco的mj_forward和mj_step混用問題。在IK循環(huán)里必須調(diào)用mj_forward而不是mj_step因?yàn)閙j_step會推進(jìn)動力學(xué)并修改速度不適合作為純運(yùn)動學(xué)校準(zhǔn)。在主控制循環(huán)里才用mj_step。如果你把IK里的mj_forward換成了mj_step會看到機(jī)械臂抖得非常厲害因?yàn)槊繋荚诟淖兯俣榷皇俏恢?。渲染線程和控制線程的同步。如果用了mujoco.viewerviewer的sync()頻率必須配合渲染幀率不要每個(gè)控制循環(huán)都sync一次否則窗口會變成PPT。在實(shí)際項(xiàng)目中我讓viewer在獨(dú)立線程里以30Hz左右同步控制線程不關(guān)心viewer是否卡頓。6.3 性能與實(shí)際操作體驗(yàn)整個(gè)鏈路跑通后我測試了勻速拖拽和快速抓取兩種典型操作。平穩(wěn)拖拽時(shí)機(jī)械臂末端和手柄目標(biāo)位置的位置誤差大概在毫米級主要來自IK迭代精度姿態(tài)誤差在1度以內(nèi)操作手感是指哪打哪?？焖偎訒r(shí)會有輕微的超調(diào)和回落這是低通濾波本身的特性但整體可控。一個(gè)意外的收獲是這套方案不僅可以控制單臂我后來擴(kuò)展成了雙臂遙操作——左手手柄控制左臂右手手柄控制右臂坐標(biāo)轉(zhuǎn)換邏輯完全復(fù)用。只要把兩個(gè)手柄的位置分別映射到兩條機(jī)械臂的期望末端位姿即可。如果你也需要做雙臂協(xié)作類的仿真驗(yàn)證這個(gè)擴(kuò)展路徑幾乎沒有額外成本。6.4 再分享一個(gè)調(diào)試小技巧最后聊一個(gè)很實(shí)用的調(diào)試方法。當(dāng)你的機(jī)械臂行為完全不符合預(yù)期時(shí)不要先在Mujoco里看結(jié)果而是把目標(biāo)末端位姿和IK輸出的關(guān)節(jié)角打印出來再用Mujoco的官方查看器單獨(dú)回放。這樣可以快速定位問題出在坐標(biāo)轉(zhuǎn)換還是IK。我寫了一個(gè)簡單的數(shù)據(jù)記錄模塊把每一幀的目標(biāo)位置、目標(biāo)四元數(shù)、IK結(jié)果關(guān)節(jié)角都寫入CSV之后用腳本離線分析。很多時(shí)候機(jī)械臂發(fā)瘋的原因是手柄數(shù)據(jù)本身就包含了異常跳變而不是你的控制代碼出了問題。這套仿真遙操作 坐標(biāo)轉(zhuǎn)換的方案從實(shí)際效果來看已經(jīng)很成熟了。如果你要復(fù)現(xiàn)這個(gè)項(xiàng)目我建議把重點(diǎn)放在坐標(biāo)轉(zhuǎn)換和IK的調(diào)試上這兩個(gè)環(huán)節(jié)一旦打通整個(gè)系統(tǒng)就像打通了任督二脈剩下的就是按需求調(diào)整參數(shù)。