python中解析URDF文件的功能包有很多,最常用的可能是集成在ROS/ROS2
中的urdf_parser_py,其次是KDL
中将URDF转成KDL模型的kdl_parser,此外,webots
官方的urdf2webots改改也能用~
但是,以上功能包都太繁琐了,想要轻量级还是自己写吧。
基于xml.dom.minidom
,从URDF文件中读取动力学参数的案例如下:
#!/usr/bin/python3
# _*_ coding: UTF-8 _*_
#
# Copyright (C) 2022, by Leon(罗伯特祥)
#
import xml.dom.minidom
import os
import numpy as np
def getInertial(joint_id,xml_obj):
""" get inertial data. """
mass = xml_obj.getElementsByTagName("mass")[joint_id].getAttribute("value")
ixx = xml_obj.getElementsByTagName("inertia")[joint_id].getAttribute("ixx")
ixy = xml_obj.getElementsByTagName("inertia")[joint_id].getAttribute("ixy")
ixz = xml_obj.getElementsByTagName("inertia")[joint_id].getAttribute("ixz")
iyy = xml_obj.getElementsByTagName("inertia")[joint_id].getAttribute("iyy")
iyz = xml_obj.getElementsByTagName("inertia")[joint_id].getAttribute("iyz")
izz = xml_obj.getElementsByTagName("inertia")[joint_id].getAttribute("izz")
xyz = xml_obj.getElementsByTagName("inertial")[joint_id].getElementsByTagName("origin")[0].getAttribute("xyz")
xyz = xyz.split(" ")
xyz = np.array(xyz,dtype=np.float32)
data = [float(mass),float(ixx),float(iyy),float(izz),float(ixy),float(ixz),float(iyz),xyz[0],xyz[1],xyz[2]]
return data
if __name__ == '__main__':
if os.path.exists("ec66.urdf"): # urdf文件预处理
os.replace("ec66.urdf","ec66.xml")
xmlDoc = xml.dom.minidom.parse("ec66.xml") # 加载文件
link_count = xmlDoc.getElementsByTagName("link").length # 统计 link 数量
joint_count = xmlDoc.getElementsByTagName("joint").length # 统计 joint 数量
print(joint_count)
data = getInertial(1,xmlDoc) # 获取 link1 动力学参数
print(data[0]) # 打印 link1 mass值
print(data[7]) # 打印 link1 质心x
print(data[8]) # 打印 link1 质心y
print(data[9]) # 打印 link1 质心z
参考文献:
- https://www.runoob.com/dom/dom-tutorial.html
欢迎分享,转载请注明来源:内存溢出
评论列表(0条)