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
