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 lane_keeping(lane_cost, ymin, ymax, y, actv_lane, yref, ACTIVATE_CONE): passed_ymin = cs.if_else(y < ymin, 1, -1) passed_ymax = cs.if_else(y > ymax, 1, -1) lane_cost += cs.fmax(0.0, passed_ymin * 10000000) lane_cost += cs.fmax(0.0, passed_ymax * 10000000) lane_cost += cs.fmax(0.0, -1000 * cs.sqrt((ymin - y)**2) + (1000 / 2)) activate_lane_keep = cs.logic_and(ACTIVATE_CONE[0], ACTIVATE_CONE[1]) # lane_cost += cs.if_else(activate_lane_keep,(cs.sqrt((actv_lane-y)**2)**2)*Qy*0.5,0) lane_cost += cs.if_else(activate_lane_keep, (cs.sqrt( (yref - y)**2)**2) * Qy * 4, 0) return lane_cost
def lane_keeping(lane_cost, y,x,actv_lane,yref,ACTIVATE_CONE): global lane_border_min,lane_border_max,lane_offset_y,lane_offset_x,center_of_road,lane1,lane2 # border constraint radius_ego=cs.sqrt((x-lane_offset_x)**2+(y+(lane_border_min-lane_offset_y))**2) # Googla x^3 så ser du att den börjar öka vid ca 5-10. # multiplicera dist_ymin med 10 så börjar den vid 0.5-1 ist dist_ymin=(lane1-radius_ego)*12 dist_ymax=(radius_ego-lane2)*12 lane_cost+=cs.fmax(0.1,dist_ymin**3+10*dist_ymin) lane_cost+=cs.fmax(0.1,dist_ymax**3+10*dist_ymax) # active lane keeping constraint activate_lane_keep=cs.logic_and(ACTIVATE_CONE[0],ACTIVATE_CONE[1]) lane_cost += cs.if_else(activate_lane_keep,500*(radius_ego-actv_lane)**2,0*(radius_ego-center_of_road)**2) #TODO return lane_cost
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 setUpRosPackageGeneration(cls): u = cs.MX.sym("u", 5) # decision variable (nu = 5) p = cs.MX.sym("p", 2) # parameter (np = 2) phi = og.functions.rosenbrock(u, p) c = cs.vertcat(1.5 * u[0] - u[1], cs.fmax(0.0, u[2] - u[3] + 0.1)) bounds = og.constraints.Ball2(None, 1.5) meta = og.config.OptimizerMeta() \ .with_optimizer_name("rosenbrock_ros") problem = og.builder.Problem(u, p, phi) \ .with_constraints(bounds) \ .with_penalty_constraints(c) ros_config = og.config.RosConfiguration() \ .with_package_name("parametric_optimizer") \ .with_node_name("open_node") \ .with_rate(35) \ .with_description("really cool ROS node") build_config = og.config.BuildConfiguration() \ .with_open_version(RustBuildTestCase.OPEN_RUSTLIB_VERSION) \ .with_build_directory(RustBuildTestCase.TEST_DIR) \ .with_build_mode(og.config.BuildConfiguration.DEBUG_MODE) \ .with_build_c_bindings() \ .with_ros(ros_config) og.builder.OpEnOptimizerBuilder(problem, metadata=meta, build_configuration=build_config, solver_configuration=cls.solverConfig()) \ .build()
def fmax(u, v): if is_numeric(u) and is_numeric(v): return np.fmax(u, v) elif is_symbolic(u) or is_symbolic(v): return cs.fmax(u, v) else: raise Exception("Illegal argument")
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 distance_squared(self, u): """Computes the squared distance between a given point `u` and this second-order cone :param u: given point; can be a list of float, a numpy n-dim array (`ndarray`) or a CasADi SX/MX symbol :return: distance from set as a float or a CasADi symbol """ # Need to check this?: # if isinstance(u, cs.SX): # warnings.warn("This function does not accept casadi.SX; use casadi.MX instead") if fn.is_symbolic(u): nu = u.size(1) elif (isinstance(u, list) and all(isinstance(x, (int, float)) for x in u)) \ or isinstance(u, np.ndarray): nu = len(u) else: raise Exception("Illegal Argument, `u`") # Partition `u = (x, r)`, where `r` is the last element of `u` a = self.__a x = u[0:nu - 1] r = u[nu - 1] eps = 1e-16 norm_x = fn.norm2(x) # norm of x sq_norm_x = cs.dot(x, x) # squared norm of x gamma = (a * norm_x + r) / (a**2 + 1) fun1 = 0. fun2 = sq_norm_x + r**2 fun3 = sq_norm_x * (1. - gamma * a / (cs.fmax(eps, norm_x)))**2 + (r - gamma)**2 condition0 = norm_x + cs.fabs(r) < eps condition1 = norm_x <= a * r condition2 = a * norm_x <= -r f = cs.if_else( condition0, 0, cs.if_else(condition1, fun1, cs.if_else(condition2, fun2, fun3, True), True), True) return f
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 __construct_function_psi(self): """ Construct function psi and its gradient :return: cs.Function objects: psi_fun, grad_psi_fun """ self.__logger.info("Defining function psi(u, xi, p) and its gradient") problem = self.__problem bconfig = self.__build_config u = problem.decision_variables p = problem.parameter_variables n2 = problem.dim_constraints_penalty() n1 = problem.dim_constraints_aug_lagrangian() phi = problem.cost_function alm_set_c = problem.alm_set_c f1 = problem.penalty_mapping_f1 f2 = problem.penalty_mapping_f2 psi = phi if n1 + n2 > 0: n_xi = n1 + 1 else: n_xi = 0 xi = cs.SX.sym('xi', n_xi, 1) if isinstance(u, cs.SX) \ else cs.MX.sym('xi', n_xi, 1) # Note: In the first term below, we divide by 'max(c, 1)', instead of # just 'c'. The reason is that this allows to set c=0 and # retrieve the value of the original cost function if n1 > 0: sq_dist_term = alm_set_c.distance_squared(f1 + xi[1:n1 + 1] / cs.fmax(xi[0], 1)) psi += xi[0] * sq_dist_term / 2 if n2 > 0: psi += xi[0] * cs.dot(f2, f2) / 2 jac_psi = cs.gradient(psi, u) psi_fun = cs.Function(bconfig.cost_function_name, [u, xi, p], [psi]) grad_psi_fun = cs.Function(bconfig.grad_function_name, [u, xi, p], [jac_psi]) return psi_fun, grad_psi_fun
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()
def build_opt(): u = cs.SX.sym('u', nu * N) p = cs.SX.sym('p', nx + nref + nObs + nu_init + n_actv_lane) (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]], [p[14], p[15]], [p[16], p[17]], [p[18], p[19]]) actv_lane = p[20] stage_cost = lane_cost = jerk_cost = 0 c = 0 ACTIVATE_CONE = [] for t in range(0, nu * N, nu): uk = u[t:t + 2] stage_cost += Qx*(x-xref)**2 + Qy*(y-yref)**2 + \ Qtheta*(theta-thetaref)**2 stage_cost += Rv * uk[0]**2 + Rw * uk[1]**2 (xkm1, ykm1) = (x, y) (x, y, theta) = model_dd(x, y, theta, uk[0], uk[1]) for j in range(len(X_OBS)): # obstacles (X_OBS[j], Y_OBS[j], THETA_OBS[j]) = model_dd(X_OBS[j], Y_OBS[j], THETA_OBS[j], V_OBS[j], W_OBS[j]) ego_dist = cs.sqrt((x - xref)**2 + (y - yref)**2) obs_dist = cs.sqrt((X_OBS[j] - xref)**2 + (Y_OBS[j] - yref)**2) c, activate_cone = cone_constraint(c, X_OBS[j], Y_OBS[j], THETA_OBS[j], V_OBS[j], R_OBS[j], uk, x, y, theta, obs_dist, ego_dist, xkm1, ykm1, xref, yref) ACTIVATE_CONE.append(activate_cone) lane_cost = lane_keeping(lane_cost, ymin, ymax, y, actv_lane, yref, ACTIVATE_CONE) # passed_obs_by3M = cs.if_else(obs_dist-(ego_dist+3)>0,1,-1) # dist_actv_lane = cs.sqrt((actv_lane-y)**2) # lane_cost += 10*cs.fmax(0.0, passed_obs_by3M*(dist_actv_lane**2)*Qy*4) terminal_cost = Qtx * (x - xref)**2 + Qty * (y - yref)**2 + Qttheta * ( theta - thetaref)**2 # -------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, 100 * (uk[0] - ukm1[0] - dv)) dw_c += cs.vertcat(cs.fmax(0.0, 100 * (uk[1] - ukm1[1] - dw)), cs.fmax(0.0, 100 * (ukm1[1] - uk[1] - dw))) jerk_cost += Rvj * (uk[0] - ukm1[0])**2 + Rwj * (uk[1] - ukm1[1])**2 ukm1 = uk total_cost = terminal_cost + stage_cost + lane_cost + jerk_cost # -------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, total_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_point") update_pen = 2 init_pen = 100 num_out_pen = 4 solver_config = og.config.SolverConfiguration()\ .with_tolerance(1e-9)\ .with_initial_tolerance(1e-5)\ .with_max_outer_iterations(num_out_pen)\ .with_delta_tolerance(1e-2)\ .with_penalty_weight_update_factor(update_pen)\ .with_initial_penalty(init_pen).with_sufficient_decrease_coefficient(0.7)\ # .with_max_duration_micros(200*1000) builder = og.builder.OpEnOptimizerBuilder(problem, meta, build_config, solver_config) \ .with_verbosity_level(1) builder.build() return update_pen, init_pen, num_out_pen
cost += Ru[0] * (u_n[0] - u_ref[0])**2 + Ru[1] * ( u_n[1] - u_ref[1])**2 + Ru[2] * (u_n[2] - u_ref[2])**2 #Input weights cost += Rd[0] * (u_n[0] - u_old[0])**2 + Rd[1] * ( u_n[1] - u_old[1])**2 + Rd[2] * (u_n[2] - u_old[2])**2 #Input rate weights ####Input Cost MAV2 u_n2 = u[(6 * i + 3):(6 * i + 6)] cost += Ru[0] * (u_n2[0] - u_ref[0])**2 + Ru[1] * ( u_n2[1] - u_ref[1])**2 + Ru[2] * (u_n2[2] - u_ref[2])**2 #Input weights cost += Rd[0] * (u_n2[0] - u_old2[0])**2 + Rd[1] * ( u_n2[1] - u_old2[1])**2 + Rd[2] * (u_n2[2] - u_old2[2])**2 #Input rate weights ######Penalty constraint c = cs.vertcat(c, cs.fmax( 0, 0.49 - ((x[0] - x2[0])**2 + (x[1] - x2[1])**2))) #(x[2] - x2[2])**2)) 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)) c = cs.vertcat(c, cs.fmax(0, -0.003 + (u_n2[1] - u_old2[1])**2)) c = cs.vertcat(c, cs.fmax(0, -0.003 + (u_n2[2] - u_old2[2])**2)) u_old = u_n u_old2 = u_n2 ####Obstacle(hoop) #c += cs.fmax(0, 0.49-(x[0]+0.5)**2 - (x[1]-4)**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 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])
def build_opt(): u = cs.SX.sym('u', nu*N) p = cs.SX.sym('p', nx+nref+nObs+nu_init+n_actv_lane) (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, Y_LANE_OBS, R_CONE) = ([p[8], p[9]], [p[10], p[11]], [p[12], p[13]],[p[14], p[15]],[p[16], p[17]],[p[18], p[19]]) actv_lane = p[20] c = stage_cost = lane_cost = jerk_cost = 0 # close_to_obs=cs.if_else(cs.sqrt((X_OBS[0]-x)**2 - (Y_OBS[0]-y)**2)<10,True,False) # close_obs_cost += cs.if_else(close_to_obs,cs.fmax(0.0, 1000*(R_OBS[0] - cs.sqrt((X_OBS[0]-x_t)**2 + (Y_OBS[0]-y_t)**2))**2),0) # radius_ego=cs.sqrt((x_t-lane_offset_x)**2+(y_t+(lane_border_min-lane_offset_y))**2) # close_obs_cost += cs.if_else(close_to_obs,1000*(radius_ego-actv_lane)**2,0) for t in range(0, nu*N, nu): uk = u[t:t+2] stage_cost += Qx*(x-xref)**2 + Qy*(y-yref)**2 + Qtheta*(theta-thetaref)**2 stage_cost += Rv*uk[0]**2+Rw*uk[1]**2 (xkm1,ykm1)=(x,y) (x, y, theta) = model_dd(x, y, theta, uk[0], uk[1]) CONE_DEACTIVATED=[] W_OBS=[0,0] # TODO for j in range(len(X_OBS)): # obstacles handeling (X_OBS[j], Y_OBS[j],THETA_OBS[j],W_OBS[j])=obs_move_line(Y_LANE_OBS[j],V_OBS[j],X_OBS[j], Y_OBS[j],THETA_OBS[j],ts) ego_dist = cs.sqrt((x-xref)**2+(y-yref)**2) obs_dist = cs.sqrt((X_OBS[j]-xref)**2+(Y_OBS[j]-yref)**2) c ,cone_deactivated= cone_constraint(c, X_OBS[j], Y_OBS[j],THETA_OBS[j], V_OBS[j], R_CONE[j], uk, x, y, theta, obs_dist,ego_dist,xkm1,ykm1,xref,yref) CONE_DEACTIVATED.append(cone_deactivated) lane_cost = lane_keeping(lane_cost, y,x,actv_lane,yref,CONE_DEACTIVATED) terminal_cost = Qtx*(x-xref)**2 + Qty*(y-yref)**2 + Qttheta*(theta-thetaref)**2 # -------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, 100*(uk[0]-ukm1[0]-dv)) dw_c += cs.vertcat(cs.fmax(0.0, 100*(uk[1]-ukm1[1]-dw)), cs.fmax(0.0, 100*(ukm1[1]-uk[1]-dw))) jerk_cost+= Rvj*(uk[0]-ukm1[0])**2+Rwj*(uk[1]-ukm1[1])**2 ukm1 = uk total_cost = terminal_cost+stage_cost+lane_cost+jerk_cost # -------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) lag_const=cs.vertcat(dv_c, dw_c,c) problem = og.builder.Problem(u, p, total_cost).with_constraints(bounds).with_aug_lagrangian_constraints(lag_const, set_c, set_y) #release 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_point") update_pen = 2 init_pen = 100 num_out_pen = 5 solver_config = og.config.SolverConfiguration()\ .with_tolerance(1e-9)\ .with_initial_tolerance(1e-5)\ .with_max_outer_iterations(num_out_pen)\ .with_delta_tolerance(1e-2)\ .with_penalty_weight_update_factor(update_pen)\ .with_initial_penalty(init_pen).with_sufficient_decrease_coefficient(0.7)\ # .with_max_duration_micros(200*1000) builder = og.builder.OpEnOptimizerBuilder(problem, meta, build_config, solver_config) \ .with_verbosity_level(1) builder.build() return update_pen, init_pen, num_out_pen
u_n = u[(3 * i):3 * i + 3] cost += Ru[0] * (u_n[0] - u_ref[0])**2 + Ru[1] * ( u_n[1] - u_ref[1])**2 + Ru[2] * (u_n[2] - u_ref[2])**2 #Input weights cost += Rd[0] * (u_n[0] - u_old[0])**2 + Rd[1] * ( u_n[1] - u_old[1])**2 + Rd[2] * (u_n[2] - u_old[2])**2 #Input rate weights u_old = u_n #####PENALTY CONSTRAINTS #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]))
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()
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()
import casadi.casadi as cs import opengen as og import numpy as np x = cs.SX.sym("x", 1) # decision variable (num_x = 1) p = cs.SX.sym("p", 2) # parameter (num_p = 2) f = (x[0] - p[0])**2 # cost function f2 = cs.vertcat(cs.fmax(0.0, x[0] - p[1])) problem = og.builder.Problem(x, p, f) \ .with_penalty_constraints(f2) \ meta = og.config.OptimizerMeta() \ .with_version("0.0.0") \ .with_licence("CC4.0-By") \ .with_optimizer_name("example_02") build_config = og.config.BuildConfiguration() \ .with_build_directory("python_build") \ .with_build_mode("debug") \ .with_tcp_interface_config() solver_config = og.config.SolverConfiguration() \ .with_lfbgs_memory(15) \ .with_tolerance(1e-5) \ .with_max_inner_iterations(155) builder = og.builder.OpEnOptimizerBuilder(problem, metadata=meta,
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)
import opengen as og import casadi.casadi as cs u = cs.SX.sym("u", 5) p = cs.SX.sym("p", 2) phi = og.functions.rosenbrock(u, p) c = cs.vertcat(1.5 * u[0] - u[1], cs.fmax(0.0, u[2] - u[3] + 0.1)) bounds = og.constraints.Ball2(None, 1.5) problem = og.builder.Problem(u, p, phi) \ .with_penalty_constraints(c) \ .with_constraints(bounds) meta = og.config.OptimizerMeta() \ .with_optimizer_name("rosenbrock") \ .with_version('0.1.1') \ .with_licence('LGPLv3') ros_config = og.config.RosConfiguration() \ .with_package_name("parametric_optimizer") \ .with_node_name("open_node") \ .with_rate(35) \ .with_description("cool ROS node") build_config = og.config.BuildConfiguration() \ .with_build_directory("my_optimizers") \ .with_build_mode("debug") \ .with_tcp_interface_config() \ .with_build_c_bindings() \ .with_ros(ros_config) solver_config = og.config.SolverConfiguration() \ .with_tolerance(1e-5) \ .with_delta_tolerance(1e-4) \ .with_initial_penalty(890) \ .with_penalty_weight_update_factor(5)
for i in range(0, N): ###State Cost for j in range(0,nMAV): #print(j) cost += Qx[0]*(x[ns*j]-x_ref[ns*j])**2 + Qx[1]*(x[ns*j+1]-x_ref[ns*j+1])**2 + Qx[2]*(x[ns*j+2]-x_ref[ns*j+2])**2 + Qx[3]*(x[ns*j+3]-x_ref[ns*j+3])**2 + Qx[4]*(x[ns*j+4]-x_ref[ns*j+4])**2 + Qx[5]*(x[ns*j+5]-x_ref[ns*j+5])**2 + Qx[6]*(x[ns*j+6]-x_ref[ns*j+6])**2 + Qx[7]*(x[ns*j+7]-x_ref[ns*j+7])**2 ####Input Cost u_n = u[(i*nMAV*nu):(i*nMAV*nu+nu*nMAV)] for j in range(0,nMAV): #print(j) cost += Ru[0]*(u_n[nu*j] - u_ref[0])**2 + Ru[1]*(u_n[nu*j+1] - u_ref[1])**2 + Ru[2]*(u_n[nu*j+2] - u_ref[2])**2 #Input weights cost += Rd[0]*(u_n[nu*j] - u_old[nu*j])**2 + Rd[1]*(u_n[nu*j+1] - u_old[nu*j+1])**2 + Rd[2]*(u_n[nu*j+2] - u_old[nu*j+2])**2 #Input rate weights ######Penalty constraint if nMAV > 1: c = cs.vertcat(c, 10*cs.fmax(0, 0.4**2 - ((x[0] - x[16])**2 + (x[1] - x[17])**2))) c = cs.vertcat(c, 10*cs.fmax(0, 0.4**2 - ((x[0] - x[24])**2 + (x[1] - x[25])**2))) c = cs.vertcat(c, 10*cs.fmax(0, 0.4**2 - ((x[8] - x[24])**2 + (x[9] - x[25])**2))) #c = cs.vertcat(c, cs.fmax(0, 0.4**2 - ((x[0] - x[32])**2 + (x[1] - x[33])**2))) #c = cs.vertcat(c, cs.fmax(0, 0.4**2 - ((x[8] - x[32])**2 + (x[9] - x[33])**2))) #c = cs.vertcat(c, cs.fmax(0, 0.4**2 - ((x[16] - x[32])**2 + (x[17] - x[33])**2))) for j in range(0,nMAV-1): c = cs.vertcat(c, 10*cs.fmax(0, 0.4**2 - ((x[ns*j] - x[ns*j+8])**2 + (x[ns*j+1] - x[ns*j+9])**2))) #cost += 100*cs.fmax(0, 0.3**2 - ((x[0] - x[8])**2 + (x[1] - x[9])**2)) #cost += 100*cs.fmax(0, 0.3**2 - ((x[16] - x[24])**2 + (x[17] - x[25])**2)) ####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))
#obstacle avoidance from 2D lidar #cost+= Q_obstacle[0]*cs.fmax(d_safe-x_obst[0]+x0[3]*dt,0) #+x #cost+= Q_obstacle[1]*cs.fmax(0.0,d_safe-x_obst[1]-x0[3]*dt) #-x #cost+= Q_obstacle[2]*cs.fmax(d_safe-x_obst[2]+x0[4]*dt,0) #+y #cost+= Q_obstacle[3]*cs.fmax(d_safe-x_obst[3]-x0[4]*dt,0) #-y #print(x0[5]) if i < N - 1: cost += cs.sumsqr(Q_u * (u0 - u[(i + 1) * nu:(i + 1) * nu + nu])) # Constraints #c= cs.vertcat(c,cs.fmax(0.0,Q_obstacle[0]*(d_safe-x_obst[0]-(-x0[3])*dt))) #+x vx #c= cs.vertcat(c,cs.fmax(0.0,Q_obstacle[1]*(d_safe-(x_obst[1])-x0[3]*dt))) #-x -vx #c= cs.vertcat(c,cs.fmax(0.0,Q_obstacle[2]*(d_safe-x_obst[2]-x0[4]*dt))) #+y vy #c= cs.vertcat(c,cs.fmax(0.0,Q_obstacle[3]*(d_safe-(x_obst[3])-(-x0[4])*dt))) #-y -vy c = cs.vertcat( c, cs.fmax(0.0, Q_obstacle[0] * (d_safe - x_obst[0] - (-x0[1]) * dt))) #+x vx c = cs.vertcat(c, cs.fmax(0.0, Q_obstacle[1] * (d_safe - x_obst[1] - (x0[1] * dt)))) #-x -vx c = cs.vertcat( c, cs.fmax(0.0, Q_obstacle[2] * (d_safe - x_obst[2] - (-x0[2] * dt)))) #+y vy c = cs.vertcat(c, cs.fmax(0.0, Q_obstacle[3] * (d_safe - x_obst[3] - (x0[2]) * dt))) #-y -vy ###Cylinders c = cs.vertcat( c, Q_Obslist[0] * cs.fmax( 0, Q_Cylinders[2]**2 - (x0[0] - Q_Cylinders[0])**2 - (x0[1] - Q_Cylinders[1])**2))
def build_problem(N, SV_N, WP_N, ts): # Assumptions assert N >= 2, f"Must generate at least 2 trajectory points, got: {N}" assert SV_N >= 0, f"Must have non-negative # of sv's, got: {SV_N}" assert WP_N >= 1, f"Must have at lest 1 trajectory reference" z0_schema = [ (1, Gain), (1, VehicleModel), # Ego (SV_N, VehicleModel), # SV's (WP_N, XRef), # reference trajectory (1, Number), # impatience (1, Number), # target_speed ] z0_dimensions = sum(n * feature.DOF for n, feature in z0_schema) z0 = cs.SX.sym("z0", z0_dimensions) u_traj = UTrajectory(N) # parse z0 into features position = 0 parsed = [] for n, feature in z0_schema: feature_group = [] for i in range(n): feature_group.append( feature(*z0[position:position + feature.DOF].elements())) position += feature.DOF if n > 1: parsed.append(feature_group) else: assert len(feature_group) == 1 parsed += feature_group assert position == len(z0.elements()) assert position == z0_dimensions gain, ego, social_vehicles, xref_traj, impatience, target_speed = parsed cost = 0 for t in range(N): # Integrate the ego vehicle forward to the next trajectory point ego.step(u_traj[t], ts) # For the current pose, compute the smallest cost to any xref cost += min_cost_by_distance(xref_traj, ego.as_xref, gain) cost += gain.speed * (ego.speed - target_speed.value)**2 / t for sv in social_vehicles: # step the social vehicle assuming no change in velocity or heading sv.step(U(accel=0, yaw_rate=0), ts) min_dist = VehicleModel.LENGTH cost += gain.obstacle * cs.fmax( -1, min_dist**2 - ((ego.x - sv.x)**2 + 9 * (ego.y - sv.y)**2), ) # To stabilize the trajectory, we attach a higher weight to the final x_ref cost += gain.terminal * sum(xref_traj[-1].weighted_distance_to( ego.as_xref, gain)[:3]) cost += u_traj.integration_cost(gain) # force acceleration when we become increasingly impatient cost += gain.impatience * ( (u_traj[0].accel - 1.0) * impatience.value**2 * -(1.0)) # cost=0 bounds = og.constraints.Rectangle(xmin=[-1, -math.pi * 0.1] * N, xmax=[1, math.pi * 0.1] * N) return og.builder.Problem(u_traj.symbolic, z0, cost).with_constraints(bounds)
cost += Ru[0] * (u_n[0] - u_ref[0])**2 + Ru[1] * ( u_n[1] - u_ref[1])**2 + Ru[2] * (u_n[2] - u_ref[2])**2 #Input weights cost += Rd[0] * (u_n[0] - u_old[0])**2 + Rd[1] * ( u_n[1] - u_old[1])**2 + Rd[2] * (u_n[2] - u_old[2])**2 #Input rate weights ####Input Cost MAV2 u_n2 = u[(6 * i + 3):(6 * i + 6)] cost += Ru[0] * (u_n2[0] - u_ref[0])**2 + Ru[1] * ( u_n2[1] - u_ref[1])**2 + Ru[2] * (u_n2[2] - u_ref[2])**2 #Input weights cost += Rd[0] * (u_n2[0] - u_old2[0])**2 + Rd[1] * ( u_n2[1] - u_old2[1])**2 + Rd[2] * (u_n2[2] - u_old2[2])**2 #Input rate weights ######Penalty constraint #c = cs.vertcat(c, cs.fmax(0, 0.49 - ((x[0] - x2[0])**2 + (x[1] - x2[1])**2))) #(x[2] - x2[2])**2)) 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)) c = cs.vertcat(c, cs.fmax(0, -0.003 + (u_n2[1] - u_old2[1])**2)) c = cs.vertcat(c, cs.fmax(0, -0.003 + (u_n2[2] - u_old2[2])**2)) u_old = u_n u_old2 = u_n2 ####Obstacle(hoop) #c += cs.fmax(0, 0.49-(x[0]+0.5)**2 - (x[1]-4)**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 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] -