def angle_error(a, b):
return cs.if_else(
a - b >= 0,
cs.fmin((a - b)**2.0, (a - (b + math.pi * 2.0))**2.0),
cs.fmin((a - b)**2.0, (a - (b - math.pi * 2.0))**2.0),
)
def angle_error(a, b):
return cs.fabs(
cs.if_else(
a - b >= 0,
cs.fmin((a - b), (-a + b + math.pi * 2.0)),
cs.fmin(-a + b, (a - b + math.pi * 2.0)),
))
def fmin(u, v):
if is_numeric(u) and is_numeric(v):
return np.fmin(u, v)
elif is_symbolic(u) or is_symbolic(v):
return cs.fmin(u, v)
else:
raise Exception("Illegal argument")
def min_cost_by_distance(xrefs: Sequence[XRef], point: XRef, gain: Gain):
x_ref_iter = iter(xrefs)
min_xref_t_cost = next(x_ref_iter).weighted_distance_to(point, gain)
for xref_t in x_ref_iter:
min_xref_t_cost = cs.fmin(
min_xref_t_cost,
xref_t.weighted_distance_to(point, gain),
)
return min_xref_t_cost
def build(self):
# Build parametric optimizer
# ------------------------------------
u = cs.SX.sym('u', self.config.nu * self.config.N_hor)
z0 = cs.SX.sym(
'z0', self.config.nz + self.config.N_hor +
self.config.Nobs * self.config.nobs +
self.config.Ndynobs * self.config.ndynobs * self.config.N_hor +
self.config.nx * self.config.N_hor
) #init + final position + cost, obstacle params, circle radius
# params, vel_ref each step, number obstacles x num params per obs, num dynamic obstacles X num param/obs X time steps, refernce path for each time step
(x, y, theta, vel_init, omega_init) = (z0[0], z0[1], z0[2], z0[3],
z0[4])
(xref, yref, thetaref, velref, omegaref) = (z0[5], z0[6], z0[7], z0[8],
z0[9])
(q, qv, qtheta, rv, rw, qN, qthetaN, qCTE, acc_penalty,
omega_acc_penalty) = (z0[10], z0[11], z0[12], z0[13], z0[14], z0[15],
z0[16], z0[17], z0[18], z0[19])
cost = 0
obstacle_constraints = 0
# Index where reference points start
base = self.config.nz + self.config.N_hor + self.config.Nobs * self.config.nobs + self.config.Ndynobs * self.config.ndynobs * self.config.N_hor
for t in range(0, self.config.N_hor): # LOOP OVER TIME STEPS
u_t = u[t * self.config.nu:(t + 1) * self.config.nu]
cost += rv * u_t[0]**2 + rw * u_t[1]**2 # Penalize control actions
cost += qv * (
u_t[0] - z0[self.config.nz + t]
)**2 # Cost for diff between velocity and reference velocity
cost += self.cost_fn((x, y, theta), (xref, yref, thetaref), q,
qtheta)
x += self.config.ts * (u_t[0] * cs.cos(theta))
y += self.config.ts * (u_t[0] * cs.sin(theta))
theta += self.config.ts * u_t[1]
xs_static = z0[self.config.nz + self.config.N_hor:self.config.nz +
self.config.N_hor + self.config.Nobs *
self.config.nobs:self.config.nobs]
ys_static = z0[self.config.nz + self.config.N_hor +
1:self.config.nz + self.config.N_hor +
self.config.Nobs *
self.config.nobs:self.config.nobs]
rs_static = z0[self.config.nz + self.config.N_hor +
2:self.config.nz + self.config.N_hor +
self.config.Nobs *
self.config.nobs:self.config.nobs]
# ordering is x,y,x_r, y_r, angle for obstacle 0 for N_hor timesteps, then x,y,x_r, y_r, angle for obstalce 1 for N_hor timesteps etc.
end_of_static_obs_idx = self.config.nz + self.config.N_hor + self.config.Nobs * self.config.nobs
end_of_dynamic_obs_idx = end_of_static_obs_idx + self.config.Ndynobs * self.config.ndynobs * self.config.N_hor
xs_dynamic = z0[end_of_static_obs_idx +
t * self.config.ndynobs:end_of_dynamic_obs_idx:self
.config.ndynobs * self.config.N_hor]
ys_dynamic = z0[end_of_static_obs_idx + t * self.config.ndynobs +
1:end_of_dynamic_obs_idx:self.config.ndynobs *
self.config.N_hor]
x_radius = z0[end_of_static_obs_idx + t * self.config.ndynobs +
2:end_of_dynamic_obs_idx:self.config.ndynobs *
self.config.N_hor]
y_radius = z0[end_of_static_obs_idx + t * self.config.ndynobs +
3:end_of_dynamic_obs_idx:self.config.ndynobs *
self.config.N_hor]
As = z0[end_of_static_obs_idx + t * self.config.ndynobs +
4:end_of_dynamic_obs_idx:self.config.ndynobs *
self.config.N_hor]
xdiff_static = x - xs_static
ydiff_static = y - ys_static
xdiff_dynamic = x - xs_dynamic
ydiff_dynamic = y - ys_dynamic
distance_inside_circle = rs_static**2 - xdiff_static**2 - ydiff_static**2
# Ellipse parameterized according to https://math.stackexchange.com/questions/426150/what-is-the-general-equation-of-the-ellipse-that-is-not-in-the-origin-and-rotate
# xs and ys are ellipse center points, xdiff is as before
# x_radius and y_radius are radii in "x" and "y" directions
# As are angles of ellipses (positive from x axis)
distance_inside_ellipse = 1 - (
xdiff_dynamic * cs.cos(As) + ydiff_dynamic * cs.sin(As))**2 / (
x_radius**2) - (xdiff_dynamic * cs.sin(As) - ydiff_dynamic
* cs.cos(As))**2 / (y_radius)**2
obstacle_constraints += cs.fmax(
0, cs.vertcat(distance_inside_circle, distance_inside_ellipse))
# our current point
p = cs.vertcat(x, y)
# Initialize list with CTE to all line segments
# https://math.stackexchange.com/questions/330269/the-distance-from-a-point-to-a-line-segment
distances = cs.SX.ones(1)
s2 = cs.vertcat(z0[base], z0[base + 1])
for i in range(1, self.config.N_hor):
# set start point as previous end point
s1 = s2
# new end point
s2 = cs.vertcat(z0[base + i * self.config.nx],
z0[base + i * self.config.nx + 1])
# line segment
s2s1 = s2 - s1
# t_hat
t_hat = cs.dot(p - s1,
s2s1) / (s2s1[0]**2 + s2s1[1]**2 + 1e-16)
# limit t
t_star = cs.fmin(cs.fmax(t_hat, 0.0), 1.0)
# vector pointing from us to closest point
temp_vec = s1 + t_star * s2s1 - p
# append distance (is actually squared distance)
distances = cs.horzcat(distances,
temp_vec[0]**2 + temp_vec[1]**2)
cost += cs.mmin(distances[1:]) * qCTE
cost += qN * ((x - xref)**2 +
(y - yref)**2) + qthetaN * (theta - thetaref)**2
# Max speeds
umin = [self.config.lin_vel_min, -self.config.ang_vel_max
] * self.config.N_hor
umax = [self.config.lin_vel_max, self.config.ang_vel_max
] * self.config.N_hor
bounds = og.constraints.Rectangle(umin, umax)
# Acceleration bounds and cost
# Selected velocities
v = u[0::2]
omega = u[1::2]
# Accelerations
acc = (v - cs.vertcat(vel_init, v[0:-1])) / self.config.ts
omega_acc = (omega -
cs.vertcat(omega_init, omega[0:-1])) / self.config.ts
acc_constraints = cs.vertcat(acc, omega_acc)
# Acceleration bounds
acc_min = [self.config.lin_acc_min] * self.config.N_hor
omega_min = [-self.config.ang_acc_max] * self.config.N_hor
acc_max = [self.config.lin_acc_max] * self.config.N_hor
omega_max = [self.config.ang_acc_max] * self.config.N_hor
acc_bounds = og.constraints.Rectangle(acc_min + omega_min,
acc_max + omega_max)
# Accelerations cost
cost += cs.mtimes(acc.T, acc) * acc_penalty
cost += cs.mtimes(omega_acc.T, omega_acc) * omega_acc_penalty
problem = og.builder.Problem(u, z0, cost).with_penalty_constraints(obstacle_constraints) \
.with_constraints(bounds) \
.with_aug_lagrangian_constraints(acc_constraints, acc_bounds)
build_config = og.config.BuildConfiguration()\
.with_build_directory(self.config.build_directory)\
.with_build_mode(self.config.build_type)\
.with_tcp_interface_config()
meta = og.config.OptimizerMeta()\
.with_optimizer_name(self.config.optimizer_name)
solver_config = og.config.SolverConfiguration()\
.with_tolerance(1e-4)\
.with_max_duration_micros(MAX_SOVLER_TIME)
builder = og.builder.OpEnOptimizerBuilder(problem,
meta,
build_config,
solver_config) \
.with_verbosity_level(1)
builder.build()
def angle_error(a, b):
return cs.fmin((a - b)**2.0, (a - (b + math.pi * 2.0))**2.0)
def step(self, u: "U", ts):
self.x += ts * self.speed * cs.cos(self.theta)
self.y += ts * self.speed * cs.sin(self.theta)
self.theta += ts * self.speed / self.LENGTH * u.yaw_rate
self.speed += ts * self.MAX_ACCEL * u.accel
self.speed = cs.fmin(cs.fmax(0, self.speed), self.MAX_SPEED)