KDL运动学动力学库类结构、接口整理

基本数据类型

• Vector：double[3]，3维向量
• Rotation：double[9]，3*3矩阵
• Frame：Vector+Rotation，变换矩阵
• Twist：Vector(平移速度)+Vector(旋转速度)
• Wrench：Vector(力)+Vector(转矩)

Kinematic Trees运动学结构数据类型

Joint关节

Joint = JointType + Scale(输入输出比) + Offset(reference->joint)+非必要（Inertia惯量+Damping阻尼+Stiffness刚度）
JointType = {RotAxis,RotX,RotY,RotZ,TransAxis,TransX,TransY,TransZ,None}

RigidBodyInertia刚体惯量

RigidBodyInertia = 质量+重力矩V3+转动惯量M3*3

Segment杆件

Segment = Joint + RigidBodyInertia + Frame_reference + Frame_tip

Chain无分支运动链

Chain = Vector(Segment)

算法

基类

• SolverI：solver interface用来存储和描述最近一次错误

正运动学

• ChainFkSolverPos继承自SolverI：Chain的正运动学接口
• ChainFkSolverPos_recursive继承自ChainFkSolverPos：递归fk
• ChainFkSolverVel_recursive继承自ChainFkSolverPos：递归fk-vel

例：ChainFkSolverPos_recursive接口

• 构造函数ChainFkSolverPos_recursive(const Chain& chain)
• 接口virtual int JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr=-1)、virtual int JntToCart(const JntArray& q_in, std::vector<Frame>& p_out, int segmentNr=-1);

逆运动学

• ChainIkSolverPos继承自SolverI：Chain的逆运动学接口
• ChainIkSolverPos_LMA继承自ChainIkSolverPos：逆运动学——莱文贝格-马夸特，Levenberg-Marquardt
• ChainIkSolverPos_NR：逆运动学——牛顿-拉夫森，Newton-Raphson
• ChainIkSolverPos_NR_JL：逆运动学——牛顿-拉夫森带关节限位，Newton-Raphson with joint limits
• ChainIkSolverVel_pinv继承自ChainIkSolverPos：ik-vel——广义伪逆，generalize pseudo inverse
• ChainIkSolverVel_pinv_nso：ik-vel——广义伪逆（冗余机器人优化）
• ChainIkSolverVel_pinv_givens：ik-vel——广义伪逆（Givens）
• ChainIkSolverVel_wdls：ik-vel——加权伪逆（阻尼最小二乘）

例：ChainIkSolverPos_LMA接口

• 构造函数

ChainIkSolverPos_LMA(
    		const KDL::Chain& _chain,
    		const Eigen::Matrix<double,6,1>& _L,
    		double _eps=1E-5,
    		int _maxiter=500,
    		double _eps_joints=1E-15);
    		
ChainIkSolverPos_LMA(
    		const KDL::Chain& _chain,
    		double _eps=1E-5,
    		int _maxiter=500,
    		double _eps_joints=1E-15);

• 接口

virtual int CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& T_base_goal, KDL::JntArray& q_out);