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')
plt.plot(xsol.t, xdes[0, :], 'g--')
plt.title("X position tracking")
plt.figure()
plt.plot(xsol.t, xsol.x[2, :], 'b')
plt.plot(xsol.t, xdes[2, :], 'g--')
plt.title("Y position tracking")
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()