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
