def cube(robot: MelfaRobot, speed: float) -> None: """ Demo Example 1 - Cube :param robot: Instance of an active robot :param speed: :return: """ square_size = 240 # Base coordinates start = Coordinate( [-square_size / 2, square_size / 2, 0, 180, 0, 0], robot.AXES ) # pragma: no mutate x_vec = Coordinate([square_size, 0, 0, 0, 0, 0], robot.AXES) # pragma: no mutate y_vec = Coordinate([0, -square_size, 0, 0, 0, 0], robot.AXES) # pragma: no mutate z_vector = Coordinate([0, 0, 5, 0, 0, 0], robot.AXES) # pragma: no mutate square_corners = [ start, start + y_vec, start + x_vec + y_vec, start + x_vec, ] # pragma: no mutate # Go to points for _ in range(10): # Square for point in square_corners: robot.linear_move_poll(point, speed) # Back to first point robot.linear_move_poll(square_corners[0], speed) # Increment z square_corners = [point + z_vector for point in square_corners]
def linear_move_poll(self, target_pos: Coordinate, speed: float = None, track_speed=False, current_pos=None): """ Moves the robot linearly to a coordinate. :param target_pos: Coordinate for the target position. :param speed: Movement speed for tool. :param track_speed: :param current_pos: :return: """ # Set speed if speed is not None: self.set_speed(speed, 'linear') # Only send command if any coordinates are passed, otherwise just set the speed if len(target_pos.values) > 0 and any(a is not None for a in target_pos.values): # Fill None values with current position to predict correct response if current_pos is None: # No need to do this twice current_pos = self.protocol.get_current_xyzabc() target_pos.add_axis(current_pos) target_pos.update_empty(current_pos) # Send move command self.protocol.linear_move(target_pos) # Wait until position is reached t, v = cmp_response(R3Protocol_Cmd.CURRENT_XYZABC, target_pos.to_melfa_response(), self.protocol.reader, track_speed=track_speed) return t, v return None, None
def test_str(self, values, axes, digits, print_axes, expected): if digits is not None: a = Coordinate(values, axes, digits=digits, print_axes=print_axes) else: a = Coordinate(values, axes, print_axes=print_axes) assert str(a) == expected
def test_reduce_to_axes_make_none(self, values, axes, reduced, expected_values): a = Coordinate(values, axes) b = a.reduce_to_axes(reduced, make_none=True) assert list(b.axes) == list(axes) and list(b.values) == expected_values assert str(a) == str(Coordinate(values, axes))
def test_init_exception(self, values, axes, digits, print_axes, ex, ex_info): if ex is not None: with pytest.raises(ex) as excinfo: Coordinate(values, axes, digits, print_axes) assert str(excinfo.value.args[0]) == ex_info else: Coordinate(values, axes, digits, print_axes) assert True
def test_dot_none_values(self): """ Test that an exception is raised if one value is None :return: """ a = Coordinate((0, 0, None), "XYZ") b = Coordinate((1, 2, 3), "XYZ") with pytest.raises(TypeError): a.dot(b)
def test_update_empty_exception(self): """ Test exception raising for not-present axis. :return: """ a = Coordinate((0, 0, None), "XYZ") b = Coordinate((1, 2, 3), "XYA") with pytest.raises(TypeError) as excinfo: a.update_empty(b) assert str(excinfo.value.args[0]) == "Incompatible axis."
def test_update_empty_successful(self): """ Test successful update. :return: """ axes = "XYZ" a = Coordinate((0, 0, None), axes) b = Coordinate((1, 2, 3), axes) a.update_empty(b) assert list(a.values) == [0, 0, 3] and list(a.axes) == ["X", "Y", "Z"]
def test_dot(self, values_first, values_second, expected): """ Test that the scalar product is calculated correctly :param values_first: :param values_second: :param expected: :return: """ a = Coordinate(values_first, "XY") b = Coordinate(values_second, "XY") assert a.dot(b) == expected
def test_dot_incompatible_axes(self): """ Test that for incompatible axes the correct exception is raised :return: """ a = Coordinate((0, 0, None), "XZ") b = Coordinate((1, 2, 3), "XYZ") with pytest.raises(TypeError) as excinfo: a.dot(b) assert str(excinfo.value.args[0]) == "Incompatible axis."
def test_reduce_to_axes(self, test_input, remaining, expected): """ Test that only remaining axes are kept :param test_input: Input axes :param remaining: Function input :param expected: Expected remaining axes :return: """ a = Coordinate((1, -3, None), test_input) b = a.reduce_to_axes(remaining) assert "".join(list(b.axes)) == expected assert str(a) == str(Coordinate((1, -3, None), test_input))
def test_add(self, a, b, expected): """ Individual coordinates of correct axes should be added. Result of None + x is defined as None. :param a: :param b: :param expected: :return: """ axes = "XYZ" add1 = Coordinate(a, axes) add2 = Coordinate(b, axes) left_sum_coordinate = add1 + add2 right_sum_coordinate = add2 + add1 assert (list(left_sum_coordinate.values) == list( right_sum_coordinate.values) == expected)
def __init__(self, io_client: IClient, speed_threshold=10, number_axes: int = 6, safe_return=False): """ Initialises the robot. :param io_client: Communication object :param number_axes: Number of robot AXES, declared by 'J[n]', n>=1 :param safe_return: Flag to specify whether the robot should start and stop at its safe position """ super().__init__(name='Melfa Robot') if number_axes <= 0: raise ValueError('Number of axes needs to be larger than zero.') self.work_coordinate_offset_xyz = (-600, 140, -38.63) self.joints = number_axes self.speed_threshold = speed_threshold # Wrap the client in the specific protocol self.protocol = R3Protocol(io_client, MelfaCoordinateService(), self.joints) # Operation Flags self.safe_return = safe_return self.servo: bool = False self.com_ctrl: bool = False self.work_coordinate_active = False # G-Code Flags self.inch_active = False self.absolute_coordinates = True self.active_plane = Plane.XY self.zero = Coordinate([0, 0, 0, None, None, None], "XYZABC")
def test_mul(self, values, factor, expected): a = Coordinate(values, "XYZ") right_result = a * factor left_result = factor * a assert list(right_result.values) == list( left_result.values) == expected
def get_safe_pos(self) -> Coordinate: """ Get the current value of the safe position. :return: Coordinate object """ answer_str = self._read_parameter("JSAFE") safe_pos_values = [ float(i) for i in answer_str.split(DELIMITER)[1].split(", ") ] return Coordinate(safe_pos_values, self.joints)
def read_coordinates(file_path): try: f = open(file_path, "r") lines = f.read().splitlines() start = re.compile("[,;]\\s*").split(lines[0]) start_x = int(start[0]) start_y = int(start[1]) end = re.compile("[,;]\\s*").split(lines[1]) end_x = int(end[0]) end_y = int(end[1]) start_coordinate = Coordinate(start_x, start_y) end_coordinate = Coordinate(end_x, end_y) return PathSpecification(start_coordinate, end_coordinate) except FileNotFoundError: print("Error reading coordinate file " + file_path) traceback.print_exc() sys.exit()
def _set_nodes(self, nodes: int): self._tmp = [[0 for _ in range(self._map.size[0])] for _ in range(self._map.size[1])] while len(self._map.nodes) != nodes: new_coordinate = Coordinate.random_coordinate( (SECURITY_ZONE, MAP_SIZE[0] - SECURITY_ZONE - 1), (SECURITY_ZONE, MAP_SIZE[1] - SECURITY_ZONE - 1)) if self._check_node_place(new_coordinate): self._map.nodes.append( Node(len(self._map.nodes), new_coordinate)) self._tmp[new_coordinate.y][new_coordinate.x] = 1
def __init__(self, c_from: Coordinate, c_to: Coordinate, i_from, i_to): self.c_from: Coordinate = c_from self.c_to: Coordinate = c_to self.i_from: int = i_from self.i_to: int = i_to self.distance: float = Coordinate.distance(c_from, c_to) self.pheromone: float = 0 self.attractiveness: float = self.pheromone / self.distance self.walks: int = 0 self.no_draw: bool = True self.color: QColor = self._get_color()
def speed_test(robot: MelfaRobot, speed: float) -> None: start = Coordinate([-150, -150, 400, 180, 0, 0], robot.AXES) # pragma: no mutate vector = Coordinate([200, 300, -350, 0, 0, 0], robot.AXES) # pragma: no mutate finish = start + vector # pragma: no mutate # Back to start robot.protocol.reset_linear_speed() robot.linear_move_poll(start) # Test distance start_time = time.clock() t, v = robot.linear_move_poll(finish, speed * 60, track_speed=True) finish_time = time.clock() # Average velocity velocity = vector.vector_len() / (finish_time - start_time) # Draw speed draw_speed(speed, t, v) print("Velocity is: " + str(velocity))
def __init__( self, code_id: str, abs_cr: Tuple[float, ...] = None, rel_cr: Tuple[float, ...] = None, speed: float = None, e_length: float = None, time_ms: int = None, misc_cmd: Union[float, str] = None, home: str = "", line_number: int = None, ) -> None: """ Initialise an object. :param code_id: G-Code identifier :param abs_cr: Optional tuple of absolute cartesian coordinates :param rel_cr: Optional tuple of relative cartesian coordinates :param speed: Optional number for speed of printer head :param e_length: Optional number for extrude length :param time_ms: Optional number for time in ms :param misc_cmd: Optional argument for M-commands :param home: Optional string for homing :param line_number: Optional number for resend identification """ self.id = code_id self.joints = [] self.cartesian_abs: Coordinate = Coordinate(abs_cr, self.ABS_AXES, self.DIGITS) self.cartesian_rel: Coordinate = Coordinate( rel_cr, self.ABS_AXES, self.DIGITS, print_axes=self.REL_AXES ) self.speed = speed self.extrude_len = e_length self.time_ms = time_ms self.machine_option = misc_cmd self.home_opt = home self.line_number = line_number self.normal_vec = [0, 0, 1] if not self._is_valid(): raise ValueError("Unsupported or unknown command passed: " + self.id)
def cylinder(robot: MelfaRobot, speed: float) -> None: """ Demo Example 2 - Cylinder :param robot: Instance of an active robot :param speed: :return: """ # Base coordinates start = Coordinate([0, 0, 0, 180, 0, 0], robot.AXES) # pragma: no mutate z_vector = Coordinate([0, 0, 15, 0, 0, 0], robot.AXES) # pragma: no mutate target_vec = Coordinate([50, 50, 0, 0, 0, 0], robot.AXES) # pragma: no mutate target = start + target_vec # pragma: no mutate center_vec = Coordinate([50, 0, 0, 0, 0, 0], robot.AXES) # pragma: no mutate center = start + center_vec # pragma: no mutate clockwise = False for _ in range(10): # Move circle segment robot.circular_move_poll(target, center, clockwise, speed, start_pos=start) # Increase height and swap start and target start, target = target + z_vector, start + z_vector center += z_vector clockwise = not clockwise
def read_specification(coordinates, product_file): try: f = open(product_file, "r") lines = f.read().splitlines() firstline = re.compile("[:,;]\\s*").split(lines[0]) product_locations = [] number_of_products = int(firstline[0]) for i in range(number_of_products): line = re.compile("[:,;]\\s*").split(lines[i + 1]) product = int(line[0]) x = int(line[1]) y = int(line[2]) product_locations.append(Coordinate(x, y)) spec = PathSpecification.read_coordinates(coordinates) return TSPData(product_locations, spec) except FileNotFoundError: print("Error reading file " + product_file) traceback.print_exc() sys.exit()
def test_get_current_joint(self, protocol, echo_server, prefix, exc): """ Test that the response string can be converted correctly :param protocol: :param echo_server: :param prefix: :param exc: :return: """ response = 'J1;290.62;J2;-0.09;J3;11.26;J4;-179.94;J5;-0.26;J6;179.93;L1;0.00;;6,0;100;0.00;00000000' expected = Coordinate((290.62, -0.09, 11.26, -179.94, -0.26, 179.93), JOINTS) # Test if exc is None: echo_server.reconfigure(pre=prefix, msg=response) actual = protocol.get_current_joint() assert str(actual) == str(expected) else: echo_server.reconfigure(pre=prefix, msg='') with pytest.raises(exc): protocol.get_current_joint()
def go_home(self, option="") -> None: """ Moves the robot to its current home point (current work coordinate origin or global safe position respectively) :return: """ if self.work_coordinate_active: # Acquire new zero coordinate zero = Coordinate(self.zero.values, self.zero.axes) if option != "": zero = zero.reduce_to_axes(option, make_none=True) # Acquire current position to determine robot orientation current_position = self.protocol.reader.get_current_xyzabc() zero.add_axis(current_position) zero.update_empty(current_position) # Move to zero self.linear_move_poll(zero) else: self.go_safe_pos()
def test_floordiv(self, values, factor, expected): a = Coordinate(values, "XYZ") right_result = a // factor assert list(right_result.values) == expected
def test_vector_len(self, axes, values, expected): a = Coordinate(values, axes) assert a.vector_len() == pytest.approx(expected, abs=0.01)
def test_vector_len_none(self): a = Coordinate([3, 2, None], "XYZ") with pytest.raises(TypeError): a.vector_len()
def test_are_axes_compatible(self, axes1, axes2, result): a = Coordinate((None, None, None), axes1) b = Coordinate((None, None, None), axes2) actual_result = a._are_axes_compatible(b) assert actual_result == result
def circular_move_poll(self, target_pos: Coordinate, center_pos: Coordinate, is_clockwise: bool, speed: Optional[float] = None, start_pos: Optional[Coordinate] = None) -> None: """ Moves the robot on a (counter-)clockwise arc around a center position to a target position. :param start_pos: Coordinate for the start position, defaults to current position if None. The robot first performs a linear movement to the start position if it is not equal to the current position. :param target_pos: Coordinate for the target position. :param center_pos: Coordinate for the center of the arc. :param is_clockwise: Flag to indicate clockwise|counter-clockwise direction. :param speed: Movement speed for tool. """ # Determine start position if start_pos is None: start_pos = self.protocol.get_current_xyzabc() start_pos = start_pos.reduce_to_axes('XYZ') # Set speed if speed is not None: self.set_speed(speed, "linear") # Only send command if any coordinates are passed, otherwise just set the speed if len(target_pos.values) > 0 and any(a is not None for a in target_pos.values): # Update positions to be complete target_pos.update_empty(start_pos) center_pos.update_empty(start_pos) start_pos_np = np.array(start_pos.values) target_pos_np = np.array(target_pos.values) center_pos_np = np.array(center_pos.values) angle = self.get_directed_angle(start_pos_np, target_pos_np, center_pos_np, is_clockwise) if abs(angle) >= pi: # Intermediate points for angles >= 180° im_pos_np = get_intermediate_point(angle, start_pos_np, target_pos_np, center_pos_np, self.active_plane) im_pos = Coordinate(list(im_pos_np), 'XYZ') im_pos.update_empty(start_pos) # Position assignments if abs(angle) == 2 * pi: # Calculate additional intermediate point angle = self.get_directed_angle(start_pos_np, im_pos_np, center_pos_np, is_clockwise) im_pos2_np = get_intermediate_point( angle, start_pos_np, im_pos_np, center_pos_np, self.active_plane) im_pos2 = Coordinate(list(im_pos2_np), 'XYZ') im_pos2.update_empty(start_pos) # Global variables self.set_global_positions(["P1", "P2", "P3"], [start_pos, im_pos2, im_pos]) # Send move command self.protocol.circular_move_full("P1", "P2", "P3") else: # Global variables self.set_global_positions(["P1", "P2", "P3"], [start_pos, im_pos, target_pos]) # Send move command self.protocol.circular_move_intermediate("P1", "P2", "P3") else: # Global variables self.set_global_positions(["P1", "P2", "P3"], [start_pos, target_pos, center_pos]) # Send move command self.protocol.circular_move_centre("P1", "P2", "P3") # Wait until position is reached cmp_response(R3Protocol_Cmd.CURRENT_XYZABC, target_pos.to_melfa_response(), self.protocol.reader)
def check_trajectory(config_f='./../config.ini', gcode_f='./../test.gcode', ip: Optional[str] = None, port: Optional[int] = None): """ Validate a trajectory for a given robot setup. :param config_f: File path for the configuration file :param gcode_f: File path for the input G-Code file :param ip: Optional host address to be used to resolve robot parameters directly :param port: Optional port to be used to resolve robot parameters directly :return: """ print(f'Reading G-Code from file {gcode_f}.') with open(gcode_f, 'r') as f: cmd_raw = f.readlines() commands = [GCmd.read_cmd_str(cmd_str.strip()) for cmd_str in cmd_raw if not cmd_str.startswith(GCmd.COMMENT)] commands = [cmd for cmd in commands if cmd is not None] config_parser = ConfigParser() config_parser.read(config_f) read_param_from_robot = False home_position = None cartesian_limits = None joint_limits = None # Parameters that always need to be configured within the config file max_jnt_speed = config_parser.get('prechecks', 'max_joint_speed') joint_velocity_limits = [float(i) for i in max_jnt_speed.split(', ')] # Increments for sampling inc_distance_mm = float(config_parser.get('prechecks', 'ds_mm')) inc_angle_tool_deg = float(config_parser.get('prechecks', 'dphi_deg', fallback=2 * pi)) urdf = config_parser.get('prechecks', 'urdf_path') default_acc = float(config_parser.get('prechecks', 'default_acc')) # Extrusion parameters extrusion_height = float(config_parser.get('prechecks', 'extrusion_height')) extrusion_width = float(config_parser.get('prechecks', 'extrusion_width')) # Tool Center Point offsets tool_offset_x = float(config_parser.get('prechecks', 'tool_offset_x', fallback=0)) tool_offset_y = float(config_parser.get('prechecks', 'tool_offset_y', fallback=0)) tool_offset_z = float(config_parser.get('prechecks', 'tool_offset_z', fallback=0)) # Heat bed offsets x_hb = float(config_parser.get('prechecks', 'bed_origin_x', fallback=0)) y_hb = float(config_parser.get('prechecks', 'bed_origin_y', fallback=0)) z_hb = float(config_parser.get('prechecks', 'bed_origin_z', fallback=0)) # Learning time prm_learning_time_s = int(config_parser.get('prechecks', 'prm_learning_time', fallback=120)) robot_config = melfa_rv_4a(atoff=tool_offset_z, rtoff=tool_offset_x) if ip is not None and port is not None: # Parameters can be read from the robot try: print(f'Attempting to read configuration from {ip}:{port}') tcp_client = TcpClientR3(host=ip, port=port) protocol = R3Protocol(tcp_client, MelfaCoordinateService()) home_position = protocol.get_safe_pos().values cartesian_limits = protocol.get_xyz_borders() joint_limits = protocol.get_joint_borders() except ClientError as e: print(f'Reading parameters from robot failed due to {e}. Falling back to config file.') else: read_param_from_robot = True if not read_param_from_robot: # Parameters that need to be configured in the config file if they are not read from the robot home_pos_str = config_parser.get('prechecks', 'home_joints') home_position = [float(i) for i in home_pos_str.split(', ')] cartesian_limits_str = config_parser.get('prechecks', 'xyz_limits') cartesian_limits = [float(i) for i in cartesian_limits_str.split(', ')] joint_limits_str = config_parser.get('prechecks', 'joint_limits') joint_limits = [float(i) for i in joint_limits_str.split(', ')] print('\nConfiguration parameters:') print(f'Joint home position in deg: {home_position}') home_position = [i / 180 * pi for i in home_position] print(f'Cartesian limits in mm: {cartesian_limits}') print(f'Joint limits in deg: {joint_limits}') joint_limits = [i / 180 * pi for i in joint_limits] print(f'Maximum joint velocities in rad/s: {joint_velocity_limits}') print(f'Checking resolution in mm: {inc_distance_mm}') print(f'URDF filepath: {urdf}') print(f'Default acceleration set to {default_acc} mm/s^2') print('\n') hb_offset = Coordinate([x_hb, y_hb, z_hb], 'XYZ') # Create the constraints traj_constraint = Constraints(cartesian_limits, joint_limits, joint_velocity_limits) # Create the increments incs = Increments(inc_distance_mm, inc_angle_tool_deg / 180 * pi) # Create the extrusion data extr = Extrusion(extrusion_height, extrusion_width) try: # Check the trajectory check_traj(commands, robot_config, traj_constraint, home_position, incs, extr, default_acc, urdf, hb_offset, prm_learning_time_s) except (CartesianLimitViolation, WorkspaceViolation): logging.error('Please verify that the limits are correct and check the positioning of the part.') raise except ConfigurationChangesError: logging.error('Robot configuration transitions are not supported within a coherent segment.') raise except JointVelocityViolation: logging.error('Autoamtically reducing the speed is not yet supported.') raise except NoValidPathFound: logging.error('Recreating graph (partially) with different tool orientation is unsupported.') raise