Mar 19, 2022 · Jan 25, 2022 · Jan 25, 2022 · Jan 25, 2022 · Jan 25, 2022 · Feb 5, 2022
diff --git a/control/flatsys/bezier.py b/control/flatsys/bezier.py
    """
    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
    -----
    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
    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

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

    # 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 orconstraints is not None) and \
    elif (cost is not None ortrajectory_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
    # 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
    # Start by solving the least squares problem
    alpha, residuals, rank, s = np.linalg.lstsq(M, Z, rcond=None)

    if cost is not None orconstraints is not None:
    if cost is not None ortrajectory_constraints is not None:
        # Search over the null space to minimize cost/satisfy constraints
        N = sp.linalg.null_space(M)

                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)
            traj_cost = lambda coeffs: coeffs @ coeffs

        # Process the constraints we were given
        traj_constraints =constraints
        ifconstraints is None:
        traj_constraints =trajectory_constraints
        iftraj_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
                    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:
    # Transform the trajectory from flat outputs to states and inputs
    #

    #Createa trajectory object to store theresul
    systraj = SystemTrajectory(sys, basis)
    #Create a trajectory object to store theresult
    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
        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.
        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
    """
    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

    """

    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):

            # 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
        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)
            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]
                             "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
        # 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)):
Original file line number	Diff line number	Diff line change
Expand Up		@@ -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
Expand Down
Original file line number	Diff line number	Diff line change
Expand Up		@@ -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
Expand DownExpand Up		@@ -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
Expand DownExpand Up		@@ -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
Expand DownExpand Up		@@ -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
Expand DownExpand Up		@@ -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))

Expand All		@@ -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 orconstraints is not None) and \
		elif (cost is not None ortrajectory_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
Expand All		@@ -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
Expand DownExpand Up		@@ -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 orconstraints is not None:
		if cost is not None ortrajectory_constraints is not None:
		# Search over the null space to minimize cost/satisfy constraints
		N = sp.linalg.null_space(M)

Expand All		@@ -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)
Expand All		@@ -429,13 +439,13 @@ def traj_cost(null_coeffs):
		traj_cost = lambda coeffs: coeffs @ coeffs

		# Process the constraints we were given
		traj_constraints =constraints
		ifconstraints is None:
		traj_constraints =trajectory_constraints
		iftraj_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
Expand All		@@ -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:
Expand DownExpand Up		@@ -507,8 +517,8 @@ def traj_const(null_coeffs):
		# Transform the trajectory from flat outputs to states and inputs
		#

		#Createa trajectory object to store theresul
		systraj = SystemTrajectory(sys, basis)
		#Create a trajectory object to store theresult
		systraj = SystemTrajectory(sys, basis, params=params)

		# Store the flag lengths and coefficients
		# TODO: make this more pythonic
Expand Down
Original file line number	Diff line number	Diff line change
Expand Up		@@ -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.
Expand All		@@ -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.
Expand Down
Original file line number	Diff line number	Diff line change
Expand Up		@@ -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):
Expand Down
Original file line number	Diff line number	Diff line change
Expand Up		@@ -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):
Expand DownExpand Up		@@ -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
Original file line number	Diff line number	Diff line change
Expand Up		@@ -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)
Expand DownExpand Up		@@ -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]
Expand All		@@ -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
Expand All		@@ -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)):
Expand Down