def testFilterInputPlot(self):
from matplotlib import gridspec
x = [0.25, 0.25, 0.3, 0, 0]
config = dwa.generate_inter_config()
u_in = [0.08, 0.1]
dt = 0.1
n = 50
states = np.zeros((n, 5))
t = np.linspace(0, dt * n, n, False)
x0, x1, y0, y1 = domain = dwa.interconfig.domain
for i in range(n):
dw = dwa.calc_dynamic_window(x, config)
u_f, cost = dwa.calc_filter_input(x, dw, u_in, config)
# u_f,cost = dwa.calc_filter_input_cont(x,dw,u_in,config)
xn = dwa.motion(x, u=u_f, dt=dt)
x = xn
states[i, :] = x
zv = np.zeros(n)
fig = plt.figure()
gs = gridspec.GridSpec(4, 5)
ax = fig.add_subplot(gs[:, :2])
ax.plot(*dwa.domain_plot_coordinates(domain), '-.k', label='domain')
ax2 = fig.add_subplot(gs[0, 2:])
ax3 = fig.add_subplot(gs[1, 2:])
ax4 = fig.add_subplot(gs[2, 2:])
ax5 = fig.add_subplot(gs[3, 2:])
ax.plot(states[:, 0], states[:, 1], label='Trajectory')
ax2.plot(t, states[:, 0], label='x')
ax2.plot(t, zv + x0, '-.k', label='bounds')
ax2.plot(t, zv + x1, '-.k')
ax3.plot(t, states[:, 1], label='y')
ax3.plot(t, zv + y0, '-.k', label='bounds')
ax3.plot(t, zv + y1, '-.k')
ax4.plot(t, states[:, 3], label='$\\upsilon$')
ax4.plot(t,
states[:, 3] * 0 + u_in[0],
'-.k',
label='$\\upsilon_{ref}$')
ax5.plot(t,
states[:, 4] * 0 + u_in[1],
'-.k',
label='$\\dot \\theta_{ref}$')
ax5.plot(t, states[:, 4], label='$\\dot \\theta$')
ax.legend()
ax2.legend()
ax3.legend()
ax4.legend()
ax5.legend()
ax.set_xlabel("Position [m]")
ax5.set_xlabel("Time [s]")
ax.set_ylabel("Position [m]")
ax2.set_ylabel("S [m]")
ax3.set_ylabel("S [m]")
ax4.set_ylabel("v [m $s^{-1}$]")
ax5.set_ylabel("$\\omega$ [rad $s^{-1}$]")
# ax.equal()
plt.show()
def test_DWA(self):
from planebots.control import dwa
config = dwa.Config()
# config.max_speed = 0.6 # [m/s] = max elisa3
# config.min_speed = -0.6 # [m/s] = min elisa3
# config.dt = 0.2
# config.robot_radius = 0.05
# config.max_yawrate = 2* np.pi
# config.max_dyawrate = 2 * np.pi /5 # [rad/ss]
# config.obstacle_cost_gain =1
# config.to_goal_cost_gain = 0.8
config.robot_radius = 1
config.max_accel = 0.5
ob = np.array([[-1, -1], [0, 2], [4.0, 2.0], [5.0, 4.0], [5.0, 5.0],
[5.0, 6.0], [5.0, 9.0], [8.0, 9.0], [7.0, 9.0],
[12.0, 12.0]])
config.max_speed = 0.6
config.min_speed = -0.6
config.v_reso *= 3
config.yawrate_reso *= 4 # config.predict_time = 0.05
x = np.array([0.0, 0.0, 9 * np.pi / 8.0, 0.0, 0.0])
u = np.array([0.0, 0.0])
logger.debug("Starting DWA")
goal = np.array([10, 10])
traj = np.array(x)
cnt = 0
while True:
u, ptraj = dwa.dwa_control(x, u, config, goal, ob)
x = dwa.motion(x, u, config.dt) # simulate robot
cnt += 1
traj = np.vstack((traj, x)) # store state history
dist_to_goal = np.linalg.norm(x[:2] - goal[:2])
logger.debug(
f"t={config.dt*cnt:5.3f} Distance to goal: {dist_to_goal:4.2f}: ({x[0]:5.3f},{x[1]:5.3f}) speeds:({x[3]:5.3f}ms,{x[4]/np.pi*180:5.3f}deg/s)"
)
if dist_to_goal <= config.robot_radius:
break
fig, ax = plt.subplots(figsize=(10, 10))
# ax.set_ylim(0,1)
# ax.set_xlim(0,1)
ax.plot(traj[:, 0], traj[:, 1])
ax.plot(ob[:, 0], ob[:, 1], "ok")
plt.show()
logger.debug("Starting DWA")
from planebots.control import model
model.Agent
def updateElisaMeasurement(self):
"""Updates when new data from the Elisa3 robots come in."""
try:
if self.rbObsOdomUKF.isChecked() or self.rbObsFuse.isChecked():
for agent in self.agents:
ts = np.min([agent.dt, 0.5])
agent.filtere.predict(dt=ts)
uw = agent.z_uw
agent.filtere.update([uw[0], uw[1]],
hx=planebots.control.observers.h_uw,
R=np.eye(2) * [1, 1] * 1E-2)
agent.x_hat = agent.filtere.x
if self.rbObsNone.isChecked():
for agent in self.agents:
if agent.mid in gctronic.elisa_ids:
uw = [agent.z_uw[0], agent.z_uw[1]]
agent.x_hat = dwa.motion([*agent.x_hat[:3], *uw],
dt=np.min([agent.dt, 0.3]),
u=None)
except np.linalg.LinAlgError as e:
logger.error(e)
def __init__(self):
super().__init__(dim_x=5,
dim_z=3,
dt=0.01,
hx=lambda x: [x[3], x[4]],
fx=lambda x, dt, u=None: dwa.motion(x, u=u, dt=dt),
points=sigmas,
sqrt_fn=None,
x_mean_fn=None,
z_mean_fn=None,
residual_x=residual_x,
residual_z=residual_z)
self.he = lambda x: [x[3], x[4]]
self.hv = h_planebots
self.Q = np.diag([0.053, 0.053, 0.005, 1, 1])
self.P = np.eye(5) * 0 + self.Q
self.tV = time.perf_counter()
self.tE = time.perf_counter()
self.tL = time.perf_counter()
self.Rv = np.eye(3) * [1, 1, 1] * 1E-4
# self.Re = np.eye(3)*[1,1,1]*1E-4
self.Re = np.eye(2) * [1, 1] * 1E-4
def f_km(x, dt):
xk1 = np.zeros_like(x)
xk1[:5] = dwa.motion(x[:5])
return xk1