def transform_vector(self, vec, cur_pos):
""" Transform vector to whatever coordinate system is used """
ret_vec = np.copy(vec)
if Path.axis_config == Path.AXIS_CONFIG_H_BELT:
X = np.dot(Path.matrix_H_inv, vec[0:2])
ret_vec[:2] = X[0]
if Path.axis_config == Path.AXIS_CONFIG_CORE_XY:
X = np.dot(Path.matrix_XY, vec[0:2])
ret_vec[:2] = X[0]
if Path.axis_config == Path.AXIS_CONFIG_DELTA:
# Subtract the current column positions
if hasattr(self.prev, "end_ABC"):
self.start_ABC = self.prev.end_ABC
else:
self.start_ABC = Delta.inverse_kinematics2(cur_pos[0], cur_pos[1],
cur_pos[2])
# Find the next column positions
self.end_ABC = Delta.inverse_kinematics2(cur_pos[0] + vec[0],
cur_pos[1] + vec[1],
cur_pos[2] + vec[2])
ret_vec[:3] = self.end_ABC - self.start_ABC
if Path.axis_config == Path.AXIS_CONFIG_SCARA:
# Subtract the current angle positions
if hasattr(self.prev, "end_ABC"):
self.start_ABC = self.prev.end_ABC
else:
self.start_ABC = Scara.inverse_kinematics(cur_pos[0], cur_pos[1],
cur_pos[2])
# Find the next angle positions
self.end_ABC = Scara.inverse_kinematics(cur_pos[0] + vec[0],
cur_pos[1] + vec[1],
cur_pos[2] + vec[2]
)
ret_vec[:3] = self.end_ABC - self.start_ABC
# Apply Automatic bed compensation
if self.use_bed_matrix:
ret_vec[:3] = np.dot(Path.matrix_bed_comp, ret_vec[:3])
return ret_vec
def reverse_transform_vector(self, vec, cur_pos):
""" Transform back from whatever """
ret_vec = np.copy(vec)
if Path.axis_config == Path.AXIS_CONFIG_H_BELT:
X = np.dot(Path.matrix_H, vec[0:2])
ret_vec[:2] = X[0]
if Path.axis_config == Path.AXIS_CONFIG_CORE_XY:
X = np.dot(Path.matrix_XY_inv, vec[0:2])
ret_vec[:2] = X[0]
if Path.axis_config == Path.AXIS_CONFIG_DELTA:
# Find the next column positions
self.end_ABC = self.start_ABC + vec[:3]
# We have the column translations and need to find what that
# represents in cartesian.
start_xyz = Delta.forward_kinematics2(self.start_ABC[0], self.start_ABC[1],
self.start_ABC[2])
end_xyz = Delta.forward_kinematics2(self.end_ABC[0], self.end_ABC[1],
self.end_ABC[2])
ret_vec[:3] = end_xyz - start_xyz
if Path.axis_config == Path.AXIS_CONFIG_SCARA:
# Find the next angle positions
self.end_ABC = self.start_ABC + vec[:3]
# We have the angle translations and need to find what that
# represents in cartesian.
start_xyz = Scara.forward_kinematics(self.start_ABC[0], self.start_ABC[1],
self.start_ABC[2])
end_xyz = Scara.forward_kinematics(self.end_ABC[0], self.end_ABC[1],
self.end_ABC[2])
ret_vec[:3] = end_xyz - start_xyz
# Apply Automatic bed compensation
if self.use_bed_matrix:
ret_vec[:3] = np.dot(Path.matrix_bed_comp_inv, ret_vec[:3])
return ret_vec
