python-control · murrayrm · Mar 19, 2022 · Jan 25, 2022 · Jan 25, 2022 · Jan 25, 2022
diff --git a/control/flatsys/bezier.py b/control/flatsys/bezier.py
@@ -55,7 +55,7 @@ class BezierFamily(BasisFamily):
     """
     def __init__(self, N, T=1):
         """Create a polynomial basis of order N."""
-        self.N = N                      # save number of basis functions
+        super(BezierFamily, self).__init__(N)
         self.T = T                      # save end of time interval
 
     # Compute the kth derivative of the ith basis function at time t

diff --git a/control/flatsys/flatsys.py b/control/flatsys/flatsys.py
@@ -108,15 +108,15 @@ class FlatSystem(NonlinearIOSystem):
     -----
     The class must implement two functions:
 
-    zflag = flatsys.foward(x, u)
+    zflag = flatsys.foward(x, u, params)
         This function computes the flag (derivatives) of the flat output.
         The inputs to this function are the state 'x' and inputs 'u' (both
         1D arrays).  The output should be a 2D array with the first
         dimension equal to the number of system inputs and the second
         dimension of the length required to represent the full system
         dynamics (typically the number of states)
 
-    x, u = flatsys.reverse(zflag)
+    x, u = flatsys.reverse(zflag, params)
         This function system state and inputs give the the flag (derivatives)
         of the flat output.  The input to this function is an 2D array whose
         first dimension is equal to the number of system inputs and whose
@@ -216,7 +216,7 @@ def _flat_updfcn(self, t, x, u, params={}):
     def _flat_outfcn(self, t, x, u, params={}):
         # Return the flat output
         zflag = self.forward(x, u, params)
-        return np.array(zflag[:][0])
+        return np.array([zflag[i][0] for i in range(len(zflag))])
 
 
 # Utility function to compute flag matrix given a basis
@@ -244,8 +244,8 @@ def _basis_flag_matrix(sys, basis, flag, t, params={}):
 
 # Solve a point to point trajectory generation problem for a flat system
 def point_to_point(
-        sys, timepts, x0=0, u0=0, xf=0, uf=0, T0=0, basis=None, cost=None,
-        constraints=None, initial_guess=None, minimize_kwargs={}, **kwargs):
+        sys, timepts, x0=0, u0=0, xf=0, uf=0, T0=0, cost=None, basis=None,
+        trajectory_constraints=None, initial_guess=None, params=None, **kwargs):
     """Compute trajectory between an initial and final conditions.
 
     Compute a feasible trajectory for a differentially flat system between an
@@ -284,7 +284,7 @@ def point_to_point(
         Function that returns the integral cost given the current state
         and input.  Called as `cost(x, u)`.
 
-    constraints : list of tuples, optional
+    trajectory_constraints : list of tuples, optional
         List of constraints that should hold at each point in the time vector.
         Each element of the list should consist of a tuple with first element
         given by :class:`scipy.optimize.LinearConstraint` or
@@ -337,8 +337,15 @@ def point_to_point(
     T0 = timepts[0] if len(timepts) > 1 else T0
 
     # Process keyword arguments
+    if trajectory_constraints is None:
+        # Backwards compatibility
+        trajectory_constraints = kwargs.pop('constraints', None)
+
+    minimize_kwargs = {}
     minimize_kwargs['method'] = kwargs.pop('minimize_method', None)
     minimize_kwargs['options'] = kwargs.pop('minimize_options', {})
+    minimize_kwargs.update(kwargs.pop('minimize_kwargs', {}))
+
     if kwargs:
         raise TypeError("unrecognized keywords: ", str(kwargs))
 
@@ -353,11 +360,14 @@ def point_to_point(
     # Make sure we have enough basis functions to solve the problem
     if basis.N * sys.ninputs < 2 * (sys.nstates + sys.ninputs):
         raise ValueError("basis set is too small")
-    elif (cost is not None or constraints is not None) and \
+    elif (cost is not None or trajectory_constraints is not None) and \
          basis.N * sys.ninputs == 2 * (sys.nstates + sys.ninputs):
         warnings.warn("minimal basis specified; optimization not possible")
         cost = None
-        constraints = None
+        trajectory_constraints = None
+
+    # Figure out the parameters to use, if any
+    params = sys.params if params is None else params
 
     #
     # Map the initial and final conditions to flat output conditions
@@ -366,8 +376,8 @@ def point_to_point(
     # and then evaluate this at the initial and final condition.
     #
 
-    zflag_T0 = sys.forward(x0, u0)
-    zflag_Tf = sys.forward(xf, uf)
+    zflag_T0 = sys.forward(x0, u0, params)
+    zflag_Tf = sys.forward(xf, uf, params)
 
     #
     # Compute the matrix constraints for initial and final conditions
@@ -400,7 +410,7 @@ def point_to_point(
     # Start by solving the least squares problem
     alpha, residuals, rank, s = np.linalg.lstsq(M, Z, rcond=None)
 
-    if cost is not None or constraints is not None:
+    if cost is not None or trajectory_constraints is not None:
         # Search over the null space to minimize cost/satisfy constraints
         N = sp.linalg.null_space(M)
 
@@ -418,7 +428,7 @@ def traj_cost(null_coeffs):
                 zflag = (M_t @ coeffs).reshape(sys.ninputs, -1)
 
                 # Find states and inputs at the time points
-                x, u = sys.reverse(zflag)
+                x, u = sys.reverse(zflag, params)
 
                 # Evaluate the cost at this time point
                 costval += cost(x, u)
@@ -429,13 +439,13 @@ def traj_cost(null_coeffs):
             traj_cost = lambda coeffs: coeffs @ coeffs
 
         # Process the constraints we were given
-        traj_constraints = constraints
-        if constraints is None:
+        traj_constraints = trajectory_constraints
+        if traj_constraints is None:
             traj_constraints = []
-        elif isinstance(constraints, tuple):
+        elif isinstance(traj_constraints, tuple):
             # TODO: Check to make sure this is really a constraint
-            traj_constraints = [constraints]
-        elif not isinstance(constraints, list):
+            traj_constraints = [traj_constraints]
+        elif not isinstance(traj_constraints, list):
             raise TypeError("trajectory constraints must be a list")
 
         # Process constraints
@@ -456,7 +466,7 @@ def traj_const(null_coeffs):
                     zflag = (M_t @ coeffs).reshape(sys.ninputs, -1)
 
                     # Find states and inputs at the time points
-                    states, inputs = sys.reverse(zflag)
+                    states, inputs = sys.reverse(zflag, params)
 
                     # Evaluate the constraint function along the trajectory
                     for type, fun, lb, ub in traj_constraints:
@@ -507,8 +517,8 @@ def traj_const(null_coeffs):
     # Transform the trajectory from flat outputs to states and inputs
     #
 
-    # Createa  trajectory object to store the resul
-    systraj = SystemTrajectory(sys, basis)
+    # Create a trajectory object to store the result
+    systraj = SystemTrajectory(sys, basis, params=params)
 
     # Store the flag lengths and coefficients
     # TODO: make this more pythonic

diff --git a/control/flatsys/linflat.py b/control/flatsys/linflat.py
@@ -113,7 +113,7 @@ def __init__(self, linsys, inputs=None, outputs=None, states=None,
         self.Cf = Cfz @ Tr
 
     # Compute the flat flag from the state (and input)
-    def forward(self, x, u):
+    def forward(self, x, u, params):
         """Compute the flat flag given the states and input.
 
         See :func:`control.flatsys.FlatSystem.forward` for more info.
@@ -130,7 +130,7 @@ def forward(self, x, u):
         return zflag
 
     # Compute state and input from flat flag
-    def reverse(self, zflag):
+    def reverse(self, zflag, params):
         """Compute the states and input given the flat flag.
 
         See :func:`control.flatsys.FlatSystem.reverse` for more info.

diff --git a/control/flatsys/poly.py b/control/flatsys/poly.py
@@ -52,7 +52,7 @@ class PolyFamily(BasisFamily):
     """
     def __init__(self, N):
         """Create a polynomial basis of order N."""
-        self.N = N                    # save number of basis functions
+        super(PolyFamily, self).__init__(N)
 
     # Compute the kth derivative of the ith basis function at time t
     def eval_deriv(self, i, k, t):

diff --git a/control/flatsys/systraj.py b/control/flatsys/systraj.py
@@ -62,14 +62,15 @@ class SystemTrajectory:
 
     """
 
-    def __init__(self, sys, basis, coeffs=[], flaglen=[]):
+    def __init__(self, sys, basis, coeffs=[], flaglen=[], params=None):
         """Initilize a system trajectory object."""
         self.nstates = sys.nstates
         self.ninputs = sys.ninputs
         self.system = sys
         self.basis = basis
         self.coeffs = list(coeffs)
         self.flaglen = list(flaglen)
+        self.params = sys.params if params is None else params
 
     # Evaluate the trajectory over a list of time points
     def eval(self, tlist):
@@ -112,6 +113,7 @@ def eval(self, tlist):
 
             # Now copy the states and inputs
             # TODO: revisit order of list arguments
-            xd[:,tind], ud[:,tind] = self.system.reverse(zflag)
+            xd[:,tind], ud[:,tind] = \
+                self.system.reverse(zflag, self.params)
 
         return xd, ud
diff --git a/control/iosys.py b/control/iosys.py
@@ -1820,6 +1820,7 @@ def input_output_response(
         legal_shapes = [(sys.ninputs, n_steps)]
     U = _check_convert_array(U, legal_shapes,
                              'Parameter ``U``: ', squeeze=False)
+    U = U.reshape(-1, n_steps)
 
     # Check to make sure this is not a static function
     nstates = _find_size(sys.nstates, X0)
@@ -1870,6 +1871,11 @@ def ivp_rhs(t, x):
             ivp_rhs, (T0, Tf), X0, t_eval=T,
             vectorized=False, **solve_ivp_kwargs)
 
+        if not soln.success or soln.status != 0:
+            # Something went wrong
+            warn("sp.integrate.solve_ivp failed")
+            print("Return bunch:", soln)
+
         # Compute the output associated with the state (and use sys.out to
         # figure out the number of outputs just in case it wasn't specified)
         u = U[0] if len(U.shape) == 1 else U[:, 0]
@@ -1886,7 +1892,7 @@ def ivp_rhs(t, x):
                              "equally spaced.")
 
         # Make sure the sample time matches the given time
-        if (sys.dt is not True):
+        if sys.dt is not True:
             # Make sure that the time increment is a multiple of sampling time
 
             # TODO: add back functionality for undersampling
@@ -1903,7 +1909,7 @@ def ivp_rhs(t, x):
         # Compute the solution
         soln = sp.optimize.OptimizeResult()
         soln.t = T                      # Store the time vector directly
-        x = [float(x0) for x0 in X0]    # State vector (store as floats)
+        x = X0                          # Initilize state
         soln.y = []                     # Solution, following scipy convention
         y = []                          # System output
         for i in range(len(T)):