欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页  >  IT编程

从URDF到KDL(C++&Python)

程序员文章站 2022-03-23 19:37:20
从urdf到kdl(c++&python) 毕竟我也是一个搞机器人的,今天就来写一些和机器人紧密相关的事情。 从urdf到kdlcpython kdl 简介 urdf模型 kdl c kdl...

从urdf到kdl(c++&python)

毕竟我也是一个搞机器人的,今天就来写一些和机器人紧密相关的事情。

从urdf到kdlcpython kdl 简介 urdf模型 kdl c kdl python


kdl 简介

pykdl是一个神奇的库。里面包含了kdl库与vector,rotation, frame, kinematics, dynamics的相关函数和接口。计算机器人学中的运动学/动力学和坐标变换都是相当的彪悍。甚至可以加载urdf模型计算运动学,调用dh相关接口或者chain接口直接创建机器人模型。这个库主要是面向c++的,通过sig的形式做了相应的python接口。这里先讨论论c++中kdl的使用。
wiki tutorials
official site and git
python documentation
python kdl git

urdf模型

urdf模型是用于描述机器人运动学参数、动力学参数的配置文件。可以从多种来源获得,比如solidworks可以直接保存出来。当然也可以自己编写,并不是很繁琐。
这篇教程清晰地写明了如何创建urdf模型。还是把最终的代码贴上来:

from urdf_parser_py.urdf import urdf
from pykdl_utils.kdl_parser import kdl_tree_from_urdf_model
robot = urdf.from_xml_file('/home/file/catkin_ws/src/kdl_test/src/ur3_robot.urdf') # get a tree

然后在python中求解机构的运动学正逆解非常方便:

# kdl version 1
#!/usr/bin/env python
#-*- coding:utf-8 -*-
############################
#file name: kdl_module.py
#author: wang
#mail: wang19920419@hotmail.com
#created time:2017-09-09 10:51:39
############################

import sys
sys.path.insert(0, "/home/file/catkin_ws/src/basic_math/hrl-kdl-indigo-devel/hrl_geom/src")
sys.path.insert(0, "/home/wangxu/catkin_ws/src/basic_math/hrl-kdl-indigo-devel/pykdl_utils/src")

from urdf_parser_py.urdf import urdf
from pykdl_utils.kdl_kinematics import kdlkinematics
from pykdl_utils.kdl_parser import kdl_tree_from_urdf_model

robot = urdf.from_xml_file("/home/file/catkin_ws/src/kdl_test/src/ur3_robot.urdf")
tree = kdl_tree_from_urdf_model(robot)
print tree.getnrofsegments()
chain = tree.getchain("base", "tool0")
print chain.getnrofjoints()
kdl_kin = kdlkinematics(robot, "base_link", "ee_link", tree)
q = [0,0,0,0,0,0]
pose = kdl_kin.forward(q) # forward kinematics (returns homogeneous 4x4 numpy.mat)
q_ik = kdl_kin.inverse(pose, [0.1,0.1,-0.1,0.1,0.1,0.1]) # inverse kinematics
if q_ik is not none:
    pose_sol = kdl_kin.forward(q_ik) # should equal pose
#j = kdl_kin.jacobian(q)
print 'q:', q
print 'q_ik:', q_ik
print 'pose:', pose
if q_ik is not none:
    print 'pose_sol:', pose_sol
#print 'j:', j

需要说明的:
1. 运动学正解输出的是4×4的坐标变换矩阵,如需变成位姿、四元数或者欧拉角需要进行额外的转换。
2. 运动学逆解的计算需要输入目标位姿和迭代角度初值,初值设置不要太离谱,不然就收敛不过去了,就好比深度学习权重初始值不能随机太夸张,否则就会not a number一样。
3. 如需其他开发,就要在这个基础上增加其他函数,见仁见智。
4. 无论是c++还是python,使用kdl都需要先将urdf parse成kdl能理解的模型。于是就有了两个语言的parser方法:

kdl是通用的运动学动力学库,对于复杂的运动学问题,比如并联机构的正解问题、串联机构的反解问题、冗余机构的无穷解问题都是利器。