def hoehe(self, robot_handler, arguments):
if len(arguments) != 1:
message = "Bitte geben Sie eine Koordinate für eine neue Hoehe an. Bsp.: hoehe(2)"
raise RobotError(ErrorCode.E0008, message)
elif arguments[0] > 3:
message = "Die eingegebene Höhe ist {}. Bitte geben sie maximal eine Höhe von 3 ein".format(
arguments[0])
raise RobotError(ErrorCode.E0008, message)
elif arguments[0] < 0:
message = "Der Motor kann nicht unterhalb des Bodens gehen"
raise RobotError(ErrorCode.E0008, message)
self.__function_calls.append({
"function": robot_handler.height_new,
"args": arguments
})
def transform_position_user_to_uarm(self, x_user, y_user, z_uarm):
"""
Transform x, y -position in user frame to x, y position in uarm frame. For user frame specification refer
to the board design.
:param x_user: x-position in user frame
:type x_user: int
:param y_user: y-position in user frame
:type y_user: int
:param z_uarm: z-position in uarm frame
:type z_uarm: float
:return: position in uarm frame {'x': x, 'y', y}
:rtype: dict
"""
# transform coordinates
# adding .5 to be in center of square
x_uarm = (x_user + .5) * self.__edge_length + self.__x_offset
y_uarm = (y_user + .5) * self.__edge_length + self.__y_offset
# check if input is in range of robot
# TODO (ALR): This is just a basic workspace restriction, add more if necessary.
xy_length = numpy.sqrt(x_uarm**2 + y_uarm**2)
xy_radius = abs(xy_length - self.__xy_base_offset)
z_radius = abs(z_uarm - self.__z_base_offset)
radius = numpy.sqrt(xy_radius**2 + z_radius**2)
if radius > (self.__max_radius_xy - self.__xy_base_offset
) or x_uarm < 0 or xy_length <= self.__min_radius_xy:
message = "Die eingegebenen Koordinaten sind nicht für den Roboter erreichbar."
raise RobotError(ErrorCode.E0000, message)
return {'x': x_uarm, 'y': y_uarm}
def transform_height_user_to_uarm(self, z_user, x_uarm, y_uarm):
"""
Transform z-position in user frame to uarm frame, check if values are valid.
:param z_user: z-position in user frame
:type z_user: int
:param x_uarm: x-position in uarm frame
:type x_uarm: float
:param y_uarm: y-position in uarm frame
:type y_uarm: float
:return: z position in uarm frame in mm
:rtype: float
"""
# calculate height in uarm frame
z_uarm = self.__z_offset + z_user * self.__edge_length
# check if height is valid
xy_length = numpy.sqrt(x_uarm**2 + y_uarm**2)
xy_radius = abs(xy_length - self.__xy_base_offset)
z_radius = abs(z_uarm - self.__z_base_offset)
radius = numpy.sqrt(xy_radius**2 + z_radius**2)
# check if z_uarm value in workspace
if radius > (
self.__max_radius_xy - self.__xy_base_offset
) or z_uarm < self.__z_offset or z_uarm < self.__edge_length + self.__z_offset:
message = "Die gewünschte Höhe ist für den Roboter nicht erreichbar, bitte geben Sie einen anderen Wert an."
raise RobotError(ErrorCode.E0004, message)
return z_uarm
def calculate_equal_wrist_rotation(self, x_uarm_old, x_uarm_new,
y_uarm_old, y_uarm_new, wrist_old):
"""
Calculates new wrist rotation that keeps the gripper rotation equal in the world frame.
:param x_uarm_old: old x-position in uarm frame
:type x_uarm_old: float
:param x_uarm_new: new x-position in uarm frame
:type x_uarm_new: float
:param y_uarm_old: old y-position in uarm frame
:type y_uarm_old: float
:param y_uarm_new: new y-position in uarm frame
:type y_uarm_new: float
:param wrist_old: old wrist position in degrees (absolute servo angle)
:type wrist_old: float
:return: new wrist angle that keeps the object in the same orientation
:rtype: float
"""
# angle from world x-axis to arm
alpha_1_rad = numpy.arctan2(y_uarm_old, x_uarm_old)
alpha_2_rad = numpy.arctan2(y_uarm_new, x_uarm_new)
alpha_1_deg = numpy.degrees(alpha_1_rad)
alpha_2_deg = numpy.degrees(alpha_2_rad)
# angle from world x-axis to end effector orientation (-90 because of the asymetric servo range 0-180)
beta_1 = alpha_1_deg + 90.0 - wrist_old
# calculate the corresponding new wrist angle for new position, so that the orientation of the grabbed object
# stays the same
wrist_new = -beta_1 + alpha_2_deg + 90.0
# check that the wrist angle is within the servos range
if not (0 <= wrist_new <= 180):
message = "Bei der gewünschten Bewegung kann die orientierung des Objekts nicht beibehalten werden," \
"bitte drehen Sie das Object vor der Bewegung."
raise RobotError(ErrorCode.E0003, message)
return wrist_new
def reset(self):
"""
Reset robot, go back to start position.
"""
# reset arm to home
self.__swift.reset(wait=True, speed=10000)
# get pose values in uarm frame
pose = self.__swift.get_position()
# check if successful
if isinstance(pose, list):
self.__x_uarm = pose[0]
self.__y_uarm = pose[1]
self.__z_uarm = pose[2]
else:
message = "Die Roboter Position konnte nicht gelesen werden, überprüfe die Verbindung."
raise RobotError(ErrorCode.E0001, message)
self.pump_off()
# set servo value in degrees
wrist_angle = 90.0
self.__swift.set_servo_angle(servo_id=3, angle=wrist_angle)
self.__wrist_angle = wrist_angle
self.__swift.flush_cmd()
time.sleep(self.__sleep_time)
# move to fix starting position
self.position_new([3, 8])
self.height_new([2])
self.__swift.flush_cmd()
def pumpe_aus(self, robot_handler, arguments):
if len(arguments) != 0:
message = "Die Funktion pumpe_aus benötigt kein Argument. Bsp.: pumpe_aus()"
raise RobotError(ErrorCode.E0010, message)
self.__function_calls.append({
"function": robot_handler.pump_off,
"args": arguments
})
def position(self, robot_handler, arguments):
if len(arguments) != 2:
message = "Bitte geben Sie zwei Koordinaten für eine neue Position an. Bsp.: position(5, 5)"
raise RobotError(ErrorCode.E0007, message)
self.__function_calls.append({
"function": robot_handler.position_new,
"args": arguments
})
def placeBlock(self,coordinates):
# Checks whether the block can be placed
check = False
bottom_coordinates = coordinates.copy()
bottom_coordinates[2] -= 1
if self.isBlock(coordinates):
message = 'There is already a block there'
Debug.msg(message)
Debug.error(message)
raise RobotError(ErrorCode.E0015, message)
return False
elif coordinates[2] == 1:
Debug.msg("The block is placed on the floor")
self.block_manipulating.placeBlock(coordinates)
self.blocks.append(self.block_manipulating)
self.block_manipulating = []
return True
elif self.isBlock(bottom_coordinates):
Debug.msg("The block is placed on another block")
self.block_manipulating.placeBlock(coordinates)
self.blocks.append(self.block_manipulating)
self.block_manipulating = []
return True
else:
# Implement method for bridge block
test_block = self.block_manipulating # Used to test whether the block can be placed on two points
test_block.placeBlock(bottom_coordinates)
possible_coordinates = test_block.get_positions
#print('Possible coordinates {}'.format(possible_coordinates))
blocks_below = 0
for cord in possible_coordinates:
#print('Coordinates to check : {}'.format(cord))
if self.isBlock(cord):
blocks_below +=1
Debug.msg('There is a block below but not in the center')
if blocks_below >= 2:
Debug.msg("The block is placed on multiple other blocks")
self.block_manipulating.placeBlock(coordinates)
self.blocks.append(self.block_manipulating)
self.block_manipulating = []
return True
else:
message = "Der Block kann nicht in der Luft losgelassen werden. Platziere den Block auf dem Boden oder auf einem anderen Block"
Debug.error(message)
raise RobotError(ErrorCode.E0015, message)
return check
def rotate_man_block(self,angle):
# rotates the block which is currently holdign by angle anticlockwise (degree)
if self.block_manipulating:
self.block_manipulating.rotate(angle)
else:
message = "Der Roboter trägt momentan keinen Block, welcher rotiert werden kann"
raise RobotError(ErrorCode.E0014, message)
return False
def pump_on(self,coordinates):
# Checks, whether a block is below the pump
if self.getBlock(coordinates):
# Everything is fine
return True
else:
message = "Die Pumpe kann nur aktiviert werden, um einen Block aufzuheben. Schau, ob die Blöcke richtig platziert sind"
raise RobotError(ErrorCode.E0013, message)
def drehen(self, robot_handler, arguments):
if len(arguments) != 1:
message = "Bitte geben Sie nur einen Winkel für die Drehung an: Bsp: drehen(90)"
raise RobotError(ErrorCode.E0100, message)
self.__function_calls.append({
"function": robot_handler.drehen,
"args": arguments
})
def __cleanup_line(line):
"""
Cleanup script line and return function string and argument list.
:param line: line of editor
:type line: str
:return: function string and corresponding argument list
:rtype: dict[str, list]
"""
# get arguments and functions
argument_begin = line.find("(")
argument_end = line.find(")")
# check if function has brackets
if argument_begin == -1 or argument_end == -1:
message = "Funktions Argumente müssen mit runden Klammern umgeben werden. Bsp.: position(1, 2) pumpe_an()"
raise RobotError(ErrorCode.E0006, message)
elif len(line) - 1 > argument_end:
message = "Funktions Argumente müssen mit Klammern enden und einzeln pro Zeile eingeben werden. Bsp.: position(1, 2)"
raise RobotError(ErrorCode.E0006, message)
function_string = line[:argument_begin]
argument_string = line[argument_begin + 1:argument_end]
# TODO (ALR): Error check? (TE): Added int check and additional bracket end check
if len(argument_string) > 0:
try:
arguments = [int(i) for i in argument_string.split(",")]
for i in arguments:
if not str(i).isdigit():
message = "Das eingegebene Argument {} ist keine positive, ganze Zahl".format(
i)
raise RobotError(ErrorCode.E0006, message)
except:
for i in argument_string.split(","):
try:
int(i)
except:
message = "Das eingegebene Argument {} ist keine gültige Zahl".format(
i)
raise RobotError(ErrorCode.E0006, message)
else:
arguments = []
return {"function_string": function_string, "arguments": arguments}
def pump_off(self,coordinates):
# Checks whether the block can be placed on the coordinates
if self.block_manipulating:
if self.placeBlock(coordinates):
# THe block could be succesfully places
return True
else:
# ToDo: The pump should be stopped here or somewhere else
return False
else:
message = "Der Roboter trägt momentan keinen oder keinen bekannten Block"
raise RobotError(ErrorCode.E0014, message)
def __init__(self, challenge):
"""
Class initialization.
:param challenge: Kind of challenge to be monitored.
:type challenge: Name of challenge, str
:param start_coordinates: start position and height of robot in user frame [x, y, z]
:type start_coordinates: list
"""
#Turns the terminal debuging comments of
Debug.error_on = True
Debug.print_on = False
# Loads all challenges out of the challenges/challenge_init folder. Feel free to define new ones.
self.__all_challenges = []
self.load_challenges()
self.load_general_options()
challenge_loaded = False
for chal in self.__all_challenges:
#print('Challenge name: {} , chal searching {}'.format(chal.name, challenge))
if chal.name == challenge:
self.__challenge = chal
print('Challenge %s ist gestartet. Viel Erfolg beim Lösen.' %{challenge})
challenge_loaded = True
break
if not challenge_loaded:
message = "Die Aufgabe %s kann nicht geladen werden. Bitte wählen Sie eine \
andere aus."%{challenge}
raise RobotError(ErrorCode.E0012, message)
self.__block = BlockKind.Null