def main(argv=sys.argv): # dynamixel parameters for communication {initialization} port_name = 'COM5' p_d = parameters.param_dyn(port_name) # serial port communication s_p = communication.Serial_Port(p_d.PORT_NAME, p_d.BAUDRATE) # controll dynamixel AX-12+ dynamixel_id = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18] dC_AX_12p = [] #dynamixel_id = [3, 4] for init_d in range(0, len(dynamixel_id)): # initial AX-12+ aux_dyn = dyn_ax_12p.dynamixel_AX12P(dynamixel_id[init_d], s_p.serial_port) dC_AX_12p.append(aux_dyn) #for i_temp in range(0, len(dC_AX_12p)): # call functions {read} #present_temperature(p_d, dC_AX_12p[i_temp], dynamixel_id[i_temp]) # call functions {write} goal_speed = 300 for i_speed in range(0, len(dC_AX_12p)): set_speed(p_d, dC_AX_12p[i_speed], dynamixel_id[i_speed], goal_speed) goal_pos = [204, 818, 250, 770, 512, 512, 358, 666, 512, 512, 444, 578, 375, 646, 599, 423, 512, 512] #goal_pos = [500, 500] for i_pos in range(0, len(dC_AX_12p)): set_position(p_d, dC_AX_12p[i_pos], dynamixel_id[i_pos], goal_pos[i_pos]) """ counter_1 = 0 while counter_1 < len(goal_pos): set_position(p_d, dC_AX_12p[init_d], dynamixel_id[counter_1], goal_pos[counter_1]) is_mM = is_motor_moving(p_d, dC_AX_12p[init_d], dynamixel_id[counter_1]) if is_mM == 0: counter_1 = counter_1 + 1 """ # exit communication if s_p.close_serialPort(): print('Serial port communication terminated.')
def main(argv=sys.argv): # dynamixel parameters for communication {initialization} port_name = 'COM4' p_d = parameters.param_dyn(port_name) # serial port communication s_p = communication.Serial_Port(p_d.PORT_NAME, p_d.BAUDRATE) # controll dynamixel AX-12+ dynamixel_id_1 = 1 # initial AX-12+ dC_AX_12p = dyn_ax_12p.dynamixel_AX12P(dynamixel_id_1, s_p.serial_port) # call functions present_temperature(p_d, dC_AX_12p, dynamixel_id_1) goal_pos = 512 set_position(p_d, dC_AX_12p, dynamixel_id_1, goal_pos) # exit communication if s_p.close_serialPort(): print('Serial port communication terminated.')
result = dC_AX_12p.result # release of variables dC_AX_12p.release_packet_param_AX_12p() else: print('Error!') return int(result) # define global class for scale global scale_hsv # initialization class for scale scale_hsv = HSV_init_scale() port_name = 'COM4' p_d = parameters.param_dyn(port_name) # serial port communication s_p = communication.Serial_Port(p_d.PORT_NAME, p_d.BAUDRATE) dynamixel_id = [1] dC_AX_12p = [] for init_d in range(0, len(dynamixel_id)): # initial AX-12+ aux_dyn = dyn_ax_12p.dynamixel_AX12P(dynamixel_id[init_d], s_p.serial_port) dC_AX_12p.append(aux_dyn) goal_speed = 50 for i_speed in range(0, len(dC_AX_12p)): set_speed(p_d, dC_AX_12p[i_speed], dynamixel_id[i_speed], goal_speed)
def mult_obj_tracker(device, frame_width, frame_height): # parameters for drawing {plot} COUNT_OF_OBJECTS = 2 # each color # identical to the frame size MIN_X = 0 MAX_X = 640 MIN_Y = 480 MAX_Y = 0 plt, fig, ax = init() fig.set_size_inches(12.5, 9.5) fig.canvas.manager.window.wm_geometry("+%d+%d" % (0, 0)) ax.set_title('Bioloid King-Spider {opencv control}', fontsize=14, fontweight='bold') ax.set_xlabel('Dimension X', fontsize=10, fontweight='bold') ax.set_ylabel('Dimension Y', fontsize=10, fontweight='bold') # x,y limitation # HSV - coordinates of the detected object lower_hsv = { 'green': (17, 71, 78), } upper_hsv = { 'green': (27, 255, 255), } # initialization colors for ectangle around the object # initialization colors for circle and rectangle around the object colors_cr = { 'green': (0, 255, 0), } # start recording mult_t = cv2.VideoCapture(device) if not (mult_t.isOpened()): mult_t.open(device) # set frame parameters mult_t.set(frame_width, 640) mult_t.set(frame_height, 480) port_name = 'COM4' p_d = parameters.param_dyn(port_name) # serial port communication s_p = communication.Serial_Port(p_d.PORT_NAME, p_d.BAUDRATE) # controll dynamixel AX-12+ dynamixel_id = [ 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18 ] dC_AX_12p = [] for init_d in range(0, len(dynamixel_id)): # initial AX-12+ aux_dyn = dyn_ax_12p.dynamixel_AX12P(dynamixel_id[init_d], s_p.serial_port) dC_AX_12p.append(aux_dyn) # call functions {write} goal_speed = 50 for i_speed in range(0, len(dC_AX_12p)): set_speed(p_d, dC_AX_12p[i_speed], dynamixel_id[i_speed], goal_speed) # dynamixel g_H = [] count_g_H = 0 position_state_H = 0 g_F = [] count_g_F = 0 position_state_F = 0 g_B = [] count_g_B = 0 position_state_B = 0 g_TL = [] count_g_TL = 0 position_state_TL = 0 g_TR = [] count_g_TR = 0 position_state_TR = 0 goal_pos = [ 459, 575, 444, 590, 513, 512, 514, 514, 592, 422, 504, 457, 532, 369, 605, 411, 526, 441 ] # initialization approx. param green_approx_x = 0 green_approx_y = 0 command = '' while True: _, frame_mT = mult_t.read() # convert to grayscale hsv = cv2.cvtColor(frame_mT, cv2.COLOR_BGR2HSV) # Gaussian Blur {filter} blur = cv2.GaussianBlur(hsv, (5, 5), 0) # ============================== detection object ============================== for color_k, _ in upper_hsv.items(): # initialization matrix {kernel} kernel_m = np.ones((5, 5), np.uint8) # comparison mask_m = cv2.inRange(blur, lower_hsv[color_k], upper_hsv[color_k]) # open structure {kernel matrix} -> MORPH_OPEN open_morph = cv2.morphologyEx(mask_m, cv2.MORPH_OPEN, kernel_m) # find contours res_cnts = cv2.findContours(open_morph.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2] obj_center = None for obj_num, obj_contour in enumerate(res_cnts): # calculation object perimeter obj_perimeter = cv2.arcLength(obj_contour, True) approx = cv2.approxPolyDP(obj_contour, 0.02 * obj_perimeter, True) if 6 < len(approx) < 15: # calculation area of the object obj_area = cv2.contourArea(obj_contour) # calculation circularity: formula {4 * pi * Area/(perimeter^2)} obj_circ = ((4 * math.pi * obj_area) / (obj_perimeter * obj_perimeter)) # condition of size {area} -> object if (50 < obj_area < 3000) and (0.3 < obj_circ < 1.5): # detect coordinates of the object contour {circle} (x_c, y_c), obj_radius = cv2.minEnclosingCircle(obj_contour) obj_center = (int(x_c), int(y_c)) obj_radius = int(obj_radius) # detect coordinates of the object contour {rectangle} x_r, y_r, obj_w, obj_h = cv2.boundingRect(obj_contour) # draw rectangle and circle obj_rect = cv2.rectangle(frame_mT, (x_r, y_r), (x_r + obj_w, y_r + obj_h), colors_cr[color_k], 2) cv2.circle(obj_rect, obj_center, int(obj_radius), colors_cr[color_k], 2) # initialization string for the object {text} str_aux_x = 'Dx:' + str(int(x_c)) str_aux_y = 'Dy:' + str(int(y_c)) # adding text {coordinates} cv2.putText(obj_rect, str_aux_x, (int(x_r - 0.5), int(y_r - 20)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, colors_cr[color_k], 2) cv2.putText(obj_rect, str_aux_y, (int(x_r - 0.5), int(y_r - 5)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, colors_cr[color_k], 2) if color_k == 'green': if abs(green_approx_x - int(x_r)) <= 1 or abs(green_approx_y - int(x_r)) <= 1: plt, command = calculation( plt, fig, ax, green_approx_x, 480 - green_approx_y) else: green_approx_x = int(x_r) green_approx_y = int(y_r) if command == 'SIT_DOWN': goal_pos = [ 381, 648, 227, 826, 283, 742, 523, 498, 818, 211, 786, 269, 656, 349, 779, 250, 725, 293 ] for i_pos in range(0, len(dC_AX_12p)): set_position(p_d, dC_AX_12p[i_pos], dynamixel_id[i_pos], goal_pos[i_pos]) print('1') elif command == 'HOME': goal_pos = [ 459, 575, 444, 590, 513, 512, 514, 514, 592, 422, 504, 457, 532, 369, 605, 411, 526, 441 ] for i_pos in range(0, len(dC_AX_12p)): set_position(p_d, dC_AX_12p[i_pos], dynamixel_id[i_pos], goal_pos[i_pos]) print('2') elif command == 'STAND_UP': goal_pos = [ 507, 534, 649, 367, 638, 344, 508, 469, 292, 634, 303, 651, 585, 358, 373, 633, 385, 636 ] for i_pos in range(0, len(dC_AX_12p)): set_position(p_d, dC_AX_12p[i_pos], dynamixel_id[i_pos], goal_pos[i_pos]) print('3') elif command == 'LEGS_RIGHT': goal_pos = [ 459, 575, 319, 589, 764, 512, 491, 413, 591, 422, 505, 457, 631, 369, 719, 411, 304, 440 ] for i_pos in range(0, len(dC_AX_12p)): set_position(p_d, dC_AX_12p[i_pos], dynamixel_id[i_pos], goal_pos[i_pos]) print('4') elif command == 'LEGS_LEFT': goal_pos = [ 459, 575, 443, 719, 511, 322, 581, 560, 606, 421, 504, 456, 632, 369, 605, 321, 526, 701 ] for i_pos in range(0, len(dC_AX_12p)): set_position(p_d, dC_AX_12p[i_pos], dynamixel_id[i_pos], goal_pos[i_pos]) print('5') # show the result in the main frame cv2.imshow("Multiple object tracker {Main frame}", frame_mT) # ending condition if cv2.waitKey(1) & 0xFF == ord('q'): return mult_t break
def main(argv=sys.argv): port_name = 'COM4' p_d = parameters.param_dyn(port_name) # serial port communication s_p = communication.Serial_Port(p_d.PORT_NAME, p_d.BAUDRATE) # controll dynamixel AX-12+ dynamixel_id = [ 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18 ] dC_AX_12p = [] for init_d in range(0, len(dynamixel_id)): # initial AX-12+ aux_dyn = dyn_ax_12p.dynamixel_AX12P(dynamixel_id[init_d], s_p.serial_port) dC_AX_12p.append(aux_dyn) # call functions {write} goal_speed = 75 for i_speed in range(0, len(dC_AX_12p)): set_speed(p_d, dC_AX_12p[i_speed], dynamixel_id[i_speed], goal_speed) # home pos goal_pos = [ 459, 575, 444, 590, 513, 512, 581, 414, 592, 422, 504, 457, 632, 369, 605, 411, 526, 441 ] goal_posTRGH = [[ 372, 610, 267, 512, 268, 512, 512, 509, 536, 256, 510, 291, 650, 364, 739, 513, 735, 523 ], [ 372, 734, 267, 512, 268, 512, 611, 509, 536, 256, 510, 291, 650, 405, 739, 513, 735, 523 ], [ 372, 734, 494, 750, 495, 733, 611, 509, 764, 495, 763, 483, 650, 405, 517, 253, 522, 259 ], [ 372, 655, 494, 750, 495, 733, 519, 509, 764, 495, 763, 483, 650, 329, 517, 253, 522, 259 ]] goal_posTLFT = [[ 414, 652, 512, 757, 512, 756, 515, 512, 760, 480, 733, 514, 660, 374, 511, 285, 501, 289 ], [ 290, 652, 512, 757, 512, 756, 515, 413, 760, 480, 733, 514, 619, 374, 511, 285, 501, 289 ], [ 290, 652, 274, 530, 291, 529, 515, 413, 521, 252, 541, 261, 619, 374, 771, 507, 765, 502 ], [ 369, 652, 274, 530, 291, 529, 515, 505, 521, 252, 541, 261, 695, 374, 771, 507, 765, 502 ]] goal_posWFWD = [[ 458, 584, 469, 767, 517, 733, 326, 569, 803, 414, 729, 411, 581, 443, 490, 236, 596, 410 ], [ 429, 575, 521, 739, 431, 610, 489, 730, 801, 390, 741, 412, 585, 447, 617, 203, 551, 307 ], [ 440, 566, 257, 555, 291, 507, 455, 698, 602, 213, 613, 289, 581, 443, 788, 534, 614, 428 ], [ 449, 595, 285, 503, 414, 593, 294, 535, 626, 215, 612, 274, 577, 439, 821, 407, 717, 473 ]] goal_posWBKW = [[ 458, 584, 469, 767, 517, 733, 326, 569, 803, 414, 729, 411, 581, 443, 490, 236, 596, 410 ], [ 449, 595, 285, 503, 414, 593, 294, 535, 626, 215, 612, 274, 577, 439, 821, 407, 717, 473 ], [ 440, 566, 257, 555, 291, 507, 455, 698, 602, 213, 613, 289, 581, 443, 788, 534, 614, 428 ], [ 429, 575, 521, 739, 431, 610, 489, 730, 801, 390, 741, 412, 585, 447, 617, 203, 551, 307 ]] for i_pos in range(0, len(dC_AX_12p)): set_position(p_d, dC_AX_12p[i_pos], dynamixel_id[i_pos], goal_pos[i_pos]) g = [] count_g = 0 position_state = 0 while 1: for i_presPos in range(0, len(dC_AX_12p)): a = is_motor_moving(p_d, dC_AX_12p[i_presPos], dynamixel_id[i_presPos]) g.append(a) for i_g in range(0, len(dC_AX_12p)): if g[i_g] == 0: count_g += 1 g = [] if count_g == 18: # call functions {write} goal_speed = 600 for i_speed in range(0, len(dC_AX_12p)): set_speed(p_d, dC_AX_12p[i_speed], dynamixel_id[i_speed], goal_speed) goal_pos = goal_pos[position_state] for i_pos in range(0, len(dC_AX_12p)): set_position(p_d, dC_AX_12p[i_pos], dynamixel_id[i_pos], goal_pos[i_pos]) if position_state == 3: position_state = 0 else: position_state += 1 else: print(count_g) count_g = 0 # exit communication if s_p.close_serialPort(): print('Serial port communication terminated.')
def mult_obj_tracker(device, frame_width, frame_height): # dynamixel parameters for communication {initialization} port_name = 'COM4' p_d = parameters.param_dyn(port_name) # serial port communication s_p = communication.Serial_Port(p_d.PORT_NAME, p_d.BAUDRATE) # controll dynamixel AX-12+ dC_AX_12p = [] dynamixel_id = [3, 4] for init_d in range(0, len(dynamixel_id)): # initial AX-12+ aux_dyn = dyn_ax_12p.dynamixel_AX12P(dynamixel_id[init_d], s_p.serial_port) dC_AX_12p.append(aux_dyn) # call functions {write speed} goal_speed = 300 for i_speed in range(0, len(dC_AX_12p)): set_speed(p_d, dC_AX_12p[i_speed], dynamixel_id[i_speed], goal_speed) # identical to the frame size MIN_X = 0 MAX_X = 640 MIN_Y = 480 MAX_Y = 0 # number of individual colors for input -> ['blue', 'green'] b_tObj = object_draw_bioloid.tracking_bioloid(1, 1) # set maximum limits {x, y} of frame b_tObj.axis_init(640, 480) # initialization triangles b_tObj.init_triangles() # HSV - coordinates of the detected object lower_hsv = {'green': (17, 71, 78), 'blue': (97, 63, 34)} upper_hsv = {'green': (27, 255, 255), 'blue': (164, 223, 184)} # initialization colors for ectangle around the object # initialization colors for circle and rectangle around the object colors_cr = {'green': (0, 255, 0), 'blue': (255, 0, 0)} # start recording mult_t = cv2.VideoCapture(device) if not (mult_t.isOpened()): mult_t.open(device) # set frame parameters mult_t.set(frame_width, 640) mult_t.set(frame_height, 480) while True: _, frame_mT = mult_t.read() hsv = cv2.cvtColor(frame_mT, cv2.COLOR_BGR2HSV) blur = cv2.GaussianBlur(hsv, (5, 5), 0) # ============================== detection object ============================== for color_k, _ in upper_hsv.items(): # initialization matrix {kernel} kernel_m = np.ones((7, 7), np.uint8) # comparison mask_m = cv2.inRange(blur, lower_hsv[color_k], upper_hsv[color_k]) # open structure {kernel matrix} -> MORPH_OPEN open_morph = cv2.morphologyEx(mask_m, cv2.MORPH_OPEN, kernel_m) # find contours res_cnts = cv2.findContours(open_morph.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2] obj_center = None for obj_num, obj_contour in enumerate(res_cnts): # calculation object perimeter obj_perimeter = cv2.arcLength(obj_contour, True) approx = cv2.approxPolyDP(obj_contour, 0.02 * obj_perimeter, True) if 6 < len(approx) < 15: # calculation area of the object obj_area = cv2.contourArea(obj_contour) # calculation circularity: formula {4 * pi * Area/(perimeter^2)} obj_circ = ((4 * math.pi * obj_area) / (obj_perimeter * obj_perimeter)) # condition of size {area} -> object if (5 < obj_area < 1000) and (0.3 < obj_circ < 1.5): # detect coordinates of the object contour {circle} (x_c, y_c), obj_radius = cv2.minEnclosingCircle(obj_contour) obj_center = (int(x_c), int(y_c)) obj_radius = int(obj_radius) # detect coordinates of the object contour {rectangle} x_r, y_r, obj_w, obj_h = cv2.boundingRect(obj_contour) # draw rectangle and circle obj_rect = cv2.rectangle(frame_mT, (x_r, y_r), (x_r + obj_w, y_r + obj_h), colors_cr[color_k], 2) cv2.circle(obj_rect, obj_center, int(obj_radius), colors_cr[color_k], 2) # initialization string for the object {text} str_aux_x = 'Dx:' + str(int(x_c)) str_aux_y = 'Dy:' + str(int(y_c)) # adding text {coordinates} cv2.putText(obj_rect, str_aux_x, (int(x_r - 0.5), int(y_r - 20)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, colors_cr[color_k], 2) cv2.putText(obj_rect, str_aux_y, (int(x_r - 0.5), int(y_r - 5)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, colors_cr[color_k], 2) if color_k == 'blue' and int(x_c) > 340: b_tObj.loop_draw_object(int(x_c), int(y_c), color_k, 0) elif color_k == 'green' and int(x_c) < 260: b_tObj.loop_draw_object(int(x_c), int(y_c), color_k, 0) # calculation angle b_tObj.calculation_of_robot_output() # set position {right hand} is_mM_dx_Id3 = is_motor_moving(p_d, dC_AX_12p[0], dynamixel_id[0]) if is_mM_dx_Id3 == 0: set_position(p_d, dC_AX_12p[0], dynamixel_id[0], b_tObj.pos_rh) # set position {left hand} is_mM_dx_Id4 = is_motor_moving(p_d, dC_AX_12p[1], dynamixel_id[1]) if is_mM_dx_Id4 == 0: set_position(p_d, dC_AX_12p[1], dynamixel_id[1], b_tObj.pos_lh) # show the result in the main frame cv2.imshow("Bioloid Humanoid", frame_mT) # ending condition if cv2.waitKey(1) & 0xFF == ord('q'): break # exit communication if s_p.close_serialPort(): print('Serial port communication terminated.') # output return mult_t