## Actuator Control Allocation Example

When controlling physical systems like spacecraft or automobiles, the controller commands a desired control input $w \in \mathbb{R}^m$, which might be a wrench vector, containg forces and torques ($m=6$).

Usually, multiple atuators are available to produce this control input. The vector $u \in \mathbb{R}^n$ contains the single actuator input values in its components. If $n > m$ and the actuators are in general position, we call the system "over-actuated", since there are many realizations of $u$ that result in the same value of $w$ via the linear mapping $A$. 

Having this freedom of choice, we want to minimize energy consumption, modeled as $\kappa^T | u |$ (with $\kappa \geq 0$), while discouraging rapid changes of the actuation values, i.e., $\lambda^\mathrm{sm} \Vert u-u^\mathrm{prev} \Vert_2^2$ with $\lambda^\mathrm{sm} \geq 0$ and $u^\mathrm{prev}$ being the actuation of the previous time step. Given the bounds $u^\mathrm{min}$ and $u^\mathrm{max}$ $(u^\mathrm{min} \leq u^\mathrm{max})$ on $u$, there might be cases when the desired control input $w$ is infeasible. Hence, we only softly penalize deviations between desired and actual control input with the cost term $\Vert A u - w \Vert_2^2$. We solve the optimization problem

\begin{equation}
\begin{array}{ll}
\text{minimize} \quad &\Vert A u - w \Vert_2^2  + \lambda^\mathrm{sm} \Vert u-u^\mathrm{prev} \Vert_2^2 + \kappa^T | u |\\
\text{subject to} \quad &u^\mathrm{min} \leq u \leq u^\mathrm{max},
\end{array}
\end{equation}

with variable $u \in \mathbb{R}^n$. To make the problem [DPP-compliant](https://www.cvxpy.org/tutorial/advanced/index.html#disciplined-parametrized-programming), we introduce the additional variable $\Delta u = u - u^\mathrm{prev}$ and solve

\begin{equation}
\begin{array}{ll}
\text{minimize} \quad &\Vert A u - w \Vert_2^2  + \lambda^\mathrm{sm} \Vert \Delta u \Vert_2^2 + \kappa^T | u |\\
\text{subject to} \quad &u^\mathrm{min} \leq u \leq u^\mathrm{max} \\
&\Delta u = u-u^\mathrm{prev}.
\end{array}
\end{equation}

Let's define the corresponding CVXPY problem.


```python
import cvxpy as cp

# define dimensions
n, m = 8, 3

# define variables
u = cp.Variable(n, name='u')
delta_u = cp.Variable(n, name='delta_u')

# define parameters
A = cp.Parameter((m, n), name='A')
w = cp.Parameter(m, name='w')
lamb_sm = cp.Parameter(nonneg=True, name='lamb_sm')
kappa = cp.Parameter(n, nonneg=True, name='kappa')
u_prev = cp.Parameter(n, name='u_prev')
u_min = cp.Parameter(n, name='u_min')
u_max = cp.Parameter(n, name='u_max')

# define objective
objective = cp.Minimize(cp.sum_squares(A@u-w) + lamb_sm*cp.sum_squares(delta_u) + kappa@cp.abs(u))

# define constraints
constraints = [u_min <= u, u <= u_max, delta_u == u-u_prev]

# define problem
problem = cp.Problem(objective, constraints)
```

Assign parameter values and solve the problem. In this case, the wrench vector $w = \left[f^T t\right]^T$ consists of force $f \in \mathbb{R}^2$ and torque $t \in \mathbb{R}$ in two-dimensional space. The control input $u \in \mathbb{R}^8$ contains four pairs of horizontal and vertical forces at 4 positions in the plane.


```python
import numpy as np

A.value = np.array([[1, 0, 1, 0, 1, 0, 1, 0],
                    [0, 1, 0, 1, 0, 1, 0, 1],
                    [1, -1, 1, 1, -1, 1, -1, -1]])

w.value = np.array([1, 1, 1])
lamb_sm.value = 0.5
kappa.value = 0.1*np.ones(8)
u_prev.value = np.zeros(8)
u_min.value = -np.ones(8)
u_max.value = np.ones(8)

val = problem.solve()
```

Generating C source for the problem is as easy as:


```python
from cvxpygen import cpg

cpg.generate_code(problem, code_dir='actuator_code')
```

Now, you can use a python wrapper around the generated code as a custom CVXPY solve method.


```python
from actuator_code.cpg_solver import cpg_solve
import numpy as np
import pickle
import time

# load the serialized problem formulation
with open('actuator_code/problem.pickle', 'rb') as f:
    prob = pickle.load(f)

# assign parameter values
prob.param_dict['A'].value = np.array([[1, 0, 1, 0, 1, 0, 1, 0],
                                       [0, 1, 0, 1, 0, 1, 0, 1],
                                       [1, -1, 1, 1, -1, 1, -1, -1]])

prob.param_dict['w'].value = np.array([1, 1, 1])
prob.param_dict['lamb_sm'].value = 0.5
prob.param_dict['kappa'].value = 0.1*np.ones(8)
prob.param_dict['u_prev'].value = np.zeros(8)
prob.param_dict['u_min'].value = -np.ones(8)
prob.param_dict['u_max'].value = np.ones(8)

# solve problem conventionally
t0 = time.time()
# CVXPY chooses eps_abs=eps_rel=1e-5, max_iter=10000, polish=True by default,
# however, we choose the OSQP default values here, as they are used for code generation as well
val = prob.solve(eps_abs=1e-3, eps_rel=1e-3, max_iter=4000, polish=False)
t1 = time.time()
print('\nCVXPY\nSolve time: %.3f ms' % (1000 * (t1 - t0)))
print('Objective function value: %.6f\n' % val)

# solve problem with C code via python wrapper
prob.register_solve('CPG', cpg_solve)
t0 = time.time()
val = prob.solve(method='CPG')
t1 = time.time()
print('\nCVXPYgen\nSolve time: %.3f ms' % (1000 * (t1 - t0)))
print('Objective function value: %.6f\n' % val)
```

Optimization results look as follows:


```python
from visualization.actuator import create_animation
from IPython.display import Image
    
create_animation(prob, 'actuator_animation')

with open('actuator_animation.gif', 'rb') as f:
    display(Image(f.read()))
```

a9debd7942d4f1c1994e0476581459524a23247d
ipynb
Jupyter Notebook
examples/actuator.ipynb
cvxgrp/cvxpygen
5aaacdb894354288e2f12a16ca738248c69181c4
Apache-2.0
2022-02-25T03:30:06.000Z
2022-03-30T16:24:40.000Z
examples/actuator.ipynb
cvxgrp/cvxpygen
5aaacdb894354288e2f12a16ca738248c69181c4
Apache-2.0
examples/actuator.ipynb
cvxgrp/cvxpygen
5aaacdb894354288e2f12a16ca738248c69181c4
Apache-2.0
Qwen/Qwen-72B
1. YES
2. YES