def compute_controls_from_xy(xy, theta0, dt, flip_theta=False):
"""
Given the xy trajectory, this computes the orientation, and v and w
commands to track this trajectory. These can then be used to close the loop
on this trajectory using an LQR controller.
"""
x, y = xy.T
theta = np.arctan2(y[1:] - y[:-1], x[1:] - x[:-1])
if flip_theta:
theta = theta + np.pi
theta = np.concatenate([[theta0], theta], axis=0)
# Unwrap theta as necessary.
old_theta = theta[0]
for i in range(theta.shape[0] - 1):
theta[i + 1] = wrap_theta(theta[i + 1] - old_theta) + old_theta
old_theta = theta[i + 1]
xyt = np.array([x, y, theta]).T
v = np.linalg.norm(xy[1:, :] - xy[:-1, :], axis=1)
w = theta[1:] - theta[:-1]
v = np.append(v, 0)
w = np.append(w, 0)
us = np.array([v, w]).T
us = us / dt
return xyt, us
def position_control_init_fn(init_type,
start_pos,
goal_pos,
dt,
max_v,
max_w,
reverse=False):
# sharp init, rotates in direction of goal, and then moves to the goal
# location and then rotates into goal orientation. It takes velcoity limits
# into account when doing so.
dist_thresh = 0.01
dist = np.linalg.norm(goal_pos[:2] - start_pos[:2])
angle = wrap_theta(goal_pos[2] - start_pos[2])
already_at_goal = dist < dist_thresh and np.abs(angle) < ANGLE_THRESH
pure_rotation = dist < dist_thresh
f = 0.5
ramp = 1
if already_at_goal:
init_states = np.array([start_pos])
elif pure_rotation:
init_states = pure_rotation_init(start_pos, goal_pos, dt, max_v * f,
max_w * f, ramp)
elif not pure_rotation:
if init_type == 'sharp':
init_states = sharp_init(start_pos, goal_pos, dt, max_v, max_w,
reverse)
elif init_type == 'smooth':
init_states = smooth_init(start_pos, goal_pos, dt, max_v, max_w,
reverse)
return init_states
def update(self, x, y, theta):
"""Updates the state being stored by the object."""
theta = wrap_theta(theta - self.old_theta) + self.old_theta
self.old_theta = theta
self.theta = theta
self.x = x
self.y = y
self._state_f = np.array([self.x, self.y, self.theta],
dtype=np.float32).T
self.update_called = True
def pure_rotation_init(start_pos, goal_pos, dt, max_v, max_w, ramp):
"""Function to generate state sequences for pure rotation."""
# Pure Rotation
dtheta = goal_pos[2] - start_pos[2]
dtheta = wrap_theta(dtheta)
T = int(np.ceil(np.abs(dtheta) / max_w / dt))
goal1_pos = goal_pos.copy()
goal1_pos[2] = start_pos[2] + dtheta
states1 = linear_interpolate_ramp(start_pos, goal1_pos, T, ramp)
return states1
def sharp_init(start_pos, goal_pos, dt, max_v, max_w, reverse=False):
f = 0.5
ramp = 1
delta_theta = np.arctan2(goal_pos[1] - start_pos[1],
goal_pos[0] - start_pos[0])
to_rotate = wrap_theta(delta_theta - start_pos[2])
if reverse and np.abs(to_rotate) > 3 * np.pi / 4:
to_rotate = wrap_theta(np.pi + to_rotate)
goal1_pos = start_pos.copy()
if np.abs(to_rotate) > ANGLE_THRESH:
num_steps_1 = np.ceil(np.abs(to_rotate) / max_w / f / dt).astype(
np.int)
num_steps_1 = np.maximum(num_steps_1, 1)
goal1_pos[2] = goal1_pos[2] + to_rotate
states1 = linear_interpolate_ramp(start_pos, goal1_pos, num_steps_1,
ramp)
else:
states1 = np.zeros([0, 3], dtype=np.float32)
dist = np.linalg.norm(goal_pos[:2] - start_pos[:2])
goal2_pos = goal1_pos.copy()
goal2_pos[:2] = goal_pos[:2]
num_steps_2 = np.maximum(np.ceil(dist / max_v / f / dt).astype(np.int), 1)
states2 = linear_interpolate_ramp(goal1_pos, goal2_pos, num_steps_2, ramp)
to_rotate = wrap_theta(goal_pos[2] - goal2_pos[2])
goal3_pos = goal2_pos.copy()
if np.abs(to_rotate) > ANGLE_THRESH:
num_steps_3 = np.ceil(np.abs(to_rotate) / max_w / f / dt).astype(
np.int)
num_steps_3 = np.maximum(num_steps_3, 1)
goal3_pos[2] = goal3_pos[2] + to_rotate
states3 = linear_interpolate_ramp(goal2_pos, goal3_pos, num_steps_3,
ramp)
else:
states3 = np.zeros([0, 3], dtype=np.float32)
init_states = np.concatenate([states1, states2, states3], 0)
return init_states