#linctrl.setByCARE(A, B, Q) K = np.array([[2, 0, 0, 5, 0, 0], [0, 3, 4, 0, 7, 11]]) #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)
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) sm = cfS.controller.simBuilder(leom, cfS) istate = structure() istate.x = path.x(0) sim = sm.firstBuild(istate, desTraj) xsol = sim.simulate() xdes = np.squeeze(desTraj.x(xsol.t)) plt.figure() plt.plot(xsol.t, xsol.x[0, :], 'b')
#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]) tspan = [0, 30] path = Explicit(line, tspan=tspan) elif (pathType == 'lineTheta'): theta = -0.2 v = 0.5 * nuEq def line(t): if (np.isscalar(t)): length = 1 else: length = len(t) xc = np.cos(theta) yc = np.sin(theta) out = np.vstack((v * xc * t, v * yc * t, theta * np.ones( (1, length)), v * xc * np.ones((1, length)), v * yc * np.ones(
def line(t): if (np.isscalar(t)): length = 1 else: length = len(t) xc = np.cos(theta) yc = np.sin(theta) out = np.vstack((v * xc * t, v * yc * t, v * xc * np.ones( (1, length)), v * yc * np.ones((1, length)))) return out fCurve = line path = Explicit(fCurve, tspan=tspan) #pdb.set_trace() desTraj = trajectory.Path(path, tspan) sm = cfS.controller.simBuilder(leom, cfS) istate = structure() istate.x = path.x(0) sim = sm.firstBuild(istate, desTraj) xsol = sim.simulate() xdes = np.squeeze(desTraj.x(xsol.t)) plt.figure() plt.plot(xsol.t, xsol.x[0, :])
import sys #sys.path.append('./') import numpy as np import Curves.Explicit as Explicit from matplotlib import pyplot as plt tspan = [0, 1] def line2D(t): out = np.array([[2], [3]]) * t return out t = np.linspace(tspan[0], tspan[1], 100) curve = Explicit(line2D, tspan) x = curve.x(t) plt.plot(x[0, :], x[1, :]) plt.show()