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)