Python control unable to simulate static nonlinearity

Question

I'm new to using python for control development (coming from MATLAB). It seems capable enough for basic problems but more complicated things get cumbersome. I'm trying to build a feedback system for satellite attitude control using interconnected IO systems. Nonlinear systems are created using updfnc(t,x,u,params) & outfcn(t,x,u,params) definitions and the control.NonlinearIOSystem(). Then connected using control.interconnect(). The problems arise when trying to simulate this interconnected closed loop system using control.input_output_response(). It appears the RK45 solver calls the outfcn of the connected systems several times per timestep, often (several times per timestep) the input to the outfcn is zero (not just at the initial part of the simulation) instead of the values of a preceding "blocks" states/output.

Is this normal behaviour for the solver? If anything, I would have expected the updfcn to be called several times rather than the outfcn as per my understanding of how Runge-Kutta solvers work.

Functions can do division by the input (allowed since some variables passed between connected systems can never be zero, quaternions), if these functions are called with zero input the simulation break, but more worrying, in the general case, the output might not be relevant for the simulation if incorrect (zero) input is used.

Below is (part of) the output when code is run with print statements for debugging. The occurance of outfcn relative to updfcn is worrying and the passing of inputs (u) that are zero valued.

Called rotKin_outfcn with u: 
[0. 0. 0.]

Called rotKin_outfcn with u: 
[0.53  0.53  0.053]

Called markleyPD_outfcn with u: 
[0. 0. 0. 0. 0. 0. 0.]

Called rotKin_outfcn with u: 
[0. 0. 0.]

Called rotKin_outfcn with u: 
[0.53  0.53  0.053]

Called markleyPD_outfcn with u: 
[0.53   0.53   0.053  0.6853 0.6953 0.1531 0.1531]

Called rotKin_outfcn with u: 
[0. 0. 0.]

Called rotKin_outfcn with u: 
[0.53  0.53  0.053]

Called markleyPD_outfcn with u: 
[0.53   0.53   0.053  0.6853 0.6953 0.1531 0.1531]

Called rotKin_outfcn with u: 
[0. 0. 0.]

Called rotKin_outfcn with u: 
[0.53  0.53  0.053]

Called rotKin_updfcn with u: 
[0.53  0.53  0.053]

Called rotKin_outfcn with u: 
[0. 0. 0.]

Called rotKin_outfcn with u: 
[0.52958536 0.5297074  0.0532223 ]

Called markleyPD_outfcn with u: 
[0. 0. 0. 0. 0. 0. 0.]

Called rotKin_outfcn with u: 
[0. 0. 0.]

Called rotKin_outfcn with u: 
[0.52958536 0.5297074  0.0532223 ]

Called markleyPD_outfcn with u: 
[0.52958536 0.5297074  0.0532223  0.6854992  0.69598091 0.15311521
 0.14910081]

Called rotKin_outfcn with u: 
[0. 0. 0.]

Called rotKin_outfcn with u: 
[0.52958536 0.5297074  0.0532223 ]

Called markleyPD_outfcn with u: 
[0.52958536 0.5297074  0.0532223  0.6854992  0.69598091 0.15311521
 0.14910081]

Called rotKin_outfcn with u: 
[0. 0. 0.]

Called rotKin_outfcn with u: 
[0.52958536 0.5297074  0.0532223 ]

Called rotKin_updfcn with u: 
[0.52958536 0.5297074  0.0532223 ]

Called rotKin_outfcn with u: 
[0. 0. 0.]

Called rotKin_outfcn with u: 
[0.52922002 0.52944959 0.05341816]

Called markleyPD_outfcn with u: 
[0. 0. 0. 0. 0. 0. 0.]

Called rotKin_outfcn with u: 
[0. 0. 0.]

Called rotKin_outfcn with u: 
[0.52922002 0.52944959 0.05341816]

I'm aware I can code my way around the divisions by zero, but that doesn't solve the underlaying problem of output functions being fed zero in the first place. As I build more complexity having more interconnected systems appears to introduce even more unpredictability.

I ask myself:

• Am I using control.interconnect() correct?
• Why does the system solver try to solve the static nonlinearities (the PD controller with no states) using the RK45 solver? It could just do the arithmetic once per step?
• Why calling outfcn so many times? This seems to just increase as well when the number of interconnected systems increase.
• Might there be an underlaying problem in the control library?

Example Trace of the error I get when using the input (u) for quaternion operations:

---------------------------------------------------------------------------
ValueError                                Traceback (most recent call last)
Input In [16], in <cell line: 117>()
    114 omega_0 = [0.53, 0.53, 0.053]
    115 x_0 = np.concatenate([omega_0, q_0])
--> 117 y = control.input_output_response(system, time, X0=x_0)

File ~/Repositories/aocs-utils/.venv_aocs_utils/lib/python3.8/site-packages/control/iosys.py:1839, in input_output_response(sys, T, U, X0, params, transpose, return_x, squeeze, solve_ivp_kwargs, t_eval, **kwargs)
   1836 if not hasattr(sp.integrate, 'solve_ivp'):
   1837     raise NameError("scipy.integrate.solve_ivp not found; "
   1838                     "use SciPy 1.0 or greater")
-> 1839 soln = sp.integrate.solve_ivp(
   1840     ivp_rhs, (T0, Tf), X0, t_eval=t_eval,
   1841     vectorized=False, **solve_ivp_kwargs)
   1842 if not soln.success:
   1843     raise RuntimeError("solve_ivp failed: " + soln.message)

File ~/Repositories/aocs-utils/.venv_aocs_utils/lib/python3.8/site-packages/scipy/integrate/_ivp/ivp.py:546, in solve_ivp(fun, t_span, y0, method, t_eval, dense_output, events, vectorized, args, **options)
    543 if method in METHODS:
    544     method = METHODS[method]
--> 546 solver = method(fun, t0, y0, tf, vectorized=vectorized, **options)
    548 if t_eval is None:
    549     ts = [t0]

File ~/Repositories/aocs-utils/.venv_aocs_utils/lib/python3.8/site-packages/scipy/integrate/_ivp/rk.py:94, in RungeKutta.__init__(self, fun, t0, y0, t_bound, max_step, rtol, atol, vectorized, first_step, **extraneous)
     92 self.max_step = validate_max_step(max_step)
     93 self.rtol, self.atol = validate_tol(rtol, atol, self.n)
---> 94 self.f = self.fun(self.t, self.y)
     95 if first_step is None:
     96     self.h_abs = select_initial_step(
     97         self.fun, self.t, self.y, self.f, self.direction,
     98         self.error_estimator_order, self.rtol, self.atol)

File ~/Repositories/aocs-utils/.venv_aocs_utils/lib/python3.8/site-packages/scipy/integrate/_ivp/base.py:138, in OdeSolver.__init__.<locals>.fun(t, y)
    136 def fun(t, y):
    137     self.nfev += 1
--> 138     return self.fun_single(t, y)

File ~/Repositories/aocs-utils/.venv_aocs_utils/lib/python3.8/site-packages/scipy/integrate/_ivp/base.py:20, in check_arguments.<locals>.fun_wrapped(t, y)
     19 def fun_wrapped(t, y):
---> 20     return np.asarray(fun(t, y), dtype=dtype)

File ~/Repositories/aocs-utils/.venv_aocs_utils/lib/python3.8/site-packages/control/iosys.py:1832, in input_output_response.<locals>.ivp_rhs(t, x)
   1831 def ivp_rhs(t, x):
-> 1832     return sys._rhs(t, x, ufun(t))

File ~/Repositories/aocs-utils/.venv_aocs_utils/lib/python3.8/site-packages/control/iosys.py:1024, in InterconnectedSystem._rhs(self, t, x, u)
   1021 u = np.array(u, ndmin=1)
   1023 # Compute the input and output vectors
-> 1024 ulist, ylist = self._compute_static_io(t, x, u)
   1026 # Go through each system and update the right hand side for that system
   1027 xdot = np.zeros((self.nstates,))        # Array to hold results

File ~/Repositories/aocs-utils/.venv_aocs_utils/lib/python3.8/site-packages/control/iosys.py:1074, in InterconnectedSystem._compute_static_io(self, t, x, u)
   1071 state_index, input_index, output_index = 0, 0, 0
   1072 for sys in self.syslist:
   1073     # Compute outputs for each system from current state
-> 1074     ysys = sys._out(
   1075         t, x[state_index:state_index + sys.nstates],
   1076         ulist[input_index:input_index + sys.ninputs])
   1078     # Store the outputs at the start of ylist
   1079     ylist[output_index:output_index + sys.noutputs] = \
   1080         ysys.reshape((-1,))

File ~/Repositories/aocs-utils/.venv_aocs_utils/lib/python3.8/site-packages/control/iosys.py:849, in NonlinearIOSystem._out(self, t, x, u)
    848 def _out(self, t, x, u):
--> 849     y = self.outfcn(t, x, u, self._current_params) \
    850         if self.outfcn is not None else x
    851     return np.array(y).reshape((-1,))

Input In [16], in markleyPD_outfcn(t, x, u, params)
     44 print('\nCalled markleyPD_outfcn with u: ')
     45 print(u)
---> 46 rot = Rotation.from_quat(u[3:7])
     48 ## Markley p.291
     49 # τ = -k_p*sign(dq_4)*dq_0:3 - k_d*ω
     50 tau = -k_p*np.sign(u[6])*u[3:6] - k_d*u[0:3]

File _rotation.pyx:627, in scipy.spatial.transform._rotation.Rotation.from_quat()

File _rotation.pyx:531, in scipy.spatial.transform._rotation.Rotation.__init__()

ValueError: Found zero norm quaternions in `quat`.

Below is minimalistic code that can reproduce the issue:

import numpy as np
import control
from scipy.spatial.transform import Rotation

# Rotational dynamics state update function
def rotDyn_updfcn(t, x, u, params):
    I = params.get('sat_inertia')
    
    ## Markley p.84
    # I*dω = τ_e - ω x (Iω)
    dx = np.matmul(np.linalg.inv(I), (u - np.cross(x, np.matmul(I,x))))
    
    return dx


# Rotational kinematics state update function
def rotKin_updfcn(t, x, u, params):
    
    print('\nCalled rotKin_updfcn with u: ')
    print(u)

    ## Markley p.71
    # dq = 1/2 * Ξ(q)*ω   
    dq = np.array([[ x[3]*u[0] - x[2]*u[1] + x[1]*u[2]],
                   [ x[2]*u[0] + x[3]*u[1] - x[0]*u[2]],
                   [-x[1]*u[0] + x[0]*u[1] + x[3]*u[2]],
                   [-x[0]*u[0] - x[1]*u[1] - x[2]*u[2]]]) / 2
    
    return dq

# Rotational kinematics output function
def rotKin_outfcn(t, x, u, params):
    
    print('\nCalled rotKin_outfcn with u: ')
    print(u)
    
    return x

# Basic quaternion attitude feedback controller
def markleyPD_outfcn(t, x, u, params):
    k_p = params.get('k_p')
    k_d = params.get('k_d')
    
    print('\nCalled markleyPD_outfcn with u: ')
    print(u)
    rot = Rotation.from_quat(u[3:7])
    
    ## Markley p.291
    # τ = -k_p*sign(dq_4)*dq_0:3 - k_d*ω
    tau = -k_p*np.sign(u[6])*u[3:6] - k_d*u[0:3]
    
    return tau


params = {
    "sat_inertia": [[10000, 0, 0],
                    [0, 9000, 0],
                    [0, 0, 12000]],
    "k_p": 50,
    "k_d": 500
}

## Plant construction
rotDyn = control.NonlinearIOSystem(rotDyn_updfcn,
                                   None,
                                   name='rotDyn',
                                   inputs=['L','M','N'],
                                   outputs=['P','Q','R'],
                                   states=['P','Q','R'],
                                   params=params,
                                   dt=0
                                  )

rotKin = control.NonlinearIOSystem(rotKin_updfcn,
                                   rotKin_outfcn,
                                   name='rotKin',
                                   inputs=['P','Q','R'],
                                   outputs=['q0','q1','q2','q3'],
                                   states=['q0','q1','q2','q3'],
                                   params=params,
                                   dt=0
                                  )

plant = control.interconnect((rotDyn, rotKin),
                             name='plant',
                             inputs=rotDyn.input_labels,
                             outputs=rotDyn.output_labels + rotKin.output_labels,
                             states=rotDyn.state_labels + rotKin.state_labels
                            )
plant.name='plant' #Required due to bug in interconnect?

## Controller construction
ctrl = control.NonlinearIOSystem(None,
                                 markleyPD_outfcn,
                                 inputs = ['P', 'Q', 'R', 'q0', 'q1', 'q2', 'q3'],
                                 outputs = ['L', 'M', 'N'],
                                 params=params,
                                 dt=0
                                )

## Closed loop system
system = control.interconnect([plant, ctrl],
                              name='system',
                              outputs=ctrl.output_labels + plant.output_labels,
                              states=plant.state_labels
                             )
system.name = 'system'

## Run simulation
time = np.linspace(0,300,3000)

# initial condition
q_0 = [0.6853, 0.6953, 0.1531, 0.1531]
omega_0 = [0.53, 0.53, 0.053]
x_0 = np.concatenate([omega_0, q_0])

y = control.input_output_response(system, time, X0=x_0)

I know this is quite the complex problem mathematically, but hoping someone could at least shed some light on my usage of interconnect() when solving of the differential equations and the usage of the python control library.