课程链接：www.bilibili.com/cheese/play...
代码链接：github.com/buglas/robo...
课程目标
- 创建惯性矩对象
- 控制惯性矩的可见性
1-惯性矩的概念
惯性矩的核心是用"距离平方加权"描述质量相对参考轴的分散程度，其数值越大转动惯性越大。
比如，同等质量的物体，因为距离平方的加权，基于不同参考轴的惯性矩是不一样的。
在上图中，同样质量的长方体，右侧的惯性矩要大于左侧，因为右侧物体到参考轴的最大距离b大于左侧物体到参考轴的最大距离a。
2-惯性矩的可视化原理和作用
在机器人场景中，URDF 文件会为我们提供机器人零件以x、y、z 轴为参考轴的惯性矩。
如下所示， 中的ixx、iyy、izz 分别对应了零件以x轴、y轴、z轴为参考轴的惯性矩。
ini
<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>
</link>在对惯性矩进行可视的时候，我们通常会把 中的惯性矩作为均质长方体的惯性矩，然后根据惯性矩逆推出长方体的尺寸。
有了长方体的尺寸后，就可以结合中的rpy 和xyz 把长方体显示出来，如下图所示：
上面的蓝色长方体就是惯性矩的可视化，其作用是：通过观察长方体的长宽高尺寸与其所在零件的长宽高尺寸是否基本一致，确定机器人零件的惯性矩数据是否可能有问题。
3-判断惯性矩的合理性
1.质量和惯性矩必须非负，否则皆代表惯性矩不合理。
以下情况皆不合理：
2.任意刚体的三轴惯性矩，必须满足：任意两轴之和≥第三轴。
以下情况皆不合理：
4-根据惯性矩逆推尺寸的算法
在机器人场景中，经常会使用宽、高、深的尺寸为2a、2b、2c的均质长方体，如下图所示：
上图的a、b、c 就叫做半边长。
这种尺寸定义方式的优点是：质心到面的距离更直观，简化通过惯性矩反向推导尺寸的逻辑。
均质长方体的惯性矩和边长存在以下关系：
当惯性矩和质量M已知的时候，就可以根据上面的三元一次方程组解出均质方法体的边长。
结果如下：
5-代码实现
在URDFLoader 中，根据 中的 惯性矩数据创建长方体。
- src/robot/URDFLoader.ts
ini
/* 根据惯性矩，逆推长方体尺寸 */
function getSizeByInertial(mass:number, ixx:number, iyy:number, izz:number) {
const x = Math.sqrt(6 * (iyy + izz - ixx) / mass);
const y = Math.sqrt(6 * (izz + ixx - iyy) / mass);
const z = Math.sqrt(6 * (ixx + iyy - izz) / mass);
return new Vector3(x, y, z);
}
// 惯性矩的材质与几何体
const inertiaMat = new MeshStandardMaterial({
color: 0x00acec,
depthTest: false,
depthWrite: false,
fog: false,
toneMapped: false,
transparent: true,
opacity: 0.6,
});
const inertiaGeometry = new BoxGeometry();
class URDFLoader {
//...
function processInertial(
inertialNode: Element,
urdfLink: Group
) {
// <link> 的name
const linkName = urdfLink.name;
// <inertial>子标签
const inertialChildren = Array.from(inertialNode.children);
let origin_rpy = [0, 0, 0];
let origin_xyz = [0, 0, 0];
let mass_value = 0;
let ixx = 0, iyy = 0, izz = 0;
inertialChildren.forEach((node) => {
switch (node.nodeName.toLowerCase()) {
case "origin":
origin_rpy = processTuple(node.getAttribute("rpy"));
origin_xyz = processTuple(node.getAttribute("xyz"));
break;
case "mass":
mass_value = parseFloat(node.getAttribute("value") || "0");
break;
case "inertia":
ixx=parseFloat(node.getAttribute("ixx") || "0");
iyy=parseFloat(node.getAttribute("iyy") || "0");
izz=parseFloat(node.getAttribute("izz") || "0");
break;
}
});
/* 创建辅助对象-质心 */
//...
/* 创建辅助对象-惯性矩 */
if(mass_value < 0 || ixx < 0 || iyy < 0 || izz < 0 ||ixx + iyy < izz || iyy + izz < ixx || izz + ixx < iyy){
// Unrealistic inertia
console.warn('The link ' + linkName + ' has unrealistic inertia, unable to visualize box of equivalent inertia.');
}else{
// origin旋转量
const originEuler = new Euler(
origin_rpy[0],
origin_rpy[1],
origin_rpy[2],
"ZYX"
);
const inertiaHelper = new Mesh(inertiaGeometry, inertiaMat);
inertiaHelper.visible = false;
inertiaHelper.position.set(x,y,z);
inertiaHelper.scale.copy(getSizeByInertial(mass_value, ixx, iyy, izz))
inertiaHelper.quaternion.setFromEuler(originEuler)
Object.assign(inertiaHelper.userData, {
isURDFHelper: true,
helperType: "inertiaHelper",
});
urdfLink.add(inertiaHelper);
inertiaMap.set(linkName, inertiaHelper);
}
}
}代码详解
inertiaHelper 是惯性矩的可视化对象。
inertiaHelper 的position 和quaternion 是分别从中的rpy 和xyz 中获取的。
inertiaHelper 中的geometry 对象是一个尺寸为1的BoxGeometry。
为了减少数据缓存，所有inertiaHelper 的geometry 都是共用一个BoxGeometry。
所以，inertiaHelper 的尺寸差异是通过其scale 属性设置的，其缩放值，就是其尺寸。
getSizeByInertial() 是通过惯性矩数据和质量逆推尺寸的方法。
总结
对于惯性矩的可视化，若知道惯性矩合理性的验证规则和逆推长方体尺寸的算法，是很好实现的。
但其难点就在于，若没有人告诉你惯性矩是什么，怎么办？
我就遇到过，搞ROS 的人告诉我："你照着惯性矩在Rviz 里的可视化效果将它在web前端可视化吧"，然后他就给我截了张Rviz的图。
这种情况的解决方法有2种：
- 从算法上理解惯性矩。
- 照搬Rviz源码。
我一般会先采用第一种，若第一种太费时间，也就是算法很复杂时，再采用第二种。
下一章，我们会说机器人的实时动画缓冲技术。