def __init__(self, poslist, ptype="deg"):
if len(poslist) != const.ROBOT_TOTAL_AXIS:
fatalError("La lista de angulos debe tener almenos " +
str(const.ROBOT_TOTAL_AXIS) + " ejes")
self.posicion = poslist.copy()
self.ptype = ptype
self.currentIdx = 0
def update(self, poslist, ptype="deg"):
# Función para crear una posición para cada motor del robot
if len(poslist) != const.ROBOT_TOTAL_AXIS:
fatalError("La lista de angulos debe tener almenos " +
str(const.ROBOT_TOTAL_AXIS) + " ejes")
self.ptype = ptype
self.posicion = posicion.copy()
self.currentIdx = 0
def __init__(self):
self.clientID = self.connect(self.port)
for i in range(5):
retCode, joint = sim.simxGetObjectHandle(
self.clientID, f"Joint_{i}", simConst.simx_opmode_blocking)
self.servos.append(joint)
if retCode == -1:
fatalError(f"no se pudo obtener la instancia del joint {i+1}")
# Obtener los handlers para posteriormente manipular el robot
retCode, self.dummy = sim.simxGetObjectHandle(
self.clientID, "Dummy", simConst.simx_opmode_blocking)
[retCode,
self.robotIq] = sim.simxGetObjectHandle(self.clientID,
'ROBOTIQ_85_attachPoint',
simConst.simx_opmode_blocking)
retCode, self.grip = sim.simxGetObjectHandle(
self.clientID, 'ROBOTIQ_85', simConst.simx_opmode_blocking)
[retCode,
self.tijeras] = sim.simxGetObjectHandle(self.clientID, 'Cuboid3',
simConst.simx_opmode_blocking)
[retCode,
self.bisturi] = sim.simxGetObjectHandle(self.clientID, 'Cuboid6',
simConst.simx_opmode_blocking)
[retCode, self.jeringuilla
] = sim.simxGetObjectHandle(self.clientID, 'Cuboid5',
simConst.simx_opmode_blocking)
[retCode,
self.pinza] = sim.simxGetObjectHandle(self.clientID, 'Cuboid4',
simConst.simx_opmode_blocking)
retCode, self.camara = sim.simxGetObjectHandle(
self.clientID, 'Vision_sensor', simConst.simx_opmode_blocking)
retCode, self.cuboid0 = sim.simxGetObjectHandle(
self.clientID, 'Cuboid0', simConst.simx_opmode_blocking)
retCode, self.cuboid = sim.simxGetObjectHandle(
self.clientID, 'Cuboid', simConst.simx_opmode_blocking)
# Inicializar el diccionario con las posiciones de los objetos
self.object_positions[20] = [0, 0, 0, self.bisturi, 0]
self.object_positions[21] = [0, 0, 0, self.tijeras, 0]
self.object_positions[22] = [0, 0, 0, self.jeringuilla, 0]
self.object_positions[23] = [0, 0, 0, self.pinza, 0]
self.code_to_object_instance = {
"20": self.bisturi,
"21": self.tijeras,
"22": self.jeringuilla,
"23": self.pinza
}
print("Simulator incializado")
def setPose(self, angles, sleep=True):
if len(angles) > len(self.servos):
fatalError(
"numero de angulos no coinciden con el numero de servos")
for i in range(len(self.servos)):
# returnCode = sim.simxSetJointTargetPosition(self.clientID, self.servos[len(angles) - i - 1], angles[len(angles) - i - 1], simConst.simx_opmode_blocking)
returnCode = sim.simxSetJointTargetPosition(
self.clientID, self.servos[i], angles[i],
simConst.simx_opmode_oneshot)
if sleep:
time.sleep(0.4)
return returnCode != -1
sys.exit()
# ------ Delete option END -------
# provide warnings and info for other optional params
if not gDb and not (gInit or gRun or gDel):
print('[ %sWARN%s]: No database specified as input.' %
(fg('yellow'), attr('reset')))
if gConfigFileName != 'challenges.yaml':
print('[ %sINFO%s]: Using custom config: %s' %
(fg('cyan'), attr('reset'), gConfigFileName))
# ------ Get challenge config file -------
# Step 1: load configuration
gConfig = utils.loadYamlFile(gConfigFileName)
if not gConfig:
utils.fatalError("Could not properly load the file " +
str(gConfigFileName))
gConfig = ch.loadChallengesConfig(gConfigFileName)
# Step 2: keep only the needed challenge's configuration
try:
gConfig = gConfig[gChal]
except KeyError:
print('[%sERROR%s]: No challenge named: %s' %
(fg('red'), attr('reset'), gChal))
sys.exit(0)
if gConfig['disable'] != False:
print(
'[ %sWARN%s]: Writing tests for a currently disabled challenge %s'
% (fg('yellow'), attr('reset'), gChal))
# Step 3: Check what's happening inside the IO folder