Obstacle avoidance in ECC 18

[Highlight123]

We try to replicate the obstacle avoidance of mobile robot with trailer in ECC 18 (Embedded nonlinear model predictive control for obstacle avoidance using PANOC) according to https://alphaville.github.io/optimization-engine/docs/example_navigation_py. Moreover, we cannot open https://kul-forbes.github.io/PANOC about ECC 18.
Can someone help a little? Looking forward to your reply~

• OS: Ubuntu18.04
• opengen 0.8.0
• rustc 1.77.2
• Python version : 3.7.16
  I have successfully ploted the (x, y)-trajectories of the vehicle. But, when obstacle avoidance constraints are added, codes fail to run.

Code:
import opengen as og
import casadi.casadi as cs
import matplotlib.pyplot as plt
import numpy as np
import math
import sys

Use Direct Interface

------------------------------------

sys.path.insert(1, './my_optimizers/navigation')
import navigation

Build parametric optimizer

N : Prediction horizon

(nu, nx, N, L, ts) = (2, 3, 60, 0.5, 0.1)

weight

(q, qtheta, r, qN, qthetaN) = (10, 0.1, 1, 200, 2)

u = cs.SX.sym('u', nu*N)

plan trajectories from any initial position and pose to any final position and pose

p = cs.SX.sym('p', 2*nx+1)
x = p[0]; y = p[1]; theta = p[2]
xref = p[3]; yref = p[4]; thetaref = p[5]
h_penalty = p[-1]

z_init = [-1.0, -2, 0]
z_ref = [1, 0.6, 0.05]

cost = 0

Obstacle (disc centered at zobs with radius c)

c = 1
zobs = np.array([0, 0])

for t in range(0, nu*N, nu):
z = np.array([x,y])
cost += q * cs.norm_2(z-np.array([1, 0.6])) + qtheta * (theta - thetaref)2
# print(cost)
cost += h_penalty * cs.fmax(c2 - cs.norm_2(z-zobs)**2, 0)**2
u_t = u[t:t+2]
theta_dot = (1/L) * (u_t[1] * cs.cos(theta) - u_t[0] * cs.sin(theta))
cost += r * cs.dot(u_t, u_t)
x += ts * (u_t[0] + L * cs.sin(theta) * theta_dot)
y += ts * (u_t[1] - L * cs.cos(theta) * theta_dot)
theta += ts * theta_dot

cost += qN*((x-xref)**2 + (y-yref)*2) + qthetaN(theta-thetaref)2
cost += h_penalty * cs.fmax(c2-cs.norm_2(z-zobs)**2, 0)**2

umin = [-3.0] * (nuN)
umax = [3.0] * (nuN)
bounds = og.constraints.Rectangle(umin, umax)

problem = og.builder.Problem(u, p, cost).with_constraints(bounds)
build_config = og.config.BuildConfiguration()
.with_build_directory("my_optimizers")
.with_build_mode("debug")
.with_tcp_interface_config()
.with_build_python_bindings()
meta = og.config.OptimizerMeta()
.with_optimizer_name("navigation")
solver_config = og.config.SolverConfiguration()
.with_tolerance(1e-4)
.with_initial_tolerance(1e-4)
.with_max_outer_iterations(5)
.with_delta_tolerance(1e-2)
builder = og.builder.OpEnOptimizerBuilder(problem,
meta,
build_config,
solver_config)
builder.build()

Use Direct Interface

solver = navigation.solver()
z_combined = z_init + z_ref
result = solver.run(p=z_combined,
initial_guess=[1.0] * (nu*N))

result = solver.run(p=z_combined)

u_star = result.solution

Plot solution

time = np.arange(0, tsN, ts)
ux = u_star[0:nuN:2]
uy = u_star[1:nu*N:2]

plt.subplot(211)
plt.plot(time, ux, '-o')
plt.ylabel('u_x')
plt.subplot(212)
plt.plot(time, uy, '-o')
plt.ylabel('u_y')
plt.xlabel('Time')
plt.show()

Error:
import sys
sys.path.insert(1, "./my_optimizers/navigation")
import navigation
solver = navigation.solver()

Process finished with exit code 139 (interrupted by signal 11: SIGSEGV)