Skip to content

第六章 运动控制综合实践:移动与抓取 ​

本章导读:第四章解决了“怎样让移动底盘按指定速度行驶、到达指定位置”的问题,第五章解决了“怎样让机械臂运动到指定关节姿态或末端位姿”的问题。本章是一个综合实践章,把这两种能力连接起来,完成一个看得见、可以检查结果的任务:G2 从初始位置移动到桌前,用右臂拿起固定位置的红色方块,再放进固定位置的蓝色方框。

本章不引入新的底层控制器,而是学习“系统集成”本身:任务怎样拆成有先后顺序的阶段(状态机 / 动作序列),同一个末端目标怎样用关节增量(relative_joint)和末端增量(delta_ee)两种方式表达,世界坐标系、机械臂基座坐标系 arm_base_link 和夹爪坐标系之间怎样换算,底盘怎样稳稳停靠,以及怎样用物理接触而不是“作弊”来抓起方块并判定成功。后续章节引入感知、环境理解和任务决策时,都会复用这里建立的“目标 → 动作 → 执行 → 验收”框架。

本章概览 ​

本章要解决的问题 ​

设想你已经能让 G2 的底盘开到某个点,也能让机械臂伸到某个末端位姿。现在桌上放着一块 4 cm 的红色方块,旁边有一个蓝色方框,你要让机器人把方块放进框里。问题马上变得具体起来:底盘开到哪里停才能让方块落入手臂工作空间?配置里写的是世界坐标,而机械臂的运动学是在 arm_base_link 下建立的,两者怎样换算?“往下探 16 cm”应该让关节平滑插值,还是让夹爪沿直线下降?夹爪合上之后,怎样确认方块真的被拿起来,而不是空夹?方块松开后,怎样确认它是“整块稳稳地落在框里”,而不是正好从框上方掉过去?

本章就围绕这些问题,用一个固定场景、固定动作序列、但带反馈与验收的程序来回答。

开始之前,先明确本章的四个边界:

  1. 这是固定场景下的纯控制实践,不是视觉抓取。 方块坐标、底盘航点和动作顺序由人提前写好,没有相机、检测器、抓取生成网络或语言模型。
  2. “写死目标”不等于“完全没有反馈”。 底盘读取实际位置,机械臂读取实际关节状态,程序还检查方块是否被抬起。固定的是任务目标,而不是要求机器人闭着眼睛按时间盲动。
  3. 移动与操作采用先后执行,而不是同时协调规划。 底盘先停到桌前,然后机械臂抓放。这不是全身运动规划,也不包含 SLAM、Nav2、MoveIt 或动态避障。
  4. 成功必须来自物理接触。 方块是有质量和重力的刚体,夹爪依靠接触与摩擦把它拿起来。程序不把方块吸附到手上,不通过焊接或瞬移制造成功。

读完本章,应当能够解释三个问题:固定目标如何经过坐标变换变成关节命令,relative_joint 与 delta_ee 为什么不是同一种动作,以及为什么“机械臂到达了”还不等于“抓取完成了”。

学习目标 ​

  • 解释本章顺序任务的 10 个阶段(准备、移动、预抓取、下探、夹紧、抬升、搬运、下放、释放与退出、验收)各自的作用,以及哪些阶段按 --mode 切换动作模式。
  • 推导relative_joint 的目标关节角 qgoal=qmeasured+Δq 与三次时间缩放轨迹,并手算某一时刻的关节目标。
  • 推导delta_ee 的分段数 N 与每个中间末端位姿,解释“整段 IK 预检通过才下发”的含义。
  • 实现世界坐标到 arm_base_link 的位姿转换,并说明为什么世界“向下 2 cm”不能直接写成 dp=[0,0,-0.02]。
  • 解释底盘停靠的比例控制、限速、容差和停车复核,以及为什么底盘位姿要读 chassis_link 而不是 articulation 根。
  • 运行两种模式的默认任务,并通过终端输出和 JSONL 记录判断任务是否真正成功。
  • 排查航点超时、IK 未收敛、TCP 未到达、空夹、放置失败等常见故障。

前置知识 ​

  • 第四章:四轮独立转向底盘的运动学、PoseController 位姿比例控制、G2BaseController 的限速与加速度约束。
  • 第五章:G2 右臂七个关节的命名与顺序、正运动学(Forward Kinematics, FK)、阻尼最小二乘逆运动学(Inverse Kinematics, IK)、三次时间缩放关节轨迹。
  • 数学:向量加减与范数、旋转矩阵(列向量含义、转置即逆)、旋转向量(轴角表示)的基本概念。
  • 编程:Python 类与异常处理、NumPy 数组运算;会用 Isaac Sim 自带的 python.sh 运行脚本(见前面的安装章节)。

知识地图 ​

本章知识地图

图 6-1 本章知识地图:任务状态机统领全章,动作模式、坐标系和底盘停靠三条支线汇入夹爪接触抓取与成功判定,最后落到代码结构与运行

对应代码 ​

  • code/code_chapter6/:本章的全部可执行程序(场景、动作接口、任务状态机、可选数据记录、运行入口)。
  • code/code_chapter4/:被直接导入的底盘运动学与底盘控制器(SwerveKinematics、G2BaseController、PoseController)。
  • code/code_chapter5/:被直接导入的机械臂控制器与运动学(G2ArmController、orientation_error、HOME_JOINT_POSITIONS 等)。

本章对应的实践代码位于 code/code_chapter6,教程位于 code/chapter6。第六章没有复制一份第九章的控制器,而是直接导入第四、第五章的模块,只补充把它们连接起来的场景、动作接口和任务逻辑。第九章仅作为本教程组织方式的参考,不是本章的运行依赖。


第一部分 先看完整任务:从两个控制器到一个移动抓放系统 ​

1.1 场景里有什么,任务究竟要完成什么 ​

直观理解:本章只保留理解控制所必需的对象——一台 G2 Omnipicker、物理地面、一张桌子、一块红色立方体,以及一个由底板和四面矮墙组成的蓝色开口方框。没有加载完整仓库背景,也没有随机散落的物体。

把场景做简单,不是为了回避系统问题,而是为了区分问题来源。如果目标位置始终固定,就可以把注意力集中在运动是否正确、坐标是否统一、夹爪是否夹紧,而不是同时排查检测精度和模型推理。

默认任务中的主要尺寸和目标如下。除底盘偏航外,表中坐标和尺寸的单位均为米,使用世界坐标系。

对象或目标默认设置这样设置的含义
底盘第一航点(0.35, 0.00, 0.00)先向前移动一段;第三个数是偏航角,不是高度
底盘最终航点(0.70, 0.00, 0.00)到桌前停靠,偏航保持为零
桌面中心与尺寸中心 (1.40, -0.42, 0.86),尺寸 (0.70, 0.90, 0.08)桌面上表面高度是 0.90
红块初始中心(1.27, -0.30, 0.921)放稳后中心约为 z=0.920,初始留出约 1 mm 间隙
红块边长、质量0.04、0.04 kg4 cm 的小方块,靠接触摩擦抓取
接收框中心 XY(1.18, -0.52)只写平面中心,高度由桌面与底板计算
接收框内边长、墙高0.18、0.045四面矮墙提供明确的“框内”区域
接收框底板上表面z=0.908桌面高度加 8 mm 底板厚度
抬升距离0.16拿起后从高处搬运,不沿桌面拖动

这些数值集中放在 config.py 中。它们组成了一个已经验证过的默认场景,不是所有桌面和机器人姿态都适用的通用参数。

对应代码(code/code_chapter6/config.py):

python
"""先读这里:单位为 m / rad / s;任务目标全部写死在世界坐标系。"""
from code_chapter4.kinematics import Pose2D

# 不随机生成目标,不使用视觉/导航框架/学习模型。
BASE_WAYPOINTS = (Pose2D(0.35, 0.0, 0.0), Pose2D(0.70, 0.0, 0.0))  # (x, y, yaw)
TABLE_CENTER = (1.40, -0.42, 0.86)
TABLE_SIZE = (0.70, 0.90, 0.08)  # 台面高度 0.90
CUBE_SIZE = 0.04
CUBE_POSITION = (1.27, -0.30, 0.921)
BOX_CENTER = (1.18, -0.52)       # 开口方框中心 XY
BOX_INNER_SIZE = 0.18
BOX_WALL = 0.012
BOX_HEIGHT = 0.045
BOX_FLOOR = 0.008
TABLE_TOP = TABLE_CENTER[2] + TABLE_SIZE[2] / 2   # 0.86 + 0.04 = 0.90
BOX_FLOOR_TOP = TABLE_TOP + BOX_FLOOR             # 0.90 + 0.008 = 0.908
# ...

代码解读:

  • 桌面上表面高度不是直接写死的,而是由“中心高度 + 半个厚度”计算: 0.86+0.08/2=0.90。改桌子尺寸时,抓取高度和框底板高度会自动跟着变。
  • 框底板上表面 0.90+0.008=0.908,方框墙厚 BOX_WALL=0.012 只用于搭几何,不参与“框内”判定(判定用的是内边长 0.18)。
  • Pose2D 来自第四章,第三个分量是偏航角 yaw(rad),不是 z 高度。

图6-1 固定场景的世界XY俯视布局、两个底盘航点与红块入框任务

图 6-2 固定场景的世界 XY 俯视布局、两个底盘航点与红块入框任务

图 6-2 怎么看: 先沿蓝色方向读底盘航点,再看桌面的红块和蓝框。绿色虚线只表示“把这个物体搬到那里”的任务关系,不是计算得到的机械臂轨迹。图中位置使用世界坐标,不能直接当成 delta_ee 的输入。

案例:从配置算出几个关键高度。 后面各阶段的 TCP 目标都由这几个常量推出,先手算一遍:

量计算结果(m)
桌面上表面 TABLE_TOP0.86+0.08/20.900
抓取点 TCP 高度 pick[2]0.900+0.04/20.920
红块上方 above_pick[2]0.920+0.161.080
框底板上表面 BOX_FLOOR_TOP0.900+0.0080.908
释放点 TCP 高度 place[2]0.908+0.04/2+0.0120.940
框上方 above_place[2]0.940+0.161.100
红块落稳后中心高度0.908+0.04/20.928

💡 要点:红块初始中心 0.921 比几何静止高度 0.920 高 1 mm,是为了生成刚体时不与桌面穿插;抓取目标用的是几何高度 0.920,而不是初始生成高度。

1.2 整个系统是怎样分工的 ​

直观理解:可以把这个系统想成“任务安排、动作翻译、执行与检查”三层,就像一个项目经理(决定先做什么)、一个翻译(把需求翻成工人能听懂的指令)和一群工人加质检员。

任务安排层决定下一步做什么:先移动、再接近、然后夹紧、抬升、搬运和松爪。它不负责计算某个轮子应该转多快,也不直接求解七个关节角。

动作翻译层把任务目标翻译成控制器能够接受的输入。底盘目标是平面位置和偏航;机械臂目标是末端的位置与朝向;夹爪目标是主动关节的开合角度。机械臂还要根据所选模式,把目标变成关节增量或末端增量。

执行与检查层让 Isaac Sim 中的关节驱动和接触物理推进,同时读取反馈。如果底盘没有停稳、IK 没有收敛、关节没有跟上、方块没有被拿起,就不能继续假装下一步已经具备条件。

系统环节输入输出主要实现来源
顺序任务固定航点、抓放位姿、执行状态当前阶段和下一段运动目标第六章 task.py
底盘位姿控制当前与目标 x/y/yaw机体坐标下的线速度和角速度第四章 PoseController
四轮独立转向控制底盘速度、当前转向角四个转向目标和四个驱动速度第四章运动学与底盘控制器
机械臂动作适配固定目标或增量动作目标关节角、短段关节轨迹第六章 MotionController
FK、IK 与关节轨迹当前关节状态、末端目标可执行的关节位置目标第五章控制器与运动学
接触物理关节目标、碰撞几何、材料和质量实际运动与抓放结果Isaac Sim 场景
结果验收实际 TCP、方块与底盘状态继续执行、失败或成功第六章控制与任务代码

这里没有复制一份第九章的控制器。第六章直接导入第四、第五章的模块,只补充把它们连接起来的场景、动作接口和任务逻辑。

图6-2 固定任务经底盘、机械臂与夹爪三条控制支路进入物理仿真并检查反馈

图 6-3 固定任务经底盘、机械臂与夹爪三条控制支路进入物理仿真并检查反馈

图 6-3 怎么看: 从上往下读:任务给出目标,控制层把目标翻译成关节命令,物理仿真产生实际结果。中间两种机械臂模式是可选支路,不是要先执行一次 relative_joint 再执行一次 delta_ee;夹爪则有自己独立的命令。最下方的实测状态用于后续控制和任务验收。

对应代码:三层在 MotionController 的构造函数里一目了然——底盘、位姿控制器、机械臂都来自前两章,第六章只是把它们装在一起(code/code_chapter6/control.py):

python
from code_chapter4.base_controller import G2BaseController, PoseController
from code_chapter4.config import ControlLimits, RobotGeometry
from code_chapter4.kinematics import SwerveKinematics
from code_chapter5.arm_controller import G2ArmController
from code_chapter5.kinematics import axis_rotation, orientation_error
from .config import GRIPPER_JOINT, GRIPPER_OPEN, GRIPPER_CLOSED


class MotionController:
    def __init__(self, env, recorder=None):
        self.env, self.config, self.recorder = env, env.config, recorder
        geometry = RobotGeometry()
        # 第四章:四轮独立转向底盘控制器(限速、加速度约束、超时刹车)
        self.base = G2BaseController(env.robot, SwerveKinematics(
            geometry.wheel_positions, geometry.wheel_radius), ControlLimits())
        # 第四章:位姿比例控制器,第六章把速度上限和容差调得更保守
        self.pose_controller = PoseController(max_linear_speed=.22, max_angular_speed=.3,
                                             position_tolerance=.006, yaw_tolerance=.012)
        # 第五章:右臂七关节控制器(FK / IK / 轨迹)
        self.arm = G2ArmController(env.robot, "right")
        # 夹爪只控制一个主动关节
        self.gripper_index = env.robot.dof_names.index(GRIPPER_JOINT)
        self.phase = "initialize"
        self.target_q = self.arm.get_joint_positions()
        self.gripper_target = GRIPPER_OPEN

1.3 任务状态机:把“移动抓放”拆成有检查的动作序列 ​

直观理解:状态机(Finite State Machine, FSM)就是“现在处于哪个阶段、满足什么条件才能进入下一阶段”。本章的任务是线性的顺序状态机:每个阶段只有两种出口——条件满足则进入下一阶段;不满足则抛出异常、整个任务终止。没有分支、没有回退、没有重试,所以它可以直接写成一段从上到下执行的 Python 函数,而不需要复杂的任务规划器。

底盘停好后,任务执行下列阶段。

阶段做什么不能省略的原因
准备右臂到 HOME,夹爪张开让抓放从可解释的起始状态开始
移动按两个固定航点停到桌前让抓放区域进入机械臂工作空间
预抓取到红块上方,夹爪调整为向下把朝向调整和接触动作分开
下探TCP 到固定抓取高度让两指进入能够夹住方块的位置
夹紧关闭主动夹爪关节并等待给驱动与接触物理建立夹持的时间
抬升抬到红块上方的安全高度验证抓取,同时离开桌面
搬运从高处移动到框上方减少拖桌面和碰框墙的风险
下放移动到固定释放位姿降低松爪时方块掉落的高度
释放、退出松开夹爪,随后向上离开避免夹爪继续影响方块落稳
验收等待并检查方块状态排除空抓、掉落和框外放置

task.py 顺序状态机

图 6-4 task.py 的顺序状态机:蓝色阶段固定用关节轨迹,橙色阶段按 --mode 选择动作模式,绿色为夹爪动作,黄色为检查点,任一检查失败即终止

对应代码(code/code_chapter6/task.py,删减了部分打印):

python
def run_task(motion):
    env, cfg = motion.env, motion.config
    # 阶段 1 准备:relative_joint 的增量 = HOME - 当前实测,即“回到 HOME”
    motion.set_phase("prepare: 收臂、张开夹爪")
    motion.relative_joint(HOME_JOINT_POSITIONS-motion.arm.get_joint_positions())
    motion.gripper(opened=True)
    # 阶段 2 移动:两个固定航点,各自闭环到达并复核
    for target in C.BASE_WAYPOINTS:
        motion.navigate(target)
    parked_pose = env.base_pose()          # 记下停车位姿,供最后检查底盘漂移

    # 由配置推出四个世界目标点(见 1.1 节手算表)
    pick = np.array(C.CUBE_POSITION)
    pick[2] = C.TABLE_TOP+C.CUBE_SIZE/2
    above_pick = pick + [0., 0., cfg.clearance]
    place = np.array([*C.BOX_CENTER, C.BOX_FLOOR_TOP+C.CUBE_SIZE/2+.012])
    above_place = place + [0., 0., cfg.clearance]

    # 高空准备姿态统一用关节轨迹;抓放阶段由 --mode 决定。
    motion.set_phase("pregrasp: 到红块上方")
    motion.move_world(above_pick, C.GRASP_ROTATION, mode="relative_joint")
    motion.set_phase("descend: 下探固定抓取点")
    motion.move_world(pick, C.GRASP_ROTATION)            # mode=None → 用 --mode
    motion.set_phase("close: 夹紧")
    motion.gripper(opened=False)
    motion.set_phase("lift: 垂直抬升")
    motion.move_world(above_pick, C.GRASP_ROTATION)
    motion.hold(.4)
    # ... 抬升检查(见第五部分)
    motion.set_phase("transfer: 高空移动到框上方")
    motion.move_world(above_place, C.GRASP_ROTATION)
    motion.set_phase("place: 下放")
    motion.move_world(place, C.GRASP_ROTATION)
    motion.set_phase("release: 张开夹爪")
    motion.gripper(opened=True)
    motion.set_phase("retreat: 向上退出")
    motion.move_world(above_place, C.GRASP_ROTATION)
    motion.hold(1.0)
    # ... 连续半秒入框验收、底盘漂移检查(见第五部分)
    motion.set_phase("success")
    return {"success": True, "mode": cfg.mode, "lift_height_m": lift_height,
            "cube_position": p.tolist(), "base_drift_m": float(drift),
            "simulation_time_s": env.steps*cfg.dt}

代码解读:

  • 状态 = set_phase() 写入的字符串。 它既打印到终端([第六章] descend: 下探固定抓取点),又被记录器写进每一行 JSONL 的 phase 字段,所以失败时能精确知道停在哪个阶段。
  • 状态转移条件 = 被调用函数“正常返回”。 navigate()、relative_joint()、delta_ee()、move_world() 内部都有检查,不满足就 raise。因此“下一行代码能执行”本身就意味着上一阶段已通过验收。
  • 模式切换点:pregrasp 显式写了 mode="relative_joint";descend 到 retreat 的 move_world() 不传 mode,于是使用 TaskConfig.mode,也就是命令行的 --mode。
  • 失败出口:异常一路冒泡到 demo_pick_place.py 的 main(),在那里记录 error 与 phase,停止底盘并关闭仿真,进程以非零码退出(见第六部分)。

⚠️ 注意:当前程序没有自动重抓或故障恢复。出现异常后,它会停止底盘、记录失败原因并关闭仿真。对本章来说,清楚地报告失败,比用不透明的补救动作把演示勉强继续下去更有教学价值。

1.4 为什么固定动作仍然需要闭环 ​

假设给底盘发出“以某个速度前进三秒”的命令。即使速度和时间固定,实际停车点也可能受加速过程、轮地接触、转向滞后和惯性影响。机械臂同样如此:发送目标关节角,只代表“要求到那里”,不代表物理关节已经到那里。

因此本章使用的是固定目标、反馈执行:

  • 底盘反复计算“目标位置减去实际位置”,离目标越近,速度越小。
  • 机械臂从当前实测关节状态出发,生成并执行轨迹。
  • 抓取后读取方块状态,确认不是空夹。
  • 放置后读取方块状态,确认不是只在空中经过目标。

程序使用仿真状态作为理想化反馈来源。真实机器人要靠编码器、定位系统和传感器提供这些信息;本章不实现这一部分传感器链路。

还要区分两种“已知”:目标是已知的,不代表执行结果也是已知的。 红块的目标位置来自配置,是否拿起来却必须通过实际结果检查。这样才能把一个动作脚本变成可验证的控制实验。

案例:开环与闭环的差别。 底盘从 x=0.35 开到 x=0.70,需要前进 0.35 m。若开环地“以 0.22 m/s 前进 1.59 s”,忽略了第四章底盘控制器的加速度限制(0.80 m/s²,从 0 加速到 0.22 m/s 至少要约 0.275 s),也忽略了停车惯性,停车点必然偏短或偏长若干厘米;而第六章的位置容差只有 6 mm。闭环做法则在每个物理步都重新读取 chassis_link 位姿,按剩余误差重新计算速度,直到误差进入容差并且停稳复核通过(见第四部分)。

💡 要点:本章所有“完成”都以测量值为准:底盘看 chassis_link,机械臂看实测关节角和 TCP 世界位姿,抓放看方块真值。下发了命令 ≠ 完成了动作。


第二部分 两种机械臂动作模式:关节增量与末端增量到底有什么不同 ​

2.1 先把名称、动作维度和直观含义说清楚 ​

本章使用的规范名称是 relative joint space(相对关节空间) 和 delta end-effector pose(末端位姿增量)。程序中的模式名称分别为 relative_joint 和 delta_ee,命令行必须使用这两个拼写。

直观理解:先用手臂做一个类比。“肩关节再转一点、肘关节再弯一点”是在描述关节应该怎样变化;“保持手掌朝向,让手向桌面靠近两厘米”是在描述手应该怎样变化。前者接近关节增量,后者接近末端增量。

比较项目relative_jointdelta_ee
动作直接描述什么七个关节各自改变多少角度末端平移多少、旋转多少
动作形式七维向量 dq三维平移 dp 加三维旋转向量 dr
单位radm 与 rad
“相对”的参考调用时的实测关节角调用时由实测关节角计算的末端位姿
直接使用该接口时是否需要 IK不需要需要
本章固定世界目标如何接入先 IK 求关节目标,再转成 dq转为机械臂基座目标,再求 dp/dr
中间路径在关节空间平滑插值在末端空间取中间位姿,再逐点 IK
TCP 路径特点通常不是直线离散近似直线;小段间仍由关节轨迹执行
最后发给仿真的是什么关节位置目标仍然是关节位置目标

关键区别不在最后调用了哪个关节执行器,而在“动作表达在哪个空间,以及中间路径怎样生成”。 两种模式都不是力矩控制,也不是仅凭名称就能得到的柔顺或阻抗控制。

图6-3 相同起终末端位姿下,关节插值的可能弯曲路径与末端模式的离散直线取样对比

图 6-5 相同起终末端位姿下,关节插值的可能弯曲路径与末端模式的离散直线取样对比(概念示意)

图 6-5 怎么看: 左图先决定关节如何变化,TCP 路径由机械臂运动学产生;右图先规定中间 TCP 位姿,再求出相应关节角。右图的小夹爪符号表示这个例子要求保持朝向。两幅图都是概念示意,没有拿示意曲线冒充真实 G2 运行轨迹。(图内自带标题沿用原稿编号,以本图注编号为准。)

2.2 相对关节空间:从当前关节角再走一段 ​

原理与公式:设当前七个关节的角度为 qmeasured(rad),输入的七维增量为 Δq(rad),则本次动作的目标是:

qgoal=qmeasured+Δq

例如,第一关节当前为 0.20 rad,本次增量为 0.05 rad,目标就是 0.25 rad。下一次再调用相同增量时,参考的是下一次调用时重新读取的实际状态,不是固定参考点,也不是一直相对于初始 HOME 姿态。

七个数字分别对应哪个关节? 本章控制的是右臂,顺序沿用第五章的 ARM_JOINT_NAMES["right"],不是随意选取机器人全部自由度中的前七个:

输入位置右臂关节名称含义
第 1 个idx61_arm_r_joint1J1 的角度增量
第 2 个idx62_arm_r_joint2J2 的角度增量
第 3 个idx63_arm_r_joint3J3 的角度增量
第 4 个idx64_arm_r_joint4J4 的角度增量
第 5 个idx65_arm_r_joint5J5 的角度增量
第 6 个idx66_arm_r_joint6J6 的角度增量
第 7 个idx67_arm_r_joint7J7 的角度增量

正负号表示沿各关节自身定义的轴正转或反转,不能统一理解为“手向左或向右”。某一项为零,只表示该关节本次目标角度不变;其他关节运动时,TCP 仍然可能移动。夹爪开合不在这七个数中,而由独立的 gripper() 接口控制。

案例:一次七维加法。 假设调用时实际角度恰好为(这也正是第五章的 HOME_JOINT_POSITIONS):

qmeasured=[0,−0.35,0,−1.10,0,0.35,0]

输入增量为:

Δq=[0.05,0,−0.02,0,0,0.03,0]

那么目标就是:

qgoal=[0.05,−0.35,−0.02,−1.10,0,0.38,0]

所有数字均为弧度;0.05 rad 约为 2.86°,不是 0.05°。这组数字只用于解释加法,不能据此判断末端是否避开了桌子。关节目标不越界,也不代表中间路径一定没有碰撞。(顺带核对限位:第五章 J6 的范围是 [−1.0472,1.0472],0.38 在范围内,因此不会触发越界报错。)

这和绝对关节命令不同。绝对命令“去 0.25 rad”重复执行,目标仍是 0.25 rad;相对命令“再增加 0.05 rad”重复执行,会继续累加。把绝对关节角误当作增量,是接入后续策略时很常见的错误。

本章的任务目标是固定的世界末端位姿,而不是人工手写每一段七关节角。因此任务调用该模式时,存在一层适配:

固定世界位姿 → 转到机械臂基座坐标系 → 第五章 IK → 目标关节角 → 减去当前关节角 → 执行关节增量。

所以,需要同时理解两句话:

  • relative_joint() 这个低层接口本身不需要 IK。
  • 本章通过 move_world() 执行固定抓放位姿时,会先用 IK 构造该接口需要的增量。

轨迹:三次时间缩放。 得到目标关节角后,轨迹沿用第五章的三次时间缩放。将归一化时间记为 τ=t/T( t 为本段已用时间, T 为本段时长,单位 s),则:

s(τ)=3τ2−2τ3,q(τ)=q0+s(τ)(q1−q0)

其中 q0 是起点关节角, q1 是目标关节角, s(τ)∈[0,1] 是“完成进度”。对 τ 求导得 s′(τ)=6τ−6τ2,在 τ=0 和 τ=1 处都为零。直观上,它让关节从静止平滑启动,再平滑停到目标,而不是一下把整个角度差作为阶跃命令发给关节。第五章生成器还会按关节位移和速度限制延长过短的时间,但这不等于工业级加速度、jerk 和碰撞约束规划。

对应代码(code/code_chapter5/arm_controller.py,第五章轨迹生成器):

python
def make_trajectory(self, target_positions, duration, dt, start_positions=None):
    """生成三次时间缩放轨迹 q=q0+(3t²-2t³)(q1-q0)。"""
    # ...
    start = self.get_joint_positions() if start_positions is None else self._check_positions(start_positions)
    target = self.clamp_positions(target_positions)

    # 自动保证平均速度不超过 URDF 速度上限的 60%,给物理 drive 留出余量。
    minimum_duration = float(np.max(np.abs(target - start) / (0.6 * self.velocity_limits)))
    duration = max(float(duration), minimum_duration)       # 只延长,不缩短
    steps = max(2, int(np.ceil(duration / dt)))              # 物理步数
    tau = np.linspace(0.0, 1.0, steps + 1)[1:]               # 去掉 τ=0(起点不必再发)
    scale = 3.0 * tau**2 - 2.0 * tau**3                      # s(τ)
    return start[None, :] + scale[:, None] * (target - start)[None, :]

代码解读:

  • minimum_duration 对应“限速延长”:第五章 JOINT_VELOCITY_LIMITS 为每关节 3.1416 rad/s,60% 即约 1.885 rad/s。若某关节要转 1.10 rad,最短时长为 1.10/1.885≈0.584 s,小于默认 2.5 s,所以不会延长;只有大角度或很短的请求时长才会被拉长。
  • steps 是按物理步离散:默认 120 Hz 下 dt≈8.33 ms,2.5 s 约为 300 个物理步,每一步发送一个绝对关节位置目标。

案例一:J1“再转 0.05 rad”,这一秒中到底发生什么?

为了看清一条指令的含义,暂时只看 J1:假设它当前实测为 0.20 rad,其余关节的增量都为零,请求动作时间为 1 s。目标角就是 0.25 rad。不考虑限速延长( 0.05/1.885≈0.027 s≪1 s,确实不会延长),用上面的时间缩放计算,可得到:

轨迹时间时间比例 τ进度 s(τ)此时下发的 J1 目标角
起点参考000.2000000 rad
0.25 s0.250.156250.2078125 rad
0.50 s0.500.50.2250000 rad
0.75 s0.750.843750.2421875 rad
轨迹终点110.2500000 rad

以 τ=0.25 为例验算: s=3×0.0625−2×0.015625=0.1875−0.03125=0.15625,目标角 0.20+0.15625×0.05=0.2078125。这是公式上的目标采样示例,不是实测记录;实现中的物理步取样未必恰好落在表中每个时刻。表格也不表示“末端走了全程的同样比例”,因为关节角与末端位置不是简单的线性关系。

再假设执行后实测 J1 为 0.247 rad,而不是恰好 0.250 rad。此时再调用一次相同的 +0.05 rad,新目标是 0.297 rad,不是 0.300 rad。下一次的起点看实际反馈,不看上一次希望达到哪里。 如果想准确回到最初的关节姿态,应保存原来的关节向量,并用“原来角度减当前实测角度”重新构造增量;简单把原始增量取负,只能近似撤销上一次动作。

对应代码(code/code_chapter6/control.py):

python
def vector(values, size, name):
    result = np.asarray(values, dtype=float)
    if result.shape != (size,) or not np.all(np.isfinite(result)):
        raise ValueError(f"{name} 必须是 {size} 个有限数")
    return result

def relative_joint(self, delta, duration=None):
    """q_goal = q_measured + dq,再由第五章三次时间缩放执行。"""
    dq = vector(delta, 7, "dq")                        # ① 7 个有限数
    goal = self.arm.get_joint_positions() + dq         # ② 以“此刻实测”为参考
    self._check_joint_goal(goal)                       #    越界直接报错
    duration = self.config.arm_duration if duration is None else duration
    if not np.isfinite(duration) or duration <= 0:     # ③ 时长合法,默认 2.5 s
        raise ValueError("duration 必须为有限正数")
    trajectory = self.arm.make_trajectory(goal, duration, self.config.dt)
    for target in trajectory:                          # ④ 每个物理步发绝对目标
        self.target_q = target.copy()
        self.arm.command_positions(target)
        self.step()
    self.hold(.3)                                      # ⑤ 再推进 0.3 s 让驱动收敛
    self._check_tracking(goal)                         #    最大误差 ≤ 0.06 rad
    return self.arm.get_joint_positions()

def _check_joint_goal(self, goal):
    if np.any(goal < self.arm.lower_limits-1e-8) or np.any(goal > self.arm.upper_limits+1e-8):
        raise ValueError("关节目标越界;不静默裁剪成另一个动作")

def _check_tracking(self, goal):
    error = float(np.max(np.abs(self.arm.get_joint_positions()-goal)))
    if not np.isfinite(error) or error > .06:
        raise RuntimeError(f"关节跟踪误差过大:{error:.3f} rad")

结合代码,一次 relative_joint() 调用可以按以下顺序理解:

  1. 检查输入必须是七个有限数,不能混入 NaN,也不能少写一个关节。
  2. 读取实际关节角,加上增量,并检查目标是否超出关节限位。越界时报错,不把动作偷偷截短。
  3. 检查动作时间为有限正数;没有指定时使用 arm_duration,默认 2.5 s。
  4. 调用第五章的 make_trajectory(),逐个物理步发送绝对关节位置目标。高层输入是相对增量,不代表底层驱动也接收增量。
  5. 轨迹结束后继续推进 0.3 s,检查七个关节中最大的实际跟踪误差是否超过 0.06 rad(约 3.44°),通过后返回实际关节角。

因此,这是一个“调用后执行完这一段才返回”的接口,不是每帧都要重复发送一次的速度指令。duration=1.0 表示请求用约一秒完成这一段,不是让关节以每秒一个增量永远运动;限速延长与末尾等待还会增加实际耗时。

⚠️ 注意:第五章的 command_positions() 本身会把目标 clip 到限位内,但第六章在之前就用 _check_joint_goal() 拒绝越界目标——宁可报错,也不让“裁剪后的另一个动作”被悄悄执行。

要注意,关节平滑不等于末端走直线。即使七个关节都按同一个时间比例变化,末端也可能画出一条弧线。task.py 中“下探”“抬升”描述的是目标方向,在关节模式下不能把它们理解为严格的竖直直线轨迹。

2.3 末端增量:先规定手怎么走,再求关节怎么配合 ​

末端也叫 end effector,简称 EE。本章使用 gripper_r_center_link 作为右夹爪的 TCP(Tool Center Point,工具中心点)。这个点的位置与朝向共同构成末端位姿。

delta_ee 输入两部分:

  • Δp:三个平移分量(m)。
  • Δr:三个旋转向量分量。方向表示旋转轴,向量长度表示旋转角,单位为弧度。

例如,dp=[0.02, 0, 0] 表示沿机械臂基座 x 轴移动 2 cm;dp=[0, 0, 0] 表示本次不改变目标位置。这里的 0.02 是米,不是毫米,也不是速度。三个平移数字不包含夹爪开合信息。

旋转输入可以先从单轴例子理解:dr=[0, 0, 0.10] 表示绕机械臂基座 z 轴旋转 0.10 rad,约 5.73°。若 dr=[0.03, 0.04, 0],则旋转角是向量长度 0.05 rad,旋转轴方向是 [0.6, 0.8, 0],不是“先绕 x 转一次,再绕 y 转一次”。这只说明旋转表示法,并不保证当前姿态下这样的目标必然可达。

旋转向量不是三个可以直接相加的欧拉角,也不是四元数。 小转角时它很直观,但组合旋转仍然必须按旋转矩阵处理。

原理与公式:本章约定平移和旋转轴均表达在 arm_base_link 坐标系,而不是当前夹爪的局部坐标系。于是:

pgoal=p0+ΔpRgoal=Exp(Δr^)R0

这里 p0、 R0 是调用时由 FK 算出的末端位置和旋转矩阵, Exp(Δr^) 表示由旋转向量得到的旋转矩阵(代码中用 Rodrigues 公式实现)。公式中左乘当前姿态,是为了表达“绕机械臂基座坐标系中的轴旋转”。如果改成右乘,就改变了旋转的参考坐标系,不能随意交换。

不熟悉矩阵指数也没有关系,可以先记住三个操作规则:平移写米,旋转写弧度,增量方向看机械臂基座坐标系。 dr 全为零表示保持当前朝向,不是把夹爪姿态恢复成某个“零姿态”。

执行时并不是只对最终末端目标做一次 IK。本章先在起点和终点之间取一串位姿:

p(α)=p0+αΔp,R(α)=Exp(αΔr^)R0,0≤α≤1

分段数量由平移和旋转共同决定,默认每段名义平移不超过约 12 mm、旋转不超过约 0.08 rad。每个中间位姿用第五章的 IK 求解,并把上一点的解作为下一点的初值,尽量保持解的连续性。

把分段规则写成公式更容易核对实现。记段数为 N,默认参数下:

N=max(1,⌈∥Δp∥0.012⌉,⌈∥Δr∥0.08⌉)

其中 ⌈⋅⌉ 表示向上取整, 0.012 来自 TaskConfig.cartesian_step, 0.08 写在 delta_ee() 中。例如平移 0.06 m、不旋转,就分成 5 段;每个中间点都保持原来的末端朝向,位置沿期望方向增加约 0.012 m。若同时旋转 0.48 rad,旋转约束要求 6 段,便按 6 段同时插值位置与姿态。这里按平移向量的总长度分段,不是分别对 x、y、z 各执行一遍。

对应代码(code/code_chapter6/control.py):

python
def rotation_increment(rotvec):
    dr = vector(rotvec, 3, "dr")
    angle = float(np.linalg.norm(dr))            # 旋转角 = 向量长度
    # 角度为 0 时返回单位阵;否则 Exp(dr^) = 绕单位轴 dr/angle 转 angle(Rodrigues)
    return np.eye(3) if angle < 1e-12 else axis_rotation(dr/angle, angle)[:3, :3]

def delta_ee(self, dp, dr=(0., 0., 0.), duration=None):
    """直线位置插值 + 旋转向量插值;预检整段 IK 后才下发关节轨迹。"""
    dp, dr = vector(dp, 3, "dp"), vector(dr, 3, "dr")
    start = self.arm.forward_kinematics()        # p0, R0(arm_base_link 下)
    count = max(1, math.ceil(np.linalg.norm(dp)/self.config.cartesian_step),
                math.ceil(np.linalg.norm(dr)/.08))           # 段数 N
    q = self.arm.get_joint_positions()           # 第一个 IK 的初值 = 实测关节
    goals = []
    for alpha in np.linspace(0., 1., count+1)[1:]:           # α = 1/N, 2/N, …, 1
        q = self.solve(start.position+alpha*dp,              # p(α)
                       rotation_increment(alpha*dr) @ start.rotation, q)  # R(α),左乘
        self._check_joint_goal(q)
        goals.append(q)                          # 只存入内存,尚未运动
    duration = self.config.arm_duration if duration is None else duration
    if not np.isfinite(duration) or duration <= 0:
        raise ValueError("duration 必须为有限正数")
    # 相邻小段共用第五章轨迹生成器,不调用外部笛卡尔控制器。
    for q in goals:
        trajectory = self.arm.make_trajectory(q, duration/count, self.config.dt)
        for target in trajectory:
            self.target_q = target.copy()
            self.arm.command_positions(target)
            self.step()
    self.hold(.3)
    self._check_tracking(goals[-1])
    return self.arm.forward_kinematics()

def solve(self, position, rotation, seed=None):
    result = self.arm.solve_ik(position, rotation, seed)
    if not result.success:
        raise RuntimeError(f"第五章 IK 未收敛:位置误差 {result.position_error:.4f} m,"
                           f"姿态误差 {result.orientation_error:.4f} rad")
    return result.joint_positions

从代码角度看,一次 delta_ee() 调用包含以下步骤:

  1. 检查 dp、dr 各有三个有限数,通过实际关节角做 FK,取得当前末端位置与旋转。
  2. 计算段数,为每个中间位置和朝向调用第五章 IK。第一点以当前关节状态为初值,后续点以上一点的解为初值。
  3. 检查每个 IK 是否成功、关节解是否越界,并保存本段全部中间关节目标。这里是在内存里预检,还没有执行机械臂动作。
  4. 检查请求时间;默认总时间为 2.5 s。将请求时间按段数分配,再为每一小段生成并执行关节轨迹;单段仍可能因为关节限速而延长。
  5. 执行后等待 0.3 s,检查末段目标的关节跟踪误差,再返回 FK 末端位姿。

七关节机械臂完成六维末端目标时,通常不只有一种关节配置。沿用上一个解作为下一次初值有助于解的连续性,但并不等于实现了全局最优姿态选择、奇异点自动规避或碰撞规划。如果中间点求解失败,应先检查坐标、目标、姿态和路径,而不是直接跳过该点。

整段的中间 IK 都成功,程序才开始下发动作。 如果走到第十个预检点发现不可达,就在本段执行前报错,而不是先走九步再卡住。这里预检的是当前这一段,不是一次性保证整个任务都可达,也没有做整条机械臂碰撞检查。

执行阶段仍通过第五章生成短关节轨迹。因此更准确的描述是“末端直线的离散近似”,而不是连续时间中精确的笛卡尔直线。小段之间会有减速和启动,不应把这个简单教学实现称为具有全局连续速度约束的高级笛卡尔规划器。

案例二:让夹爪在基座系里“前进 3 cm、侧移 −2 cm、再移 1 cm”。

假设 FK 读到当前末端位置为 p0=(0.40,0.10,0.20),位置单位是米,坐标表达在 arm_base_link 中。现在输入:

Δp=(0.03,−0.02,0.01),Δr=(0,0,0)

目标位置便是 p1=(0.43,0.08,0.21),目标朝向与本次调用起点相同。这里的数字仅用于解释算法,不是已经验证可达的 G2 抓取点,不能当作默认任务坐标直接使用。

平移总长度为:

∥Δp∥=0.032+(−0.02)2+0.012≈0.03742m

按默认每段 0.012 m 的规则, 0.03742/0.012≈3.12,向上取整得到 4 段。于是先生成下表中的位姿,再为每一行求 IK:

取样点α基座系中的末端目标位置(m)末端朝向
第 1 点0.25(0.4075, 0.0950, 0.2025)与起点相同
第 2 点0.50(0.4150, 0.0900, 0.2050)与起点相同
第 3 点0.75(0.4225, 0.0850, 0.2075)与起点相同
第 4 点1.00(0.4300, 0.0800, 0.2100)与起点相同

每一小段的平移长度约为 9.35 mm( 37.42/4)。若请求时间为 2 s,每小段先分配 0.5 s,再由第五章轨迹生成器检查是否需要延长。必须四个 IK 都成功,才开始执行第一个小段。 假如第三点不可达,这一次 delta_ee() 调用会在开始下发轨迹前失败,不会先移动到第二点再让机器人悬在那里。

这里没有人手工填写七个关节的增量,IK 会为每个末端目标求出相应关节角。dr=0 也不表示手腕关节都不动:为了在平移时维持夹爪朝向,多个关节可能需要一起调整。

两种模式数值例子对比

图 6-6 两种动作模式的数值对比:左为案例一中 J1 的三次时间缩放目标曲线(与线性插值对照),右为案例二中 delta_ee 的 4 个末端取样点(x–y 投影)

案例三(补充):默认任务下探一段,delta_ee 每小段多长时间? 完整默认任务从 z=1.08 下探到 z=0.92,共 0.16 m。理想起点、无旋转修正时 N=⌈0.16/0.012⌉=⌈13.33⌉=14。默认总时长 2.5 s,每段分到 2.5/14≈0.179 s,在 120 Hz 下为 ⌈0.179/0.00833⌉=22 个物理步。再加上 14 次 IK 预检和末尾 0.3 s 等待,这一段的仿真耗时会略长于 2.5 s——这就是“末端增量模式还包含分段求解、分段执行和等待,不能要求整个程序在一段配置时长内完成”的具体含义。

💡 要点:relative_joint 的“直线”在关节空间,delta_ee 的“直线”在末端空间(且是离散近似)。两者最后都只给仿真发送关节位置目标。

2.4 同一个“向下 2 cm”,两种模式实际做了什么 ​

同一个“向下 2 cm”,两种模式实际做了什么? 假设底盘已经停稳,夹爪位于红块上方,希望保持世界朝向并降低 TCP 高度。先在世界中把目标写成当前 TCP 位置加 [0, 0, -0.02],然后交给 move_world():

执行环节选择 relative_joint选择 delta_ee
世界目标转换将目标位置、朝向转到机械臂基座系相同
构造动作对终点求 IK,计算目标关节角减当前角度计算基座系中的位置差与旋转差
中间路径在起终关节角之间插值在起终末端位姿之间取样,逐点求 IK
下探过程中保持朝向终点有朝向要求,中间朝向没有额外约束每个取样位姿都带有朝向要求,但执行仍存在离散与跟踪误差
末端经过哪里通常弯曲,不能只根据终点判断途中净空更接近期望的下探直线,也不自动避开障碍
到位后检查检查实际世界 TCP 位置和朝向相同

对应代码(code/code_chapter6/control.py):

python
def move_world(self, position, rotation, mode=None):
    """方便任务调用的适配层:固定世界目标 -> 两种增量动作。"""
    position = vector(position, 3, "world_position")
    p, r = self.env.world_to_arm(position, rotation)     # 世界 → arm_base_link(第三部分)
    mode = self.config.mode if mode is None else mode
    if mode == "relative_joint":
        q = self.solve(p, r)                              # 终点 IK
        self.relative_joint(q-self.arm.get_joint_positions())   # dq = q_goal - q_now
    elif mode == "delta_ee":
        current = self.arm.forward_kinematics()
        self.delta_ee(p-current.position, orientation_error(r, current.rotation))
    else:
        raise ValueError("未知动作模式")
    # 无论哪种模式,最终都在“世界系”里用实测 TCP 验收
    actual_p, actual_r = self.env.tcp_pose()
    error = float(np.linalg.norm(actual_p-position))
    angle = float(np.linalg.norm(orientation_error(rotation, actual_r)))
    print(f"  TCP 误差 {error*1000:.1f} mm / {angle:.3f} rad", flush=True)
    if error > .012 or angle > .06:
        raise RuntimeError("实测 TCP 未到达;检查坐标系、底盘漂移或关节跟踪")

在 move_world() 的末端增量分支中,平移差为 Δp=ptarget−pcurrent;旋转差由 orientation_error() 根据目标旋转与当前旋转计算,不是将两个欧拉角数组直接相减。若目标朝向与当前实际朝向之间存在微小误差,计算出的 dr 也可能非零,因此“任务期望保持向下”并不代表每次实际传入的 dr 都严格为零。

orientation_error(target, current) 来自第五章(code/code_chapter5/kinematics.py):它先算相对旋转 Rrel=RtargetRcurrentT,再转成四元数并取出“轴 × 角”的旋转向量。由于 Rtarget=RrelRcurrent,这个旋转向量正好就是 delta_ee 需要的、左乘在当前姿态上的 Δr,两处约定是一致的。

案例四:把这个区别放回红块抓取任务。 假设预抓取已经理想到位,TCP 世界位置为 (1.27, -0.30, 1.08),朝向也已调整为向下。现在只做一个高处练习:保持朝向,降低 2 cm,到 (1.27, -0.30, 1.06),还不接触红块。

  • 关节模式:对终点位姿做 IK,得到七个目标角,再减去当前七个实际角度。执行时沿关节空间插值。我们知道终点应低 2 cm,但中途是否偏向侧面、朝向是否有变化,必须通过 FK 或实际状态检查,不能只看终点就断言路径是竖直的。
  • 末端模式:先把这段世界位移转换为基座系位移。理想状态下旋转差为零,平移长度仍为 0.02 m,所以会分成两段( ⌈0.02/0.012⌉=2),中间世界目标对应 z=1.07 和 z=1.06。在每个点上都要求保持指定朝向,再分别求 IK;点间由短关节轨迹执行。

实际运行时会有起点和朝向误差,段数以当次计算为准。两段取样的说明基于上面的理想起点,不是保证每次运行日志都恰好产生两个点。第七部分的独立接口示例可分别选择这两种模式来做此练习,每轮重新创建环境,避免把不同起点的结果拿来直接比较。

完整默认任务不是只下探 2 cm,而是从 z=1.08 下探到 z=0.92,共 16 cm。若起点理想到位且不需要旋转修正,末端模式会预检 ⌈0.16/0.012⌉=14 段;关节模式则对该阶段终点求 IK,再生成一段关节空间轨迹。这正是两种模式在同一个抓取任务中的实际区别。

2.5 如何选择模式,以及为什么准备阶段固定用关节轨迹 ​

如何选择? 已经知道一组关节姿态、想收臂或进行关节教学时,relative_joint 更直接;希望表达接近、下探、抬升等末端运动意图时,delta_ee 更直观,但需要求解多个中间 IK。本章保留两种模式,是让读者比较“同一个终点,采用不同的路径生成方式”,不是宣称某一种模式对所有任务都更好。

还要分清“重复增量”和“重复目标”:直接重复 relative_joint(dq) 或 delta_ee(dp, dr),会从新的状态继续增加动作;重复 move_world() 的同一个固定世界目标,则会重新计算剩余误差,不是要求机械臂沿原来的方向再走同样远。

另外,选择 --mode delta_ee 并不意味着启动后的每一瞬间都必须走末端直线。当前任务中,两种模式都先用关节轨迹完成 HOME 准备和向下预抓取姿态,然后才在下探、抬升、搬运、下放和退出阶段按所选模式执行。

原因是:起点和终点分别可达,不代表把它们用末端直线连起来之后,每个中间姿态也可达。 初始机械臂朝向与向下抓取朝向相差较大,强行用一条同时平移、旋转的直线进入桌面区域,反而可能遇到不可达点。这里是明确设计的分阶段动作,不是 IK 失败后偷偷切换模式。

阶段--mode relative_joint--mode delta_ee
prepare(回 HOME)relative_jointrelative_joint
pregrasp(到红块上方并转为向下)relative_jointrelative_joint(代码中显式指定)
descend / lift / transfer / place / retreatrelative_jointdelta_ee
close / release夹爪接口,与模式无关夹爪接口,与模式无关

⚠️ 注意:大姿态变化(比如从水平伸展转成竖直向下)若用 delta_ee, ∥Δr∥ 很大,段数由旋转决定,并且每个中间姿态都必须可解;这类“换姿态”动作交给关节插值更稳妥。


3.1 三个坐标系各管什么 ​

直观理解:同一个红块,“在房间里的位置”“相对机器人肩膀的位置”“相对手掌的位置”是三组不同的数字。本章涉及三个(加上底盘共四个)参考系,各有分工:

坐标系本章中的 prim / 来源用来做什么典型量
世界系 W仿真世界原点,z 轴向上摆放桌子、方块、框;写任务目标;最终验收CUBE_POSITION、BOX_CENTER、tcp_pose()
底盘系/genie/chassis_link底盘平面导航的 x/y/yawbase_pose()
机械臂基座系 A/genie/arm_base_link第五章 FK/IK 在这里建立;delta_ee 的 dp/dr 也在这里表达forward_kinematics()、world_to_arm() 输出
夹爪 / TCP 系 T/genie/gripper_r_center_link描述手的位置与朝向;随夹爪一起转动GRASP_ROTATION 的三列

世界坐标系适合摆放桌子、方块和接收框;机械臂运动学却是在 arm_base_link 下建立的。底盘移动以后,这两个坐标系之间的相对位置会改变。世界中同一个红块,在移动前后相对于机械臂的位置当然不同。

图6-4 世界向下两厘米的目标如何转换到机械臂基座系,以及工具坐标系为何不能混用

图 6-7 世界向下两厘米的目标如何转换到机械臂基座系,以及工具坐标系为何不能混用

图 6-7 怎么看: 世界坐标用于描述“到桌上哪个点”,基座坐标用于计算机械臂运动学,工具坐标跟随夹爪旋转。图中不假设基座轴恰好和世界轴平行,实际转换必须读取运行时位姿。

原理与公式:设机械臂基座在世界中的位置和旋转分别为 WpA、 WRA(左上标表示“在哪个系中表达”),目标 TCP 的世界位姿为 WpT、 WRT,转换到机械臂基座坐标系是:

ApT=(WRA)T(WpT−WpA)ART=(WRA)TWRT

可以把它理解成:先减去机械臂基座的位置,把原点搬过去;再旋转坐标轴,让目标用机械臂的方向来描述。旋转矩阵是正交矩阵,转置就是逆,所以 (WRA)T 就是“从世界方向转回基座方向”。environment.py 的 world_to_arm() 就负责这一步。

对应代码(code/code_chapter6/environment.py):

python
def world_to_arm(self, position, rotation):
    p, q = self.arm_base.get_world_pose()        # 运行时读取 arm_base_link 的世界位姿,q 为 [w,x,y,z]
    r = quaternion_to_matrix(q)                  # W_R_A
    return r.T @ (np.asarray(position)-p), r.T @ rotation   # A_p_T, A_R_T

def tcp_pose(self):
    p, q = self.tcp.get_world_pose()             # gripper_r_center_link 的世界位姿
    return np.asarray(p, dtype=float), quaternion_to_matrix(q)

其中 self.arm_base = SingleXFormPrim("/genie/arm_base_link"),self.tcp = SingleXFormPrim("/genie/gripper_r_center_link"),都在 _build() 里创建。注意 world_to_arm() 每次调用都重新读取基座位姿,所以底盘停靠位置有几毫米偏差、或者抓放中底盘轻微漂移,都会自动反映在转换结果里——这是“固定世界目标 + 运行时转换”比“预先写死基座系目标”更稳健的原因。

点与位移的区别。 上面的公式用于点(位置)。如果要转换的是一个位移或方向(比如“向下 2 cm”),它与原点无关,只需要旋转: Av=(WRA)TWv。

本模型的 arm_base_link 轴向并不能直接当作世界轴向使用。世界向上移动两厘米,不应不加转换地写成 delta_ee 的第三个分量增加 0.02。 想在世界中上下移动时,先构造世界目标,再通过 move_world() 转换最清楚。第七部分给出对应调用示例。

案例:一个假设性的数值转换。 为了看清代数,假设(仅为演示,不是 G2 的真实值)基座位于 WpA=(0.80,−0.10,1.00),且基座系相对世界绕 x 轴转了 90∘,即基座三根轴在世界中的方向为 xA=(1,0,0)、 yA=(0,0,1)、 zA=(0,−1,0)(这三根轴就是 WRA 的三列)。

  1. 预抓取点 WpT=(1.27,−0.30,1.08)。先减原点: d=(0.47,−0.20,0.08)。再旋转: ApT=(xA⋅d, yA⋅d, zA⋅d)=(0.47, 0.08, 0.20)。
  2. 世界“向下 2 cm”位移 Wv=(0,0,−0.02): Av=(xA⋅v, yA⋅v, zA⋅v)=(0, −0.02, 0)。所以正确的 dp 是 (0, -0.02, 0)。
  3. 如果错误地写 dp=(0, 0, -0.02),它在世界中的位移是 −0.02zA=(0, 0.02, 0)——本想下探,手却沿世界 +y 侧移了 2 cm。

坐标系数值例子

图 6-8 左:GRASP_ROTATION 每一列是 TCP 的一根轴在世界系中的方向;右:假设性数值演示点与位移的坐标转换,以及把世界“向下”直接写进 dp 的后果

⚠️ 注意:delta_ee 的 dp、dr 既不是世界系,也不是夹爪自己的局部系,而是 arm_base_link。control.py 的模块注释写明:“dp 与旋转向量 dr 均表达在 arm_base_link 坐标系,不是工具自身坐标系;旋转左乘当前姿态。”

3.3 夹爪系:用完整旋转矩阵固定抓放姿态 ​

直观理解:只说“手到方块中心”是不够的,手掌可能侧着、斜着、甚至翻过来。本章用一个完整的 3×3 旋转矩阵 GRASP_ROTATION 规定夹爪在世界中的朝向。

对应代码(code/code_chapter6/config.py):

python
# TCP 的 z 轴朝下、x 轴沿世界 y:夹爪沿 y 方向夹紧。
GRASP_ROTATION = np.array([[0., 1., 0.], [1., 0., 0.], [0., 0., -1.]])

怎样读一个旋转矩阵: WRT 的第 i 列,就是 TCP 系第 i 根轴在世界系中的方向。

列数值含义
第 1 列(0,1,0)TCP x 轴指向世界 +y
第 2 列(1,0,0)TCP y 轴指向世界 +x
第 3 列(0,0,−1)TCP z 轴指向世界 −z,即朝下

验算:三列两两正交、长度为 1;行列式 detR=0⋅(0⋅(−1)−0⋅0)−1⋅(1⋅(−1)−0⋅0)+0=1,是合法的旋转(右手系),不是镜像。

抓放姿态用完整旋转矩阵固定,夹爪 TCP 的 z 轴朝世界下方,x 轴沿世界 y 方向。这样不只是“手到了方块中心”,还保证夹爪以正确朝向接近。仅约束位置的 IK 可能把手腕翻到不适合夹取的姿态,因此本任务使用同时约束位置与朝向的 IK。所有六个抓放阶段(pregrasp 到 retreat)都使用同一个 GRASP_ROTATION,所以在理想情况下这些阶段之间 Δr≈0,delta_ee 只在做平移。

move_world() 的验收也在世界系中进行:位置误差用 ∥Wpactual−Wptarget∥≤0.012 m,姿态误差用 orientation_error(rotation, actual_r) 的模长 ≤0.06 rad。这一步直接比较“想去的世界位姿”和“实测的世界位姿”,因此只要坐标系某处用错(比如把 articulation 根当成基座),就会在这里暴露出来,而不是悄悄报告成功。

💡 要点:任务层只写世界目标;转换交给 world_to_arm();验收回到世界系。这条“世界 → 基座 → 世界”的闭环,是本章不把坐标系弄混的关键。


第四部分 底盘停靠:怎样到达固定点并停稳 ​

4.1 位姿比例控制:误差越小速度越小 ​

直观理解:开车靠边停车时,离车位越近踩油门越轻。底盘目标用三个数描述:平面横坐标、纵坐标和偏航角。第四章的 PoseController 计算世界平面位置误差,再根据当前偏航把误差转换到机体坐标系,输出前后速度、左右速度和转动速度。

原理与公式:记目标为 (x∗,y∗,ψ∗),当前为 (x,y,ψ),世界系误差 exW=x∗−x, eyW=y∗−y,偏航误差 eψ=wrap(ψ∗−ψ)(归一化到 [−π,π])。转换到机体系:

[exBeyB]=[cos⁡ψsin⁡ψ−sin⁡ψcos⁡ψ][exWeyW]

如果位置误差的机体分量为 exB,eyB,偏航误差为 eψ,可以直观理解为:

vx∝exB,vy∝eyB,ωz∝eψ

在第四章实现中,比例系数默认为位置增益 kp=1.2、偏航增益 kψ=2.0,并对线速度合成长度 vx2+vy2 和角速度分别限幅:

vx=kpexB,vy=kpeyB,ωz=kψeψ,vx2+vy2≤vmax,|ωz|≤ωmax

随后由第四章的四轮独立转向运动学计算车轮转向角和驱动速度,再经过限速、加速度约束和指令超时保护。第六章不使用差速底盘公式替代 G2 的实际四轮转向结构。

本章的位姿控制器把最大平移速度设置为 0.22 m/s、最大偏航速度设置为 0.30 rad/s,到达容差为 6 mm 和 0.012 rad。

对应代码(code/code_chapter4/base_controller.py,第四章):

python
def compute(self, current: Pose2D, target: Pose2D) -> PoseControlResult:
    error_world_x = target.x - current.x
    error_world_y = target.y - current.y
    distance_error = math.hypot(error_world_x, error_world_y)
    yaw_error = normalize_angle(target.yaw - current.yaw)

    reached = (
        distance_error <= self.position_tolerance
        and abs(yaw_error) <= self.yaw_tolerance
    )
    if reached:
        return PoseControlResult(ChassisVelocity(), distance_error, yaw_error, True)

    cos_yaw = math.cos(current.yaw)
    sin_yaw = math.sin(current.yaw)
    error_body_x = cos_yaw * error_world_x + sin_yaw * error_world_y    # 世界 → 机体
    error_body_y = -sin_yaw * error_world_x + cos_yaw * error_world_y
    # ...
    vx = self.position_gain * error_body_x
    vy = self.position_gain * error_body_y
    command = _limit_linear_speed(
        ChassisVelocity(vx, vy, self.yaw_gain * yaw_error),
        self.max_linear_speed,                                   # 第六章传入 0.22
    )
    # ... 角速度限幅到 ±max_angular_speed(第六章传入 0.30)
    return PoseControlResult(command, distance_error, yaw_error, False)

案例:从第一航点驶向最终航点。 底盘在 (0.35, 0, 0),目标 (0.70, 0, 0),偏航为零。

  • 起步时 d=0.35, kpd=0.42 m/s>0.22,被限为 0.22 m/s。
  • 速度从限幅段转入比例段的临界距离是 0.22/1.2≈0.183 m。
  • 剩 d=0.10 时, v=0.12 m/s;剩 d=0.01 时, v=0.012 m/s。
  • 进入 d≤0.006 且 |eψ|≤0.012 时,reached=True,速度命令为零。
  • 偏航方面, kψeψ 在 |eψ|>0.15 rad 时会被 0.30 rad/s 限幅。

底盘停靠速度曲线

图 6-9 第一航点到最终航点的速度命令随剩余距离变化:远处被 0.22 m/s 限幅,0.183 m 以内与距离成正比,6 mm 容差内宣布到达(按公式绘制,非实测)

💡 要点:纯比例控制越接近目标速度越小,理论上要无限时间才能“正好到达”,因此必须有到达容差。容差太大停不准,太小可能因噪声和摩擦永远进不去——第六章把第四章默认的 0.04 m / 0.05 rad 收紧到 0.006 m / 0.012 rad,因为抓取对停车精度要求更高。

4.2 navigate():停车复核与超时 ​

每个航点有控制循环上限;停止后额外等待 0.5 s 再复核,以免刚经过容差区域就把“路过目标”误判为“已经停好”。配置的 25 s 用于限制航点控制循环次数,并不是严格的墙钟计时器,停车复核的等待还会推进仿真。

对应代码(code/code_chapter6/control.py):

python
def navigate(self, target):
    self.set_phase(f"navigate({target.x:.2f}, {target.y:.2f}, {target.yaw:.2f})")
    try:
        # 循环上限 = ceil(25 / dt) = 3000 次(120 Hz)
        for _ in range(math.ceil(self.config.waypoint_timeout/self.config.dt)):
            result = self.pose_controller.compute(self.env.base_pose(), target)
            if result.reached:
                self.base.stop()
                self.hold(.5)
                # 停车惯性后再检查,不能刚经过容差区就认为成功。
                if self.pose_controller.compute(self.env.base_pose(), target).reached:
                    return
                # 复核未通过:不返回,继续下一轮控制
            command = result.command
            self.base.set_velocity(command.vx, command.vy, command.wz)
            self.base.update(self.config.dt)    # 第四章:限速、加速度约束、四轮逆运动学
            self.step()
        raise RuntimeError(f"固定航点超时:{target};实际 {self.env.base_pose()}")
    finally:
        self.base.stop()                        # 无论成功失败都刹车

代码解读:

  • base.set_velocity() 只记录期望速度,base.update(dt) 才真正施加第四章的线加速度 0.80 m/s²、角加速度 1.80 rad/s² 约束并写入车轮关节。
  • 复核失败(停车后惯性把底盘带出容差区)时,循环继续用新的误差控制回来。由于每次复核都推进了 0.5 s 仿真,但只消耗一次循环计数,“25 s”不是严格的仿真时间上限。
  • finally 中的 stop() 保证即使抛异常也会刹车。

4.3 为什么不能读错机器人根坐标 ​

这里有一个重要的模型细节:G2 的 articulation 根位姿不能直接视为底盘位姿。 本模型根位姿位于躯干一侧的运动链参考位置,若直接用 robot.get_world_pose() 作为底盘坐标,原点和轴向可能都不符合平面导航的要求。

第六章明确读取 /genie/chassis_link 作为底盘参考,读取 /genie/arm_base_link 做机械臂坐标变换。把这两个用途分开,比为了“代码少一行”而混用机器人根坐标更重要。

对应代码(code/code_chapter6/environment.py):

python
def base_pose(self):
    p, q = self.chassis.get_world_pose()          # /genie/chassis_link,q = [w, x, y, z]
    w, x, y, z = q
    # 四元数 → 偏航角(绕世界 z 轴)
    return Pose2D(float(p[0]), float(p[1]), math.atan2(2*(w*z+x*y), 1-2*(y*y+z*z)))

案例:偏航公式验算。 若底盘绕 z 轴转了 ψ,对应四元数 q=[cos⁡(ψ/2),0,0,sin⁡(ψ/2)]。代入:分子 2wz=2cos⁡(ψ/2)sin⁡(ψ/2)=sin⁡ψ,分母 1−2z2=1−2sin2⁡(ψ/2)=cos⁡ψ,于是 atan2(sin⁡ψ,cos⁡ψ)=ψ。比如 ψ=0.3 时 q≈[0.9888,0,0,0.1494],算回 ≈0.300 rad。

⚠️ 注意:航点超时时,优先检查是否改了地面、航点、车轮参数,以及是否读对了 chassis_link。不要靠传送底盘掩盖控制问题。


第五部分 夹爪接触抓取与成功判定 ​

5.1 抓取与释放的目标高度 ​

直观理解:抓取点和放置点都由几何推出,而且“释放时手的位置”和“方块最后停下的位置”是两个不同的量。

抓取目标高度不是初始生成高度 0.921,而是按桌面上表面加半个方块边长计算,即 0.920。初始的小间隙是为了生成刚体,真正抓取时以落在桌面上的几何高度为目标。

放置目标是框底板高度,加方块半高,再留 12 mm 的释放间隙。对应 TCP 高度约为 0.940,释放后方块靠重力落到底板上,稳定中心高度约为 0.928。释放位姿和最终静止位置不是同一个量。

图6-5 抓取放置阶段的TCP目标高度、两种模式适用阶段和结果验收条件

图 6-10 抓取放置阶段的 TCP 目标高度、两种模式适用阶段和结果验收条件

图 6-10 怎么看: 横向是任务进程,不是机器人向某个世界方向一直移动。红块上方目标高度是 1.080 m,框上方目标高度是 1.100 m,所以“高空搬运”并非严格水平等高。虚线仅连接阶段目标,不把关节模式的实际路径假定为直线。框中的最终高度标注属于落稳后的红块中心,其余阶段标注的是 TCP 目标,两者不要混为一谈。

案例:12 mm 自由落体有多快? 释放时方块底面离底板约 0.012 m,自由落体时间 t=2h/g=2×0.012/9.81≈0.049 s,落地速度 v=2gh≈0.485 m/s。恢复系数为 0,碰撞后基本不反弹;再经过退出阶段与 hold(1.0),方块早已静止,因此后面的速度阈值 0.04 m/s 足以区分“已稳定”和“仍在下落/滚动”。(这是忽略夹爪张开过程与接触摩擦的理想估算。)

5.2 夹爪为什么不是一条“开关指令”那么简单 ​

在任务层,夹爪只有“张开”和“闭合”两个动作;但物理层仍是关节驱动、连杆约束、碰撞网格和摩擦共同起作用。

本模型右夹爪的主动关节名是 idx81_gripper_r_outer_joint1。第六章只向这个主动关节发送位置目标,其余夹爪连杆由原资产中的 mimic 与机械约束带动。默认张开目标为 0.72 rad,闭合目标为 0 rad。

对应代码(code/code_chapter6/control.py 与 config.py):

python
# config.py
GRIPPER_JOINT = "idx81_gripper_r_outer_joint1"
GRIPPER_OPEN = 0.72
GRIPPER_CLOSED = 0.0

# control.py
def gripper(self, opened):
    """只驱动右夹爪主动关节;原 USD mimic/闭环连杆带动其余关节。"""
    from isaacsim.core.utils.types import ArticulationAction
    self.gripper_target = GRIPPER_OPEN if opened else GRIPPER_CLOSED
    self.env.robot.apply_action(ArticulationAction(
        joint_positions=np.array([self.gripper_target]),     # 只给一个关节的位置目标
        joint_indices=np.array([self.gripper_index])))
    self.hold(1.0)                                          # 推进 1 s(120 个物理步)让接触建立

不要把“角度越大”想当然地理解为“夹得越紧”。开合方向由模型定义决定,本章已经按原始资产确认。也不要要求抓住方块以后实际角度必须到达零:方块挡在两指之间,实际角度停在中间位置往往正是接触结果。所以 gripper() 没有像机械臂那样做跟踪误差检查——夹住物体时目标与实际本来就不相等,抓取是否成功要交给后面的抬升检查。

场景沿用原夹爪碰撞网格,并在本次运行中设置接触材料和较高的关节求解迭代精度。方块质量为 0.04 kg,材料设置静摩擦系数 1.2、动摩擦系数 1.0、恢复系数 0。这些是默认教学场景的物理参数,不是通过把方块固定到夹爪上来绕过抓取。

对应代码(code/code_chapter6/environment.py):

python
material = PhysicsMaterial("/World/Chapter6/ContactMaterial", static_friction=1.2,
                           dynamic_friction=1.0, restitution=0.0)
# 原模型手指已有碰撞网格;只在本次场景设置接触材料和求解精度。
for prim in self.world.stage.Traverse():
    if str(prim.GetPath()).startswith("/genie/gripper_r") and prim.HasAPI(UsdPhysics.CollisionAPI):
        UsdShade.MaterialBindingAPI.Apply(prim).Bind(
            material.material, UsdShade.Tokens.weakerThanDescendants, "physics")
root = self.world.stage.GetPrimAtPath(ROBOT_PRIM_PATH)
# ArticulationRootAPI 可能位于子节点,逐一查找,不硬编码根关节。
for prim in self.world.stage.Traverse():
    if prim.GetPath().HasPrefix(root.GetPath()) and prim.HasAPI(UsdPhysics.ArticulationRootAPI):
        api = PhysxSchema.PhysxArticulationAPI.Apply(prim)
        api.CreateSolverPositionIterationCountAttr(32)
        api.CreateSolverVelocityIterationCountAttr(4)
# ...
self.cube = self.world.scene.add(DynamicCuboid(
    "/World/Chapter6/RedCube", name="red_cube", position=np.array(C.CUBE_POSITION),
    size=C.CUBE_SIZE, mass=0.04, color=np.array([.9, .025, .025]), physics_material=material))

案例:摩擦够不够? 方块重力 mg=0.04×9.81≈0.392 N。两指从两侧夹持时,两个接触面各提供最大静摩擦 μsN,要托住方块需要 2μsN≥mg,即每指法向力 N≥0.392/(2×1.2)≈0.163 N。这只是理想的两点摩擦估算,用来说明“为什么 4 cm、40 g 的方块靠摩擦就能拿起”,不是对 G2 夹爪实际夹持力的测量或力闭合分析。

本章没有新增一个外部 PID 力矩环。第五章控制器发送位置目标,模型中的关节驱动负责底层跟踪;再叠加一套未经设计的力矩控制,不仅不能自动提高抓取质量,还会混淆本章要学习的控制层次。

💡 要点:位置控制的夹爪“想去 0 rad 却被方块挡住”,驱动会持续施力,这个持续的接触力就是夹持力的来源。所以夹紧后实际角度停在中间值是正常现象。

5.3 怎样判断是真成功,而不是“脚本跑到了最后” ​

一次可靠的演示必须能说明失败发生在哪里。本章把检查安排在运动和接触的关键边界,而不是只在结尾打印一句“完成”。

检查项当前实现的判定主要防止的问题
参数与动作合法性维度正确、数值有限、关节目标不越界NaN、动作维度错误、静默裁剪
IK求解器报告收敛;末端模式预检本段全部中间点向不可达位姿运动
关节跟踪最大关节角误差不超过 0.06 rad已发指令但关节没跟上
TCP 到位位置误差不超过 12 mm,姿态误差不超过 0.06 rad坐标系错了却仍报告到达
抬升检查至少升高计划抬升量的 60%,且方块中心距 TCP 不超过 9 cm空夹或已经掉落
放置检查整块方块位于框内,底部接近底板,线速度不超过 0.04 m/s方块中心在框内但边角越界,或仍在下落
连续稳定检查满足放置条件并连续观察约半秒只在一个瞬间经过目标
底盘漂移抓放期间平面位置变化不超过 2 cm底盘慢慢滑走却仍声称固定基座操作成功

抬升检查(code/code_chapter6/task.py):

python
motion.move_world(above_pick, C.GRASP_ROTATION)
motion.hold(.4)
cube_p, _ = env.cube.get_world_pose()
tcp_p, _ = env.tcp_pose()
lift_height = float(cube_p[2]-(C.TABLE_TOP+C.CUBE_SIZE/2))   # 相对桌面静止高度 0.920
if lift_height < cfg.clearance*.6 or np.linalg.norm(cube_p-tcp_p) > .09:
    raise RuntimeError(f"抓取失败:红块仅抬升 {lift_height:.3f} m;停止搬运")
print(f"  红块实际抬升 {lift_height:.3f} m", flush=True)

案例:抬升阈值。 clearance=0.16,阈值 0.16×0.6=0.096 m,即方块中心要高于 0.920+0.096=1.016 m。历史记录中 relative_joint 模式实际抬升 0.1589 m,明显超过阈值;而空夹时方块仍在桌上,lift_height≈0,立即报错。第二个条件“块心距 TCP ≤ 9 cm”排除了“方块被碰飞得很高但不在手里”这种巧合。

入框检查(code/code_chapter6/task.py):

python
def cube_inside_box(position, quaternion):
    """整块立方体而非仅中心在框内,且位于底板上;允许 6 mm 接触容差。"""
    position = np.asarray(position)
    # 旋转后方块在世界 x/y/z 方向上的包围盒半尺寸:|R| · (s/2, s/2, s/2)
    extent = np.abs(quaternion_to_matrix(quaternion)) @ np.full(3, C.CUBE_SIZE/2)
    inside_xy = np.all(np.abs(position[:2]-C.BOX_CENTER)+extent[:2] <= C.BOX_INNER_SIZE/2-.002)
    bottom = position[2]-extent[2]
    return bool(inside_xy and abs(bottom-C.BOX_FLOOR_TOP) < .006)

入框检查不是只看方块中心。程序根据方块旋转计算它在世界坐标轴方向上的包围盒半尺寸,再判断整个 XY 范围是否位于框内,并保留 2 mm 边界余量。方块底部高度需要在底板上表面的 6 mm 容差内。

案例:用历史落点手算一次入框检查。 取 relative_joint 模式的最终方块中心 (1.1766, -0.5052, 0.9280),假设方块未旋转:

  1. 包围盒半尺寸 extent = (0.02, 0.02, 0.02)。
  2. 允许的单边上限: 0.18/2−0.002=0.088。
  3. x 方向: |1.1766−1.18|+0.02=0.0034+0.02=0.0234≤0.088 ✓;y 方向: |−0.5052+0.52|+0.02=0.0148+0.02=0.0348≤0.088 ✓。
  4. 底面: 0.9280−0.02=0.908,与底板上表面 0.908 相差约 0, <0.006 ✓。

若方块绕 z 轴转了 45∘,x/y 方向的半尺寸变为 0.02(cos⁡45∘+sin⁡45∘)≈0.0283,中心允许区的半宽从 0.088−0.02=0.068 缩小到 0.088−0.0283≈0.0597——这就是“只看中心”会漏判边角越界的原因。

入框判定几何

图 6-11 cube_inside_box() 的两项几何判定:俯视看整块是否位于“内边长减 2 mm 余量”的区域内,侧视看底面是否在底板上表面 ±6 mm 内

连续稳定与底盘漂移(code/code_chapter6/task.py):

python
# 连续半秒验收:防止方块仅短暂穿过目标区域就被标成成功。
for _ in range(round(.5/cfg.dt)):                 # 120 Hz → 60 个物理步
    p, q = env.cube.get_world_pose()
    if not cube_inside_box(p, q) or np.linalg.norm(env.cube.get_linear_velocity()) > .04:
        raise RuntimeError(f"放置失败:红块未稳定落入方框,实际位置 {p}")
    motion.step()
final_base = env.base_pose()
drift = np.hypot(final_base.x-parked_pose.x, final_base.y-parked_pose.y)
if drift > .02:
    raise RuntimeError(f"抓取过程中底盘漂移过大:{drift:.3f} m")

这些检查也有边界。抬升条件是一个简洁的“已拿起且仍靠近手”的判据,不是力闭合分析;放稳条件检查了位置和线速度,但没有完整接触力分析;底盘漂移验收只检查平面平移,不等于六维基座稳定性认证。

当前程序没有自动重抓或故障恢复。出现异常后,它会停止底盘、记录失败原因并关闭仿真。

⚠️ 注意:“机械臂到达了放置点”只说明 move_world() 通过了 TCP 误差检查,不说明方块在框里。抓取成功看抬升检查,放置成功看连续 0.5 s 的入框与速度检查,两者都通过且底盘未漂移,才返回 success: True。


第六部分 代码结构与实现解析 ​

6.1 文件组织 ​

项目根目录记为 /path/to/project-root(占位符,替换为本仓库在你电脑上的实际位置)。它下面的 code/chapter6 保存本教程,code/code_chapter6 保存可执行程序,assets/robot/G2_omnipicker 保存共享机器人资产。

text
code/
├── code_chapter4/              # 被复用:底盘
│   ├── base_controller.py      # G2BaseController、PoseController
│   ├── kinematics.py           # SwerveKinematics、Pose2D
│   └── config.py               # ROBOT_USD、ROBOT_PRIM_PATH、RobotGeometry、ControlLimits
├── code_chapter5/              # 被复用:机械臂
│   ├── arm_controller.py       # G2ArmController(轨迹、FK、IK)
│   ├── kinematics.py           # axis_rotation、orientation_error、quaternion_to_matrix
│   └── config.py               # ARM_JOINT_NAMES、HOME_JOINT_POSITIONS、关节限位
└── code_chapter6/              # 本章
    ├── __init__.py
    ├── config.py               # 固定场景常量、GRASP_ROTATION、TaskConfig
    ├── environment.py          # PickPlaceEnvironment:启动仿真、搭场景、读取底盘/基座/TCP 位姿
    ├── control.py              # MotionController:navigate / relative_joint / delta_ee / move_world / gripper
    ├── task.py                 # run_task() 顺序状态机、cube_inside_box() 验收
    ├── recording.py            # TrajectoryRecorder:可选 JSONL 记录(约 20 Hz)
    └── demo_pick_place.py      # 唯一运行入口:参数解析、资源创建、运行与清理
文件主要职责阅读时关注什么
config.py定义固定场景和任务参数哪些值改变任务,哪些值改变控制节奏
environment.py启动仿真,创建场景,读取底盘与 TCP世界、机械臂基座和底盘坐标怎么区分
control.py复用第四、第五章,提供增量动作和夹爪接口目标如何逐层变成实际关节命令
task.py按阶段组织移动抓放与验收为什么必须按这个顺序执行
recording.py可选的流式 JSONL 采集记录的是哪些状态、哪些命令
demo_pick_place.py参数解析、资源创建、运行与清理如何启动、失败如何退出
README.md快速运行和接口说明查命令、参数和历史验证记录

建议按 config.py → task.py → control.py → environment.py → recording.py 阅读。先知道场景和任务,再看控制细节,比从仿真初始化开始逐行读更容易建立整体理解。

发布或迁移本章时,不能只复制 code_chapter6。第四、第五章的模块以及机器人 USD 引用的子资产也需要保留。这里是为了沿用前面章节的代码,而不是把第六章做成完全独立的第三方安装包。

启动脚本通过自身位置找到 code 目录,使不同章节按包名导入,避免多个 config.py 同名冲突。资产路径也由模块位置推导,而不是依赖启动时的当前目录。但 Python 必须先找到入口脚本,才能运行这段路径处理逻辑。 这正是“脚本支持任意目录启动”和“随便写相对路径也能找到脚本”之间的区别。

TaskConfig:控制节奏的参数(code/code_chapter6/config.py):

python
@dataclass(frozen=True)
class TaskConfig:
    mode: str = "relative_joint"
    headless: bool = False
    physics_hz: int = 120
    waypoint_timeout: float = 25.0
    arm_duration: float = 2.5
    clearance: float = 0.16
    cartesian_step: float = 0.012

    def __post_init__(self):
        if self.mode not in ("relative_joint", "delta_ee"):
            raise ValueError("mode 必须是 relative_joint 或 delta_ee")
        # ... 其余数值必须是有限正数,physics_hz 必须是整数

    @property
    def dt(self):
        return 1.0 / self.physics_hz

frozen=True 让配置在创建后不可修改,避免运行中途被意外改动;命令行只能设置 mode 和 headless,其余字段要在代码中构造 TaskConfig(...) 时传入。

6.2 模块调用关系 ​

模块调用关系

图 6-12 code_chapter6 模块调用关系:入口依次创建记录器、环境、控制器并执行任务;控制器调用环境读取状态和推进仿真,并复用第四、五章模块

6.3 关键流程逐段解读 ​

① 入口与路径处理(code/code_chapter6/demo_pick_place.py):

python
# 只增加兄弟章节的父目录;不让多个章节的 config.py 发生同名导入冲突。
if __package__ in (None, ""):
    sys.path.insert(0, str(Path(__file__).resolve().parents[1]))   # 即 code/

from code_chapter6.config import TaskConfig
from code_chapter6.control import MotionController
from code_chapter6.environment import PickPlaceEnvironment
from code_chapter6.recording import TrajectoryRecorder
from code_chapter6.task import run_task

parents[1] 是 code/ 目录,于是 code_chapter4、code_chapter5、code_chapter6 都以包名导入,三个 config.py 互不冲突。

② 参数解析与资源生命周期:

python
def main():
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument("--mode", choices=("relative_joint", "delta_ee"), default="relative_joint")
    parser.add_argument("--headless", action="store_true")
    parser.add_argument("--keep-open", action="store_true", help="成功后保留窗口观察")
    parser.add_argument("--record", type=Path, help="可选 JSONL 轨迹路径,已存在则拒绝覆盖")
    args = parser.parse_args()
    if args.headless and args.keep_open:
        parser.error("--keep-open 仅适用于有窗口模式")
    cfg = TaskConfig(mode=args.mode, headless=args.headless)
    env = motion = recorder = None
    try:
        # 先检查输出路径,再启动耗时的仿真。
        if args.record:
            recorder = TrajectoryRecorder(args.record, cfg)
        env = PickPlaceEnvironment(cfg)
        motion = MotionController(env, recorder)
        result = run_task(motion)
        print("[结果] "+json.dumps(result, ensure_ascii=False), flush=True)
        if recorder:
            recorder.write({"type": "result", **result})
        if args.keep_open:
            while env.app.is_running() and env.world.is_playing():
                env.step()
    except Exception as exc:
        if recorder:
            recorder.write({"type": "result", "success": False, "error": str(exc),
                            "phase": motion.phase if motion else "initialization"})
        raise  # 失败返回非零退出码,不能伪装成成功
    finally:
        try:
            if motion:
                motion.stop()
        finally:
            if recorder:
                recorder.close()
            if env:
                env.close()

代码解读:记录器在仿真启动之前创建——若文件已存在,FileExistsError 会立刻抛出,不必白等仿真加载;失败时写入 success: False、error 和 phase,然后重新抛出异常;嵌套的 finally 保证即使 stop() 出错,记录文件和仿真应用也会被关闭。

③ 环境创建(environment.py 的 __init__ 与 _build()):

environment.py 先创建 SimulationApp,再导入需要应用扩展的 Isaac Sim 核心模块。这与官方独立 Python 脚本的初始化顺序一致。[参考 2、3] 随后创建物理世界、地面、灯光、G2、桌面和方框,初始化机器人与方块句柄,再推进一段预热时间。

python
from isaacsim import SimulationApp
self.app = SimulationApp({"headless": config.headless,
                          "disable_viewport_updates": config.headless,
                          "renderer": "RaytracedLighting", "limit_cpu_threads": 16,
                          "extra_args": shlex.split(os.environ.get("HELLO_ROBOT_KIT_ARGS", ""))})
# _build() 中:
self.world = World(stage_units_in_meters=1.0, physics_dt=self.config.dt,
                   rendering_dt=1 / 60)
# ... 地面、灯光、G2 USD、接触材料、桌面与桌腿、五块长方体组成的方框、红块
self.world.play()
self.robot = self.world.scene.add(SingleArticulation(ROBOT_PRIM_PATH, name="G2_chapter6"))
self.robot.initialize()
self.cube.initialize()
self.arm_base = SingleXFormPrim("/genie/arm_base_link")
self.tcp = SingleXFormPrim("/genie/gripper_r_center_link")
self.chassis = SingleXFormPrim("/genie/chassis_link")
# ...
for _ in range(120):          # 预热 120 个物理步(1 s),让方块落稳、机器人静止
    self.step()

方框由一块底板加四面墙(BoxX0/1、BoxY0/1)共五个 FixedCuboid 拼成;HELLO_ROBOT_KIT_ARGS 环境变量可向 Kit 追加启动参数(可选,默认为空)。

④ 仿真推进与时间:

python
def step(self):
    if not self.app.is_running() or not self.world.is_playing():
        raise RuntimeError("仿真已停止;请重新运行示例,不在任务中途暂停/重置")
    self.world.step(render=not self.config.headless)
    self.steps += 1

默认物理频率为 120 Hz,即一个物理步约 8.33 ms。渲染时间步单独设置为 1/60 s;无窗口运行时不绘制每一步画面。任务中的等待和轨迹通过推进物理步实现,而不是简单调用系统睡眠来假装机器人已经运动。

要区分三种时间:

  • 轨迹参数时间:例如一段机械臂动作的默认 2.5 s,生成器可能因为关节位移而延长。
  • 仿真时间:推进的物理步数乘以时间步,包括等待和稳定检查。
  • 实际运行耗时:从终端启动到退出的真实时间,还包括应用启动、资产加载和计算。

程序报告的 simulation_time_s 是仿真时间(env.steps*cfg.dt),不是实际耗时。末端增量模式还包含分段求解、分段执行和等待,不能要求整个程序在一段配置时长内完成。

图形窗口中可以观察动作,但本例不支持在任务执行中随意暂停、停止或重置后继续沿用旧控制状态。环境检测到停止或暂停会报错退出;需要重新运行一次任务。

⑤ 数据记录(code/code_chapter6/recording.py):

python
class TrajectoryRecorder:
    def __init__(self, path, config):
        path = Path(path)
        path.parent.mkdir(parents=True, exist_ok=True)
        self.file = path.open("x", encoding="utf-8")  # 不覆盖已有采集结果
        self.interval = max(1, round(config.physics_hz/20))     # 120/20 = 每 6 步一行
        self.write({"type": "metadata", "schema_version": 1, "mode": config.mode,
                    "physics_dt": config.dt, "sample_interval_steps": self.interval,
                    "units": "meter, radian, second", "quaternion_order": "wxyz"})

    def sample(self, motion):
        env = motion.env
        if env.steps % self.interval:
            return
        # ... 写入 time、phase、base_pose、base_command、arm_position、arm_target、
        #     gripper_target、gripper_position、tcp_world、cube_world、cube_quaternion

可选记录器大约每 20 Hz 写一行 JSON。相对于 120 Hz 的物理循环,它是一份下采样的控制观察记录,并不是逐物理步完整保存仿真。

字段含义容易产生的误解
time、phase仿真时间与任务阶段不是终端墙钟时间
base_pose实际底盘 x/y/yaw第三个数不是 z 高度
base_command限速后的 vx/vy/wz 命令不是实际测量速度
arm_position实际七关节角是状态,不是本次增量
arm_target该时刻已下发的七关节绝对目标不是 dq,也不是 delta_ee
gripper_position/target夹爪实际与目标角夹住物体后两者可以不相等
tcp_worldTCP 世界位置当前记录没有保存完整 TCP 朝向
cube_world/quaternion方块世界位置与姿态真值用于验收和分析,不是视觉检测输出
最后一个 result成功指标或失败原因应检查这一行,而不是只数动作阶段

四元数顺序在文件元数据中明确写为 [w, x, y, z]。文件以独占创建方式打开,已存在时拒绝覆盖,避免无意丢掉上一轮实验。

本章不包含强化学习微调、奖励设计或策略推理,记录中也没有相机图像。它可以作为后续研究的数据起点,但不能直接称为标准 VLA 数据集。尤其不要把记录中的相邻关节目标简单相减,就宣称恢复了原始高层 relative_joint 动作;中间存在轨迹插值和采样降频,两者含义不同。


第七部分 代码运行 ​

7.1 环境部署 ​

💡 路径约定:下文命令中 /path/to/project-root 表示本仓库在你电脑上的根目录,/path/to/isaac-sim 表示 Isaac Sim 的安装目录,均为占位符,请替换为自己机器上的实际路径。

本章已有物理验证使用的是 Isaac Sim 5.1,并采用安装目录中的 python.sh 启动。这是复现基线,不代表 5.1 是最新版本。安装前请自行查看对应版本的官方支持状态;如升级到其他版本,需要重新验证 API、资产加载和抓放行为,不能直接沿用本章的成功记录。[参考 1]

本教程采用“工作站安装 + 仿真器内置 Python”一条主路径,不同时展开 Docker、源码编译和多种 Conda 环境。当前安装的 python.sh 指向内置 Python 3.11,并设置仿真扩展需要的环境。运行方式可对照官方 Python 环境说明。[参考 2、3]

依赖层级本章需要什么准备方式
硬件与驱动满足所选 Isaac Sim 版本要求的 NVIDIA GPU 与驱动按对应版本的官方要求检查,必要时运行兼容性检查器
仿真运行时Isaac Sim、Kit、PhysX、USD、isaacsim.core 等使用完整 Isaac Sim 安装,不逐个从 pip 拼装底层模块
Python 数值计算NumPy优先使用仿真器配套版本,先检查再考虑修复
Python 标准库argparse、json、pathlib、unittest 等不需要单独安装
项目模块code_chapter4、code_chapter5、code_chapter6保留当前目录结构
本地资产原始 G2 USD 和它引用的配置、几何、材质文件保留完整共享资产与引用关系

本章沿用前面安装教程准备的环境。若第四、第五章能够运行,可直接运行本章任务,无需重新安装 Isaac Sim,也没有额外的训练依赖要装。

--headless 只是关闭窗口显示,不代表本教程支持没有兼容 GPU 的纯 CPU 运行。GPU、驱动、显示与资源要求应按所用版本的官方说明和兼容性检查结果判断,不把“能运行普通 NumPy”当作“能运行完整 Isaac Sim”。[参考 1、4]

7.2 代码执行 ​

下面直接运行任务。本节默认已按前面的安装教程准备好 Isaac Sim,并能运行第四、第五章代码,不再重复安装步骤。所有命令都使用完整路径和普通半角字符;不要复制终端提示符、Markdown 转义符或网页中的 &#x20;。

当前入口只有 --mode、--headless、--keep-open 和 --record 四个命令行选项。时长、抬升距离、航点等在 TaskConfig 或 config.py 中设置,程序没有 --clearance、--arm 或 --train 这类未实现的参数。

7.2.1 先跑通默认任务,再切换模式比较 ​

运行关节增量模式。 如果有桌面显示,建议先看完整动作。这条命令以有窗口方式执行 relative_joint 模式的完整移动抓放任务:

bash
/path/to/isaac-sim/python.sh \
  /path/to/project-root/code/code_chapter6/demo_pick_place.py \
  --mode relative_joint --keep-open

--keep-open 表示成功后保留窗口观察;不带它则任务完成后关闭应用。保留窗口不是暂停物理,成功后的场景仍会继续推进。

再运行末端增量模式。 关闭上一轮窗口后,在同一场景参数下执行,这条命令切换为 delta_ee 模式(下探、抬升、搬运、下放、退出阶段走末端分段直线):

bash
/path/to/isaac-sim/python.sh \
  /path/to/project-root/code/code_chapter6/demo_pick_place.py \
  --mode delta_ee --keep-open

观察重点不是“哪个看起来更快”,而是下探和抬升时的 TCP 路径、搬运时夹爪姿态变化,以及相同终点目标对应的关节运动是否不同。两个模式都成功时,也不表示它们经过了相同的中间姿态。

没有桌面或只需验证时,分别使用无窗口模式。 下面两条命令依次以无窗口方式运行两种模式:

bash
/path/to/isaac-sim/python.sh \
  /path/to/project-root/code/code_chapter6/demo_pick_place.py \
  --headless --mode relative_joint

/path/to/isaac-sim/python.sh \
  /path/to/project-root/code/code_chapter6/demo_pick_place.py \
  --headless --mode delta_ee

不要同时传入 --headless --keep-open,程序会拒绝这组相互冲突的参数。刚开始也不建议同时启动两个仿真实例,以免资源竞争干扰对比。

7.2.2 记录两条轨迹 ​

为两个模式指定不同文件名。记录器会自动创建父目录,但不会覆盖同名文件。下面两条命令在无窗口运行的同时把轨迹写入 JSONL:

bash
/path/to/isaac-sim/python.sh \
  /path/to/project-root/code/code_chapter6/demo_pick_place.py \
  --headless --mode relative_joint \
  --record /path/to/project-root/code/code_chapter6/outputs/joint_001.jsonl

/path/to/isaac-sim/python.sh \
  /path/to/project-root/code/code_chapter6/demo_pick_place.py \
  --headless --mode delta_ee \
  --record /path/to/project-root/code/code_chapter6/outputs/delta_001.jsonl

第一次采集后可以直接查看文件首尾(第一行是元数据,最后一行通常是结果):

bash
head -n 1 /path/to/project-root/code/code_chapter6/outputs/joint_001.jsonl
tail -n 1 /path/to/project-root/code/code_chapter6/outputs/joint_001.jsonl
tail -n 1 /path/to/project-root/code/code_chapter6/outputs/delta_001.jsonl

如果进程被外部强行终止,文件可能不完整,没有结果行,此时不能把它算作成功。再次采集时改成 joint_002.jsonl 等新名字,不需要删除旧数据。

下面这段离线读取代码只依赖标准库,不会启动仿真。它检查记录是否以成功结果结束,并概览样本数量、阶段和采样时间顺序:

bash
python3 - /path/to/project-root/code/code_chapter6/outputs/joint_001.jsonl <<'PY'
import json
import sys
from pathlib import Path

rows = [json.loads(line) for line in Path(sys.argv[1]).read_text().splitlines()]
samples = [row for row in rows if row.get("type") == "sample"]
if not rows or rows[-1].get("type") != "result":
    raise SystemExit("记录不完整:缺少最后的 result 行")

result = rows[-1]
print("结果:", result)
print("样本数:", len(samples))
print("阶段:", list(dict.fromkeys(row["phase"] for row in samples)))
print("时间严格递增:", all(
    right["time"] > left["time"] for left, right in zip(samples, samples[1:])
))
if not result.get("success"):
    raise SystemExit("本次任务失败,请查看 error 和 phase")
PY

7.2.3 调用接口:世界向下 2 cm 的模式对比练习 ​

调用接口时,优先复用现有模块,不复制一套新控制器。 下面是一个独立脚本示例;可以另存为项目内的实验脚本,用 Isaac Sim 的 python.sh 执行。它先进入默认任务的高空预抓取位置,再做“世界向下 2 cm,随后回到原位”的对比练习,不执行夹紧与放置。第一次把 mode 设为 "relative_joint",第二次重新运行脚本并改成 "delta_ee";不要在同一轮里连续做两次下探来假装起点相同。

python
import sys
from pathlib import Path

# 如果迁移项目,修改这里;该目录同时包含第四、第五、第六章的包。
sys.path.insert(0, str(Path("/path/to/project-root/code")))

from code_chapter5.config import HOME_JOINT_POSITIONS
from code_chapter6 import config as C
from code_chapter6.config import TaskConfig
from code_chapter6.control import MotionController
from code_chapter6.environment import PickPlaceEnvironment

import numpy as np

mode = "relative_joint"  # 第二轮重新运行时改成 "delta_ee"。
env = motion = None
try:
    env = PickPlaceEnvironment(TaskConfig(mode=mode, headless=False))
    motion = MotionController(env)
    motion.relative_joint(HOME_JOINT_POSITIONS - motion.arm.get_joint_positions())
    motion.gripper(opened=True)

    for waypoint in C.BASE_WAYPOINTS:
        motion.navigate(waypoint)

    above_pick = np.array([
        C.CUBE_POSITION[0], C.CUBE_POSITION[1],
        C.TABLE_TOP + C.CUBE_SIZE / 2 + motion.config.clearance,
    ])
    motion.move_world(above_pick, C.GRASP_ROTATION, mode="relative_joint")

    # 记录同一个起点;这里只在红块上方练习,不下探到接触高度。
    start_world, rotation_world = env.tcp_pose()
    target_world = start_world + np.array([0.0, 0.0, -0.02])
    print("模式:", mode)
    print("起点:", start_world, "目标:", target_world)

    # 不直接把世界 z 位移塞给 delta_ee;由 move_world 做坐标转换。
    motion.move_world(target_world, rotation_world, mode=mode)
    motion.hold(1.0)
    reached_world, _ = env.tcp_pose()
    print("实际世界位移:", reached_world - start_world)

    # 回到保存的绝对世界目标,而不是盲目反向发送同样大小的增量。
    motion.move_world(start_world, rotation_world, mode=mode)
    motion.hold(1.0)
finally:
    try:
        if motion is not None:
            motion.stop()
    finally:
        if env is not None:
            env.close()

这是接口使用示范,不属于前述两种默认任务的历史物理验收记录。修改目标前仍需检查可达性和碰撞风险。

如果只想理解两种增量接口的写法,可阅读下面的调用片段。它们需要已经初始化的 motion,不是可直接粘贴到终端执行的完整程序,也不建议不看机器人姿态就连续执行:

python
# 第一个关节相对当前实测角度增加 0.05 rad,其余关节不变。
motion.relative_joint([0.05, 0, 0, 0, 0, 0, 0], duration=1.0)

# TCP 沿 arm_base_link 的 x 轴移动 1 cm,保持当前朝向。
# 注意:不是世界 x,也不是夹爪自己的 x。
motion.delta_ee(dp=[0.01, 0, 0], dr=[0, 0, 0], duration=1.0)

改变实验时,先一次只改一类变量。先比较两种模式,再调整动作时长,最后才小幅修改位置或几何参数。改桌高会影响抓取高度和 IK,改框位置会影响搬运路径,改红块尺寸还会影响夹爪开口与接触。不能把这些变化都当成“换一个坐标就行”。

7.3 运行结果 ​

有窗口运行时的场景截图:

图 6-13 有窗口模式下 G2 停靠桌前执行移动抓放任务的仿真画面

正常日志会依次出现这些阶段,下面是阶段名示意,不是需要输入的命令:

text
prepare → navigate → navigate → pregrasp → descend
        → close → lift → transfer → place → release → retreat → success

终端会打印 TCP 位置和姿态误差、方块实际抬升高度,最后输出带 success 字段的 JSON 结果。如果中途异常,应查看第一个与任务有关的异常阶段,不要只关注最后一句统一的 Python 启动失败提示。

已有开发验证记录如下,来源为 code/code_chapter6/README.md 中的默认场景记录,不是读者每次运行必须逐位复现的数值,也不是本教程写作时重新做出的成功率统计:

模式结果红块实际抬升抓放期间底盘平移漂移仿真时间
relative_joint稳定落入框内0.1589 m0.00194 m33.95 s
delta_ee稳定落入框内0.1585 m0.00189 m34.22 s

对应最终方块中心约为 (1.1766, -0.5052, 0.9280) 和 (1.1759, -0.5054, 0.9280),逐段到位时的 TCP 位置误差均小于 1.4 mm。这些记录来自无窗口物理验证,有窗口界面未在该记录中单独做交互验收。

如何读懂这些输出:

  • 阶段序列:日志中的 [第六章] xxx 行来自 set_phase(),顺序应与图 6-4 一致;navigate 出现两次对应两个航点。若最后一行阶段不是 success,失败就发生在最后打印的那个阶段里。
  • TCP 误差 x mm / y rad:每次 move_world() 之后打印,阈值是 12 mm / 0.06 rad。历史记录 <1.4 mm 说明坐标转换与 IK 远在容差之内。
  • 抬升高度:0.1589 m 对照计划抬升 0.16 m 和阈值 0.096 m,说明方块几乎随手完整抬起(约 1 mm 差异可来自夹持时的微小滑动或下垂)。
  • 最终方块中心:两种模式 x 差 0.7 mm、y 差 0.2 mm,z 都是 0.9280,与第一部分手算的落稳高度 0.908+0.02=0.928 一致;按第五部分手算,离框边还有很大余量。
  • 底盘漂移:约 1.9 mm,远小于 2 cm 阈值,说明抓放过程中底盘基本静止。
  • 仿真时间:delta_ee 比 relative_joint 多约 0.27 s,与“分段执行有段间减速和启动”的分析方向一致;但这只是两次记录的差异,不是性能结论。

真正应该关注的是:方块有没有被抬起、是不是整个落在框内、是否已经稳定,以及误差是否在判定范围内,而不是要求两种模式的最终落点小数完全一样。


本章小结 ​

知识回顾 ​

移动抓放并不是“底盘代码执行一次,再让机械臂代码执行一次”这么简单。真正需要连接的是目标坐标、当前状态、动作表达、轨迹、接触和结果判定。relative_joint 适合直接表达关节变化,delta_ee 适合表达末端运动意图;两者都必须经过正确的坐标处理与执行检查。

主题核心结论关键代码 / 参数
任务状态机线性顺序执行;函数正常返回即通过,异常即终止,不重试task.py run_task()、set_phase()
闭环执行目标固定,但完成与否以实测为准base_pose()、get_joint_positions()、tcp_pose()
relative_jointqgoal=qmeasured+Δq;三次时间缩放;关节空间平滑,TCP 不一定走直线默认 2.5 s,跟踪阈值 0.06 rad
delta_eepgoal=p0+Δp, Rgoal=Exp(Δr^)R0;分 N 段、整段 IK 预检后才下发cartesian_step=0.012,旋转 0.08 rad/段
坐标系任务写世界目标 → world_to_arm() → IK/增量 → 世界系验收;dp/dr 在 arm_base_linkTCP 验收 12 mm / 0.06 rad
抓取姿态GRASP_ROTATION 三列是 TCP 三轴的世界方向;z 朝下,x 沿世界 y位置 + 朝向同时约束的 IK
底盘停靠比例控制 + 限幅 + 容差 + 停车 0.5 s 复核;读 chassis_link0.22 m/s、0.30 rad/s、6 mm、0.012 rad
夹爪只控制主动关节;位置目标被方块挡住即产生夹持力0.72 开 / 0 合,摩擦 1.2/1.0
成功判定抬升 ≥ 60%、整块入框、底面贴底板、速度小、连续 0.5 s、底盘漂移 ≤ 2 cmcube_inside_box()

本章只完成默认场景下的教学控制,不提供全身碰撞规划、动态避障、视觉纠偏或真机安全保证。原机器人资产中部分连杆碰撞本身未开启,IK 收敛更不能当作全路径无碰撞证明。后续章节再逐步引入感知、环境理解和任务决策,而不是把尚未实现的能力提前写进本章。

课后练习 ​

  1. (手算)三次时间缩放:J4 当前 -1.10 rad,relative_joint 增量为 -0.20 rad,请求时长 2.0 s。求 t=0.5 s 和 t=1.5 s 时下发的 J4 目标角;并判断该目标是否在第五章 J4 限位 [−2.5307,1.0472] 之内、是否会触发限速延长。(提示: s(0.25)=0.15625, s(0.75)=0.84375;最短时长 =0.2/(0.6×3.1416)。)
  2. (手算)分段数:delta_ee(dp=[0.05, 0, 0.05], dr=[0, 0, 0.20]) 会分成几段?由平移还是旋转决定?默认 2.5 s 下每段分到多少时间?
  3. (思考)坐标系:若底盘最终停在 yaw=0.10 rad 而不是 0,任务中的世界目标需要修改吗?world_to_arm() 的输出会怎样变化?如果有人把预抓取点预先换算成基座系坐标并写死,会发生什么?
  4. (动手)模式对比:用第七部分的“世界向下 2 cm”脚本,分别以两种模式运行,记录打印的“实际世界位移”。比较 x、y 分量的偏差,并结合图 6-5 解释差异。
  5. (动手)记录分析:用 --record 采集两种模式的 JSONL,修改 7.2.2 的离线脚本,统计每个 phase 的样本数(约为该阶段仿真时长 × 20),对比 descend 阶段在两种模式下的耗时,并解释原因。
  6. (动手 / 思考)验收阈值:把 TaskConfig 的 clearance 改为 0.10 后运行(在代码中构造 TaskConfig(mode=..., clearance=0.10) 或修改默认值)。抬升阈值变成多少?above_place 的 TCP 高度变成多少?预测 delta_ee 下探段数的变化,再运行验证。

参考资料 ​

环境资料说明。 代码行为、参数和验证数值以本仓库 code/code_chapter4、code/code_chapter5、code/code_chapter6 为依据。以下列出对应版本的官方资料入口,供安装时查阅;本文不对在线页面当前的可用性或版本支持状态作保证。文中的历史版本仅用于复现,安装前仍需确认支持状态与安全维护要求。

  • 参考 1:NVIDIA《Workstation Installation》,Isaac Sim 5.1.0 文档,工作站安装与兼容性检查。地址:https://docs.isaacsim.omniverse.nvidia.com/5.1.0/installation/install_workstation.html。
  • 参考 2:NVIDIA《Python Environment Installation》,Isaac Sim 5.1.0 文档,内置 Python 3.11、包安装入口与应用初始化顺序。地址:https://docs.isaacsim.omniverse.nvidia.com/5.1.0/installation/install_python.html。
  • 参考 3:NVIDIA《Python Environment》,Isaac Sim 5.1.0 文档,独立脚本启动与 python.sh 的环境设置。地址:https://docs.isaacsim.omniverse.nvidia.com/5.1.0/python_scripting/manual_standalone_python.html。
  • 参考 4:NVIDIA《Isaac Sim Requirements》,Isaac Sim 5.1.0 文档,硬件、驱动和系统要求。地址:https://docs.isaacsim.omniverse.nvidia.com/5.1.0/installation/requirements.html。

Datawhale 开源教程