IK 与 Direction-Up IK

LambdaSim 的 Direction-Up IK 根据末端的目标位置、工作方向和 Up 方向求解关节坐标，适用于夹爪抓取、工具插接和末端姿态对齐。本文介绍 IK 基础、目标定义、使用步骤、参数和求解原理。

1. IK 基础

逆运动学（Inverse Kinematics，IK）根据末端的目标位置和朝向计算关节坐标。正运动学（Forward Kinematics，FK）根据已知关节坐标计算末端位置和朝向。

运算	输入	输出
FK	机器人关节坐标。	末端位置和朝向。
IK	末端目标位置和朝向。	满足目标约束的关节坐标。

图中以两根连杆表示平面位置关系。三维机械臂的 IK 还需要处理姿态约束、关节限位和参与求解的关节范围。本文使用 q 表示关节坐标向量；旋转关节使用弧度，移动关节使用长度单位。

同一个末端目标可以对应多组关节坐标。迭代求解器以当前关节状态为初始值，结果受初始姿势影响。超出工作范围、受关节限位阻挡或自由度不足的目标无法完全满足。

1.1 工具中心点

末端目标作用于工具中心点（Tool Center Point，TCP）坐标系。TCP 可以位于两根夹指之间，与 URDF 中夹爪 link 的原点分离。

使用 RobotKinematics.from_scene_object() 的 end_effector_local_xforms 配置末端 link 到 TCP 的局部变换：

TCP 世界变换 = 末端 link 世界变换 × link 到 TCP 的局部变换

IK 目标、FK 结果和视口操作柄应使用同一份 TCP 标定。

2. Direction-Up 目标

图中夹爪的 TCP 与把手的目标抓取点各有一套坐标轴。目标抓取位姿要求两个原点重合，并使三根同色轴同向。蓝色 Direction 表示夹指伸入空洞的方向，绿色 Up 沿把手长度，红色轴表示夹指跨把手厚度的闭合方向。把手上的目标坐标系位于选定的夹持区域，与箱体模型原点分离。

Direction-Up IK 使用以下三个目标量：

目标量	参数	含义
Position	position_world	TCP 原点的目标世界位置。
Direction	direction_world	工具工作轴的目标世界方向。
Up	up_world	用于确定绕 Direction 转角的第二个目标世界方向。

位置约束保留全部旋转自由度。增加 Direction 后，工具仍可绕该方向旋转；增加 Up 后，目标姿态被完整确定。

图中三个工具的中心位置和 Direction 相同，绕 Direction 的转角不同。Up 用于区分这些姿态。Up 属于工具参考轴，随目标姿态改变，不固定指向世界上方；在把手示例中，Up 沿把手长度。

约束方式	旋转约束
Position	不约束旋转。
Position + Direction（Axis IK）	对齐工作轴，保留绕该轴的旋转自由度。
Position + Direction + Up	完整约束 TCP 朝向。

对于允许工具自由绕工作轴旋转的任务，使用 Axis IK。对于需要固定夹指排列或工具滚转角的任务，使用 Direction-Up IK。

2.1 局部轴映射

direction_world 和 up_world 使用世界坐标。local_direction 和 local_up 指定与它们对应的 TCP 局部轴。

配置方式	Direction 对应的局部轴	Up 对应的局部轴
省略两个局部轴字段。	默认 +X。	默认 +Y。
显式传入两个局部向量。	local_direction。	local_up。
两个字段同时设为 None。	完整 TCP 约定的 +Z。	完整 TCP 约定的 +Y。

两个字段必须同时为向量，或同时为 None。仅将其中一个设为 None 会触发输入校验错误。

本文配图和使用示例采用完整 TCP 约定：

TCP +Z：Direction / Forward
TCP +Y：Up
TCP +X：Up × Direction

把手示例的 TCP 将 +X 标定为夹指闭合轴。夹爪安装位置和朝向由 end_effector_local_xforms 配置。

2.2 输入要求

目标位置和方向使用 ek.vec3，所有分量必须为有限值。局部与世界 Direction、Up 均须为非零向量，且同一组的两个方向不得平行或反平行。

接口会归一化方向，并将 Up 在垂直于 Direction 的平面上投影后归一化。输入长度不影响目标姿态。退化方向组合、非有限值或无效字段类型会触发输入校验错误。

3. 设置并求解 IK

本节使用已创建的 RobotKinematics 对象 robot。末端 ee_id=0 已登记 TCP，active_dof_ids 为允许调整的关节坐标索引元组。完整的机器人加载与标定步骤见 双臂 IK 和 Viewport 控制。

以下目标数值用于说明接口。目标的可达性取决于机器人模型、关节限位和初始状态。

3.1 创建末端目标

创建 DirectionUpTarget，设置目标世界位置和两根目标世界方向。将两个局部轴字段设为 None，使用完整 TCP 约定。

import ektro as ek
from lambdasim import kinematics as kin

# 完整 TCP 目标：+Z 朝世界 +X，+Y 朝世界 +Z。
# Complete TCP target: local +Z faces world +X; local +Y faces world +Z.
target = kin.DirectionUpTarget(
    ee_id=0,
    position_world=ek.vec3(0.45, -0.20, 0.75),
    direction_world=ek.vec3(1, 0, 0),
    up_world=ek.vec3(0, 0, 1),
    local_direction=None,
    local_up=None,
)

该目标要求 TCP 的 +Z 朝世界 +X，TCP 的 +Y 朝世界 +Z。由右手坐标系得到 TCP 的 +X 朝世界 +Y。

3.2 执行求解

调用 robo_solve_ik_direction_up_multi_batch()，将目标放入 candidates。以下调用求解一个候选中的一个末端目标。

# 一组候选，包含一个末端目标。One candidate containing one end-effector target.
result = kin.robo_solve_ik_direction_up_multi_batch(
    robot,
    candidates=((target,),),
    active_dof_ids=active_dof_ids,
    device="cpu",
    max_position_error=0.03,
    max_iters=32,
    position_tol=0.002,
    frame_tol_rad=0.02,
    max_step_norm=0.1,
)

接口返回 IKBatchResult，求解结果保存在 result.joint_q 中。求解过程不自动写回机器人或场景。

3.3 检查并应用结果

检查 best_index 后，将成功候选的关节坐标写入机器人，再更新 FK 并同步场景几何。

# 无成功候选时直接暴露失败。Require a converged candidate before applying it.
assert result.best_index is not None, "本轮没有达到精度要求的 IK 解"
ek.copy(robot.joint_q, result.joint_q[result.best_index, :])
robot.update_fk()
robot.sync_geo_pose()

执行后，机器人和场景几何使用选中候选的关节状态。没有成功候选时，本示例触发断言并保留原状态。

3.4 配置多末端与多候选

candidates 的外层表示独立的目标组合，内层表示同一组合中需要同时满足的末端目标：

((target,),)                         一个候选，一个末端
((left_target, right_target),)        一个候选，同时约束双臂
((left_A, right_A), (left_B, right_B)) 两个候选，各自同时约束双臂

同一批候选必须使用相同的末端数量、末端顺序和局部轴定义，每个候选中的末端编号不得重复。每个候选均从调用时的同一份机器人关节状态开始求解，候选之间不构成连续运动轨迹。

4. 参数与返回值

本节参数适用于 robo_solve_ik_direction_up_multi_batch()。

4.1 求解参数

参数	默认值	含义
max_position_error	必填。	迭代中的位置误差保护阈值。不是最终成功容差；接受规则见第 5.4 节。
active_dof_ids	None。	允许调整的关节坐标索引元组。None 表示全部关节坐标，未选中的关节保持输入值。
device	"cpu"。	执行求解的显式逻辑设备。
root_world_xform	None。	FK 根节点的世界变换。省略时使用已绑定场景对象的根世界变换；未绑定对象时使用单位矩阵。
max_iters	32。	每个候选的最大迭代次数，须为正整数。
position_tol	1e-3。	每个末端的位置误差须严格小于该值。米制场景中，0.002 对应 2 mm。
frame_tol_rad	1e-2。	每个末端的 Direction 和对齐后 Up 的角误差均须严格小于该值，单位为弧度。0.02 rad 约为 1.15°。
position_weight	1.0。	位置误差分量的权重。
rotation_weight	1.0。	Direction 与 Up 角误差分量的权重。
damping	1e-3。	阻尼最小二乘的对角线增量。
max_step_norm	0.25。	每轮整组活动关节调整量的范数上限，不是单个关节的角度上限。

容差、权重、阻尼、位置保护阈值和步长上限均须为有限正数。权重影响优化过程，成功判断使用未加权误差。

4.2 求解结果

设候选数量为 B，机器人关节坐标数量为 D，返回字段如下：

字段	类型或形状	含义
joint_q	float32[B, D]。	各候选的最终关节坐标。
success	int32[B]。	1 表示该候选的所有末端均满足位置和角度容差，0 表示未满足。
cost	float64[B]。	成功候选相对于初始状态的活动关节坐标变化平方和；失败候选为无穷大。
best_index	int 或 None。	成功候选中 cost 最小者的索引。没有成功候选时为 None。

结果数组保留在 device 指定的设备上。cost 用于选择关节变化最小的成功候选，与优化过程中的末端误差平方和不同。

5. 求解原理

Direction-Up IK 以当前关节状态为初值，反复执行 FK、误差计算和受限关节更新。

5.1 目标坐标系构造

使用归一化、点积和叉积，将 Direction 与 Up 构造成正交坐标轴：

d  = normalize(Direction)
u0 = normalize(Up)
u  = normalize(u0 - dot(u0, d) * d)
x  = normalize(cross(u, d))
u  = cross(d, x)

d 为工作方向，u 为正交化后的 Up，x 为第三根轴。完整 TCP 约定下，目标旋转矩阵由三根世界方向按列组成：

R_target = [ x | u | d ]

显式指定其它局部轴时，先分别构造局部与世界方向基，再计算 TCP 的目标旋转：

F_local = [ d_local | cross(u_local, d_local) | u_local ]
F_world = [ d_world | cross(u_world, d_world) | u_world ]
R_target = F_world × transpose(F_local)

上述方向均已经归一化和正交化。局部轴映射改变工具轴与目标轴的对应关系，TCP 安装变换仍由 end_effector_local_xforms 表达。

5.2 位置与姿态误差

FK 计算当前 TCP 的世界变换，并将局部 Direction、Up 变换为当前世界方向。每个末端的误差包括以下三项：

e_position  = 当前 TCP 位置 - 目标位置
e_direction = axis_error(当前 Direction, 目标 Direction)
aligned_up  = R_align × 当前 Up
e_up        = axis_error(aligned_up, 目标 Up)

R_align 是将当前 Direction 对齐到目标 Direction 的最小旋转。axis_error 返回三维角误差向量，其长度为两轴夹角，单位为弧度。两轴反向时，使用确定的正交轴表示 π 角误差。

先将 Direction 对齐后再评估 Up，可使 Up 误差表示剩余的绕轴转动。此顺序用于构造误差；每轮关节更新同时考虑位置、Direction 和 Up。

加权误差向量为：

r(q) = [ position_weight × e_position,     3 个分量
         rotation_weight × e_direction,   3 个分量
         rotation_weight × e_up ]         3 个分量

每个末端贡献九个误差分量。该表达不增加独立自由度，刚体位姿仍包含三个平移自由度和三个旋转自由度。

5.3 关节调整量

求解器通过有限差分计算误差对活动关节坐标的 Jacobian（雅可比矩阵）。对每个活动关节施加 ε=1e-4 的坐标扰动，重新执行 FK 并计算误差：

J 的第 j 列 ≈ [ r(q + ε × 第 j 个关节的单位步) - r(q) ] / ε

r(q + Δq) ≈ r(q) + J × Δq

扰动量使用对应关节的坐标单位。active_dof_ids 决定 Jacobian 包含的关节列。

求解器使用阻尼最小二乘（Damped Least Squares，DLS）计算调整量。忽略关节限位时，对应线性方程为：

(transpose(J) × J + damping × I) × Δq = -transpose(J) × r

damping 直接加到矩阵对角线上，用于缓解奇异姿态附近的数值问题。实际求步还使用关节上下限约束，并通过 max_step_norm 限制整组调整量的范数。

5.4 步长接受与停止条件

候选步长按 1、1/2、1/4、…、1/128 缩放。接受的候选必须使加权误差平方和下降，并满足位置保护规则：

当前所有末端中的最大位置误差	新一步的位置要求
不大于 max_position_error。	新的最大位置误差不得超过该阈值。
大于 max_position_error。	新的最大位置误差必须严格减小。

max_position_error 允许机器人从阈值以外逐步接近目标，不限制初始目标距离。最终成功条件由 position_tol 和 frame_tol_rad 决定。

候选达到容差后完成求解。没有可接受步长、关节步长过小或迭代预算用完时，停止更新，并按最终误差判断是否成功。阻尼和步长控制不保证不可达目标收敛。

5.5 多末端联合求解

同一个候选中的多个末端共享一份 q，其误差和 Jacobian 按末端顺序拼接：

r_total = [ 左手的位置、Direction、Up 误差,
            右手的位置、Direction、Up 误差 ]

一次关节更新同时考虑所有末端目标。对共享腰部等关节的双臂分别调用单末端 IK，会使后一次求解改变前一次的结果。

在双臂 Gizmo 交互中，更新被拖动一侧的目标，保留另一侧目标，再将两个目标放入同一候选联合求解。腰部、升降或底盘是否参与调整，由 active_dof_ids 控制。

6. 失败处理与使用范围

根据返回值或异常类型检查输入和求解状态：

状态或现象	处理方法
方向输入校验失败。	检查类型、有限值、非零长度，以及 Direction 与 Up 的非平行条件。
局部轴配置校验失败。	将两个局部轴字段同时设为向量或同时设为 None，并统一各候选的轴定义。
初始活动关节越过限位。	在求解前将机器人设置为符合模型限位的初始状态。
success 为 0，或 best_index 为 None。	检查目标可达性、活动关节范围、限位、初始姿态和容差，再重新求解。
抛出 FloatingPointError。	按异常列出的候选索引检查引发数值失败的目标和模型数据。
返回成功结果但场景未更新。	将选中候选写入 robot.joint_q，再调用 update_fk() 和 sync_geo_pose()。

Batch 接口支持固定、旋转和移动关节；robot.joint_q 使用一维 float32 关节坐标数组。

Direction-Up IK 仅求解关节状态。碰撞检测、动力学、执行时间和速度规划由对应模块处理。直接应用 IK 结果会更新姿态，不会生成连续运动轨迹。

7. 相关文档

• 双臂 IK 和 Viewport 控制：机器人加载、TCP 标定和 Gizmo 交互示例。
• DirectionUpTarget 与结果类型：目标字段和结果类型。
• Direction-Up Batch IK 实现：Batch 参数、求解和结果选择实现。
• 103.3 Axis 与 Direction-Up IK 设计：方向误差与多末端约束设计。
• 104.1.1 GripperSlot 分组末端位姿采样设计：完整 TCP 约定和 Batch 参数修订。