不装 ROS 也能用 PyKDL！使用kdl_parser解析URDF并进行IK

视频讲解：

不装 ROS 也能用 PyKDL！使用kdl

代码仓库：https://github.com/LitchiCheng/mujoco-learning.git

PyKDL可以直接解析URDF或者dh参数，如果不是ROS环境，则可以安装kdl_parser，以及lxml和urdf_parser_py

git clone https://github.com/jvytee/kdl_parser.git
cd kdl_parser
uv pip install .
uv pip install lxml urdf_parser_py

实验目的，通过urdf读取，使用kdl进行FK正运动学后将结果再和IK逆运动学进行对比，代码命名为kdl_urdf_test.py，同时上面的环境依赖也打包在requirements.txt中

import PyKDL
from kdl_parser.urdf import treeFromFile, treeFromUrdfModel
from urdf_parser_py.urdf import URDF
import numpy as np
import osdef load_urdf_to_kdl(urdf_file_path):"""从URDF文件加载KDL树结构参数:urdf_file_path: URDF文件的路径返回:success: 加载是否成功tree: KDL树对象"""if not os.path.exists(urdf_file_path):print(f"urdf not exists - {urdf_file_path}")return False, Nonetry:success, tree = treeFromFile(urdf_file_path)if success:print(f"load sucess: {urdf_file_path}")print(f"chain has {tree.getNrOfSegments()} segs and {tree.getNrOfJoints()} joints")return success, treeexcept Exception as e:print(f"methods1 failed: {e}")# 使用二进制模式读取文件并移除编码声明try:with open(urdf_file_path, 'rb') as f:# 读取二进制数据xml_content = f.read()# 尝试解码为字符串try:xml_str = xml_content.decode('utf-8')except UnicodeDecodeError:# 尝试其他编码xml_str = xml_content.decode('latin-1')# 移除XML编码声明if xml_str.startswith('<?xml'):xml_str = xml_str[xml_str.find('?>') + 2:]# 从字符串创建URDF模型robot_model = URDF.from_xml_string(xml_str)# 从URDF模型创建KDL树success, tree = treeFromUrdfModel(robot_model)if success:print(f"method2 sucess: {urdf_file_path}")print(f"chain has {tree.getNrOfSegments()} segs and {tree.getNrOfJoints()} joints")return success, treeelse:print("method2 failed")except Exception as e:print(f"method2 failed: {e}")try:robot_model = URDF.from_xml_file(urdf_file_path)success, tree = treeFromUrdfModel(robot_model)if success:print(f"method3 sucess: {urdf_file_path}")print(f"chain has {tree.getNrOfSegments()} segs and {tree.getNrOfJoints()} joints")return success, treeelse:print("method3 failed")except Exception as e:print(f"method3 failed: {e}")print(f"can not load: {urdf_file_path}")return False, Nonedef create_chain_from_tree(tree, base_link, tip_link):"""从KDL树创建运动链参数:tree: KDL树对象base_link: 基链接名称tip_link: 末端执行器链接名称返回:chain: KDL链对象"""try:chain = tree.getChain(base_link, tip_link)print(f"create chain from {base_link} to {tip_link}")print(f"chain has {chain.getNrOfSegments()} segs and {chain.getNrOfJoints()} joints")return chainexcept Exception as e:print(f"create chain failed: {e}")return Nonedef setup_kinematics(chain):"""设置正逆运动学求解器参数:chain: KDL链对象返回:fk_solver: 正向运动学求解器ik_vel_solver: 逆速度运动学求解器ik_pos_solver: 逆位置运动学求解器chain: 运动链对象 (添加此返回值以便在其他函数中使用)"""fk_solver = PyKDL.ChainFkSolverPos_recursive(chain)ik_vel_solver = PyKDL.ChainIkSolverVel_pinv(chain)max_iterations = 100eps = 1e-6ik_pos_solver = PyKDL.ChainIkSolverPos_NR(chain, fk_solver, ik_vel_solver, max_iterations, eps)return fk_solver, ik_vel_solver, ik_pos_solver, chaindef perform_forward_kinematics(fk_solver, joint_positions):"""执行正向运动学计算参数:fk_solver: 正向运动学求解器joint_positions: 关节位置列表返回:frame: KDL Frame对象，表示末端执行器的位姿"""q = PyKDL.JntArray(len(joint_positions))for i in range(len(joint_positions)):q[i] = joint_positions[i]frame = PyKDL.Frame()status = fk_solver.JntToCart(q, frame)if status >= 0:position = [frame.p.x(), frame.p.y(), frame.p.z()]rotation = frame.M.GetQuaternion() print(f"fk sucess:")print(f"pos: {position}")print(f"quat: {rotation}")return frameelse:print("fk failed")return Nonedef perform_inverse_kinematics(ik_pos_solver, chain, target_frame, initial_joints=None):"""执行逆运动学计算参数:ik_pos_solver: 逆位置运动学求解器chain: 运动链对象target_frame: 目标位姿 (KDL Frame)initial_joints: 初始关节配置 (可选)返回:joint_positions: 关节位置列表"""num_joints = chain.getNrOfJoints()if initial_joints is None:q_init = PyKDL.JntArray(num_joints)else:q_init = PyKDL.JntArray(num_joints)for i in range(num_joints):q_init[i] = initial_joints[i]q_out = PyKDL.JntArray(num_joints)status = ik_pos_solver.CartToJnt(q_init, target_frame, q_out)if status >= 0:joint_positions = [q_out[i] for i in range(num_joints)]print(f"ik sucess:")print(f"Joints: {joint_positions}")return joint_positionselse:print("ik failed")return Nonedef main():urdf_file = "model/so_arm100_description/so100.urdf"success, tree = load_urdf_to_kdl(urdf_file)if not success:returnbase_link = "Base"tip_link = "Moving Jaw"chain = create_chain_from_tree(tree, base_link, tip_link)if chain is None:returnfk_solver, ik_vel_solver, ik_pos_solver, chain = setup_kinematics(chain)print("\n=== FK ===")joint_positions = [0.2, 0.2, 0.3, 0.4, 0.5, 0.6]if len(joint_positions) != chain.getNrOfJoints():print(f"joints ({len(joint_positions)}) not match ({chain.getNrOfJoints()})")joint_positions = [0.0] * chain.getNrOfJoints()end_effector_frame = perform_forward_kinematics(fk_solver, joint_positions)print("\n=== IK ===")if end_effector_frame is not None:# 将FK得到的末端执行器位姿作为目标inverse_joints = perform_inverse_kinematics(ik_pos_solver, chain, end_effector_frame, joint_positions)if inverse_joints is not None:verified_frame = perform_forward_kinematics(fk_solver, inverse_joints)if verified_frame is not None:# 计算位置误差error_pos = np.sqrt((verified_frame.p.x() - end_effector_frame.p.x())**2 +(verified_frame.p.y() - end_effector_frame.p.y())**2 +(verified_frame.p.z() - end_effector_frame.p.z())**2)print(f"error: {error_pos}")if __name__ == "__main__":main()

可以看到，直接进行加载，会因为xml的原因失败，使用方法2进行去除xml声明即可

同时指定base和ee，能正确识别到运动链和关节，正向运动学和逆向运动学对比误差为0.0，表示结果一致。