def move_to_tcp(target_tcp): tool_acc = 1.2 # Safe: 0.5 tool_vel = 0.25 # Safe: 0.2 tool_pos_tolerance = [0.001, 0.001, 0.001, 0.05, 0.05, 0.05] tcp_command = "movel(p[%f,%f,%f,%f,%f,%f],a=%f,v=%f,t=0,r=0)\n" % ( target_tcp[0], target_tcp[1], target_tcp[2], target_tcp[3], target_tcp[4], target_tcp[5], tool_acc, tool_vel) tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) tcp_socket.connect((HOST, PORT)) tcp_socket.send( str.encode(tcp_command)) # 利用字符串的encode方法编码成bytes,默认为utf-8类型 tcp_socket.close() # 确保已达到目标点,就可以紧接着发送下一条指令 actual_pos = get_current_tcp() target_rpy = util.rv2rpy(target_tcp[3], target_tcp[4], target_tcp[5]) rpy = util.rv2rpy(actual_pos[3], actual_pos[4], actual_pos[5]) while not (all([ np.abs(actual_pos[j] - target_tcp[j]) < tool_pos_tolerance[j] for j in range(3) ]) and all([ np.abs(rpy[j] - target_rpy[j]) < tool_pos_tolerance[j + 3] for j in range(3) ])): actual_pos = get_current_tcp() rpy = util.rv2rpy(actual_pos[3], actual_pos[4], actual_pos[5]) time.sleep(0.01)
def increase_move(delta_x, delta_y, delta_z, delta_theta): tcp = get_current_tcp() rpy = util.rv2rpy(tcp[3], tcp[4], tcp[5]) rpy[2] = rpy[2] + delta_theta target_rv = util.rpy2rv(rpy) target_tcp = np.asarray([tcp[0] + delta_x, tcp[1] + delta_y, tcp[2] + delta_z, target_rv[0], target_rv[1], target_rv[2]]) move_to_tcp(target_tcp)
def get_current_pos_same_with_simulation(): tcp = get_current_tcp() rpy = util.rv2rpy(tcp[3], tcp[4], tcp[5]) return np.asarray([tcp[1], tcp[0], rpy[-1]])
def get_current_pos(): # x, y, theta tcp = get_current_tcp() rpy = util.rv2rpy(tcp[3], tcp[4], tcp[5]) return np.asarray([tcp[0], tcp[1], rpy[-1]])