【kdl】API 使用参考

KDL

这是运动学和动力学库（KDL）的 API 参考，它是 Orocos 的一个子项目，但可以独立于 Orocos 使用。KDL 提供了不同种类的功能，分为以下几个模块：

• Geometric Primitives：几何原始体是指一些基本的几何形状，如点，线，圆，矩形，椭圆，三角形等。它们可以用来表示空间中的物体或坐标系的位置和方向。KDL 中提供了一些几何原始体的类，如 KDL::Vector，KDL::Rotation，KDL::Frame，KDL::Wrench 等，用来表示向量，旋转，坐标系，力和力矩等。

• Kinematic Families：运动学族是指一组具有相似属性的运动学结构，例如相同的连接拓扑，相同的求解算法等。运动学结构是指由一个或多个段组成的刚体运动的模型，例如机器人的运动学模型。段是指由一个关节和一个偏移坐标系组成的刚体，表示两个关节之间的几何姿态。关节是指具有一个参数化的自由度和一个标量的动力学属性的简单的运动元件，例如旋转，平移，固定或无效。运动学族的功能是指可以构建运动学链并访问它们的运动学和动力学属性的功能，例如正向和逆向的运动学和动力学。正向运动学是指根据关节的位置，速度或加速度，计算末端执行器的位置，速度或加速度。逆向运动学是指根据末端执行器的位置，速度或加速度，计算关节的位置，速度或加速度。

• Motion：运动是指刚体或运动学链在空间中随时间变化的状态，例如位置，速度，加速度等。运动的功能是指可以指定坐标系或运动学链的运动轨迹的功能，例如梯形速度曲线。运动轨迹是指描述运动状态随时间变化的规律的函数，例如位置函数，速度函数，加速度函数等。梯形速度曲线是指一种常用的运动轨迹，它的速度函数是一个梯形的曲线，即速度先以恒定的加速度增加，然后保持最大速度一段时间，再以恒定的减速度减小。

• KDLTK：KDLTK 是指用于将 KDL 集成到 Orocos 实时工具包（RTT）的接口代码。Orocos RTT 是一个用于开发实时控制系统的软件框架，它提供了一些基本的组件，如任务，数据流，服务，事件等。KDLTK 是一个 Orocos RTT 的组件，它可以将 KDL 的运动学和动力学的功能封装成 Orocos RTT 的服务，从而可以在 Orocos RTT 的环境中使用 KDL 的功能。

Geometric Primitives

• 几何原始体是指一些基本的几何形状，如向量，旋转，坐标系，扭转，力矩等，它们可以用来表示空间中的物体或运动的状态。KDL 提供了一些几何原始体的类，以及一些操作和转换的方法。

• 扭转和力矩有三种不同类型的转换，分别是坐标系乘以扭转或力矩，旋转乘以扭转或力矩，和扭转或力矩的参考点变换。这些转换可以改变扭转或力矩的参考点或基准。

• 为了提高效率，建议使用 p2 = A*(B*(Cp1))) 而不是 p2=ABCp1 的写法。

• 提出了一种命名约定，用于区分不同类型的几何原始体，例如坐标系，旋转，扭转，力矩，向量等。这种命名约定可以方便地表示不同坐标系之间的关系，以及进行运算和赋值。

• 规定了一些用于机器人的约定，例如世界坐标系，安装板坐标系，任务坐标系，传感器坐标系等。这些约定可以用来表示机器人的运动学和动力学的模型，以及进行运动和力控制。

• 规定了一些用于单位的约定，通常使用标准的国际单位制，如牛顿，米，秒等。

• KDL::Vector

• KDL::Rotation

• KDL::Frame

• KDL::Twist

• KDL::Wrench

• 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 等类进行转换或运算。

Twist and Wrench transformations

运动旋量和力旋量有三种不同类型的变换。

*      1) Frame * Twist or Frame * Wrench :
 *              this transforms both the velocity/force reference point
 *             and the basis to which the twist/wrench are expressed.
 *      2) Rotation * Twist or Rotation * Wrench :
 *              this transforms the basis to which the twist/wrench are
 *              expressed, but leaves the reference point intact.
 *      3) Twist.RefPoint(v_base_AB) or Wrench.RefPoint(v_base_AB)
 *              this transforms only the reference point. v is expressed
 *              in the same base as the twist/wrench and points from the
 *              old reference point to the new reference point.

• Warning:警告

• 效率可以通过写 p2 = A*(B*(C*p1)))而不是 p2=A*B*C*p1来提高

PROPOSED NAMING CONVENTION FOR FRAME-like OBJECTS

建议的坐标系类对象的命名约定

*      A naming convention of objects of the type defined in this file :
 *          (1) Frame : F...
 *              Rotation : R ...
 *          (2) Twist    : T ...
 *              Wrench   : W ...
 *              Vector   : V ...
 *      This prefix is followed by :
 *      for category (1) :
 *          F_A_B : w.r.t. frame A, frame B expressed
 *          ( each column of F_A_B corresponds to an axis of B,
 *            expressed w.r.t. frame A )
 *          in mathematical convention :
 *                   A
 *         F_A_B ==    F
 *                   B
 *
 *      for category (2) :
 *          V_B   : a vector expressed w.r.t. frame B
 *
 *      This can also be prepended by a name :
 *          e.g. : temporaryV_B
 *
 *      With this convention one can write :
 *
 *      F_A_B = F_B_A.Inverse();
 *      F_A_C = F_A_B * F_B_C;
 *      V_B   = F_B_C * V_C;    // both translation and rotation
 *      V_B   = R_B_C * V_C;    // only rotation
 *

CONVENTIONS FOR WHEN USED WITH ROBOTS :

用于机器人时的约定：

*       world : represents the frame ([1 0 0,0 1 0,0 0 1],[0 0 0]')
 *       mp    : represents mounting plate of a robot末端法兰 安装板
 *               (i.e. everything before MP is constructed by robot manufacturer
 *                    everything after MP is tool )
 *       tf    : represents task frame of a robot 任务坐标系
 *               (i.e. frame in which motion and force control is expressed)
 *       sf    : represents sensor frame of a robot 传感器坐标系
 *               (i.e. frame at which the forces measured by the force sensor
 *               are expressed )
 *
 *          Frame F_world_mp=...;
 *          Frame F_mp_sf(..)
 *          Frame F_mp_tf(,.)
 *     力矩是在传感器坐标系SF中测量的，所以可以写成：
 *          Wrench are measured in sensor frame SF, so one could write :
 *                Wrench_tf = F_mp_tf.Inverse()* ( F_mp_sf * Wrench_sf );
 *

Kinematic Families

• 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::Joint 和 KDL::Segment）和它们的互连的运动学结构（KDL::Chain，KDL::Tree 和 KDL::Graph），到针对特定运动学族的运动学和动力学的求解算法。

运动学族是一组具有相似属性的运动学结构，例如相同的连接拓扑，相同的数值或解析求解算法等。同一运动学族的不同成员只是在它们的运动学和动力学属性（连杆长度，质量等）的具体值上有所不同。

每个运动学结构都是由一个或多个段（KDL::Segment）构建的。KDL::Chain 是这些段的串联连接；KDL:Tree 是一个树形结构的连接；KDL:Graph 是一个具有一般图拓扑的运动学结构。（当前的实现只支持 KDL::Chain。）

一个 KDL::Segment 包含一个 KDL::Joint 和一个偏移坐标系（“连杆长度”，由一个 KDL::Frame 定义），它表示前一个段的 KDL::Joint 和它自己的 KDL::Joint 之间的几何姿态。

运动学族是指一组具有相似属性的运动学结构，例如相同的连接拓扑，相同的求解算法等。运动学结构是指由一个或多个段组成的刚体运动的模型，例如机器人的运动学模型。段是指由一个关节和一个偏移坐标系组成的刚体，表示两个关节之间的几何姿态。关节是指具有一个参数化的自由度和一个标量的动力学属性的简单的运动元件，例如旋转，平移，固定或无效。

运动学族的类包括基本的构建块（KDL::Joint 和 KDL::Segment），以及它们的互连的运动学结构（KDL::Chain，KDL::Tree 和 KDL::Graph），还有针对特定运动学族的运动学和动力学的求解算法。求解算法是指可以计算运动学结构的末端执行器或关节的位置，速度，加速度等的数值或解析方法，例如正向或逆向的运动学求解器。

运动学结构的类型可以是串联的（KDL::Chain），树形的（KDL::Tree）或图形的（KDL::Graph）。串联的运动学结构是指由多个段依次连接而成的线性结构，例如机械臂，机器人腿等。树形的运动学结构是指由多个段以树形的方式连接而成的分支结构，例如人形机器人，多足机器人等。图形的运动学结构是指由多个段以任意的方式连接而成的复杂结构，例如具有闭环的机器人。目前的实现只支持串联的运动学结构。

Motion

• 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：表示一个半梯形的速度曲线，即速度只有加速或减速的阶段，没有匀速的阶段。可以指定一个最大速度，一个最大加速度和一个总距离。

https://www.orocos.org/rtt/

https://docs.ros.org/en/indigo/api/orocos_kdl/html/index.html