def w_ikine_test():
##初始位置
qr = np.array([90, 40, 30, 80, 0, 30, 90]) * (pi / 180)
qr_init = np.array([91, 41, 31, 81, 1, 31, 91]) * (pi / 180)
Te = kin.fkine(theta0 + qr_init, alpha, a, d)
n = len(qr_init)
##臂形角参数化后求解逆运动学
d_h0 = np.zeros(n)
tt1 = time.clock()
[qq, dh] = kin.w_ikine(DH_0, qr, q_max, q_min, d_h0, Te)
tt2 = time.clock()
print "逆运动学时间：", tt2 - tt1
# print np.around(Q*180/pi, decimals=6)
##关节角选泽函数
# q = kin.analysis_ikine_chose(Q,qr_init)
print np.around(qq * 180 / pi, decimals=6)
print dh
# 正运动学求解解
T0_e = kin.fkine(theta0 + qq, alpha, a, d)
##测试结果：当臂形角等于pi时，关节角发生突变
###解决办法：臂形角取值范围改为【-pi,pi)
print T0_e - Te
def null_space_plan_test():
##初始位置
qr_init = np.array([0, 30, 0, 60, 0, 30, 0]) * (pi / 180)
Te = kin.fkine(theta0 + qr_init, alpha, a, d)
n = len(qr_init)
#获取零空间规划关节空间点
[qq, qv, qa] = pap.null_space_plan(qr_init, Te)
#时间变量/规划时间为38秒
k = len(qq[:, 0])
t = np.linspace(0, 38, k)
#MyPlot.plot2d(t,psi, "psi")
#关节空间单变量随时间绘图
for i in range(n):
i_string = "qq " + str(i + 1) + "plot"
MyPlot.plot2d(t, qq[:, i], i_string)
#正运动学求解末端位位置
Xe = np.zeros([k, 6])
for i in range(k):
T0_e = kin.fkine(theta0 + qq[i, :], alpha, a, d)
Xe[i, 0:3] = T0_e[0:3, 3]
Xe[i, 3:6] = T0_e[0:3, 2]
#笛卡尔单变量随时间绘图
for i in range(6):
i_string = "Xe " + str(i + 1) + "plot"
MyPlot.plot2d(t, Xe[:, i], i_string)
#末端轨迹三维图
xx = np.around(Xe[:, 0], decimals=6)
yy = np.around(Xe[:, 1], decimals=6)
zz = np.around(Xe[:, 2], decimals=6)
MyPlot.plot3d(xx, yy, zz, "3D plot")
def teach_line_plan_test():
##直线轨迹测试
l = 0.2
[qr_init, X0_e, T] = enp.multipointLine_armc(l)
n = len(qr_init)
#从家点运动到初始位置
[qq1, qv1, qa1] = jp.home_to_init(qr_init)
k1 = len(qq1[:, 0])
#停顿点
qq2 = jp.keep_pos(400, qq1[-1, :])
k2 = len(qq2[:, 0])
#位置级求逆函数测试测试
[qq3, qv3, qa3] = pap.multipoint_plan_position(qr_init, X0_e, T)
k3 = len(qq3[:, 0])
#合成完整路径
kk1 = k1
kk2 = k1 + k2
kk3 = k1 + k2 + k3
qq = np.zeros([kk3, n])
qq[0:kk1, :] = qq1
qq[kk1:kk2, :] = qq2
qq[kk2:kk3, :] = qq3
#关节点写入文档
parent_path = os.path.abspath('..')
file_name = "data/teaching/teaching_data.txt"
path = os.path.join(parent_path, file_name)
FileOpen.write(qq, path)
#时间变量
k = len(qq[:, 0])
n = len(qq[0, :])
t = np.linspace(0, T * (k - 1), k)
#关节空间单变量随时间绘图
for i in range(n):
i_string = "qq " + str(i + 1) + "plot"
MyPlot.plot2d(t, qq[:, i], i_string)
#正运动学求解末端位位置
Xe = np.zeros([k, 6])
for i in range(k):
T0_e = kin.fkine(theta0 + qq[i, :], alpha, a, d)
Xe[i, 0:3] = T0_e[0:3, 3]
Xe[i, 3:6] = T0_e[0:3, 1]
#笛卡尔单变量随时间绘图
for i in range(6):
i_string = "Xe " + str(i + 1) + "plot"
MyPlot.plot2d(t, Xe[:, i], i_string)
#末端轨迹三维图
xx = np.around(Xe[:, 0], decimals=6)
yy = np.around(Xe[:, 1], decimals=6)
zz = np.around(Xe[:, 2], decimals=6)
MyPlot.plot3d(xx, yy, zz, "3D plot")
def spline_test():
##生产曲线
nodeNum = 10
#x = np.linspace(0,pi,nodeNum)
t = np.linspace(0, 10, nodeNum)
x = pi / 10 * t
y = np.zeros(nodeNum)
for i in range(nodeNum):
y[i] = np.sin(x[i])
#求取加速度测试
#[m,h] = bf.spline_param(t,y,pi/10,-pi/10)
##测试结果：正确，但起点和终点速度设置，对加速度影响较大，甚至会出现震荡现象或使其失真，
## 解决：尽可能给定合适的起始和末端速度
#单变量加速度随时间绘图
#i_string = "m " + "plot"
#MyPlot.plot2d(t,m, i_string)
#三次样条曲线插值测试
[s, vel, acc] = bf.spline1(t, y, 0.1, 0, 0) #pi/10,-pi/10)
##测试结果：正确，但起点和终点速度设置，对加速度影响较大，甚至会出现震荡现象或使其失真，
## 解决：尽可能给定合适的起始和末端速度
kk = len(s)
tt = np.linspace(0, 10, kk)
#单变量位置随时间绘图
i_string = "s " + "plot"
MyPlot.plot2d(tt, s, i_string)
#单变量速度随时间绘图
i_string = "vel " + "plot"
MyPlot.plot2d(tt, vel, i_string)
#单变量加速度随时间绘图
i_string = "acc " + "plot"
MyPlot.plot2d(tt, acc, i_string)
#笛卡尔参数测试
for i in range(6):
i_string = "M " + str(i + 1) + "plot"
MyPlot.plot2d(tt, s[:, i], i_string)
#正运动学求解末端位位置
Xe = np.zeros([6, kk])
for i in range(kk):
T0_e = kin.fkine(theta0 + s[:, i], alpha, a, d)
Xe[0:3, i] = T0_e[0:3, 3]
Xe[3:6, i] = T0_e[0:3, 1]
#笛卡尔单变量随时间绘图
for i in range(6):
i_string = "Xe " + str(i + 1) + "plot"
MyPlot.plot2d(t, Xe[i, :], i_string)
#末端轨迹三维图
xx = np.around(Xe[0, :], decimals=6)
yy = np.around(Xe[1, :], decimals=6)
zz = np.around(Xe[2, :], decimals=6)
MyPlot.plot3d(xx, yy, zz, "3D plot")
def jaco_test():
##初始位置
DH_0 = rp.DH0_ur5
qr = np.array([100, 50, 40, 80, 100, 30]) * (pi / 180)
#采用构造发求雅克比矩阵
t1 = time.clock()
Jaco1 = kin.jacobian(DH_0, qr)
t2 = time.clock()
#采用迭代法求雅克比矩阵
Jaco2 = kin.jeco_0(DH_0, qr)
t3 = time.clock()
#信息显示
print "构造法求取雅克比：\n", np.around(Jaco1, 6)
print "构造法求取雅克比所需时间：", t2 - t1
print "迭代法求取雅克比：\n", np.around(Jaco2, 6)
print "迭代法求取雅克比所需时间：", t3 - t2
def analysis_ikine_test():
##初始位置
qk = np.array([170, 80, 170, 60, -170, 80, 160]) * (pi / 180)
qr_init = np.array([170, 80, -170, 80, -170, 80, 160]) * (pi / 180)
Te = kin.fkine(theta0 + qr_init, alpha, a, d)
##臂形角参数化后求解逆运动学
psi = pi
tt1 = time.clock()
#[qq, succeed_label] = kin.arm_angle_ikine(Te, psi, qk, DH_0, q_min, q_max)
[qq, succeed_label] = kin.arm_angle_ikine_limit(Te, qk, DH_0, q_min, q_max)
tt2 = time.clock()
print "第一种求解时间：", tt2 - tt1
print "第一种方法：\n", np.around(qq * 180 / pi, decimals=3)
#正运动学求解解
T0_e = kin.fkine(theta0 + qq, alpha, a, d)
print "关节角：\n", np.around(T0_e - Te, 6)
def multipoint_plan_test():
##圆轨迹测试
rc = 0.1
[qr_init, X0_e, T] = enp.circlePoint_armc(rc)
##直线轨迹测试
#l = 0.3
#[qr_init,X0_e,T] = enp.multipointLine_armc(l)
n = len(qr_init)
#速度级求逆函数测试测试(规定每行为一个数据点）
[qq, qv, qa] = pap.multipoint_plan_position(qr_init, X0_e, T)
#时间变量
k = len(qq[:, 0])
t = np.linspace(0, T * (k - 1) / 10.0, k)
#插值部分测试
#[s,vel,acc]=pap.spline1(tq,q[:,0],0.01,0,0)
#i_string = "s " + "plot"
#MyPlot.plot2d(t,s, i_string)
#关节空间单变量随时间绘图
for i in range(n):
i_string = "qq " + str(i + 1) + "plot"
MyPlot.plot2d(t, qq[:, i], i_string)
#正运动学求解末端位位置
Xe = np.zeros([k, 6])
for i in range(k):
T0_e = kin.fkine(theta0 + qq[i, :], alpha, a, d)
Xe[i, 0:3] = T0_e[0:3, 3]
Xe[i, 3:6] = T0_e[0:3, 2]
#笛卡尔单变量随时间绘图
for i in range(6):
i_string = "Xe " + str(i + 1) + "plot"
MyPlot.plot2d(t, Xe[:, i], i_string)
#末端轨迹三维图
xx = np.around(Xe[:, 0], decimals=6)
yy = np.around(Xe[:, 1], decimals=6)
zz = np.around(Xe[:, 2], decimals=6)
MyPlot.plot3d(xx, yy, zz, "3D plot")
def ur_ikine_test():
##初始位置
DH_0 = rp.DH0_ur5
qr = np.array([100, 50, 40, 80, 100, 30]) * (pi / 180)
qq_k = np.array([100, 50, 40, 80, 100, 30]) * (pi / 180)
n = 6
#正运动学求取末端位姿
kin1 = kin.GeneralKinematic(DH_0)
tt1 = time.clock()
Te = kin1.fkine(qr)
tt2 = time.clock()
#采用解析解求8组逆解
qr = kin1.ur_ikine(Te, qq_k)
tt3 = time.clock()
print np.around(qr * 180 / pi, decimals=6)
print "正解所需时间：", tt2 - tt1
print "逆解所需时间：", tt3 - tt2
T0_e = kin1.fkine(qr)
print "第", "组误差：", np.around(T0_e - Te, 7)
from robot_python import FileOpen
from robot_python import MyPlot
#DH参数
DH_0 = rp.DHfa_armc
qq_min = rp.q_min_armc
qq_max = rp.q_max_armc
#初始位置
qq_init = np.array([0, 30, 0, 90, 0, 60, 0]) * pi / 180.0
#期望力
f = np.array([0.0, 0.0, -20.0, 0.0, 0.0, 0.0])
#正运动学求取末端位置
X_init = kin.fkine_euler(DH_0, qq_init)
print X_init
#设计轨迹
X_end = X_init + np.array([0.15, 0, 0, 0, 0, 0])
#平滑轨迹
num = 1000
t = np.linspace(0, 1, num)
XX = np.zeros([num, 6])
F = np.zeros([num, 6])
for i in range(num):
XX[i, :] = X_init + (X_end - X_init) * np.sin(pi * t[i])
F[i, :] = f * np.sin(pi * t[i])
#笛卡尔单变量随时间绘图
#自定义函数模块 from robot_python import Kinematics as kin from robot_python import PathPlan as pap from robot_python import MyPlot from robot_python import EndPoint as enp from robot_python import RobotParameter as rp from robot_python import TeachingLearning as tl from robot_python import FileOpen #*****示教阶段******# DH_0 = rp.DH0_armc qq_path = "/home/d/catkin_ws/src/robot_python/data/teaching/teaching_data.txt" qq_demo = FileOpen.read(qq_path) #获取起始点 X0 = kin.fkine_euler(DH_0, qq_demo[0, :]) #获取目标点 X_goal = kin.fkine_euler(DH_0, qq_demo[0, :]) #时间系列 T = 0.01 num = len(qq_demo[:, ]) tt = np.linspace(0, T*(num - 1), num) #*****学习阶段******# #末端学习自由度 n_dof = 3 #文件路径 file_path = "/home/d/catkin_ws/src/robot_python/paramter/teaching_learn_param" dmps_path = file_path + "/dmps_param.txt" rbf_path = file_path + "/rbf_param.txt"
def line_plan_test():
##圆轨迹测试
#rc = 0.1
#[qr_init,X0_e,T] = enp.circlePoint_armc(rc)
##直线轨迹测试
l = 0.2
[qr_init, X0_e, T] = enp.multipointLine_armc(l)
n = len(qr_init)
#从家点运动到初始位置
[qq1, qv1, qa1] = jp.home_to_init(qr_init)
k1 = len(qq1[0, :])
#停顿点
qq2 = jp.keep_pos(400, qq1[:, -1])
k2 = len(qq2[0, :])
#位置级求逆函数测试测试
[qq3, qv3, qa3] = pap.multipoint_plan_position(qr_init, X0_e, T)
# [qq3, qv3, qa3] = pap.multipoint_plan_position_FGA(qr_init, X0_e, T)
k3 = len(qq3[0, :])
#停顿点
qq4 = jp.keep_pos(400, qq3[:, -1])
k4 = len(qq4[0, :])
#规划完毕，原路返回
k = 2 * (k1 + k2 + k3) + k4
qq = np.zeros([n, k])
#合成完整路径
kk1 = k1
kk2 = k1 + k2
kk3 = k1 + k2 + k3
kk4 = k1 + k2 + k3 + k4
kk5 = k1 + k2 + 2 * k3 + k4
kk6 = k1 + 2 * k2 + 2 * k3 + k4
kk7 = 2 * (k1 + k2 + k3) + k4
qq[:, 0:kk1] = qq1
qq[:, kk1:kk2] = qq2
qq[:, kk2:kk3] = qq3
qq[:, kk3:kk4] = qq4
qq[:, kk4:kk5] = qq3[:, ::-1]
qq[:, kk5:kk6] = qq2[:, ::-1]
qq[:, kk6:kk7] = qq1[:, ::-1]
#关节点写入文档
parent_path = os.path.abspath('..')
file_name = "data/position.txt"
path = os.path.join(parent_path, file_name)
FileOpen.write(qq.T, path)
#时间变量
k = len(qq[0, :])
n = len(qq[:, 0])
t = np.linspace(0, T * (k - 1), k)
#关节空间单变量随时间绘图
for i in range(n):
i_string = "qq " + str(i + 1) + "plot"
MyPlot.plot2d(t, qq[i, :], i_string)
#正运动学求解末端位位置
Xe = np.zeros([6, k])
for i in range(k):
T0_e = kin.fkine(theta0 + qq[:, i], alpha, a, d)
Xe[0:3, i] = T0_e[0:3, 3]
Xe[3:6, i] = T0_e[0:3, 1]
#笛卡尔单变量随时间绘图
for i in range(6):
i_string = "Xe " + str(i + 1) + "plot"
MyPlot.plot2d(t, Xe[i, :], i_string)
#末端轨迹三维图
xx = np.around(Xe[0, :], decimals=6)
yy = np.around(Xe[1, :], decimals=6)
zz = np.around(Xe[2, :], decimals=6)
MyPlot.plot3d(xx, yy, zz, "3D plot")
def null_space_plan_demo():
##初始位置
qr_init = np.array([0, 30, 0, 30, 0, 30, 0]) * (pi / 180)
Te = kin.fkine(theta0 + qr_init, alpha, a, d)
n = len(qr_init)
T = 0.1
#获取零空间规划关节空间点
[qq3, qv3, qa3] = pap.null_space_plan(qr_init, Te)
#时间变量/规划时间为38秒
k3 = len(qq3[:, 0])
#从家点运动到初始位置,此处10秒1001个点
[qq1, qv1, qa1] = jp.home_to_init(qq3[0, :])
k1 = len(qq1[:, 0])
#停顿点1，停顿时间4秒，398个点
qq2 = jp.keep_pos(398, qq1[-1, :])
k2 = len(qq2[:, 0])
#停顿点2,停顿4秒，398个点
qq4 = jp.keep_pos(398, qq3[-1, :])
k4 = len(qq4[:, 0])
#规划完毕，返回家点
[qq5, qv5, qa5] = jp.go_to_home(qq4[-1, :])
k5 = len(qq5[:, 0])
#合成完整路径
kk1 = k1
kk2 = k1 + k2
kk3 = kk2 + k3
kk4 = kk3 + k4
kk5 = kk4 + k5
k = kk5
qq = np.zeros([k, n])
qq[0:kk1, :] = qq1
qq[kk1:kk2, :] = qq2
qq[kk2:kk3, :] = qq3
qq[kk3:kk4, :] = qq4
qq[kk4:kk5, :] = qq5
#关节点写入文档
parent_path = os.path.abspath('..')
#file_name = "data/null_position.txt"
file_name = "data/position.txt"
path = os.path.join(parent_path, file_name)
FileOpen.write(qq, path)
#时间变量
k = len(qq[:, 0])
n = len(qq[0, :])
t = np.linspace(0, T * (k - 1), k)
#关节空间单变量随时间绘图
for i in range(n):
i_string = "qq " + str(i + 1) + "plot"
MyPlot.plot2d(t, qq[:, i], i_string)
#正运动学求解末端位位置
Xe = np.zeros([k, 6])
for i in range(k):
T0_e = kin.fkine(theta0 + qq[i, :], alpha, a, d)
Xe[i, 0:3] = T0_e[0:3, 3]
Xe[i, 3:6] = T0_e[0:3, 2]
#笛卡尔单变量随时间绘图
for i in range(6):
i_string = "Xe " + str(i + 1) + "plot"
MyPlot.plot2d(t, Xe[:, i], i_string)
#末端轨迹三维图
xx = np.around(Xe[:, 0], decimals=6)
yy = np.around(Xe[:, 1], decimals=6)
zz = np.around(Xe[:, 2], decimals=6)
MyPlot.plot3d(xx, yy, zz, "3D plot")
def circle_plan_demo():
##圆轨迹测试
rc = 0.05
[qr_init, X0_e, T] = enp.circlePoint_armc(rc)
n = len(qr_init)
#位置级求逆获取圆的轨迹,3801个点38秒
time1 = time.clock()
#[qq3,qv3,qa3] = pap.multipoint_plan_position(qr_init,X0_e,T)
[qq3, qv3, qa3] = pap.multipoint_plan_position_aa(qr_init, X0_e, T)
#[qq3, qv3, qa3,Psi] = pap.multipoint_plan_position_FGA(qr_init, X0_e, T,)
time2 = time.clock()
print "plan one time need : %s s" % (time2 - time1)
#psi_t = np.linspace(0, 38, len(Psi))
#MyPlot.plot2d(psi_t, Psi, "Psi plot!")
k3 = len(qq3[:, 0])
#从家点运动到初始位置,此处10秒1001个点
[qq1, qv1, qa1] = jp.home_to_init(qq3[0, :])
k1 = len(qq1[:, 0])
#停顿点1，停顿时间4秒，398个点
qq2 = jp.keep_pos(398, qq1[-1, :])
k2 = len(qq2[:, 0])
#停顿点2,停顿4秒，398个点
qq4 = jp.keep_pos(398, qq3[-1, :])
k4 = len(qq4[:, 0])
#规划完毕，返回家点
[qq5, qv5, qa5] = jp.go_to_home(qq4[-1, :])
k5 = len(qq5[:, 0])
#合成完整路径
kk1 = k1
kk2 = k1 + k2
kk3 = kk2 + k3
kk4 = kk3 + k4
kk5 = kk4 + k5
k = kk5
qq = np.zeros([k, n])
qq[0:kk1, :] = qq1
qq[kk1:kk2, :] = qq2
qq[kk2:kk3, :] = qq3
qq[kk3:kk4, :] = qq4
qq[kk4:kk5, :] = qq5
#关节点写入文档
parent_path = os.path.abspath('..')
#file_name = "data/circular_position.txt"
file_name = "data/circular_position.txt"
path = os.path.join(parent_path, file_name)
FileOpen.write(qq, path)
#时间变量
k = len(qq[:, 0])
n = len(qq[0, :])
t = np.linspace(0, T * (k - 1), k)
#关节空间单变量随时间绘图
for i in range(n):
i_string = "qq " + str(i + 1) + "plot"
MyPlot.plot2d(t, qq[:, i], i_string)
#正运动学求解末端位位置
Xe = np.zeros([k, 6])
for i in range(k):
T0_e = kin.fkine(theta0 + qq[i, :], alpha, a, d)
Xe[i, 0:3] = T0_e[0:3, 3]
Xe[i, 3:6] = T0_e[0:3, 2]
#笛卡尔单变量随时间绘图
for i in range(6):
i_string = "Xe " + str(i + 1) + "plot"
MyPlot.plot2d(t, Xe[:, i], i_string)
#末端轨迹三维图
xx = np.around(Xe[:, 0], decimals=6)
yy = np.around(Xe[:, 1], decimals=6)
zz = np.around(Xe[:, 2], decimals=6)
MyPlot.plot3d(xx, yy, zz, "3D plot")