def __init__(self):
# environment parameters
self.deterministic_s0 = True
self.goal = self.generate_goal(1.5)
self.goal_thresh = 0.1
self.t = 0
self.T = 2.5
self.r = 1.5
self.action_space = 4
self.observation_space = 15+self.action_space+3
# simulation parameters
self.params = cfg.params
self.iris = quad.Quadrotor(self.params)
self.ctrl_dt = 0.05
self.sim_dt = self.params["dt"]
self.steps = range(int(self.ctrl_dt/self.sim_dt))
self.hov_rpm = self.iris.hov_rpm
self.trim = [self.hov_rpm, self.hov_rpm,self.hov_rpm, self.hov_rpm]
self.action_bound = [0, self.iris.max_rpm]
self.H = int(self.T/self.ctrl_dt)
# define bounds here
self.xzy_bound = 1.
self.zeta_bound = pi/2
self.uvw_bound = 10
self.pqr_bound = 1.
self.vec = None
self.dist_norm = None
def __init__(self):
metadata = {'render.modes': ['human']}
# environment parameters
self.deterministic_s0 = True
self.t = 0
self.T = 15
self.r = 1.5
self.action_space = 4
self.observation_space = 15+self.action_space
# simulation parameters
self.params = cfg.params
self.iris = quad.Quadrotor(self.params)
self.sim_dt = self.params["dt"]
self.ctrl_dt = 0.05
self.steps = range(int(self.ctrl_dt/self.sim_dt))
self.action_bound = [0, self.iris.max_rpm]
self.H = int(self.T/self.ctrl_dt)
self.hov_rpm = self.iris.hov_rpm
self.trim = [self.hov_rpm, self.hov_rpm,self.hov_rpm, self.hov_rpm]
self.trim_np = np.array(self.trim)
self.bandwidth = 25.
# define bounds here
self.xzy_bound = 1.
self.zeta_bound = pi/2
self.uvw_bound = 10
self.pqr_bound = 1.
self.fig = None
self.axis3d = None
self.v = None
def __init__(self):
metadata = {'render.modes': ['human']}
# environment parameters
self.r_max = 1.5
self.goal_xyz = self.generate_goal()
self.goal_zeta_sin = np.sin(np.array([[0.],
[0.],
[0.]]))
self.goal_zeta_cos = np.cos(np.array([[0.],
[0.],
[0.]]))
self.goal_uvw = np.array([[0.], ##XYZ of actual quad
[0.],
[0.]])
self.goal_pqr = np.array([[0.], ##PQR
[0.],
[0.]])
self.t = 0
self.T = 10
self.time_state = self.T
self.action_space = np.zeros((4,))
self.observation_space = np.zeros((35,))
# simulation parameters
self.params = cfg.params
self.iris = quad.Quadrotor(self.params)
self.sim_dt = self.params["dt"]
self.ctrl_dt = 0.05
self.steps = range(int(self.ctrl_dt/self.sim_dt))
self.action_bound = [0, self.iris.max_rpm]
self.H = int(self.T/self.ctrl_dt)
self.hov_rpm = self.iris.hov_rpm
self.trim = [self.hov_rpm, self.hov_rpm,self.hov_rpm, self.hov_rpm]
self.trim_np = np.array(self.trim)
self.prev_action = self.trim_np.copy()
self.bandwidth = 35.
self.iris.set_state(self.goal_xyz, np.zeros((3,1)), np.zeros((3,1)), np.zeros((3,1)))
xyz, zeta, uvw, pqr = self.iris.get_state()
self.vec_xyz = xyz-self.goal_xyz
self.vec_zeta_sin = np.sin(zeta)-self.goal_zeta_sin
self.vec_zeta_cos = np.cos(zeta)-self.goal_zeta_cos
self.vec_uvw = uvw-self.goal_uvw
self.vec_pqr = pqr-self.goal_pqr
self.dist_norm = np.linalg.norm(self.vec_xyz)
self.att_norm_sin = np.linalg.norm(self.vec_zeta_sin)
self.att_norm_cos = np.linalg.norm(self.vec_zeta_cos)
self.vel_norm = np.linalg.norm(self.vec_uvw)
self.ang_norm = np.linalg.norm(self.vec_pqr)
self.prev_uvw = np.array([[0.],[0.],[0.]])
self.prev_pqr = np.array([[0.],[0.],[0.]])
self.init_rendering = False
def __init__(self):
metadata = {'render.modes': ['human']}
# environment parameters
self.goal_xyz = np.array([[1.5],
[0.],
[0.]])
self.goal_zeta_sin = np.sin(np.array([[0.],
[0.],
[0.]]))
self.goal_zeta_cos = np.cos(np.array([[0.],
[0.],
[0.]]))
self.goal_uvw = np.array([[0.],
[0.],
[0.]])
self.goal_pqr = np.array([[0.],
[0.],
[0.]])
self.goal_thresh = 0.05
self.t = 0
self.T = 3
self.action_space = np.zeros((4,))
self.observation_space = np.zeros((34,))
# simulation parameters
self.params = cfg.params
self.iris = quad.Quadrotor(self.params)
self.sim_dt = self.params["dt"]
self.ctrl_dt = 0.05
self.steps = range(int(self.ctrl_dt/self.sim_dt))
self.action_bound = [0, self.iris.max_rpm]
self.H = int(self.T/self.ctrl_dt)
self.hov_rpm = self.iris.hov_rpm
self.trim = [self.hov_rpm, self.hov_rpm,self.hov_rpm, self.hov_rpm]
self.trim_np = np.array(self.trim)
self.prev_action = self.trim_np.copy()
self.bandwidth = 35.
xyz, zeta, uvw, pqr = self.iris.get_state()
self.vec_xyz = xyz-self.goal_xyz
self.vec_zeta_sin = np.sin(zeta)-self.goal_zeta_sin
self.vec_zeta_cos = np.cos(zeta)-self.goal_zeta_cos
self.vec_uvw = uvw-self.goal_uvw
self.vec_pqr = pqr-self.goal_pqr
self.dist_norm = np.linalg.norm(self.vec_xyz)
self.att_norm_sin = np.linalg.norm(self.vec_zeta_sin)
self.att_norm_cos = np.linalg.norm(self.vec_zeta_cos)
self.vel_norm = np.linalg.norm(self.vec_uvw)
self.ang_norm = np.linalg.norm(self.vec_pqr)
self.fig = None
self.axis3d = None
self.v = None
def __init__(self):
metadata = {'render.modes': ['human']}
self.r_max = 2.5
self.goal_thresh = 0.05
self.t = 0
self.T = 10
self.action_space = np.zeros((4, ))
self.observation_space = np.zeros((34, ))
self.goal_xyz = np.array([[0.], [0.], [0.]])
self.spawn = np.array([[random.uniform(-3, 3)],
[random.uniform(-3, 3)], [3.]])
self.goal_zeta_sin = np.sin(np.array([[0.], [0.], [0.]]))
self.goal_zeta_cos = np.cos(np.array([[0.], [0.], [0.]]))
self.goal_uvw = np.array([[0.], [0.], [0.]])
self.goal_pqr = np.array([[0.], [0.], [0.]])
# simulation parameters
self.params = cfg.params
self.iris = quad.Quadrotor(self.params)
self.sim_dt = self.params["dt"]
self.ctrl_dt = 0.05
self.steps = range(int(self.ctrl_dt / self.sim_dt))
self.action_bound = [0, self.iris.max_rpm]
self.H = int(self.T / self.ctrl_dt)
self.hov_rpm = self.iris.hov_rpm
self.trim = [self.hov_rpm, self.hov_rpm, self.hov_rpm, self.hov_rpm]
self.trim_np = np.array(self.trim)
self.prev_action = self.trim_np.copy()
self.bandwidth = 35.
# define bounds here
self.xzy_bound = 0.5
self.zeta_bound = pi / 3
self.uvw_bound = 0.5
self.pqr_bound = 0.25
self.iris.set_state(self.spawn, np.arcsin(self.goal_zeta_sin),
self.goal_uvw, self.goal_pqr)
self.iris.set_rpm(np.array(self.trim))
xyz, zeta, uvw, pqr = self.iris.get_state()
self.vec_xyz = xyz - self.goal_xyz
self.vec_zeta_sin = np.sin(zeta) - self.goal_zeta_sin
self.vec_zeta_cos = np.cos(zeta) - self.goal_zeta_cos
self.vec_uvw = uvw - self.goal_uvw
self.vec_pqr = pqr - self.goal_pqr
self.goal_decent_speed = 0
self.decent_speed = abs(self.iris.get_inertial_velocity()[2][0])
self.iner_speed = np.linalg.norm(self.iris.get_inertial_velocity())
self.dist_norm = np.linalg.norm(self.vec_xyz)
self.height = xyz[2][0]
self.att_norm_sin = np.linalg.norm(self.vec_zeta_sin)
self.att_norm_cos = np.linalg.norm(self.vec_zeta_cos)
self.vel_norm = np.linalg.norm(self.vec_uvw)
self.ang_norm = np.linalg.norm(self.vec_pqr)
self.distance_decrease = False
self.init_rendering = False
def __init__(self):
metadata = {'render.modes': ['human']}
# environment parameters
self.goal_xyz = np.array([[0.],
[0.],
[0.]])
self.goal_zeta_sin = np.sin(np.array([[0.],
[0.],
[0.]]))
self.goal_zeta_cos = np.cos(np.array([[0.],
[0.],
[0.]]))
self.goal_uvw = np.array([[0.],
[0.],
[0.]])
self.goal_pqr = np.array([[0.],
[0.],
[0.]])
self.goal_alt = 0;
self.goal_sway = 0;
self.goal_dir = np.array([[1.0],[0.0],[0.0]]);
self.vec_to_path = np.array([[0.0],[0.0],[0.0]]);
self.alt_deviance = 1.0;
self.max_alt_deviance = 2.5;
self.sway_deviance = 1.0;
self.max_sway_deviance = 2.5;
self.on_path = True;
self.too_far_from_path = False;
self.t = 0
self.T = 20
self.action_space = np.zeros((4,))
self.observation_space = np.zeros((25,))
self.params = cfg.params
self.iris = quad.Quadrotor(self.params)
self.sim_dt = self.params["dt"]
self.ctrl_dt = 0.05
self.steps = range(int(self.ctrl_dt/self.sim_dt))
self.action_bound = [0, self.iris.max_rpm]
self.H = int(self.T/self.ctrl_dt)
self.hov_rpm = self.iris.hov_rpm
self.trim = [self.hov_rpm, self.hov_rpm,self.hov_rpm, self.hov_rpm]
self.trim_np = np.array(self.trim)
self.bandwidth = 25.
self.iris.set_state(self.goal_xyz, np.arcsin(self.goal_zeta_sin), self.goal_uvw, self.goal_pqr)
xyz, zeta, uvw, pqr = self.iris.get_state()
self.fig = None
self.axis3d = None
def __init__(self):
metadata = {'render.modes': ['human']}
self.r_max = 1.5
self.goal_thresh = 0.075
self.t = 0
self.T = 3.
self.action_space = np.zeros((4,))
self.observation_space = np.zeros((34,))
self.goal_xyz = self.generate_goal()
self.goal_zeta_sin = np.sin(np.array([[0.],
[0.],
[0.]]))
self.goal_zeta_cos = np.cos(np.array([[0.],
[0.],
[0.]]))
# the velocity of the aircraft in the inertial frame is probably a better metric here, but
# since our goal state is (0,0,0), this should be fine.
self.goal_uvw = np.array([[0.],
[0.],
[0.]])
self.goal_pqr = np.array([[0.],
[0.],
[0.]])
# simulation parameters
self.params = cfg.params
self.iris = quad.Quadrotor(self.params)
self.sim_dt = self.params["dt"]
self.ctrl_dt = 0.05
self.steps = range(int(self.ctrl_dt/self.sim_dt))
self.action_bound = [0, self.iris.max_rpm]
self.H = int(self.T/self.ctrl_dt)
self.hov_rpm = self.iris.hov_rpm
self.trim = [self.hov_rpm, self.hov_rpm,self.hov_rpm, self.hov_rpm]
self.trim_np = np.array(self.trim)
self.prev_action = self.trim_np.copy()
self.bandwidth = 35.
xyz, zeta, uvw, pqr = self.iris.get_state()
self.vec_xyz = xyz-self.goal_xyz
self.vec_zeta_sin = np.sin(zeta)-self.goal_zeta_sin
self.vec_zeta_cos = np.cos(zeta)-self.goal_zeta_cos
self.vec_uvw = uvw-self.goal_uvw
self.vec_pqr = pqr-self.goal_pqr
self.dist_norm = np.linalg.norm(self.vec_xyz)
self.att_norm_sin = np.linalg.norm(self.vec_zeta_sin)
self.att_norm_cos = np.linalg.norm(self.vec_zeta_cos)
self.vel_norm = np.linalg.norm(self.vec_uvw)
self.ang_norm = np.linalg.norm(self.vec_pqr)
self.init_rendering = False
self.lazy_action = False
self.lazy_change = False
def __init__(self):
metadata = {'render.modes': ['human']}
self.r = 0.5
self.goal_thresh = 0.1
self.t = 0
self.T = 2.5
self.action_space = np.zeros((4, ))
self.observation_space = np.zeros((34, ))
self.goal_xyz = self.generate_goal(0.5)
self.goal_zeta_sin = np.sin(np.array([[0.], [0.], [0.]]))
self.goal_zeta_cos = np.cos(np.array([[0.], [0.], [0.]]))
self.goal_uvw = np.array([[0.], [0.], [0.]])
self.goal_pqr = np.array([[0.], [0.], [0.]])
# simulation parameters
self.params = cfg.params
self.iris = quad.Quadrotor(self.params)
self.ctrl_dt = 0.05
self.sim_dt = self.params["dt"]
self.steps = range(int(self.ctrl_dt / self.sim_dt))
self.hov_rpm = self.iris.hov_rpm
self.trim = [self.hov_rpm, self.hov_rpm, self.hov_rpm, self.hov_rpm]
self.trim_np = np.array(self.trim)
self.bandwidth = 25.
self.action_bound = [0, self.iris.max_rpm]
self.H = int(self.T / self.ctrl_dt)
# define bounds here
self.xzy_bound = 0.5
self.zeta_bound = pi / 3
self.uvw_bound = 0.5
self.pqr_bound = 0.25
xyz, zeta, uvw, pqr = self.iris.get_state()
self.vec_xyz = xyz - self.goal_xyz
self.vec_zeta_sin = np.sin(zeta) - self.goal_zeta_sin
self.vec_zeta_cos = np.cos(zeta) - self.goal_zeta_cos
self.vec_uvw = uvw - self.goal_uvw
self.vec_pqr = pqr - self.goal_pqr
self.dist_norm = np.linalg.norm(self.vec_xyz)
self.att_norm_sin = np.linalg.norm(self.vec_zeta_sin)
self.att_norm_cos = np.linalg.norm(self.vec_zeta_cos)
self.vel_norm = np.linalg.norm(self.vec_uvw)
self.ang_norm = np.linalg.norm(self.vec_pqr)
self.fig = None
self.axis3d = None
def __init__(self):
# environment parameters
self.goal_xyz = np.array([[0.],
[0.],
[1.5]])
self.goal_zeta_sin = np.sin(np.array([[0.],
[0.],
[0.]]))
self.goal_zeta_cos = np.cos(np.array([[0.],
[0.],
[0.]]))
self.goal_thresh = 0.05
self.t = 0
self.T = 5
self.action_space = 4
self.observation_space = 15+self.action_space+9
# simulation parameters
self.params = cfg.params
self.iris = quad.Quadrotor(self.params)
self.sim_dt = self.params["dt"]
self.ctrl_dt = 0.05
self.steps = range(int(self.ctrl_dt/self.sim_dt))
self.action_bound = [0, self.iris.max_rpm]
self.H = int(self.T/self.ctrl_dt)
self.hov_rpm = self.iris.hov_rpm
self.trim = [self.hov_rpm, self.hov_rpm,self.hov_rpm, self.hov_rpm]
self.iris.set_state(self.goal_xyz, np.arcsin(self.goal_zeta_sin), np.array([[0.],[0.],[0.]]), np.array([[0.],[0.],[0.]]))
xyz, zeta, uvw, pqr = self.iris.get_state()
self.vec_xyz = xyz-self.goal_xyz
self.vec_zeta_sin = np.sin(zeta)-self.goal_zeta_sin
self.vec_zeta_cos = np.cos(zeta)-self.goal_zeta_cos
self.dist_norm = np.linalg.norm(self.vec_xyz)
self.att_norm_sin = np.linalg.norm(self.vec_zeta_sin)
self.att_norm_cos = np.linalg.norm(self.vec_zeta_cos)
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, quaternion=True)
counter = 0
frames = 5
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_quat(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 __init__(self):
metadata = {'render.modes': ['human']}
self.r_max = 2.5
self.goal_thresh = 0.05
self.t = 0
self.T = 10
self.action_space = np.zeros((4, ))
self.observation_space = np.zeros((34, ))
self.goal_xyz = np.array([[random.uniform(-3, 3)],
[random.uniform(-3, 3)],
[random.uniform(-3, 0)]])
self.spawn = np.array([[random.uniform(-3,
3)], [random.uniform(-3, 3)],
[random.uniform(0, 3)]])
self.goal_zeta_sin = np.sin(np.array([[0.], [0.], [0.]]))
self.goal_zeta_cos = np.cos(np.array([[0.], [0.], [0.]]))
# the velocity of the aircraft in the inertial frame is probably a better metric here, but
# since our goal state is (0,0,0), this should be fine.
self.goal_uvw = np.array([[0.], [0.], [0.]])
self.goal_pqr = np.array([[0.], [0.], [0.]])
# simulation parameters
self.params = cfg.params
self.iris = quad.Quadrotor(self.params)
self.sim_dt = self.params["dt"]
self.ctrl_dt = 0.05
self.steps = range(int(self.ctrl_dt / self.sim_dt))
self.action_bound = [0, self.iris.max_rpm]
self.H = int(self.T / self.ctrl_dt)
self.hov_rpm = self.iris.hov_rpm
self.trim = [self.hov_rpm, self.hov_rpm, self.hov_rpm, self.hov_rpm]
self.trim_np = np.array(self.trim)
self.bandwidth = 30.
# define bounds here
self.xzy_bound = 0.5
self.zeta_bound = pi / 3
self.uvw_bound = 0.5
self.pqr_bound = 0.25
self.iris.set_state(self.spawn, np.arcsin(self.goal_zeta_sin),
self.goal_uvw, self.goal_pqr)
self.iris.set_rpm(np.array(self.trim))
xyz, zeta, uvw, pqr = self.iris.get_state()
self.vec_xyz = xyz - self.goal_xyz
self.vec_zeta_sin = np.sin(zeta) - self.goal_zeta_sin
self.vec_zeta_cos = np.cos(zeta) - self.goal_zeta_cos
self.vec_uvw = uvw - self.goal_uvw
self.vec_pqr = pqr - self.goal_pqr
self.dist_norm = np.linalg.norm(self.vec_xyz)
self.att_norm_sin = np.linalg.norm(self.vec_zeta_sin)
self.att_norm_cos = np.linalg.norm(self.vec_zeta_cos)
self.vel_norm = np.linalg.norm(self.vec_uvw)
self.ang_norm = np.linalg.norm(self.vec_pqr)
#self.landing_angle = (180/np.pi)*abs(np.arcsin(temp_O/dist_hat))[0])-90
self.fig = None
self.axis3d = None
self.update_path()
def __init__(self):
metadata = {'render.modes': ['human']}
self.r_max = 2.5
self.goal_thresh = 0.05
self.t = 0
self.T = 10
self.action_space = np.zeros((4, ))
self.observation_space = np.zeros((34, ))
self.goal_xyz = self.generate_goal(self.r_max)
self.goal_zeta_sin = np.sin(np.array([[0.], [0.], [0.]]))
self.goal_zeta_cos = np.cos(np.array([[0.], [0.], [0.]]))
self.spawn = np.array([[2], [2], [-2]])
# the velocity of the aircraft in the inertial frame is probably a better metric here, but
# since our goal state is (0,0,0), this should be fine.
self.goal_uvw = np.array([[0.], [0.], [0.]])
self.goal_pqr = np.array([[0.], [0.], [0.]])
# simulation parameters
self.params = cfg.params
self.iris = quad.Quadrotor(self.params)
self.sim_dt = self.params["dt"]
self.ctrl_dt = 0.05
self.steps = range(int(self.ctrl_dt / self.sim_dt))
self.action_bound = [0, self.iris.max_rpm]
self.H = int(self.T / self.ctrl_dt)
self.hov_rpm = self.iris.hov_rpm
self.trim = [self.hov_rpm, self.hov_rpm, self.hov_rpm, self.hov_rpm]
self.trim_np = np.array(self.trim)
self.bandwidth = 35.
xyz, zeta, uvw, pqr = self.iris.get_state()
self.spawn = xyz
self.vec_xyz = xyz - self.goal_xyz
self.vec_zeta_sin = np.sin(zeta) - self.goal_zeta_sin
self.vec_zeta_cos = np.cos(zeta) - self.goal_zeta_cos
self.vec_uvw = uvw - self.goal_uvw
self.vec_pqr = pqr - self.goal_pqr
self.dist_norm = np.linalg.norm(self.vec_xyz)
self.att_norm_sin = np.linalg.norm(self.vec_zeta_sin)
self.att_norm_cos = np.linalg.norm(self.vec_zeta_cos)
self.vel_norm = np.linalg.norm(self.vec_uvw)
self.ang_norm = np.linalg.norm(self.vec_pqr)
self.sensor_total = 3 ##This is pretty much the max before big preformance reduction
self.sensor_spacing = 0.25
self.circle_sensors = []
self.obstacles = []
self.total_obstacles = 5
self.largest_sensor = 0
##This needs to be added to quadrotor3.py
self.prop_radius = self.iris.get_prop_radius()
self.gen_sensors()
self.gen_obstacles()
self.obstacle_rew_val = self.get_obstacle_rew(xyz)
self.fig = None
self.axis3d = None
def __init__(self):
metadata = {'render.modes': ['human']}
# environment parameters
self.goal_xyz = np.array([[2.0],
[1.5],
[0.0]])
self.start_pos = np.array([[0.0],
[0.0],
[0.0]])
self.goal_zeta_sin = np.sin(np.array([[0.],
[0.],
[0.]]))
self.goal_zeta_cos = np.cos(np.array([[0.],
[0.],
[0.]]))
self.goal_uvw = np.array([[0.],
[0.],
[0.]])
self.goal_pqr = np.array([[0.],
[0.],
[0.]])
#Sets the xyz dimensions of the environment
self.x_dim = 4;
self.y_dim = 4;
self.z_dim = 4;
#The current running time (in seconds) of the simulation
self.t = 0
#The total maximum time of the simulation
self.T = 25
#The number of action input parameters
self.num_actions = 4
#The number of input options
self.action_space = np.zeros((self.num_actions,))
#The number of values that pertain to the current state of the agent
self.nStateVals = 22;
#The number of goal values
self.nGoals = 1;
#The total number of values that pertain to an observation (given state)
self.observation_space = np.zeros((self.nStateVals+self.num_actions+self.nGoals,))
# simulation parameters
self.params = cfg.params
self.iris = quad.Quadrotor(self.params)
self.sim_dt = self.params["dt"]
self.ctrl_dt = 0.05
self.steps = range(int(self.ctrl_dt/self.sim_dt))
self.action_bound = [0, self.iris.max_rpm]
self.H = int(self.T/self.ctrl_dt)
self.hov_rpm = self.iris.hov_rpm
self.trim = [self.hov_rpm, self.hov_rpm,self.hov_rpm, self.hov_rpm]
self.trim_np = np.array(self.trim)
self.bandwidth = 35.
self.iris.set_state(self.start_pos, np.arcsin(self.goal_zeta_sin), self.goal_uvw, self.goal_pqr)
xyz, zeta, uvw, pqr = self.iris.get_state()
self.vec_xyz = xyz-self.start_pos
self.vec_zeta_cos = np.cos(zeta)-self.goal_zeta_cos
self.fig = None
self.axis3d = None
self.v = None
self.init_rendering = False;
def __init__(self):
metadata = {'render.modes': ['human']}
self.r_max = 2.5
self.goal_thresh = 0.2
self.t = 0
self.T = 10
self.action_space = np.zeros((4, ))
self.observation_space = np.zeros((34, ))
#Dictionary to hold the wall data
self.wall_data = {
"wall_plane": None,
"PR": None,
"ELA": None,
"wall_pos": None
}
wall_goal = self.get_wall_goal()
self.wall = wall_goal[0]
self.goal_xyz = wall_goal[1]
zeta_goal = self.get_goal_zeta()
self.goal_zeta_sin = np.sin(zeta_goal)
self.goal_zeta_cos = np.cos(zeta_goal)
self.goal_uvw = np.array([[0.], [0.], [0.]])
self.goal_pqr = np.array([[0.], [0.], [0.]])
# simulation parameters
self.params = cfg.params
self.iris = quad.Quadrotor(self.params)
self.sim_dt = self.params["dt"]
self.ctrl_dt = 0.05
self.steps = range(int(self.ctrl_dt / self.sim_dt))
self.action_bound = [0, self.iris.max_rpm]
self.H = int(self.T / self.ctrl_dt)
self.hov_rpm = self.iris.hov_rpm
self.trim = [self.hov_rpm, self.hov_rpm, self.hov_rpm, self.hov_rpm]
self.trim_np = np.array(self.trim)
self.bandwidth = 35.
xyz, zeta, uvw, pqr = self.iris.get_state()
self.vec_xyz = xyz - self.goal_xyz
self.vec_zeta_sin = np.sin(zeta) - self.goal_zeta_sin
self.vec_zeta_cos = np.cos(zeta) - self.goal_zeta_cos
self.vec_uvw = uvw - self.goal_uvw
self.vec_pqr = pqr - self.goal_pqr
self.vel_r = pqr[2][0]
exp_wall_approach_angle = self.wall_data["ELA"]
if self.wall_data["PR"] == "R":
self.wall_approach_angle = abs(exp_wall_approach_angle -
zeta[0][0]) + abs(0 - zeta[1][0])
self.approach_line = ((self.goal_xyz[0][0] - xyz[0][0])**2 +
(self.goal_xyz[2][0] - xyz[2][0])**2)**0.5
else:
self.wall_approach_angle = abs(exp_wall_approach_angle -
zeta[1][0]) + abs(0 - zeta[0][0])
self.approach_line = ((self.goal_xyz[1][0] - xyz[1][0])**2 +
(self.goal_xyz[2][0] - xyz[2][0])**2)**0.5
self.dist_norm = np.linalg.norm(self.vec_xyz)
self.att_norm_sin = np.linalg.norm(self.vec_zeta_sin)
self.att_norm_cos = np.linalg.norm(self.vec_zeta_cos)
self.vel_norm = np.linalg.norm(self.vec_uvw)
self.ang_norm = np.linalg.norm(self.vec_pqr)
self.init_rendering = False
def __init__(self):
metadata = {'render.modes': ['human']}
self.r_max = 1.5
self.goal_thresh = 0.1
self.t = 0
self.T = 3
# build list of waypoints for the aircraft to fly to
self.traj_len = 2
self.goal_list = []
x = np.array([[0.], [0.], [0.]])
for i in range(self.traj_len):
x += self.generate_goal()
self.goal_list.append(x.copy())
print("Trajectory Length: ", len(self.goal_list))
print("Waypoints: ")
for g in self.goal_list:
print(g.reshape(1, -1)[0])
self.goal_curr = 0
self.goal_next_curr = self.goal_curr + 1
self.goal_xyz = self.goal_list[self.goal_curr]
self.goal_xyz_next = self.goal_list[self.goal_next_curr]
self.goal_zeta_sin = np.sin(np.array([[0.], [0.], [0.]]))
self.goal_zeta_cos = np.cos(np.array([[0.], [0.], [0.]]))
# the velocity of the aircraft in the inertial frame is probably a better metric here, but
# since our goal state is (0,0,0), this should be fine.
self.goal_uvw = np.array([[0.], [0.], [0.]])
self.goal_pqr = np.array([[0.], [0.], [0.]])
self.datum = np.array([[0.], [0.], [0.]])
# simulation parameters
self.params = cfg.params
self.iris = quad.Quadrotor(self.params)
self.sim_dt = self.params["dt"]
self.ctrl_dt = 0.05
self.steps = range(int(self.ctrl_dt / self.sim_dt))
self.action_bound = [0, self.iris.max_rpm]
self.H = int(self.T / self.ctrl_dt)
self.hov_rpm = self.iris.hov_rpm
self.trim = [self.hov_rpm, self.hov_rpm, self.hov_rpm, self.hov_rpm]
self.trim_np = np.array(self.trim)
self.prev_action = self.trim_np.copy()
self.action_space = spaces.Box(low=0,
high=self.iris.max_rpm,
shape=(4, ))
self.observation_space = np.zeros((37, ))
self.bandwidth = 35.
xyz, zeta, uvw, pqr = self.iris.get_state()
self.vec_xyz = xyz - self.goal_xyz
self.vec_zeta_sin = np.sin(zeta) - self.goal_zeta_sin
self.vec_zeta_cos = np.cos(zeta) - self.goal_zeta_cos
self.vec_uvw = uvw - self.goal_uvw
self.vec_pqr = pqr - self.goal_pqr
self.dist_norm = np.linalg.norm(self.vec_xyz)
self.att_norm_sin = np.linalg.norm(self.vec_zeta_sin)
self.att_norm_cos = np.linalg.norm(self.vec_zeta_cos)
self.ang_norm = np.linalg.norm(self.vec_uvw)
self.ang_norm = np.linalg.norm(self.vec_pqr)
self.init_rendering = False
self.lazy_action = False
self.lazy_change = False
