def dynamicsAtHover(qcParm):
'''
The state first has position coordinates, then angular coordinates,
followed by their velocities in the same order. This is preferred
format to align with decomposition of tangent space into base and
vector spaces. The dynamics live in the second order jet.
'''
A = np.diag(np.ones((6)), 6) #Top right block is identity matrix.
B = np.zeros((12, 4))
uEq = ((qcParm.m * qcParm.g) / (4 * qcParm.KT)) * np.ones((4, 1))
A[6, 4] = qcParm.g #Control/state coupling.
A[7, 3] = -qcParm.g
if 'Dt' in qcParm:
A[0:3, 6:9] = -qcParm.Dt * np.eye(3) / qcParm.m
if 'Dr' in qcParm:
A[3:6, 9:12] = -qcParm.Dr * np.eye(3) / qcParm.m
ThZ = 2 * qcParm.KT / qcParm.m
ThR = 2 * qcParm.r * qcParm.KT / qcParm.Jx
ThP = 2 * qcParm.r * qcParm.KT / qcParm.Jy
ThYaw = 2 * qcParm.KD / qcParm.Jz
B[8, :] = ThZ
B[9, 2] = ThR
B[9, 3] = -ThR
B[10, 0] = -ThP
B[10, 1] = ThP
B[11, [0, 1]] = -ThYaw
B[11, [2, 3]] = ThYaw
dyn = linear.systemDynamics(A, B)
return (dyn, A, B, uEq)
A = np.zeros((4, 4))
A[0, 1] = 1
A[2, 3] = 1
B = np.zeros((4, 2))
B[1, 0] = 1
B[3, 1] = 1
Q = np.eye(4)
linctrl = linear(np.zeros((4, 1)), np.zeros((2, 1)))
linctrl.setByCARE(A, B, Q)
linctrl.noControl()
leom = linear.systemDynamics(A, B)
pathgen = linepath.linepath()
ts = structure()
ts.Th = 0.5
ts.Td = 0.2
ts.vec2state = lambda x: x
cfS = structure(dt=0.05,
odeMethod=niODERK4,
controller=timepoints(pathgen, linctrl, ts))
tspan = [0, 20]
fCirc = lambda t: np.array([[np.sin(t / 2)], [(1 / 2) * np.cos(t / 2)],
[1 - np.cos(t / 2)], [(1 / 2) * np.sin(t / 2)]])