add doctest to user guide + internally generate figures

Author: murrayrm

control/tests/flatsys_test.py

@@ -24,48 +24,6 @@
 atol = 1e-4
 rtol = 1e-4
 
-# Define the kinematic car system
-def vehicle_flat_forward(x, u, params={}):
-    b = params.get('wheelbase', 3.)             # get parameter values
-    zflag = [np.zeros(3), np.zeros(3)]          # list for flag arrays
-    zflag[0][0] = x[0]                          # flat outputs
-    zflag[1][0] = x[1]
-    zflag[0][1] = u[0] * np.cos(x[2])           # first derivatives
-    zflag[1][1] = u[0] * np.sin(x[2])
-    thdot = (u[0]/b) * np.tan(u[1])             # dtheta/dt
-    zflag[0][2] = -u[0] * thdot * np.sin(x[2])  # second derivatives
-    zflag[1][2] =  u[0] * thdot * np.cos(x[2])
-    return zflag
-
-def vehicle_flat_reverse(zflag, params={}):
-    b = params.get('wheelbase', 3.)             # get parameter values
-    x = np.zeros(3); u = np.zeros(2)            # vectors to store x, u
-    x[0] = zflag[0][0]                          # x position
-    x[1] = zflag[1][0]                          # y position
-    x[2] = np.arctan2(zflag[1][1], zflag[0][1]) # angle
-    u[0] = zflag[0][1] * np.cos(x[2]) + zflag[1][1] * np.sin(x[2])
-    thdot_v = zflag[1][2] * np.cos(x[2]) - zflag[0][2] * np.sin(x[2])
-    u[1] = np.arctan2(thdot_v, u[0]**2 / b)
-    return x, u
-
-def vehicle_update(t, x, u, params):
-    b = params.get('wheelbase', 3.)             # get parameter values
-    dx = np.array([
-        np.cos(x[2]) * u[0],
-        np.sin(x[2]) * u[0],
-        (u[0]/b) * np.tan(u[1])
-    ])
-    return dx
-
-def vehicle_output(t, x, u, params): return x
-
-# Create differentially flat input/output system
-vehicle_flat = fs.FlatSystem(
-    vehicle_flat_forward, vehicle_flat_reverse, vehicle_update,
-    vehicle_output, inputs=('v', 'delta'), outputs=('x', 'y', 'theta'),
-    states=('x', 'y', 'theta'), name='vehicle_flat')
-
-
 class TestFlatSys:
     """Test differential flat systems"""
 
@@ -100,9 +58,48 @@ def test_double_integrator(self, xf, uf, Tf, basis):
         t, y, x = ct.forced_response(sys, T, ud, x1, return_x=True)
         np.testing.assert_array_almost_equal(x, xd, decimal=3)
 
-    @pytest.fixture(name='vehicle_flat')
-    def vehicle_flat_fixture(self):
-        return vehicle_flat
+    @pytest.fixture
+    def vehicle_flat(self):
+        """Differential flatness for a kinematic car"""
+        def vehicle_flat_forward(x, u, params={}):
+            b = params.get('wheelbase', 3.)             # get parameter values
+            zflag = [np.zeros(3), np.zeros(3)]          # list for flag arrays
+            zflag[0][0] = x[0]                          # flat outputs
+            zflag[1][0] = x[1]
+            zflag[0][1] = u[0] * np.cos(x[2])           # first derivatives
+            zflag[1][1] = u[0] * np.sin(x[2])
+            thdot = (u[0]/b) * np.tan(u[1])             # dtheta/dt
+            zflag[0][2] = -u[0] * thdot * np.sin(x[2])  # second derivatives
+            zflag[1][2] =  u[0] * thdot * np.cos(x[2])
+            return zflag
+
+        def vehicle_flat_reverse(zflag, params={}):
+            b = params.get('wheelbase', 3.)             # get parameter values
+            x = np.zeros(3); u = np.zeros(2)            # vectors to store x, u
+            x[0] = zflag[0][0]                          # x position
+            x[1] = zflag[1][0]                          # y position
+            x[2] = np.arctan2(zflag[1][1], zflag[0][1]) # angle
+            u[0] = zflag[0][1] * np.cos(x[2]) + zflag[1][1] * np.sin(x[2])
+            thdot_v = zflag[1][2] * np.cos(x[2]) - zflag[0][2] * np.sin(x[2])
+            u[1] = np.arctan2(thdot_v, u[0]**2 / b)
+            return x, u
+
+        def vehicle_update(t, x, u, params):
+            b = params.get('wheelbase', 3.)             # get parameter values
+            dx = np.array([
+                np.cos(x[2]) * u[0],
+                np.sin(x[2]) * u[0],
+                (u[0]/b) * np.tan(u[1])
+            ])
+            return dx
+
+        def vehicle_output(t, x, u, params): return x
+
+        # Create differentially flat input/output system
+        return fs.FlatSystem(
+            vehicle_flat_forward, vehicle_flat_reverse, vehicle_update,
+            vehicle_output, inputs=('v', 'delta'), outputs=('x', 'y', 'theta'),
+            states=('x', 'y', 'theta'))
 
     @pytest.mark.parametrize("basis", [
         fs.PolyFamily(6), fs.PolyFamily(8), fs.BezierFamily(6),
@@ -814,61 +811,3 @@ def test_flatsys_factory_function(self, vehicle_flat):
 
         with pytest.raises(TypeError, match="incorrect number or type"):
             flatsys = fs.flatsys(1, 2, 3, 4, 5)
-
-
-if __name__ == '__main__':
-    # Generate images for User Guide
-    import matplotlib.pyplot as plt
-
-    #
-    # Point to point
-    #
-    # Define the endpoints of the trajectory
-    x0 = [0., -2., 0.]; u0 = [10., 0.]
-    xf = [100., 2., 0.]; uf = [10., 0.]
-    Tf = 10
-
-    # Define a set of basis functions to use for the trajectories
-    poly = fs.PolyFamily(6)
-
-    # Find a trajectory between the initial condition and the final condition
-    traj = fs.point_to_point(vehicle_flat, Tf, x0, u0, xf, uf, basis=poly)
-
-    # Create the trajectory
-    timepts = np.linspace(0, Tf, 100)
-    xd, ud = traj.eval(timepts)
-    resp_p2p = ct.input_output_response(vehicle_flat, timepts, ud, X0=xd[:, 0])
-
-    #
-    # Solve OCP
-    #
-    # Define the cost along the trajectory: penalize steering angle
-    traj_cost = ct.optimal.quadratic_cost(
-        vehicle_flat, None, np.diag([0.1, 10]), u0=uf)
-
-    # Define the terminal cost: penalize distance from the end point
-    term_cost = ct.optimal.quadratic_cost(
-        vehicle_flat, np.diag([1e3, 1e3, 1e3]), None, x0=xf)
-
-    # Use a straight line as the initial guess
-    evalpts = np.linspace(0, Tf, 10)
-    initial_guess = np.array(
-        [x0[i] + (xf[i] - x0[i]) * evalpts/Tf for i in (0, 1)])
-
-    # Solve the optimal control problem, evaluating cost at timepts
-    bspline = fs.BSplineFamily([0, Tf/2, Tf], 4)
-    traj = fs.solve_flat_ocp(
-        vehicle_flat, evalpts, x0, u0, traj_cost,
-	terminal_cost=term_cost, initial_guess=initial_guess, basis=bspline)
-
-    xd, ud = traj.eval(timepts)
-    resp_ocp = ct.input_output_response(vehicle_flat, timepts, ud, X0=xd[:, 0])
-
-    #
-    # Plot the results
-    #
-    cplt = ct.time_response_plot(
-        ct.combine_time_responses([resp_p2p, resp_ocp]),
-        overlay_traces=True, trace_labels=['point_to_point', 'solve_ocp'])
-
-    plt.savefig('flatsys-steering-compare.png')

control/tests/timeplot_test.py

@@ -715,58 +715,3 @@ def test_legend_customization():
     fig = plt.figure()
     cplt = ct.step_response([sys1, sys2]).plot()
     cplt.set_plot_title("[List] " + fig._suptitle._text)
-
-    #
-    # Servomechanism example for documentation
-    #
-
-    # Parameter values
-    servomech_params = {
-        'J': 100,             # Moment of inertia of the motor
-        'b': 10,              # Angular damping of the arm
-        'k': 1,               # Spring constant
-        'r': 1,               # Location of spring contact on arm
-        'l': 2,               # Distance to the read head
-        'eps': 0.01,          # Magnitude of velocity-dependent perturbation
-    }
-
-    # State derivative
-    def servomech_update(t, x, u, params):
-        # Extract the configuration and velocity variables from the state vector
-        theta = x[0]                # Angular position of the disk drive arm
-        thetadot = x[1]             # Angular velocity of the disk drive arm
-        tau = u[0]                  # Torque applied at the base of the arm
-
-        # Get the parameter values
-        J, b, k, r = map(params.get, ['J', 'b', 'k', 'r'])
-
-        # Compute the angular acceleration
-        dthetadot = 1/J * (
-            -b * thetadot - k * r * np.sin(theta) + tau)
-
-        # Return the state update law
-        return np.array([thetadot, dthetadot])
-
-    # System output (tip radial position + angular velocity)
-    def servomech_output(t, x, u, params):
-        l = params['l']
-        return np.array([l * x[0], x[1]])
-
-    # System dynamics
-    servomech = ct.nlsys(
-        servomech_update, servomech_output, name='servomech',
-        params=servomech_params, states=['theta', 'thdot'],
-        outputs=['y', 'thdot'], inputs=['tau'])
-
-    timepts = np.linspace(0, 10)
-
-    U1 = np.sin(timepts)
-    resp1 = ct.input_output_response(servomech, timepts, U1)
-
-    U2 = np.cos(2*timepts)
-    resp2 = ct.input_output_response(servomech, timepts, U2)
-
-    plt.figure()
-    cplt = ct.combine_time_responses(
-      [resp1, resp2], trace_labels=["Scenario #1", "Scenario #2"]).plot()
-    plt.savefig('timeplot-servomech-combined.png')

doc/figures/Makefile

@@ -2,9 +2,7 @@
 # RMM, 26 Dec 2024
 
 # List of figures that need to be created (first figure generated is OK)
-FIGS = classes.pdf timeplot-mimo_step-default.png \
-  freqplot-siso_bode-default.png rlocus-siso_ctime-default.png \
-  phaseplot-dampedosc-default.png ctrlplot-servomech.png
+FIGS = classes.pdf
 
 # Location of the control package
 SRCDIR = ../..
@@ -16,18 +14,3 @@ clean:
 
 classes.pdf: classes.fig
 	fig2dev -Lpdf $< $@
-
-timeplot-mimo_step-default.png: $(SRCDIR)/control/tests/timeplot_test.py
-	PYTHONPATH=$(SRCDIR) python $<
-
-freqplot-siso_bode-default.png: $(SRCDIR)/control/tests/freqplot_test.py
-	PYTHONPATH=$(SRCDIR) python $<
-
-rlocus-siso_ctime-default.png: $(SRCDIR)/control/tests/rlocus_test.py
-	PYTHONPATH=$(SRCDIR) python $<
-
-phaseplot-dampedosc-default.png: $(SRCDIR)/control/tests/phaseplot_test.py
-	PYTHONPATH=$(SRCDIR) python $<
-
-ctrlplot-servomech.png: $(SRCDIR)/control/tests/ctrlplot_test.py
-	PYTHONPATH=$(SRCDIR) python $<