# Conexión de hardware y determinación de punto objetivo
# --------------------------------------------------------------
print("\tConectando cámara...")
camara = cv2.VideoCapture(1)
print("\tConectando mano...")
mano = mf.Mano(camara, pos_inicial_pulgar="arriba")
print("\tListo!\n")
print("---------------------------------------------------------------------")
print("\tPosiciona el objeto (rojo). Luego, presiona enter.")
print("---------------------------------------------------------------------")
_ = input()
# Posición objetivo
imagen = mf.take_picture(camara)
# filtrada = mf.color_filter(imagen, "red")
# [r_objetivo, _] = mf.get_centroid(filtrada, method="contorno")
# Posición objetivo ahora es el contorno, no el centroide
r_objetivo = mf.determinar_contacto(imagen, "indice")
print("\n\tPunto objetivo: {}".format(r_objetivo))
# --------------------------------------------------------------
# Control
# --------------------------------------------------------------
ctrl = controladores.Ctrl_ERC(
dedo="indice",
cantidad_motores=cantidad_motores,
tercer_servo_auto=True,
def main(args):
print("Iniciando...")
# Argumentos
guardar_ex = args.explorar
guardar = args.guardar
extension = args.cargar
# =================================================
# Inicialización hardware e instanciación controlador
camara = cv2.VideoCapture(1)
mano = mf.Mano(camara)
ctrl = controladores.Ctrl_Pulgar()
if guardar_ex:
# Comienzo de la exploración exhaustiva
print("Exploración exhaustiva...")
ctrl.explorar_ex(camara, mano)
# Guardar las primitivas y terminar
primitivas = mf.primitivas_np2list(ctrl.primitivas.copy())
mf.guardar_datos("pulgar", primitivas)
return
# else:
datos = mf.cargar_datos("pulgar", extension)
# si no se pudo cargar datos, termina
if datos == None:
return
else:
ctrl.primitivas = datos
primitivas_base = len(datos)
print(
"---------------------------------------------------------------------"
)
print("\tPosiciona el objeto (rojo). Luego, presiona enter.")
print(
"---------------------------------------------------------------------"
)
_ = input()
# =================
# Posición objetivo
imagen = mf.take_picture(camara)
r_objetivo = mf.determinar_contacto(imagen, "pulgar", offset_hor=15)
print("Posición objetivo: {}".format(r_objetivo))
print(r_objetivo)
# ============================
# Primer movimiento de la mano
print("Calculando...")
# Hay contacto yema-objeto?
imagen = mf.take_picture(camara)
contacto, dist = mf.hay_contacto(imagen, "pulgar", kernel=[5, 7])
if not (contacto or (dist < 7)):
ctrl.mover(r_objetivo, camara, mano)
# OJO: la función mover del pulgar está pensado para
# mover el dedo sólo una vez y desde la posición inicial
# del dedo. Se asume que es suficiente, ya que su control
# es más sencillo (sólo se estima un servo). Luego hay que
# verificar si se alcanzó el objetivo (hay_contacto) y
# entrar en control_fino de ser necesario (muy probable,
# ya que el objetivo de primer movimiento no es el objeto
# sino un punto cercano a su contorno.
ctrl.ajuste_fino(camara, mano)
if guardar:
print("[DEBUG] Primitivas:\n")
print(ctrl.primitivas)
primitivas_trayectoria = ctrl.primitivas[primitivas_base:]
primitivas_trayectoria = mf.primitivas_np2list(primitivas_trayectoria)
# OJO: Las magnitudes, diferencias y posiciones se están
# guardando como listas en lugar de numpy arrays.
datos = {
"dedo": "pulgar",
"r_objetivo": [int(r_objetivo[0]),
int(r_objetivo[1])],
"primitivas": primitivas_trayectoria,
}
mf.guardar_datos(
nombre="pulgar",
datos=datos,
directory="trayectorias_1_dedo/",
)
def ajuste_fino(self, camara, mano):
"""Mueve el segundo servo del dedo en saltos de 1 o 3 grados
hasta que haya contacto entre la yema y el objeto."""
# Caso pulgar: sólo mover servo 2, cerrándose (sumando)
# Primitiva temporal guarda el "movimiento fino"
while True:
# Hay contacto yema-objeto?
imagen = mf.take_picture(camara)
contacto, dist = mf.hay_contacto(imagen, "pulgar", kernel=[5, 7])
# Si se alcanza el objetivo, termina
if contacto or (dist < 7):
print("Objetivo alcanzado por dedo pulgar")
break
else:
# Un aumento de x grados se logra con -x/m
grados = 1
print("[DEBUG] {} grados más...".format(grados))
# Nueva primitiva temporal
primitiva_temp = {}
# Posición actual de la yema en la imagen
ri = mano.actualizar_yema(camara, "pulgar")
primitiva_temp["pos_inicial"] = ri
# Posición actual del dedo
dedo_pos = mano.entregar_dedo_pos("pulgar")
# Actualización servo 2
magnitud = grados / self.m_n2a
dedo_pos[1] += int(magnitud)
primitiva_temp["magnitud"] = magnitud
# Compensación servo 3
magnitud = -grados / self.m_n2a
dedo_pos[2] += int(magnitud)
# Ajuste y movimiento
_, _, _ = mano.ajustar_dedo("pulgar", dedo_pos)
mano.mover()
# Posición final de la yema en la imagen
rf = mano.actualizar_yema(camara, "pulgar")
primitiva_temp["pos_final"] = rf
# Diferencia en la imagen
primitiva_temp["diferencia"] = rf - ri
# Guardar primitiva y borrar para no sobreescribir
primitiva_temp["ajuste_fino"] = True
self.primitivas.append(primitiva_temp)
del primitiva_temp
# Un último movimiento (de 1°)
primitiva_temp = {}
# Posición actual de la yema en la imagen
ri = mano.actualizar_yema(camara, "pulgar")
primitiva_temp["pos_inicial"] = ri
magnitud = 1 / self.m_n2a
primitiva_temp["magnitud"] = int(magnitud)
dedo_pos = mano.entregar_dedo_pos("pulgar")
dedo_pos[1] += int(magnitud)
_, _, _ = mano.ajustar_dedo("pulgar", dedo_pos)
mano.mover()
# Posición final de la yema en la imagen
rf = mano.actualizar_yema(camara, "pulgar")
primitiva_temp["pos_final"] = rf
# Diferencia en la imagen
primitiva_temp["diferencia"] = rf - ri
# Guardar la última primitiva
primitiva_temp["ajuste_fino"] = True
self.primitivas.append(primitiva_temp)
def ajuste_fino(self, camara, mano):
"""Para mover los dedos luego de que se considere
alcanzado el objetivo (en primera instancia). Esta
función asegura el contacto entre yema y objeto."""
# Caso índice: sólo mover servo 1, cerrándose (restando)
while True:
# Hay contacto yema-objeto?
imagen = mf.take_picture(camara)
contacto, dist = mf.hay_contacto(imagen, self.dedo)
# Si se alcanza el objetivo, termina
if contacto:
print("Objetivo alcanzado por dedo {}".format(self.dedo))
break
if dist > 7:
# Un aumento de x grados se logra con -x/m
grados = 3
else:
# Un aumento de x grados se logra con -x/m
grados = 1
print("[DEBUG] {} grados más...".format(grados))
# Nueva primitiva temporal
primitiva_temp = {}
# Posición actual de la yema en la imagen
ri = mano.actualizar_yema(camara, "indice")
primitiva_temp["pos_inicial"] = ri
# Posición actual del dedo
dedo_pos = mano.entregar_dedo_pos(self.dedo)
# Actualización servo 1
magnitud = -grados / self.m_n2a_s1
dedo_pos[0] += int(magnitud)
primitiva_temp["magnitud"] = magnitud
# Compensación tercer servo
magnitud = grados / self.m_n2a_s2
dedo_pos[2] += int(magnitud)
# Ajuste y movimiento
_, _, _ = mano.ajustar_dedo(self.dedo, dedo_pos)
mano.mover()
# Posición final de la yema en la imagen
rf = mano.actualizar_yema(camara, "indice")
primitiva_temp["pos_final"] = rf
# Diferencia en la imagen
primitiva_temp["diferencia"] = rf - ri
# Guardar primitiva y borrar para no sobreescribir
primitiva_temp["ajuste_fino"] = True
self.primitivas.append(primitiva_temp)
del primitiva_temp
# Un último movimiento (de 1°)
primitiva_temp = {}
# Posición actual de la yema en la imagen
ri = mano.actualizar_yema(camara, "indice")
primitiva_temp["pos_inicial"] = ri
magnitud = -1 / self.m_n2a_s1
dedo_pos = mano.entregar_dedo_pos(self.dedo)
dedo_pos[0] += int(magnitud)
_, _, _ = mano.ajustar_dedo(self.dedo, dedo_pos)
mano.mover()
# Posición final de la yema en la imagen
rf = mano.actualizar_yema(camara, "indice")
primitiva_temp["pos_final"] = rf
# Diferencia en la imagen
primitiva_temp["diferencia"] = rf - ri
# Guardar la última primitiva
primitiva_temp["ajuste_fino"] = True
self.primitivas.append(primitiva_temp)
# La idea es separar dos objetos que se ven del mismo
# color en la imagen (dos yemas verdes)
import mis_funciones as mf
import numpy as np
import cv2
camara = cv2.VideoCapture(1)
for i in range(20):
image = mf.take_picture(camara)
filtrada = mf.color_filter(image, "green")
r = mf.get_centroid(filtrada)
# Máscara para obtener uno de los dos objetos
mask_izq = np.zeros(filtrada.shape[:2], dtype="uint8")
mask_izq[:, :r[0]] = 255
mask_der = cv2.bitwise_not(mask_izq)
objeto_izq = cv2.bitwise_and(filtrada, mask_izq)
objeto_der = cv2.bitwise_and(filtrada, filtrada, mask=mask_der)
print("Foto #{:02d}".format(i))
# cv2.imshow("mask_izq", mask_izq)
# cv2.imshow("mask_der", mask_der)
# cv2.imshow("Foto", image)
cv2.imshow("Objeto Izquierdo", objeto_izq)
cv2.imshow("Objeto Derecho", objeto_der)
cv2.waitKey(1000)
def main(args):
print("Iniciando...")
# Argumentos
guardar = args.guardar
extension_indice = args.cargar_indice
extension_pulgar = args.cargar_pulgar
# =================================================
# Inicialización hardware e instanciación controlador
camara = cv2.VideoCapture(1)
mano = mf.Mano(
camara,
pos_inicial_pulgar="arriba",
pos_inicial_indice="estirado",
)
ctrl_indice = controladores.Ctrl_ERC(
dedo="indice",
tercer_servo_auto=True,
)
ctrl_pulgar = controladores.Ctrl_Pulgar()
# NO HAY EXPLORACION EXHAUSTIVA EN ESTE SCRIPT
# Cargar datos de primitivas en ambos dedos
datos_indice = mf.cargar_datos("indice", extension_indice)
datos_pulgar = mf.cargar_datos("pulgar", extension_pulgar)
# si no se pudo cargar datos, termina
if (datos_pulgar == None) or (datos_indice == None):
print("No se pudo cargar datos...")
return
else:
ctrl_indice.primitivas = datos_indice
ctrl_pulgar.primitivas = datos_pulgar
primitivas_base_indice = len(datos_indice)
primitivas_base_pulgar = len(datos_pulgar)
print(
"---------------------------------------------------------------------"
)
print("\tPosiciona el objeto (rojo). Luego, presiona enter.")
print(
"---------------------------------------------------------------------"
)
_ = input()
# =================
# Posición objetivo
imagen = mf.take_picture(camara)
r_objetivo_pulgar = mf.determinar_contacto(imagen, "pulgar", offset_hor=15)
r_objetivo_indice = mf.determinar_contacto(imagen, "indice")
print("Posición objetivo indice: {}".format(r_objetivo_indice))
print("Posición objetivo pulgar: {}".format(r_objetivo_pulgar))
if (r_objetivo_indice == False) or (r_objetivo_indice == False):
return 0
# ============================
# Primer movimiento de la mano
print("Calculando...")
# ===============================================
# Hay contacto yema-objeto?
imagen = mf.take_picture(camara)
contacto, dist = mf.hay_contacto(imagen, "pulgar", kernel=[5, 7])
# Mover pulgar al objetivo aproximado
if not (contacto or (dist < 7)):
ctrl_pulgar.mover(r_objetivo_pulgar, camara, mano)
# Mover indice al objetivo aproximado
while not ctrl_indice.flags["_alcanzado_"]:
# Ya no se necesita el flag de singular
_ = ctrl_indice.calcular_primitiva(
r_objetivo_indice,
camara,
mano,
)
# Verificación de objetivo alcanzado por medio de hay_contacto
imagen = mf.take_picture(camara)
_, dist = mf.hay_contacto(imagen, "indice")
if dist < 30:
break
# Si no se pudo calcular la primitiva: explorar (paso = 1)
if ctrl_indice.flags["_explorar_"]:
ctrl_indice.explorar(camara, mano)
estado = ctrl_indice.evaluar_estado(r_objetivo_indice)
print("Estado: {}".format(estado))
# Mover pulgar al objetivo final (contorno del objeto)
print(" ")
print("Ajustando dedo pulgar...")
ctrl_pulgar.ajuste_fino(camara, mano)
# Mover indice al objetivo final (contorno del objeto)
print(" ")
print("Ajustando dedo indice...")
ctrl_indice.ajuste_fino(camara, mano)
if guardar:
# print("[DEBUG] Primitivas:\n")
# print(ctrl.primitivas)
primitivas_tray_indice = ctrl_indice.primitivas[
primitivas_base_indice:]
primitivas_tray_indice = mf.primitivas_np2list(primitivas_tray_indice)
primitivas_tray_pulgar = ctrl_pulgar.primitivas[
primitivas_base_pulgar:]
primitivas_tray_pulgar = mf.primitivas_np2list(primitivas_tray_pulgar)
# OJO: Las magnitudes, diferencias y posiciones se están
# guardando como listas en lugar de numpy arrays.
datos_pulgar = {
"dedo": "pulgar",
"r_objetivo":
[int(r_objetivo_pulgar[0]),
int(r_objetivo_pulgar[1])],
"primitivas": primitivas_tray_pulgar,
}
datos_indice = {
"dedo": "indice",
"r_objetivo":
[int(r_objetivo_indice[0]),
int(r_objetivo_indice[1])],
"tercer_servo_auto": ctrl_indice.tercer_servo_auto,
"cantidad_motores": ctrl_indice.cantidad_motores,
"primitivas": primitivas_tray_indice,
}
mf.guardar_datos(
nombre="indice_pulgar",
datos=[datos_indice, datos_pulgar],
directory="trayectorias_2_dedos/",
)
camara.release()
def main(args):
guardar_ex = args.explorar
guardar = args.guardar
extension = args.cargar
# Cantidad de motores
if args.motores == None:
cantidad_motores = 2
else:
cantidad_motores = int(args.motores)
# --------------------------------------------------------------
# Conexión de hardware y determinación de punto objetivo
# --------------------------------------------------------------
print("\tConectando cámara...")
camara = cv2.VideoCapture(1)
print("\tConectando mano...")
mano = mf.Mano(camara,
pos_inicial_pulgar="arriba",
pos_inicial_indice="estirado",
)
# Controlador
ctrl = controladores.Ctrl_ERC(dedo="indice",
cantidad_motores=cantidad_motores,
tercer_servo_auto=True,
)
if guardar_ex:
# Comienzo de la exploración exhaustiva
print("Exploración exhaustiva...")
ctrl.explorar_ex(camara, mano)
# Guardar las primitivas y terminar
primitivas = mf.primitivas_np2list(ctrl.primitivas.copy())
mf.guardar_datos("indice", primitivas)
return
# Cargar datos anteriores
datos = mf.cargar_datos("indice", extension)
# si no se pudo cargar datos, termina
if datos == None:
print("[ERROR]: No se pudo cargar datos. Revisar existencia de archivo")
print("Programa terminado!")
return
else:
ctrl.primitivas = datos
primitivas_base = len(datos)
print("\tListo!\n")
print("---------------------------------------------------------------------")
print("\tPosiciona el objeto (rojo). Luego, presiona enter.")
print("---------------------------------------------------------------------")
_ = input()
# Posición objetivo
imagen = mf.take_picture(camara)
# filtrada = mf.color_filter(imagen, "red")
# [r_objetivo, _] = mf.get_centroid(filtrada, method="contorno")
# Posición objetivo ahora es el contorno, no el centroide
r_objetivo = mf.determinar_contacto(imagen, "indice")
print("\n\tPunto objetivo: {}".format(r_objetivo))
# Loop de cálculo de primitivas
while not ctrl.flags["_alcanzado_"]:
# Ya no se necesita el flag de singular
_ = ctrl.calcular_primitiva(r_objetivo, camara, mano)
estado = ctrl.evaluar_estado(r_objetivo)
print("Estado: {}".format(estado))
# Verificar si la yema está cerca del objeto
imagen = mf.take_picture(camara)
_, dist = mf.hay_contacto(imagen, "indice")
if dist < 30:
break
# Si no se pudo calcular la primitiva: explorar (paso = 1)
if ctrl.flags["_explorar_"]:
ctrl.explorar(camara, mano)
estado = ctrl.evaluar_estado(r_objetivo)
print("Estado: {}".format(estado))
# Verificar si la yema está cerca del objeto
imagen = mf.take_picture(camara)
_, dist = mf.hay_contacto(imagen, "indice")
if dist < 30:
break
# ========================================================
# Ajuste "fino"
print(" ")
print("Ajustando dedo...")
ctrl.ajuste_fino(camara, mano)
# ========================================================
# Guardar datos al finalizar
if guardar:
# JSON no permite guardar arreglos de numpy por lo que
# hay que convertirlos a lista antes de guardarlos.
primitivas = ctrl.primitivas[primitivas_base:]
primitivas = mf.primitivas_np2list(primitivas)
# OJO: Las magnitudes, diferencias y posiciones se están
# guardando como listas en lugar de numpy arrays.
datos = {"dedo": ctrl.dedo,
"r_objetivo": [int(r_objetivo[0]),int(r_objetivo[1])],
"tercer_servo_auto": ctrl.tercer_servo_auto,
"cantidad_motores": ctrl.cantidad_motores,
"primitivas": primitivas
}
mf.guardar_datos(nombre="indice",
datos=datos,
directory="trayectorias_1_dedo/",
)
camara.release()