#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()