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 _test_relative_position_control(
posn,
create_robot,
base_controller,
base_planner,
close_loop,
smooth,
trans_thresh,
angular_thresh,
):
bot = create_robot
bot.base.load_controller(base_controller)
bot.base.load_planner(base_planner)
start_state = np.array(bot.base.get_state("odom"))
desired_target = _get_absolute_pose(posn, start_state)
bot.base.go_to_relative(posn,
use_map=False,
close_loop=close_loop,
smooth=smooth)
end_state = np.array(bot.base.get_state("odom"))
dist = np.linalg.norm(end_state[:2] - desired_target[:2])
angle = end_state[2] - desired_target[2]
angle = np.abs(wrap_theta(angle))
assert dist < trans_thresh
assert angle * 180.0 / np.pi < angular_thresh
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 _test_relative_position_control(posn, botname, base_controller,
base_planner, close_loop, smooth,
trans_thresh, angular_thresh):
bot = Robot(botname,
base_config={
'base_controller': base_controller,
'base_planner': base_planner
},
use_arm=False,
use_camera=False)
start_state = np.array(bot.base.get_state('odom'))
desired_target = _get_absolute_pose(posn, start_state)
bot.base.go_to_relative(posn,
use_map=False,
close_loop=close_loop,
smooth=smooth)
end_state = np.array(bot.base.get_state('odom'))
if False:
# Plot results to file
file_name = '{:s}_position_{:s}_controller_{:s}_planner_{:s}_close{:d}_smooth{:d}-{:s}.pdf'
posn_str = ['{:0.1f}'.format(x) for x in posn]
file_name = file_name.format(botname, ','.join(posn_str),
base_controller, base_planner,
int(close_loop), int(smooth),
_get_time_str())
tmp_dir = os.path.join(os.path.dirname(__file__), 'tmp')
_mkdir_if_missing(tmp_dir)
file_name = os.path.join(tmp_dir, file_name)
bot.base.controller.plot_plan_execution(file_name)
dist = np.linalg.norm(end_state[:2] - desired_target[:2])
angle = end_state[2] - desired_target[2]
angle = np.abs(wrap_theta(angle))
assert dist < trans_thresh
assert angle * 180. / np.pi < angular_thresh
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 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 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
def test_absolute_position_control(
create_robot, posn,
):
bot = create_robot
bot.base.go_to_absolute(posn)
end_state = np.array(bot.base.get_state("odom"))
dist = np.linalg.norm(end_state[:2] - posn[:2])
angle = end_state[2] - posn[2]
angle = np.abs(wrap_theta(angle))
assert dist < trans_thresh
assert angle < angular_thresh
def test_relative_position_control(
create_robot, posn,
):
bot = create_robot
start_state = np.array(bot.base.get_state("odom"))
desired_target = _get_absolute_pose(posn, start_state)
bot.base.go_to_relative(posn)
end_state = np.array(bot.base.get_state("odom"))
dist = np.linalg.norm(end_state[:2] - desired_target[:2])
angle = end_state[2] - desired_target[2]
angle = np.abs(wrap_theta(angle))
assert dist < trans_thresh
assert angle < angular_thresh
def _states_close(state1, state2):
dist = np.linalg.norm(state1[:2] - state2[:2])
angle = wrap_theta(state1[2] - state2[2])
assert dist < 0.05
assert np.abs(angle) < 10 / 180. * np.pi
