def interpolate(self, s1, s2): """Returns a sequence of states between two states given a motion that connects the two states. We use a shooting method type approach to interpolate between the two states @param: s1 - parent state @param: s2 - child state @return edge - sequence of states (including end points) """ step_size = self.path_resolution s1_n = (s1[0], s1[1], angles.normalize_angle_positive(s1[2])) s2_n = (s2[0], s2[1], angles.normalize_angle_positive(s2[2])) edge, _ = dubins.path_sample(s1, s2, self.radius - 0.01, step_size) #Dubins returns edge in [0, 2pi], we got to normalize it to (-pi, pi] for our code normalized_edge = [] for config in edge: new_config = (config[0], config[1], angles.normalize_angle(config[2])) normalized_edge.append(new_config) normalized_edge.append(tuple(s2)) return normalized_edge
def node_to_state(self, node): """Convert a discrete node to a world state taking origin and rotation of lattice into account @param node - a tuple containing di screte (x,y,heading) coordinates @return state - np array of state in continuous world coordinates """ xd = node[0] yd = node[1] pos = self.origin + np.array([ self.resolution[0] * (cos(self.rotation) * xd - sin(self.rotation) * yd), self.resolution[1] * (sin(self.rotation) * xd + cos(self.rotation) * yd) ]) rot = angles.normalize_angle( (node[2] * (2.0 * np.pi)) / (self.num_heading * 1.0)) state = (pos[0], pos[1], rot) return state