def render(self, mode='human', close=False): if self.fig is None: # rendering parameters pl.close("all") pl.ion() self.fig = pl.figure("Flying Skills") self.axis3d = self.fig.add_subplot(111, projection='3d') self.vis = ani.Visualization(self.iris, 6, quaternion=True) pl.figure("Flying Skills") self.axis3d.cla() self.vis.draw3d_quat(self.axis3d) self.axis3d.plot(self.x_para, self.y_para, self.z_para) cir = pl.Circle((self.goal_xyz[0], self.goal_xyz[1]), 0.5) self.axis3d.add_patch(cir) art3d.pathpatch_2d_to_3d(cir, z=self.goal_xyz[2], zdir="z") self.axis3d.set_xlim(-3, 3) self.axis3d.set_ylim(-3, 3) self.axis3d.set_zlim(-3, 3) self.axis3d.set_xlabel('West/East [m]') self.axis3d.set_ylabel('South/North [m]') self.axis3d.set_zlabel('Down/Up [m]') self.axis3d.set_title("Time %.3f s" % (self.t)) pl.pause(0.001) pl.draw()
def render(self, mode='human', close=False): if self.fig is None: pl.close("all") pl.ion() self.fig = pl.figure("Straight Level Flight") self.axis3d = self.fig.add_subplot(111, projection='3d') self.vis = ani.Visualization(self.iris, 6, quaternion=True) pl.figure("Straight Level Flight") self.axis3d.cla() self.vis.draw3d_quat(self.axis3d) self.vis.draw_goal(self.axis3d, self.goal_xyz) #Move environment with agent self.axis3d.set_xlim(-3 + self.x_pos, 3 + self.x_pos) self.axis3d.set_ylim(-3, 3) self.axis3d.set_zlim(-3, 3) self.axis3d.set_xlabel('West/East [m]') self.axis3d.set_ylabel('South/North [m]') self.axis3d.set_zlabel('Down/Up [m]') self.axis3d.set_title("Time %.3f s" %(self.t)) xyz, _, _, _ = self.iris.get_state() start_line_pos = [max(0.0, self.x_pos - 3.0), 0, 0]; end_line_pos = [3 + self.x_pos, 0, 0]; color = [0,1,0]; if(not self.on_path): color = [1,0,0]; self.vis.draw_line(self.axis3d, start_line_pos, end_line_pos, color=color); self.vis.draw_line(self.axis3d, xyz.T.tolist()[0], (xyz + self.vec_to_path).T.tolist()[0], color=[1,0,1]); pl.pause(0.0001) pl.draw()
def init_rendering(self): # rendering parameters pl.close("all") pl.ion() self.fig = pl.figure("Hover") self.axis3d = self.fig.add_subplot(111, projection='3d') self.vis = ani.Visualization(self.iris, 6, quaternion=True)
def render(self, mode='human', close=False): #Legacy render model can still be used for debugging if(mode == 'debug'): if self.fig is None: # rendering parameters pl.close("all") pl.ion() self.fig = pl.figure("Target Following") self.axis3d = self.fig.add_subplot(111, projection='3d') self.vis = ani.Visualization(self.iris, 6, quaternion=True) pl.figure("Target Following") self.axis3d.cla() #Draw the quadrotor self.vis.draw3d_quat(self.axis3d) #Draw the goal point self.vis.draw_goal(self.axis3d, self.goal_xyz) #Set the limits of the container to the given dimensions self.axis3d.set_xlim(-self.x_dim, self.x_dim) self.axis3d.set_ylim(-self.y_dim, self.y_dim) self.axis3d.set_zlim(-self.z_dim, self.z_dim) self.axis3d.set_xlabel('West/East [m]') self.axis3d.set_ylabel('South/North [m]') self.axis3d.set_zlabel('Down/Up [m]') self.axis3d.set_title("Time %.3f s" %(self.t)) #Get the position of the agent xyz, _, _, _, = self.iris.get_state(); #Draw a line between goal and quadrotor, that will be more green depending on how close #the distance is to the goal distance to help visualize goal distance if(self.dist_hat < self.goal_dist): self.vis.draw_line(self.axis3d, self.goal_xyz.T.tolist()[0], xyz.T.tolist()[0], color=[0,1,0]) else: self.vis.draw_line(self.axis3d, self.goal_xyz.T.tolist()[0], xyz.T.tolist()[0], color=[1,0,0]) self.vis.draw_goal(self.axis3d, xyz + self.closest_goal_pos, color=[0,0,1]); pl.pause(0.001) pl.draw() else: self.renderGl(mode=mode);
def render(self, mode='human', close=False): if self.fig is None: pl.close("all") pl.ion() self.fig = pl.figure("Hover") self.axis3d = self.fig.add_subplot(111, projection='3d') self.vis = ani.Visualization(self.iris, 6, quaternion=True) pl.figure("Hover") self.axis3d.cla() self.vis.draw3d_quat(self.axis3d) self.vis.draw_goal(self.axis3d, self.goal_xyz) self.axis3d.set_xlim(-3, 3) self.axis3d.set_ylim(-3, 3) self.axis3d.set_zlim(-3, 3) self.axis3d.set_xlabel('West/East [m]') self.axis3d.set_ylabel('South/North [m]') self.axis3d.set_zlabel('Down/Up [m]') self.axis3d.set_title("Time %.3f s" % (self.t)) pl.pause(0.001) pl.draw()
def render(self, mode='human', close=False): if self.fig is None: # rendering parameters pl.close("all") pl.ion() self.fig = pl.figure("Flying Skills") self.axis3d = self.fig.add_subplot(111, projection='3d') self.vis = ani.Visualization(self.iris, 6, quaternion=True) pl.figure("Flying Skills") self.axis3d.cla() self.vis.draw3d_quat(self.axis3d) xyz, zeta, uvw, pqr = self.iris.get_state() # if self.distance_decrease: # self.axis3d.plot([self.goal_xyz[0][0],xyz[0][0]],[self.goal_xyz[1][0],xyz[1][0]],[self.goal_xyz[2][0],xyz[2][0]],c='g') # else: # self.axis3d.plot([self.goal_xyz[0][0],xyz[0][0]],[self.goal_xyz[1][0],xyz[1][0]],[self.goal_xyz[2][0],xyz[2][0]],c='r') #n_obj = self.get_nearest_obstacle(xyz)[1] #self.axis3d.plot([n_obj.get_center()[0][0],xyz[0][0]],[n_obj.get_center()[1][0],xyz[1][0]],[n_obj.get_center()[2][0],xyz[2][0]],c='b') #self.sensor.draw_sensors(self.axis3d) for c in self.obstacles: c.plot_sphere(self.axis3d) for p1 in self.circle_sensors: p1.plot(self.axis3d) self.vis.draw_goal(self.axis3d, self.goal_xyz) self.axis3d.set_xlim(-3, 3) self.axis3d.set_ylim(-3, 3) self.axis3d.set_zlim(-3, 3) self.axis3d.set_xlabel('West/East [m]') self.axis3d.set_ylabel('South/North [m]') self.axis3d.set_zlabel('Down/Up [m]') self.axis3d.set_title("Time %.3f s" % (self.t)) pl.pause(0.001) pl.draw()
def main(): pl.close("all") pl.ion() fig = pl.figure(0) axis3d = fig.add_subplot(111, projection='3d') params = cfg.params iris = quad.Quadrotor(params) T = 3.5 sim_dt = iris.dt ctrl_dt = 0.05 dt_ratio = int(ctrl_dt/sim_dt) time = np.linspace(0, T, T/ctrl_dt) hover_rpm = iris.hov_rpm trim = np.array([hover_rpm, hover_rpm, hover_rpm, hover_rpm]) vis = ani.Visualization(iris, 10) counter = 0 frames = 2 rpm = trim+50 for t in time: for k in range(dt_ratio): iris.step(rpm) if counter%frames == 0: pl.figure(0) axis3d.cla() vis.draw3d(axis3d) axis3d.set_xlim(-3, 3) axis3d.set_ylim(-3, 3) axis3d.set_zlim(0, 6) axis3d.set_xlabel('West/East [m]') axis3d.set_ylabel('South/North [m]') axis3d.set_zlabel('Down/Up [m]') axis3d.set_title("Time %.3f s" %t) pl.pause(0.001) pl.draw() rpm += np.array([0., 0., 0., 0.25])
def main(): pl.close("all") pl.ion() fig = pl.figure(0) axis3d = fig.add_subplot(111, projection='3d') params = cfg.params iris = quad.Quadrotor(params) vis = ani.Visualization(iris, 10) goal_zeta = np.array([[0.], [0.], [0.]]) goal_xyz = np.array([[3.], [0.], [3.]]) xyz_init = np.array([[0.], [0.], [1.5]]) uvw_init = np.array([[0.], [0.], [0.]]) pqr_init = np.array([[0.], [0.], [0.]]) # add some noise to the initial attitude of the aircraft eps = np.random.rand(3, 1) / 10. zeta_init = goal_zeta + eps iris.set_state(xyz_init, zeta_init, uvw_init, pqr_init) xyz, zeta, uvw, pqr = iris.get_state() # initialize a PD controller by setting the I term to zero pids = { "linear": { "p": np.array([[1.], [1.], [1.]]), "i": np.array([[0.], [0.], [0.]]), "d": np.array([[1.], [1.], [1.]]) }, "angular": { "p": np.array([[0.5], [0.5], [0.05]]), "i": np.array([[0.], [0.], [0.]]), "d": np.array([[0.2], [0.2], [0.2]]) } } targets = {"xyz": goal_xyz, "zeta": goal_zeta} controller = ctrl.PID_Controller(iris, pids) counter = 0 frames = 100 running = True done = False t = 0. i = 1 H = 5 sim_dt = iris.dt ctrl_dt = 0.001 sim_steps = int(ctrl_dt / sim_dt) while running: states = {"xyz": xyz, "zeta": zeta} if counter % frames == 0: pl.figure(0) axis3d.cla() vis.draw3d(axis3d) vis.draw_goal(axis3d, goal_xyz) axis3d.set_xlim(-3, 3) axis3d.set_ylim(-3, 3) axis3d.set_zlim(0, 6) axis3d.set_xlabel('West/East [m]') axis3d.set_ylabel('South/North [m]') axis3d.set_zlabel('Down/Up [m]') axis3d.set_title("Time %.3f s" % t) pl.pause(0.001) pl.draw() i += 1 actions = controller.action(targets, states) for j in range(sim_steps): xyz, zeta, uvw, pqr = iris.step(actions, rpm_commands=False) done = terminal(xyz, zeta, uvw, pqr) t += ctrl_dt counter += 1 if t > H: break if done: print("Resetting vehicle to: {}, {}, {}, {}".format( xyz_init, zeta_init, uvw_init, pqr_init)) iris.set_state(xyz_init, zeta_init, uvw_init, pqr_init) xyz, zeta, uvw, pqr = iris.get_state() t = 0 counter = 0 done = False