def plotBoundary(self, dg): hbLines = self.getHalfBoundaries(dg) c1 = structure(X=None, Y=None, Z=None) (c1.X, c1.Y, c1.Z) = DiffDriveFO.cylinder(hbLines.outer[1, :]) c1.Z = np.tile(hbLines.outer[0, :, np.newaxis], (1, np.shape(c1.X)[1])) c2 = structure(X=None, Y=None, Z=None) (c2.X, c2.Y, c2.Z) = DiffDriveFO.cylinder(hbLines.inner[1, :]) c2.Z = np.tile(hbLines.inner[0, :, np.newaxis], (1, np.shape(c2.X)[1])) c3 = structure(X=None, Y=None, Z=None) (c3.X, c3.Y, c3.Z) = DiffDriveFO.cylinder(hbLines.side[1, :]) c3.Z = np.tile(hbLines.side[0, :, np.newaxis], (1, np.shape(c3.X)[1])) c = structure() c.X = np.vstack((c1.Z, c3.Z, np.flipud(c2.Z))) c.Y = np.vstack((c1.Y, c3.Y, np.flipud(c2.Y))) c.Z = np.vstack((c1.X, c3.X, np.flipud(c2.X))) fig = plt.figure() ax = fig.add_subplot(projection='3d') ax.plot_surface(c1.X, c1.Y, c1.Z) ax.plot_surface(c2.X, c2.Y, c2.Z) fig = plt.figure() ax = fig.add_subplot(projection='3d') ax.plot_surface(c.X, c.Y, c.Z, alpha=0.05) plt.show()
def simulate(self, tspan=None, x0=None, varargin=None): if x0 is not None and tspan is not None: self.initialize(tspan=tspan, x0=x0) else: if self.solver.state != self.solver.INITIALIZED: print("ERROR!") else: if tspan is not None: self.initialize(tspan=tspan, x0=self.solver.xc) while self.solver.state != self.solver.DONE: self.advance(varargin) if isinstance(self.cLaw, base): (self.uc, rc) = self.cLaw.compute(self.solver.tc, self.solver.xc) else: (self.uc, rc) = self.cLaw(self.solver.tc, self.solver.xc) self.u[:, self.solver.ci:self.solver.ci+1] = self.uc t = self.solver.t x = self.solver.x u = self.u sol = structure(t=t,x=x,u=u) return sol
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
def load_qmfiles(regex): from structures import structure #sorry! files = glob.glob(regex) structures = [] for i in files: io = horton.IOData.from_file(i) try: grepfield = sh.grep("E-field", i, "-A1") field = np.array([float(x) for x in grepfield.stdout.split()[6:9]]) except: print( "INFO: no field information found in {}. Assuming zero field.". format(i)) field = np.array([0., 0., 0.]) s = structure() s.load_qm(i, field) structures.append(s) return structures
def structBuilder(ceom, cfs): solver = cfs.odeMethod(ceom, cfs.dt) def theInitializer(tspan, istate, rsig, uFF, statedep=False): control = cfs.controller.tracker(rsig, uFF, statedep) theSim = simController(solver, control) theSim.initializeByStruct(tspan, istate) return theSim def theInitializerFromStruct(istate, theTraj): if (hasattr(theTraj, 'statedep')): if theTraj.statedep is None: theTraj.statedep = False else: theTraj.statedep = False theSim = theInitializer(theTraj.tspan, istate, theTraj.x, theTraj.u, theTraj.statedep) return theSim def reconfigure(theSim, tspan, istate, rsig, uFF, statedep): control = cfs.controller.tracker(rsig, uFF, statedep) theSim.setController(control) theSim.reset() theSim.initializeByStruct(tspan, istate) def reconfigureFromStruct(theSim, istate, theTraj): if theTraj.statedep is None: theTraj.statedep = False reconfigure(theSim, theTraj.tspan, istate, theTraj.x, theTraj.u, theTraj.statedep) simInit = structure() simInit.firstBuild = theInitializer simInit.reconfig = reconfigure simInit.firstBuildFromStruct = theInitializerFromStruct simInit.reconfigFromStruct = reconfigureFromStruct return simInit
def simBuilder(ceom, cfS): def initialize(istate, desTraj): cfS.controller.trackerNew(desTraj) theSim = simController(solver, cfS.controller) theSim.initializeByStruct(desTraj.tspan, istate) return theSim def reconfigure(theSim, istate, desTraj): cfS.controller.trackerNew(desTraj) theSim.controller = cfS.controller theSim.reset() theSim.initializeByStruct(desTraj.tspan, istate) return theSim solver = cfS.odeMethod(ceom, cfS.dt) simInit = structure() simInit.firstBuild = initialize simInit.reconfig = reconfigure return simInit
def point2point(self, istate, fstate, tspan=None): ceom = self.regulator(istate, fstate) if tspan is not None: simout = ceom.simulate(tspan=tspan) else: simout = ceom.simulate() if simout.x.shape[0] == 1 and False: xTraj = scipy.interpolate.RegularGridInterpolator( points=(simout.t, ), values=simout.x) else: xgrid_interps = [ scipy.interpolate.RegularGridInterpolator(points=(simout.t, ), values=simout.x[ind], bounds_error=False) for ind in range(simout.x.shape[0]) ] xTraj = lambda t: np.vstack( [g(np.array(t).reshape(-1, )) for g in xgrid_interps]) if simout.u.shape[0] == 1 and False: uTraj = scipy.interpolate.RegularGridInterpolator( points=(simout.t, ), values=simout.u) else: ugrid_interps = [ scipy.interpolate.RegularGridInterpolator(points=(simout.t, ), values=simout.u[ind], bounds_error=False) for ind in range(simout.u.shape[0]) ] uTraj = lambda t: np.vstack( [g(np.array(t).reshape(-1, )) for g in ugrid_interps]) self.xTraj = xTraj self.uTraj = uTraj tSol = structure(tspan=simout.t[[0, -1]], x=self.xTraj, u=self.uTraj) return tSol
def simBuilder(theGroup, ceom, cfS): #------------------------- theInitializer ------------------------ #! #! def theInitializer(istate, desTraj): #! Controller needs building. cfS.controller.tracker(desTraj) #! simController configuration should be instantiated. theSim = simController(solver, cfS.controller) theSim.initializeByStruct(desTraj.tspan, istate) #-------------------------- reconfigure -------------------------- #! #! def reconfigure(theSim, istate, desTraj): #! The controller needs reconfiguration. cfS.controller.tracker(desTraj) theSim.setController(cfS.controller) #! The simulated system needs to be reset and given new initial state. theSim.reset() theSim.initializeByStruct(desTraj.tspan, istate) #!--[1] Base equations. if ('simOpts' in cfS and cfS.simOpts is not None): solver = SimFirstOrder( theGroup, ceom, cfS.dt, \ cfS.simOpts) else: solver = SimFirstOrder(theGroup, ceom, cfS.dt) # @todo What about optional args. #!--[4] Provide access to the function handles. simInit = structure() simInit.firstBuild = theInitializer simInit.reconfig = reconfigure return simInit
def getHalfBoundaries(self, dg): if (np.isscalar(self.specs.dt)): thetaMax = self.specs.wLim * self.specs.dt / 2 #== Inner arc. thetaGrd = np.linspace(0, thetaMax[0], round(thetaMax[0] / dg.theta)) inRad = self.inRadius(thetaGrd) pIn = inRad * np.vstack((np.cos(thetaGrd), np.sin(thetaGrd))) else: thetaMax = self.specs.wLim * self.specs.dt[1] / 2 thetaGrd = self.rmin.a inRad = self.rmin.r pIn = inRad * np.vstack((np.cos(thetaGrd), np.sin(thetaGrd))) thetaGrd = np.linspace(0, thetaMax[1], round(thetaMax[1] / dg.theta)) outRad = self.outRadius(thetaGrd) pOut = outRad * np.vstack((np.cos(thetaGrd), np.sin(thetaGrd))) #== Edges r1edge = inRad[-1] r2edge = outRad[-1] radGrd = np.linspace(r1edge, r2edge, round((r2edge - r1edge) / dg.r)) thetaEdge = self.angLimit(radGrd) eLeft = radGrd * np.vstack((np.cos(thetaEdge), np.sin(thetaEdge))) #== Combine in a structure halfBounds = structure() halfBounds.outer = pOut halfBounds.inner = pIn halfBounds.side = eLeft return halfBounds
def nextHorizon(self, tc, xc): #print(tc) #input('printing tc in nextHorizon') tTerm = tc + self.specs.Th #print(self.specs.Th) tNext = tc + self.specs.Td #print(self.specs.Td) #print(self.desTraj.tspan) #nput() if tTerm > self.desTraj.tspan[-1]: tTerm = self.desTraj.tspan[-1] if tNext > self.desTraj.tspan[-1]: tNext = self.desTraj.tspan[-1] #print([tc,tTerm]) #input('self desTraj tspan in next horion') #print(tTerm) #print(tc) #input() pathSeg = self.desTraj.segment([tc, tTerm]) #print([tc,tTerm]) #input() myarr = np.arange(tc, tTerm, self.specs.Ts).tolist() myt = 0 + tc #print(finalx) #plt.plot(finalx[:],finaly[:], 'b') istate = structure() istate = self.specs.state2state(xc) #print(xc) #print(istate) #input() plt.scatter(istate[0, 0], istate[1, 0]) self.synTraj = self.synthesizer.followPath(istate, pathSeg, tc)
from examples.hoverplane.hoverplane import hoverplane from controller.TSE2.linear import linear as linearSO from controller.linear import linear from structures import structure import numpy as np from niODERK4 import niODERK4 from trajSynth.naive import naive import pdb parms = structure() parms.linD = 0 * np.array([0.01, 0.023]) parms.angD = 0 * 0.015 parms.Md = 0 * 0.50 # Mr != 0 :: weather-vane effect. parms.Ma = 0 * 0.02 # Ma != 0 :: weather-vane effect. parms.lagF = 1 # lagF = 1 :: perfect cancellation. nuEq = 10 (nlDyn, A, B, uEq) = hoverplane.dynamicsForward(parms, nuEq) tsys = structure() tsys.A = A tsys.B = B tsys.Q = 4 * np.diag([6, 1, 3, 10, 3, 17]) tsys.dt = 0.01 tsys.solver = niODERK4 tsys.tspan = [0, 10] metaBuilder = structure(regulator=naive.trajBuilder_LinearReg, tracker=naive.trajBuilder_LinearTracker)
#linctrl.setByCARE(A, B, Q) linctrl.set(K) #set system Dynamics leom = nlDyn tspan = [0, 10] #fCirc = lambda t: np.array([ [np.sin(t/2)],[1-np.cos(t/2)],[np.arctan2(np.cos(t/2)-np.cos((t+.01)/2),np.sin((t+.01)/2)-np.sin(t/2))]]) fCirc = lambda t: np.array( [[nuEq * np.sin(t / 2)], [nuEq * (1 - np.cos(t / 2))], [t / 2], [nuEq * 1 / 2 * np.cos(t / 2)], [nuEq * 1 / 2 * np.sin(t / 2)], [1 / 2]]) path = Explicit(fCirc, tspan=tspan) desTraj = trajectory.Path(path, tspan) #define MPC paramaters parms = structure() parms.Ts = .01 parms.x0 = np.array([[0], [0], [0], [0], [0], [0]]) parms.Td = 10 tSynth = mpcDiffSO(parms) ts = structure() ts.Th = 10 ts.Td = 10 ts.Ts = .01 tSynth.updatefPtr(desTraj.x) #linctrl.trackerPath(desTraj) cfS = structure(dt=0.01, odeMethod=niODERK4, controller=fixedHorizons(tSynth, linctrl, ts))
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)]]) path = Explicit(fCirc, tspan=tspan) #pdb.set_trace() desTraj = trajectory.Path(path, tspan)
import numpy as np from reachable.SE2.fsDiffDriveFO import DiffDriveFO from structures import structure from matplotlib import pyplot as plt import pdb #==[1] Reachable set instance specs = structure() specs.dt = 3 specs.vLim = np.array([5, 7]) specs.wLim = np.array([np.pi / 5, np.pi / 10]) specs.Gmax = 3.5 specs.angType = 'GForce' rs = DiffDriveFO(specs) rs.compute() dg = structure() dg.r = 0.2 dg.theta = 0.05 rs.plotBoundary(dg) plt.plot(0, 0, 'r+') plt.axis('equal') theBdy = rs.getBoundary(dg) pMin = np.amin(theBdy, axis=1) pMax = np.amax(theBdy, axis=1) ng = 40 [xx, yy] = np.meshgrid(np.linspace(pMin[0], pMax[0], ng),
task = os.environ['SLURM_ARRAY_TASK_ID'] task = int(task) - 1 version = 3 data_path = 'data{}'.format('' if version == 1 else str(version)) parameters = getInfo(task) print(parameters) G, geo, contig, min_cutoff, ratio_cutoff, group_cutoff, basis, homogOption, method, order, flag = parameters mmap = [0, 0, 0, 0, 0, 1, 1, 1, 1, 1] dgmstructure = computeBounds(G, geo, contig, min_cutoff, ratio_cutoff, group_cutoff) mapping = structure(G, dgmstructure) xs_name = 'XS/{}gXS.anlxs'.format(G) if order >= dgmstructure.maxOrder: print('{} >= {}, stopping'.format(order, dgmstructure.maxOrder)) exit() if flag: run_reference(G, mmap, xs_name, mapping) run_homogenized(G, mmap, xs_name, structure(G, dgmstructure, order), order) run_homogenized(G, mmap, xs_name, structure(G, dgmstructure, order), order, False) run_dgm_homogenized(G, mmap, xs_name, mapping, order, method, basis, dgmstructure, homogOption) print('complete')
import numpy as np import math import sys from niODERK4 import niODERK4 from controller.linear import linear, care from simController import simController import matplotlib.pyplot as plt from trajSynth.naive import naive from controlSystem import controlSystem from structures import structure sys = structure() sys.A = np.array([[0, 1], [0.15, 0.25]]) sys.B = np.array([[0], [1]]) sys.solver = niODERK4 sys.dt = 0.01 sys.tspan = [0, 10] sys.rtol = 4e-3 Q = 50 * np.diag([6.00, 3.50]) [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)
import numpy as np from reachable.SE2.fsDiffDriveFO import DiffDriveFO from structures import structure from matplotlib import pyplot as plt import pdb #==[1] Reachable set instance specs = structure() specs.dt = 3 specs.vLim = np.array([5, 7]) specs.wLim = np.array([np.pi / 5, np.pi / 10]) specs.angType = 'interp' rs = DiffDriveFO(specs) rs.compute() dg = structure dg.r = 0.2 dg.theta = 0.05 rs.plotBoundary(dg) plt.plot(0, 0, 'r+') plt.axis('equal') theBdy = rs.getBoundary(dg) pMin = np.amin(theBdy, axis=1) pMax = np.amax(theBdy, axis=1) ng = 40 [xx, yy] = np.meshgrid(np.linspace(pMin[0], pMax[0], ng), np.linspace(pMin[1], pMax[1], ng))
# Path Gen Setup bpOrd = 3 pathgen = Flight3D(bezierOrder=bpOrd) pathgen.setDynConstraints(0, 20, 4) pathgen.optParams.Wlen = 0 pathgen.optParams.Wcurv = 0 pathgen.optParams.Wkdev = 10 pathgen.optParams.Wspdev = 1 pathgen.optParams.Wagree = 0 pathgen.optParams.doTimeWarp = False pathgen.spec.vec2state = mapToSE3 ts = structure() ts.Th = 0.5 ts.Td = 0.2 def vec2Tangent(x): g = mapToSE3(x) v = np.vstack((x[3:6], np.zeros((3,1)))) return Element(g, v) ts.vec2state = vec2Tangent # Path Specification tSpan = [0, 10] curveType = 'carousel'
from examples.quadcopter.linQuadCopter import linQuadCopter from niODERK4 import niODERK4 from trajSynth.naive import naive from controller.linear import linear from structures import structure import numpy as np from numpy.matlib import repmat #==[1] Setup the control system. # #--[1.1] Define the dynamics of the system. # Parms = structure() Parms.m = 1.52 # kg Parms.r = 0.09 # m Parms.Jx = 0.0347563 # kg-m^2 Parms.Jy = 0.0458529 # kg-m^2 Parms.Jz = 0.0977 # kg.m^2 Parms.KT = 0.00000854858 # ??? Parms.KD = 0.016 * Parms.KT # ??? Parms.g = 9.80 # m/s^2 (linDyn, A, B, uEq) = linQuadCopter.dynamicsAtHover(Parms) #--[1.2] Define the trajectory synthesizer. # tsys = structure() tsys.A = A tsys.B = B tsys.Q = (4e8) * np.diag( [15, 15, 20, 0.1, 0.1, 0.05, 80, 80, 140, 0.3, 0.3, 0.1]) #tsys.Q = np.diag([200, 300, 400, 100, 10, 10, 20, 20, 10, 10, 10, 10])
import numpy as np import math from niODERK4 import niODERK4 from controller.linear import linear, care from simController import simController import matplotlib.pyplot as plt from trajSynth.naive import naive from structures import structure #==[1] Configure the linear system. sys = structure() sys.A = np.array([[0, 1], [0.15, 0.25]]) sys.B = np.array([[0], [1]]) sys.solver = niODERK4 sys.dt = 0.01 sys.tspan = [0, 10] sys.rtol = 4e-3 # Initial and final states initial = np.array([[3], [0]]) final = np.array([[0.3], [0]]) Q = 50 * np.diag([6.00, 3.50]) [P, L, K] = care(sys.A, sys.B, Q) sys.K = -K sys.cons.sat.min = -5 sys.cons.sat.max = 5 #--[2] Instantiate the trajectory synthesizer.
def getState(self): sol = structure(t=self.solver.tc, x=self.solver.xc, u=self.uc) return sol
x0 = np.array([X_s_0, Y_s_0, T_s_0]) model = template_model() """ Setup graphic: """ def desTraj(t): myarr =[0,0] myarr[0] = 1-cos(1/2*t) myarr[1] = sin(1/2*t) return myarr #fig, ax, graphics = do_mpc.graphics.default_plot(mpc.data, figsize=(8,5)) #plt.ion() Ts = .01 #define the parameters param = structure() param.x0 = x0 param.Td = 1 param.Ts = Ts #define MPC objects myMPC = mpcDiff(param) #graphics.default_plot(states_list =['X_s','Y_s']) #this is basically a test of MPC fixed Horizons just in an unclean way fig, ax = plt.subplots() ax.set_prop_cycle(color =['red', 'black', 'yellow']) for i in range(10): myX = myMPC.followPath(x0,desTraj) x0 = myX[-1,:] plt.plot(myMPC.mpc.data['_x'][:,0],myMPC.mpc.data['_x'][:,1])
B[5, 1] = 1 #print(B) Q = np.diag(np.array([10, 10, 5, 2, 2, 2])) linctrl = linearSO( np.array([0, 0, 0, nuEq, 0, 0]).reshape((6, 1)), np.zeros((2, 1))) K = np.array([[2, 0, 0, 5, 0, 0], [0, 3, 4, 0, 7, 11]]) #linctrl.setByCARE(A, B, Q) linctrl.set(K) #linctrl.noControl() #leom = linearSO.systemDynamics(A, B) cfS = structure(dt=0.05, odeMethod=niODERK4, controller=linctrl) #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)]]) pathType = "arc" if (pathType == "lineX"): def line(t): if (np.isscalar(t)): length = 1 else: length = len(t) return nuEq * np.vstack((t, np.zeros((2, length)), np.ones( (1, length)), np.zeros((2, length)))) xi = np.array([0, 10, -np.pi / 3, 0, 0, 0])
break print('SPH factors') print(mu) rxn_ref = ref.phi_homo * ref.sig_t_homo # - np.sum(ref.sig_s_homo, axis=0)) rxn_homo = h**o.phi_homo * h**o.sig_t_homo # - np.sum(h**o.sig_s_homo, axis=0)) print('Make sure these reactions rates are the same if SPH is working properly') print(rxn_homo - rxn_ref) return ref_XS if __name__ == '__main__': np.set_printoptions(precision=6) G = 44 data_path = 'data3' assay_map = [0, 1] dgmstructure = computeBounds(G, 'full', 1, 0.0, 1.3, 60) O = dgmstructure.maxOrder xs_name = 'XS/{}gXS.anlxs'.format(G) for o in range(dgmstructure.maxOrder): # Get the homogenized cross sections mapping = structure(G, dgmstructure, o) ass1 = runSPH(G, assay_map, xs_name, mapping) pickle.dump(ass1, open('{}/refXS_sph_{}_o{}.p'.format(data_path, G, o), 'wb'))
#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() ts.Th = 1 ts.Td = 1 ts.Ts = .01 #linctrl.trackerPath(desTraj) fixedHorizonscontroller = fixedHorizons(tSynth, linctrl, ts) #Set up Timepoints