PyKDL程序

作为测试的例子吧

PyKDL官方文档很差劲，写的不清晰，建议参考

KDL c++官方文档

#coding=utf-8
import numpy as np
import PyKDL as kdl
# import copy #用于深度拷贝

# 注意一些基本概念
# Segment初始化由绕的轴和末端姿态来生成，可能跟书上的D-H方法有点不一样，区别在于用末端和轴来定义一个关节
#   轴是在当前关节的坐标系中绕轴V旋转，默认设置为kdl.Joint.RotZ
#   f_tip，末端在当前坐标系中的姿态

jnts = []
frms = []

for i in range(6):
    jnts.append(kdl.Joint(kdl.Joint.RotZ))

# 注意，这个框架是f_tip，末端
frm1 = kdl.Frame(kdl.Rotation.RotX(-np.pi/2), kdl.Vector(0, 0, 239.5))
frm2 = kdl.Frame(kdl.Rotation.RotX(0), kdl.Vector(250, 0, 0))
frm3 = kdl.Frame(kdl.Rotation.RotX(np.pi/2), kdl.Vector(0, 262, 0))
frm4 = kdl.Frame(kdl.Rotation.RotX(-np.pi/2), kdl.Vector(0, 0, 0))
fr