撰写主配置文件

1. 准备

请准备好机器人的URDF文件

接下来以OpenLoong为例，介绍如何构造配置文件。

在此获取OpenLoong的urdf

确认URDF可用

在OpenLoong路径下，使用以下python3脚本测试：

import pybullet as p
import time
p.connect(p.GUI)
p.loadURDF("azureloong_description/urdf/AzureLoong_sr.urdf")
while 1:
    time.sleep(0.1)

正确运行后应有机器人正常显示在pybullet中。

2. 确定机器人home姿态

调整机器人home姿态

在OpenLoong路径下，使用以下python3脚本测试：

import pybullet as p
import time

p.connect(p.GUI)
robot_id = p.loadURDF("azureloong_description/urdf/AzureLoong_sr.urdf")
joint_index_debug_param_dt = {}
for i in range(0, p.getNumJoints(robot_id)):
    joint_info = p.getJointInfo(robot_id, i)
    print(joint_info)
    if joint_info[2] != p.JOINT_FIXED:
        joint_index_debug_param_dt[i] = p.addUserDebugParameter(
            paramName=f"   {i}    {joint_info[1].decode('utf-8')}",
            rangeMin=joint_info[8],
            rangeMax=joint_info[9],
            startValue=p.getJointState(robot_id, i)[0],
        )
while 1:
    for joint_index, param_id in joint_index_debug_param_dt.items():
        p.resetJointState(robot_id, joint_index, p.readUserDebugParameter(param_id))
    time.sleep(0.1)

程序运行后，应当看到如下可视化界面

通过调节右侧的滑块，调整机器人手臂的关节角度到期望的home姿态。 例如：

将右手臂大致调整到如下位置： [0.20, -0.97, -2.00, 1.70, 0.63, 0.00, -0.43]

将左手臂大致调整到如下位置： [-0.20, -0.97, 2.00, 1.70, -0.63, 0.00, 0.43]

3. 添加EE定义

将机械臂调整至home姿态后，需要在URDF中添加EE link 和 joint，用来做末端的位姿映射。

运行调整机器人home姿态的脚本时，终端会打印出机器人的joint信息，找到机器人手臂的末端link，一般是最后一个关节，或者有gripper，hand，palm这样的关键词。

例如，在OpenLoong中，我们选择：

(11, b'J_arm_r_palm', 4, -1, -1, 0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'Link_arm_r_palm', (0.0, 0.0, 0.0), (0.007859, 0.00817000000000001, 0.037736), (0.0, 0.0, 0.0, 1.0), 10)
(47, b'J_arm_l_palm', 4, -1, -1, 0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'Link_arm_l_palm', (0.0, 0.0, 0.0), (0.0077872, -0.30705, 0.027733), (0.0, 0.0, 0.0, 1.0), 46)

在urdf中,找到相应的link，添加EE link和joint，注意urdf树的依赖顺序：

  <link name="io_teleop_ee_link_left"/>
  <joint name="io_teleop_ee_joint_left" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="Link_arm_l_palm"/>
    <child link="io_teleop_ee_link_left"/>
  </joint>
  <link name="io_teleop_ee_link_right"/>
  <joint name="io_teleop_ee_joint_right" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="Link_arm_r_palm"/>
    <child link="io_teleop_ee_link_right"/>
  </joint>

添加完成后再次运行调整机器人home姿态的脚本，

记录下终端打印信息中，手臂关节的id（每一行打印的第一个元素），与ee link的id

对于OpenLoong，应该是：

joint_id_dt = {
    "right_arm_joint_ids": [4, 5, 6, 7, 8, 9, 10],
    "left_arm_joint_ids": [41, 42, 43, 44, 45, 46, 47],
}

ee_id_dt = {"right_ee_link_id": 12, "left_ee_link_id": 49}

在OpenLoong路径下，运行下方python3脚本，可视化ee坐标系

import pybullet as p
import time
import numpy as np


def debug_draw_pose(pose, replace_line_ids=None, line_width=5, line_length=0.3):
    axes = np.identity(3) * line_length
    colors = np.identity(3)
    line_ids = [] if replace_line_ids is None else replace_line_ids
    for i in range(3):
        if replace_line_ids is None:
            line_id = p.addUserDebugLine(
                lineFromXYZ=pose[0],
                lineToXYZ=p.multiplyTransforms(pose[0], pose[1], axes[i], [0, 0, 0, 1])[
                    0
                ],
                lineColorRGB=colors[i],
                lineWidth=line_width,
                lifeTime=0,
            )
            line_ids.append(line_id)
        else:
            p.addUserDebugLine(
                lineFromXYZ=pose[0],
                lineToXYZ=p.multiplyTransforms(pose[0], pose[1], axes[i], [0, 0, 0, 1])[
                    0
                ],
                lineColorRGB=colors[i],
                lineWidth=line_width,
                lifeTime=0,
                replaceItemUniqueId=replace_line_ids[i],
            )
    return line_ids


p.connect(p.GUI)
robot_id = p.loadURDF("azureloong_description/urdf/AzureLoong_sr.urdf")
joint_id_dt = {
    "right_arm_joint_ids": [4, 5, 6, 7, 8, 9, 10],
    "left_arm_joint_ids": [41, 42, 43, 44, 45, 46, 47],
}
ee_id_dt = {"right_ee_link_id": 12, "left_ee_link_id": 49}
# joint home is list concat of right and left arm home pos, it must be right arm first then left arm
joint_home = [0.20, -0.97, -2.00, 1.70, 0.63, 0.00, -0.43] + 
             [-0.20, -0.97, 2.00, 1.70, -0.63, 0.00, 0.43]
joint_ids = [id for ids in joint_id_dt.values() for id in ids]
ee_ids = [id for id in ee_id_dt.values()]
for i in range(p.getNumJoints(robot_id)):
    if i in joint_ids:
        p.resetJointState(robot_id, i, joint_home[joint_ids.index(i)])
    if i in ee_ids:
        debug_draw_pose(p.getLinkState(robot_id, i)[4:6], line_width=10)

while 1:
    time.sleep(0.1)

正确运行后，应当看到如下可视化界面

接下来通过修改urdf中io_teleop_ee_joint_right和io_teleop_ee_joint_left的origin rpy和xyz字段，

调整ee link的位置和姿态，使其满足：坐标系原点position落在掌心（或夹爪）中心（TCP）

下面区分使用相对或绝对姿态旋转的两种情况：

1. 绝对姿态旋转（推荐）：

末端的旋转相对与机器人的base，可以以任意姿态手持VR控制器

2. 相对姿态旋转：

末端的旋转相对于按下A键时的末端姿态，要求每次按下A键时，人手臂姿态与机器人手臂姿态尽量保持一致

在这种情况下，需要额外调整URDF中的io_teleop_ee_joint_right和io_teleop_ee_joint_left的origin rpy字段，使其满足：

1. Z轴竖直向上
2. Y轴与手面平行，且朝前（从机器人的角度，朝向机器人base的前方）

小提示：在pybullet中，为定轴旋转

对于青龙机器人调整后的结果应该是：

  <link name="io_teleop_ee_link_left"/>
  <joint name="io_teleop_ee_joint_left" type="fixed">
    <origin rpy="3.14 -1.57 0" xyz="0 0 0"/>
    <parent link="Link_arm_l_palm"/>
    <child link="io_teleop_ee_link_left"/>
  </joint>
  <link name="io_teleop_ee_link_right"/>
  <joint name="io_teleop_ee_joint_right" type="fixed">
    <origin rpy="3.14 1.57 0" xyz="0 0 0"/>
    <parent link="Link_arm_r_palm"/>
    <child link="io_teleop_ee_link_right"/>
  </joint>

4. 填写配置文件

在OpenLoong路径下，创建vr_configs.yml文件

添加vr_configs.yml文件，填入如下内容：

（所有与joint或link index有关数值可以按照调整机器人home姿态脚本运行后的终端打印中获取）

urdf_path: azureloong_description/urdf/AzureLoong_sr.urdf  # [required] 明确urdf的加载路径，对vr_configs.yml的相对路径
robot_name: OpenLoong # [required] 机器人的名字，与vr_configs.yml的上一层文件夹名字一致，命名规则为首字母大写
wo_controller: false # [required] 是否使用IO通用控制器，如果为true，则不会使用IO通用控制器
base_pose: # [required] 机器人的base位姿，需要将机器人调整到：世界系的Y周朝向机器人的前方，Z轴朝向机器人的上方，按照下方“关于base_pose的说明”确定具体数值
  position: [0, 0, 0.9]
  orientation: [0, 0, 0.707, 0.707]
arms: # [required] 手臂的joint index，ee_link index，以及home姿态下的关节角（rad），具体数值参考“调整机器人home姿态”章节中的内容。注意！：右手在前，左手在后
- joint_index: [4, 5, 6, 7, 8, 9, 10] # 右臂joint index
  ee_index: 12 # 右手ee link index
  rest_j_pos: [0.20, -0.97, -2.00, 1.70, 0.63, 0.00, -0.43] # 右臂home姿态
- joint_index: [41, 42, 43, 44, 45, 46, 47] # 左臂joint index
  ee_index: 49 # 左手ee link index
  rest_j_pos: [-0.20, -0.97, 2.00, 1.70, -0.63, 0.00, 0.43] # 左臂home姿态
grippers: # [optional] 对于有末端执行器的机器人，可以在此添加相应的joint index。注意！：右手在前，左手在后
- joint_index: [36, 37, 38, 15, 16, 17, 20, 21, 22, 25, 26, 27, 31, 32, 33] # 右手的全部joint index，可按照下方的数值顺序排序，如无手指（如为二指夹爪），按照index大小排序即可
  thumb: [36, 37, 38] # 右手大拇指的joint index
  index: [15, 16, 17] # 右手食指的joint index
  middle: [20, 21, 22] # 右手中指的joint index
  ring: [25, 26, 27] # 右手无名指的joint index
  pinky: [31, 32, 33] # 右手小拇指的joint index
- joint_index: [74, 75, 76, 53, 54, 55, 58, 59, 60, 63, 64, 65, 69, 70, 71] # 左手的全部joint index
  thumb: [74, 75, 76] # 左手大拇指的joint index
  index: [53, 54, 55] # 左手食指的joint index
  middle: [58, 59, 60] # 左手中指的joint index
  ring: [63, 64, 65] # 左手无名指的joint index
  pinky: [69, 70, 71] # 左手小拇指的joint index
waist: # [optional] 对于有腰部关节的机器人，可以在此添加相应的joint index
  joint_index: [79, 80, 81] # joint index
  rest_j_pos: [0, 0, 0] # home姿态下的关节角（rad），一般数值都为0
fixed_link: "Link_waist_yaw" # [optional] 对于有腰部关节的机器人，可以指定，想要固定的机器人link，一般为腰部关节的最后一个link，若不填写此字段，则默认为机器人的base link
head: # [optional] 对于有头部关节的机器人，可以在此添加相应的joint index
 joint_index: [-1, 3, 2] # 注意，头部关节的顺序应为： [roll, pitch, yaw], 缺失的关节用-1补全
 rest_j_pos: [-1, 0, 0] # home姿态下的关节角（rad），一般数值都为0，同样的，缺失的关节角度也用-1补全
controller_indices: # [optional]， 若“wo_controller”的值为“false”，则此处为强制项，否则可不填
  cmd_ee: [10, 47] # 发送给控制器的ee link index（右手在前，左手在后），一般可能与“arms”中制定的io_teleop_ee_link的index不同，可能为机器人的最后一个输出轴，具体以真实机器人的控制接口为准。
  base: [1, 1] # 发送给控制器的base link index（右手在前，左手在后），声明目标ee pose是相对于哪一个坐标系的，具体以真实机器人的控制接口为准。若为urdf tree中的第一个link，需要在urdf中添加一个虚拟的world link和world joint。注意！ 此时可能需要调整所有的urdf link index，+1。
dorsal: # [optional] 对于有单一的躯干升降自由度的机器人，一般与“waist”关键字不会同时存在，这里仅为示例全所有的config
  joint_index: 10  # joint index
  rest_j_pos: 0 # home姿态下的关节位置
dorsal_move_scale: 0.1 # [optional] 对于有单一的躯干升降自由度的机器人，这里用来控制升降速度
base_move_scale: [0.1, 0.1] # [optional] 对于有base可动的机器人，这里用来控制平移速度和旋转速度
vibration_thresholds: # [optional] 可以通过以下内容来控制VR手柄的震动
  ee_dist: [0.15, 0.2] # 若左右手的ee link距离在此范围内，则手柄震动
  tabletop_height: 0.85 # 设置桌面高度，若ee link的Z坐标接近此值，则手柄震动
  collision: true # 若设置为true，则夹爪与物体碰撞时，手柄震动
arm_pos_scale: 1.0 # [optional] 当机器人的臂展与人的臂展有明显差异时，可以通过此参数调整位置映射的缩放比例，若不填写此项，则默认为1.0, 机器人末端位置变化与VR手柄的位置变化一致，<1.0时，机器人的运动幅度会减小，>1.0时，机器人的运动幅度会增大
filter_window_size: 5 # [optional] 用于修改末端位置指令的滤波窗口大小，若不填写此项，则默认为5，若机器人底层自带滤波功能，可将此值设为0
abs_rot: True # [optional] 默认为True，若需要相对姿态映射，请将此项设为False 

关于base_pose的说明

base_pose是机器人的base位姿，

需要将机器人的姿态调整到：世界系的Y轴朝向机器人的前方，Z轴朝向机器人的上方;

位置上不做强制修改，但可以通过调整机器人的高度将其底部调整到地面上。

在OpenLoong路径下，使用以下python3脚本测试：

import pybullet as p
import time

p.connect(p.GUI)
p.loadURDF(
    "azureloong_description/urdf/AzureLoong_sr.urdf",
    basePosition=[0, 0, 0.9],
    baseOrientation=[0, 0, 0.707, 0.707],
)
while 1:
    time.sleep(0.1)

5. 验证配置文件

接下来，拉取IO提供的，应当在机器人工控机上运行的示范代码仓，此仓库依赖ros1环境

git clone https://github.com/ioai-tech/TeleXperience_robot_ros1_ws
cd TeleXperience_robot_ros1_ws
catkin build
source devel/setup.bash

在此仓库下，原本有示范的包含配置文件的OpenLoong文件夹，可以先将其拷贝到其他路径下或删除，用上方生成的文件替换。

在连接好TeleXperience所有设备后，首先在一个终端启动roscore

export ROS_IP=192.168.123.24
export ROS_MASTER_URI=http://192.168.123.24:11311
roscore

然后运行以下命令，启动仿真机器人的无控制器控制节点

cd TeleXperience_robot_ros1_ws
export ROS_IP=192.168.123.24
export ROS_MASTER_URI=http://192.168.123.24:11311
python3 src/io_teleop_robot_control_node/scripts/general_sim_robot_control_node.py --robot_name OpenLoong

开启遥操作后，机器人应当能够跟随头显以及手柄的运动而运动。