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