上一篇主要講述了機器人學中用到的幾何基礎知識，以及在KDL中的實現和使用。本篇將更加深入的涉及到機器人學中的內容，主要是如何表示一個串聯型機械臂，以及KDL中的實現方式。
KDL中使用了兩個使用廣泛且技術成熟的機器人作爲例子：puma560（六自由度）和kukaLWR（七自由度）。本篇主要使用六自由度的puma560作爲例子。閱讀本篇需要有一定的機器人學常識，至少要讀過《機器人學導論》的前三章。
在KDL中創建puma560機器人很簡單，只需要調用一個函數就可以了。

Chain p560=Puma560();

要想知道這個函數裏面做了什麼，首先有三個KDL裏的概念要清楚：Joint，Segment，Chain。

1. KDL::Joint

A Joint allows translation or rotation in one degree of freedom between two Segments

Joint（關節）可以使兩個Segments之間有一個自由度的旋轉或平移。

1.1 創建一個Joint

首先，Joint類裏面定義了一個Joint的類型（平移、旋轉、None）：

typedef enum { RotAxis,RotX,RotY,RotZ,TransAxis,TransX,TransY,TransZ,None} JointType;

創建Joint的函數有幾個，先看第一個：

explicit Joint(const std::string& name, const JointType& type=None,const double& scale=1,const double& offset=0,
              const double& inertia=0,const double& damping=0,const double& stiffness=0);

各個參數的含義如下：
name: Joint的名字；
type: Joint的類型，旋轉、平移還是None；
scale: the scale between joint input and actual geometric movement, default: 1；類似減速比的概念，在旋轉關節中常用；
offset: offset between joint input and actual geometric input, default: 0；關節輸入與輸出之間的偏移，在平移關節中常用；
inertia: 沿關節軸向的慣性，默認：0；
damping: 沿關節軸向的阻尼，默認：0；
stiffness: 沿關節軸向的剛度，默認：0；
再看第二個創建Joint的函數：

explicit Joint(const JointType& type=None,const double& scale=1,const double& offset=0,
               const double& inertia=0,const double& damping=0,const double& stiffness=0);

與上個函數一樣，省略了name參數，name默認是“NoName”。
再看第三個函數：

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);

與第一個函數一樣，不同在於多了兩個參數：
_origin和_axis，這兩個參數的意思是，可以在任意原點沿任意軸線建立Joint。這種情況下type只能是RotAxis或者TransAxis。

joint_pose.p = origin;
joint_pose.M = Rotation::Rot2(axis, offset);

在函數的實現中多了這兩個操作，第一步是記錄原點位置到joint_pose.p，第二步是將joint_pose.M繞axis軸旋轉offset角度後返回旋轉矩陣。
在看另外一個函數：

Joint(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);

與上一個函數一樣，省略了name參數，name默認是“NoName”。

1.2 獲取Joint特徵

首先是獲取Joint位姿的函數：

Frame pose(const double& q)const;

Request the 6D-pose between the beginning and the end of the joint at joint position q.

當關節位置爲q時，獲取關節位姿。
當type=RotAxis時：

joint_pose.M = Rotation::Rot2(axis, scale*q+offset);
return joint_pose;

當type=RotX時：

return Frame(Rotation::RotX(scale*q+offset));

當type=TransAxis時：

return Frame(origin + (axis * (scale*q+offset)));

當type=TransX時：

return  Frame(Vector(scale*q+offset,0.0,0.0));

其次是獲取Joint速度的函數：

Twist twist(const double& qdot)const;

Request the resulting 6D-velocity with a joint velocity qdot

當關節速度爲qdot時，Joint的速度向量。
當type=RotAxis時：

return Twist(Vector(0,0,0), axis * ( scale * qdot));

當type=RotX時：

return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0));

當type=TransAxis時：

return Twist(axis * (scale * qdot), Vector(0,0,0));

當type=TransX時：

return Twist(Vector(scale*qdot,0.0,0.0),Vector(0.0,0.0,0.0));

然後是獲取關節軸的向量函數：

Vector JointAxis() const;

Request the Vector corresponding to the axis of a revolute joint.

獲取對應關節軸的向量。
例如，當type=RotX時：

return Vector(1.,0.,0.);

當type=TransY時：

return Vector(0.,1.,0.);

最後就是一些獲取Joint參數的函數：

Vector Joint::JointOrigin() const
{
  return origin;
}
const std::string& getName()const
{
    return name;
}
const JointType& getType() const
{
    return type;
};
const std::string getTypeName() const;

2. KDL::Segment

this class encapsulates a simple segment, that is a “rigid body” (i.e., a frame and a rigid body inertia) with a joint and with “handles”, root and tip to connect to other segments. A simple segment is described by the following properties :

Joint

Rigid Body Inertia: of the rigid body part of the Segment

Offset from the end of the joint to the tip of the segment: the joint is located at the root of the segment.

Segment類有幾個私有變量：

std::string name;
Joint joint;
RigidBodyInertia I;
Frame f_tip;

name: Segment的名字；
joint: Segment的關節；
I: Segment剛體的慣量；
f_tip: Segment末端固連的一個座標系；

2.1 一個例子

先看一個創建Segment的例子，從中可以看到Segment的組織結構。這個例子是創建Puma560基座關節的Segment。

Segment(Joint(Joint::RotZ),Frame::DH(0.4318,0.0,0.0,0.0),
RigidBodyInertia(17.4,Vector(-.3638,.006,.2275),RotationalInertia(0.13,0.524,0.539,0,0,0)));

這段代碼創建的Segment如下圖所示：

其中root和tip的座標系是由Segment的DH決定的。

2.1.1 Joint

可以看到這個Segment有一個繞Z軸的旋轉關節。這個旋轉關節上固連的座標系就叫root座標系，而Segment的末端固連的座標系叫做tip座標系。當Joint旋轉一個角度的時候，root座標系不變而tip座標系因爲固連在Segment上，所以要改變。當Joint角度爲30°時的座標系如上圖虛線所示。

2.1.2 Frame

這裏的Frame是用DH參數來構建的。我們知道機器人座標系確定了之後，DH參數也唯一確定，所以用DH參數來表示Segment的座標系沒有問題。

2.1.3 Inertia

這裏RigidBodyInertia函數裏的三個參數分別表示質量、質心位置和轉動慣量。

2.2 Segment的構造函數

常用的Segment構造函數有兩種：

explicit Segment(const std::string& name, const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero());

explicit Segment(const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero());

兩個構造函數基本一樣。都是指定Segment的幾個變量，即名字、關節、座標系和慣量。

除此之外，Segment還設有複製和賦值構造函數：

Segment(const Segment& in);
Segment& operator=(const Segment& arg);

2.3 Segment的其他函數

Frame pose(const double& q)const;

當joint爲q時，獲得tip座標系相對於root座標系的位姿矩陣；例如在Segment圖中虛線所示，joint=30°，tip相對於root的位置：p=[0.3739, 0.2159, 0]，tip相對於root的旋轉：
$M=\begin{bmatrix} 0.866 & -0.5 & 0\\ 0.5 & 0.866 & 0\\ 0 & 0 & 1 \end{bmatrix}$

Twist twist(const double& q,const double& qdot)const;

當joint爲q、joint的速度爲qdot時，獲取tip座標系相對於root座標系的平移和旋轉速度向量，其中以tip座標系的原點作爲參考點。例如在Segment圖中虛線，q=30°，qdot=0.1°/s，那麼twist=[-0.02159, 0.03739, 0, 0, 0, 0.1]。

const std::string& getName()const
{
    return name;
}
const Joint& getJoint()const
{
    return joint;
}
const RigidBodyInertia& getInertia()const
{
    return I;
}

這三個函數分別是獲取Segment的name、joint和慣量I。

void setInertia(const RigidBodyInertia& Iin)
{
    this->I=Iin;
}

設置Segment的慣量。

Frame getFrameToTip()const
{
    return joint.pose(0)*f_tip;
}

獲取當joint=0時，tip座標系相對於root座標系的位姿；例如在Segment圖中實線所示，joint=0°，tip相對於root的位置：p=[0.4318, 0, 0]，tip相對於root的旋轉：
$M=\begin{bmatrix} 1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & 1 \end{bmatrix}$

3. KDL::Chain

簡單的說就是將幾個Segment連接起來就形成了Chain。
Chain類裏面主要有這幾個變量：

private:
	unsigned int nrOfJoints;
        unsigned int nrOfSegments;
public:
        std::vector<Segment> segments;

nrOfJoints表示Joint的個數，nrOfSegments表示Segment的個數；segments中記錄了所有的Segments。

3.1 Chain的構造函數

Chain();
Chain(const Chain& in);
Chain& operator = (const Chain& arg);

其中，Chain()表示構造一個空的Chain。後面兩個都是複製構造函數。

3.2 其他函數

3.2.1 在Chain中增加Segment

void addSegment(const Segment& segment);

這個函數首先將新增的這個segment push到Chain::segments這個vector中，然後nrOfSegments++；如果這個segment包含一個Joint，即這個segment的Joint類型不爲None，就讓nrOfJoints++。

3.2.2 在Chain中增加另一個Chain

void addChain(const Chain& chain);

將新的chain直接添加到當前chain的後面。

3.2.3 get函數

unsigned int getNrOfJoints()const {return nrOfJoints;};

獲取Chain的Joint個數；

unsigned int getNrOfSegments()const {return nrOfSegments;};

獲取Chain的Segment個數；
這裏需要注意的是Joint的個數不一定與Segment的個數相同，因爲一個Segment不一定會有一個Joint。

const Segment& getSegment(unsigned int nr)const;

獲取第nr個Segment，nr從0開始，注意這個函數沒有邊界檢查，nr的值不要越界。

4. puma560

最後，在KDL中給出了建立一個完整puma560的Chain的例程：

namespace KDL{
    Chain Puma560(){
        Chain puma560;
        puma560.addSegment(Segment(Joint(Joint::RotZ),
                                   Frame::DH(0.0,M_PI_2,0.0,0.0),
                                   RigidBodyInertia(0,Vector::Zero(),RotationalInertia(0,0.35,0,0,0,0))));
        puma560.addSegment(Segment(Joint(Joint::RotZ),
                                   Frame::DH(0.4318,0.0,0.0,0.0),
                                   RigidBodyInertia(17.4,Vector(-.3638,.006,.2275),RotationalInertia(0.13,0.524,0.539,0,0,0))));
        puma560.addSegment(Segment(Joint(Joint::RotZ),
                                   Frame::DH(0.0203,-M_PI_2,0.15005,0.0),
                                   RigidBodyInertia(4.8,Vector(-.0203,-.0141,.070),RotationalInertia(0.066,0.086,0.0125,0,0,0))));
        puma560.addSegment(Segment(Joint(Joint::RotZ),
                                   Frame::DH(0.0,M_PI_2,0.4318,0.0),
                                   RigidBodyInertia(0.82,Vector(0,.019,0),RotationalInertia(1.8e-3,1.3e-3,1.8e-3,0,0,0))));
        puma560.addSegment(Segment(Joint(Joint::RotZ),
                                   Frame::DH(0.0,-M_PI_2,0.0,0.0),
                                   RigidBodyInertia(0.34,Vector::Zero(),RotationalInertia(.3e-3,.4e-3,.3e-3,0,0,0))));
        puma560.addSegment(Segment(Joint(Joint::RotZ),
                                   Frame::DH(0.0,0.0,0.0,0.0),
                                   RigidBodyInertia(0.09,Vector(0,0,.032),RotationalInertia(.15e-3,0.15e-3,.04e-3,0,0,0))));
        return puma560;
    }
}

其中，DH參數、質量、質心位置和轉動慣量等參數與Matlab機器人工具箱中的一模一樣。任何一種構型的串聯機械臂，只要按照《機器人建模和控制》或《Robotics,Vision and Control - Fundamental Algorithms in MATLAB》（這兩本書的DH建立方式與KDL中相同）建立各關節的座標系，並確定DH參數、質量、質心位置和轉動慣量等參數後，就能在KDL中建立該機械臂的運動鏈。

本篇主要講述了KDL中運動鏈的建立方式，以及與其相關的段（Segment）和關節（Joint）的概念，這些是串聯機械臂運動學的基礎。下一篇講述KDL中正運動學解的實現方式及其使用。

開源機器人庫orocos KDL 學習筆記三：Kinematric Chain

1. KDL::Joint

1.1 創建一個Joint

1.2 獲取Joint特徵

2. KDL::Segment

2.1 一個例子

2.1.1 Joint

2.1.2 Frame

2.1.3 Inertia

2.2 Segment的構造函數

2.3 Segment的其他函數

3. KDL::Chain

3.1 Chain的構造函數

3.2 其他函數

3.2.1 在Chain中增加Segment

3.2.2 在Chain中增加另一個Chain

3.2.3 get函數

4. puma560

自學編程兩個月，現在我月入 4 萬元

「實戰應用」如何用圖表控件LightningChart創建2D氣泡圖

百度安全多篇議題入選Blackhat Asia以硬技術發現“芯”問題

Google Chrome驅動程序 124.0.6367.62（正式版本）去哪下載？

慣性測量單元IMU基礎

下肢康復訓練外骨骼機器人Lokomat核心技術簡介

三相異步電機基於模型的效率估計算法

UR機器人之二：直線插補詳解

單輸入單輸出離散時間MPC解析

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結