RM65 打开抽屉#

概述#

本文档描述 rm65-open-cabinet 操作任务环境。该环境使用 RM65 六自由度机械臂与并联夹爪，目标是接近柜体底部抽屉把手、完成抓取并将抽屉拉开。

环境描述#

该任务基于 RM65 机械臂和柜体抽屉场景构建。虽然环境注册名为 rm65-open-cabinet，但当前实际操作对象是柜体底部抽屉，代码中对应目标为 drawer_bottom_handle 与 drawer_bottom_joint。

机器人结构#

RM65 机器人在该环境中包含以下主要部分：

  • 基座（base_link）：固定在工作台前方

  • 6 个机械臂关节：joint_1 ~ joint_6

  • 并联夹爪：主驱动关节为 gripper_Left_1_Joint，其余 5 个夹爪关节通过联动关系同步

  • 末端执行器（TCP）：gripper 站点，用于计算末端与把手的相对位姿

  • 手指接触点：left_finger_pad、right_finger_pad，用于判定是否对准把手以及是否发生穿透

场景物体#

  • 柜体：包含多个门和抽屉

  • 目标把手：drawer_bottom_handle

  • 目标关节：drawer_bottom_joint，滑动范围为 0.0 ~ 0.4 m

任务目标#

机器人需要完成以下阶段：

  1. 靠近把手：将末端执行器移动到抽屉把手附近

  2. 姿态对齐：让夹爪姿态与把手姿态匹配

  3. 稳定抓取：闭合夹爪并连续保持抓取

  4. 打开抽屉：沿抽屉滑动方向将底部抽屉拉开

动作空间#

动作空间为 Box(-inf, inf, (7,), float32)。

其中前 6 维控制机械臂关节目标，最后 1 维控制夹爪开合。

控制模式#

  • 机械臂：默认使用 joint_target 模式，并启用归一化目标控制 机械臂动作会先裁剪到 [-1, 1]，再线性映射到各关节控制范围

  • 夹爪：默认使用 binary 模式 原始动作经过 Sigmoid 映射为闭合概率，再通过带迟滞的二值开关决定夹爪开闭

动作维度详细说明#

编号

动作说明

原始输入范围

实际控制对象

0

关节 1 目标

(-inf, inf)

joint_1

1

关节 2 目标

(-inf, inf)

joint_2

2

关节 3 目标

(-inf, inf)

joint_3

3

关节 4 目标

(-inf, inf)

joint_4

4

关节 5 目标

(-inf, inf)

joint_5

5

关节 6 目标

(-inf, inf)

joint_6

6

夹爪开合命令

(-inf, inf)

gripper_Left_1_Joint

控制约束#

  • 控制周期 ctrl_dt = 0.025s，对应 40Hz 控制频率

  • 机械臂默认启用速度限制、加速度限制、动作延迟与执行器一阶滞后

  • 训练时会在每个 episode 内随机化机械臂延迟、滞后、速度上限和加速度上限，以增强 sim2real 鲁棒性

  • 夹爪使用迟滞阈值控制：

    • 开启到闭合阈值：0.78

    • 闭合到开启阈值：0.62

    • 最小切换间隔：0.25s

观测空间#

观测空间为 Box(-inf, inf, (84,), float32)，最终观测会裁剪到 [-5, 5]。

观测组成#

观测由以下 4 部分拼接而成：

  1. 关节位置（7 维）

    • 6 个机械臂关节

    • 1 个夹爪主驱动关节

    • 统一归一化到 [-1, 1]

  2. 关节速度（7 维）

    • 使用相邻两帧关节位置差分估计

    • 再除以 2 做缩放

  3. 目标相对位姿（7 维）

    • 末端到把手的相对位置 Δx, Δy, Δz

    • 把手相对末端的四元数姿态差

  4. 动作历史（63 维）

    • 最近 9 步原始动作历史

    • 每步 7 维，共 9 × 7 = 63 维

观测维度详细说明#

编号范围

内容说明

维度

0-6

归一化关节位置

7

7-13

关节速度

7

14-16

末端到把手的相对位置

3

17-20

末端到把手的相对姿态四元数

4

21-83

最近 9 步动作历史

63

观测噪声#

该环境默认开启 sim2real 观测扰动，主要包括：

  • 关节位置与速度噪声

  • 把手位置与姿态噪声

  • 把手观测偏置

  • 把手观测随机丢帧，并在丢帧时保持上一帧结果

奖励函数#

奖励函数采用分阶段复合设计，既鼓励接近与对齐，也鼓励稳定抓取和持续拉开抽屉。

主要奖励项#

  1. 距离奖励

    dist_reward = 15.0 * (1 - tanh(distance / 0.4))

    鼓励末端执行器持续靠近把手。

  2. 姿态奖励

    基于末端姿态与把手姿态的四元数相似度计算，并仅在末端距离把手较近时生效。

  3. 夹爪闭合奖励

    当末端距离小于 0.035m 且左右手指在把手上下方对齐时，闭合夹爪可获得正奖励； 否则闭合会受到惩罚。该项还会随夹爪闭合程度缩放。

  4. 抽屉打开奖励

    open_reward = (exp(open_dist) - 1.0) * 420.0

    只有在满足“已经抓住或进入抓取阶段”且末端仍靠近把手时才生效。

  5. 抽屉增量奖励

    对本步新增的打开距离给予额外奖励，鼓励稳定、连续地拉动抽屉。

  6. 稳定抓取奖励

    当末端距离小于 0.03m、夹爪闭合比例大于 0.7，并持续保持 6 步后，环境认为已经抓住把手，并提供持续奖励。

  7. 里程碑奖励

    • 抽屉打开超过 0.15m 时奖励 35

    • 抽屉打开超过 0.22m 时额外奖励 70

惩罚项#

  1. 滑脱惩罚

    如果已经进入抓取阶段但后来失去抓取，同时抽屉已经被拉开，则施加额外惩罚。

  2. 手指穿透惩罚

    当手指接触点穿过把手上下边界时施加惩罚，用于抑制不合理接触。

  3. 夹爪频繁切换惩罚

    当末端靠近把手时，频繁在开合之间切换会触发惩罚。

  4. 动作变化惩罚

    对相邻两步动作差的平方和施加惩罚。

  5. 关节速度惩罚

    对关节速度平方和施加惩罚，后期训练权重更高。

  6. 终止惩罚

    当触发终止条件时，额外施加 -10.0 惩罚。

初始状态#

机器人初始化#

  • 机械臂默认关节初值为零位姿

  • 夹爪默认处于张开状态

  • 复位时默认不添加关节角随机噪声

  • 所有关节速度初始化为零

场景初始化#

  • 目标抽屉初始为完全关闭状态

  • 抽屉与柜体其余部分固定在场景默认位置

随机化内容#

每次 reset 会重新采样部分 sim2real 参数，包括：

  • 机械臂动作延迟

  • 机械臂执行器滞后

  • 机械臂速度/加速度上限

  • 把手观测偏置

Episode 终止条件#

以下任一条件满足时，episode 会提前终止：

  1. TCP 落到把手后方过多

    • 终止阈值：tcp_x - handle_x < -0.02

  2. 关节速度过大

    • 任意机器人关节速度绝对值超过 3.93 rad/s

此外，环境最大时长为 30s。

使用方法#

训练#

uv run scripts/train.py --env rm65-open-cabinet --train-backend torch

策略评估#

uv run scripts/play.py --env rm65-open-cabinet

TensorBoard#

uv run tensorboard --logdir runs/rm65_open_cabinet