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
start_ABC = Delta.inverse_kinematics(cur_pos[0], cur_pos[1],
cur_pos[2])
# Find the next column positions
end_ABC = Delta.inverse_kinematics(cur_pos[0] + vec[0],
cur_pos[1] + vec[1],
cur_pos[2] + vec[2])
ret_vec[:3] = end_ABC - start_ABC
return ret_vec
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
start_ABC = Delta.inverse_kinematics(cur_pos[0], cur_pos[1],
cur_pos[2])
# Find the next column positions
end_ABC = Delta.inverse_kinematics(cur_pos[0] + vec[0],
cur_pos[1] + vec[1],
cur_pos[2] + vec[2])
ret_vec[:3] = end_ABC - start_ABC
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:
# Subtract the current column positions
start_ABC = Delta.inverse_kinematics(cur_pos[0], cur_pos[1],
cur_pos[2])
# Find the next column positions
end_ABC = start_ABC + vec[:3]
# We have the column translations and need to find what that
# represents in cartesian.
start_xyz = Delta.forward_kinematics(start_ABC[0], start_ABC[1],
start_ABC[2])
end_xyz = Delta.forward_kinematics(end_ABC[0], end_ABC[1],
end_ABC[2])
ret_vec[:3] = end_xyz - start_xyz
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:
# Subtract the current column positions
start_ABC = Delta.inverse_kinematics(cur_pos[0], cur_pos[1],
cur_pos[2])
# Find the next column positions
end_ABC = start_ABC + vec[:3]
# We have the column translations and need to find what that
# represents in cartesian.
start_xyz = Delta.forward_kinematics(start_ABC[0], start_ABC[1],
start_ABC[2])
end_xyz = Delta.forward_kinematics(end_ABC[0], end_ABC[1],
end_ABC[2])
ret_vec[:3] = end_xyz - start_xyz
return ret_vec
