def draw(self):
circle = CirclePatch(self.circle.center[:2],
radius=self.circle.radius,
facecolor=self.facecolor,
edgecolor=self.edgecolor,
fill=self.fill,
zorder=self.zorder)
self._mpl_circle = self.plotter.axes.add_artist(circle)
def draw(self):
circle = CirclePatch(self.circle.center[:2],
linewidth=self.linewidth,
linestyle=self.linestyle,
radius=self.circle.radius,
facecolor=self.facecolor,
edgecolor=self.edgecolor,
fill=self.fill,
zorder=self.zorder)
self._mpl_circle = self.plotter.axes.add_artist(circle)
self.update_data()
def draw(self) -> None:
"""Draw the circle on the plotter canvas.
Returns
-------
None
"""
circle = CirclePatch(self.circle.center[:2],
linewidth=self.linewidth,
linestyle=self.linestyle,
radius=self.circle.radius,
facecolor=self.facecolor,
edgecolor=self.edgecolor,
fill=self.fill,
alpha=self.alpha,
zorder=self.zorder)
self._mpl_circle = self.plotter.axes.add_artist(circle)
self.update_data()
'uss': uss,
'Jss': Jss,
'obs_list': obs_list
}, open('unicycle_samples.pickle', 'wb'))
f = plt.figure(1)
plt.clf()
ax = f.add_subplot(111)
ax.set_aspect('equal')
plt.plot(ddp.xs[:, 0], ddp.xs[:, 1], 'b*-')
for j in range(M):
plt.plot(xss[j][:, 0], xss[j][:, 1], 'g*-')
plt.plot(xd[0], xd[1], 'r*')
plt.xlabel('x (m)')
plt.ylabel('y (m)')
for obs in obs_list:
circ_patch = CirclePatch(obs.center, obs.radius, fill=False, ec='r')
ax.add_patch(circ_patch)
ax.plot(obs.center[0], obs.center[1], 'r*')
plt.figure(2)
plt.subplot(2, 1, 1)
plt.plot(ts[:-1], ddp.us[:, 0])
plt.ylabel('Velocity (m/s)')
plt.subplot(2, 1, 2)
plt.plot(ts[:-1], ddp.us[:, 1])
plt.ylabel('Angular rate (rad/s)')
plt.figure(3)
plt.plot(ts, ddp.xs[:, 2])
plt.xlabel('Time (seconds)')
plt.ylabel('Angle (radians)')
plt.show()
def singleTrial(M=100, plot=False, sigma_inflation=1.0):
# Run regular ddp first:
us_opt, Ks_opt = regularDdp()
BufferedSphericalObstacle.sigma_inflation = sigma_inflation
# ksigma is cost on trying to minimize ellipsoids
cost = RobustLQRObstacleCost(N, Q, R, Qf, xd, ko=5000, obstacles=obs_list,
kSigma = kSigma, ud=ud)
ddp = RobustDdp(dynamics, cost, us_opt, x0, dt, max_step, Sigma0, Sigma_w,
integrator=integrator, Ks=Ks_opt)
V = ddp.V
print("V0: ", V)
for i in range(max_iters):
#cost.ko = np.tanh(i*ko_gain+0.5*max_iters)*max_ko
#ddp.update_dynamics(ddp.us, ddp.xs)
ddp.iterate()
V = ddp.V
print("Vfinal: ", V)
# Sample example trajectories
# Stdeviation!!:
Sigma0_sqr = np.square(Sigma0)
Sigmaw_sqr = np.square(Sigma_w)
ws_sampling_fun = lambda : np.random.multivariate_normal(np.zeros(dynamics.n),
Sigmaw_sqr)
x0_sampling_fun = lambda : np.random.multivariate_normal(x0, Sigma0_sqr)
sampler = DiscreteSampleTrajectories(dynamics, integrator, cost,
ws_sampling_fun, x0_sampling_fun)
cost.ko = 0 #Ignore obstacle avoidance when computing costs
BufferedSphericalObstacle.buffer_ellipsoid = None
cost.ksigma = 0
xss, uss, Jss = sampler.sample(M, ts, ddp)
collision_array = np.full(M, False)
for i, sample_traj in enumerate(xss):
collision_array[i] = sampler.isColliding(obs_list, sample_traj)
Ncollisions = np.sum(collision_array)
print("Ncollisions: ", Ncollisions)
print("Jmean: ", np.mean(Jss[:, 0]))
print("Jstd: ", np.std(Jss[:, 0]))
sigma_infl_str = '{0:.1f}'.format(sigma_inflation)
sigma_infl_str = sigma_infl_str.replace('.','_')
if plot:
f = plt.figure(1)
plt.clf()
ax = f.add_subplot(111)
ax.set_aspect('equal')
plt.plot(ddp.xs[:, 0], ddp.xs[:, 1], 'b*-')
for j in range(M):
if collision_array[j]:
color='m*-'
else:
color='g*-'
plt.plot(xss[j][:,0], xss[j][:, 1], color)
plt.plot(xd[0], xd[1], 'r*')
plt.xlabel('x (m)')
plt.ylabel('y (m)')
for obs in obs_list:
circ_patch = CirclePatch(obs.center, obs.radius, fill=False,
ec='r')
ax.add_patch(circ_patch)
ax.plot(obs.center[0], obs.center[1], 'r*')
for i, Sigma_i in enumerate(ddp.Sigma):
ellipse = findEllipsoid(ddp.xs[i], sigma_inflation*Sigma_i)
plotEllipse(3, ellipse, ax)
ax.set_xlim(left=-Sigma0[0,0])
ax.set_ylim(bottom=-Sigma0[1,1])
plt.tight_layout()
plt.savefig('trajectories_'+sigma_infl_str+'.eps', bbox_inches='tight')
plt.figure(2)
plt.clf()
plt.subplot(2, 1, 1)
plt.plot(ts[:-1], ddp.us[:, 0])
plt.ylabel('Velocity (m/s)')
plt.subplot(2, 1, 2)
plt.plot(ts[:-1], ddp.us[:, 1])
plt.ylabel('Angular rate (rad/s)')
plt.tight_layout()
plt.savefig('controls_'+sigma_infl_str+'.eps', bbox_inches='tight')
plt.figure(3)
plt.clf()
plt.plot(ts, ddp.xs[:, 2])
plt.xlabel('Time (seconds)')
plt.ylabel('Angle (radians)')
plt.tight_layout()
plt.savefig('angle_'+sigma_infl_str+'.eps', bbox_inches='tight')
return Ncollisions, Jss
def singleTrial(M=100, plot=False, buf=0.0, return_ddp=False):
SphericalObstacle.buf = buf
us0 = np.tile(ud, (N, 1))
cost = LQRObstacleCost(N,
Q,
R,
Qf,
xd,
ko=ko_start,
obstacles=obs_list,
ud=ud)
ddp = Ddp(dynamics, cost, us0, x0, dt, max_step, integrator=integrator)
V = ddp.V
print("V0: ", V)
for i in range(max_iters):
cost.ko = np.tanh(i * ko_gain) * (max_ko - ko_start) + ko_start
ddp.update_dynamics(ddp.us, ddp.xs)
ddp.iterate()
V = ddp.V
print("Vfinal: ", V)
# Sample example trajectories
# Stdeviation!!:
Sigma0_sqr = np.diag(np.square(Sigma0))
Sigmaw_sqr = np.diag(np.square(Sigma_w))
ws_sampling_fun = lambda: np.random.multivariate_normal(
np.zeros(dynamics.n), Sigmaw_sqr)
x0_sampling_fun = lambda: np.random.multivariate_normal(x0, Sigma0_sqr)
sampler = DiscreteSampleTrajectories(dynamics, integrator, cost,
ws_sampling_fun, x0_sampling_fun)
cost.ko = 0 #Ignore obstacle avoidance when computing costs
SphericalObstacle.buf = 0 #Ignore buffer when checking actual collisions
xss, uss, Jss = sampler.sample(M, ts, ddp)
collision_array = np.full(M, False)
for i, sample_traj in enumerate(xss):
collision_array[i] = sampler.isColliding(obs_list, sample_traj)
Ncollisions = np.sum(collision_array)
print("Ncollisions: ", Ncollisions)
print("Jmean: ", np.mean(Jss[:, 0]))
print("Jstd: ", np.std(Jss[:, 0]))
buf_str = '{0:.3f}'.format(buf)
buf_str = buf_str.replace('.', '_')
if plot:
f = plt.figure(1)
plt.clf()
ax = f.add_subplot(111)
ax.set_aspect('equal')
plt.plot(ddp.xs[:, 0], ddp.xs[:, 1], 'b*-')
for j in range(M):
if collision_array[j]:
color = 'm*-'
else:
color = 'g*-'
plt.plot(xss[j][:, 0], xss[j][:, 1], color)
plt.plot(xd[0], xd[1], 'r*')
plt.xlabel('x (m)')
plt.ylabel('y (m)')
for obs in obs_list:
circ_patch = CirclePatch(obs.center,
obs.radius,
fill=False,
ec='r')
ax.add_patch(circ_patch)
ax.plot(obs.center[0], obs.center[1], 'r*')
plt.tight_layout()
plt.savefig('trajectories_' + buf_str + '.eps', bbox_inches='tight')
plt.figure(2)
plt.clf()
plt.subplot(2, 1, 1)
plt.plot(ts[:-1], ddp.us[:, 0])
plt.ylabel('Velocity (m/s)')
plt.subplot(2, 1, 2)
plt.plot(ts[:-1], ddp.us[:, 1])
plt.ylabel('Angular rate (rad/s)')
plt.tight_layout()
plt.savefig('controls_' + buf_str + '.eps', bbox_inches='tight')
plt.figure(3)
plt.clf()
plt.plot(ts, ddp.xs[:, 2])
plt.xlabel('Time (seconds)')
plt.ylabel('Angle (radians)')
plt.tight_layout()
plt.savefig('angle_' + buf_str + '.eps', bbox_inches='tight')
if return_ddp:
return Ncollisions, Jss, ddp
return Ncollisions, Jss