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