Three.js 如何旋转和推拉关节

机器人模型加载进来之后，下一步是让它动。这篇不讲 URDF 怎么解析，只讲一件事：手上有一个关节角度、或者一段推拉距离，怎么在 Three.js 里让对应那截模型动起来。

1. 动的是关节那一层

上一篇把 <joint> 和 <link> 交替挂成了一条链：pelvis → left_hip_yaw_joint → left_hip_yaw_link → left_hip_pitch_joint → …。所以最后整个机器人就形成了一棵 Three.js 的场景树。

这样做的好处是：后面只要改变某一个 joint 的 position 或 quaternion，它下面的 link 和后续 joint 都会利用 Three.js 的父子矩阵自动跟着变化，不需要自己重新递归算整条机械臂的世界矩阵。

要动的只有 joint 节点，连杆和几何体都不用碰。剩下的内容就是一件事：给关节节点算出正确的 quaternion（旋转）或 position（推拉）。

2. 关节只有两种动法

不管关节叫什么名字，动起来只有两种：

动法	要改的属性
旋转：绕一根轴转一个角度	quaternion
推拉：沿一根轴移动一段距离	position

至于是哪一种，由 URDF 里的 <joint type="..."> 决定：

<joint type="...">	动法	这个数是什么
revolute	绕轴转，有角度范围	弧度
continuous	绕轴转，没有范围	弧度
prismatic	沿轴移动，有距离范围	米
fixed	不动	—

轴来自 <joint> 里的 <axis xyz="...">，范围来自 <limit lower="..." upper="...">。

两个容易踩的地方：

一、角度是弧度，不是度。 URDF 里写 <limit lower="-3.14" upper="2.5">，这个 -3.14 是 π 的近似值，不是 180。代码里要写 Math.PI / 2，不要写 90。

二、值是绝对位置。 传 0 是回到加载时的姿态，传 0.5 是绕轴 0.5 弧度 —— 和之前传过什么无关，也不需要自己在当前值上做加减。这一条决定了后面整套代码长什么样，理由放在第 5 节。

下面两节分别讲这两种改法。

3. 旋转关节：setFromAxisAngle + premultiply

先看最直接的那一行

绕某个轴转 n 弧度，对应到 Three.js 就是给节点设一个新的 quaternion：

joint.quaternion.setFromAxisAngle(axis, n)

axis 是 <axis xyz="..."> 里那个方向，n 是弧度。setFromAxisAngle 返回的是 this，所以可以接着链式调用。

但这一行不够。 它把 quaternion 整个覆盖掉，相当于认为”关节在零位时朝向是没转过的”。实际上很多关节的 <origin rpy> 不是零：

<joint name="left_hip_pitch_joint" type="revolute">
  <origin xyz="0 0.06 0" rpy="1.5707963267948966192313216916398 0 0"/>
  <axis xyz="0 1 0"/>
  ...

这个关节的零位 roll 是 90°，本身就是带朝向的。直接覆盖，安装朝向就丢了，后面无论怎么转都是歪的。

完整写法

joint.quaternion.setFromAxisAngle(axis, n).premultiply(origQuaternion)

origQuaternion 是关节零位时的朝向，加载时先存一份：

joint.userData.origQuaternion = joint.quaternion.clone()

premultiply 是左乘

Quaternion 上有两个长得很像的方法：

方法	等价于	乘在哪边
multiply(q)	this = this × q	右乘
premultiply(q)	this = q × this	左乘

四元数乘法不满足交换律，a × b 和 b × a 是两个完全不同的姿态。所以上面那行算出来的是：

R(origin.rpy) × R(axis, n)

也就是「先绕 axis 转 n，再套上零位的安装朝向」。这正是 URDF 规定的关节变换。

为什么必须左乘：旋转发生在哪个坐标系

R(a) × R(b) 作用到一个向量上是先 b 后 a。所以 R(axis, n) 作用在最内层的坐标系 —— 关节自己的坐标系。

而 URDF 里 <axis xyz> 的定义恰好就是”关节坐标系下的方向”。两个对上了，axis 才会按你写的方向转。

反过来写：

R(axis, n) × R(origin.rpy)     ← 错

R(axis, n) 就跑到父坐标系里去了，轴的方向跟着变。拿上面那个 left_hip_pitch_joint 算一遍：<origin rpy> 的 roll 是 90°，<axis xyz="0 1 0"> 在关节坐标系里指的是 Y；叠上 90° 的安装朝向后，在父坐标系里这个轴指向 Z。左乘得到的正是绕 Z 转，右乘会变成绕父坐标系的 Y 转 —— 差 90°。

<origin rpy> 全是零的关节看不出区别。 那类关节的 origQuaternion 是单位四元数，单位 × R = R × 单位，两种写法结果一模一样。所以这个错误只在部分关节上现形。

另一种等价写法

const delta = new Quaternion().setFromAxisAngle(axis, n)
joint.quaternion.copy(origQuaternion).multiply(delta)

先把零位朝向 copy 回去，再在右边乘上关节转角。得到的还是 R(origin.rpy) × R(axis, n)。

两种写法只差中间结果放在哪：一个是先覆盖 quaternion 再左乘，另一个是放在独立的 delta 上再右乘。选前者是因为 setFromAxisAngle 本来就要写一次 quaternion，少一次赋值。

改完 quaternion，rotation 会自动同步

quaternion 和 rotation 是同一个旋转的两种表示，写其中一个，另一个自动更新，不用手动对齐。

顺带一提，quaternion 不是 Group 才有的属性，所有 Object3D（Mesh、Camera、Scene）都有。

4. 推拉关节：改 position 之前先把轴转过来

最直接的那一行

沿关节轴平移 n 米：

joint.position.copy(origPosition)
joint.position.addScaledVector(axis, n)

结构和旋转一样：先回到零位，再走 n。但这里有一处不一样，也是推拉最容易写错的地方。

position 和 axis 不在同一个坐标系

• position 是节点相对父节点的位置
• <axis xyz> 是关节坐标系下的方向

两者中间差了一个 <origin rpy>。所以要把轴先转过去：

// 实际项目里 _tempAxis 是模块级常量，避免每帧堆分配
const _tempAxis = new Vector3()
_tempAxis.copy(axis).applyQuaternion(joint.quaternion)
joint.position.addScaledVector(_tempAxis, n)

applyQuaternion(q) 表示用四元数 q 旋转这个向量（原地修改）。joint.quaternion 此刻正是零位朝向，所以：

_tempAxis = R(origin.rpy) · axis

这就是 axis 在父坐标系里的方向，再拿去平移就对了。

为什么旋转关节不需要这一步

对照一下两个分支改的属性：

要改的属性	属性表达在	<axis> 表达在	要不要先转轴
quaternion	关节坐标系	关节坐标系	不用
position	父坐标系	关节坐标系	要

旋转那支改的是 quaternion，axis 和它本来就在同一层坐标系，所以 setFromAxisAngle 直接吃 axis 就行。推拉那支改的是 position，跨了一层，必须先转。

addScaledVector：沿某个方向走一段

addScaledVector(v, s)   // this = this + v × s

展开就是三个分量各加一遍：

this.x += v.x * s
this.y += v.y * s
this.z += v.z * s

所以 position.addScaledVector(_tempAxis, n) 读作「从当前位置出发，沿 _tempAxis 方向走 n 米」。

它是原地修改，不产生临时对象 —— 比 position.add(_tempAxis.clone().multiplyScalar(n)) 少一次 clone() 的堆分配。一帧里几十个关节都要走一遍，能省则省。

5. 每次都从零位重算，不累加

把两个分支放一起看：

// 推拉
joint.position.copy(origPosition)
joint.position.addScaledVector(_tempAxis, n)

// 旋转
joint.quaternion.setFromAxisAngle(axis, n).premultiply(origQuaternion)

两边都是 「零位 + 当前值」，不是「当前位置 + 增量」。origPosition / origQuaternion 是加载时从 <origin> 存下来的底稿。

为什么不在当前位置上再转个 delta？两个原因：

一、误差累积。 每一次浮点乘法都会留下舍入误差，累加写法让这些误差一直留在 quaternion 里。滑条来回拖几十次之后，传 0 未必回得到加载时那个姿态，而且偏多少完全不可预测。

二、不支持跳转。 动画数据是逐帧给的，播放器随时可能从第 10 帧跳到第 500 帧。累加写法必须把中间每一帧都算过一遍；从零位重算不需要知道历史。

这也解释了为什么 n 可以随便给大 —— 传 10 弧度就是绕轴 10 弧度，和之前转过什么毫无关系。

6. 轴的方向就是正负号

h1_2.urdf 里 <axis> 的取值分布：

19 个  <axis xyz="0 0 1"/>
12 个  <axis xyz="0 0 -1"/>
12 个  <axis xyz="0 1 0"/>
 8 个  <axis xyz="1 0 0"/>

注意 0 0 -1 和 0 0 1 数量相当。同样的正数 n，轴写成 0 0 -1 的关节转向完全相反。

所以某个关节转反了，第一个该看的是 <axis xyz> 的符号，而不是去怀疑四元数算错了。

<origin xyz> 里的平移也一样 —— left_hip_yaw_joint 的 xyz="0 0.0875 -0.1632"，z 是负的，它描述的是零位在哪，不是往哪个方向运动。

上面统计里还省了一行 <axis xyz="0 0 0"/>。零向量定不出旋转轴，setFromAxisAngle 拿到它会算出无效四元数 —— 这是模型数据的问题，不是代码的问题。

7. 完整的一次调用

// 关节名的索引在加载时就建好了
const joint = robot.userData.jointMap.get('left_hip_pitch_joint')

// 给一个绝对角度（弧度），或者一个绝对距离（米）
joint.setValue(0.5)

// 剩下来的交给 Three.js：下一帧渲染前它会把矩阵重算一遍

robot.userData.jointMap 是 Map<关节名, 关节节点>，robot.setJointValue(name, n) 做的就是查这张表再调 setValue。

参考

• Three.js Quaternion（setFromAxisAngle / multiply / premultiply）：https://threejs.org/docs/#api/en/math/Quaternion
• Three.js Vector3.addScaledVector：https://threejs.org/docs/#api/en/math/Vector3
• Three.js Object3D（quaternion 与 matrixWorld）：https://threejs.org/docs/#api/en/core/Object3D
• URDF XML 规范中 <joint> 的 axis、origin、limit：https://wiki.ros.org/urdf/XML/joint
• 上一篇：Three.js 如何解析 URDF 机器人模型 —— https://wywzixin.github.io/threejs/urdf-parsing/
• 下一篇：Three.js 如何用鼠标拖动关节 —— https://wywzixin.github.io/threejs/urdf-joint-drag/