def joint_write_dmps_pramter(): # 时间尺度 tau = 10 # 所有变量时间尺度同时,保证运动统一性 # 正则化常数 alpha = 1 #刚度项 k = np.array([1000, 1000, 1000, 1000, 1000, 1000, 1000]) # 刚度项 #阻尼项 d = np.array([100, 100, 100, 100, 100, 100, 100]) # 阻尼项 # 存储dmps的模型参数 dmps_param = np.zeros([7, 3]) dmps_param[0, 0] = tau # 存储在第一位,其他位用0补齐 dmps_param[1, 0] = alpha dmps_param[:, 1] = k dmps_param[:, 2] = d #存储参数 current_path = os.getcwd() file_path = os.path.join(current_path, "../..", "data/teaching") file_path = os.path.abspath(file_path) dmps_file_name = "dhg_dmps_parameter.txt" dmps_path = os.path.join(file_path, dmps_file_name) #写入参数 FileOpen.write(dmps_param, dmps_path)
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 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_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 clikedOpenDesiredImage(self): fo = FileOpen.App() global imgPathDesired imgPathDesired = fo.openFileNameDialog() img = cv2.imread(imgPathDesired) cv2.imshow("Desired Image", img) return imgPathDesired
def clickedOpenImage(self): fo = FileOpen.App() global imgPathOrg imgPathOrg = fo.openFileNameDialog() img = cv2.imread(imgPathOrg) cv2.imshow("Orginal Image", img) return imgPathOrg
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 start_machine(self): self.finished.grid_forget() self.stepButton.grid(row=3, column=12) if (self.startEntry.get()) != "": try: int(self.startEntry.get()) fileType = "Exam" except: fileType = "Custom" if fileType == "Exam": fileChosen = int(self.startEntry.get()) tape_values, start, final, transition_rules, head, raw_rules = File.getRules( fileChosen) tape_length = (len(tape_values) - 1) print(tape_length) self.exam_question(tape_values, start, final, transition_rules, head, tape_length, raw_rules) elif fileType == "Custom": fileChosen = self.startEntry.get() tape_values, start, final, transition_rules, head, raw_rules = File.getRules( fileChosen) tape_length = (len(tape_values) - 1) self.exam_question(tape_values, start, final, transition_rules, head, tape_length, raw_rules) else: tape_values = (self.seg1.get(), self.seg2.get(), self.seg3.get(), self.seg4.get(), self.seg5.get(), self.seg6.get(), self.seg7.get(), self.seg8.get(), self.seg9.get(), self.seg10.get(), self.seg11.get()) print(tape_values) tape_length = 11 head = 5 start = 0 final = 0 transition_rules = None self.user_ran(tape_values, start, final, transition_rules, head, tape_length)
def get_dmps_paramter(self): # 加载dmps参数 dmps_param = FileOpen.read(self.dmps_path) # 时间尺度 self.tau = dmps_param[0, 0] # 正则系数 self.alpha = dmps_param[1, 0] # 刚度项 self.k = dmps_param[:, 1] # 阻尼项 self.d = dmps_param[:, 2]
def FileOpen(): data_store = fo.openFile() data_axis = data_store[0] data_value = data_store[1] global filenamelabel filenamelabel["text"] = 'Data Place : ' + data_store[2] global dirname dirname = os.path.dirname(data_store[2]) global basename basename = data_store[3] + '_fitting' dt.set_data(data_axis, data_value) fig.canvas.draw_idle()
def read_paramter(self): # 加载rbf参数 rbf_param = FileOpen.read(self.rbf_path) # 方差sigma self.sigma = rbf_param[0, 0] # 中心值 self.c = rbf_param[1, :] # 权重w self.w = rbf_param[2:, :] # w= m*h # 输出自由度 self.m = len(self.w[:, 0]) # 加载dmps参数 dmps_param = FileOpen.read(self.dmps_path) # 时间尺度 self.tau = dmps_param[0, 0] # 正则系统常数 self.alpha = dmps_param[1, 0] # 刚度项 self.k = dmps_param[:, 1] # 阻尼项 self.d = dmps_param[:, 2]
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 read_paramter(self): #加载rbf参数 local_param = FileOpen.read(self.weight_path) # 正则系统常数 self.h = local_param[0, 0] #中心值 self.c = local_param[:, 1] #权重w self.w = local_param[:, 2:] #N*m #输出自由度 self.m = len(self.w[0, :]) #权重个数 self.w_num = len(self.w[:, 0]) #加载dmps参数 dmps_param = FileOpen.read(self.dmps_path) #时间尺度 self.tau = dmps_param[0, 0] # 正则系统常数 self.alpha = dmps_param[1, 0] # 刚度项 self.k = dmps_param[:, 1] # 阻尼项 self.d = dmps_param[:, 2]
def main(): openFile = input("Do you want to launch from a file? ") if openFile == "yes" or openFile == "y" or openFile == "1" or openFile == "Yes": fileChosen = int(input("Enter a question number ")) tape_values, start, final, transition_rules, head = file.getRules(fileChosen) tape_length = (len(tape_values)-1) print(tape_length) examQuestion(tape_values, start, final, transition_rules, head, tape_length) elif openFile == "no" or openFile == "n" or openFile == "0" or openFile == "No": tape_values = ("1","0","1","/","0","1","1","0","/","1","0") tape_length = 11 head = 5 start = 0 final = 0 transition_rules = None userRan(tape_values, start, final, transition_rules, head, tape_length) else: print("Enter yes or no")
def talker(): #建立节点 pub = rospy.Publisher("/armc/armc_position_controller/command", Float64MultiArray, queue_size=1) rospy.init_node("joint_position_command", anonymous=True) rate = rospy.Rate(500) # 10hz #读取命令文件 file_path = os.path.abspath("..") file_name = 'data/circular_position.txt' path = os.path.join(file_path, file_name) command_pos = np.array(FileOpen.read(path)) #print command_pos.shape() #重写数据 kk = len(command_pos[:, 0]) n = len(command_pos[0, :]) print "数据个数:%d" % kk print "数据长度:%d" % n command_data = np.zeros([kk, n]) for i in range(kk): for j in range(n): command_data[i, j] = command_pos[i, j] k = 0 while not rospy.is_shutdown(): if k == kk: break tip_str = "第 %s 次命令:" % k rospy.loginfo(tip_str) send_data = Float64MultiArray() send_data.data = command_data[k, :] print send_data.data pub.publish(send_data) rate.sleep() k = k + 1
def write_data(self): # rbf参数 FileOpen.write(self.weight_param, self.weight_path)
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 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")
thetaMax = np.array([180, 90, 180, 90, 180, 90, 180]) * (np.pi / 180) thetaMin = np.array([-180, -90, -180, -90, -180, -90, -180]) * (np.pi / 180) theta = np.zeros([500, 7]) pos = np.zeros([500, 12]) for i in range(500): for j in range(7): theta[i, j] = thetaMin[j] + (thetaMax[j] - thetaMin[j]) * np.random.random() T = fk.T_7to0(theta[i, :], alpha, a, d) #末端位置用12个参数表示,分别为姿态角旋转矩阵9参数,末端位置3参数 #写入方式为齐次矩阵前三行从左到右,从上到下 k = 0 for j_i in range(4): for j_j in range(3): pos[i, k] = T[j_j, j_i] k = k + 1 #存储关节角位置 print theta #FileOpen.write(theta,'data/randtheta500_data1.txt') #存储位置信息 print pos FileOpen.write(pos, 'data/randpos500_data2.txt') #绘制点云图 #MyPlot.plot3Point(pos[:,9],pos[:,10],pos[:,11])
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 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 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 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 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
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]
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")
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')
import MyPlot import numpy as np import ForwardKinematics as fk import FileOpen #七自由度机械臂D_H参数 theta0 = np.array([0, 180, 180, 180, 180, 180, 0]) * (np.pi / 180) #初始状态 a = np.zeros(7) alpha = np.array([90, 90, 90, 90, 90, 90, 0]) * (np.pi / 180) d = np.array([323.5, 0, 316, 0, 284.5, 0, 201]) * 0.001 #初始关节角 theta_k = np.zeros(7) #读取目标位置参数 data1 = FileOpen.read('data/randtheta_data.txt') data2 = FileOpen.read('data/wishCircle_theta1.txt') data3 = FileOpen.read('data/randpos_data.txt') data3 = np.array(data3) k = 0 data1 = np.array(data1) data2 = np.array(data2) [N, l] = data1.shape Theta1 = np.zeros([N, l]) #Theta2 = np.zeros([N,l]) x1 = np.zeros(N) y1 = np.zeros(N) z1 = np.zeros(N)
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")