人形机器人之VR遥操作
好久不记录了，上次记录我还是个萌新，不知不觉从事腿足式机器人控制方向也有两年多了，应公司需求，成功从四足干到人形，从传统控制干到强化学习，这段时间一直在提升自己，前段时间搞了一阵子的AMP,现在人形的locomotion几乎差不多了，公司项目需求往作业方面靠，那我就拿VR遥操练手吧。
前言
一个项目从零开始，作为一个合格的开发人员，必须一人一项目，那就开干吧。从各种调研，评估各个开源项目的可复现性，可落地性，写出详细的技术路线，最终落地的具体的执行步骤。这些完成后，就成功了一大半了。
由于之前没有接触过这方面，在一顿调研之后，其实可用的框架屈指可数，human-arm-retarget、宇树的avp_teleoperate、Open-television等，等等，宇树的avp_teleoperate官方文档不就是妥妥的明确的执行步骤，在一顿研究后，发现把这套框架改成自己的机器人并不难，落地指日可待。由于我做这个项目的时候是七月份，现在写这个时候已经从avp_teleoperate升级成了xr_teleoperate,我的项目还是基于avp_teleoperate去做的。
一、VR设备选型及环境配置
三种可用设备：Apple Vision Pro、PICO 4 Ultra Enterprise 或 Meta Quest 3，官方教程是按照Apple Vision Pro去写的，但Apple Vision Pro成本比较高，最终决定还是先用PICO 4 Ultra Enterprise把功能做出来。配置过程与官方的几乎一样。
在自己的笔记本上：
1、创建conda环境
conda create -n tv python=3.8
conda activate tv
conda install pinocchio -c conda-forge
pip install meshcat2、使用 git 克隆本项目至本地后，进入到项目路径下，安装基本功能库：
cd ~
git clone https://github.com/unitreerobotics/avp_teleoperate.git
cd avp_teleoperate
pip install -r requirements.txt3、安装mkcert
安装brew
sudo apt-get install build-essential procps curl file git
cd ~
/bin/bash -c "$(curl -fsSL https://raw.githubusercontent.com/Homebrew/install/HEAD/install.sh)"
当要求输入回车键时，请按下回车。然后继续执行：
echo 'eval "$(/home/linuxbrew/.linuxbrew/bin/brew shellenv)"' >> $HOME/.bash_profile
配置环境变量
eval "$(/home/linuxbrew/.linuxbrew/bin/brew shellenv)"
使用 brew 安装mkcert
brew install mkcert4、生成证书
查看本机(你自己的办公电脑) IP 地址
(tv) user@user:~/avp_teleoperate$ ifconfig | grep inet
假设本机 IP 地址是192.168.100.133
创建证书
mkcert -install && mkcert -cert-file cert.pem -key-file key.pem 192.168.123.2 localhost 127.0.0.1
把生成的cert.pem 和 key.pem 两个文件拷贝到项目的 teleop 目录下：
cp cert.pem key.pem ~/avp_teleoperate/teleop/
系统防火墙设置
sudo ufw allow 8012
在 Apple Vision Pro 中安装证书
mkcert -CAROOT二、修改avp_teleoperate
1、代码目录
avp_teleoperate/
│
├── act 存放模仿学习策略相关文件
│
├── assets 存放机器人 URDF 相关文件
│
├── scripts 存放一些工具类文件
│
├── teleop 存放遥操作主要文件
│ │
│ ├── image_server/ 图像推流服务端与客户端相关代码
│ │ |
│ │ ├── image_client.py 客户端（仅用于测试 Unitree H1_2 上的图像推流服务是否正常）
│ │ |
│ │ ├── image_server.py 服务端（捕获相机的图像并通过网络对外发送，该代码在 Unitree H1_2 上运行）
│ │
│ ├── robot_control/ 存放IK解算、手臂控制相关文件
│ │ |
│ │ ├── robot_arm_ik.py 双臂IK解算代码
│ │ |
│ │ ├── robot_arm.py 双臂控制代码
│ │ |
│ │ ├── robot_hand.py 机械手控制代码
│ │
│ │──teleop_hand_and_arm.py 遥操作的启动程序代码
│ │
│ │──teleop_hand.py 可用于测试环境配置
熟悉整个代码框架后，发现每个模块都可以单独测试，然后需要修改以下几部分：加入自己机器人的urdf，完成自己机器人的逆运动学解算，重新搭建通信代码。
2.机器人urdf
机器人的urdf创建过程就不多说了，作者是和负责结构的同事合作完成的，其实就是按照我说的建立好坐标系，由soildworks导出的，然后我进一步的手动优化urdf。完成后，可以用一些工具显示一下，例如robot viewer,或者自己用ros的rviz可视化一下，确定一下关节的旋转，关节的限位，连杆的位置等是否正确。
3.逆运动学验证
有了urdf,进一步地，就需要构建逆运动学。这个是在robot_arm_ik.py中，仿照原文创建一个自己机器人的逆运动学类
把路径换成你自己的机器人urdf存放路径，把除了胳膊的关节都加到self.mixed_jointsToLockIDs里变成固定关节，不参与逆运动学解算，只对胳膊进行逆运动学解算，然后加入两个末端帧，加末端帧的时候，注意一下位置和旋转方向就可以了。剩下的交给库，感受库的强大。
我简单说一下吧：这里是将传统的逆运动学问题重新构建为一个带权重平衡的多目标优化问题，而非简单的误差最小化，同时优化位置精度、姿态精度、运动平滑性和能量效率。采用CasADi符号计算框架，实现高效的自动微分， 配置IPOPT求解器的实时优化参数，构建"一次建模，多次求解"的优化问题结构求解时间满足实时控制要求（<10ms）。总之，库够用。
完成后，修改robot_arm_ik.py里main函数，实例化一个类对象
运行python robot_arm_ik.py就可以看到以下画面了（出于一些原因作者就不展示我添加的机器人了）
按键"s"后可看到机器人能够根据一定的轨迹运动。
4.搭建通信
官方是通过dds实现通信完成关节层面的控制。这个需要按照自己机器人可用的通信方式去创建robot_ArmController控制器能以一定的频率实现数据的收发就可以了。
5.主程序启动
在teleop_hand_and_arm.py 中，进行一些修改，导入逆机器人的运动学库，通信控制器，机器人类型等
进行实例化后，ok了
其实还一个图传模块，作者也是验证了的，手里正好一个realsenseD435相机，也懒得去找其他相机了，目的是可以在vr里看到机器人操作视角，有需要的小伙伴也可以搞一下，其实不用也可以，聪明的小伙伴带着vr也可以看到实物。
5.验证
上实物前可以自己搭建个仿真，实现能通过vr，控制仿真里的机器人的手臂运动，这一步成功后，就可以落地了。
默认你已经有了这个仿真了。
执行 python teleop_hand_and_arm.py --arm=your_robot
接下来，执行以下步骤：
戴上您的 XR 头显设备（pico4 ultra enterprise等）
连接对应的 WiFi 热点
打开浏览器应用（PICO Browser），输入并访问网址：https://192.168.100.133:8012?ws=wss://192.168.100.133:8012
注意1：此 IP 地址应与您的 主机 IP 地址匹配。该地址可以使用 ifconfig 等类似命令查询。
注意2：此时可能弹出下图所示的警告信息。请点击Advanced按钮后，继续点击 Proceed to ip (unsafe) 按钮，使用非安全方式继续登录服务器。点击enter vr
此时，如果你没有图像服务，将会在 XR 头显设备中会看到一片黑。有图传的话会看到机器人第一人称操作视野，同时，终端打印出链接建立的信息：
websocket is connected. id:dbb8537d-a58c-4c57-b49d-cbb91bd25b90
default socket worker is up, adding clientEvents
Uplink task running. id:dbb8537d-a58c-4c57-b49d-cbb91bd25b90
然后输入r，就可以控制机器人手臂运动了。
6.归纳
整个流程下来对于一个合格的研发人员来说可以说是非常简单的。OK了，就记录到这吧。下次全身遥操见。