def __init__(self, nc, na):
super().__init__()
N = nc + na
self.Y = BaseSystem(shape=(N, 1))
self.Xi = BaseSystem(shape=(N, N))
self.D = BaseSystem(shape=(N, 1))
self.Dast = BaseSystem(shape=(N, 1))
def __init__(self, phic0, na):
super().__init__()
self.y = BaseSystem()
self.xic = BaseSystem(phic0)
self.xia = BaseSystem(shape=(na, 1))
self.d = BaseSystem()
self.dast = BaseSystem()
def __init__(self, pos_bound, att_bound, mass, J):
super().__init__()
self.pos = BaseSystem(np.vstack((
np.random.uniform(
low=pos_bound[0][0],
high=pos_bound[0][1]
),
np.random.uniform(
low=pos_bound[1][0],
high=pos_bound[1][1]
),
np.random.uniform(
low=pos_bound[2][0],
high=pos_bound[2][1]
)
)))
self.vel = BaseSystem(np.vstack((0., 0., 0.)))
self.dcm = BaseSystem(rot.angle2dcm(
np.random.uniform(
low=att_bound[2][0],
high=att_bound[2][1]
),
np.random.uniform(
low=att_bound[1][0],
high=att_bound[1][1]
),
np.random.uniform(
low=att_bound[0][0],
high=att_bound[0][1]
)
).T)
self.omega = BaseSystem(np.vstack((0., 0., 0.)))
self.mass = mass
self.J = J
def __init__(self, pos=np.zeros((3, 1)), vel=np.zeros((3, 1)),
dcm=np.eye(3), omega=np.zeros((3, 1))):
super().__init__()
self.pos = BaseSystem(pos)
self.vel = BaseSystem(vel)
self.dcm = BaseSystem(dcm)
self.omega = BaseSystem(omega)
def __init__(
self,
pos=np.zeros((3, 1)),
vel=np.zeros((3, 1)),
angle=np.zeros((3, 1)),
omega=np.zeros((3, 1)),
config="quadrotor",
):
super().__init__()
self.pos = BaseSystem(pos)
self.vel = BaseSystem(vel)
self.angle = BaseSystem(angle)
self.omega = BaseSystem(omega)
# u_factor * U = M
self.u_factor = np.diag([self.l, self.l, self.d / self.b])
# The variables defined below relate to
# specific configuration of the multirotor
if config == "quadrotor":
# Allocation matrix
self.B = np.array([[1, 1, 1, 1], [0, -1, 0, 1], [-1, 0, 1, 0],
[1, -1, 1, -1]])
self.b_gyro = np.vstack((-1, 1, -1, 1))
# Distance vector to each rotor (P. Pounds et al., 2010)
self.d_rotor = np.array([
[self.l, 0, 0],
[0, -self.l, 0],
[-self.l, 0, 0],
[0, self.l, 0],
])
# Number of rotors
self.n = 4
elif config == "hexarotor":
s2 = 1 / 2
s3 = np.sqrt(3) / 2
self.B = np.array([[1, 1, 1, 1, 1, 1], [0, -s3, -s3, 0, s3, s3],
[-1, -s2, s2, 1, s2, -s2],
[1, -1, 1, -1, 1, -1]])
self.b_gyro = np.vstack((1, -1, 1, -1, 1, -1))
l = self.l
self.d_rotor = np.array([
[l, 0, 0],
[l * s2, -l * s3, 0],
[-l * s2, -l * s3, 0],
[-l, 0, 0],
[-l * s2, l * s3, 0],
[l * s2, l * s3, 0],
])
self.n = 6
else:
raise ValueError
self.fmax = self.m * self.g * 2 / self.n
def __init__(self, basis, B):
super().__init__()
n, m = cfg.Winit.shape
self.xi = BaseSystem(shape=(n, 1))
self.z = BaseSystem(shape=(m, 1))
self.Bdagger = np.linalg.pinv(B)
self.tauf = cfg.ValueLearner.tauf
def __init__(self,
pos=np.zeros((3, 1)),
vel=np.zeros((3, 1)),
R=np.eye(3),
omega=np.zeros((3, 1)),
config="Quadrotor"):
super().__init__()
self.pos = BaseSystem(pos)
self.vel = BaseSystem(vel)
self.R = BaseSystem(R)
self.omega = BaseSystem(omega)
def __init__(self, velocity, omega, quaternion, position):
self.vel = BaseSystem(velocity, name="velocity")
self.omega = BaseSystem(omega, name="omega")
self.quat = BaseSystem(quaternion, name="quaternion")
self.pos = BaseSystem(position, name="position")
super().__init__(
systems_dict={
"velocity": self.vel,
"omega": self.omega,
"quaternion": self.quat,
"position": self.pos,
})
def __init__(self):
super().__init__(
{
"main":
BaseEnv({
"main_1": BaseSystem(np.zeros((3, 3))),
"main_2": BaseSystem(np.ones(4)),
}),
"sub":
BaseSystem([2, 2, 2]),
},
dt=0.01,
max_t=10)
def __init__(self, length, anchor, uvec_bound):
super().__init__()
self.uvec = BaseSystem(rot.sph2cart2(
1,
np.random.uniform(
low=uvec_bound[0][0],
high=uvec_bound[0][1]
),
np.random.uniform(
low=uvec_bound[1][0],
high=uvec_bound[1][1]
)
))
self.omega = BaseSystem(np.vstack((0., 0., 0.)))
self.len = length
self.anchor = anchor
def __init__(self,
pos=np.zeros((3, 1)),
vel=np.zeros((3, 1)),
dcm=np.eye(3),
omega=np.zeros((3, 1)),
J=np.diag([0.0820, 0.0845, 0.1377]),
m=4.34,
config="Quadrotor",
):
super().__init__()
self.pos = BaseSystem(pos)
self.vel = BaseSystem(vel)
self.dcm = BaseSystem(dcm)
self.omega = BaseSystem(omega)
self.J = J
self.m = m
def __init__(self):
super().__init__(**vars(cfg.env_kwargs))
self.x = MorphingLon()
self.PI = BaseSystem()
trims = self.x.get_trim()
self.trim = {k: v for k, v in zip(["x", "u", "eta"], trims)}
self.A = jacob_analytic(self.x.deriv, 0)(*trims)
self.B = jacob_analytic(self.x.deriv, 1)(*trims)
self.Kopt, self.Popt = LQR.clqr(self.A, self.B, cfg.Q, cfg.R)
self.behave_K, _ = LQR.clqr(self.A, self.B, cfg.Qb, cfg.Rb)
self.add_noise = True
def __init__(self):
BaseEnv.__init__(self, **cfg.DoubleMRAC.env_kwargs)
self.x = System()
self.xr = ReferenceSystem()
self.P = cfg.P
self.W = CMRAC(self.P, cfg.B)
self.F = BaseSystem(np.zeros_like(cfg.Wcirc.dot(cfg.Wcirc.T)))
self.J = PerformanceIndex()
self.cmd = Command()
self.basis = self.x.unc.basis
self.BTBinv = np.linalg.inv(cfg.B.T.dot(cfg.B))
self.logger = fym.logging.Logger(Path(cfg.dir, "double-mrac-env.h5"))
self.logger.set_info(cfg=cfg)
def __init__(self):
super().__init__(dt=0.05, max_t=50)
# for level flight
self.x = BaseSystem(np.vstack([VT0 + 5, gamma0, h0 + 30, alpha0, Q0]))
## for VT tracking
# t = self.clock.get()
# if t < 10:
# self.x = BaseSystem(np.vstack
self.A = A_trim
self.B = B_trim
Q = np.diag([0.003, 0.4, 0.002, 0.4, 1.7])
R = 1.7 * np.identity(2)
self.K, *_ = LQR.clqr(self.A, self.B, Q, R)
def __init__(self):
BaseEnv.__init__(self, **cfg.ValueLearner.env_kwargs)
self.x = System()
self.xr = ReferenceSystem()
self.P = cfg.P
self.W = CMRAC(self.P, cfg.B)
self.F = BaseSystem(np.zeros_like(cfg.Wcirc.dot(cfg.Wcirc.T)))
self.J = PerformanceIndex()
self.basis = self.x.unc.basis
self.filter = ResidualFilter(basis=self.basis, B=cfg.B)
self.cmd = Command()
self.BTBinv = np.linalg.inv(cfg.B.T.dot(cfg.B))
self.logger = fym.logging.Logger(Path(cfg.dir, "value-learner-env.h5"))
self.logger.set_info(cfg=cfg)
def __init__(self):
BaseEnv.__init__(self, **cfg.HMRAC.env_kwargs)
self.multirotor = MultiRotor(config=cfg.multirotor_config)
self.xr = ReferenceSystem()
self.B = self.multirotor.Jinv.dot(
self.multirotor.u_factor.dot(self.multirotor.B[1:]))
self.P = scipy.linalg.solve_continuous_are(cfg.Am, self.B, cfg.Q_lyap,
cfg.R)
self.W = CMRAC(self.P, self.B, cfg.HMRAC.Gamma)
self.What = BaseSystem(cfg.W_init)
self.J = PerformanceIndex()
self.filter = ResidualFilter(basis=self.basis, B=self.B)
# Not BaseEnv or BaseSystem
self.cmd = Command()
self.wind = Wind()
self.ut = self.multirotor.m * self.multirotor.g / self.multirotor.n
self.BTBinv = np.linalg.inv(self.B.T.dot(self.B))
self.logger = fym.logging.Logger(Path(cfg.dir, "hmrac-env.h5"))
self.logger.set_info(cfg=cfg)
def __init__(self, basis):
super().__init__()
self.xi = BaseSystem(shape=(5, 1))
self.z = BaseSystem()
self.Bdagger = np.linalg.pinv(cfg.B)
def __init__(self, nc, na):
super().__init__()
self.wc = BaseSystem(shape=(nc, 1))
self.wa = BaseSystem(shape=(na, 1))
def __init__(self, mass, J):
super().__init__()
self.dcm = BaseSystem(np.eye(3))
self.omega = BaseSystem(np.vstack((0., 0., 0.)))
self.mass = mass
self.J = J
def __init__(self):
super().__init__()
self.pos = BaseSystem(np.vstack((0., 0.)))
self.vel = BaseSystem(np.vstack((0., 0.)))
def __init__(self, velocity, omega, quaternion, position):
self.vel = BaseSystem(velocity, name="velocity") # 3x1
self.omega = BaseSystem(omega, name="omega") # 3x1
self.quat = BaseSystem(quaternion, name="quaternion") # 4x1
self.pos = BaseSystem(position, name="position") # 3x1