课程链接：www.bilibili.com/cheese/play…
课程目标
- 理解URDF 文件的解析原理
- 创建自己的URDFLoader，加载URDF文件
1-URDF 文件的解析原理
URDF (Unified Robot Description Format, 统一机器人描述格式) 是一种基于 XML 的文件格式，专门用于在 ROS (机器人操作系统)中描述机器人的物理结构、运动学与动力学特性。
URDF的可视化是重要且灵活多变的，掌握URDF的解析方法是我们必须要具备的能力。
通过一个简单示例说一下URDF 文件的解析原理。
1.在项目中建立一个简单的测试文件。
- /public/models/test/01.urdf
<?xml version="1.0"?>
<robot name="myfirst">
<link name="base_link">
<visual>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
</visual>
</link>
</robot>
2.我们可以使用fetch请求上面的URDF文件，并将其转换为DOM。
在上节课创建的vue3项目的App.vue 中写fetch 请求。
/* 机器人可视化 */
const hdrURL = "/texture/venice_sunset_1k.hdr";
let robotVisual = new RobotVisual(hdrURL);
robotVisual.continuousRender();
fetch('/public/models/test/01.urdf')
.then(res=>{
if (res.ok) {
return res.text();
} else {
throw new Error(
`URDFLoader: Failed to load url with error code ${res.status} : ${res.statusText}.`
);
}
})
.then((data) => {
const parser = new DOMParser();
const urdf = parser.parseFromString(data, "application/xml");
console.log(urdf);
})
.catch((e) => {
console.error("URDFLoader: Error loading file.", e);
});
3.解析DOM 对象中的元素。
fetch('/public/models/test/01.urdf')
.then(res=>{
console.log(res);
if (res.ok) {
return res.text();
} else {
throw new Error(
`URDFLoader: Failed to load url with error code ${res.status} : ${res.statusText}.`
);
}
})
.then((data) => {
const parser = new DOMParser();
const urdf = parser.parseFromString(data, "application/xml");
// 机器人
const urdfRobot = new Group();
// 获取urdf 中的cylinder
const cylinderNode=urdf.getElementsByTagName('cylinder')[0]
if(!cylinderNode){return}
// 获取中的cylinder 中的属性
const radius = parseFloat(cylinderNode.getAttribute("radius") || "0");
const length = parseFloat(cylinderNode.getAttribute("length") || "0");
// 将中的cylinder 可视化
const cylinderMesh = new Mesh(
new CylinderGeometry(radius, radius, length, 12),
new MeshStandardMaterial({
color: 0xcccccc,
metalness: 1,
roughness: 0.1,
})
);
// 设置圆柱的高度方向，urdf中，圆柱的高度方向是z向，在webgl 中，圆柱的高度方向是y向
// 需要绕webgl的x轴逆时针旋转90°
cylinderMesh.rotation.set(Math.PI / 2, 0, 0);
urdfRobot.add(cylinderMesh);
robotVisual.scene.add(urdfRobot);
// 调整机器人坐标系，urdf中的z轴为高度方向，webgl中y轴为高度方向
// 需要绕webgl的x轴顺时针旋转90°
urdfRobot.rotation.set(-Math.PI / 2, 0, 0);
// 将机器人的最底部对齐到地面
const bb = new Box3();
bb.setFromObject(urdfRobot);
urdfRobot.position.y -= bb.min.y;
})
.catch((e) => {
console.error("URDFLoader: Error loading file.", e);
});
效果如下：
解析URDF的基本原理就是这样。
当然，实际的解析方法会更复杂，但这也只是一些复杂的业务逻辑，难度并不大。
接下来，我会重点解释如何用three.js 把URDF的图形树可视化。
在可视化方面，我们这一章只考虑link 中的visual 对象的可视化。
collision、inertial等辅助对象的可视化，我们在后面的课程中详解。
2-从URDF中解析的图形类
我已经提前准备好了两个URDF模型：宇树的H1 机器人和一个ros2的PR2 机器人。这两个模型都是开源的，大家可以在我的源码中看到。
若大家想用宇树的其它模型，可以去宇树官网下载。
在URDF 中，有些标签是可以理解为图形对象或图形集合的，比如 可以解析为three.js中的CylinderGeometry 。
接下来，我们以宇树的H1机器人为例，说一下如何根据URDF内容创建图形类。
<robot name="h1_2">
<mujoco>
<compiler meshdir="meshes" discardvisual="false"/>
</mujoco>
<!-- [CAUTION] uncomment when convert to mujoco -->
<!-- <link name="world"></link>
<joint name="floating_base_joint" type="floating">
<parent link="world"/>
<child link="pelvis"/>
</joint> -->
<link name="pelvis">
<inertial>
<origin xyz="-0.0004 3.7E-05 -0.046864" rpy="0 0 0"/>
<mass value="5.983"/>
<inertia ixx="49168.411E-06" ixy="-19.869E-06" ixz="-48.460E-06" iyy="9025.844E-06" iyz="3.431E-06" izz="53155.891E-06"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/pelvis.STL"/>
</geometry>
<material name="dark">
<color rgba="0.1 0.1 0.1 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.05"/>
</geometry>
</collision>
</link>
<!-- L Leg -->
<link name="left_hip_yaw_link">
<inertial>
<origin xyz="0 -0.026197 0.006647" rpy="0 0 0"/>
<mass value="2.829"/>
<inertia ixx="4553.605E-06" ixy="0.005E-06" ixz="0.817E-06" iyy="5688.730E-06" iyz="-345.157E-06" izz="3548.907E-06"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/left_hip_yaw_link.STL"/>
</geometry>
<material name="dark">
<color rgba="0.1 0.1 0.1 1"/>
</material>
</visual>
<collision>
<origin xyz="0.02 0 0" rpy="0 1.5707963267948966192313216916398 0"/>
<geometry>
<cylinder radius="0.01" length="0.02"/>
</geometry>
</collision>
</link>
<joint name="left_hip_yaw_joint" type="revolute">
<origin xyz="0 0.0875 -0.1632" rpy="0 0 0"/>
<parent link="pelvis"/>
<child link="left_hip_yaw_link"/>
<axis xyz="0 0 1"/>
<limit lower="-0.43" upper="0.43" effort="200" velocity="23"/>
</joint>
...
</robot>
根据urdf 中的标签名，可以生成相应的图形类，如：
-
- URDFRobot
-
- URDFLink
- 中的 - LinkVisual
-
- URDFJoint
- 包含的 - URDFMimicJoint
根据urdf 标签的层级结构，可以生成相应的图形树：
本课程会用到两种图形：实际可见的图形和辅助图形。
实际可见的图形就是中的 图形。
辅助图形是以下4种：
- 坐标系： 的本地坐标系
- 碰撞体：中的
- 质心：中的中的
- 惯性矩：中的中的
坐标系可以作为URDFJoint的子对象，其余的可以作为LinkVisual 的子对象。
在根据URDF标签创建完相应three.js图形后，我还需要在图形上挂在相关数据，比如中的数据。
我会将这种数据存储在three.js图形的userData 对象中，以避免与原始three.js图形的属性混淆。
接下来我创建一些核心图形类。
- /src/robot/URDFClasses.ts
import { Group, Object3D, Quaternion, Vector3 } from 'three'
const _tempAxis = new Vector3()
/* <robot> URDF机器人类，图形树的根节点 */
export class URDFRobot extends Group {
userData: {
isURDFRobot: true
linkMap: Map<string, URDFLink>
jointMap: Map<string, URDFJoint>
jointAxisMap: Map<string, Object3D>
massMap: Map<string, Object3D>
inertiaMap: Map<string, Object3D>
collisionMap: Map<string, Object3D>
[k: string]: any
} = {
// 是否是URDFRobot 对象
isURDFRobot: true,
// link 图形集合
linkMap: new Map(),
// joint 图形集合
jointMap: new Map(),
// 辅助对象-关节坐标系集合
jointAxisMap: new Map(),
// 辅助对象-质心集合
massMap: new Map(),
// 辅助对象-惯性集合
inertiaMap: new Map(),
// 辅助对象-碰撞体集合
collisionMap: new Map(),
}
// 设置关节的旋转值或推拉值
setJointValue(name: string, n: number) {
const joint = this.userData.jointMap?.get(name)
if (joint) {
joint.setValue(n)
} else {
console.warn(`Joint ${name} not found in robot ${this.name}`)
}
}
/* 深拷贝 */
copy(source: URDFRobot) {
super.copy(source)
// 深拷贝Map
const { userData } = this
const mapKeys = [
'linkMap',
'jointMap',
'jointAxisMap',
'massMap',
'inertiaMap',
'collisionMap',
]
for (let k of mapKeys) {
userData[k] = new Map()
}
this.traverse((child: Object3D) => {
if (child.userData.isURDFLink) {
userData.linkMap.set(child.name, child as URDFLink)
} else if (child.userData.isURDFJoint) {
userData.jointMap.set(child.name, child as URDFJoint)
} else if (child.userData.isURDFHelper) {
const name = child.parent?.name || ''
switch (child.userData.helperType) {
case 'jointAxisHelper':
userData.jointAxisMap.set(name, child)
break
case 'massHelper':
userData.massMap.set(name, child)
break
case 'inertiaHelper':
userData.inertiaMap.set(name, child)
break
case 'collisionHelper':
userData.collisionMap.set(name, child)
break
}
}
})
return this
}
}
/* <link >*/
export class URDFLink extends Group {
userData: {
isURDFLink: true
[k: string]: any
} = {
isURDFLink: true,
}
}
/* <link>中的<visual> */
export class LinkVisual extends Group {
userData: {
isLinkVisual: true
[k: string]: any
} = {
isLinkVisual: true,
}
}
/*
关节类型
fixed：固定关节，不可旋转或移动
continuous：连续关节，可无限旋转
revolute：旋转关节，可在一定范围内旋转
prismatic：推拉关节，可在一定范围内移动
planar：平面关节，可在平面内移动
floating：浮动关节，可在三维空间内自由移动和旋转
*/
export type JointType =
| 'fixed'
| 'continuous'
| 'revolute'
| 'planar'
| 'prismatic'
| 'floating'
interface URDFJointDataType {
isURDFJoint: true
// 关节类型，对应<joint type="..."> 中的type属性
type: JointType
// 旋转轴，对应<joint>中的<axis>
axis: Vector3
// 当前关节的变换值
value: number
// 关节限值，对应<joint>中的<limit>
limit: { lower: number; upper: number }
// 是否忽略关节限值
ignoreLimits: Boolean
// mimic关节集合，其中的关节会模仿当前关节的运动
mimicJoints: URDFMimicJoint[]
// 初始位置，对应<joint>中<origin rpy="0 0 0" xyz="0 0 0.0325"> 的xyz属性
origPosition: Vector3
// 初始旋转量，对应<joint>中<origin rpy="0 0 0" xyz="0 0 0.0325"> 的rpy属性
origQuaternion: Quaternion
[k: string]: any
}
/* <joint> */
export class URDFJoint extends Group {
userData: URDFJointDataType = {
isURDFJoint: true,
type: 'fixed',
axis: new Vector3(1, 0, 0),
value: 0,
limit: { lower: 0, upper: 0 },
ignoreLimits: false,
mimicJoints: [],
origPosition: this.position.clone(),
origQuaternion: this.quaternion.clone(),
}
/* 设置关节的旋转值或推拉值 */
setValue(n: number) {
const {
mimicJoints,
type,
value,
ignoreLimits,
limit: { lower, upper },
axis,
origPosition,
origQuaternion,
} = this.userData
// 更新模仿此关节的mimic关节
mimicJoints.forEach((joint) => {
joint.updateFromMimickedJoint(n)
})
// 只考虑revolute,continuous,prismatic 类型的关节
if (
n == null ||
n === value ||
!['revolute', 'continuous', 'prismatic'].includes(type)
) {
return
}
// 限值对continuous关节无效，只对revolute 和prismatic 关节生效
if (type != 'continuous' && !ignoreLimits) {
n = Math.max(lower, Math.min(upper, n))
}
// 存储旋转值
this.userData.value = n
if (type == 'prismatic') {
// 关节初始位置
this.position.copy(origPosition)
// 推拉方向
_tempAxis.copy(axis).applyQuaternion(this.quaternion)
// 从origPosition，沿_tempAxis方向移动n 距离
this.position.addScaledVector(_tempAxis, n)
} else {
// 在初始四元数的基础上，绕axis旋转
this.quaternion.setFromAxisAngle(axis, n).premultiply(origQuaternion)
}
}
/* 深拷贝 */
copy(source: URDFRobot) {
super.copy(source)
this.userData.axis = source.userData.axis.clone()
this.userData.origPosition = source.userData.origPosition.clone()
this.userData.origQuaternion = source.userData.origQuaternion.clone()
return this
}
}
/* mimic joint */
export class URDFMimicJoint extends URDFJoint {
// joint,multiplier,offset 对应<mimic joint="..." multiplier="..." offset="..."> 中的属性
userData: URDFJointDataType & {
joint?: string
multiplier: number
offset: number
} = {
isURDFJoint: true,
type: 'fixed',
axis: new Vector3(1, 0, 0),
value: 0,
limit: { lower: 0, upper: 0 },
ignoreLimits: false,
mimicJoints: [],
origPosition: this.position.clone(),
origQuaternion: this.quaternion.clone(),
offset: 0,
multiplier: 0,
}
/* 根据被模仿关节更新mimic关节 */
updateFromMimickedJoint(x: number) {
const { multiplier, offset } = this.userData
this.setValue(x * multiplier + offset)
}
}
代码详解
URDFRobot
对应，是图形树的根节点。
URDFRobot 具有多个图形集合，可以对link 图形、joint 图形和辅助对象进行统一管理。
// link 图形集合
linkMap:new Map(),
// joint 图形集合
jointMap:new Map(),
// 辅助对象-关节坐标系集合
jointAxisMap:new Map(),
// 辅助对象-质心集合
massMap:new Map(),
// 辅助对象-惯性集合
inertiaMap:new Map(),
// 辅助对象-碰撞体集合
collisionMap:new Map(),
setJointValue(name: string, value: number) 根据关节名称设置关节的旋转值或推拉值。
URDFLink
对应，其children 中会有 ,,, 的可视化图形。
URDF 中的 示例：
<link name="base">
<inertial>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
<mass value="0.01"/>
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
URDFJoint
对应，其children 中会有坐标系对象。
URDF 中的 示例：
<joint name="R_pinky_finger_distal_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0.0325"/>
<parent link="R_pinky_finger_proximal"/>
<child link="R_pinky_finger_distal"/>
<axis xyz="0 1 0"/>
<limit effort="1000" lower="0.33811" upper="3.58322" velocity="1"/>
<mimic joint="R_pinky_finger_proximal_joint" multiplier="1.005" offset="0.6665"/>
</joint>
对应上面的数据，URDFJoint具备以下属性：
isURDFJoint: true
// 关节类型，对应<joint type="..."> 中的type属性
type:JointType
// 旋转轴，对应<joint>中的<axis>
axis: Vector3
// 当前关节的旋转弧度
value: number
// 关节限值，对应<joint>中的<limit>
limit: { lower: number; upper: number }
// 是否忽略关节限值
ignoreLimits: Boolean
// mimic关节集合，其中的关节会模仿当前关节的运动
mimicJoints: URDFMimicJoint[]
// 初始位置，对应<joint>中<origin rpy="0 0 0" xyz="0 0 0.0325"> 的xyz属性
origPosition: Vector3
// 初始旋转量，对应<joint>中<origin rpy="0 0 0" xyz="0 0 0.0325"> 的rpy属性
origQuaternion: Quaternion
常见的joint 类型有6 种：
- fixed：固定关节，不可旋转或移动
- continuous：连续旋转关节，可无限旋转
- revolute：旋转关节，可在一定范围内旋转
- prismatic：推拉关节，可在一定范围内移动
- planar：平面关节，可在平面内移动
- floating：浮动关节，可在三维空间内自由移动和旋转
URDFJoint 的setValue 方法可以设置关节的旋转值或推拉值。
目前setValue 方法不兼容planar和floating 关节，而fixed 关节因为是固定关节，所以无需改变。
当joint 类型为prismatic 时，joint 可以从一个基点，沿特定方向位移。
// 关节初始位置
this.position.copy(origPosition)
// 推拉方向
_tempAxis.copy(axis).applyQuaternion(this.quaternion)
// 从origPosition，沿_tempAxis方向移动value 距离
this.position.addScaledVector(_tempAxis, value)
当joint 类型为continuous时，joint 可以从一个初始方向，沿特定轴旋转。
this.quaternion.setFromAxisAngle(axis, value).premultiply(origQuaternion)
URDFMimicJoint
会模仿其它joint进行变换的关节。
URDF 中的 示例：
<joint name="R_pinky_finger_distal_joint" type="fixed">
……
<mimic joint="R_pinky_finger_proximal_joint" multiplier="1.005" offset="0.6665"/>
</joint>
中的属性：
- joint ：当前joint 要模仿的关节
- multiplier：模仿系数
- offset：模仿偏移值
模仿方法如下：
updateFromMimickedJoint(x:number) {
const { multiplier, offset } = this.userData
this.setValue(x * multiplier + offset)
}
3-创建URDFLoader 类
我们接下来要说的URDFLoader 类，是参照Garrett Johnson 的urdf-loader 写的，我在其中加入了更多的功能，比如辅助对象的可视化。
3-1-URDF的整体解析思路
1.请求urdf文件，将urdf 转document 对象。
2.将的子元素分成material、link、joint 3类。
3.将urdf中的material 转three.js material。
4.将urdf中的link转three.js 图形，并根据其材质名，匹配上一步解析出的材质。
5.将urdf中的joint转three.js 图形，并根据joint 中的父子关系关联link图形。
6.找到link 图形的根节点，将其添加到URDFRobot图形中。
3-2-代码
URDFLoader 类的主要作用就是解析URDF 文件，其整体代码如下：
- src/robot/URDFLoader.ts
import {
Vector3,
DefaultLoadingManager,
Group,
LoadingManager,
Material,
Mesh,
MeshStandardMaterial,
Object3D,
SRGBColorSpace,
TextureLoader,
BoxGeometry,
SphereGeometry,
CylinderGeometry,
} from 'three'
import { STLLoader } from 'three/examples/jsm/loaders/STLLoader.js'
import { ColladaLoader } from 'three/examples/jsm/loaders/ColladaLoader.js'
import {
URDFJoint,
URDFLink,
URDFMimicJoint,
URDFRobot,
LinkVisual,
type JointType,
} from './URDFClasses'
import { applyEulerZYX, classifyNodeByName, processTuple } from './utils'
type NumberTuple3 = [number, number, number]
type NumberTuple4 = [number, number, number, number]
/* URDF加载器 */
class URDFLoader {
// loader管理器
manager: LoadingManager
// fetch请求参数
fetchOptions: { [k: string]: any } = {}
// 模型加载方法
meshParsers: {
[k: string]: (
filePath: string,
material: Material,
manager?: LoadingManager,
) => Promise<Object3D | void>
} = {
stl: (
filePath: string,
material: Material,
manager: LoadingManager = this.manager,
) => {
const loader = new STLLoader(manager)
return loader.loadAsync(filePath).then(
(geom) => {
const mesh = new Mesh(geom, material)
mesh.castShadow = true
return mesh
},
(err) => {
console.error('URDFLoader: Error loading mesh.', err)
},
)
},
dae: (
filePath: string,
material: Material,
manager: LoadingManager = this.manager,
) => {
const loader = new ColladaLoader(manager)
return loader.loadAsync(filePath).then(
(dae) => {
if(!dae || !dae.scene) {return}
const daeScene = dae.scene
daeScene.traverse((obj) => {
if (obj instanceof Mesh) {
obj.material = material
}
})
return daeScene
},
(err) => {
console.error('URDFLoader: Error loading mesh.', err)
},
)
},
}
/* 构造函数
manager：loader管理器，默认DefaultLoadingManager(LoadingManager 的单例模式)
*/
constructor(manager: LoadingManager = DefaultLoadingManager) {
this.manager = manager
}
/* 动态加载URDF */
loadAsync(urdfPath: string) {
return new Promise((resolve, reject) => {
this.load(urdfPath, resolve, undefined, reject)
})
}
/* 加载URDF
url URDF文件链接
onLoad URDF文件加载完成
onProgress URDF子文件加载完成
onError 加载错误
*/
load(
url: string,
onLoad: (robot: Object3D) => void,
onProgress?: (progress?: any) => void,
onError?: (err?: any) => void,
) {
const { manager } = this
// 解析URDF文件链接，manager.resolveURL() 可以修改url，若未设置resolveURL方法，返回url
const urdfPath = this.manager.resolveURL(url)
// 开始加载文件，需要在加载完成后，与manager.itemEnd(urdfPath)方法搭配使用
manager.itemStart(urdfPath)
// 请求URDF文件
fetch(urdfPath, this.fetchOptions)
.then((res) => {
if (res.ok) {
onProgress && onProgress(res)
return res.text()
} else {
throw new Error(
`URDFLoader: Failed to load url '${urdfPath}' with error code ${res.status} : ${res.statusText}.`,
)
}
})
.then((data) => {
// 解析URDF文件
const model = this.processUrdf(data, urdfPath)
model && onLoad(model)
// 结束加载
manager.itemEnd(urdfPath)
})
.catch((e) => {
if (onError) {
onError(e)
} else {
console.error('URDFLoader: Error loading file.', e)
}
manager.itemError(urdfPath)
manager.itemEnd(urdfPath)
})
}
/* 解析URDF文件
data：URDF的text数据，可兼容多种数据格式
urdfPath：urdf文件的路径，相对路径的子文件可以基于urdfPath做解析
subPathReplace[str1,str2]：str2可替换子路径中等于str1的部分
*/
processUrdf(data: string | Document | Element, urdfPath: string) {
// urdf标签集合
let children: Element[]
// 将URDF文件解析为DOM元素
if (typeof data == 'string') {
const parser = new DOMParser()
const urdf = parser.parseFromString(data, 'application/xml')
children = Array.from(urdf.children)
} else if (data instanceof Document) {
children = Array.from(data.children)
} else {
children = [data]
}
// console.log('urdf标签',children);
// 获取<robot>
const robotNode = children.filter((c) => c.nodeName === 'robot').pop()
if (!robotNode) {
return
}
const _this = this
// URDF机器人对象，图形树的根节点，对应<robot>
const urdfRobot = new URDFRobot()
urdfRobot.name = robotNode.getAttribute('name') || ''
// 将<robot>中的一级子元素<link>、<joint>、<material> 归类
const nodes = classifyNodeByName<'link' | 'joint' | 'material'>(robotNode, [
'link',
'joint',
'material',
])
// console.log('nodes',nodes);
/* 为各类图形对象创建集合，便于管理 */
// materia集合，可被<visual>图形复用
const materialMap: Map<string, Material> = new Map()
// link 图形集合
const linkMap: Map<string, URDFLink> = new Map()
// joint 图形集合
const jointMap: Map<string, URDFJoint> = new Map()
// 辅助对象-关节坐标系集合
const jointAxisMap: Map<string, Object3D> = new Map()
// 辅助对象-质心集合
const massMap: Map<string, Object3D> = new Map()
// 辅助对象-惯性矩集合
const inertiaMap: Map<string, Object3D> = new Map()
// 辅助对象-碰撞体集合
const collisionMap: Map<string, Group> = new Map()
//遍历urdf中的material元素，将其解析成three.js材质，写入materialMap中
nodes.get('material')?.forEach((materialNode: Element) => {
const materialName = materialNode.getAttribute('name')
materialName &&
materialMap.set(materialName, processMaterial(materialNode))
})
//遍历urdf中的link元素，将其解析成three.js 图形对象，写入linkMap中
nodes.get('link')?.forEach((linkNode: Element) => {
// link 名称
const linkName = linkNode.getAttribute('name') || ''
// link 图形
const urdfLink = new URDFLink()
urdfLink.name = linkName
// 若当前link 不是任何joint的child，那它就是最根部的link。
if (!robotNode.querySelector(`child[link="${linkName}"]`)) {
urdfRobot.add(urdfLink)
}
// 解析<link>
processLink(linkNode, urdfLink)
linkMap.set(linkName, urdfLink)
})
//遍历urdf中的joint元素，将其解析成three.js 图形对象，写入jointMap中
nodes.get('joint')?.forEach((jointNode: Element) => {
processJoint(jointNode)
})
// 将各类对象的集合挂在到机器人对象的userData 上
Object.assign(urdfRobot.userData, {
linkMap,
jointMap,
jointAxisMap,
massMap,
inertiaMap,
collisionMap,
})
// 将mimic关节挂载到被模仿关节上，方便被模仿关节变换时，带动mimic关节的变换
jointMap.forEach((joint) => {
if (joint instanceof URDFMimicJoint) {
// 被模仿的关节
const joint2 = jointMap.get(joint.userData.joint || '')
// 一个关节可能被多个关节模仿
joint2 && joint2.userData.mimicJoints.push(joint)
}
})
// 检查mimic关节的有效性，避免a模仿b，b模仿a的情况
jointMap.forEach((joint1) => {
//a>b>c>a
//uniqueJoints [a,b,c]
// 被模仿关节集合
const uniqueJoints = new Set()
// 判断mimic关节中是否包含被模仿关节
const iterFunction = (joint2: URDFJoint) => {
if (uniqueJoints.has(joint2)) {
throw new Error(
'URDFLoader: Detected an infinite loop of mimic joints.',
)
}
uniqueJoints.add(joint2)
joint2.userData.mimicJoints.forEach((joint3: URDFJoint) => {
iterFunction(joint3)
})
}
iterFunction(joint1)
})
/*
解析link
<link name="base">
<inertial>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
<mass value="0.01"/>
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
*/
function processLink(linkNode: Element, urdfLink: Group) {
// <link>中的所有子标签
const linkChildren = Array.from(linkNode.children)
// 遍历<link>中的所有子标签
linkChildren.forEach((linkChild) => {
// 子标签类型
const type = linkChild.nodeName.toLowerCase()
if (type === 'inertial') {
// 解析<inertial>，将inertial中的图形添加到urdfLink中
processInertial(linkChild, urdfLink)
} else if (type === 'visual') {
// 解析<visual>，将visual图形添加到urdfLink中
processVisual(linkChild, urdfLink)
} else if (type === 'collision') {
// 解析<collision>，将collision图形添加到urdfLink中
processCollision(linkChild, urdfLink)
}
})
}
/*
将URDF中的材质解析为three.js里的材质，若需其它材质，可在外部重写此方法
<material name="steel">
<color rgba="0.7 0.65 0.55 1"/>
<texture filename="../texture/head.jpg"/>
</material>
*/
function processMaterial(materialNode: Element) {
const { manager } = _this
// 默认材质
const material = _this.createCommonMaterial()
material.name = materialNode.getAttribute('name') || ''
// 遍历<material> 子元素
for (let child of Array.from(materialNode.children)) {
const nodeName = child.nodeName.toLowerCase()
switch (nodeName) {
case 'color':
// 解析rgba颜色
const rgbaAttr = child.getAttribute('rgba')
if (rgbaAttr) {
const rgba = processTuple(rgbaAttr) as NumberTuple4
// 设置rgb颜色
material.color.setRGB(rgba[0], rgba[1], rgba[2])
// 设置透明度
material.opacity = rgba[3]
material.transparent = rgba[3] < 1
}
break
case 'texture':
// 纹理路径
const filename = child.getAttribute('filename')
if (filename) {
// 纹理加载器
const loader = new TextureLoader(manager)
// 解析纹理路径
const filePath = _this.resolveSubPath(filename)
material.map = loader.load(filePath)
material.map.colorSpace = SRGBColorSpace
}
break
}
}
return material
}
/*
解析joint
<joint name="R_pinky_finger_distal_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0.0325"/>
<parent link="R_pinky_finger_proximal"/>
<child link="R_pinky_finger_distal"/>
<axis xyz="0 1 0"/>
<limit effort="1000" lower="0.33811" upper="3.58322" velocity="1"/>
<mimic joint="R_pinky_finger_proximal_joint" multiplier="1.005" offset="0.6665"/>
</joint>
*/
function processJoint(jointNode: Element) {
const jointName = jointNode.getAttribute('name') || ''
const jointType = jointNode.getAttribute('type')
const jointChildren = Array.from(jointNode.children)
let jointObj: URDFJoint | URDFMimicJoint
// mimic节点
const mimicTag = jointChildren.find(
(n) => n.nodeName.toLowerCase() === 'mimic',
)
if (mimicTag) {
// 若joint 中存在mimic节点，则此joint会模仿<mimic>中指定的joint
jointObj = new URDFMimicJoint()
// 被模仿关节的名称
const joint = mimicTag.getAttribute('joint') || ''
// 模仿系数
const multiplier = mimicTag.getAttribute('multiplier')
// 偏移量
const offset = mimicTag.getAttribute('offset')
Object.assign(jointObj.userData, {
joint,
multiplier: multiplier ? parseFloat(multiplier) : 1,
offset: offset ? parseFloat(offset) : 0,
})
} else {
// 正常关节
jointObj = new URDFJoint()
}
const { userData } = jointObj
// 遍历关节子标签
jointChildren.forEach((jointChild: Element) => {
// 关节类型
const type = jointChild.nodeName.toLowerCase()
// 将不同类型的关节数据写入关节图形
switch (type) {
case 'origin':
// 关节位置
const xyz = processTuple(jointChild.getAttribute('xyz')) as NumberTuple3
jointObj.position.set(xyz[0], xyz[1], xyz[2])
userData.origPosition.set(xyz[0], xyz[1], xyz[2])
// 欧拉旋转
const rpy = processTuple(jointChild.getAttribute('rpy')) as NumberTuple3
applyEulerZYX(jointObj, rpy)
userData.origQuaternion.copy(jointObj.quaternion)
break
case 'parent':
// 一个子joint连接一个父link
const parent = linkMap.get(jointChild.getAttribute('link') || '')
parent?.add(jointObj)
break
case 'child':
// 一个父joint连接一个子link
const child = linkMap.get(jointChild.getAttribute('link') || '')
child && jointObj.add(child)
break
case 'axis':
// 关节变换轴
const axis = processTuple(jointChild.getAttribute('xyz')) as NumberTuple3
userData.axis.set(axis[0], axis[1], axis[2])
break
case 'limit':
// 旋转范围
const lower = jointChild.getAttribute('lower')
const upper = jointChild.getAttribute('upper')
const { limit } = userData
lower && (limit.lower = parseFloat(lower))
upper && (limit.upper = parseFloat(upper))
break
}
})
jointObj.name = jointName
userData.type = jointType as JointType
jointMap.set(jointName, jointObj)
}
/* 解析inertial
<inertial>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
<mass value="0.01"/>
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
</inertial>
*/
function processInertial(inertialNode: Element, urdfLink: Group) {}
/* 解析<collision>
<collision>
<origin xyz="0.02 0 0" rpy="0 1.5707963267948966192313216916398 0"/>
<geometry>
<cylinder radius="0.01" length="0.02"/>
</geometry>
</collision>
*/
function processCollision(collisionNode: Element, urdfLink: Group) {}
/* 解析visual
<visual name="head">
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<material name="steel">
<color rgba="0.7 0.65 0.55 1"/>
</material>
</visual>
*/
function processVisual(visualNode: Element, urdfLink: Group) {
let material = getMaterialFromNode(visualNode)
const linkVisual = new LinkVisual()
linkVisual.name = visualNode.getAttribute('name') || ''
processOriginAndGeometry(visualNode, material, linkVisual)
urdfLink.add(linkVisual)
}
/* 解析包含<origin>和<geometry>的元素，比如<visual>和<collision>*/
function processOriginAndGeometry(
node: Element,
material: Material,
parent: Group,
) {
const visualChildren = Array.from(node.children)
visualChildren.forEach((childNode) => {
const type = childNode.nodeName.toLowerCase()
if (type === 'geometry') {
processGeometry(childNode, material, parent)
} else if (type === 'origin') {
const { xyz, rpy } = processOrigin(childNode)
parent.position.set(xyz[0], xyz[1], xyz[2])
applyEulerZYX(parent, rpy)
}
})
}
/* 解析geometry
<geometry>
<mesh filename="" scale="1e-3 1e-3 1e-3"/>
</geometry>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<geometry>
<sphere radius="0.0065" />
</geometry>
<geometry>
<cylinder length="0.13" radius="0.053"/>
</geometry>
*/
function processGeometry(
geometryNode: Element,
material: Material,
parent: Group,
) {
const geometryChildNode = geometryNode.children[0]
if(!geometryChildNode) {return}
const geoType = geometryChildNode.nodeName.toLowerCase()
switch (geoType) {
case 'mesh':
processMesh(geometryChildNode, material, parent)
break
case 'box':
processBox(geometryChildNode, material, parent)
break
case 'sphere':
processSphere(geometryChildNode, material, parent)
break
case 'cylinder':
processCylinder(geometryChildNode, material, parent)
break
}
}
// 解析 <mesh filename="" scale="1e-3 1e-3 1e-3"/>
function processMesh(meshNode: Element, material: Material, parent: Group) {
const { meshParsers } = _this
// 模型路径
const filename = meshNode.getAttribute('filename')
if (!filename) {
return
}
let filePath = _this.resolveSubPath(filename)
if (!filePath) {
return
}
// 模型文件的格式
const suffix = filePath.split('.').pop()?.toLowerCase()
if (suffix && meshParsers[suffix]) {
// 模型解析方法
meshParsers[suffix](filePath, material).then((obj) => {
if (!obj) {
return
}
// 模型缩放
const scaleAttr = meshNode.getAttribute('scale')
if (scaleAttr) {
const [x, y, z] = processTuple(scaleAttr)
obj.scale.multiply(new Vector3(x, y, z))
}
// 将模型添加到visual图形
parent.add(obj)
})
} else {
console.warn(`无法解析以 ${suffix} 为后缀的模型.`)
}
}
// 解析<box size="0.224 0.08 0.071"/>
function processBox(boxNode: Element, material: Material, parent: Group) {
const [x, y, z] = processTuple(boxNode.getAttribute('size'))
const boxMesh = new Mesh(new BoxGeometry(x, y, z), material)
parent.add(boxMesh)
}
// 解析<sphere radius="0.0065" />
function processSphere(
sphereNode: Element,
material: Material,
parent: Group,
) {
const radius = parseFloat(sphereNode.getAttribute('radius') || '0')
const sphereMesh = new Mesh(new SphereGeometry(radius, 8, 6), material)
parent.add(sphereMesh)
}
// 解析<cylinder length="0.13" radius="0.053"/>
function processCylinder(
cylinderNode: Element,
material: Material,
parent: Group,
) {
const radius = parseFloat(cylinderNode.getAttribute('radius') || '0')
const length = parseFloat(cylinderNode.getAttribute('length') || '0')
const cylinderMesh = new Mesh(
new CylinderGeometry(radius, radius, length, 6),
material,
)
cylinderMesh.rotation.set(Math.PI / 2, 0, 0)
parent.add(cylinderMesh)
}
// 解析<origin rpy="0 0 0" xyz="0 0 -0.1077"/>
function processOrigin(originNode: Element) {
return {
xyz: processTuple(originNode.getAttribute('xyz')) as NumberTuple3,
rpy: processTuple(originNode.getAttribute('rpy')) as NumberTuple3,
}
}
// 根据node数据创建材质
function getMaterialFromNode(node: Element) {
const children = Array.from(node.children)
let material: Material
// 若node中存在<material>子标签，则使用<material>中的材质
const materialNodes = children.filter(
(n) => n.nodeName.toLowerCase() === 'material',
)
const materialNode = materialNodes[0]
if (materialNode) {
// 若materialMap中存在与materialNode相同name的材质，则使用materialMap中存在的材质，否则新建材质
const tempMat = materialMap.get(materialNode.getAttribute('name') || '')
if (tempMat) {
material = tempMat
} else {
material = processMaterial(materialNode)
}
}else{
material = _this.createCommonMaterial()
}
return material
}
return urdfRobot
}
/* 创建通用材质，可在外部按需重写 */
createCommonMaterial() {
return new MeshStandardMaterial({
color: 0xcccccc,
metalness: 1,
roughness: 0.1,
})
}
/*子路径解析方法，可在外部重写*/
resolveSubPath(filename: string) {
return filename
}
}
export { URDFLoader }
代码详解
URDFLoader 的代码有点多，但都比较简单，我给大家捋一下其逻辑。
loader 管理器
URDF 文件可能会关联多个子文件，比如模型文件，所以我们需要一个loader 管理器来监听所有文件的加载。
three.js 的LoadingManager 对象可以实现此功能。
urdf 文件的加载管理，需要手动调用LoadingManager 对象的itemStart() 和itemEnd() 方法。
load(
url: string,
onLoad: (robot: Object3D) => void,
onProgress?: (progress?: any) => void,
onError?: (err?: any) => void
) {
const { manager } = this;
// 解析URDF文件链接，manager.resolveURL() 可以修改url，若未设置resolveURL方法，返回url
const urdfPath = this.manager.resolveURL(url);
// 开始加载文件，需要在加载完成后，与manager.itemEnd(urdfPath)方法搭配使用
manager.itemStart(urdfPath);
// 请求URDF文件
fetch(urdfPath, this.fetchOptions)
.then((res) => {
//……
})
.then((data) => {
// 解析URDF文件
const model = this.processUrdf(data, urdfPath);
model&&onLoad(model);
// 结束加载
manager.itemEnd(urdfPath);
})
.catch((e) => {
//……
manager.itemError(urdfPath);
manager.itemEnd(urdfPath);
});
}
除此之外，我们还需要使用LoadingManager 监听其它外部资源的加载。
//贴图文件的加载管理。
const loader = new TextureLoader(manager);
//stl 模型文件的加载管理。
const loader = new STLLoader(manager);
//dae 模型文件的加载管理。
const loader = new ColladaLoader(manager);
上面的TextureLoader、STLLoader 和ColladaLoader 都会在其内部执行LoadingManager 对象的itemStart() 和itemEnd() 方法。
获取URDF 文件内容
在load() 方法中，通过fetch 请求URDF文件，然后使用res.text() 方法获取URDF文件的内容，最后使用processUrdf() 方法解析此内容。
load(
url: string,
onLoad: (robot: Object3D) => void,
onProgress?: (progress?: any) => void,
onError?: (err?: any) => void
) {
...
// 请求URDF文件
fetch(urdfPath, this.fetchOptions)
.then((res) => {
if (res.ok) {
onProgress && onProgress(res);
return res.text();
}
...
})
.then((data) => {
// 解析URDF文件
const model = this.processUrdf(data, urdfPath);
model&&onLoad(model);
// 结束加载
manager.itemEnd(urdfPath);
})
...
}
processUrdf() 方法
processUrdf() 方法完成了所有的URDF内容的解析工作，其中内嵌了许多URDF子标签的解析方法，我们先整体的说一下其解析逻辑。
1.将URDF内容转DOM，方便获取其内的标签、标签内容和标签属性。
const parser = new DOMParser();
const urdf = parser.parseFromString(data, "application/xml");
2.建立机器人对象。
const urdfRobot = new URDFRobot();
urdfRobot.name = robotNode.getAttribute("name")||'';
3.将中的一级子元素、、 归类，方便后续解析。
const nodes = classifyNodeByName<"link" | "joint" | "material">(
robotNode,
["link", "joint", "material"]
);
classifyNodeByName()方法可以根据标签名称，对标签内的元素进行分类。
function classifyNodeByName<key>(node: Element, names: key[]) {
const nodeMap: Map<key, Element[]> = new Map()
for (let name of names) {
nodeMap.set(name, [])
}
for (let child of Array.from(node.children)) {
const key = child.nodeName.toLowerCase() as key
if (names.includes(key)) {
nodeMap.get(key)?.push(child)
}
}
return nodeMap
}
4.为各类图形对象创建集合，便于管理。
// materia集合，可被<visual>图形复用
const materialMap: Map<string, Material> = new Map();
// link 图形集合
const linkMap: Map<string, URDFLink> = new Map();
// joint 图形集合
const jointMap: Map<string, URDFJoint> = new Map();
// 辅助对象-关节坐标系集合
const jointAxisMap: Map<string, Object3D> = new Map();
// 辅助对象-质心集合
const massMap: Map<string, Object3D> = new Map();
// 辅助对象-惯性集合
const inertiaMap: Map<string, Object3D> = new Map();
// 辅助对象-碰撞体集合
const collisionMap: Map<string, Group> = new Map();
5.按顺序解析URDF标签：material → link → joint
material 会被link 中的图形用到，link 会被joint 连接在一起。
//遍历urdf中的material元素，将其解析成three.js材质，写入materialMap中
nodes.get("material")?.forEach((materialNode: Element) => {
const materialName = materialNode.getAttribute("name");
materialName&&materialMap.set(materialName,processMaterial(materialNode))
});
//遍历urdf中的link元素，将其解析成three.js 图形对象，写入linkMap中
nodes.get('link')?.forEach((linkNode: Element) => {
// link 名称
const linkName = linkNode.getAttribute("name")||'';
// link 图形
const urdfLink = new URDFLink();
urdfLink.name = linkName;
// 若当前link 不是任何元素的child，那它只能是<robot> 的子节点。
if(!robotNode.querySelector(`child[link="${linkName}"]`)){
urdfRobot.add(urdfLink)
}
// 解析<link>
processLink(linkNode, urdfLink);
linkMap.set(linkName, urdfLink);
});
//遍历urdf中的joint元素，将其解析成three.js 图形对象，写入jointMap中
nodes.get('joint')?.forEach((jointNode: Element) => {
processJoint(jointNode);
});
material、link 和 joint 标签的具体解析方法，我们稍后详解。
6.将mimic joint 挂载到它所模仿的关节上，并检测模仿死循环。
// 将mimic关节挂载到被模仿关节上，方便被模仿关节变换时，带动mimic关节的变换
jointMap.forEach((joint) => {
if (joint instanceof URDFMimicJoint) {
// 被模仿的关节
const BeImitatedJoint = jointMap.get(joint.userData.joint||'');
// 一个关节可能被多个关节模仿
BeImitatedJoint&&BeImitatedJoint.userData.mimicJoints.push(joint);
}
});
// 检查mimic关节的有效性，避免a模仿b，b模仿a的情况
jointMap.forEach((joint1) => {
// 被模仿关节集合
const uniqueJoints = new Set();
// 判断mimic关节中是否包含被模仿关节
const iterFunction = (joint2: URDFJoint) => {
if (uniqueJoints.has(joint2)) {
throw new Error(
"URDFLoader: Detected an infinite loop of mimic joints."
);
}
uniqueJoints.add(joint2);
joint2.userData.mimicJoints.forEach((joint3: URDFJoint) => {
iterFunction(joint3);
});
};
iterFunction(joint1);
});
接下来我们具体说一下material、link 和 joint 标签的解析方法。
解析material
URDF中的 实例
<material name="steel">
<color rgba="0.7 0.65 0.55 1"/>
<texture filename="../texture/head.jpg"/>
</material>
processMaterial() 方法可以解析 中的颜色和纹理。
function processMaterial(materialNode: Element) {
const { manager } = _this;
// 默认材质
const material = _this.createCommonMaterial();
material.name = materialNode.getAttribute("name") || "";
// 遍历<material> 子元素
for (let child of Array.from(materialNode.children)) {
const nodeName = child.nodeName.toLowerCase();
switch (nodeName) {
case "color":
// 解析rgba颜色
const rgbaAttr = child.getAttribute("rgba");
if (rgbaAttr) {
const rgba = processTuple(rgbaAttr);
// 设置rgb颜色
material.color.setRGB(rgba[0], rgba[1], rgba[2]);
// 设置透明度
material.opacity = rgba[3];
material.transparent = rgba[3] < 1;
}
break;
case "texture":
// 纹理路径
const filename = child.getAttribute("filename");
if (filename) {
// 纹理加载器
const loader = new TextureLoader(manager);
// 解析纹理路径
const filePath = _this.resolveSubPath(filename, urdfPath);
material.map = loader.load(filePath);
material.map.colorSpace = SRGBColorSpace;
}
break;
}
}
return material;
}
processTuple()方法可以将"x y z" 格式的数据转成[x,y,z]格式。
function processTuple(val: string | null) {
if (!val) return [0, 0, 0]
// trim() 去掉字符串两端的空白
// split() 按照特点规律将字符串分割为数组
// \s：匹配任何空白字符，包括空格、制表符、换页符等
// +：表示前面的字符（在这个情况下是 \s）可以出现1次或多次
// g：全局匹配标志,对整个字符串进行匹配,而不是在找到第1个匹配后就停止
return val
.trim()
.split(/\s+/g)
.map((num) => parseFloat(num))
}
解析link
URDF中的示例
<link name="base">
<inertial>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
<mass value="0.01"/>
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
processLink() 方法可以解析，将其内容可视化。
- 是的可视图形。
- 是 的惯性。
- 是 的碰撞体。
function processLink(linkNode: Element, urdfLink: Group) {
// <link>中的所有子标签
const linkChildren = Array.from(linkNode.children);
// 遍历<link>中的所有子标签
linkChildren.forEach((linkChild) => {
// 子标签类型
const type = linkChild.nodeName.toLowerCase();
if (type === "inertial" ) {
// 解析<inertial>，将inertial中的图形添加到urdfLink中
processInertial(linkChild, urdfLink);
} else if (type === "visual") {
// 解析<visual>，将visual图形添加到urdfLink中
processVisual(linkChild, urdfLink);
} else if (type === "collision") {
// 解析<collision>，将collision图形添加到urdfLink中
processCollision(linkChild, urdfLink);
}
});
}
本章我们只关注 的可视化，其它的我们会在后面的章节里细说。
processVisual() 是解析 的方法
function processVisual(
visualNode: Element,
urdfLink: Group,
) {
let material = getMaterialFromNode(visualNode);
const linkVisual = new LinkVisual();
linkVisual.name = visualNode.getAttribute("name") || "";
processOriginAndGeometry(visualNode, material, linkVisual);
urdfLink.add(linkVisual);
}
getMaterialFromNode() 方法可以根据当前的 获取相应材质。
function getMaterialFromNode( node: Element) {
const children = Array.from(node.children);
let material: Material = _this.createCommonMaterial();
// 若node中存在<material>子标签，则使用<material>中的材质
const materialNodes = children.filter(
(n) => n.nodeName.toLowerCase() === "material"
);
if (materialNodes.length) {
const materialNode = materialNodes[0];
// 若materialMap中存在与materialNode相同name的材质，则使用materialMap中存在的材质，否则新建材质
const tempMat=materialMap.get(materialNode.getAttribute("name") || "")
if (tempMat) {
material =tempMat;
} else {
material = processMaterial(materialNode);
}
}
return material;
}
processOriginAndGeometry() 方法可以解析 中的和
function processOriginAndGeometry(
node: Element,
material: Material,
parent: Group,
) {
const visualChildren = Array.from(node.children);
visualChildren.forEach((childNode) => {
const type = childNode.nodeName.toLowerCase();
if (type === "geometry") {
processGeometry(childNode, material, parent);
} else if (type === "origin") {
const { xyz, rpy } = processOrigin(childNode);
parent.position.set(xyz[0], xyz[1], xyz[2]);
applyEulerZYX(parent, rpy);
}
});
}
在上面的代码中可以看到，processGeometry() 方法可以解析，processOrigin方法可以解析。
中的模型有两种：
- URDF 内置几何体：、、。
- 自定义几何体
的解析方法如下：
function processGeometry(
geometryNode: Element,
material: Material,
parent: Group,
) {
const geometryChildNode = geometryNode.children[0];
const geoType = geometryNode.children[0].nodeName.toLowerCase();
switch (geoType) {
case "mesh":
processMesh(geometryChildNode, material, parent);
break;
case "box":
processBox(geometryChildNode, material, parent);
break;
case "sphere":
processSphere(geometryChildNode, material, parent);
break;
case "cylinder":
processCylinder(geometryChildNode, material, parent);
break;
}
}
// 解析 <mesh filename="" scale="1e-3 1e-3 1e-3"/>
function processMesh(
meshNode: Element,
material: Material,
parent: Group
) {
const { meshParsers } = _this;
// 模型路径
const filename = meshNode.getAttribute("filename");
if(!filename){return}
let filePath = _this.resolveSubPath(filename,urdfPath);
if (!filePath) {
return;
}
// 模型文件的格式
const suffix = filePath.split(".").pop()?.toLowerCase();
const meshParser=meshParsers[suffix||'']
if (meshParser) {
// 模型解析方法
meshParser(filePath, material).then((obj) => {
if (!obj) {
return;
}
// 模型缩放
const scaleAttr = meshNode.getAttribute("scale");
if (scaleAttr) {
const [x, y, z] = processTuple(scaleAttr);
obj.scale.multiply(new Vector3(x, y, z));
}
// 将模型添加到visual图形
parent.add(obj);
});
} else {
console.warn(`无法解析以 ${suffix} 为后缀的模型.`);
}
}
// 解析<box size="0.224 0.08 0.071"/>
function processBox(boxNode: Element, material: Material, parent: Group) {
const [x, y, z] = processTuple(boxNode.getAttribute("size"));
const boxMesh = new Mesh(new BoxGeometry(x, y, z), material);
parent.add(boxMesh);
}
// 解析<sphere radius="0.0065" />
function processSphere(
sphereNode: Element,
material: Material,
parent: Group
) {
const radius = parseFloat(sphereNode.getAttribute("radius") || "0");
const sphereMesh = new Mesh(new SphereGeometry(radius, 8, 6), material);
parent.add(sphereMesh);
}
// 解析<cylinder length="0.13" radius="0.053"/>
function processCylinder(
cylinderNode: Element,
material: Material,
parent: Group
) {
const radius = parseFloat(cylinderNode.getAttribute("radius") || "0");
const length = parseFloat(cylinderNode.getAttribute("length") || "0");
const cylinderMesh = new Mesh(
new CylinderGeometry(radius, radius, length, 6),
material
);
cylinderMesh.rotation.set(Math.PI / 2, 0, 0);
parent.add(cylinderMesh);
}
的解析方法如下：
// 解析<origin rpy="0 0 0" xyz="0 0 -0.1077"/>
function processOrigin(originNode: Element) {
return {
xyz: processTuple(originNode.getAttribute("xyz")),
rpy: processTuple(originNode.getAttribute("rpy")),
};
}
// "x y z" 转[x,y,z]
function processTuple(val: string|null) {
if (!val) return [0, 0, 0];
return val
.trim()
.split(/\s+/g)
.map((num) => parseFloat(num));
}
applyEulerZYX() 方法可以根据urdf的欧拉值旋转THREE.js 图形
/* 根据urdf的欧拉值旋转THREE.js 图形
THREE.js
Y
|
|
.-----X
／
Z
rpy=zyx
ROS URDf
Z
| X
| ／
Y-----.
rpy=xyz
*/
function applyEulerZYX(obj: Object3D, rpy: number[]) {
tempEuler.set(rpy[0], rpy[1], rpy[2], 'ZYX')
tempQuaternion.setFromEuler(tempEuler)
tempQuaternion.multiply(obj.quaternion)
obj.quaternion.copy(tempQuaternion)
}
解析joint
URDF中的示例
<joint name="R_pinky_finger_distal_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0.0325"/>
<parent link="R_pinky_finger_proximal"/>
<child link="R_pinky_finger_distal"/>
<axis xyz="0 1 0"/>
<limit effort="1000" lower="0.33811" upper="3.58322" velocity="1"/>
<mimic joint="R_pinky_finger_proximal_joint" multiplier="1.005" offset="0.6665"/>
</joint>
processJoint() 方法可以解析，然后连接其parent link和child link。
function processJoint(
jointNode: Element
) {
const jointName = jointNode.getAttribute("name")||'';
const jointType = jointNode.getAttribute("type");
const jointChildren = Array.from(jointNode.children);
let jointObj: URDFJoint | URDFMimicJoint;
// mimic节点
const mimicTag = jointChildren.find(
(n) => n.nodeName.toLowerCase() === "mimic"
);
if (mimicTag) {
// 若joint 中存在mimic节点，则此joint 是mimic关节，它会模仿<mimic>中指定的joint
jointObj = new URDFMimicJoint();
// 被模仿关节的名称
const joint = mimicTag.getAttribute("joint")||'';
// 模仿倍数
const multiplier = mimicTag.getAttribute("multiplier");
// 偏移量
const offset = mimicTag.getAttribute("offset");
Object.assign(jointObj.userData, {
joint,
multiplier: multiplier ? parseFloat(multiplier) : 1,
offset: offset ? parseFloat(offset) : 0,
});
} else {
// 正常关节
jointObj = new URDFJoint();
}
const { userData } = jointObj;
// 遍历关节子标签
jointChildren.forEach((jointChild: Element) => {
// 关节类型
const type = jointChild.nodeName.toLowerCase();
// 将不同类型的关节数据写入关节图形
switch (type) {
case "origin":
// 关节位置
const xyz = processTuple(jointChild.getAttribute("xyz"));
jointObj.position.set(xyz[0], xyz[1], xyz[2]);
userData.origPosition.set(xyz[0], xyz[1], xyz[2]);
// 欧拉旋转
const rpy = processTuple(jointChild.getAttribute("rpy"));
applyEulerZYX(jointObj, rpy);
userData.origQuaternion.copy(jointObj.quaternion);
break;
case "parent":
// 一个子joint连接一个父link
const parent = linkMap.get(jointChild.getAttribute("link")||'');
parent?.add(jointObj);
break;
case "child":
// 一个父joint连接一个子link
const child = linkMap.get(jointChild.getAttribute("link")||'');
child&&jointObj.add(child);
break;
case "axis":
// 四元数旋转轴
const axis = processTuple(jointChild.getAttribute("xyz"));
userData.axis.set(axis[0], axis[1], axis[2]);
break;
case "limit":
// 旋转范围
const lower = jointChild.getAttribute("lower");
const upper = jointChild.getAttribute("upper");
const { limit } = userData;
lower && (limit.lower = parseFloat(lower));
upper && (limit.upper = parseFloat(upper));
break;
}
});
jointObj.name = jointName;
userData.type = jointType as JointType;
jointMap.set(jointName, jointObj);
}
4-测试URDFLoader 类
1.在之前RobotVisual.ts 文件中添加loadURDF 方法，在此方法中实例化URDFLoader 类。
- src/robot/RobotVisual.ts
/* 机器可视化类 */
class RobotVisual extends EventDispatcher<any> {
//...
/* 加载URDF模型
url URDF文件链接
resolvePath 子路径解析方法
*/
loadURDF(
url: string,
onLoad?: (robot: URDFRobot) => void) {
const manager = new LoadingManager();
const loader = new URDFLoader(manager);
let robot: URDFRobot;
loader.load(url, (res: any) => {
robot = res;
});
manager.onLoad = () => {
// 初始化模型
this.initModel(robot);
onLoad&&onLoad(robot);
};
manager.onError = () => {
console.error("URDFLoader: Error loading model.");
};
return loader;
}
// 初始化模型
initModel(robot: URDFRobot ) {
const {
scene,
resourceTracker,
} = this;
// 使robot面朝z轴
robot.rotation.set(-halfPI, 0, -halfPI);
scene.add(robot);
// 添加缓存清理机制
resourceTracker.track(robot);
// 模型落地
this.toGround(robot);
}
// 使机器人的底部落地
toGround(robot: URDFRobot) {
const bb = new Box3();
bb.setFromObject(robot);
robot.position.y -= bb.min.y;
}
//...
}
export { RobotVisual };
上面的代码做了以下事情：
1.实例化URDFLoader，为其指定LoadingManager。
2.使用URDFLoader 加载URDF 文件。
3.使用LoadingManager 监听所有URDF资源的加载成功。
4.旋转URDF模型，使模型从URDF坐标系适配到three.js 的坐标系。
5.使机器人的底部落地。
2.在App.vue 文件中调用RobotVisual 对象的loadURDF 方法。
const hdrURL = "/texture/venice_sunset_1k.hdr";
const urdfURL = "./models/PR2/urdf/PR2.urdf";
let robotVisual = new RobotVisual(hdrURL);
const urdfLoader= robotVisual.loadURDF(urdfURL);
urdfLoader.resolveSubPath=(subPath)=>{
const path=subPath.replace('package://urdf_tutorial','/models/PR2');
return path;
}
robotVisual.continuousRender();
运行项目，效果如下：
3.测试宇树的h1机器人。
h1_2.urdf 中的资源路径用的是相对路径。如下所示：
<geometry>
<mesh filename="meshes/left_ankle_pitch_link.STL"/>
</geometry>
这就需要我们重新resolveSubPath 方法：
const hdrURL = "/texture/venice_sunset_1k.hdr";
const urdfURL = './models/h1_2_description/h1_2.urdf'
//const urdfURL = "./models/PR2/urdf/PR2.urdf";
let robotVisual = new RobotVisual(hdrURL);
const urdfLoader= robotVisual.loadURDF(urdfURL);
// 重写h1 资源路径解析方法
urdfLoader.resolveSubPath=(filename: string)=>{
return './models/h1_2_description/'+filename
}
robotVisual.continuousRender();
效果如下：
这个模型虽丑，但它具备了多种模型类型和关节类型，很适合用来测试。
总结
这一章我们说了URDF文件的解析原理和方法，下一章我们会说一下URDF模型交互控制。
