Franka 打开抽屉#

概述#

本文档详细描述了基于 Franka Emika Panda 机械臂的打开抽屉任务环境。

环境描述#

Franka 打开柜门任务环境基于真实的 Franka Emika Panda 7 自由度机械臂构建，旨在训练机器人接近柜门把手、抓取并拉开抽屉。

机器人结构#

Franka Emika Panda 是一个 7 自由度机械臂，由以下主要部分组成：

  • 基座（Base）：固定在地面上的机器人基座

  • 7 个关节：

    • joint1 ~ joint4：肩部和臂部旋转关节

    • joint5 ~ joint7：手腕部旋转关节

  • 夹爪（Gripper）：双指夹爪，包含两个手指关节

    • finger_joint1：左指关节，带有接触垫（left_finger_pad）

    • finger_joint2：右指关节，带有接触垫（right_finger_pad）

  • 末端执行器（TCP）：夹爪中心点，用于抓取操作

环境物体#

  • 柜子：包含一个可打开的抽屉

  • 抽屉把手（drawer_top_handle）：机器人需要抓取的目标部位

  • 抽屉关节（drawer_top_joint）：抽屉的滑动关节，1 个自由度

任务目标#

机器人需要完成以下操作目标：

  1. 接近把手：从初始位置移动到抽屉把手位置

  2. 姿态对齐：调整末端姿态与把手对齐

  3. 抓取把手：闭合夹爪抓取抽屉把手

  4. 拉开抽屉：向后拉动抽屉将其打开

动作空间#

动作空间为 Box(-inf, inf, (8,), float32)，表示施加到 8 个关节的位置控制指令（相对于当前关节位置的偏移量）。

控制模式#

环境使用位置控制模式，动作通过以下方式转换为关节目标位置：

目标关节角度 = 当前关节角度 + 动作值

动作维度详细说明#

编号

动作说明

控制范围

对应关节名称

关节类型

0

关节 1 偏移量

-inf ~ inf

joint1

revolve

1

关节 2 偏移量

-inf ~ inf

joint2

hinge

2

关节 3 偏移量

-inf ~ inf

joint3

hinge

3

关节 4 偏移量

-inf ~ inf

joint4

hinge

4

关节 5 偏移量

-inf ~ inf

joint5

hinge

5

关节 6 偏移量

-inf ~ inf

joint6

hinge

6

关节 7 偏移量

-inf ~ inf

joint7

hinge

7

夹爪动作（概率）

-inf ~ inf

finger_joint*

hinge

夹爪控制#

夹爪动作使用概率控制：

  1. Sigmoid 映射：将动作值映射到 [0, 1] 区间的概率

    p = 1 / (1 + exp(-action))

  2. 伯努利采样：根据概率 p 进行随机采样

    • 采样结果 < p：夹爪闭合（0.0）

    • 采样结果 >= p：夹爪打开（0.04）

观测空间#

观测空间为 Box(-5, 5, (25,), float32)，包含机器人的本体感知信息、任务相关信息和抽屉状态。

观测组成部分#

观测向量由以下部分组成（按顺序）：

  1. 关节角度（8 维）

    • 7 个机械臂关节角度（归一化到 [-1, 1]）

    • 归一化公式：2 × (关节角度 - 下限) / (上限 - 下限) - 1

  2. 关节速度（8 维）

    • 8 个关节的角速度（除以 2 进行缩放）

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

    • 位置偏差（3 维）：把手位置 - 末端位置 [Δx, Δy, Δz]

    • 姿态偏差（4 维）：把手姿态 - 末端姿态（四元数）

  4. 抽屉关节位置（1 维）

    • 抽屉当前打开距离

  5. 抽屉关节速度（1 维）

    • 抽屉当前打开速度

观测详细说明#

编号

观测内容

维度

范围

单位

0-7

归一化关节角度（8 个关节）

8

[-1, 1]

无量纲

8-15

归一化关节速度（8 个关节）

8

≈[-π/2, π/2]

rad/s

16-18

到把手的相对位置

3

[-5, 5]

m

19-22

到把手的相对姿态（四元数）

4

[-5, 5]

无量纲

23

抽屉关节位置

1

[-5, 5]

m

24

抽屉关节速度

1

[-5, 5]

m/s

所有观测值被裁剪在 [-5, 5] 范围内以保持数值稳定性。

奖励函数#

奖励函数采用复合设计，包含多个奖励和惩罚项。

主要奖励项#

  1. 距离奖励（权重：10）

    • 计算公式：10 × (1 - tanh(d_gripper_handle / 0.1))

    • 激励机器人末端接近抽屉把手

    • d_gripper_handle：末端到把手的欧氏距离

  2. 姿态匹配奖励

    • 计算公式：四元数相似度函数

    • 激励机器人末端姿态与把手姿态对齐

  3. 夹爪闭合奖励（条件奖励）

    • 当距离 < 0.025m：闭合夹爪获得 +100 奖励

    • 当距离 >= 0.025m：闭合夹爪获得 -20 惩罚

    • 打开夹爪：无奖励（0）

    • 激励机器人在接近时闭合夹爪抓取

  4. 打开抽屉奖励（指数奖励）

    • 计算公式：20 × (exp(open_dist) - 1)

    • open_dist：抽屉打开距离（裁剪在 [0, 1] 范围）

    • 随着抽屉打开程度增加，奖励指数增长

  5. 防止非法打开

    • 当抽屉已打开（open_dist > 0）但末端未接触把手（距离 > 0.03m）时，取消打开奖励

    • 防止机器人使用其他方式强行打开抽屉

惩罚项#

  1. 动作变化率惩罚

    • 计算公式：||current_action - last_action||²

  2. 关节速度惩罚

    • 计算公式：||joint_vel||²

  3. 手指位置穿透惩罚

    • 当手指接触垫低于把手表面时施加惩罚

    • 防止手指模型穿透抽屉

惩罚系数调度#

惩罚项系数随训练进度调整：

惩罚项

前期权重（steps < 8000）

后期权重（steps >= 8000）

动作变化率

1e-3

2e-3

关节速度平方和

0

2e-7

终止惩罚#

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

初始状态#

机器人初始化#

位置初始化：

机器人在世界坐标系中的初始位置固定：

  • 基座位置：固定在地面上

  • 关节角度：设置为默认姿态

默认关节姿态：

[0.0, -30°, 0°, -156°, 0.0, 186°, -45°, 0.04, 0.04] (弧度)

关节角度噪声：

每个关节角度在 [-0.125, 0.125] 弧度范围内添加均匀随机噪声。

速度初始化：

所有线速度和角速度初始化为零。

柜子初始化#

柜子固定在地面上，抽屉处于关闭状态（关节位置为 0）。

使用方法#

训练#

uv run scripts/train.py --env franka-open-cabinet

策略评估#

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

TensorBoard#

uv run tensorboard --logdir runs/franka-open-cabinet