Mapeo 3D de un objeto con sensor de fuerza y ServoCart (SDK de Python)

TEN EN CUENTA QUE ESTE CÓDIGO ES UN EJEMPLO O PLANTILLA Y NO DEBE CORRERSE TAL CUAL. ÚSALO COMO BASE Y AJÚSTALO PARA GARANTIZAR LA SEGURIDAD Y LA COMPATIBILIDAD CON TU ROBOT.

Lo que necesitas para correrlo:

- Un robot Fairino (este código se escribió con un FR5; los valores articulares y las posiciones cartesianas cambian según el modelo)

- Un sensor de fuerza/torque XJC

- Cualquier herramienta de extremo de brazo con una punta de precisión para palpar

- Paquetes de Python: numpy, matplotlib, time y el SDK de Fairino (disponible en la página de documentación de Fairino)

El programa principal (Grid_test.py) hace lo siguiente:

- A partir de una posición articular inicial, un tamaño y un número de ciclos (palpados por fila y por columna), se mueve a un punto suspendido sobre el objeto (los desplazamientos se calculan sobre el sistema de coordenadas de la base).

- Desde ese punto usa movimientos servoCart() para acercarse lentamente hasta encontrar una fuerza de resistencia: un contacto controlado con el objeto.

- Registra el punto de detección y manda ese dato a un archivo .txt.

El graficador (joint_torque_grapher.py) hace esto:

- Lee y procesa los valores de los archivos .txt.

- Muestra el resultado como un mapa de calor 3D con la forma aproximada del objeto palpado; la precisión depende del número de ciclos y del tamaño del mapa, como se ve abajo.

Código del programa principal de palpado (escrito para la versión 3.8.2):

from fairino import Robot
import time
START_POS = [29.74836540222168, -134.0897674560547, -89.1602554321289, -46.7747802734375, 89.99434661865234, 29.713558197021484]
START_CART = [459.6044006347656, 145.47499084472656, -23.17245864868164, 179.98126220703125, -0.017205478623509407, -89.9651870727539]
length = 200 # Length/width of a square grid in mm
def get_z_force(robot: Robot.RPC):
 e, ft = robot.FT_GetForceTorqueRCS()
 if e != 0:
 print("Error getting force/torque:", e)
 return False
 return ft[2] # Z-axis force
def make_grid(robot: Robot.RPC, loops=20):
 file_object = open(f"{loops}x{loops}_data.txt", "w") # open graph data file
 robot.SetSpeed(40) # Set speed to 40%
 # Loop over specified grid size and probe for depth
 for i in range(loops): 
 for j in range(loops):
 robot.MoveL(START_CART, tool=1, user=0, vel=100, offset_flag=1, offset_pos=[(i -(length / loops)), (j -(length / loops)), 0, 0, 0, 0]) # Go to probe prep
 time.sleep(0.5)
 # Use servo cart to slowly lower the tool until force is detected
 for _ in range(400):
 robot.ServoCart(1, [0, 0,-0.15, 0, 0, 0], vel=100) 
 time.sleep(0.008)
 fz = get_z_force(robot)
 if( fz is not False):
 if(abs(fz) > 0.5):
 coord = robot.GetActualTCPPose()[1][:3] # Get current TCP position of contact point
 file_object.write(f"{[i for i in coord]} \n".replace('[', '').replace(']', '')) # Write to file
 print(f"Force detected: {fz} at position {coord}")
 break
 else:
 return False
 robot.MoveL(robot.GetActualTCPPose()[1], tool=1, user=0, vel=100, offset_flag=1, offset_pos=[0, 0, 50, 0, 0, 0]) # Retract 50mm
def main():
 robot = Robot.RPC('192.168.58.2')
 robot.SetSpeed(20) # Set speed to 20%
 robot.FT_SetZero(1) # Set force/torque sensor zero
 robot.MoveJ(START_POS, tool=0, user=0, vel=50) # Move to start position
 make_grid(robot, loops=10)
if name == "__main__":
 main()

Código del graficador (joint_torque_grapher.py):

import matplotlib.pyplot as plt
import numpy as np
data = np.loadtxt('10x10_data.txt', delimiter=',')
x = data[:, 0]
y = data[:, 1]
z = data[:, 2]
print(max(z) - min(z))
fig = plt.figure(figsize=(10, 8))
ax = plt.axes(122, projection='3d')
plt.xlim(min(x) - 100, max(x) + 100)
plt.ylim(min(y) - 100, max(y) + 100)
ax.set_xlabel('X Position')
ax.set_ylabel('Y Position')
ax.set_zlabel('Z Position')
ax_dims = max([max(x) - min(x), max(y) - min(y), max(z) - min(z)])
ax.set(zlim=(min(z), min(z) + ax_dims))
ax.set(xlim=(min(x), min(x) + ax_dims))
ax.set(ylim=(min(y), min(y) + ax_dims))
plt.title('3D Probe Data')
ax.plot_trisurf(x, y, z, linewidth=0.2, antialiased=False, cmap='coolwarm')
############################################
data = np.loadtxt('20x20_data.txt', delimiter=',')
x = data[:, 0]
y = data[:, 1]
z = data[:, 2]
ax2 = plt.axes(121, projection='3d')
plt.xlim(min(x) - 100, max(x) + 100)
plt.ylim(min(y) - 100, max(y) + 100)
ax2.set_xlabel('X Position')
ax2.set_ylabel('Y Position')
ax2.set_zlabel('Z Position')
ax_dims = max([max(x) - min(x), max(y) - min(y), max(z) - min(z)])
ax2.set(zlim=(min(z), min(z) + ax_dims))
ax2.set(xlim=(min(x), min(x) + ax_dims))
ax2.set(ylim=(min(y), min(y) + ax_dims))
plt.title('3D Probe Data')
ax2.plot_trisurf(x, y, z, linewidth=0.2, antialiased=False, cmap='coolwarm')
plt.show()