【kdl】用户手册 Kinematic Trees 、Kinematic and Dynamic Solvers

Kinematic Trees

一个KDL::Chain或KDL::Tree由KDL::Segments的串联组成/构成。一个KDL::Segment由一个KDL::Joint和KDL::RigidBodyInertia组成，并在该段上定义了参考帧和尖端帧。以下图像分别展示了一个KDL::Segment，KDL::Chain，和KDL::Tree。在此页面底部，你会找到更详细描述的链接。

KDL段

黑色：KDL::Segment：

•  reference frame 参考帧{F_reference} （通过相对于此帧的其他帧的定义隐式定义）

• tip frame 尖端帧{F_tip}：从关节的终端到段的尖端的帧，默认值：Frame::Identity()。从关节到尖端的转换被标记为T_tip（在KDL中直接由KDL::Frame表示）。在一个运动链或树中，子段添加到父段的尖端帧（父段的尖端帧=子段的参考帧）。

• 组成了一个KDL::Joint（红色）和一个KDL::RigidBodyInertia（绿色）
  

红色：KDL::Joint：单DOF关节在或沿着关节帧{F_joint}的轴。此关节帧具有与参考帧{F_reference}相同的方向，但可以偏移相对于此参考帧的向量p_origin（默认：无偏移）。
绿色：KDL::RigidBodyInertia：笛卡尔空间惯性矩阵，参数是质量，从参考帧{F_reference}到重心（p_cog）的向量和重心帧{F_cog}中的旋转惯性。

KDL链

KDL树

选择你的版本：（1.0.x是已发布的版本 ，1.1.x正在讨论中（参见kinfam_refactored git分支））(注：2021 年 9 月 12 日就发布了1.5.1 版)

https://www.orocos.org/wiki/main-page/kdl-wiki/user-manual/kinematic-trees/kinematic-trees-kdl-10x.html

https://www.orocos.org/wiki/main-page/kdl-wiki/user-manual/kinematic-chains/kinematic-chains-kdl-11x.html

Kinematic Trees - KDL 1.1.x

KDL::Joint

Joint允许两个Segments之间在一个自由度上进行转动或者平移

创建关节

Joint rx = Joint(Joint::RotX);//关于X轴的旋转关节
Joint ry = Joint(Joint::RotY);//关于Y轴的旋转关节
Joint rz = Joint(Joint::RotZ);//关于Z轴的旋转关节
Joint tx = Joint(Joint::TransX);//沿X轴的平移关节
Joint ty = Joint(Joint::TransY);//沿Y轴的平移关节
Joint tz = Joint(Joint::TransZ);//沿Z轴的平移关节
Joint fixed = Joint(Joint::None);//刚性连接

注意：查看API文档了解所有构造方法
关节的姿态和twist

Joint rx = Joint(Joint::RotX);
double q = M_PI/4;//关节位置
Frame f = rx.pose(q);
double qdot = 0.1;//关节速度
Twist t = rx.twist(qdot);

f是从关节的零位置移动到关节值q的结果姿态，t是从应用关节速度qdot得出的，以关节的零位置对应的帧表示的twist

KDL::Segment

一个Segment是一个理想的刚体，连接了一个单一的Joint和一个明确的tip frame。它包含：

• 一个位于Segment根帧的Joint。

• 一个描述位于关节末端和Segment尖端帧之间姿态的Frame。
  

创建Segment

Segment s = Segment(Joint(Joint::RotX),
                Frame(Rotation::RPY(0.0,M_PI/4,0.0),
                          Vector(0.1,0.2,0.3) )
                    );

注意：构造函数复制参数，你不能在之后更改frame或joint！！！
Segment的姿态和twist

double q=M_PI/2;//关节位置
Frame f = s.pose(q);//s如之前的例子那样构建
double qdot=0.1;//关节速度
Twist t = s.twist(q,qdot);

从关节的零位置移动到关节值q，f是得到的姿态，它表示出了新的tip frame 相对于 Segment s的根帧的新姿态。从在关节值q上应用关节速度qdot得出的t，是在Segment s的根帧中表示的tip frame的twist。

KDL::Chain

一个KDL::Chain是

• 一个由关节连接的串行物体链的运动描述。
  

• 由KDL::Segments构建。

Chain有

• 一个默认构造函数，创建一个没有任何segments的空chain。

• 一个复制构造函数，创建一个已存在chain的副本。

• 也支持赋值运算符。

Chain chain1;
Chain chain2(chain3);
Chain chain4 = chain5;

通过在chain末端添加segments或存在chains创建Chain。所有segments必须有不同的名字(或"NoName")，否则methods将返回false并且segments不会被添加。函数添加了参数的拷贝，而不是参数本身！

bool exit_value;
bool exit_value = chain1.addSegment(segment1);
exit_value = chain1.addChain(chain2);

你可以得到关节数量和segments数量（这不总是一样的，因为一个segment可以有Joint::None，它不包括在关节数量内）：

unsigned int nj = chain1.getNrOfJoints();
unsigned int js = chain1.getNrOfSegments();

你可以通过得到每一连续的segment的引用来遍历一条chain中的segments。如果索引超出范围，method将返回false。

Segment segment3;
bool exit_value = chain1.getSegment(3, segment3);

你也可以通过名字请求一个segment：

Segment segment3;
bool exit_value = chain1.getSegment("Segment 3", segment3);

可以请求根段和叶段，以及所有segments in the chain。

bool exit_value;
Segment root_segment;//根段
Segment leaf_segment;//叶段
std::vector<Segment> segments;//多有段
exit_value = chain1.getRootSegment(root_segment);
exit_value = chain1.getLeafSegment(leaf_segment);
exit_value = chain1.getSegments(segments);

你可以请求在某个根和尖端之间的chain的一部分：

bool exit_value;
Chain part_chain;
 
exit_value = chain1.getChain_Including(1,3, part_chain);
exit_value = chain1.getChain_Including("Segment 1","Segment 3", part_chain);
//Segment 1 and Segment 3 are included in the new chain!
 
exit_value = chain1.getChain_Excluding(1,3, part_chain);
exit_value = chain1.getChain_Excluding("Segment 1","Segment 3", part_chain);
//Segment 1 is not included in the chain. Segment 3 is included in the chain.

有一个函数可以复制到给定segment数或segment名的chain：

bool exit_value;
Chain chain_copy;
exit_value = chain1.copy(3, chain_copy);
exit_value = chain1.copy("Segment 3", chain_copy);
//Segment 3, 4,... are not included in the copy!

KDL::Tree

一个KDL::Tree是

• 一个由关节连接的物体树的运动描述。

• 由KDL::Segments构建。

A Tree有

• 一个构造函数，创建一个没有任何segments并且给定名称为其根名字的空tree。如果没有给定名称，根名字将是"root"。
  

• 一个复制构造器，创建一个已存在tree的副本。
  

• 赋值操作符。

Tree tree1;
Tree tree2("RootName");
Tree tree3(tree4);
Tree tree5 = tree6;

通过向给定的hook 钩名添加segments，存在的chains或存在的trees来构建Trees。如果给定的钩名不在tree内，methods将返回false。这个函数添加了参数的拷贝，而不是参数本身！

bool exit_value;
exit_value = tree1.addSegment(segment1,"root");
exit_value = tree1.addChain(chain1,"Segment 1");
exit_value = tree1.addTree(tree2,"root");

你可以得到关节数量和segments数量（这不总是一样的，因为一个segment可以有Joint::None，它不包括在关节数量中）：

unsigned int nj = tree1.getNrOfJoints();
unsigned int js = tree1.getNrOfSegments();

你可以获取root segment和leaf segments：

bool exit_value;
std::map<std::string,TreeElement>::const_iterator root;
std::map<std::string,TreeElement> leafs;
exit_value = tree1.getRootSegment(root);
exit_value = tree1.getLeafSegments(leafs);

你也可以通过名字来获取tree中的specific segment：

std::map<std::string,TreeElement>::const_iterator segment3;
bool exit_value = tree1.getSegment("Segment 3",segment3);

你可以获取tree中的segments：

std::map<std::string,TreeElement> segments;
bool exit_value = tree1.getSegments(segments);

在tree中你可以请求在某个root和tip之间的chain：

bool exit_value;
Chain chain;
exit_value = tree1.getChain("Segment 1","Segment 3",chain);
//Segment 1 and segment 3 are included but segment 1 is renamed.
Chain chain2;
exit_value = tree1.getChain("Segment 3","Segment 1",chain2);
//Segment 1 and segment 3 are included but segment 3 is renamed.

这个chain也可以用给定的root name ("root" if no name is given)在一个tree结构中请求。

bool exit_value;
Tree tree;
exit_value = tree1.getChain("Segment 1","Segment 3",tree,"RootName");
Tree tree2;
exit_value = tree1.getChain("Segment 3","Segment 1",tree2,"RootName");

有一个函数可以复制树，不包括某些段及其所有后代。

bool exit_value;
Tree tree_copy;
exit_value = tree1.copy("Segment 3", tree_copy);
//tree1 is copied up to segment 3 (excluding segment 3).
std::vector<std::string> vect;
vect.push_back("Segment 1");
vect.push_back("Segment 7");
exit_value = tree1.copy(vect,tree_copy);

Kinematic and Dynamic Solvers

KDL当前只包含用于运动链的通用解算器。他们可以（小心地）用于每一个KDL::Chain。

通用解算器背后的想法是拥有一个统一的API。我们通过从每种类型的解算器的抽象类继承来实现这一点：

• ChainFkSolverPos
  

• ChainFkSolverVel
  

• ChainIkSolverVel
  

• ChainIkSolverPos

每条链需要创建一个单独的解算器。在构造时，它将分配所有必要的资源。

特定类型的解算器可以在接口中添加一些解算器特定的函数/参数，但它的主要解算目的仍然必须使用通用接口。

正向运动学使用函数JntToCart(...)从关节点空间值计算笛卡尔空间值。反向运动学使用函数CartToJnt(...)从笛卡尔空间值计算关节点空间值。

Recursive forward kinematic solvers

递归正向运动学解算器
目前我们只有一个通用的正向和速度位置运动学解算器。它递归地添加连续segments的位姿/速度，从第一个segment到最后一个，你也可以通过给出Segment的数量来得到中间结果：

ChainFkSolverPos_recursive fksolver(chain1);
JntArray q(chain1.getNrOfJoints);
q=...
Frame F_result;
fksolver.JntToCart(q,F_result,segment_nr);

注：文档过旧，目前的kdl已经包含很多算法了。

网址:

Release v1.5.1 · orocos/orocos_kinematics_dynamics (github.com)

https://github.com/orocos/orocos_kinematics_dynamics/releases

Kinematic and Dynamic Solvers | The Orocos Project

https://www.orocos.org/wiki/Kinematic_and_Dynamic_Solvers.html

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

The End