上一篇主要講述了機器人學中用到的幾何基礎知識，以及在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.