def test_control_limit_from_final_time():
for i in range(N_RAND):
x_0 = np.random.rand(2) * 2.0 - 1.0
x_f = np.random.rand(2) * 2.0 - 1.0
u_lim = random() * 5.0 + 0.1
di = DoubleIntegrator(u_sys_lim=u_lim)
res, traj = di.time_opt_control(x_0=x_0, x_f=x_f)
assert np.isclose(
di.control_limit_from_final_time(x_0=x_0,
x_f=x_f,
t_f=traj.t_f,
s_0=traj.S[0]), u_lim)
def test_simple():
x_0 = np.array([0.5, -1.0])
x_f = np.array([0.0, 0.0])
di = DoubleIntegrator()
res, traj = di.time_opt_control(x_0=x_0, x_f=x_f)
assert res
assert np.isclose(np.sum(np.abs(traj.S)), 0)
assert np.isclose(np.linalg.norm(x_f - traj.X[-1]), 0)
assert np.isclose(np.linalg.norm(x_0 - traj.X[0]), 0)
assert np.isclose(traj.U[-1], 1)
assert traj.t_s < traj.t_f
def test_equal_start_end():
x_0 = np.array([0.0, 1.0])
x_f = np.array([0.0, 1.0])
di = DoubleIntegrator()
res, traj = di.time_opt_control(x_0=x_0, x_f=x_f)
assert res
assert np.isclose(np.sum(np.abs(traj.S)), 0)
assert np.isclose(np.linalg.norm(x_f - traj.X[-1]), 0)
assert np.isclose(np.linalg.norm(x_0 - traj.X[0]), 0)
assert np.isclose(traj.U[-1], 0)
assert traj.t_s == traj.t_f
def test_simple():
x_0 = np.array([0.5, -1.0])
x_f = np.array([-0.5 , 1.0])
di = DoubleIntegrator(dt=0.001, u_sys_lim=1.0)
t_f_opt = di.time_optimal_solution(x_0, x_f)[1]
t_f = t_f_opt + 1.5
res, traj = di.energy_opt_control(x_0=x_0, x_f=x_f, t_f=t_f)
if res:
assert np.linalg.norm(x_0 - traj.X[0]) < 1e-3
assert np.linalg.norm(x_f - traj.X[-1]) < 1e-2
def test_simple():
x_0 = np.array([0.6, 0.5])
x_f = np.array([0.1, 0.2])
di = DoubleIntegrator(dt=0.001, u_sys_lim=1.0)
t_f_opt = di.time_optimal_solution(x_0, x_f)[1]
t_f = t_f_opt + 1.0
res, traj = di.fuel_opt_control(x_0=x_0, x_f=x_f, t_f=t_f)
assert traj.t_s[0] < traj.t_s[1]
assert traj.t_s[1] < traj.t_f
assert np.linalg.norm(x_0 - traj.X[0]) < 1e-3
assert np.linalg.norm(x_f - traj.X[-1]) < 1e-3
def test_final_state():
success_count = 0
for i in range(N_RAND):
np.random.seed(i)
x_0 = np.random.rand(2) * 2.0 - 1.0
x_f = np.random.rand(2) * 2.0 - 1.0
u_lim = random() * 5.0 + 0.5
di = DoubleIntegrator(dt=0.001, u_sys_lim=u_lim)
res, traj = di.time_opt_control(x_0=x_0, x_f=x_f)
if res:
success_count += 1
assert np.linalg.norm(x_f - traj.X[-1]) < 1e-2
assert traj.t_s < traj.t_f
print('time optimal control success rate : ', success_count, ' / ', N_RAND)
def test_bound_states():
for i in range(N_RAND):
np.random.seed(i)
x_0 = np.random.rand(2) * 2.0 - 1.0
x_f = np.copy(x_0)
u_lim = random() * 5.0 + 0.5
di = DoubleIntegrator(dt=0.001, u_sys_lim=u_lim)
t_f_opt = di.time_optimal_solution(x_0, x_f)[1]
t_f = t_f_opt + 1.0
res, traj = di.energy_opt_control(x_0=x_0, x_f=x_f, t_f=t_f)
if res:
assert np.linalg.norm(x_f - traj.X[-1]) < 1e-2
def test_equal_start_end():
x_0 = np.array([0.0, 1.0])
x_f = np.array([0.0, 1.0])
di = DoubleIntegrator()
t_f_opt = di.time_optimal_solution(x_0, x_f)[1]
t_f = t_f_opt * (random() + 1)
res, traj = di.fuel_opt_control(x_0=x_0, x_f=x_f, t_f=t_f)
assert res
assert np.isclose(np.sum(np.abs(traj.S)), 0)
assert np.isclose(np.linalg.norm(x_f - traj.X[-1]), 0)
assert np.isclose(np.linalg.norm(x_0 - traj.X[0]), 0)
assert np.isclose(traj.U[-1], 0)
assert traj.t_s == traj.t_f
def test_final_state():
success_count = 0
for i in range(N_RAND):
np.random.seed(i)
x_0 = np.random.rand(2) * 2.0 - 1.0
x_f = np.random.rand(2) * 2.0 - 1.0
u_lim = 1.0
di = DoubleIntegrator(dt=0.001, u_sys_lim=u_lim)
t_f_opt = di.time_optimal_solution(x_0, x_f)[1]
t_f = t_f_opt * 1.05
res, traj = di.fuel_opt_control(x_0=x_0, x_f=x_f, t_f=t_f)
if res:
success_count += 1
assert np.linalg.norm(x_f - traj.X[-1]) < 1e-2
assert traj.t_s[0] <= traj.t_s[1]
assert traj.t_s[1] <= traj.t_f
print(' fuel optimal control success rate ', success_count, ' / ', N_RAND)
def test_switching_boundary():
x_0 = np.array([0.5, -1.0])
x_f = np.array([0.0, 0.0])
di = DoubleIntegrator(dt=0.001, u_sys_lim=1.0)
t_f_opt = di.time_optimal_solution(x_0, x_f)[1]
t_f = t_f_opt + 1.0
res, traj = di.fuel_opt_control(x_0=x_0, x_f=x_f, t_f=t_f)
assert traj.t_s[0] <= traj.t_s[1]
assert traj.t_s[1] <= traj.t_f
assert np.linalg.norm(x_0 - traj.X[0]) < 1e-3
assert np.linalg.norm(x_f - traj.X[-1]) < 1e-3
x_0 = np.array([-0.5, 1.0])
x_f = np.array([0.0, 0.0])
di = DoubleIntegrator(dt=0.001, u_sys_lim=1.0)
t_f_opt = di.time_optimal_solution(x_0, x_f)[1]
t_f = t_f_opt + 1.0
res, traj = di.fuel_opt_control(x_0=x_0, x_f=x_f, t_f=t_f)
assert traj.t_s[0] <= traj.t_s[1]
assert traj.t_s[1] <= traj.t_f
assert np.linalg.norm(x_0 - traj.X[0]) < 1e-3
assert np.linalg.norm(x_f - traj.X[-1]) < 1e-3
def __init__(self, u_sys_lim=1, dt=0.1):
self.di_x = DoubleIntegrator(u_sys_lim, dt)
self.di_y = DoubleIntegrator(u_sys_lim, dt)
class DoubleIntegrator2D:
def __init__(self, u_sys_lim=1, dt=0.1):
self.di_x = DoubleIntegrator(u_sys_lim, dt)
self.di_y = DoubleIntegrator(u_sys_lim, dt)
def time_opt_metric(self, x_0, x_f, y_0, y_f):
t_f_x = self.di_x.time_optimal_solution(x_0=x_0, x_f=x_f)[1]
t_f_y = self.di_y.time_optimal_solution(x_0=y_0, x_f=y_f)[1]
return np.max([t_f_x, t_f_y])
def steer_energy_opt(self, x_0, x_f, y_0, y_f):
s_x0, t_f_x = self.di_x.time_optimal_solution(x_0=x_0, x_f=x_f)[0:2]
s_y0, t_f_y = self.di_y.time_optimal_solution(x_0=y_0, x_f=y_f)[0:2]
t_f = np.max(np.array([t_f_x, t_f_y]))
t_f_min = t_f
t_f_max = t_f * 5.0
res_x = self.di_x.energy_optimal_solution(x_0=x_0,
x_f=x_f,
t_f=t_f_max)[0]
res_y = self.di_y.energy_optimal_solution(x_0=y_0,
x_f=y_f,
t_f=t_f_max)[0]
if not (res_x and res_y):
return False, None, None
# binary search to find shortest feasible duration
n_iter = 10
for i in range(n_iter):
t_f_i = (t_f_max + t_f_min) / 2.0
res_x = self.di_x.energy_optimal_solution(x_0=x_0,
x_f=x_f,
t_f=t_f_i)[0]
res_y = self.di_y.energy_optimal_solution(x_0=y_0,
x_f=y_f,
t_f=t_f_i)[0]
if res_x and res_y:
t_f_max = t_f_i
else:
t_f_min = t_f_i
t_f_opt = t_f_max
res_x, traj_x = self.di_x.energy_opt_control(x_0=x_0,
x_f=x_f,
t_f=t_f_opt)
res_y, traj_y = self.di_y.energy_opt_control(x_0=y_0,
x_f=y_f,
t_f=t_f_opt)
return True, traj_x, traj_y
def steer_time_opt(self, x_0, x_f, y_0, y_f):
s_x0, t_f_x = self.di_x.time_optimal_solution(x_0=x_0, x_f=x_f)[0:2]
s_y0, t_f_y = self.di_y.time_optimal_solution(x_0=y_0, x_f=y_f)[0:2]
if np.isclose(t_f_x, 0) and np.isclose(t_f_y,
0): # trivial case: start==goal
traj_x = Trajectory(t_f=0,
t_s=0,
x_s=x_0,
x_0=x_0,
x_f=x_f,
X=np.array([y_f]),
U=np.zeros(1),
S=np.zeros(1),
T=np.zeros(1))
traj_y = Trajectory(t_f=0,
t_s=0,
x_s=y_0,
x_0=y_0,
x_f=y_f,
X=np.array([y_f]),
U=np.zeros(1),
S=np.zeros(1),
T=np.zeros(1))
return True, traj_x, traj_y
elif np.isclose(t_f_x, 0): # movement only along y direction
res, traj_y = self.di_y.time_opt_control(x_0=y_0, x_f=y_f)
n = len(traj_y.T)
X = np.repeat(np.array([x_0]), n, axis=0)
traj_x = Trajectory(t_f=t_f_y,
t_s=0,
x_s=y_0,
x_0=x_0,
x_f=x_f,
X=X,
U=np.zeros(n),
S=np.zeros(n),
T=traj_y.T)
return True, traj_x, traj_y
elif np.isclose(t_f_y, 0): # movement only along x direction
res, traj_x = self.di_x.time_opt_control(x_0=x_0, x_f=x_f)
n = len(traj_x.T)
Y = np.repeat(np.array([y_0]), n, axis=0)
traj_y = Trajectory(t_f=t_f_x,
t_s=0,
x_s=y_0,
x_0=y_0,
x_f=y_f,
X=Y,
U=np.zeros(n),
S=np.zeros(n),
T=traj_x.T)
return True, traj_x, traj_y
elif np.isclose(t_f_x,
t_f_y): # x and y motion take same amount of time
res, traj_x = self.di_x.time_opt_control(x_0=x_0, x_f=x_f)
res, traj_y = self.di_y.time_opt_control(x_0=y_0, x_f=y_f)
return True, traj_x, traj_y
elif t_f_x < t_f_y: # x motion is faster than y, adapt x to y's duration
res, traj_y = self.di_y.time_opt_control(x_0=y_0, x_f=y_f)
# case 1: bang bang with t_f_x
u_lim_new = self.di_x.control_limit_from_final_time(x_0=x_0,
x_f=x_f,
t_f=t_f_y,
s_0=s_x0)
t_f_x = self.di_x.time_optimal_solution(x_0=x_0,
x_f=x_f,
u_lim=u_lim_new)[1]
if np.isclose(t_f_x, t_f_y):
res, traj_x = self.di_x.time_opt_control(x_0=x_0,
x_f=x_f,
u_lim=u_lim_new)
return True, traj_x, traj_y
# case 2: inverted bang bang with t_f_y
u_lim_new = self.di_x.control_limit_from_final_time(x_0=x_0,
x_f=x_f,
t_f=t_f_y,
s_0=-s_x0)
if 0 < u_lim_new < self.di_x.u_sys_lim:
t_f_x = self.di_x.time_optimal_solution(x_0=x_0,
x_f=x_f,
u_lim=u_lim_new)[1]
if np.isclose(t_f_x, t_f_y):
res, traj_x = self.di_x.time_opt_control(x_0=x_0,
x_f=x_f,
u_lim=u_lim_new)
return True, traj_x, traj_y
# case 3: fuel optimal control [bang-zero-bang with t_f_y
res, traj_x = self.di_x.fuel_opt_control(x_0=x_0,
x_f=x_f,
t_f=t_f_y)
if res:
return True, traj_x, traj_y
# no feasible trajectory found
return False, None, None
elif t_f_x > t_f_y: # y motion is faster than x, adapt y to x's duration
res, traj_x = self.di_x.time_opt_control(x_0=x_0, x_f=x_f)
# case 1: bang bang with t_f_x
u_lim_new = self.di_y.control_limit_from_final_time(x_0=y_0,
x_f=y_f,
t_f=t_f_x,
s_0=s_y0)
t_f_y = self.di_x.time_optimal_solution(x_0=y_0,
x_f=y_f,
u_lim=u_lim_new)[1]
if np.isclose(t_f_x, t_f_y):
res, traj_y = self.di_y.time_opt_control(x_0=y_0,
x_f=y_f,
u_lim=u_lim_new)
return True, traj_x, traj_y
# case 2: inverted bang bang with t_f_x
u_lim_new = self.di_y.control_limit_from_final_time(x_0=y_0,
x_f=y_f,
t_f=t_f_x,
s_0=-s_y0)
if 0 < u_lim_new < self.di_y.u_sys_lim:
t_f_y = self.di_y.time_optimal_solution(x_0=y_0,
x_f=y_f,
u_lim=u_lim_new)[1]
if np.isclose(t_f_x, t_f_y):
res, traj_y = self.di_y.time_opt_control(x_0=y_0,
x_f=y_f,
u_lim=u_lim_new)
return True, traj_x, traj_y
# case 3: fuel optimal control with t_f_x
res, traj_y = self.di_y.fuel_opt_control(x_0=y_0,
x_f=y_f,
t_f=t_f_x)
if res:
return True, traj_x, traj_y
# no feasible trajectory found
return False, None, None
def plot_xy(self, traj_x, traj_y, resize=True):
import matplotlib.pyplot as plt
if resize:
ax = plt.gca()
ax.set_aspect('equal')
ax.grid(True, which='both')
lim = np.max(
np.abs(np.concatenate([traj_x.X[:, 0], traj_y.X[:, 0]]))) * 1.2
plt.ylim([-lim, lim])
plt.xlim([-lim, lim])
plt.xlabel('x')
plt.ylabel('y')
plt.plot(traj_x.X[:, 0], traj_y.X[:, 0])
delta = 0.1
plt.plot(traj_x.x_0[0], traj_y.x_0[0], '.c')
plt.plot(traj_x.x_f[0], traj_y.x_f[0], '.g')
plt.arrow(traj_x.x_0[0],
traj_y.x_0[0],
delta * traj_x.x_0[1],
delta * traj_y.x_0[1],
color='c',
width=0.01)
plt.arrow(traj_x.x_f[0],
traj_y.x_f[0],
delta * traj_x.x_f[1],
delta * traj_y.x_f[1],
color='g',
width=0.01)
def plot_traj(self, traj_x, traj_y):
traj_x.plot_traj()
traj_y.plot_traj()
def __init__(self, agent_num=0, tot_agents=1):
DoubleIntegrator.__init__(self)
rospy.init_node('agent{}'.format(agent_num))
self.rate = rospy.Rate(30)
self.agent_num = agent_num
self.tot_agents = tot_agents
self.agent_name = 'agent{}'.format(agent_num)
self.model = DoubleIntegrator()
self.t_dist = TargetDist(
num_nodes=3) #TODO: remove example target distribution
self.controller = RTErgodicControl(self.model,
self.t_dist,
horizon=15,
num_basis=5,
batch_size=200,
capacity=500) #, batch_size=-1)
# setting the phik on the ergodic controller
self.controller.phik = convert_phi2phik(self.controller.basis,
self.t_dist.grid_vals,
self.t_dist.grid)
self.tdist_sub = rospy.Subscriber('/target_distribution', Target_dist,
self.update_tdist)
self.reset() # reset the agent
self.broadcast = tf.TransformBroadcaster()
self.broadcast.sendTransform(
(self.state[0], self.state[1], 1.0),
(0, 0, 0, 1), # quaternion
rospy.Time.now(),
self.agent_name,
"world")
self.__max_points = 1000
self.marker_pub = rospy.Publisher(
'/agent{}/marker_pose'.format(agent_num), Marker, queue_size=1)
self.marker = Marker()
self.marker.id = agent_num
self.marker.type = Marker.LINE_STRIP
self.marker.header.frame_id = "world"
self.marker.scale.x = .01
self.marker.scale.y = .01
self.marker.scale.z = .01
color = [0.0, 0.0, 0.0]
color[agent_num] = 1.0
self.marker.color.a = .7
self.marker.color.r = color[0]
self.marker.color.g = color[1]
self.marker.color.b = color[2]
# Publisher for Ck Sharing
self.ck_pub = rospy.Publisher('agent{}/ck'.format(agent_num),
Ck_msg,
queue_size=1)
# Ck Subscriber for all agent ck's
self.ck_list = [None] * tot_agents
for i in range(tot_agents):
rospy.Subscriber('agent{}/ck'.format(i), Ck_msg, self.update_ck)