almost working initial version

murrayrm · murrayrm · commit b0fa82ddf361 · 2012-11-11T15:59:06.000Z
diff --git a/examples/doubleint.py b/examples/doubleint.py
@@ -5,18 +5,26 @@
 # integrator system.  Mainly useful to show the simplest type of trajectory
 # generation computation.
 
+import numpy as np
+import matplotlib.pyplot as plt
 import control as ctrl                 # control system toolbox
-import trajgen as tg          # trajectory generation toolbox
+import trajgen as tg                   # trajectory generation toolbox
 
 # Define a double integrator system
 sys1 = ctrl.tf2ss(ctrl.tf([1], [1, 0, 0]))
 
 # Set the initial and final conditions
 x0 = (0, 0);
-xf = (1, 1);
+xf = (2, 1);
 
 # Find a trajectory
-xd, ud = tg.linear_point_to_point(sys1, x0, xf, 1)
-print(xd)
+systraj = tg.linear_point_to_point(sys1, x0, xf, 1)
 
 # Plot the trajectory
+t = np.linspace(0, 1, 100)
+xd, ud = systraj.eval(t)
+
+plt.figure(1); plt.clf()
+plt.plot(t, xd[:,0], 'b-', t, xd[:,1], 'g-', t, ud[:,0], 'r--')
+plt.legend(('x1', 'x2', 'u'))
+plt.show()
diff --git a/trajgen/__init__.py b/trajgen/__init__.py
@@ -1,5 +1,7 @@
 # Basis function sets
 from trajgen.poly import Poly
+from trajgen.systraj import SystemTrajectory
+from trajgen.linflat import LinearFlatSystem
 
 from flatsys import linear_point_to_point
 
diff --git a/trajgen/flatsys.py b/trajgen/flatsys.py
@@ -9,6 +9,7 @@
 import numpy as np
 import control
 from trajgen import Poly
+from trajgen import SystemTrajectory, LinearFlatSystem
 
 # Solve a point to point trajectory generation problem for a linear system
 def linear_point_to_point(sys, x0, xf, Tf, basis=None, cost=None, T0 = 0):
@@ -22,6 +23,9 @@ def linear_point_to_point(sys, x0, xf, Tf, basis=None, cost=None, T0 = 0):
         raise control.ControlNotImplemented(
             "only single input, single output systems are supported")
 
+    # Create a flat system representation
+    system = LinearFlatSystem(sys)
+    
     #
     # Determine the basis function set to use and make sure it is big enough
     #
@@ -33,23 +37,16 @@ def linear_point_to_point(sys, x0, xf, Tf, basis=None, cost=None, T0 = 0):
     if (basis.N < 2*sys.states):
         raise ValueError("basis set is too small")
 
-    #
-    # Find the transformation to bring the system into reachable form
-    # and use this to determine the flat output variable z = Cf*x
-    #
-    zsys, Tr = control.reachable_form(sys)
-    Cfz = np.zeros(np.shape(sys.C)); Cfz[-1] = 1
-    Cfx = Cfz * Tr
-
     #
     # Map the initial and final conditions to flat output conditions
     #
     # We need to compute the output "flag": [z(t), z'(t), z''(t), ...]
     # and then evaluate this at the initial and final condition.
     #
+    #! TODO: should be able to represent flag variables as 1D arrays
     zflag_T0 = np.zeros((sys.states, 1));
     zflag_Tf = np.zeros((sys.states, 1));
-    H = Cfx                             # initial state transformation
+    H = system.C                        # initial state transformation
     for i in range(sys.states):
         zflag_T0[i, 0] = H * np.matrix(x0).T
         zflag_Tf[i, 0] = H * np.matrix(xf).T
@@ -85,8 +82,11 @@ def linear_point_to_point(sys, x0, xf, Tf, basis=None, cost=None, T0 = 0):
     #
     # Transform the trajectory from flat outputs to states and inputs
     #
-    xdfcn = alpha
-    udfcn = lambda t: 0
+    systraj = SystemTrajectory(sys.states, sys.inputs)
+    systraj.system = system
+    systraj.basis = basis
+    systraj.coeffs = alpha
 
     # Return a function that computes inputs and states as a function of time
-    return xdfcn, udfcn
+    return systraj
+
diff --git a/trajgen/linflat.py b/trajgen/linflat.py
@@ -0,0 +1,27 @@
+import numpy as np
+import control
+
+class LinearFlatSystem:
+    def __init__(self, sys):
+        self.A = sys.A
+        self.B = sys.B
+
+        # Find the transformation to bring the system into reachable form
+        zsys, Tr = control.reachable_form(sys)
+        self.F = zsys.A[0,:]            # input function coeffs
+        self.T = Tr                     # state space transformation
+        self.Tinv = np.linalg.inv(Tr)   # computer inverse once
+        
+        # Compute the flat output variable z = C x
+        Cfz = np.zeros(np.shape(sys.C)); Cfz[-1] = 1
+        self.C = Cfz * Tr
+
+        # Keep track of the number of states and inputs
+        self.states = sys.states
+        self.inputs = sys.inputs
+
+    # Compute state and input from flat flag
+    def reverse(self, zflag):
+        x = self.Tinv * np.matrix(zflag[0:-1]).T
+        u = zflag[-1] - self.F * x
+        return np.reshape(x, self.states), np.reshape(u, self.inputs)
diff --git a/trajgen/systraj.py b/trajgen/systraj.py
@@ -0,0 +1,27 @@
+import numpy as np
+
+class SystemTrajectory:
+    def __init__(self, states, inputs):
+        self.states = states
+        self.inputs = inputs
+
+    # Evaluate the trajectory over a list of time points
+    def eval(self, tlist):
+        # Allocate space for the outputs
+        xd = np.zeros((len(tlist), self.states))
+        ud = np.zeros((len(tlist), self.inputs))
+
+        # Go through each time point and compute xd and ud via flat variables
+        for k in range(len(tlist)):
+            zflag = np.zeros(self.states + self.inputs)
+            for i in range(self.states + self.inputs):
+                for j in range(self.basis.N):
+                    #! TODO: rewrite eval_deriv to take in time vector
+                    zflag[i] += self.coeffs[j] * \
+                        self.basis.eval_deriv(j, i, tlist[k])
+
+            # Now copy the states and inputs
+            xd[k,:], ud[k,:] = self.system.reverse(zflag)
+
+        return xd, ud
+
