def trajBuilder_LinearReg(sys):
#Doesn't do anything?
xdes = []
def linSysBuilder(istate, fstate):
if 'cons' in sys:
(ctrl,
xdes) = theControl.regulatorConstrained(fstate, sys.cons)
else:
(ctrl, xdes) = theControl.regulator(fstate)
if theControl.K.shape[1] == 2 * istate.size:
istate = np.pad(istate.flatten(), (0, istate.size),
mode='constant').reshape(
(theControl.K.shape[1], 1))
csim = simController(solver, ctrl)
csim.initialize(tspan=sys.tspan, x0=istate)
return csim
# Doesn't do anything?
def detectArrival(t, x):
errNorm = np.linalg.norm(x - xdes) - sys.rtol
if errNorm < 0:
errNorm = 0
isterminal = True
direction = 0
return (errNorm, isterminal, direction)
# Doesn't do anything?
opts = structure(event=detectArrival)
ceom = linear().systemDynamics(sys.A, sys.B)
solver = sys.solver(ceom, sys.dt, opts)
theControl = linear()
if 'K' in sys:
theControl.set(sys.K)
elif 'Q' in sys:
theControl.setByCareFromStruct(sys)
theBuilder = linSysBuilder
return theBuilder
def trajBuilder_LinearTracker(sys):
xdes = []
def linSysBuilder(istate, desTraj):
ctrl = theControl.tracker(desTraj.x, desTraj.u, desTraj.statedep)
if theControl.K.shape[1] == 2 * istate.x.size:
istate.x = np.pad(istate.x.flatten(), (0, istate.x.size),
mode='constant').reshape(
(theControl.K.shape[1], 1))
csim = simController(solver, ctrl)
csim.initialize(tspan=desTraj.tspan, x0=istate.x)
return csim
def detectArrival(t, x):
errNorm = np.linalg.norm(x - xdes) - sys.rtol
if errNorm < 0:
errNorm = 0
isterminal = True
direction = 0
return (errNorm, isterminal, direction)
opts = structure(event=detectArrival)
ceom = linear().systemDynamics(sys.A, sys.B)
#ceom = simController.linearControlSys(A, B)
solver = sys.solver(ceom, sys.dt, opts)
theControl = linear()
if 'K' in sys:
theControl.set(sys.K)
elif 'Q' in sys:
theControl.setByCareFromStruct(sys)
theBuilder = linSysBuilder
return theBuilder
from niODERK4 import niODERK4
import trajectory.Path
import pdb
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))
import linepath from matplotlib import pyplot as plt from structures import structure from niODERK4 import niODERK4 import trajectory.Path from controller.SynthTracker.FixedHorizons import fixedHorizons from trajSynth.mpcTrivial import mpcTrivial #set up the system for simulation A = np.zeros((2, 2)) B = np.eye(2) Q = np.eye(2) linctrl = linear(np.array([[0], [0]]), np.array([[0], [0]])) linctrl.setByCARE(A, B, Q) linctrl.noControl() leom = linear.systemDynamics(A, B) #set up the MPC #define MPC paramaters parms = structure() parms.Ts = .01 parms.x0 = np.array([[0], [0]]) parms.Td = 1 tSynth = mpcTrivial(parms) ts = structure()
tsys.dt = 0.05
tsys.solver = niODERK4
tsys.tspan = [0, 40]
metaBuilder = structure()
metaBuilder.regulator = naive.trajBuilder_LinearReg
metaBuilder.tracker = naive.trajBuilder_LinearTracker
tMaker = naive(tsys, metaBuilder)
#--[1.3] Define the trajectory tracking builder.
#
csys = structure()
csys.dt = 0.05
csys.odeMethod = niODERK4
csys.controller = linear(np.zeros((12, 1)), 0 * uEq)
csys.Q = (1e8) * np.diag([8, 8, 14, 2, 2, 1, 15, 15, 20, 3, 3, 2
]) # Can be weaker than the tsys.Q
#csys.Q = np.diag([200, 300, 400, 100, 10, 10, 20, 20, 10, 10, 10, 10])
csys.controller.setByCARE(A, B, csys.Q)
tTracker = csys.controller.structBuilder(linDyn, csys)
#--[1.4] Pack together into control system.
#
cSim = linQuadCopter(linDyn, tMaker, tTracker)
#==[2] Simulate the control system.
#
simType = 'curve3'
[P, L, K] = care(sys.A, sys.B, Q)
sys.K = -K
L = L.reshape((2, 1))
sys.cons.sat.min = -5
sys.cons.sat.max = 5
#--[1.2] Instantiate the trajectory synthesizer.
metaBuilder = structure(regulator=naive.trajBuilder_LinearReg,
tracker=naive.trajBuilder_LinearTracker)
tMaker = naive(theSystem=sys, metaBuilder=metaBuilder)
#==[2] Configure and instantiate the trajectory tracking builder.
actA = np.array([[0, 1], [0.35, 0.05]])
actB = np.array([[0], [1]])
ceom = linear().systemDynamics(actA, actB)
theController = linear()
theController.set(sys.K)
cfs = structure(dt=0.05, odeMethod=niODERK4, controller=theController)
tTracker = theController.structBuilder(ceom, cfs)
#%==[3] Pack together into a controlSystem object and simulate.
cSim = controlSystem(ceom, tMaker, tTracker)
istate = structure(x=np.array([3, 0]).reshape((2, 1)))
cSim.setInitialState(istate)
sol = cSim.goto(np.array([0.3, 0]).reshape((2, 1)))
#==[3.1] Plot outcomes
istate = np.array([[0], [0]])
#cons.sat.min = -5
#cons.sat.max = 5
#==[2] Specify the trajectory to track.
ra = 1
rw = 1.5
rsig = lambda t: np.array([[ra * np.sin(rw * t)], [(ra * rw) * np.cos(rw * t)]]
)
xacc = lambda t: -(ra * rw * rw) * np.sin(rw * t)
sFF = 1
uFF = lambda t: sFF * bg * (xacc(t))
stateDep = False
#==[3] Trajectory Tracker
theController = linear()
theController.set(K)
#ceom = simController.linearControlSys(A, B)
ceom = theController.systemDynamics(A, B)
solver = solver(ceom, dt)
#theController = controller.linear()
ctrl = theController.tracker(rsig, uFF, stateDep)
csim = simController(solver, ctrl)
#csim.initialize(tspan, istate)
sol = csim.simulate(tspan, istate)
#%==[4] Plot outcomes.
plt.figure(1)
plt.plot(sol.t, sol.x[0, :], 'b', sol.t, sol.x[1, :], 'g-')
# hold on;