def __init__(self, x0, t0, dynamics_config_file): self.attrs = None try: with open(dynamics_config_file) as f: self.attrs = yaml.load(f) except: raise IOError(errno.ENOENT, 'File not found', dynamics_config_file) self.dynamics = FixedWingUAVDynamics(x0, t0, 0.001, self.attrs)
class FixedWingUAV(object): def __init__(self, x0, t0, dynamics_config_file): self.attrs = None try: with open(dynamics_config_file) as f: self.attrs = yaml.load(f) except: raise IOError(errno.ENOENT, 'File not found', dynamics_config_file) self.dynamics = FixedWingUAVDynamics(x0, t0, 0.001, self.attrs) def move_to(self, pos): self.x[0:3] = pos def R_v1v(self, yaw_rad): cy = np.cos(yaw_rad) sy = np.sin(yaw_rad) R = np.matrix(([cy, sy, 0], [-sy, cy, 0], [0, 0, 1]), dtype=np.double) return R def R_v2v1(self, pitch_rad): cp = np.cos(pitch_rad) sp = np.sin(pitch_rad) R = np.matrix(([cp, 0, -sp], [0, 1, 0], [sp, 0, cp]), dtype=np.double) return R def R_bv2(self, roll_rad): cr = np.cos(roll_rad) sr = np.sin(roll_rad) R = np.matrix(([1, 0, 0], [0, cr, sr], [0, -sr, cr]), dtype=np.double) return R def R_bv(self, rpy_rad): yaw = rpy_rad[2] pitch = rpy_rad[1] roll = rpy_rad[0] R = self.R_bv2(roll) * self.R_v2v1(pitch) * self.R_v1v(yaw) return R def update_state(self, dt): if self.dynamics.control_inputs is not None: t = dt + self.dynamics.integrator.t self.dynamics.integrate(t) else: raise Exception('set control inputs first') def set_state(self, x, t): self.dynamics.integrator.set_initial_value(x, t) def set_control_inputs(self, control_inputs): self.dynamics.control_inputs = control_inputs def get_control_inputs(self): return self.dynamics.control_inputs # def __call__(self, t): # self.dynamics.integrate(t) # self.x = self.dynamics.integrator.y
class FixedWingUAV(object): def __init__(self, x0, t0, dynamics_config_file): self.attrs = None try: with open(dynamics_config_file) as f: self.attrs = yaml.load(f) except: raise IOError(errno.ENOENT, 'File not found', dynamics_config_file) self.dynamics = FixedWingUAVDynamics(x0, t0, 0.001, self.attrs) def move_to(self, pos): self.x[0:3] = pos def R_v1v(self, yaw_rad): cy = np.cos(yaw_rad) sy = np.sin(yaw_rad) R = np.matrix(([cy, sy, 0], [-sy, cy, 0], [0, 0, 1]), dtype = np.double) return R def R_v2v1(self, pitch_rad): cp = np.cos(pitch_rad) sp = np.sin(pitch_rad) R = np.matrix(([cp, 0, -sp], [0, 1, 0], [sp, 0, cp]), dtype = np.double) return R def R_bv2(self, roll_rad): cr = np.cos(roll_rad) sr = np.sin(roll_rad) R = np.matrix(([1, 0, 0], [0, cr, sr], [0, -sr, cr]), dtype = np.double) return R def R_bv(self, rpy_rad): yaw = rpy_rad[2] pitch = rpy_rad[1] roll = rpy_rad[0] R = self.R_bv2(roll) * self.R_v2v1(pitch) * self.R_v1v(yaw) return R def update_state(self, dt): if self.dynamics.control_inputs is not None: t = dt + self.dynamics.integrator.t self.dynamics.integrate(t) else: raise Exception('set control inputs first') def set_state(self, x, t): self.dynamics.integrator.set_initial_value(x, t) def set_control_inputs(self, control_inputs): self.dynamics.control_inputs = control_inputs def get_control_inputs(self): return self.dynamics.control_inputs # def __call__(self, t): # self.dynamics.integrate(t) # self.x = self.dynamics.integrator.y