# OpEn Rust Examples: Obstacle Avoidance Constraint

In this example, we are going to test a little bit more complicated constraint that was used for **nonlinear-shaped obstacle avoidance** in robotics systems in the following papers [1][2].

- [1] [Small et al., 2018, "Aerial navigation in obstructed environments with embedded nonlinear model predictive control"](https://arxiv.org/abs/1812.04755)

- [2] [Sathya et al., 2019, "Embedded nonlinear model predictive control for obstacle avoidance using PANOC"](https://arxiv.org/abs/1904.10546)

Now, I am going to introduce the key idea in terms of how to mathematically formulate the obstacle avoidance constraint, then we are going to implement the constraint with a very simple robot model. 


## Obstacle Avoidance Constraint: A Breif Description 

### Basic form
Let's say there are multiple obstacles around a robot. We can consider the $j$-th obstacle as the intersection of a set of $m_j(t)$ nonlinear inequalities:

$$ O_j(t) = \{ p \in \mathbb{R}^{n_d} : h_j^i(p,t) > 0, \forall i \in \mathbb{N}_{[1,m_j(t)]} \} $$  
<div style="text-align: right"> [1, Eqn (11)] </div>


where 
- $n_d$: the number of space dimensions (normally 2 or 3)
- $h^i_j$: the $i$-th nonlinear function that is formed of the $j$-th obstacle shape
- $\mathbb{N}_{[1,m_j(t)]} = \{1,2,...,m_j(t)\}$: a natural number set from 1 to $m_j(t)$, which is the number of nonlinear functions for the obstacle.


Let's say $x(t)$ is the position of a mobile robot at time $t$. In order for the robot to avoid the obstacle, we should make sure the following constraint:

$$ x(t) \notin O_j(t) $$
<div style="text-align: right"> [1, Eqn (9e)] </div>



### Another form

Since the above constraint is complicated to be considered in an optimisation problem setting, now we are going to reformulate it. 

#### Reformulation

The constraint above is satisfied if and only if

$$ h^{i_0}_j( x(t) ,t) \le 0, \text{  for some } i_0 \in \mathbb{N}_{[1,m_j(t)]} $$
<div style="text-align: right"> [2, Eqn (8)] </div>

or equvalently

$$ [h^{i_0}_j(x(t),t)]^2_{+} := \max \{ 0, h^{i_0}_j(x(t),t) \}^2 = 0. $$

This constraint can be encoded as

$$ \psi_{O_j(t)} (x(t)) = 0$$
<div style="text-align: right"> [1, Eqn (12)] </div>

where

$$ \psi_{O_j(t)} (p) := \frac{1}{2} \prod_{i=1}^{m_j(t)} [h^{i}_j(p,t)]^2_{+}.$$
<div style="text-align: right"> [1, Eqn (13)] </div>

$\psi_{O_j(t)} (p)$ becomes $0$ if $p$ is outside $O_j(t)$; and increases in the interior of it as $p$ moves away from $O_j(t)$'s boundary. 


#### Gradient

Function $\psi_{O_j(t)} (p)$ is **differentiable** inside $O_j(t)$ with gradient


  \begin{equation}
    \nabla \psi_{O_j(t)} (p) = 
    \begin{cases}
      \sum_{i=1}^{m_j(t)} \{  \nabla h_j^i(p,t)  \cdot h^i_j(p,t) \cdot \prod_{k \neq i} ( h^{k}_j(p,t) ) ^2 \} , & \text{if}\ p \in O_j(t) \\
      0, & \text{otherwise}
    \end{cases}
  \end{equation}
<div style="text-align: right"> [2, Eqn (10)] </div>  
  

#### In a nutshell

As long as we can define an obstacle shape by using $h^i(p,t)$ functions, then we can formulate the corresponding obstacle avoidance constraint by using [1, Eqn (12)] and [2, Eqn (10)]. 


### Examples of Obstacle Avoidance Constraints

#### (1) Ellipsoids

An ellipsoid can be modelled by 

$$ O = \{ p \in \mathbb{R}^{n_d} : h(p) > 0 \} $$

and it has a single $h$ function

$$ h(p) = 1 - (p - c)^{\top} E (p - c) $$

where

- $c \in \mathbb{R}^{n_d}$: the centre of the ellipsoid
- $E$: a diagonal matrix in $\mathbb{R}^{n_d \times n_d}$ that controls the ellipsoid's size

Then, its reformulated constraint (i.e. [1, Eqn (12)]) becomes

$$ \psi_{O}(p) = \frac{1}{2}[ 1 - (p - c)^{\top} E (p - c)]^2_{+} = 0$$


## Rust Implementation

Now, we are going to implement optimisation problems with the constraints above, by using OpEn rust. Here, we use ALM/PM method (for details, see example [03](http://localhost:8888/notebooks/Jupyter/OpEn_Rust_example_ALMPM.ipynb) [04](https://github.com/inmo-jang/optimisation_tutorial/blob/master/tools_examples/OpEn/examples_rust/OpEn_Rust_example_04.ipynb) [05](https://github.com/inmo-jang/optimisation_tutorial/blob/master/tools_examples/OpEn/examples_rust/OpEn_Rust_example_05.ipynb)). 






#### OpEn Implementation

First, we need to import "optimization_engine"
- In your local PC, it should be also declared in "Cargo.toml".
- Instead, in this jupyter notebook, we need to have "extern crate" as follows. 



```Rust
extern crate optimization_engine;
use optimization_engine::{
    alm::*,
    constraints::*, panoc::*, *
};
```


### (1) A simple minimisation problem with an ellipsoid obstacle constraint

The optimisation problem that we are going to solve is:

Minimise $$f(\mathbf{u},\mathbf{p}) = p_1 \cdot (u_1 - p_2)^2 + p_3$$ 


subject to 

$$ \psi_{O}(\mathbf{u}) = \frac{1}{2}[ 1 - (\mathbf{u} - \mathbf{c})^{\top}(\mathbf{u} - \mathbf{c})]^2_{+} = 0$$


where 
- $\mathbf{p} = [p_1, p_2, p_3]^{\top}$: a parameter set for the objective function
- $\mathbf{u} = [u_1, f(\mathbf{u},\mathbf{p})]^{\top}$: $u_1$ is the actual decision variable; $f(\mathbf{u},\mathbf{p})$ is the objective function
- $\mathbf{c} = [c_1, c_2]^{\top}$: the centre for the ellipsoid constraint


For simplicity, the above can be rewritten as

Minimise $$f(\mathbf{u}) = u_2$$ 
<div style="text-align: right"> (P1) </div>

subject to 

$$ \psi_{O}(\mathbf{x}) =  [1 - (\mathbf{u} - \mathbf{c})^{\top}(\mathbf{u} - \mathbf{c})]_{+} = 0$$
<div style="text-align: right"> (P1C) </div> 

$$ u_2 = p_1 \cdot (u_1 - p_2)^2 + p_3$$
<div style="text-align: right"> (P2C) </div> 


Then, we need to define the objective function that we are going to optimise, and its gradient function. 




```Rust
pub fn f(u: &[f64], cost: &mut f64) -> Result<(), SolverError> {
    *cost = u[1];
    Ok(())
}
```

Its gradient is 

$$ \nabla f(u) = [0,1] $$


```Rust
pub fn df(u: &[f64], grad: &mut [f64]) -> Result<(), SolverError> {
    grad[0] = 0.0;
    grad[1] = 1.0;
    Ok(())
}
```

The two constraints (P1C and P2C) are rewritten as


$$F_1(u) = 
\begin{bmatrix}
 \max \{ 0, 1- (u_1 - c_1)^2 - (u_2 - c_2)^2 \}  \\
p_1 \cdot (u_1 - p_2)^2 + p_3 - u_2 
\end{bmatrix} = \{0\}$$



```Rust
pub fn _f1(u: &[f64], p: &[f64], centre: &[f64], f1u: &mut [f64]) {
    f1u[0] = (1.0 - (u[0]-centre[0]).powi(2) - (u[1]-centre[1]).powi(2) ).max(0.0);
    f1u[1] = p[0]*(u[0] - p[1]).powi(2) + p[2] - u[1];
}
```

We need to define its Jacobian product $JF_1^{\top} \cdot d$, as follows:

$$JF_1 = \begin{bmatrix}
\frac{\partial F_{1,1}}{\partial u_1} & \frac{\partial F_{1,1}}{\partial u_2} \\
\frac{\partial F_{1,2}}{\partial u_1} & \frac{\partial F_{1,2}}{\partial u_2}
\end{bmatrix}
=
 \begin{bmatrix}
-(u_1 - c_1) & -(u_2 - c_2)  \\
2 p_1 (u_1 - p_2) & -1 
\end{bmatrix}
$$

Hence, 
$$JF_1^{\top} \cdot d = \begin{bmatrix}
-(u_1 - c_1)d_1 + 2 p_1 (u_1 - p_2) d_2  \\
-(u_2 - c_2)d_1 - d_2
\end{bmatrix}$$


```Rust
pub fn _f1_jacobian_product(u: &[f64], d: &[f64], p: &[f64], centre: &[f64], res: &mut [f64]) {

    
    let test = (1.0 - (u[0]-centre[0]).powi(2) - (u[1]-centre[1])).powi(2);
    let f11u1 = 0.0;
    let f11u2 = 0.0;
    if  test > 0.0 { // Inside the obstacle
        let f11u1 = -(u[0] - centre[0]);
        let f11u2 = -(u[1] - centre[1]);
    } 
    
    res[0] = f11u1*d[0] + 2.0*p[0]*(u[0]-p[1])*d[1];
    res[1] = f11u2*d[0] - d[1];
}
```

Now, let's solve this problem:


```Rust
fn main(p: &[f64], centre: &[f64]) {
    let tolerance = 1e-5;
    let nx = 2; // problem_size: dimension of the decision variables
    let n1 = 2; // range dimensions of mappings F1
    let n2 = 0; // range dimensions of mappings F2
    let lbfgs_mem = 5; // memory of the LBFGS buffer
    
    // PANOCCache: All the information needed at every step of the algorithm
    let panoc_cache = PANOCCache::new(nx, tolerance, lbfgs_mem);
    
    // AlmCache: A cache structure that contains all the data 
    // that make up the state of the ALM/PM algorithm
    // (i.e., all those data that the algorithm updates)
    let mut alm_cache = AlmCache::new(panoc_cache, n1, n2);

    let set_c = Zero::new(); // Set C
    let bounds = Ball2::new(None, 100.0); // Set U
    let set_y = Ball2::new(None, 1e12);  // Set Y

    // ============= 
    // Re-define the functions linked to user parameters
    let f1 = |u: &[f64], f1u: &mut [f64]| -> Result<(), SolverError> {
        _f1(u, &p, &centre, f1u);
        Ok(())
    };    
    
    let f1_jacobian_product = |u: &[f64], d: &[f64], res: &mut [f64]| -> Result<(), SolverError> {
        _f1_jacobian_product(u, d, &p, &centre, res);
        Ok(())
    };      
    // ==============
    
    // AlmFactory: Prepare function psi and its gradient 
    // given the problem data such as f, del_f and 
    // optionally F_1, JF_1, C, F_2
    let factory = AlmFactory::new(
        f, // Cost function
        df, // Cost Gradient
        Some(f1), // MappingF1
        Some(f1_jacobian_product), // Jacobian Mapping F1 Trans
        NO_MAPPING, // MappingF2
        NO_JACOBIAN_MAPPING, // Jacobian Mapping F2 Trans
        Some(set_c), // Constraint set
        n2,
    );

    // Define an optimisation problem 
    // to be solved with AlmOptimizer
    let alm_problem = AlmProblem::new(
        bounds,
        Some(set_c),
        Some(set_y),
        |u: &[f64], xi: &[f64], cost: &mut f64| -> Result<(), SolverError> {
            factory.psi(u, xi, cost)
        },
        |u: &[f64], xi: &[f64], grad: &mut [f64]| -> Result<(), SolverError> {
            factory.d_psi(u, xi, grad)
        },
        Some(f1),
        NO_MAPPING,
        n1,
        n2,
    );

    let mut alm_optimizer = AlmOptimizer::new(&mut alm_cache, alm_problem)
        .with_delta_tolerance(1e-5)
        .with_max_outer_iterations(200)
        .with_epsilon_tolerance(1e-6)
        .with_initial_inner_tolerance(1e-2)
        .with_inner_tolerance_update_factor(0.5)
        .with_initial_penalty(100.0)
        .with_penalty_update_factor(1.05)
        .with_sufficient_decrease_coefficient(0.2)
        .with_initial_lagrange_multipliers(&vec![5.0; n1]);

    let mut u = vec![0.0; nx]; // Initial guess
    let solver_result = alm_optimizer.solve(&mut u);
    let r = solver_result.unwrap();
    println!("\n\nSolver result : {:#.7?}\n", r);
    println!("Solution u = {:#.6?}", u);
}
```

#### Result

##### Case (1) : $\mathbf{p} = [0.5, 0.0, 0.0]$; and $\mathbf{c} = [0.0, 0.0]$



```Rust
main(&[0.5, 0.0, 0.0], &[0.0, 0.0]);
```

    
    
    Solver result : AlmOptimizerStatus {
        exit_status: NotConvergedIterations,
        num_outer_iterations: 200,
        num_inner_iterations: 294357,
        last_problem_norm_fpr: 0.0000002,
        lagrange_multipliers: Some(
            [
                418903.3411792,
                -0.1598174,
            ],
        ),
        solve_time: 1.2080253s,
        penalty: 1422664.9032171,
        delta_y_norm: 11169.1870569,
        f2_norm: 0.0000000,
    }
    
    Solution u = [
        -0.906969,
        0.411296,
    ]


##### Discussion regarding `with_max_outer_iterations`
- The above result shows that $\sqrt{(-0.906969)^2 + (0.411296)^2} = 0.9957$, which should have been $1$. This violation of the constraint seems to be because of `num_outer_iterations`. See the result saying that `exit_status: NotConvergedIterations`. If we have `.with_max_outer_iterations(20000)` instead, the result becomes much closer to 1. 

```
Solver result : AlmOptimizerStatus {
    exit_status: Converged,
    num_outer_iterations: 15841,
    num_inner_iterations: 294598,
    last_problem_norm_fpr: 0.0000003,
    lagrange_multipliers: Some(
        [
            13042371154156472153855234973664144029470618410860165048517000607377595587216413717926002755063360427685147309074955380215484788703290607983858748435202048.0000000,
            -423753861818101173183033768863923337961929353698038541150171630273247468286124610675679897724729598403957822352154925420157726222643676129874240493060096.0000000,
        ],
    ),
    solve_time: 685.3194070ms,
    penalty: 1314173191634037641867321827952707564425574569942640742965913261261991610173351306165449104273636083623511981029636043416492478574566316102250934036316035219456.0000000,
    delta_y_norm: 13049253337190544769480745317718605727474957653716969434776105632101374675741179779251287164398950375716031261056815864055903190176215976986135912459010048.0000000,
    f2_norm: 0.0000000,
}

Solution u = [
    -0.910176,
    0.414210,
]
```

 



##### Case (2) : $\mathbf{p} = [0.5, 0.0, 0.0]$; and $\mathbf{c} = [-0.25, 0.0]$




```Rust
main(&[0.5, 0.0, 0.0], &[-0.25, 0.0]);
```

    
    
    Solver result : AlmOptimizerStatus {
        exit_status: NotConvergedIterations,
        num_outer_iterations: 200,
        num_inner_iterations: 135491,
        last_problem_norm_fpr: 0.0000005,
        lagrange_multipliers: Some(
            [
                1408361.2510441,
                -0.0600980,
            ],
        ),
        solve_time: 565.9462160ms,
        penalty: 1290399.0051856,
        delta_y_norm: 66981.4338340,
        f2_norm: 0.0000000,
    }
    
    Solution u = [
        -1.050587,
        0.551867,
    ]


#### Discussion regarding initial guess $u$

- Actually, the result should have positive $u_1$, which is not the case here. That is because of the initial guess being used. If we have `let mut u = vec![0.01; nx];`, then the result becomes

```
Solver result : AlmOptimizerStatus {
    exit_status: Converged,
    num_outer_iterations: 143,
    num_inner_iterations: 124212,
    last_problem_norm_fpr: 0.0000001,
    lagrange_multipliers: Some(
        [
            4945.6339502,
            3.6234874,
        ],
    ),
    solve_time: 265.9254640ms,
    penalty: 83969.8295572,
    delta_y_norm: 0.7404363,
    f2_norm: 0.0000000,
}

Solution u = [
    0.716492,
    0.256681,
]
```

- The result also shows that an appropriate initial guess can reduce the number of required iterations for convergence. 


##### Case (3) : $\mathbf{p} = [0.5, -0.5, 0.0]$; and $\mathbf{c} = [0.0, 0.0]$


```Rust
main(&[0.5, -0.5, 0.0], &[0.0, 0.0]);
```

    
    
    Solver result : AlmOptimizerStatus {
        exit_status: NotConvergedIterations,
        num_outer_iterations: 200,
        num_inner_iterations: 146713,
        last_problem_norm_fpr: 0.0000000,
        lagrange_multipliers: Some(
            [
                24603222.0202653,
                0.9999934,
            ],
        ),
        solve_time: 518.7304490ms,
        penalty: 1646912.4585867,
        delta_y_norm: 1173307.9920330,
        f2_norm: 0.0000000,
    }
    
    Solution u = [
        -0.501946,
        0.000002,
    ]


#### Discussion

- I used another initial guess `let mut u = vec![-10.0,100.0]`

- Also used more max_iteration `.with_max_outer_iterations(200000)`

```
Solver result : AlmOptimizerStatus {
    exit_status: Converged,
    num_outer_iterations: 4712,
    num_inner_iterations: 361,
    last_problem_norm_fpr: 0.0000001,
    lagrange_multipliers: Some(
        [
            5976883622830658845248063464346752524709298600838388224476889604094220605747149755684405781725184.0000000,
            -69101021379080966758879847606054738488281126479954440574070188930121413196685134303076548608000.0000000,
        ],
    ),
    solve_time: 5.1134950ms,
    penalty: 603127763126907906755649330948729811734730445314381263460459221876363493539180583510258581622531031040.0000000,
    delta_y_norm: 5977283061058509043015099253727812474755596146491414834183257557507118217200494501180480675643392.0000000,
    f2_norm: 0.0000000,
}

Solution u = [
    -0.992607,
    0.121331,
]
```

##### Case (4) : $\mathbf{p} = [0.1, 0.0, 0.0]$; and $\mathbf{c} = [0.5, 0.0]$


```Rust
main(&[0.1, 0.0, 0.0], &[0.5, 0.0]);
```

    
    
    Solver result : AlmOptimizerStatus {
        exit_status: NotConvergedIterations,
        num_outer_iterations: 200,
        num_inner_iterations: 6262,
        last_problem_norm_fpr: 0.0000003,
        lagrange_multipliers: Some(
            [
                24702201.4098780,
                1.1272551,
            ],
        ),
        solve_time: 26.2890600ms,
        penalty: 1646912.4585867,
        delta_y_norm: 1176366.0421865,
        f2_norm: 0.0000000,
    }
    
    Solution u = [
        0.000000,
        -0.000000,
    ]


- Re-solve with another initial guess `let mut u = vec![-10.0, 0.0];` along with `with_max_outer_iterations(200000)`

```
Solver result : AlmOptimizerStatus {
    exit_status: Converged,
    num_outer_iterations: 58212,
    num_inner_iterations: 505,
    last_problem_norm_fpr: 0.0000000,
    lagrange_multipliers: Some(
        [
            13404549262479835859589751960528458747089645770673533789043717319399913427339339649894823677542737890532912175920611259535236215980552009743086975979618304.0000000,
            -36087273675454447961635736381068040923436891998998803552880018912595707759007342697397663450575419910571250849302105127966573820973304429961460481785856.0000000,
        ],
    ),
    solve_time: 56.1390930ms,
    penalty: 1379881851215739514205223699612867219579361955336128074649839728497056344247153372644434875675550324928731008007705435712857942748898935359088556740355945398272.0000000,
    delta_y_norm: 13404597838859994525841441037492313287571256763071118491702576132599371299634447257190855189024608715345730234127782723897537779342065412648792381468966912.0000000,
    f2_norm: 0.0000000,
}

Solution u = [
    -0.499683,
    0.024968,
]
```


##### Case (5) : $\mathbf{p} = [-0.5, -0.5864, 0.0]$; and $\mathbf{c} = [-
3.0, 2.0]$


```Rust
main(&[0.5, -0.5864, 0.0], &[-3.0, 2.0]);
```

    
    
    Solver result : AlmOptimizerStatus {
        exit_status: Converged,
        num_outer_iterations: 15,
        num_inner_iterations: 17,
        last_problem_norm_fpr: 0.0000001,
        lagrange_multipliers: Some(
            [
                5.0000000,
                0.9999987,
            ],
        ),
        solve_time: 37.4400000µs,
        penalty: 171.0339358,
        delta_y_norm: 0.0000026,
        f2_norm: 0.0000000,
    }
    
    Solution u = [
        -0.586389,
        0.000000,
    ]


In this case, the circle constraint does not cover the minimum point of the cost function. Thus, the optimal value should be zero, where $u_1 = p_2$.




## Summary

- If your outcome's `exit_status` is not converged, then you should modify `with_max_outer_iterations` or **initial guess**. 

## Future work

- In this example, we had to manually define `df` and `f1_jacobian_product`. Later, let's use a general function that can obtain such gradient values. 



91b861c9ab696062d12a896f2f80fff270684555
ipynb
Jupyter Notebook
tools_examples/OpEn/examples_rust/OpEn_Rust_examples_obs_avoidance_simplified.ipynb
inmo-jang/optimisation
601af985d4a83f075dd8bd8760b16784515231b8
MIT
tools_examples/OpEn/examples_rust/OpEn_Rust_examples_obs_avoidance_simplified.ipynb
inmo-jang/optimisation
601af985d4a83f075dd8bd8760b16784515231b8
MIT
tools_examples/OpEn/examples_rust/OpEn_Rust_examples_obs_avoidance_simplified.ipynb
inmo-jang/optimisation
601af985d4a83f075dd8bd8760b16784515231b8
MIT
2020-04-27T23:28:18.000Z
2020-04-27T23:28:18.000Z
Qwen/Qwen-72B
1. YES
2. YES