VisionPro遥操机械臂

VisionPro遥操机械臂

使用visionpro对机械臂进行遥操作是模仿学习收集真机数据的一大来源，使用vp进行遥操作的原因主要是应对后面灵巧手高自由度的遥操作，因此没有选择其他的遥操作方式。

在开发的过程中，遇到了不少的工程性的问题，将vp捕获的手部位姿与机械臂的末端位姿对齐需要进行平移向量和旋转矩阵的变换。

Software Vesion

• VisionProTeleop，但对其stream.py文件进行更改，添加了获取帧率控制
• RM_API2 睿尔曼机械臂SDK
• 系统所用版本为 ubuntu20.04

Hardware

• Visionpro
• 睿尔曼机械臂
• EG2-4C系列夹爪

原理实现

平移部分

平移向量采用增量式的控制方式，由于visionpro的pose量纲和机械臂的不统一，因此还加了缩放系数。伪代码实现如下：

vp = VisionProStreamer(ip=visionpro_ip, record=True, frequency=30)	# vp获取手部pose
base_hand_pose = vp.latest["left_wrist"].squeeze(0)	# shape由(1，4，4)转换为(4, 4)
initial_hand_xyz = base_hand_pose[:3, 3]	# 提取平移向量
while True:
    curr_ee_pose = algo_handle.rm_algo_forward_kinematics(robot_controller.robot.rm_get_joint_degree()[1], 1) # 获取机械臂末端pose
    curr_ee_pos = curr_ee_pose[:3]	# 提取机械臂末端pose的平移向量
    # 坐标系与机器人的坐标系对齐，并乘以相应的缩放系数
    d_pos_scaled = np.array([
        d_pos_raw[1] * vp_pose_scale_y, # X_arm = Y_vp
        d_pos_raw[0] * vp_pose_scale_x * -1, # Y_arm = -X_vp
        d_pos_raw[2] * vp_pose_scale_z
    ])
    ee_pos_target = curr_ee_pos + d_pos_scaled

旋转部分

机械臂的末端旋转是最让人头疼的，我在这里de了三天的bug，后面配合着数学推导，终于搞明白这块了，现在我们开始推导。

上边是visionpro的left wrist的参考坐标系，下边是机械臂是参考坐标系，可以发现，他们的坐标系不是统一的，是存在旋转的。

他们的相对旋转为：
$$\left[\begin{array}{ccc}0& 0& 1\\ 1& 0& 0\\ 0& -1& 0\end{array}\right]$$
我们可以由增量式作为启发，对旋转矩阵也实行增量式控制：

我们定义：

• 初始的vp获得的手部旋转矩阵：${}^{VP}{R}_{init\_hand}$
• 一直更新的vp获得的手部旋转矩阵： ${}^{VP}{R}_{hand}$
• VP坐标系与arm坐标系的相对旋转矩阵：${}^A{R}_{VP}$
• Arm坐标系下的arm末端初始旋转矩阵： ${}^A{R}_{init\_ee}$
• Arm坐标系下的目标ee的旋转矩阵：${}^A{R}^{}$