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