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