def force_plan_armc_stiff():
# 获取机器人参数
DH_0 = rp.DHfa_armc
qq_max = rp.q_max_armc
qq_min = rp.q_min_armc
kin1 = kin.GeneralKinematic(DH_0)
#规划起点和终点
Xb = np.array([0.454, 0, 0.013, 0, 3.14, 0])
Tb = np.eye(4)
Tb[0:3, 3] = Xb[0:3]
Tb[0:3, 0:3] = bf.euler_zyx2rot(Xb[3:6])
#获取起点关节角猜测值
qq_guess = np.array([0, 60, 0, 60, 0, 60, 0])*np.pi/180.0
#获取关节角
qd = kin1.iterate_ikine(qq_guess, Tb)
print "qd:", np.round(qd*180/np.pi, 2)
#时间和周期
T = 0.01
#期望力规划：在工具坐标中规划
num = 6500
t = np.linspace(0, T*(num-1), num)
Fd = np.zeros([num, 6])
#z轴力变换
for i in range(num):
if(i>num/2):
j = num - i
else:
j = i
if(j<1000):
Fd[i, 2] = -5
else:
Fd[i, 2] = (j/500)*(-5)
#求取关节角度
qq = np.zeros([num, 7])
for i in range(num):
qq[i, :] = qd
#绘制关节角
MyPlot.plot2_nd(t, Fd, title="Fd", lable='fd')
MyPlot.plot2_nd(t, qq, title="qd", lable='qd')
# 写入文件
parent_path = os.path.join(os.getcwd(), '../..', 'data/impedance')
parent_path = os.path.abspath(parent_path)
pos_path = parent_path + "/armc_stiff_position1.txt"
force_path = parent_path + "/armc_stiff_force1.txt"
FileOpen.write(qq, pos_path)
FileOpen.write(Fd, force_path)
return qq
def line_force_plan_armt():
#建立规划类
linePlan = PathPlan.LinePlan()
# 获取机器人参数
DH_0 = rp.DHf_armt
qq_max = rp.q_max_armt
qq_min = rp.q_min_armt
linePlan.get_robot_parameter(DH_0, qq_min, qq_max)
#规划起点和终点
Xb = np.array([0.42, 0, 0.020, 0, 3.14, 0])
Xe = Xb + np.array([0.14, 0, 0, 0, 0, 0])
linePlan.get_begin_end_point(Xb, Xe)
#获取起点关节角猜测值
qq_guess = np.array([0, 46, 0, 90, 0, 42, 0])*np.pi/180.0
linePlan.get_init_guess_joint(qq_guess)
#时间和周期
T = 0.01
linePlan.get_period(T)
linePlan.get_plan_time(40)
#求取关节角度
qq = linePlan.out_joint()
#合成位置曲线
num_init = 501
num_pos = len(qq)
num = num_init + num_pos
t = np.linspace(0, T * (num - 1), num)
qd = np.zeros([num, 7])
for i in range(num_init):
qd[i, :] = qq[0, :]
qd[num_init:] = qq
#生成受力曲线
fd = -5
Fd = np.zeros([num, 6])
[Fd[:num_init, 2], _, _] = bf.interp5rdPoly1(0.0, 0.0, 0.0, fd, 0.0, 0.0, 5, 0.01)
for i in range(num_pos):
Fd[num_init + i, 2] = -5
# 绘制关节角
MyPlot.plot2_nd(t, Fd, title="Fd", lable='fd')
MyPlot.plot2_nd(t, qd, title="qd", lable='qd')
# 写入文件
parent_path = os.path.join(os.getcwd(), '../..', 'data/impedance')
parent_path = os.path.abspath(parent_path)
pos_path = parent_path + "/armt_line_position3.txt"
force_path = parent_path + "/armt_line_force3.txt"
FileOpen.write(qd, pos_path)
FileOpen.write(Fd, force_path)
return qq
def ur5s_move_object_plan_hand():
Xb = rp.board_X
Xe = Xb + np.array([0.0, 0.35, 0.250, 0, 0, 0])
T = 0.01
t = 30
#建立多臂规划类
robots1 = Robots.RobotsHandMoveObject()
#获取DH参数
robots1.get_robot1_paramter(rp.DH0_ur5, rp.q_min_ur5, rp.q_max_ur5)
robots1.get_robot2_paramter(rp.DH0_ur5, rp.q_min_ur5, rp.q_max_ur5)
#获取基座到世界坐标系参数
robots1.get_robots_base_to_world(rp.robot1_base_T, rp.robot2_base_T)
#加入手抓
robots1.get_grasp_end(rp.T_grasp)
#获取抓取点相对于工件坐标系
T_o_t1 = np.eye(4)
T_o_t2 = np.eye(4)
T_o_t1[0:3, 0:3] = np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0.0]])
T_o_t1[0:3, 3] = np.array([-0.1, 0.0, 0.044])
T_o_t2[0:3, 0:3] = np.array([[0, 0, -1], [1, 0, 0], [0, -1, 0.0]])
T_o_t2[0:3, 3] = np.array([0.1, 0.0, 0.044])
robots1.get_robots_tool_to_object(T_o_t1, T_o_t2)
#输入准备长度
l = 0.10
ready_num = 400
robots1.get_ready_distance(l, ready_num)
#输入工件规划点
Xe_list = workpiece_moving_track_line(Xb, Xe, T, t)
robots1.get_object_plan_list_zyx(Xe_list)
#获取关节角
qq1_guess = np.array([-90, -120, -120, 60, 90, 0]) * np.pi / 180.0
qq2_guess = np.array([-90, -60, 120, 120, -90, 0]) * np.pi / 180.0
[qq1, qq2] = robots1.put_robots_joint_position(qq1_guess, qq2_guess)
# 绘制关节角
num12 = len(qq1)
t12 = np.linspace(0, T * (num12 - 1), num12)
MyPlot.plot2_nd(t12, qq1, title="qq1")
MyPlot.plot2_nd(t12, qq2, title="qq2")
# 写入文件
parent_path = os.path.join(os.getcwd(), '../..')
parent_path = os.path.abspath(parent_path)
file_name1 = "data/robots/robot1_hand_move_position.txt"
path1 = os.path.join(parent_path, file_name1)
FileOpen.write(qq1, path1)
# 写入文件
file_name2 = "data/robots/robot2_hand_move_position.txt"
path2 = os.path.join(parent_path, file_name2)
FileOpen.write(qq2, path2)
return [qq1, qq2]
def line_plan():
#建立规划类
linePlan = PathPlan.LinePlan()
# 获取机器人参数
DH_0 = rp.DHf_armt
qq_max = rp.q_max_armc
qq_min = rp.q_min_armc
linePlan.get_robot_parameter(DH_0, qq_max, qq_min)
#规划起点和终点
Xb = np.array([0.43, 0, 0.013, 0, 3.14, 0])
Xe = Xb + np.array([0.15, 0, 0, 0, 0, 0])
linePlan.get_begin_end_point(Xb, Xe)
#获取起点关节角猜测值
qq_guess = np.array([0, 60, 0, 60, 0, 60, 0])*np.pi/180.0
linePlan.get_init_guess_joint(qq_guess)
#获取姿态角
R = np.array([[0, 0, 0],
[0, 0, 0],
[0, 0, 0]])
#时间和周期
T = 0.01
linePlan.get_period(T)
linePlan.get_plan_time(30)
#求取关节角度
qq = linePlan.out_joint()
#绘制关节角
num = len(qq)
t = np.linspace(0, T * (num - 1), num)
MyPlot.plot2_nd(t, qq, title="qq", lable='qq')
# 写入文件
parent_path = os.path.join(os.getcwd(), '../..', 'data/impedance')
parent_path = os.path.abspath(parent_path)
file_name = "armt_line_position.txt"
path = os.path.join(parent_path, file_name)
FileOpen.write(qq, path)
return qq
def end_point_plan_test():
##圆轨迹测试
rc = 0.1
[qr_init, X0_e, T] = enp.circlePoint_armc(rc)
print len(X0_e[:, 1])
##直线轨迹测试
#l = 100
#[qr_init,X0_e,T] = enp.multipointLine_armc(l)
#时间变量
k = len(X0_e[:, 0])
t = np.linspace(0, T * (k - 1), k)
#单变量随时间变化绘图
for i in range(6):
i_string = "Xe " + str(i + 1) + "plot"
MyPlot.plot2d(t, X0_e[:, i], i_string)
#末端轨迹三维图
MyPlot.plot3d(X0_e[:, 0], X0_e[:, 1], X0_e[:, 2], "3D plot")
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 single_joint_test():
##初始位置
qr_init = np.array([0, 0, 0, 80, 0, 0, 0]) * (pi / 180)
n = len(qr_init)
T = 0.01
#从家点运动到初始位置,此处10秒1001个点
[qq1, qv1, qa1] = jp.home_to_init(qr_init, T)
k1 = len(qq1[:, 0])
#规划完毕，返回家点
[qq2, qv2, qa2] = jp.go_to_home(qq1[-1, :], T)
k2 = len(qq2[:, 0])
#合成完整路径
kk1 = k1
kk2 = k1 + k2
k = kk2
qq = np.zeros([k, n])
qq[0:kk1, :] = qq1
qq[kk1:kk2, :] = qq2
#关节点写入文档
parent_path = os.path.abspath('..')
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)
def __init__(self, Multiplot=False):
'''
Constructor
'''
#alarm level , if temperature goes below this value we send an email.
self.lowtemp = 36. # below 38 we will send an alarm
self.email = '*****@*****.**' #adress for warning
self.counter = 0 # this counter makes sure that we don't get messages every time.
# we do it only every 50 times
self.Multiplot = Multiplot
#initalize the plotting
if (Multiplot):
self.MMPL = MMP.MyMultiPlot([40., 0., 755., 900., 0.],
[105., 50., 795., 1100., 50.], 5)
self.MMPL.SetAxisLabels('Time', [
'Temperature [F]', 'Humidity [%]', 'Pressure [hPa]',
'Barometric P [hPa]', 'Dew [F]'
])
else:
self.MPL = MP.MyPlot(ymin=0., ymax=100.)
self.MPL.SetAxisLabels('Time', 'Temperature')
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")
k = k + 1
'''
with open('data/PrePos_line3.txt', 'a') as file_to_write:
for col in range(4):
for row in range(3):
file_to_write.write(str(T1[row,col]))
file_to_write.write(' ')
file_to_write.write('\n')
'''
data3 = FileOpen.read('data/pentagram_pos.txt')
data3 = np.array(data3)
print data3.shape
print x1.shape
MyPlot.plot3d(x1 * 1000, y1 * 1000, z1 * 1000, 'genetics_line', 'x/mm', 'y/mm',
'z/mm')
MyPlot.plot3d(x2 * 1000, y2 * 1000, z2 * 1000, 'wish_line', 'x/mm', 'y/mm',
'z/mm')
MyPlot.plot3Point(x2 * 1000, y2 * 1000, z2 * 1000, x1 * 1000, y1 * 1000,
z1 * 1000, 'genetic_wish_Circle', 'x/mm', 'y/mm', 'z/mm')
MyPlot.plot2r(
x2 * 1000,
x1 * 1000,
'x_genetic_wish_line',
'k/point',
'value/mm',
)
MyPlot.plot2r(
y2 * 1000,
y1 * 1000,
def rbf_test1():
# 获取目标路径
current_path = os.getcwd()
file_path = os.path.join(current_path, "../..", "data/teaching")
file_path = os.path.abspath(file_path)
# *****示教阶段******#
# 读取数据
T = 0.01
num = 2000
tt = np.linspace(0, T*(num - 1), num)
a = 1
Xx_demo = np.zeros([num, 2])
Xv_demo = np.zeros([num, 2])
Xa_demo = np.zeros([num, 2])
Xx_demo[:, 0] = a*np.sin(np.pi/20*tt)
Xv_demo[:, 0] = a * np.cos(np.pi / 20 * tt)*np.pi / 20
Xa_demo[:, 0] = -a * np.sin(np.pi / 20 * tt) * (np.pi / 20)**2
Xx_demo[:, 1] = a * np.sin(np.pi / 20 * tt) + a*0.1*np.cos(np.pi/4*tt)
Xv_demo[:, 1] = a * np.cos(np.pi / 20 * tt) * np.pi / 20 - a*0.1*np.sin(np.pi/4*tt)*(np.pi/4)
Xa_demo[:, 1] = -a * np.sin(np.pi / 20 * tt) * (np.pi / 20) ** 2 + a*0.1*np.cos(np.pi/4*tt)*(np.pi/4)**2
# 绘制数据图
MyPlot.plot2_nd(tt, Xx_demo, title="Xx_demo")
MyPlot.plot2_nd(tt, Xv_demo, title="Xv_demo")
MyPlot.plot2_nd(tt, Xa_demo, title="Xa_demo")
# *****学习阶段******#
dmps_file_name = "dmps_parameter.txt"
dmps_path = os.path.join(file_path, dmps_file_name)
rbf_file_name = "rbf_parameter.txt"
rbf_path = os.path.join(file_path, rbf_file_name)
# 创建一个示教学习器
m = 2
teach_learn1 = TeachingLearn1(m, dmps_path, rbf_path)
# 获取rbf参数
h = 1000 # 隐藏层个数
teach_learn1.get_rbf_paramter(h)
# 获取示教数据
teach_learn1.get_teaching_data(Xx_demo, Xv_demo, Xa_demo,
Xx_demo[-1, :], Xx_demo[0, :], T)
# 采用最小二乘学习获取权重
teach_learn1.learn()
# 将权重写入文件
teach_learn1.write_data()
# 获取强迫项
f_demo = teach_learn1.f_demo
MyPlot.plot2_nd(tt, f_demo, title="f_demo")
# *****示教再现阶段******#
# 穿件示教再现器
dmps_file_name = "dmps_parameter.txt"
dmps_path = os.path.join(file_path, dmps_file_name)
rbf_file_name = "rbf_parameter.txt"
rbf_path = os.path.join(file_path, rbf_file_name)
teach_repro1 = TeachingReproduction(dmps_path, rbf_path)
# 规划新轨迹
# 时间系列
T = 0.01
num = 2000
tt = np.linspace(0, T * (num - 1), num)
xx0 = Xx_demo[0, :] + np.array([0.0, 0.0]) # 轨迹起点
gg =Xx_demo[-1, :] + np.array([0.10, 0.20]) # 规划目标点
teach_repro1.reproduction(xx0, gg, tt, T)
X_data = teach_repro1.get_plan_tcp()
# 获取强迫项
f = teach_repro1.f
# 绘制力跟踪图
MyPlot.plot2_nd(tt, f, title="f")
MyPlot.plot2_nd(tt, X_data, title="X_data")
def local_test():
# 获取目标路径
current_path = os.getcwd()
file_path = os.path.join(current_path, "../..", "data/teaching")
file_path = os.path.abspath(file_path)
# *****示教阶段******#
# 读取数据
qq_file_name = "teaching_data.txt"
qq_path = os.path.join(file_path, qq_file_name)
qq_demo = FileOpen.read(qq_path)[0:1000, :]
print "shape:", qq_demo.shape
# 创建运动学
DH_0 = np.copy(rp.DHfa_armc)
kin1 = kin.GeneralKinematic(DH_0)
# 获取起始点
X0 = kin1.fkine_euler(qq_demo[0, :])
# 获取目标点
X_goal = kin1.fkine_euler(qq_demo[-1, :])
# 时间系列
T = 0.01
num = len(qq_demo)
tt = np.linspace(0, T * (num - 1), num)
# 绘制数据图
MyPlot.plot2_nd(tt, qq_demo, title="qq_demo")
# 求取正运动学
X_demo = np.zeros([num, 6])
for i in range(num):
X_demo[i, :] = kin1.fkine_euler(qq_demo[i, :])
# 绘制数据图
MyPlot.plot2_nd(tt, X_demo, title="X_demo")
# *****学习阶段******#
dmps_file_name = "dmps_parameter.txt"
dmps_path = os.path.join(file_path, dmps_file_name)
local_file_name = "local_parameter.txt"
local_path = os.path.join(file_path, local_file_name)
# 创建一个示教学习器
m = 6
teach_learn1 = TeachingLearnLocal(m, dmps_path, local_path)
# 获取机器人参数
teach_learn1.get_robot_paramter(DH_0)
# 获取局部拟合参数
N = 1000 # 隐藏层个数
h = 0.1 #带宽
teach_learn1.get_weight_num(N, h)
# 获取示教数据
teach_learn1.get_teaching_data(qq_demo, X_goal, X0, T)
# 采用最小二乘学习获取权重
teach_learn1.learn()
# 将权重写入文件
teach_learn1.write_data()
# 获取强迫项
f_demo = teach_learn1.f_demo
MyPlot.plot2_nd(tt, f_demo, title="f_demo")
# *****示教再现阶段******#
# 穿件示教再现器
dmps_file_name = "dmps_parameter.txt"
dmps_path = os.path.join(file_path, dmps_file_name)
rbf_file_name = "rbf_parameter.txt"
rbf_path = os.path.join(file_path, rbf_file_name)
teach_repro1 = TeachingReproduction(dmps_path, rbf_path)
# 规划新轨迹
# 时间系列
T = 0.01
num = len(qq_demo)
tt = np.linspace(0, T * (num - 1), num)
print "X0:", X0
print "X_goal:", X_goal
xx0 = X0 + np.array([0.0, 0, -0.0, 0, 0, 0]) # 轨迹起点
gg = X_goal + np.array([0.09, 0.02, -0.09, 0, 0, 0]) # 规划目标点
teach_repro1.reproduction(xx0, gg, tt, T)
X_data = teach_repro1.get_plan_tcp()
# 获取强迫项
f = teach_repro1.f
# 绘制力跟踪图
MyPlot.plot2_nd(tt, f, title="f")
MyPlot.plot2_nd(tt, X_data, title="X_data")
def door_handle_grab():
# 获取目标路径
current_path = os.getcwd()
file_path = os.path.join(current_path, "../..", "data/teaching")
file_path = os.path.abspath(file_path)
# *****示教阶段******#
# 读取数据
qq_file_name = "teaching_data.txt"
qq_path = os.path.join(file_path, qq_file_name)
qq_demo = FileOpen.read(qq_path)[0:2000, :]
# 绘制原始数据图
# 时间系列
T = 0.01
num = len(qq_demo)
tt = np.linspace(0, T * (num - 1), num)
# 绘制数据图
MyPlot.plot2_nd(tt, qq_demo, title="qq_demo", lable="qq")
print "shape:", qq_demo.shape
# 创建运动学
DH_0 = np.copy(rp.DHfa_armc)
kin1 = kin.GeneralKinematic(DH_0)
# 获取起始点
X0 = kin1.fkine_euler(qq_demo[0, :])
# 获取目标点
X_goal = kin1.fkine_euler(qq_demo[-1, :])
# 求取正运动学
X_demo = np.zeros([num, 6])
for i in range(num):
X_demo[i, :] = kin1.fkine_euler(qq_demo[i, :])
# 绘制数据图
MyPlot.plot2_nd(tt, X_demo, title="X_demo")
# *****学习阶段:关节空间学习,直接得到规划关节角******#
dmps_file_name = "dhg_dmps_parameter.txt"
dmps_path = os.path.join(file_path, dmps_file_name)
rbf_file_name = "dhg_rbf_parameter.txt"
rbf_path = os.path.join(file_path, rbf_file_name)
# 创建一个示教学习器
teach_learn1 = JointTeachingLearn(dmps_path, rbf_path)
# 获取rbf参数
h = 1000 # 隐藏层个数
teach_learn1.get_rbf_paramter(h)
# 获取示教数据
teach_learn1.get_teaching_data(qq_demo, T)
# 采用最小二乘学习获取权重
teach_learn1.learn()
# 绘制拟合图
qq_qva = teach_learn1.qq_qva
MyPlot.plot2_nd(tt, qq_qva[:, :, 0], title="qq", lable="qq")
MyPlot.plot2_nd(tt, qq_qva[:, :, 1], title="qv", lable="qv")
MyPlot.plot2_nd(tt, qq_qva[:, :, 2], title="qa", lable="qa")
# 将权重写入文件
teach_learn1.write_data()
# 获取强迫项
f_demo = teach_learn1.f_demo
MyPlot.plot2_nd(tt, f_demo, title="f_demo", lable="fd")
# *****示教再现阶段******#
# 穿件示教再现器
# dmps_file_name = "dhg_dmps_parameter.txt"
# dmps_path = os.path.join(file_path, dmps_file_name)
#
# rbf_file_name = "dhg_rbf_parameter.txt"
# rbf_path = os.path.join(file_path, rbf_file_name)
teach_repro1 = JointTeachingReproduction(dmps_path, rbf_path)
# 规划新轨迹
# 时间系列
T = 0.01
num = len(qq_demo)
tt = np.linspace(0, T * (num - 1), num)
print "X0:", X0
print "X_goal:", X_goal
xx0 = X0 + np.array([0.0, 0, -0.0, 0, 0, 0]) # 轨迹起点
gg = X_goal + np.array([0.01, 0.00, -0.1, 0, 0, 0]) # 规划目标点
#转化为关节空间
qq0 = qq_demo[0, :]
qe = qq_demo[-1, :] + np.array([0.1, 0.00, -0.1, 0, 0, 0.1, 0.4])
qq_data = teach_repro1.reproduction(xx0, gg, tt, T)
# 获取强迫项
f = teach_repro1.f
# 绘制力跟踪图
MyPlot.plot2_nd(tt, f, title="f", lable="f")
MyPlot.plot2_nd(tt, qq_data, title="qq_data")
def rbf_test():
# 获取目标路径
current_path = os.getcwd()
file_path = os.path.join(current_path, "../..", "data/teaching")
file_path = os.path.abspath(file_path)
# *****示教阶段******#
# 读取数据
qq_file_name = "teaching_data.txt"
qq_path = os.path.join(file_path, qq_file_name)
qq_demo = FileOpen.read(qq_path)[0:2000, :]
#绘制原始数据图
# 时间系列
T = 0.01
num = len(qq_demo)
tt = np.linspace(0, T * (num - 1), num)
# 绘制数据图
MyPlot.plot2_nd(tt, qq_demo, title="qq_demo", lable="qq")
print "shape:", qq_demo.shape
# 创建运动学
DH_0 = np.copy(rp.DHfa_armc)
kin1 = kin.GeneralKinematic(DH_0)
# 获取起始点
X0 = kin1.fkine_euler(qq_demo[0, :])
# 获取目标点
X_goal = kin1.fkine_euler(qq_demo[-1, :])
# 求取正运动学
X_demo = np.zeros([num, 6])
for i in range(num):
X_demo[i, :] = kin1.fkine_euler(qq_demo[i, :])
# 绘制数据图
MyPlot.plot2_nd(tt, X_demo, title="X_demo")
for i in range(num):
for j in range(6):
X_demo[i, j] = X_demo[i, j] + 0.01 * (np.random.random() - 0.5)
# 绘制数据图
MyPlot.plot2_nd(tt, X_demo, title="X_rdemo", lable="Xr")
# *****学习阶段******#
dmps_file_name = "dmps_parameter.txt"
dmps_path = os.path.join(file_path, dmps_file_name)
rbf_file_name = "rbf_parameter.txt"
rbf_path = os.path.join(file_path, rbf_file_name)
# 创建一个示教学习器
m = 6
teach_learn1 = TeachingLearn(m, dmps_path, rbf_path)
# 获取机器人参数
teach_learn1.get_robot_paramter(DH_0)
# 获取rbf参数
h = 1000 # 隐藏层个数
teach_learn1.get_rbf_paramter(h)
# 获取示教数据
teach_learn1.get_teaching_data(qq_demo, X_goal, X0, T)
# 采用最小二乘学习获取权重
teach_learn1.learn()
#绘制拟合图
X_xva = teach_learn1.X_xva
MyPlot.plot2_nd(tt, X_xva[:, :, 0], title="Xx", lable="Xx")
MyPlot.plot2_nd(tt, X_xva[:, :, 1], title="Xv", lable="Xv")
MyPlot.plot2_nd(tt, X_xva[:, :, 2], title="Xa", lable="Xa")
# 将权重写入文件
teach_learn1.write_data()
# 获取强迫项
f_demo = teach_learn1.f_demo
MyPlot.plot2_nd(tt, f_demo, title="f_demo", lable= "fd")
# *****示教再现阶段******#
# 穿件示教再现器
dmps_file_name = "dmps_parameter.txt"
dmps_path = os.path.join(file_path, dmps_file_name)
rbf_file_name = "rbf_parameter.txt"
rbf_path = os.path.join(file_path, rbf_file_name)
teach_repro1 = TeachingReproduction(dmps_path, rbf_path)
# 规划新轨迹
# 时间系列
T = 0.01
num = len(qq_demo)
tt = np.linspace(0, T * (num - 1), num)
print "X0:", X0
print "X_goal:", X_goal
xx0 = X0 + np.array([0.0, 0, -0.0, 0, 0, 0]) # 轨迹起点
gg = X_goal + np.array([0.01, 0.00, -0.1, 0, 0, 0]) # 规划目标点
teach_repro1.reproduction(xx0, gg, tt, T)
X_data = teach_repro1.get_plan_tcp()
# 获取强迫项
f = teach_repro1.f
# 绘制力跟踪图
MyPlot.plot2_nd(tt, f, title="f", lable="f")
MyPlot.plot2_nd(tt, X_data, title="X_data")
def line_force_plan_armc():
#建立规划类
linePlan = PathPlan.LinePlan()
# 获取机器人参数
DH_0 = rp.DHfa_armc
qq_max = rp.q_max_armc
qq_min = rp.q_min_armc
kin1 = kin.GeneralKinematic(DH_0, rp.q_min_armc, rp.q_max_armc)
linePlan.get_robot_parameter(DH_0, qq_min, qq_max)
#规划起点和终点
Xb = np.array([0.410, 0, 0.022, 0, 3.14, 0])
Xe = Xb + np.array([0.15, 0, 0, 0, 0, 0])
linePlan.get_begin_end_point(Xb, Xe)
#获取起点关节角猜测值
qq_guess = np.array([0, 30, 0, 85, 0, 65, 0])*np.pi/180.0
linePlan.get_init_guess_joint(qq_guess)
#时间和周期
T = 0.01
linePlan.get_period(T)
linePlan.get_plan_time(50)
#求取关节角度
qq = linePlan.out_joint()
#合成位置曲线
num_init = 1000
num_pos = len(qq)
num = num_init + num_pos
t = np.linspace(0, T * (num - 1), num)
qd = np.zeros([num, 7])
for i in range(num_init):
qd[i, :] = qq[0, :]
qd[num_init:] = qq
#获得末端位姿
XX_c = np.zeros([num, 3])
for i in range(num):
Te = kin1.fkine(qd[i, :])
XX_c[i, :] = Te[0:3, 3]
#生成受力曲线
fd = -5
Fd = np.zeros([num, 6])
Fd[:num_init, 2] = fd*np.sin(np.pi/20*t[:num_init])
for i in range(num_pos):
Fd[num_init + i, 2] = fd
#绘制期望力
# 绘制数据图
dpi = 500
# 搬运臂期望位置
plt.figure(1)
plt.plot(t, 1000*XX_c[:, 0], linewidth='2', label='x', color='r', linestyle='--')
plt.plot(t, 1000*XX_c[:, 1], linewidth='2', label='y', color='g', linestyle='-.')
plt.plot(t, 1000*XX_c[:, 2], linewidth='2', label='z', color='b')
plt.title(u"曲面恒力跟踪期望位置")
plt.xlabel("t(s)")
plt.ylabel("X(mm)")
plt.ylim(-100, 800)
plt.legend()
plt.rcParams['savefig.dpi'] = dpi # 图片像素
# 曲面直线
plt.figure(2)
plt.ylim(-6, 0)
plt.plot(t, Fd[:, 2], linewidth='2', label='Fz', color='b')
plt.title(u"曲面力恒跟踪期望力")
plt.xlabel("t(s)")
plt.ylabel("Fz(N)")
plt.legend()
plt.rcParams['savefig.dpi'] = dpi # 图片像素
# 绘制关节角
MyPlot.plot2_nd(t, Fd, title="Fd", lable='fd')
MyPlot.plot2_nd(t, qd, title="qd", lable='qd')
# 写入文件
parent_path = os.path.join(os.getcwd(), '../..', 'data/impedance')
parent_path = os.path.abspath(parent_path)
pos_path = parent_path + "/armc_line_position2.txt"
force_path = parent_path + "/armc_line_force2.txt"
FileOpen.write(qd, pos_path)
FileOpen.write(Fd, force_path)
return qq
def line_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)
#从家点运动到初始位置
[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, :])
#停顿点
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 repeat_positionin_plan():
#建立规划类
linePlan = PathPlan.LinePlan()
# 获取机器人参数
DH_0 = rp.DH0_armc
qq_max = rp.q_max_armc
qq_min = rp.q_min_armc
linePlan.get_robot_parameter(DH_0, qq_min, qq_max)
my_kin = kin.GeneralKinematic(DH_0, qq_min, qq_max)
#建立测试平面
l = 0.08
P1 = np.array([0, 0, 0, 1.0])
P2 = np.array([-l, -l, 0, 1.0])
P3 = np.array([l, -l, 0, 1.0])
P4 = np.array([l, l, 0, 1.0])
P5 = np.array([-l, l, 0, 1.0])
#测试平面到基坐标系转换
qq0 = np.array([0, -45, 0, 30, 0, 60, 0.0])*np.pi/180.0
T0_c = my_kin.fkine(qq0)
print "tc:", np.around(T0_c, 4)
P2 = np.dot(T0_c, P2)
P3 = np.dot(T0_c, P3)
P4 = np.dot(T0_c, P4)
P5 = np.dot(T0_c, P5)
X1 = my_kin.fkine_euler(qq0)
X2 = np.copy(X1)
X2[0:3] = P2[0:3]
X3 = np.copy(X1)
X3[0:3] = P3[0:3]
X4 = np.copy(X1)
X4[0:3] = P4[0:3]
X5 = np.copy(X1)
X5[0:3] = P5[0:3]
print "X1:", np.around(X1, 3)
print "X2:", np.around(X2, 3)
print "X3:", np.around(X3, 3)
print "X4:", np.around(X4, 3)
print "X5:", np.around(X5, 3)
# 时间和周期
T = 0.01
linePlan.get_period(T)
linePlan.get_plan_time(8)
#规划起点和终点
linePlan.get_begin_end_point(X1, X2)
linePlan.get_init_guess_joint(qq0)
qq1 = linePlan.out_joint()
linePlan.get_begin_end_point(X2, X3)
linePlan.get_init_guess_joint(qq1[-1, :])
qq2 = linePlan.out_joint()
linePlan.get_begin_end_point(X3, X4)
linePlan.get_init_guess_joint(qq2[-1, :])
qq3 = linePlan.out_joint()
linePlan.get_begin_end_point(X4, X5)
linePlan.get_init_guess_joint(qq3[-1, :])
qq4 = linePlan.out_joint()
linePlan.get_begin_end_point(X5, X1)
linePlan.get_init_guess_joint(qq4[-1, :])
qq5 = linePlan.out_joint()
q0 = np.zeros(7)
[qq6, _, _] = bf.interp5rdPoly(qq5[-1, :], q0, q0, qq1[0, :], q0, q0, 1.0, T)
#直接关节空间规划
k1 = len(qq1)
k2 = len(qq2)
k3 = len(qq3)
k4 = len(qq4)
k5 = len(qq5)
k6 = len(qq6)
ks = 200
n = len(qq0)
kk = k1 + k2 + k3 + k4 + k5 + 5*ks + k6
qq = np.zeros([kk, n])
k = 0
qq[k:k1, :] = qq1
k = k + k1
qq[k:k+ks] = np.dot(np.ones([ks, n]), np.diag(qq1[-1, :]))
k = k + ks
qq[k:k + k2, :] = qq2
k = k + k2
qq[k:k + ks] = np.dot(np.ones([ks, n]), np.diag(qq2[-1, :]))
k = k + ks
qq[k:k + k3, :] = qq3
k = k + k3
qq[k:k + ks] = np.dot(np.ones([ks, n]), np.diag(qq3[-1, :]))
k = k + ks
qq[k:k + k4, :] = qq4
k = k + k4
qq[k:k + ks] = np.dot(np.ones([ks, n]), np.diag(qq4[-1, :]))
k = k + ks
qq[k:k + k5, :] = qq5
k = k + k5
qq[k:k+k6, :] = qq6
k = k + k6
qq[k:k + ks] = np.dot(np.ones([ks, n]), np.diag(qq6[-1, :]))
t = np.linspace(0, T * (kk - 1), kk)
MyPlot.plot2_nd(t, qq, title="qq", lable='qq')
m = 10
qq_m = np.zeros([m*kk, n])
for i in range(m):
qq_m[i*kk:(i+1)*kk, :] = qq
tm = np.linspace(0, T * (m*kk - 1), m*kk)
MyPlot.plot2_nd(tm, qq_m, title="qq_m", lable='qq_m')
# 写入文件
parent_path = os.path.join(os.getcwd(), '../..', 'data')
parent_path = os.path.abspath(parent_path)
file_name = "armc_repeat _position_m.txt"
path = os.path.join(parent_path, file_name)
FileOpen.write(qq_m, path)
return qq
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 robots_polish_object_plan():
Xb = rp.Wooden_X
Xe = Xb + np.array([0.00, 0.2, 0.100, 0, 0, 0])
T = 0.01
t = 30
#建立多臂规划类
robots1 = Robots.RobotsPolishObject()
#获取DH参数
robots1.get_robot1_paramter(rp.DH0_ur5, rp.q_min_ur5, rp.q_max_ur5)
robots1.get_robot2_paramter(rp.DH0_ur5, rp.q_min_ur5, rp.q_max_ur5)
robots1.get_robot3_paramter(rp.DH0_ur5, rp.q_min_ur5, rp.q_max_ur5)
#获取基座到世界坐标系参数
robots1.get_robots_base_to_world(rp.robot1_base_T, rp.robot2_base_T,
rp.robot3_base_T)
#获取抓取点相对于工件坐标系
T_o_t1 = np.eye(4)
T_o_t2 = np.eye(4)
T_o_t1[0:3, 0:3] = np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0.0]])
T_o_t1[0:3, 3] = np.array([-0.048, 0.0, 0.050])
T_o_t2[0:3, 0:3] = np.array([[0, 0, -1], [1, 0, 0], [0, -1, 0.0]])
T_o_t2[0:3, 3] = np.array([0.048, 0.0, 0.050])
robots1.get_robot12_tool_to_object(T_o_t1, T_o_t2)
#输入准备长度
l = 0.060
ready_num = 400
robots1.get_ready_distance(l, ready_num)
#输入工件规划点
Xe_list = workpiece_moving_track_line(Xb, Xe, T, t)
robots1.get_object_plan_list_zyx(Xe_list)
#输入机械臂3的打磨轨迹(相对于工件坐标系)
#打磨起始点
Xo_t3_init = np.array([-0.05, 0.0, 0.099, 0, np.pi, 0])
#打磨终点
Xo_t3_end = np.array([0.05, 0.0, 0.099, 0, np.pi, 0])
#打磨轨迹
t3 = 20
Xo_t3 = workpiece_moving_track_line(Xo_t3_init, Xo_t3_end, T, t3)
#输入工件相对位置
robots1.get_robot3_tool_to_object_zyx(Xo_t3)
#获取末端位姿
[X1, X2, X3] = robots1.put_robots_tool_position_zyx()
# 笛卡尔单变量随时间绘图
# 绘制关节角
T = 0.01
num12 = len(X1)
t12 = np.linspace(0, T * (num12 - 1), num12)
num3 = len(X3)
t3 = np.linspace(0, T * (num3 - 1), num3)
#获取关节角
qq1_guess = np.array([-90, -120, -120, 60, 90, 0]) * np.pi / 180.0
qq2_guess = np.array([-90, -60, 120, 120, -90, 0]) * np.pi / 180.0
qq3_guess = np.array([0, -90, -90, -90, 90, 0]) * np.pi / 180.0
[qq1, qq2,
qq3] = robots1.put_robots_joint_position(qq1_guess, qq2_guess, qq3_guess)
#获取规划的力
Fd = np.zeros(6)
Fd[2] = -10
fd3 = robots1.put_polish_force(Fd)
# 绘制关节角
num12 = len(qq1)
t12 = np.linspace(0, T * (num12 - 1), num12)
num3 = len(qq3)
t3 = np.linspace(0, T * (num3 - 1), num3)
# 笛卡尔单变量随时间绘图
MyPlot.plot2_nd(t12, qq1, title="qq1")
MyPlot.plot2_nd(t12, qq2, title="qq2")
MyPlot.plot2_nd(t3, qq3, title="qq3")
MyPlot.plot2_nd(t3, fd3, title="fd3")
# 写入文件
parent_path = os.path.join(os.getcwd(), '../..')
parent_path = os.path.abspath(parent_path)
file_name1 = "data/robots/robot1_polish_position1.txt"
path1 = os.path.join(parent_path, file_name1)
FileOpen.write(qq1, path1)
# 写入文件
file_name2 = "data/robots/robot2_polish_position1.txt"
path2 = os.path.join(parent_path, file_name2)
FileOpen.write(qq2, path2)
# 写入文件
file_name3_qq = "data/robots/robot3_polish_position1.txt"
path3_qq = os.path.join(parent_path, file_name3_qq)
FileOpen.write(qq3, path3_qq)
# 写入文件
file_name3_f = "data/robots/robot3_polish_force1.txt"
path3_f = os.path.join(parent_path, file_name3_f)
FileOpen.write(fd3, path3_f)
return [qq1, qq2, qq3, fd3]
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(tm, m[:, 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 armct_move_object_plan():
Xb = rp.ball_X
Xe = Xb + np.array([0.0, 0.0, 0.100, 0, 0, 0])
T = 0.01
t = 30
#建立多臂规划类
robots1 = Robots.ArmctMoveObject()
#获取DH参数
robots1.get_robot1_paramter(rp.DHfx_armt, rp.q_min_armt, rp.q_max_armt)
robots1.get_robot2_paramter(rp.DHfx_armc, rp.q_min_armc, rp.q_max_armc)
#获取基座到世界坐标系参数
robots1.get_robots_base_to_world(rp.armt_base_T, rp.armc_base_T)
#获取抓取点相对于工件坐标系
T_o_t1 = np.eye(4)
T_o_t2 = np.eye(4)
T_o_t1[0:3, 0:3] = np.array([[0, 0, -1], [0, -1, 0], [-1, 0, 0.0]])
T_o_t1[0:3, 3] = np.array([0.127, 0.0, 0.000])
T_o_t2[0:3, 0:3] = np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0.0]])
T_o_t2[0:3, 3] = np.array([-0.127, 0.0, 0.000])
robots1.get_robots_tool_to_object(T_o_t1, T_o_t2)
#输入准备长度
l = 0.030
ready_num = 400
robots1.get_ready_distance(l, ready_num)
num_s = 500
robots1.get_move_stop_num(num_s)
#输入工件规划点
Xe_list = workpiece_moving_track_line(Xb, Xe, T, t)
robots1.get_object_plan_list_zyx(Xe_list)
#获取关节角
qq1_guess = np.array([0, -45, 0, 85, 0, 50, 0]) * np.pi / 180.0
qq2_guess = np.array([0, -45, 0, 85, 0, 50, 0]) * np.pi / 180.0
[qq1, qq2] = robots1.put_robots_joint_position(qq1_guess, qq2_guess)
fr1 = -0.0
fr2 = 0.0
fd = -30
[f1, f2] = robots1.put_expect_force(fr1, fd, fr2)
# 绘制关节角
num12 = len(qq1)
t12 = np.linspace(0, T * (num12 - 1), num12)
MyPlot.plot2_nd(t12, qq1, title="qq1")
MyPlot.plot2_nd(t12, qq2, title="qq2")
MyPlot.plot2_nd(t12, f1, title="f")
MyPlot.plot2_nd(t12, f2, title="f2")
# 写入文件
parent_path = os.path.join(os.getcwd(), '../..')
parent_path = os.path.abspath(parent_path)
file_name1 = "data/robots/armct/armt_position.txt"
path1 = os.path.join(parent_path, file_name1)
FileOpen.write(qq1, path1)
# 写入文件
file_name2 = "data/robots/armct/armc_position.txt"
path2 = os.path.join(parent_path, file_name2)
FileOpen.write(qq2, path2)
file_name3 = "data/robots/armct/armt_force.txt"
path3 = os.path.join(parent_path, file_name3)
FileOpen.write(f1, path3)
# 写入文件
file_name4 = "data/robots/armct/armc_force.txt"
path4 = os.path.join(parent_path, file_name4)
FileOpen.write(f2, path4)
return [qq1, qq2]
elif j < 6:
pos_began[i, j] = o_s[i, j - 3]
elif j < 9:
pos_began[i, j] = a_s[i, j - 6]
else:
pos_began[i, j] = xyz_sf[i, j - 9]
#12参数回程位置轨迹
pos_back = np.zeros([1000, 12])
for i in range(1000):
pos_back[i, :] = pos_began[999 - i, :]
pos[:, 11]
#总轨迹12参数表示
pos_all = np.zeros([4000, 12])
for i in range(4000):
if i < 1000:
pos_all[i, :] = pos_began[i, :]
elif i < 3000:
pos_all[i, :] = pos[i - 1000, :]
else:
pos_all[i, :] = pos_back[i - 3000, :]
print pos_all.shape
#绘制图形
MyPlot.plot3d(pos_all[:, 9], pos_all[:, 10], pos_all[:, 11], 'pentagram_pos')
#将12参数写入文件
FileOpen.write(pos_all, 'data/pentagram_pos.txt')
def armctr_move_object_plan():
## 建立多臂规划类
robots1 = Robots.robotsMoveObject()
robots2 = Robots.robotsMoveObject()
robots3 = Robots.robotsMoveObject()
#机械臂关节角度
n1 = 6
n2 = 7
n3 = 7
# 获取DH参数
robots1.get_robot_paramter(rp.DH0_ur5, rp.q_min_ur5, rp.q_max_ur5)
robots2.get_robot_paramter(rp.DHfx_armt, rp.q_min_armt, rp.q_max_armt)
robots3.get_robot_paramter(rp.DHfx_armc, rp.q_min_armc, rp.q_max_armc)
kin2 = kin.GeneralKinematic(rp.DHfx_armt, rp.q_min_armt, rp.q_max_armt)
kin3 = kin.GeneralKinematic(rp.DHfx_armc, rp.q_min_armc, rp.q_max_armc)
# 获取基座到世界坐标系参数
# Tb1 = rp.robotsr_armr_base_T
# Tb2 = rp.robotsr_armt_base_T
# Tb3 = rp.robotsr_armc_base_T
a = np.sqrt(2) / 2
Tb1 = np.array([[-a, -a, 0, 0.6], [a, -a, 0, 0], [0, 0, 1, 0.020],
[0, 0, 0, 1.0]])
Tb2 = np.array([[1, 0, 0, -0.825], [0, 1, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1.0]])
Tb3 = np.array([[0, 1, 0, 0], [-1, 0, 0, 1.05], [0, 0, 1, 0],
[0, 0, 0, 1.0]])
robots1.get_robot_base_to_world(Tb1)
robots2.get_robot_base_to_world(Tb2)
robots3.get_robot_base_to_world(Tb3)
##物体运动轨迹轨迹
T = 0.01
t = 10
Tw_o = np.eye(4)
xo_1 = np.array([0, 0, 0.2])
xo_2 = xo_1 + np.array([0, 0, 0.25])
xo_3 = xo_2 + np.array([0.125, 0, 0])
x0 = np.zeros(3)
#搬运段
[xo_12_array, _, _] = bf.interp5rdPoly(xo_1, x0, x0, xo_2, x0, x0, t, T)
num_o12 = len(xo_12_array)
Tw_o12 = np.zeros([num_o12, 4, 4])
for i in range(num_o12):
Tw_o[0:3, 3] = xo_12_array[i, :]
Tw_o12[i, :, :] = Tw_o
#协同打磨段
[xo_23_array, _, _] = bf.interp5rdPoly(xo_2, x0, x0, xo_3, x0, x0, t, T)
num_o23 = len(xo_23_array)
Tw_o23 = np.zeros([num_o23, 4, 4])
for i in range(num_o23):
Tw_o[0:3, 3] = xo_23_array[i, :]
Tw_o23[i, :, :] = Tw_o
##获取抓取点相对于工件坐标系
To_t1 = np.array([[0, 0, -1, 0.126], [1, 0, 0, 0], [0, -1, 0, 0],
[0, 0, 0, 1.0]])
To_t2 = np.array([[0, 0, 1, -0.126], [0, 1, 0, 0], [-1, 0, 0, 0],
[0, 0, 0, 1.0]])
To_t3 = np.array([[0, 1, 0, 0], [0, 0, -1, 0.14], [-1, 0, 0, 0.1],
[0, 0, 0, 1.0]])
xo_t3_1 = np.array([0, 0.14, 0.1])
xo_t3_2 = xo_t3_1 + np.array([0, 0, -0.2])
t = 10
[xo_t3_array, _, _] = bf.interp5rdPoly(xo_t3_1, x0, x0, xo_t3_2, x0, x0, t,
T)
num_t3 = len(xo_t3_array)
To_t3_array = np.zeros([num_t3, 4, 4])
for i in range(num_t3):
To_t3_array[i, :, :] = To_t3
To_t3_array[i, 0:3, 3] = xo_t3_array[i, :]
#输入准备长度
l = 0.040
t_r = 5
[l_array, _, _] = bf.interp5rdPoly1(l, 0.0, 0.0, 0.0, 0.0, 0.0, t_r, T)
num_r = len(l_array)
To_t1_l_array = np.zeros([num_r, 4, 4])
To_t2_l_array = np.zeros([num_r, 4, 4])
To_t3_l_array = np.zeros([num_r, 4, 4])
for i in range(num_r):
To_t1_l_array[i, :, :] = To_t1
To_t1_l_array[i, 0, 3] = To_t1_l_array[i, 0, 3] + l_array[i]
To_t2_l_array[i, :, :] = To_t2
To_t2_l_array[i, 0, 3] = To_t2_l_array[i, 0, 3] - l_array[i]
To_t3_l_array[i, :, :] = To_t3_array[0, :, :]
To_t3_l_array[i, 1, 3] = To_t3_l_array[i, 1, 3] + l_array[i]
#获取关节角
qq1_guess = np.array([45, -120, -120, 60, 90, 0]) * np.pi / 180.0
qq2_guess = np.array([0, 60, 0, 90, 0, -60, 0]) * np.pi / 180.0
qq3_guess = np.array([0, 30, 0, 30, 0, 30, 0]) * np.pi / 180.0
#获取机械臂12的关节角
#准备段关节角
qq1_1 = robots1.out_robot_joint(qq1_guess, Tw_o12[0, :, :], To_t1_l_array)
print "机械臂1准备段求取成功！"
qq2_1 = robots2.out_robot_joint(qq2_guess, Tw_o12[0, :, :], To_t2_l_array)
print "机械臂2准备段求取成功！"
#等待加载力
num_l = 401
qq1_2 = np.dot(np.ones([num_l, n1]), np.diag(qq1_1[-1, :]))
qq2_2 = np.dot(np.ones([num_l, n2]), np.diag(qq2_1[-1, :]))
#将物体搬运到一定高度
qq1_3 = robots1.out_robot_joint(qq1_2[-1, :], Tw_o12, To_t1)
print "机械臂1搬运段求取成功！"
qq2_3 = robots2.out_robot_joint(qq2_2[-1, :], Tw_o12, To_t2)
print "机械臂2搬运段求取成功！"
#等待机械臂3准备并加载力
num_s = num_l + num_r
qq1_4 = np.dot(np.ones([num_s, n1]), np.diag(qq1_3[-1, :]))
qq2_4 = np.dot(np.ones([num_s, n2]), np.diag(qq2_3[-1, :]))
#协同运动段
qq1_5 = robots1.out_robot_joint(qq1_4[-1, :], Tw_o23, To_t1)
qq2_5 = robots2.out_robot_joint(qq2_4[-1, :], Tw_o23, To_t2)
print "机械臂12协同段求取成功！"
#获取机械臂3的关节角
#准备段关节角度
qq3_1 = robots3.out_robot_joint(qq3_guess, Tw_o23[0, :, :], To_t3_l_array)
print "机械臂3准备段求取成功！"
#等待机械臂3加载力
qq3_2 = np.dot(np.ones([num_l, n3]), np.diag(qq3_1[-1, :]))
#协同运动动
qq3_3 = robots3.out_robot_joint(qq3_2[-1, :], Tw_o23, To_t3_array)
print "机械臂3协同段求取成功！"
##按时序拼接对应段
qq1_ = np.concatenate((qq1_1, qq1_2, qq1_3, qq1_4, qq1_5), axis=0)
qq2_ = np.concatenate((qq2_1, qq2_2, qq2_3, qq2_4, qq2_5), axis=0)
qq3_ = np.concatenate((qq3_1, qq3_2, qq3_3), axis=0)
qq1 = np.concatenate((qq1_, qq1_[::-1, :]), axis=0)
qq2 = np.concatenate((qq2_, qq2_[::-1, :]), axis=0)
qq3 = np.concatenate((qq3_, qq3_[::-1, :]), axis=0)
##规划控制力
#机械臂2
num2 = len(qq2)
Fd2 = np.array([0, 0, -40, 0, 0, 0.0])
F0 = np.zeros(6)
f2_r = -np.ones([num_r, 6]) * 0
t_l = (num_l - 1) * T
[f2_l, _, _] = bf.interp5rdPoly(F0, F0, F0, Fd2, F0, F0, t_l, T)
f2_3 = np.dot(np.ones([num2 - 2 * num_r - 2 * num_l, 6]), np.diag(Fd2))
ff2 = np.concatenate((f2_r, f2_l, f2_3, f2_l[::-1, :], f2_r[::-1, :]),
axis=0)
#机械臂3
num3 = len(qq3)
Fd3 = np.array([0, 0, -5, 0, 0, 0.0])
f3_r = -0 * np.ones([num_r, 6])
[f3_l, _, _] = bf.interp5rdPoly(F0, F0, F0, Fd3, F0, F0, t_l, T)
f3_3 = np.dot(np.ones([num3 - 2 * num_r - 2 * num_l, 6]), np.diag(Fd3))
ff3 = np.concatenate((f3_r, f3_l, f3_3, f3_l[::-1, :], f3_r[::-1, :]),
axis=0)
#存储期望力
f2 = ff2[:, 2]
f3 = np.zeros(num2)
f3[((num2 - num3) / 2):((num2 + num3) / 2)] = ff3[:, 2]
f = np.zeros([num2, 2])
f[:, 0] = f2
f[:, 1] = f3
#存储末端轨迹
XX_t = np.zeros([num2, 3])
XX_c = np.zeros([num3, 3])
for i in range(num2):
Te = kin2.fkine(qq2[i, :])
XX_t[i, :] = Te[0:3, 3]
for i in range(num3):
Te = kin3.fkine(qq3[i, :])
XX_c[i, :] = Te[0:3, 3]
#绘制论文图
t12 = np.linspace(0, T * (num2 - 1), num2)
t3 = np.linspace(0, T * (num3 - 1), num3)
# 绘制数据图
dpi = 500
#搬运臂期望位置
plt.figure(1)
plt.plot(t12,
1000 * XX_t[:, 0],
linewidth='2',
label='x',
color='r',
linestyle='--')
plt.plot(t12,
1000 * XX_t[:, 1],
linewidth='2',
label='y',
color='g',
linestyle='-.')
plt.plot(t12, 1000 * XX_t[:, 2], linewidth='2', label='z', color='b')
plt.title(u"搬运机械臂Armt期望轨迹")
plt.xlabel("t(s)")
plt.ylabel("X(mm)")
plt.ylim(-100, 1000)
plt.legend()
plt.rcParams['savefig.dpi'] = dpi # 图片像素
#曲面直线
plt.figure(2)
plt.plot(t3,
1000 * XX_c[:, 0],
linewidth='2',
label='x',
color='r',
linestyle='--')
plt.plot(t3,
1000 * XX_c[:, 1],
linewidth='2',
label='y',
color='g',
linestyle='-.')
plt.plot(t3, 1000 * XX_c[:, 2], linewidth='2', label='z', color='b')
plt.title(u"未知曲面直线运动机械臂期望轨迹")
plt.xlabel("t(s)")
plt.ylabel("X(mm)")
plt.ylim(-100, 1250)
plt.legend()
plt.rcParams['savefig.dpi'] = dpi # 图片像素
#夹持力
plt.figure(3)
plt.plot(t12, f2, linewidth='2', label='Fz', color='b')
plt.title(u"搬运机械臂Armt期望力")
plt.ylim(-45, 8)
plt.xlabel("t(s)")
plt.ylabel("Fz(N)")
plt.legend()
plt.rcParams['savefig.dpi'] = dpi # 图片像素
#曲面力
plt.figure(4)
plt.plot(t3, ff3[:, 2], linewidth='2', label='Fz', color='b')
plt.ylim(-6, 1)
plt.title(u"未知曲面直线运动机械臂期望力")
plt.xlabel("t(s)")
plt.ylabel("Fz(N)")
plt.legend()
plt.rcParams['savefig.dpi'] = dpi # 图片像素
plt.show()
#计算机械臂1被动受力
ff1 = -ff2
#绘制关节角
MyPlot.plot2_nd(t12, qq1, title="qq1", lable="q_")
MyPlot.plot2_nd(t12, qq2, title="qq2", lable="q_")
MyPlot.plot2_nd(t3, qq3, title="qq3", lable="q_")
#绘制末端位置
MyPlot.plot2_nd(t12, XX_t, title="XX_t", lable="X")
MyPlot.plot2_nd(t3, XX_c, title="XX_c", lable="X")
print "对比：", len(ff1), num2, len(ff3), num3
MyPlot.plot2_nd(t12, ff1, title="F1", lable="F")
MyPlot.plot2_nd(t12, ff2, title="F2", lable="F")
MyPlot.plot2_nd(t3, ff3, title="F3", lable="F")
MyPlot.plot2_nd(t12, f, title="F", lable="F")
#写入位置
parent_path = os.path.join(os.getcwd(), '../..')
parent_path = os.path.abspath(parent_path)
file_force = "data/robots/armctr/expect_force.txt"
path_force = os.path.join(parent_path, file_force)
FileOpen.write(f, path_force)
file_pos1 = "data/robots/armctr/armr_position.txt"
path_pos1 = os.path.join(parent_path, file_pos1)
FileOpen.write(qq1, path_pos1)
file_pos2 = "data/robots/armctr/armt_position.txt"
path_pos2 = os.path.join(parent_path, file_pos2)
FileOpen.write(qq2, path_pos2)
file_pos3 = "data/robots/armctr/armc_position.txt"
path_pos3 = os.path.join(parent_path, file_pos3)
FileOpen.write(qq3, path_pos3)
#写入写入力
file_f1 = "data/robots/armctr/armr_force.txt"
path_f1 = os.path.join(parent_path, file_f1)
FileOpen.write(ff1, path_f1)
file_f2 = "data/robots/armctr/armt_force.txt"
path_f2 = os.path.join(parent_path, file_f2)
FileOpen.write(ff2, path_f2)
file_f3 = "data/robots/armctr/armc_force.txt"
path_f3 = os.path.join(parent_path, file_f3)
FileOpen.write(ff3, path_f3)
return 1