PyKDL---运动段、运动链和运动树

如何描述机器人，这是一个很重要的问题，方法得当，可以大大简化问题，KDL采用段、链、树三个层级来描述机器人，可以方便地运用于串联和并联机器人。

机器人描述最小单元：Segment

首先，我们思考一个问题，描述机器人最小结构需要那些参数：关节、坐标、连杆质量、连杆惯性张量。
在图中可以看出，segment拥有一个参考坐标系、关节坐标系、质心坐标系，tip坐标系，其中参考坐标系通常与关节坐标系重合，tip坐标系用于描述杆件姿态，同时作为下一个segment的参考坐标系，质心坐标系与参考坐标系同向。
在KDL中采用三个结构体来描述，关节、坐标，刚体惯量，PyKDL中建立方法如下

#创建段
segment1 = PyKDL.Segment(joint1, frame1, RBI)
print "segment1:\n", segment1
---输出结果---
segment1:
NoName:[NoName:[RotZ, axis: [           0,           0,           1], origin[           0,           0,           0]],
 tip: 
[[    0.540302,           0,    0.841471;
            0,           1,           0;
    -0.841471,           0,    0.540302]
[           3,           2,           4]]]

建立坐标：Frame

在建立segment中，Frame表示tip坐标系相对于参考坐标系的位姿，坐标系可以表示连杆的姿态和位置，类似齐次矩阵，详细内容参照PyKDL—常用变量详解，PyKDL建立坐标如下

#坐标
frame1 = PyKDL.Frame(PyKDL.Rotation.RPY(0, 1, 0), PyKDL.Vector(3, 2, 4))
print "frame1:\n", frame1
---输出结果---
frame1:
[[    0.540302,           0,    0.841471;
            0,           1,           0;
    -0.841471,           0,    0.540302]
[           3,           2,           4]]

关节描述：Joint

KDL库源码中对关节参数进行定义：

• 1 关节名字：name
• 2 原点：origin
• 3 轴：_axis
• 4 关节类型：JointType
  JointType = None # (!) real value is ‘’
  None = 8
  RotAxis = 0
  RotX = 1
  RotY = 2
  RotZ = 3
  TransAxis = 4
  TransX = 5
  TransY = 6
  TransZ = 7
• 5 传动比：scale，一般默认设置就可以
• 6 偏置：offset
• 7 惯量：inertia，绕转动轴的一维惯量
• 8 阻尼：damping
• 9 刚度：stiffness

  /**
         * Constructor of a joint.
         *
         * @param name of the joint
         * @param origin the origin of the joint
         * @param axis the axis of the joint
         * @param scale scale between joint input and actual geometric
         * movement, default: 1
         * @param offset offset between joint input and actual
         * geometric input, default: 0
         * @param inertia 1D inertia along the joint axis, default: 0
         * @param damping 1D damping along the joint axis, default: 0
         * @param stiffness 1D stiffness along the joint axis,
         * default: 0
         */
        Joint(const std::string& name, const Vector& _origin, const Vector& _axis, const JointType& type, const double& _scale=1, const double& _offset=0,
	      const double& _inertia=0, const double& _damping=0, const double& _stiffness=0);

在Python中建立关节也是相同，可以输入相应的参数，但是有更简单的方法，直接用调用一些建好的关节来建立，如下所示

#关节
joint1 = PyKDL.Joint(PyKDL.Joint.RotZ)
print "joint:", joint1
---输出结果---
joint: NoName:[RotZ, axis: [0,0,1], origin[0,0,0]]

连杆动力学参数：RigidBodyInertia

连杆的动力学参数包括了三个量：

• 1 质量：m
• 2 质心：oc
• 3 惯性张量矩阵：Ic

RotationalInertia(double Ixx=0,double Iyy=0,double Izz=0,double Ixy=0,double Ixz=0,double Iyz=0) 

Python中输入对应参数，建立如下

#惯性张量
m = 0.5
oc = PyKDL.Vector(1, 0, 1)
RBI = PyKDL.RotationalInertia(1.0, 2.0, 3.0, 4.0, 5.0, 6.0)

I1 = PyKDL.RigidBodyInertia(m, oc, RBI)

串联机器人链：Chain

由基本元素段segment相连，构成了一个串联的链，

PyKDL建立链的方法如下

#********创建链*******#
#通过复制建立多个segment
links = []
for i in range(6):
    links.append(segment1)

#创建链
chain1 = PyKDL.Chain()
#为链加入segment
for i in range(6):
    chain1.addSegment(links[i])

#获取链的信息
joint_num = chain1.getNrOfJoints()
print "joint_num:", joint_num
Segment_num = chain1.getNrOfSegments()
print "Segment_num:", Segment_num
Segment_6 = chain1.getSegment(5)
print "Segment_6:", Segment_6
---输出结果---
joint_num: 6
Segment_num: 6
Segment_6: NoName:[NoName:[RotZ, axis: [           0,           0,           1], origin[           0,           0,           0]],
 tip: 
[[    0.540302,           0,    0.841471;
            0,           1,           0;
    -0.841471,           0,    0.540302]
[           3,           2,           4]]]

建立树：tree

一般，树不会直接代码建立，而是采用更简单的方法，通过URDF文件获取，获取方法如下所示。

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

#读取URDF文件
robot = URDF.from_xml_file("/home/d/catkin_ws/src/robot_description/armc_description/urdf/armc_description.urdf")

#将URDF转换为树
tree = kdl_tree_from_urdf_model(robot)

#截取需要的部分构成链
chain = tree.getChain("base_link", "sensor_link")