Obstacle avoidance in ECC 18

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(c**2 - 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(c**2-cs.norm_2(z-zobs)**2, 0)**2

umin = [-3.0] * (nu*N)
umax = [3.0] * (nu*N)
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, ts*N, ts)
ux = u_star[0:nu*N: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)

alphaville / optimization-engine

Obstacle avoidance in ECC 18 #347