コード例 #1
0
ファイル: __init__.py プロジェクト: fireofearth/carla-collect
    def make_local_params(self, frame):
        """Get the linearized bicycle model using the vehicle's
        immediate orientation."""
        params = util.AttrDict()
        params.frame = frame
        """Dynamics parameters"""
        p_0_x, p_0_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle,
                                                        flip_y=True)
        v_0_x, v_0_y, _ = carlautil.actor_to_velocity_ndarray(
            self.__ego_vehicle, flip_y=True)

        # initial_state - state at current frame in world/local coordinates
        #   Local coordinates has initial position and heading at 0
        initial_state = util.AttrDict(
            world=np.array([p_0_x, p_0_y, v_0_x, v_0_y]))
        params.initial_state = initial_state
        A, B = self.__params.A, self.__params.B
        nx, nu = self.__params.nx, self.__params.nu
        T = self.__control_horizon
        # make state computation account for initial position and velocity
        initial_rollout = np.concatenate([
            np.linalg.matrix_power(A, t) @ initial_state.world
            for t in range(T + 1)
        ])
        params.initial_rollout = util.AttrDict(world=initial_rollout)

        return params
コード例 #2
0
 def get_current_velocity(self):
     """
     Get current velocity of vehicle in
     """
     v_0_x, v_0_y, _ = carlautil.actor_to_velocity_ndarray(
         self.__ego_vehicle, flip_y=True)
     return np.sqrt(v_0_x**2 + v_0_y**2)
コード例 #3
0
    def make_local_params(self, frame, ovehicles):
        """Get Local LCSS parameters that are environment dependent."""
        params = util.AttrDict()
        """General local params."""
        params.frame = frame
        # O - number of obstacles
        params.O = len(ovehicles)
        # K - for each o=1,...,O K[o] is the number of outer approximations for vehicle o
        params.K = np.zeros(params.O, dtype=int)
        for idx, vehicle in enumerate(ovehicles):
            params.K[idx] = vehicle.n_states
        p_0_x, p_0_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle,
                                                        flip_y=True)
        v_0_x, v_0_y, _ = carlautil.actor_to_velocity_ndarray(
            self.__ego_vehicle, flip_y=True)
        # x0 : np.array
        #   Initial state
        x0 = np.array([p_0_x, p_0_y, v_0_x, v_0_y])
        params.x0 = x0
        A, T = self.__params.A, self.__params.T
        # States_free_init has shape (nx*(T+1))
        params.States_free_init = np.concatenate(
            [np.linalg.matrix_power(A, t) @ x0 for t in range(T + 1)])
        """Local params for (random) multiple coinciding controls."""
        # N_traj - number of planned trajectories possible to compute
        params.N_traj = np.prod(params.K)
        max_K = np.max(params.K)
        # TODO: figure out a way to specify number of subtrajectories
        params.N_select = min(int(1.5 * max_K), params.N_traj)
        # subtraj_indices - the indices of the sub-trajectory to optimize for
        params.subtraj_indices = np.random.choice(np.arange(params.N_traj),
                                                  size=params.N_select,
                                                  replace=False)

        return params
コード例 #4
0
ファイル: __init__.py プロジェクト: fireofearth/carla-collect
    def make_local_params(self, ovehicles):
        """Get Local LCSS parameters that are environment dependent."""
        params = util.AttrDict()
        params.O = len(ovehicles)  # number of obstacles
        params.K = np.zeros(params.O, dtype=int)
        for idx, vehicle in enumerate(ovehicles):
            params.K[idx] = vehicle.n_states

        p_0_x, p_0_y, _ = carlautil.actor_to_location_ndarray(
            self.__ego_vehicle)
        p_0_y = -p_0_y  # need to flip about x-axis
        v_0_x, v_0_y, _ = carlautil.actor_to_velocity_ndarray(
            self.__ego_vehicle)
        v_0_y = -v_0_y  # need to flip about x-axis
        # x0 : np.array
        #   Initial state
        x0 = np.array([p_0_x, p_0_y, v_0_x, v_0_y])
        A, T = self.__params.A, self.__params.T
        # States_free_init has shape (nx*(T+1))
        params.States_free_init = np.concatenate(
            [np.linalg.matrix_power(A, t) @ x0 for t in range(T + 1)])
        return params
コード例 #5
0
ファイル: __init__.py プロジェクト: fireofearth/carla-collect
    def make_local_params(self, ovehicles):
        """Get Local LCSS parameters that are environment dependent."""
        params = util.AttrDict()
        # O - number of obstacles
        params.O = len(ovehicles)
        # K - for each o=1,...,O K[o] is the number of outer approximations for vehicle o
        params.K = np.zeros(params.O, dtype=int)
        for idx, vehicle in enumerate(ovehicles):
            params.K[idx] = vehicle.n_states

        p_0_x, p_0_y, _ = carlautil.actor_to_location_ndarray(
            self.__ego_vehicle, flip_y=True)
        v_0_x, v_0_y, _ = carlautil.actor_to_velocity_ndarray(
            self.__ego_vehicle, flip_y=True)
        # x0 : np.array
        #   Initial state
        x0 = np.array([p_0_x, p_0_y, v_0_x, v_0_y])
        params.x0 = x0
        A, T = self.__params.A, self.__params.T
        # States_free_init has shape (nx*(T+1))
        params.States_free_init = np.concatenate(
            [np.linalg.matrix_power(A, t) @ x0 for t in range(T + 1)])
        return params
コード例 #6
0
ファイル: __init__.py プロジェクト: fireofearth/carla-collect
    def make_highlevel_params(self, ovehicles):
        p_0_x, p_0_y, _ = carlautil.actor_to_location_ndarray(
            self.__ego_vehicle)
        p_0_y = -p_0_y  # need to flip about x-axis
        v_0_x, v_0_y, _ = carlautil.actor_to_velocity_ndarray(
            self.__ego_vehicle)
        v_0_y = -v_0_y  # need to flip about x-axis
        """Get LCSS parameters"""
        # TODO: refactor this long chain
        params = util.AttrDict()
        params.Ts = 0.5
        # A, sys.A both have shape (4, 4)
        A = np.diag([1, 1], k=2)
        # B, sys.B both have shape (4, 2)
        B = np.concatenate((
            np.diag([0, 0]),
            np.diag([1, 1]),
        ))
        # C has shape (2, 4)
        C = np.concatenate((
            np.diag([1, 1]),
            np.diag([0, 0]),
        ), axis=1)
        # D has shape (2, 2)
        D = np.diag([0, 0])
        sys = control.matlab.c2d(control.matlab.ss(A, B, C, D), params.Ts)
        params.A = np.array(sys.A)
        params.B = np.array(sys.B)
        # number of state variables x, number of input variables u
        # nx = 4, nu = 2
        params.nx, params.nu = sys.B.shape
        # TODO: remove car, truck magic numbers
        # only truck.d is used
        params.car = util.AttrDict()
        params.car.d = np.array([4.5, 2.5])
        # truck should be renamed to car, and only truck.d is used
        params.truck = util.AttrDict()
        params.truck.d = [4.5, 2.5]
        # params.x0 : np.array
        #   Initial state
        params.x0 = np.array([p_0_x, p_0_y, v_0_x, v_0_y])
        params.diag = np.sqrt(params.car.d[1]**2 + params.car.d[0]**2) / 2.
        # TODO: remove constraint of v, u magic numbers
        # Vehicle dynamics restrictions
        params.vmin = np.array([0 / 3.6, -20 / 3.6])  # lower bounds
        params.vmax = np.array([80 / 3.6, 20 / 3.6])  # upper bounds
        params.umin = np.array([-10, -5])
        params.umax = np.array([3, 5])
        # Prediction parameters
        params.T = self.__prediction_horizon
        params.O = len(ovehicles)  # number of obstacles
        params.L = 4  # number of faces of obstacle sets
        params.K = np.zeros(params.O, dtype=int)
        for idx, vehicle in enumerate(ovehicles):
            params.K[idx] = vehicle.n_states
        """
        3rd and 4th states have COUPLED CONSTRAINTS
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %       x4 - c1*x3 <= - c3
        %       x4 - c1*x3 >= - c2
        %       x4 + c1*x3 <= c2
        %       x4 + c1*x3 >= c3
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        """
        vmin, vmax = params.vmin, params.vmax
        # they work only assuming vmax(2) = -vmin(2)
        params.c1 = vmax[1] / (0.5 * (vmax[0] - vmin[0]))
        params.c2 = params.c1 * vmax[0]
        params.c3 = params.c1 * vmin[0]
        """
        Inputs have COUPLED CONSTRAINTS
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %       u2 - t1*u1 <= - t3
        %       u2 - t1*u1 >= - t2
        %       u2 + t1*u1 <= t2
        %       u2 + t1*u1 >= t3
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        """
        umin, umax = params.umin, params.umax
        params.t1 = umax[1] / (0.5 * (umax[0] - umin[0]))
        params.t2 = params.t1 * umax[0]
        params.t3 = params.t1 * umin[0]

        A, B, T, nx, nu = params.A, params.B, params.T, params.nx, params.nu
        # C1 has shape (nx, T*nx)
        C1 = np.zeros((
            nx,
            T * nx,
        ))
        # C2 has shape (nx*(T - 1), nx*(T-1)) as A has shape (nx, nx)
        C2 = np.kron(np.eye(T - 1), A)
        # C3 has shape (nx*(T - 1), nx)
        C3 = np.zeros((
            (T - 1) * nx,
            nx,
        ))
        # C, Abar have shape (nx*T, nx*T)
        C = np.concatenate((
            C1,
            np.concatenate((
                C2,
                C3,
            ), axis=1),
        ), axis=0)
        Abar = np.eye(T * nx) - C
        # Bbar has shape (nx*T, nu*T) as B has shape (nx, nu)
        Bbar = np.kron(np.eye(T), B)
        # Gamma has shape (nx*(T + 1), nu*T) as Abar\Bbar has shape (nx*T, nu*T)
        Gamma = np.concatenate((
            np.zeros((
                nx,
                T * nu,
            )),
            np.linalg.solve(Abar, Bbar),
        ))
        params.Abar = Abar
        params.Bbar = Bbar
        params.Gamma = Gamma
        A, T, x0 = params.A, params.T, params.x0
        # States_free_init has shape (nx*(T+1))
        params.States_free_init = np.concatenate(
            [np.linalg.matrix_power(A, t) @ x0 for t in range(T + 1)])
        return params
コード例 #7
0
ファイル: __init__.py プロジェクト: fireofearth/carla-collect
def get_speed(vehicle):
    """Get velocity of vehicle in m/s."""
    v_0_x, v_0_y, _ = carlautil.actor_to_velocity_ndarray(vehicle, flip_y=True)
    return np.sqrt(v_0_x**2 + v_0_y**2)