solving a feedback controller for a dynamical system with 16 states and 4 inputs.

[Kamyab-Majid]

I have already solved it in Matlab, But I cannot do it using simupy.
It is a 16 states dynamical system, and the controller is a sliding mode.
I tried just the system (without controller) and it worked out in simupy but I cannot add controller to it.
this is the code, the full code is given in the file:

1. DEFAULT_INTEGRATOR_OPTIONS['nsteps'] = 1000
2. pi = sp.symbols('pi')
3. state_equations = Array([x_dot1_3[0],x_dot1_3[1],x_dot1_3[2],x_dot4_6[0],x_dot4_6[1],x_dot4_6[2],x_dot7_9[0],x_dot7_9[1],x_dot7_9[2],x_dot10_12[0],x_dot10_12[1],x_dot10_12[2],x_dot13,x_dot14,x_dot15,x_dot16])
6. ctrl_input=Array([ctrl[0], ctrl[1],ctrl[2],ctrl[3]])
7. sys = DynamicalSystem(state_equation=state_equations,state=x_state,input_=U_input)
8. sys.initial_condition = np.array([0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0])
9. ctr=DynamicalSystem(output_equation=ctrl_input,state=U_input,input_=x_state)
10. BD = BlockDiagram(sys,ctr)
11. BD.connect(sys, ctr)
12. BD.connect(ctr, sys)
13. res = BD.simulate(0.01)
14. plt.figure()
15. plt.plot(res.t, res.x)

it gives the error:

`---------------------------------------------------------------------------
ValueError Traceback (most recent call last)
in
9 sys = DynamicalSystem(state_equation=state_equations,state=x_state,input_=U_input)
10 sys.initial_condition = np.array([0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0])
---> 11 ctr=DynamicalSystem(output_equation=ctrl_input,state=U_input,input_=x_state)
12 ctr.initial_condition = np.array([0,0,0,0])
13 BD = BlockDiagram(sys,ctr)

~/.local/lib/python3.8/site-packages/simupy/systems/symbolic.py in init(self, state_equation, state, input_, output_equation, constants_values, dt, initial_condition, code_generator, code_generator_args)
67 self.code_generator_args = code_gen_args_to_set
68
---> 69 self.state_equation = state_equation
70 self.output_equation = output_equation
71

~/.local/lib/python3.8/site-packages/simupy/systems/symbolic.py in state_equation(self, state_equation)
120
121 self._state_equation = state_equation
--> 122 self.update_state_equation_function()
123
124 self.state_jacobian_equation = grad(self.state_equation, self.state)

~/.local/lib/python3.8/site-packages/simupy/systems/symbolic.py in update_state_equation_function(self)
158
159 def update_state_equation_function(self):
--> 160 if not self.dim_state or self.state_equation == empty_array():
161 return
162 self.state_equation_function = self.code_generator(

~/.local/lib/python3.8/site-packages/sympy/tensor/array/ndim_array.py in eq(self, other)
494 return dict(self._sparse_array) == dict(other._sparse_array)
495
--> 496 return list(self) == list(other)
497
498 def ne(self, other):

~/.local/lib/python3.8/site-packages/sympy/tensor/array/ndim_array.py in iterator()
459 yield self[i]
460 else:
--> 461 yield self[()]
462
463 return iterator()

~/.local/lib/python3.8/site-packages/sympy/tensor/array/dense_ndim_array.py in getitem(self, index)
59 return type(self)(array, nshape)
60 else:
---> 61 index = self._parse_index(index)
62 return self._array[index]
63

~/.local/lib/python3.8/site-packages/sympy/tensor/array/ndim_array.py in _parse_index(self, index)
76
77 if self._loop_size == 0:
---> 78 raise ValueError("Index not valide with an empty array")
79
80 if len(index) != self._rank:

ValueError: Index not valide with an empty array`

the whole part of the code is given in the link
https://hastebin.com/monoculihe.makefile
, the only part that might be the issue is the
ctrl = sp.UnevaluatedExpr(b_flapping)**-1*(-f + sdotr - kmod)
also I used sympy.Matrix for defining the terms.
does simupy works with UnevaluatedExpr? because the inverse is too hard it takes forever to calculate the inverse so I changed it to unevaluated. other than that I think the problem is with the given part of the code that I have given in the comment section. I did not quite fully understand how to work with simupy Although I saw all of your examples and documents
please help :(

[sixpearls]

Hi @MKamyab1991, there are a few issues here:

1. I believe you are using SymPy version >= 1.5, which made a breaking change to some of the Array object internals that SimuPy relies on. Try downgrading SymPy to version 1.4.
2. I can't tell what the equations are for your controller; do you mean to include state variables (i.e., variables that integrate over time) ? If not, your controller declaration DynamicalSystem(output_equation=ctrl_input,state=U_input,input_=x_state) is incorrect. I'm guessing you don't need a state argument and should change the output_equation argument to be the expression for the control output. I'm also not quite sure what the difference is between U_input and ctrl_input, if your plant requires both you can combine them.
3. You cannot use the UnevaluatedExpr to skip the inverse because you need it to be evaluated to calculate the controller output. You could write your own numerical function to evaluate the control law (perhaps using SymPy codegen to write the b_flapping matrix as a function of the plant's output) or you could do some more work with SymPy to do the symbolic manipulations. One step that might help is to create a simple matrix with one symbol in each position of the Matrix (taking advantage of the sparsity pattern if relevant). Taking the inverse of the simpler Matrix will be faster, then you can replace the elements with the more complicated b_flapping matrix elements. This can sometimes make taking the inverse of complicated matrices more tractable. I have also moved away from using the symbolic DynamicalSystem's in SimuPy. Due to issues with symbolic computations, I prefer to do the symbolics one time and use codegen to write numeric python functions to a file that can be used without re-computing the symbolics.

[Kamyab-Majid]

Hi @MKamyab1991, there are a few issues here:

1. I believe you are using SymPy version >= 1.5, which made a breaking change to some of the Array object internals that SimuPy relies on. Try downgrading SymPy to version 1.4.
2. I can't tell what the equations are for your controller; do you mean to include state variables (i.e., variables that integrate over time) ? If not, your controller declaration DynamicalSystem(output_equation=ctrl_input,state=U_input,input_=x_state) is incorrect. I'm guessing you don't need a state argument and should change the output_equation argument to be the expression for the control output. I'm also not quite sure what the difference is between U_input and ctrl_input, if your plant requires both you can combine them.
3. You cannot use the UnevaluatedExpr to skip the inverse because you need it to be evaluated to calculate the controller output. You could write your own numerical function to evaluate the control law (perhaps using SymPy codegen to write the b_flapping matrix as a function of the plant's output) or you could do some more work with SymPy to do the symbolic manipulations. One step that might help is to create a simple matrix with one symbol in each position of the Matrix (taking advantage of the sparsity pattern if relevant). Taking the inverse of the simpler Matrix will be faster, then you can replace the elements with the more complicated b_flapping matrix elements. This can sometimes make taking the inverse of complicated matrices more tractable. I have also moved away from using the symbolic DynamicalSystem's in SimuPy. Due to issues with symbolic computations, I prefer to do the symbolics one time and use codegen to write numeric python functions to a file that can be used without re-computing the symbolics.
   Thank you so much for your response and Happy holidays, hereby I discussed each issue.

First issue

it is resolved thanks.

the second issue:

Yes you are right I did not want to integrate, control output is given as a function of the previous states of the system.
"your controller declaration DynamicalSystem(output_equation=ctrl_input,state=U_input,input_=x_state) is incorrect. I'm guessing you don't need a state argument and should change the output_equation argument to be the expression for the control output".
So it would look like this right? DynamicalSystem(output_equation=ctrl_input,input_=x_state), by ctrl_input I mean the control input to the system or as you say the control output.
"I'm also not quite sure what the difference is between U_input and ctrl_input, if your plant requires both you can combine them."
U_input and ctrl_input are the same, but when I want to declare the state equations, (AX+BU) what would I replace U with? I assume I did wrong but I used "U_input =U1,U2,U3,U4= Array(dynamicsymbols('U1:5'))". I thought the way to imploy a control system is to first use this Array in state equations and then declare the control input. So the schematic of what I had in mind is given below:

The Third issue:

perhaps using SymPy codegen to write the b_flapping matrix as a function of the plant's output
I am learning what you said here. Please let me know if there is any example, sample code, or something else that might help.
Thanks
Majid

[sixpearls]

1. Declaring systems: Okay, I figured this is what you intended. From your hastebin, your control system declaration would be something like

   ctrl_expr = sp.UnevaluatedExpr(b_flapping)**-1*(-f + sdotr - kmod) # line 434
   ctr=DynamicalSystem(output_equation=ctrl_expr, input_=x_state) # line 465

   Then each element of ctrl_expr will be routed to each element of U_input on the plant side when you connect them.

2. Constructing b_flapping inverse more effectively: I cannot guarantee this will be substantially faster, but I have found this to be a useful pattern:

   from simupy.matrices import construct_explicit_matrix
   matrix_placeholder = construct_explicit_matrix('a', 4, 4, dynamic=False)
   placeholder_solve_dict = sp.solve(matrix_placeholder-b_flapping, matrix_placeholder) 
   b_flapping_inv = (matrix_placeholder**-1).subs(placeholder_solve_dict)
   ctrl_expr = b_flapping_inv*(-f + sdotr - kmod) # line 434

   The construct_explicit_matrix is just a helper so that matrix_placeholder is a symbolic matrix of the form

   Matrix([
   [a_11, a_12, a_13, a_14],
   [a_21, a_22, a_23, a_24],
   [a_31, a_32, a_33, a_34],
   [a_41, a_42, a_43, a_44]])

   and placeholder_solve_dict holds the assignments of the a_ij elements to the corresponding elements of b_flapping. SymPy should handle inverting the simpler place-holder. Then you substitute the elements. You could add a sp.simplify call, but that may be computationally expensive as well. I am still working on documentation to use codegen to write a python function that could be used directly without re-running the symbolic algebra. If you haven't solved the problem by then I'll point you to it.

   By the way, you said you had solved the problem using matlab. What did you use to compute the controller expressions? You may be able to write your plant and controller model following your approach from matlab without using symbolics at all. For example, your RbI and thetabi functions could compute numerical matrices by using numpy and assuming theta is numeric.

[Kamyab-Majid]

Thanks Ben for your response and happy new year.
I used your suggestion about codegen and I solved everything numerically using
from scipy.integrate import odeint for integrating.
the strategy is to find state equations using sympy symbols and then I used lambdify to define a symbolic function for the state equations.
Then control is derived numerically based on the current states

the full code is given in:
https://hastebin.com/wehejujale.yaml

I tried your method and again it take forever to derive the inverse
placeholder_solve_dict = sp.solve(matrix_placeholder-b_flapping, matrix_placeholder)

By the way, you said you had solved the problem using Matlab
I do not know how Matlab solves its equation cause it is not open source but you just have to define the control function and system and connect these two using Simulink. so there are no symbolic equations involved. It solves numerically I guess using a numerical function called ode45.

I am still working on documentation to use codegen to write a python function that could be used directly without re-running the symbolic algebra
I can close this if you insist but I would like to know about it. So if you want me to close it please let me know about your new function.
Thanks

[sixpearls]

Hi @MKamyab1991

I saw your post a few days ago and didn't have the chance to respond and it looks like the hastebin may have expired?

From what I remember, you wrote a class method for the state equation (helicoptor model) and for the controller equation. You could put them in blocks something along the lines

helicoptor_block = simupy.systems.DynamicalSystem(state_equation_function=your_model.helicoptor_state_equation, dim_state=16, dim_input=4)
controller_block = simupy.systems.DynamicalSystem(output_equation_function=your_model.controller_function, dim_state=0, dim_output=4, dim_input=16)

where your_model is the instance of the class you developed the methods in, helicoptor_state_equation(self, time, state, input) is the method for helicoptor's state equation function that returns state's time derivative as a function of time, state, and input and controller_function(self, time, state) is the method that returns the controller input as a function of time and the helicopter state. Note that because of the signature, you may have to unwrap the state or input into individual elements to compute the state.

I can write some draft documentation and examples for the codegen and post it here for you to try before closing this. Until then, the functions you wrote in matlab (without any symbolic equation) should be pretty directly translateable to python using numpy and scipy.

[Kamyab-Majid]

https://hastebin.com/akakususug.yaml
this is the new link :). the procedure is simple. state_equations are solved using odeint and controller equations are found by getting the output of odeint.
the problem of this line
controller_block = simupy.systems.DynamicalSystem(output_equation_function=your_model.controller_function, dim_state=0, dim_output=4, dim_input=16)
was that there was an inverse that seemed to be impossible to be solved using symbolic equations (probably too hard). so
your_model.controller_function cannot be found symbolically.
I can write some draft documentation and examples for the codegen and post it here for you to try before closing this
I am looking forward to seeing it. thanks 👍