def cone_constraint(c, x_obs, y_obs, theta_obs, v_obs, r_obs, uk, x, y, theta,
obs_dist, ego_dist, xkm1, ykm1, xref, yref):
v_obs_x = cs.cos(theta_obs) * v_obs
v_obs_y = cs.sin(theta_obs) * v_obs
rterm = (x - x_obs)**2 + (y - y_obs)**2
uterm = ((cs.cos(theta) * uk[0] - v_obs_x) * (x - x_obs) +
(cs.sin(theta) * uk[0] - v_obs_y) * (y - y_obs))
lterm = (cs.cos(theta) * uk[0] - v_obs_x)**2 + (cs.sin(theta) * uk[0] -
v_obs_y)**2
# -------distance const
# c += cs.fmax(0.0, r_obs - (x_obs-x)**2 - (y_obs-y)**2)
# -------regular cone
#c += cs.fmax(0.0, r_obs**2*lterm-(rterm*lterm-uterm**2))
# -------cone only when dist ego > dist obs
cone = r_obs**2 * lterm - (rterm * lterm - uterm**2)
passed_obs = cs.if_else((obs_dist - ego_dist) > 0, True, False)
obs_faster_than_ego = cs.if_else(uk[0] < v_obs, True, False)
obs_driving_towards = cs.if_else(
cs.norm_2(theta - theta_obs) >= (cs.pi / 2), True, False)
skip_due_todir_and_vel = cs.if_else(
obs_driving_towards, False, cs.if_else(obs_faster_than_ego, True,
False))
deactivate_activate_cone = cs.if_else(
passed_obs, True, cs.if_else(skip_due_todir_and_vel, True, False))
c += cs.fmax(0.0,
cs.if_else(deactivate_activate_cone, 0, cs.fmax(0.0, cone)))
# decide what side to drive
# side_obs =cs.sign((x_obs-xkm1)*(yref-ykm1)-(y_obs-ykm1)*(xref-xkm1)) # neg if on left
# s=cs.sign((x-xkm1)*(y_obs-ykm1)-(y-ykm1)*(x_obs-xkm1)) # neg if on left
# #c +=cs.fmax(0.0,s*5)
return c, deactivate_activate_cone
def cone_constraint(c, x_obs, y_obs,theta_obs, v_obs, r_cone, uk, x, y, theta, obs_dist,ego_dist,xkm1,ykm1,xref,yref):
# -------distance const
# c += cs.fmax(0.0, 1000*(r_cone -cs.sqrt((x_obs-x)**2 +(y_obs-y)**2)))
# ------- cone with activation and deactivation constraints
v_obs_x=cs.cos(theta_obs)*v_obs
v_obs_y=cs.sin(theta_obs)*v_obs
r_vec=cs.vertcat(x-x_obs,y-y_obs)
vab=cs.vertcat(cs.cos(theta)*uk[0]-v_obs_x,cs.sin(theta)*uk[0]-v_obs_y)
rterm = (cs.norm_2(r_vec))**2
lterm = (cs.norm_2(vab))**2
uterm = (cs.dot(r_vec,vab))**2
cone = (r_cone**2)*lterm-(rterm*lterm-uterm)
passed_obs=cs.if_else(obs_dist>ego_dist ,True,False)
obs_faster_than_ego=cs.if_else(uk[0]<cs.norm_2(v_obs) ,True,False)
obs_driving_towards=cs.if_else(passed_obs,False,cs.if_else(v_obs<=0,True,False))
skip_due_to_dir_and_vel=cs.if_else(obs_driving_towards,False,cs.if_else(obs_faster_than_ego,True,False))
skip_due_far_away=cs.if_else(cs.sqrt((x_obs-x)**2 - (y_obs-y)**2)<10,False,True)
skip=cs.logic_or(skip_due_to_dir_and_vel,skip_due_far_away)
deactivate_cone=cs.if_else(passed_obs,True,skip)
c += cs.if_else(deactivate_cone,0,cs.fmax(0.0, cone))
# decide what side to drive
#side_obs =cs.sign((x_obs-xkm1)*(yref-ykm1)-(y_obs-ykm1)*(xref-xkm1)) # neg if on left
# s=cs.sign((x-xkm1)*(y_obs-ykm1)-(y-ykm1)*(x_obs-xkm1)) # neg if on left
# c +=cs.if_else(deactivate_cone,cs.fmax(0.0,s*decide_side*10),0)
return c,False
def dynamic_model(state, control, forces, calc_casadi):
# State variables
x = state[0]
y = state[1]
phi = state[2]
v_x = state[3]
v_y = state[4]
omega = state[5]
# Control variables
delta = control[1]
# Tire forces
f_fy = forces[0]
f_rx = forces[1]
f_ry = forces[2]
x_next = v_x * cs.cos(phi) - v_y * cs.sin(phi)
y_next = v_x * cs.sin(phi) + v_y * cs.cos(phi)
phi_next = omega
v_x_next = 1 / p.m * (f_rx - f_fy * cs.sin(delta) + p.m * v_y * omega)
v_y_next = 1 / p.m * (f_ry + f_fy * cs.cos(delta) - p.m * v_x * omega)
omega_next = 1 / p.I_z * (f_fy * p.l_f * cs.cos(delta) - f_ry * p.l_r)
if calc_casadi:
return cs.vertcat(x_next, y_next, phi_next, v_x_next, v_y_next, omega_next)
else:
return x_next, y_next, phi_next, v_x_next, v_y_next, omega_next
def cone_constraint(c, x_obs, y_obs, theta_obs, v_obs, r_cone, v, x, y, theta,
passed_obs):
v_obs_x = cs.cos(theta_obs) * v_obs
v_obs_y = cs.sin(theta_obs) * v_obs
r_vec = cs.vertcat(x - x_obs, y - y_obs)
vab = cs.vertcat(cs.cos(theta) * v - v_obs_x, cs.sin(theta) * v - v_obs_y)
rterm = (cs.norm_2(r_vec))**2
lterm = (cs.norm_2(vab))**2
uterm = (cs.dot(r_vec, vab))**2
cone = (r_cone**2) * lterm - (rterm * lterm - uterm)
# c += cs.if_else(passed_obs,0,cs.fmax(0.0, cone))
c += cs.fmax(0.0, cone)
return c
def get_tang_v_ego(v, x, y, th):
vx = -v * cs.cos(th)
vy = -v * cs.sin(th)
b_hat_x = (x - center[0]) / cs.sqrt((x - center[0])**2 +
(y - center[1])**2)
b_hat_y = (y - center[1]) / cs.sqrt((x - center[0])**2 +
(y - center[1])**2)
vb = vx * b_hat_x + vy * b_hat_y
v_tan = (v - cs.sqrt(vb**2))
return v_tan
def pacejka_tire_forces(state, control):
# State variables
v_x = state[3]
v_y = state[4]
omega = state[5]
# Control variables
D = control[0]
delta = control[1]
# Force calculations
alpha_f = - cs.arctan2(p.l_f * omega + v_y, v_x) + delta
alpha_r = cs.arctan2(p.l_r * omega - v_y, v_x)
F_fy = p.D_f * cs.sin(p.C_f * cs.arctan2(p.B_f * alpha_f, 1))
F_ry = p.D_r * cs.sin(p.C_r * cs.arctan2(p.B_r * alpha_r, 1))
F_rx = (p.C_m1 - p.C_m2 * v_x) * D # - C_r0 - C_r2 * v_x ** 2
return [F_fy, F_rx, F_ry]
def model_dd_tilde(x, y, theta, v_tild, w_tild, xr, yr, thetar, vk):
x_tild = x - xr
y_tild = y - yr
theta_tild = theta - thetar
x_tild += -ts * np.sin(thetar) * vk * theta_tild + ts * cs.cos(thetar) * vk
y_tild += ts * np.cos(thetar) * vk * theta_tild + ts * cs.sin(thetar) * vk
theta_tild += ts * w_tild
x = x_tild + xr
y = y_tild + yr
theta = theta_tild + thetar
return x, y, theta, x_tild, y_tild, theta_tild
def weighted_distance_to(self, other: "XRef", gain: Gain):
theta_err = angle_error(self.theta, other.theta)
pos_errx = (other.x - self.x)**2
pos_erry = (other.y - self.y)**2
heading_vector = [cs.cos(self.theta), cs.sin(self.theta)]
lateral_error = (self.x - other.x)**2 + (self.y - other.y)**2
pos_err_theta = gain.position * lateral_error**1
return (
gain.position * pos_errx,
gain.position * pos_erry,
gain.theta * theta_err,
pos_err_theta,
)
def kinematic_model(state, control, calc_casadi):
# get states
x = state[0] # Longitudinal position
y = state[1] # Lateral Position
psi = state[2] # Heading Angle
v = state[3] # Velocity
# get inputs
a = control[0] # Acceleration
d = control[1] # Front steering angle
# compute slip angle
beta = cs.arctan2(p.l_r * cs.tan(d), (p.l_f + p.l_r))
# compute change in state
x_next = v * cs.cos(psi + beta)
y_next = v * cs.sin(psi + beta)
psi_next = v / p.l_r * cs.sin(beta)
v_next = a
if calc_casadi:
return cs.vertcat(x_next, y_next, psi_next, v_next)
else:
return x_next, y_next, psi_next, v_next
def test_integration_1(self):
u = cs.SX.sym('u', 3)
xi = cs.SX.sym('xi', 2)
p = cs.SX.sym('p', 2)
f = cs.sin(cs.norm_2(p)) * cs.dot(xi, p) * cs.norm_2(u)
fun = cs.Function('f', [u, xi, p], [f])
transpiler = cc.CasadiRustTranspiler(fun, 'xcst_kangaroo')
transpiler.transpile()
self.assertTrue(transpiler.compile())
(u, xi, p) = ([1, 2, 3], [4, 5], [6, 7])
expected = fun(u, xi, p)
result = transpiler.call_rust(u, xi, p)
self.assertAlmostEqual(expected, result[0], 8)
def cone_constraint(c, x_obs, y_obs, r_cone, uk, x, y, theta, passed_obs):
v_obs_x = 0
v_obs_y = 0
r_vec = cs.vertcat(x - x_obs, y - y_obs)
vab = cs.vertcat(
cs.cos(theta) * uk[0] - v_obs_x,
cs.sin(theta) * uk[0] - v_obs_y)
rterm = (cs.norm_2(r_vec))**2
lterm = (cs.norm_2(vab))**2
uterm = (cs.dot(r_vec, vab))**2
cone = (r_cone**2) * lterm - (rterm * lterm - uterm)
skip_due_far_away = cs.if_else(
cs.sqrt((x_obs - x)**2 - (y_obs - y)**2) < 10, False, True)
deactivate_cone = cs.if_else(passed_obs, True, skip_due_far_away)
c += cs.if_else(deactivate_cone, 0, cs.fmax(0.0, cone))
return c, False
def test_integration_2(self):
u = cs.SX.sym('u', 3)
xi = cs.SX.sym('xi', 2)
p = cs.SX.sym('p', 2)
f = cs.sin(cs.norm_2(p)) * cs.dot(xi, p)**2 * cs.norm_2(u)
f = 1/f
df = cs.gradient(f, u)
fun = cs.Function('df', [u, xi, p], [df])
transpiler = cc.CasadiRustTranspiler(fun, 'xcst_panda')
transpiler.transpile()
self.assertTrue(transpiler.compile())
(u, xi, p) = ([1, 2, 3], [4, 5], [6, 7])
expected = fun(u, xi, p)
result = transpiler.call_rust(u, xi, p)
for i in range(3):
self.assertAlmostEqual(expected[i], result[i], 8)
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
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 model_dd(x, y, theta, v, w):
x += ts * cs.cos(theta) * v
y += ts * cs.sin(theta) * v
theta += ts * w
return x, y, theta
####Obstacle(hoop)
for j in range(0,nMAV):
#c = cs.vertcat(c,cs.fmax(0, obs_data[3]**2-(x[ns*j]-obs_data[0])**2 - (x[ns*j+1]-obs_data[1])**2))
#c = cs.vertcat(c,cs.fmax(0, obs_data[3]**2-(x[ns*j]+7)**2 - (x[ns*j+1]-1.5)**2))
#c = cs.vertcat(c,cs.fmax(0, obs_data[3]**2-(x[ns*j]+7)**2 - (x[ns*j+1]+1.5)**2))
#c = cs.vertcat(c,cs.fmax(0, obs_data[3]**2-(x[ns*j]+11)**2 - (x[ns*j+1])**2))
c = cs.vertcat(c, cs.fmax(0, -0.005 + (u_n[nu*j+1] - u_old[nu*j+1])**2))
c = cs.vertcat(c, cs.fmax(0, -0.005 + (u_n[nu*j+2] - u_old[nu*j+2])**2))
#c += 0*cs.fmax(0,-(obs_data[3]**2 - ((x[1]-obs_data[1])**2 + (x[2]-obs_data[2])**2))) * cs.fmax(0, (x[0] - obs_data[0]) + 0.4) * cs.fmax(0, -(x[0] - obs_data[0]) + 0.4)
####State update MAV1
u_old = u_n
for j in range(0,nMAV):
x[ns*j] = x[ns*j] + dt * x[ns*j+3]
x[ns*j+1] = x[ns*j+1] + dt * x[ns*j+4]
x[ns*j+2] = x[ns*j+2] + dt * x[ns*j+5]
x[ns*j+3] = x[ns*j+3] + dt * (cs.sin(x[ns*j+7]) * cs.cos(x[ns*j+6]) * u_n[nu*j] - 0.1 * x[ns*j+3])
x[ns*j+4] = x[ns*j+4] + dt * (-cs.sin(x[ns*j+6]) * u_n[nu*j] - 0.1*x[ns*j+4])
x[ns*j+5] = x[ns*j+5] + dt * (cs.cos(x[ns*j+7]) * cs.cos(x[ns*j+6]) * u_n[nu*j] - 0.2 * x[ns*j+5] - 9.81)
x[ns*j+6] = x[ns*j+6] + dt * ((1 / 0.5) * (u_n[nu*j+1] - x[ns*j+6]))
x[ns*j+7] = x[ns*j+7] + dt * ((1 / 0.5) * (u_n[nu*j+2] - x[ns*j+7]))
#print(x[ns*j])
umin = [5, -0.4, -0.4] * (N*nMAV)
umax = [13.5, 0.4, 0.4] * (N*nMAV)
bounds = og.constraints.Rectangle(umin, umax)
problem = og.builder.Problem(u, z0, cost).with_penalty_constraints(c) \
.with_constraints(bounds)
build_config = og.config.BuildConfiguration() \
.with_build_directory("MAVsim") \
import casadi.casadi as cs
import opengen as og
u = cs.SX.sym("u", 5) # decision variable (nu = 5)
p = cs.SX.sym("p", 2) # parameter (np = 2)
phi = og.functions.rosenbrock(u, p) # cost function
f2 = cs.fmax(0.0, u[2] - u[3] + 0.1)
f1 = cs.vertcat(1.5 * u[0] - u[1], cs.sin(u[2] + cs.pi/5) - 0.2)
C = og.constraints.Ball2(None, 1.0)
UA = og.constraints.FiniteSet([[1, 2, 3], [1, 2, 2], [1, 2, 4], [0, 5, -1]])
UB = og.constraints.Ball2(None, 1.0)
U = og.constraints.CartesianProduct(5, [2, 4], [UA, UB])
problem = og.builder.Problem(u, p, phi) \
.with_constraints(U) \
.with_aug_lagrangian_constraints(f1, C)
meta = og.config.OptimizerMeta() \
.with_version("0.0.0") \
.with_authors(["P. Sopasakis", "E. Fresk"]) \
.with_licence("CC4.0-By") \
.with_optimizer_name("the_optimizer")
build_config = og.config.BuildConfiguration() \
.with_build_directory("my_optimizers") \
.with_build_mode("debug") \
.with_tcp_interface_config()
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)
def build_opt():
u = cs.SX.sym('u', nu * N)
p = cs.SX.sym('p', nx + nref + nObs + nu_init + ntraj)
(x, y, theta) = (p[0], p[1], p[2])
ukm1 = [p[3], p[4]]
(xref, yref, thetaref) = (p[5], p[6], p[7])
(x_obs, y_obs, theta_obs, v_obs, w_obs, r_obs) = (p[8], p[9], p[10], p[11],
p[12], p[13])
for i in range(14, 14 + len_traj):
xtraj.append(p[i])
ytraj.append(p[i + len_traj])
thetatraj.append(p[i + 2 * len_traj])
cost = 0
c = 0
# -------Collission constrint
k = 0
x_tild = 0
y_tild = 0
theta_tild = 0
for j, t in enumerate(range(0, nu * N, nu)):
uk = u[t:t + 2]
# -------Reference trajectory
cost += Qx * (x_tild)**2 + Qy * (y_tild)**2 + Qtheta * (theta_tild)**2
v_tild = uk[0] - v_ref
w_tild = uk[1] - w_ref
cost += Rv * v_tild**2 + Rw * w_tild**2
x, y, theta, x_tild, y_tild, theta_tild = model_dd_tilde(
x, y, theta, v_tild, w_tild, xtraj[j], ytraj[j], thetatraj[j],
uk[0])
(x_obs, y_obs, theta_obs) = model_dd(x_obs, y_obs, theta_obs, v_obs,
w_obs)
rterm = (x - x_obs)**2 + (y - y_obs)**2
uterm = ((cs.cos(theta) * uk[0] - v_obs) * (x - x_obs) +
(cs.sin(theta) * uk[0] - v_obs) * (y - y_obs))**2
lterm = (cs.cos(theta) * uk[0] - v_obs)**2 + (cs.sin(theta) * uk[0] -
v_obs)**2
# -------distance const
# c += cs.fmax(0.0, r_obs - (x_obs-x)**2 - (y_obs-y)**2)
# -------regular cone
#c += cs.fmax(0.0, r_obs**2*lterm-(rterm*lterm-uterm**2))
# -------cone only when dist ego > dist obs
cone = r_obs**2 * lterm - (rterm * lterm - uterm)
ego_dist = cs.sqrt((x - xref)**2 + (y - yref)**2)
obs_dist = cs.sqrt((x_obs - xref)**2 + (y_obs - yref)**2)
c += cs.fmax(0.0, -(obs_dist - ego_dist)) * (cs.fmax(0.0, cone))
# -------Acceleration constraint
(dv_c, dw_c) = (0, 0)
for t in range(0, nu * N, nu):
uk = u[t:t + 2]
dv_c += cs.fmax(0.0, uk[0] - ukm1[0] - dv)
dw_c += cs.vertcat(cs.fmax(0.0, uk[1] - ukm1[1] - dw),
cs.fmax(0.0, ukm1[1] - uk[1] - dw))
ukm1 = uk
# -------Boundery constrint
umin = []
umax = []
for i in range(N):
umin.extend([vmin, wmin])
umax.extend([vmax, wmax])
set_c = og.constraints.Zero()
set_y = og.constraints.BallInf(None, 1e12)
bounds = og.constraints.Rectangle(umin, umax)
C = cs.vertcat(dv_c, dw_c)
problem = og.builder.Problem(u, p, cost).with_penalty_constraints(
C).with_constraints(bounds).with_aug_lagrangian_constraints(
c, set_c, set_y)
build_config = og.config.BuildConfiguration()\
.with_build_directory("optimizers")\
.with_build_mode("debug")\
.with_rebuild(False)\
.with_tcp_interface_config()
meta = og.config.OptimizerMeta()\
.with_optimizer_name("ref_traj_tilde")
solver_config = og.config.SolverConfiguration()\
.with_tolerance(1e-5)\
.with_initial_tolerance(1e-5)\
.with_max_outer_iterations(10)\
.with_delta_tolerance(1e-2)\
.with_penalty_weight_update_factor(5).with_initial_penalty(100).with_sufficient_decrease_coefficient(0.7)
builder = og.builder.OpEnOptimizerBuilder(problem,
meta,
build_config,
solver_config) \
.with_verbosity_level(1)
builder.build()
#Obstacle(hoop)
#c += cs.fmax(0, 0.49-(x[0]+0.5)**2 - (x[1]-4)**2)
#c += cs.fmax(0, -(obs_data[3]**2 - ((x[1]-obs_data[1])**2 + (x[2] - obs_data[2])**2))) * cs.fmax(0, (x[0] - obs_data[0]) + 0.4) * cs.fmax(0, -(x[0] - obs_data[0]) + 0.4)
c = cs.vertcat(
c, 10 * cs.fmax(
0, -(obs_data[3]**2 - ((x[1] - obs_data[1])**2 +
(x[2] - obs_data[2])**2))) *
cs.fmax(0, (x[0] - obs_data[0]) + 0.4) *
cs.fmax(0, -(x[0] - obs_data[0]) + 0.4))
c = cs.vertcat(c, cs.fmax(0, -0.003 + (u_n[1] - u_old[1])**2))
c = cs.vertcat(c, cs.fmax(0, -0.003 + (u_n[2] - u_old[2])**2))
#######State update
x[0] = x[0] + dt * x[3]
x[1] = x[1] + dt * x[4]
x[2] = x[2] + dt * x[5]
x[3] = x[3] + dt * (cs.sin(x[7]) * cs.cos(x[6]) * u_n[0] - 0.1 * x[3])
x[4] = x[4] + dt * (-cs.sin(x[6]) * u_n[0] - 0.1 * x[4])
x[5] = x[5] + dt * (cs.cos(x[7]) * cs.cos(x[6]) * u_n[0] - 0.2 * x[5] -
9.81)
x[6] = x[6] + dt * ((1 / 0.5) * (u_n[1] - x[6]))
x[7] = x[7] + dt * ((1 / 0.5) * (u_n[2] - x[7]))
cost += 3 * Qx[0] * (x[0] - x_ref[0])**2 + Qx[1] * (x[1] - x_ref[1])**2 + Qx[
2] * (x[2] - x_ref[2])**2 + Qx[3] * (x[3] - x_ref[3])**2 + Qx[4] * (
x[4] - x_ref[4])**2 + Qx[5] * (x[5] - x_ref[5])**2 + Qx[6] * (
x[6] - x_ref[6])**2 + Qx[7] * (x[7] - x_ref[7])**2
umin = [5, -0.3, -0.3] * (nu * N)
#print(umin)
umax = [13.5, 0.3, 0.3] * (nu * N)
bounds = og.constraints.Rectangle(umin, umax)
problem = og.builder.Problem(
# Same as second example, but using F1 (ALM)
import casadi.casadi as cs
import opengen as og
import json
nu = 3
np = 1
u = cs.SX.sym("u", nu)
p = cs.SX.sym("p", np)
f = cs.dot(u, u)
for i in range(nu):
f += p * u[i]
F1 = cs.sin(u[0]) - 0.3
C = og.constraints.Zero()
U = og.constraints.Ball2(None, 0.5)
problem = og.builder.Problem(u, p, f) \
.with_constraints(U) \
.with_aug_lagrangian_constraints(F1, C)
meta = og.config.OptimizerMeta() \
.with_version("0.0.0") \
.with_authors(["Shane Trimble"]) \
.with_licence("CC4.0-By") \
.with_optimizer_name("shane")
build_config = og.config.BuildConfiguration() \
.with_build_directory("python_build") \
p_0 = x0[0:3] #; % position vector
v = x0[3:6] #; % velocity vector
roll = x0[6] #; % roll angle
pitch = x0[7] #; % pitch angle
T = u0[0] #; % mass normalized thrust
roll_g = u0[1] #; %desired angles
pitch_g = u0[2] #;
#R_x = np.matrix([[1, 0, 0],[ 0, np.cos(roll), -np.sin(roll)],[0, np.sin(roll), np.cos(roll)]])#; %Rotation around x (roll)
#R_y = np.matrix([[np.cos(pitch), 0, np.sin(pitch)],[ 0, 1, 0],[-np.sin(pitch), 0, np.cos(pitch)]])#; %Rotation around y (pitch)
#R = R_y*R_x#; % Rotation matrix without rotation around z since coordinates are fixed to the frame
#x_0 = p_0[0]
#y_0 = p_0[1]
#z_0 = p_0[2]
#v_dot = R*np.matrix([[0],[ 0],[T]]) + np.matrix([[0],[ 0],[-g]]) - np.matrix([[a_x, 0, 0],[ 0,a_y, 0],[ 0, 0, a_z]])*v
ddx = (cs.sin(pitch) * cs.cos(roll)) * T - a_x * x0[3] + f_e[0]
ddy = (-cs.sin(roll)) * T - a_y * x0[4] + f_e[1]
ddz = -g + (cs.cos(pitch) * cs.cos(roll)) * T - a_z * x0[5] + f_e[2]
roll_dot = (1 / tao_roll) * (k_roll * roll_g - roll)
pitch_dot = (1 / tao_pitch) * (k_pitch * pitch_g - pitch)
x0[0] += x0[3] * dt
x0[1] += x0[4] * dt
x0[2] += x0[5] * dt
x0[3] += ddx * dt
x0[4] += ddy * dt
x0[5] += ddz * dt
x0[6] += roll_dot * dt
x0[7] += pitch_dot * dt
