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