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