some optimization in the code;
known issue: lin_angles is conceptual not possible; lin_position is not working because origin position and target position is the same in the code --> set target position not in self.config_pos!