【kdl】用户手册 Geometric primitives

Kinematic Chain: Skeleton of a serial robot arm with six revolute joints.

Skeleton of a serial robot arm with six revolute joints. This is one example of a kinematic structure, reducing the motion modelling and specification to a geometric problem of relative motion of reference frames. The Kinematics and Dynamics Library (KDL) develops an application independent framework for modelling and computation of kinematic chains, such as robots, biomechanical human models, computer-animated figures, machine tools, etc. It provides class libraries for geometrical objects (point, frame, line,... ), kinematic chains of various families (serial, humanoid, parallel, mobile,... ), and their motion specification and interpolation.

具有六个旋转关节的串联机器人臂的骨架。这是一种运动结构的例子，将运动建模和规范简化为参考系之间相对运动的几何问题。运动学和动力学库（KDL）开发了一个应用独立的框架，用于建模和计算运动链，例如机器人、生物力学人体模型、计算机动画人物、机床等。它提供了几何对象（点、框架、线，…）、各种类型的运动链（串联、仿人、并联、移动，…）以及它们的运动规范和插值的类库。

KDL Overview

What can I use KDL for?

• 3D frame and vector transformations: KDL includes excellent support to work with vectors, points, frame transformations, etc. You can calculate a vector product, transform a point into a different reference frame, or even change the reference point of a 6d twist. For more information take a look at the geometry documentation on the KDL homepage.

• Kinematics and Dynamics of kinematic chains: You can represent a kinematic chain by a KDL Chain object, and use KDL solvers to compute anything from forward position kinematics, to inverse dynamics. For more information take a look at the chain documentation on the KDL homepage. The kdl_parser includes support to construct a KDL chain from a XML Robot Description Format (URDF) file.

• Kinematics of kinematic trees: You can represent a kinematic chain by a KDL Chain object, and use KDL solvers to compute forward position kinematics. Currently no other solvers are provided.

KDL 概述 

我可以用 KDL 做什么？ 

3D 帧和向量变换：KDL 提供了优秀的支持，可以处理向量、点、帧变换等。您可以计算向量积，将一个点变换到不同的参考系，或者改变一个 6d twist 的参考点。更多信息请查看 KDL 主页上的几何文档 https://www.orocos.org/kdl/geometry 。

运动链的运动学和动力学：您可以用一个 KDL Chain 对象来表示一个运动链，并使用 KDL 求解器来计算从正向位置运动学到逆向动力学的任何内容。更多信息请查看 KDL 主页上的链文档。kdl_parser 包括了从 XML 机器人描述格式（URDF）文件构造一个 KDL 链的支持https://wiki.ros.org/kdl_parser。

运动树的运动学：您可以用一个 KDL Chain 对象来表示一个运动链，并使用 KDL 求解器来计算正向位置运动学。目前没有提供其他求解器。

User Manual

Why to use KDL?

• Extensive support for :

  • Geometric primitives: point, frame, twist ...

  • Kinematic Trees: chain and tree structures. In literature, multiple definitions exist for a kinematic structure: a chain as the equivalent for all types of kinematic structures (chain, tree, graph) or chain as the serial version of a kinematic structure. KDL uses the last, or using graph-theory terminology:

    • • • • A closed-loop mechanism is a graph,

          • an open-loop mechanism is a tree, and

          • an unbranched tree is a chain.

Next to kinematics, also parameters for dynamics are included (inertia...)

Kinematic and Dynamic Solvers: various generic forward and inverse kinematic algorithms, redundancy resolution, ...

Instantaneous Motions: under construction (Expected release, Spring 2011)

Motion Trajectories: Cartesian paths, velocity profiles, Cartesian trajectories

Realtime-safe operations/functions whenever relevant: they do not lead to dynamic memory allocations and all of them are deterministic in time.

Python bindings

• Typekits and transport-kits for Orocos/RTT

• Integrated in ROS

为什么要使用KDL？
它提供了广泛的支持，包括：
几何基元：点、框架、扭曲等。
运动学树：链与树结构。在文献中，运动学结构有多个定义：一个链可以等同于所有类型的运动学结构（链、树、图）或者链就是运动学结构的串行版本。KDL使用的是后者，或者用图论术语来说：
一个闭环机构是一个图，
一个开环机构是一棵树，和
一个无分支的树是一条链。

除了运动学，也包括了动态学的参数（惯性等）
运动学和动态学求解器：各种通用的前向和反向运动学算法，冗余解决等。
瞬时运动：正在建设中（预计释放日期，2011年春季）
运动轨迹：笛卡尔路径，速度轮廓，笛卡尔轨迹
与实时相关的操作/功能尽可能安全：它们不会导致动态内存分配，并且所有操作在时间上都是确定的。
Python绑定
为Orocos/RTT提供了类型工具和传输工具
集成在ROS中

Geometric primitives

KDL::Vector

向量是一个包含X-Y-Z坐标值的3x1矩阵。它被用来表示：相对于参考框架的点的3D位置，6D运动或力实体的旋转和平移部分：<方程 id="vector"><方程>

创建向量

Vector v1; //默认构造函数，X-Y-Z被初始化为零
Vector v2(x,y,z); //X-Y-Z用给定的值初始化
Vector v3(v2); //复制构造函数
Vector v4 = Vector::Zero(); //所有值设为零

获取/设置单个元素
操作符[ ]和( )使用从0..2的索引，索引检查由DEBUG/NDEBUG定义启用/禁用：

v1[0]=v2[1];//将v2的y值复制到v1的x值
v2(1)=v3(3);//将v3的z值复制到v2的y值
v3.x( v4.y() );//将v4的y值复制到v3的x值

乘以/除以一个标量
您可以使用操作符*和/将一个向量乘以或除以一个双精度数：

v2=2*v1;
  v3=v1/2;

向量的加法和减法

v2+=v1;
  v3-=v1;
  v4=v1+v2;
  v5=v2-v3;

向量的叉积和标量积

v3=v1v2; //叉积
double a=dot(v1,v2)//标量积

重置
您可以将向量的值重置为零：

SetToZero(v1);

比较向量
元素对元素的比较，有或无用户定义的精度：

v1==v2;
v2!=v3;
Equal(v3,v4,eps);//精度为eps

KDL::Rotation

旋转是表示对象相对于参考帧的3D旋转的3x3矩阵。<equation id="rotation"><equation>

创建旋转

创建旋转的安全方法
以下结果始终在一致的旋转中。这意味着行/列始终是归一化和正交的：

Rotation r1; //默认构造函数，初始化为3x3的单位矩阵
Rotation r1 = Rotation::Identity();//单位旋转 = 零旋转
Rotation r2 = Rotation::RPY(roll,pitch,yaw); //从Roll-Pitch-Yaw角度构建的旋转
Rotation r3 = Rotation::EulerZYZ(alpha,beta,gamma); //从欧拉Z-Y-Z角构建的旋转
Rotation r4 = Rotation::EulerZYX(alpha,beta,gamma); //从欧拉Z-Y-X角度构建的旋转
Rotation r5 = Rotation::Rot(vector,angle); //从等效轴（向量）和角度构建的旋转。

其他方法
以下方法应谨慎使用，因为它们可能导致不一致的旋转矩阵，因为没有检查列/行是否归一化或正交

Rotation r6( Xx,Yx,Zx,Xy,Yy,Zy,Xz,Yz,Zz);//给每个单独的元素（列为主）
Rotation r7(vectorX,vectorY,vectorZ);//给每个独立的列

获取值

单个值，索引范围从0..2：

double Zx = r1(0,2);

获取EulerZYZ，Euler ZYX，Roll-Pitch-Yaw角度，等效旋转轴和角度：

r1.GetEulerZYZ(alpha,beta,gamma);
r1.GetEulerZYX(alpha,beta,gamma);
r1.GetRPY(roll,pitch,yaw);
axis = r1.GetRot();//只给出旋转轴
angle = r1.GetRotAngle(axis);//给出角度和旋转轴

获取单位向量：

vecX=r1.UnitX();//or
  r1.UnitX(vecX);
  vecY=r1.UnitY();//or
  r1.UnitY(vecY);
  vecZ=r1.UnitZ();//or
  r1.UnitZ(vecZ);

旋转的反转
将旋转替换为其逆：

r1.SetInverse();//r1被反转并覆盖
获取逆旋转而不覆盖原始旋转：
r2=r1.Inverse();//r2是r1的逆旋转

组成旋转
将两个旋转组合为一个新的旋转，旋转的顺序很重要：

r3=r1*r2;

用X-Y-Z的基本旋转组合一个旋转：

r1.DoRotX(angle);
r2.DoRotY(angle);
r3.DoRotZ(angle);

这是以下表达的简写版本：

r1 = r1*Rotation::RotX(angle)

向量的旋转
用操作符旋转向量：

v2=r1*v1;

比较旋转
元素对元素比较，不论是否有用户定义的精度：

r1==r2;
r1!=r2;
Equal(r1,r2,eps);

KDL::Frame

帧是表示对象/帧相对于参考帧的姿态的4x4矩阵。它包括：旋转矩阵M，表示对象/帧相对于参考帧的旋转  矢量p，表示对象/帧原点在参考帧中的位置<equation id="frame"><equation>

创建帧

Frame f1;//创建单位帧
Frame f1=Frame::Identity();//创建一个单位帧：Rotation::Identity() 和 Vector::Zero()
Frame f2(your_rotation);//用your_rotation和零向量创建一个帧
Frame f3(your_vector);//用your_vector和单位旋转创建一个帧
Frame f4(your_rotation,your_vector);//用your_rotation创建帧
Frame f5(your_vector,your_rotation);//和your_vector
Frame f5(f6);//复制构造函数

获取值
从4x4矩阵获得单个值，索引从0..3：

double x = f1(0,3);
double Yy = f1(1,1);

另一种方式是通过底层的旋转和矢量:

Vector p = f1.p;
Rotation M = f1.M;

组合帧
您可以使用*操作符来组合帧。如果你有一个帧F_A_B，表示帧B相对于帧A的姿态，以及一个帧F_B_C，表示帧C相对于帧B的姿态，计算帧F_A_C，表示帧C相对于帧A的姿态，如下所示：

Frame F_A_C = F_A_B * F_B_C;

F_A_C.p是帧C原点在帧A中表示的位置，F_A_C.M是帧C在帧A中表示的旋转。
反转帧
将帧替换为其反转：

//尚未实现

获取反转：

f2=f1.Inverse();//f2是f1的反转

比较帧
元素与元素的比较，无论是否有用户定义的精度：

f1==f2;
f1!=f2;
Equal(f1,f2,eps);

KDL::Twist

创建Twist

Twist t1; //默认构造函数，初始化vel和rot为零
Twist t2(vel,rot);//向量vel, 和向量rot
Twist t3 = Twist::Zero();//零Twist

注意：与创建帧不同，向构造函数提供vel和rot向量的顺序很重要。
获取值
使用操作符[ ]和( )，索引从0..2返回vel的元素，索引从3..5返回rot的元素：

double vx = t1(0);
double omega_y = t1[4];
t1(1) = vy;
t1[5] = omega_z;

由于一些机器人学文献将旋转部分放在顶部，所以使用vel, rot成员访问单个元素更安全：

double vx = t1.vel.x();//或者
vx = t1.vel(0);
double omega_y = t1.rot.y();//或者
omega_y = t1.rot(1);
t1.vel.y(v_y);//或者
t1.vel(1)=v_y;
//等等

乘以/除以一个标量
和Vector可用的操作符相同：

t2=2*t1;
  t2=t1*2;
  t2=t1/2;

添加/减少Twists
和Vector可用的操作符相同：

t1+=t2;
  t1-=t2;
  t3=t1+t2;
  t3=t1-t2;

比较Twists
元素与元素的比较，无论是否有用户定义的精度：

t1==t2;
  t1!=t2;
  Equal(t1,t2,eps);

KDL::Wrench

创建Wrench

Wrench w1; //默认构造函数，初始化force和torque为零
Wrench w2(force,torque);//向量force, 和向量torque
Wrench w3 = Wrench::Zero();//零Wrench

获取值
使用操作符[ ]和( )，索引从0..2返回force的元素，索引从3..5返回torque的元素：

double fx = w1(0);
double ty = w1[4];
w1(1) = fy;
w1[5] = tz;

由于一些机器人学文献将力矩部分放在顶部，所以使用torque, force成员访问单个元素更安全：

double fx = w1.force.x();//或者
fx = w1.force(0);
double ty = w1.torque.y();//或者
ty = w1.torque(1);
w1.force.y(fy);//或者
w1.force(1)=fy;//等等

乘以/除以一个标量
和Vector可用的操作符相同：

w2=2*w1;
  w2=w1*2;
  w2=w1/2;

添加/减少Wrenchs
和Twist可用的操作符相同：

w1+=w2;
  w1-=w2;
  w3=w1+w2;
  w3=w1-w2;

比较Wrenchs
元素与元素的比较，无论是否有用户定义的精度：

w1==w2;
  w1!=w2;
  Equal(w1,w2,eps);

Twist and Wrench transformations

Wrench和Twist都是在某个参考帧中表示的；Twist的平移向量vel和Wrench的力矩向量torque分别表示参考帧中某一参考点的速度和力矩。常见的参考点选择包括参考帧的原点或任务特定点.
如果更改了参考帧或参考点，Wrench或Twist的值就会改变。

只改变参考点
如果你想改变参考点，你需要从老的参考点到新的参考点的向量v_old_new，这个向量是在Wrench或Twist的参考帧中表示的：

t2 = t1.RefPoint(v_old_new);
w2 = w1.RefPoint(v_old_new);

只改变参考帧
如果你想改变参考帧但想保持参考点不变，你可以使用一个旋转矩阵R_AB，它表示当前参考帧B相对于新参考帧A的旋转：

ta = R_AB*tb;
wa = R_AB*wb;

注意：这个操作似乎将一个3x3的矩阵R_AB与6x1的矩阵tb或wb相乘，而实际上它使用的是从R_AB导出的6x6的螺旋变换矩阵。

改变参考帧和参考点
如果你想改变参考帧和参考点，你可以使用一个帧F_AB，它包含（i）表示当前参考帧B相对于新参考帧A旋转的旋转矩阵R_AB，并（ii）旧的参考点和在A中表示的新参考点的向量v_old_new：

ta = F_AB*tb;
wa = F_AB*wb;

注意：这个操作似乎将一个4x4的矩阵F_AB与6x1的矩阵tb或wb相乘，而实际上它使用的是从F_AB导出的6x6的螺旋变换矩阵。

First order differentiation and integration

t = diff(F_w_A,F_w_B,timestep)//微分
F_w_B = F_w_A.addDelta(t,timestep)//积分

t是在timestep秒内移动帧A到帧B的twist。t是在参考帧w中表示的，使用A的原点作为速度参考点。

t is the twist that moves frame A to frame B in timestep seconds. t is expressed in reference frame w using the origin of A as velocity reference point.

网址

Orocos Project documentation — Orocos documentation

https://docs.orocos.org/

The Kinematics and Dynamics Library | The Orocos Project  

https://orocos.org/kdl_old

Open Robot Control Software (github.com) 

https://github.com/orocos

作者陈晓永：智能装备专业高级职称，软件工程师，机械设计中级职称，机器人与自动化产线仿真动画制作        

The End