KDL 基本数据结构与设计思想

基本数据结构及解释
运动学类
轨迹规划类
KDL中的OOP思想
参考 & 出处

基本数据结构及解释

KDL::Vector

表示一个三维的向量，可以用来表示空间中的一个点或一个方向。它有三个分量，分别是 x，y 和 z，可以用下标或括号访问。它提供了一些常用的操作，如加法，减法，数乘，点乘，叉乘，取模，归一化，旋转等。它还可以和 KDL::Rotation，KDL::Frame，KDL::Twist，KDL::Wrench 等类进行转换或运算。

KDL::Rotation

表示一个三维的旋转，可以用来表示空间中的一个坐标系或一个姿态。它是一个 3x3 的矩阵，每一列对应一个坐标轴的方向。它提供了一些常用的操作，如乘法，逆，转置，求迹，求行列式，求特征值，求特征向量等。它还提供了一些常用的构造方法，如从欧拉角，四元数，轴角，RPY 角等创建旋转，或者从两个向量之间的旋转，或者从三个向量之间的旋转创建旋转。它还可以和 KDL::Vector，KDL::Frame，KDL::Twist，KDL::Wrench 等类进行转换或运算。

KDL::Frame

表示一个三维的坐标系，可以用来表示空间中的一个物体或一个参考系。它由一个 KDL::Rotation 和一个 KDL::Vector 组成，分别表示坐标系的旋转和平移。它提供了一些常用的操作，如乘法，逆，求逆运动，求相对运动，求相对速度，求相对加速度等。它还可以和 KDL::Vector，KDL::Rotation，KDL::Twist，KDL::Wrench 等类进行转换或运算。

KDL::Twist

表示一个三维的扭转，可以用来表示空间中的一个刚体的速度或加速度。它由一个 KDL::Vector 和一个 KDL::Vector 组成，分别表示刚体的线速度和角速度。它提供了一些常用的操作，如加法，减法，数乘，点乘，叉乘，取反，取模，归一化，旋转等。它还可以和 KDL::Vector，KDL::Rotation，KDL::Frame，KDL::Wrench 等类进行转换或运算。

KDL::Wrench

表示一个三维的力矩，可以用来表示空间中的一个刚体的力或力矩。它由一个 KDL::Vector 和一个 KDL::Vector 组成，分别表示刚体的力和力矩。它提供了一些常用的操作，如加法，减法，数乘，点乘，叉乘，取反，取模，归一化，旋转等。它还可以和 KDL::Vector，KDL::Rotation，KDL::Frame，KDL::Twist 等类进行转换或运算。

运动学类

KDL::Chain

封装了一个串联的运动学连接结构，由多个段组成。每个段包含一个关节和一个偏移坐标系，表示前一个段的关节和当前段的关节之间的几何姿态。这个类可以用来表示机器人的运动学模型，如机械臂，机器人腿等。

KDL::ChainFkSolverAcc

正向加速度运动学求解器，可以计算 KDL::Chain 的末端执行器的加速度，给定关节的位置，速度和加速度。这个类是一个抽象类，需要由具体的子类实现具体的求解算法。

KDL::ChainFkSolverPos

正向位置运动学求解器，可以计算 KDL::Chain 的末端执行器的位置，给定关节的位置。这个类是一个抽象类，需要由具体的子类实现具体的求解算法。

KDL::ChainFkSolverPos_recursive

另一个版本的正向位置运动学求解器，使用递归算法，从根坐标系开始，依次乘以每个段的偏移坐标系和关节变换，得到末端执行器的位置。

KDL::ChainFkSolverVel

正向速度运动学求解器，可以计算 KDL::Chain 的末端执行器的速度，给定关节的位置和速度。这个类是一个抽象类，需要由具体的子类实现具体的求解算法。

KDL::ChainFkSolverVel_recursive

另一个版本的正向速度运动学求解器，使用递归算法，从根坐标系开始，依次乘以每个段的偏移坐标系和关节变换，以及每个段的刚体速度，得到末端执行器的速度。

KDL::ChainIkSolverAcc

逆向加速度运动学求解器，可以计算 KDL::Chain 的关节加速度，给定末端执行器的加速度，关节的位置和速度。这个类是一个抽象类，需要由具体的子类实现具体的求解算法。

KDL::ChainIkSolverPos

逆向位置运动学求解器，可以计算 KDL::Chain 的关节位置，给定末端执行器的位置。这个类是一个抽象类，需要由具体的子类实现具体的求解算法。

KDL::ChainIkSolverPos_LMA

使用 Levenberg-Marquardt 方法的逆向位置运动学求解器，可以求解 KDL::Chain 的关节位置，给定末端执行器的位置。这个方法是一种迭代的数值优化方法，可以处理非线性的问题，也可以考虑关节的限制。

KDL::ChainIkSolverPos_NR

使用牛顿-拉夫逊方法的逆向位置运动学求解器，可以求解 KDL::Chain 的关节位置，给定末端执行器的位置。这个方法是一种迭代的数值优化方法，可以处理非线性的问题，但不能考虑关节的限制。

KDL::ChainIkSolverPos_NR_JL

使用牛顿-拉夫逊方法，并考虑关节限制的逆向位置运动学求解器，可以求解 KDL::Chain 的关节位置，给定末端执行器的位置。这个方法在每次迭代时，检查关节是否超出限制，如果超出，就将其设置为最大或最小值，然后继续迭代。

KDL::ChainIkSolverVel

逆向速度运动学求解器，可以计算 KDL::Chain 的关节速度，给定末端执行器的速度，关节的位置。这个类是一个抽象类，需要由具体的子类实现具体的求解算法。

KDL::ChainIkSolverVel_pinv

使用伪逆方法的逆向速度运动学求解器，可以计算 KDL::Chain 的关节速度，给定末端执行器的速度，关节的位置。这个方法是一种线性的最小二乘法，可以处理冗余或欠驱动的情况，但不能考虑关节的限制。

KDL::ChainIkSolverVel_pinv_givens

使用伪逆和 Givens 旋转的逆向速度运动学求解器，可以计算 KDL::Chain 的关节速度，给定末端执行器的速度，关节的位置。这个方法是一种线性的最小二乘法，使用 Givens 旋转来加速计算，可以处理冗余或欠驱动的情况，但不能考虑关节的限制。

KDL::ChainIkSolverVel_pinv_nso

使用伪逆和空间优化的逆向速度运动学求解器，可以计算 KDL::Chain 的关节速度，给定末端执行器的速度，关节的位置。这个方法是一种线性的最小二乘法，使用空间优化来减少关节的运动范围，可以处理冗余或欠驱动的情况，但不能考虑关节的限制。

KDL::ChainIkSolverVel_wdls

使用加权的最小二乘法的逆向速度运动学求解器，可以计算 KDL::Chain 的关节速度，给定末端执行器的速度，关节的位置。这个方法是一种非线性的最小二乘法，使用加权矩阵来平衡关节的运动和末端执行器的误差，可以处理冗余或欠驱动的情况，也可以考虑关节的限制。

KDL::Joint

表示一个简单的关节，有一个参数化的自由度和一个标量的动力学属性。关节的类型可以是旋转，平移，固定或无效。关节的变换可以由关节的位置和类型决定。

KDL::Segment

表示一个简单的段，是一个“刚体”（即一个坐标系和一个刚体惯性），包含一个关节和一些“手柄”，根和尖，用来连接其他的段。段的刚体速度可以由关节的速度和类型决定。

KDL::Tree

封装了一个树形的运动学连接结构，由多个段组成。每个段有一个唯一的名字，可以连接到其他段的根或尖。这个类可以用来表示机器人的运动学模型，如人形机器人，多足机器人等。

KDL::TreeFkSolverPos

正向位置运动学求解器，可以计算 KDL::Tree 的任意末端执行器的位置，给定关节的位置。这个类是一个抽象类，需要由具体的子类实现具体的求解算法。

KDL::TreeFkSolverPos_recursive

另一个版本的正向位置运动学求解器，使用递归算法，从根坐标系开始，依次乘以每个段的偏移坐标系和关节变换，得到任意末端执行器的位置。

KDL::TreeIkSolverPos

逆向位置运动学求解器，可以计算 KDL::Tree 的关节位置，给定任意末端执行器的位置。这个类是一个抽象类，需要由具体的子类实现具体的求解算法。

KDL::TreeIkSolverPos_NR_JL

使用牛顿-拉夫逊方法，并考虑关节限制的逆向位置运动学求解器，可以求解 KDL::Tree 的关节位置，给定任意末端执行器的位置。这个方法在每次迭代时，检查关节是否超出限制，如果超出，就将其设置为最大或最小值，然后继续迭代。

KDL::TreeIkSolverPos_Online

使用在线的逆向位置运动学求解器，可以求解 KDL::Tree 的关节位置，给定任意末端执行器的位置。这个方法不需要预先构建整个运动学树，而是根据需要动态地添加或删除段，可以适应运动学结构的变化。

KDL::TreeIkSolverVel

逆向速度运动学求解器，可以计算 KDL::Tree 的关节速度，给定任意末端执行器的速度，关节的位置。这个类是一个抽象类，需要由具体的子类实现具体的求解算法。

轨迹规划类

KDL::Path_Circle

处理圆形的路径，可以指定圆心，半径，起始角度，终止角度和旋转方向。

KDL::Path_Composite

管理复合的路径，可以将不同类型的路径组合在一起，如直线，圆弧，点等。

KDL::Path_Cyclic_Closed

处理闭合的循环路径，可以指定一个路径列表，使得最后一个路径的终点和第一个路径的起点重合。

KDL::Path_Line

处理线性的路径，可以指定起点，终点和插值方法。

KDL::Path_Point

管理点到点的路径，可以指定一个点的列表，按顺序连接。

KDL::Path_RoundedComposite

可能处理具有圆角或圆边的复合路径，可以指定一个路径列表和一个圆角半径。

KDL::RotationalInterpolation_SingleAxis

沿着单个轴插值旋转，可以指定旋转轴和旋转角度。

KDL::Trajectory

不同类型轨迹的基类，可以指定一个路径和一个速度曲线。

KDL::Trajectory_Composite

管理复合的轨迹，可以将不同的轨迹段或类型组合在一起。

KDL::Trajectory_Segment

处理单个的轨迹段，可以指定一个轨迹和一个起始时间。

KDL::Trajectory_Stationary

可能表示静止或不动的轨迹，没有任何运动。

KDL::VelocityProfile_Dirac

表示瞬时或未定义的速度曲线，类似于 Dirac delta 函数的行为，可以指定一个峰值速度。

KDL::VelocityProfile_Rectangular

表示一个矩形的速度曲线，即速度在一段时间内保持恒定，然后突然变为零。可以指定一个最大速度和一个总时间。

KDL::VelocityProfile_Spline

表示一个样条的速度曲线，即速度由一系列的三次多项式拼接而成，保证连续性和光滑性。可以指定一个速度点的列表，以及每个点的时间。

KDL::VelocityProfile_Trap

表示一个梯形的速度曲线，即速度先以恒定的加速度增加，然后保持最大速度一段时间，再以恒定的减速度减小。可以指定一个最大速度，一个最大加速度和一个总距离。

KDL::VelocityProfile_TrapHalf

表示一个半梯形的速度曲线，即速度只有加速或减速的阶段，没有匀速的阶段。可以指定一个最大速度，一个最大加速度和一个总距离。

KDL中的OOP思想

C++语言的一个重要特征是可以进行面向对象的设计（OOP），而OOP的核心思想之一便是使用继承，可以定义相似的类型并对其相似关系进行建模。通过继承联系在一起的类构成一种层次关系，层次关系的根部有一个基类（base class），其它类则直接或间接地从基类继承而来，这些继承得到的类称为派生类（derived class）。

机器人轨迹规划算法中，一个很重要的问题是路径的计算，但是机器人的路径有多种，包含直线路径，圆弧路径，复合路径等等。所以在KDL中就定义了一个基类: 路径（class path），和其它6个派生类: 开口圆路径（Path_Circle）、组合路径（Path_Composite）、封闭圆路径（Path_Cyclic_Closed）、直线路径（Path_Line）、仅含点的路径（Path_Point）、含圆弧过渡的路径（Path_RoundedComposite）。

需注意一下基类（path）中的虚函数（virtual）很多是纯虚函数，这些是派生类中必须给出自己的定义，例如Path_Line中相应的虚函数，可看出每种路径的都需要定义并重新定义:LengthToS，PathLength，Pos，Vel，Acc，Write。

class Path
	{
		virtual double LengthToS(double length)  = 0;
		virtual double PathLength() = 0;
		virtual Frame Pos(double s) const = 0;
		virtual Twist Vel(double s,double sd) const  = 0;
		virtual Twist Acc(double s,double sd,double sdd) const  = 0;
		virtual void Write(std::ostream& os)  = 0;
		static Path* Read(std::istream& is);
		virtual Path* Clone() = 0;
		virtual IdentifierType getIdentifier() const=0;
		virtual ~Path() {}
	};
 
}
 
class Path_Line : public Path
	{
		Path_Line(const Frame& F_base_start,const Frame& F_base_end,RotationalInterpolation* orient,double eqradius,bool _aggregate=true);
		Path_Line(const Frame& F_base_start,const Twist& twist_in_base,RotationalInterpolation* orient,double eqradius,bool _aggregate=true);
		double LengthToS(double length);
		virtual double PathLength();
		virtual Frame Pos(double s) const;
		virtual Twist Vel(double s,double sd) const ;
		virtual Twist Acc(double s,double sd,double sdd) const;
		virtual void Write(std::ostream& os);
		virtual Path* Clone();
		virtual IdentifierType getIdentifier() const {return ID_LINE;}
		virtual ~Path_Line();
	};
}

参考 & 出处

[1] 机器人开源项目KDL源码学习:（10）KDL中的OOP思想—-继承

[2] KDL API 使用参考