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