Skip to content

Model Predictive Control

commonroad_control.control.model_predictive_control.model_predictive_control

ModelPredictiveControl

Bases: ControllerInterface

Model predictive controller (MPC). This class mostly serves as a wrapper for a given optimal control problem (OCP) solver and provides methods for computing an initial guess, e.g., by shifting the solution from the previous time step or linear interpolation between the initial state and a desired final state.

Source code in commonroad_control/control/model_predictive_control/model_predictive_control.py 
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
class ModelPredictiveControl(ControllerInterface):
    """
    Model predictive controller (MPC). This class mostly serves as a wrapper for a given optimal control problem (OCP)
    solver and provides methods for computing an initial guess, e.g., by shifting the solution from the previous time
    step or linear interpolation between the initial state and a desired final state.
    """

    def __init__(self, ocp_solver: OptimalControlSolverInterface):
        """
        Initialize controller.
        :param ocp_solver: OCP solver - OptimalControlSolver
        """

        # init base class
        super().__init__()

        # set optimal control problem solver
        self.ocp_solver = ocp_solver

        # store initial guess
        self._x_init = None
        self._u_init = None

    def compute_control_input(
        self,
        x0: StateInterface,
        x_ref: TrajectoryInterface,
        u_ref: TrajectoryInterface,
        x_init: TrajectoryInterface = None,
        u_init: TrajectoryInterface = None,
    ) -> InputInterface:
        """
        Computes the control input by solving an optimal control problem.
        :param x0: initial state of the vehicle - StateInterface
        :param x_ref: state reference trajectory - TrajectoryInterface
        :param u_ref: input reference trajectory - TrajectoryInterface
        :param x_init: (optional) initial guess for the state trajectory -TrajectoryInterface
        :param u_init: (optional) initial state for the input trajectory - TrajectoryInterface
        :return: optimal control input - InputInterface
        """

        # set initial guess for optimal control: if no initial guess is provided (x_init = None and or u_init = None),
        # an initial guess is computed by shifting an old solution or linearly interpolating the state between the
        # initial state and the final reference state
        if x_init is None or u_init is None:
            if self._x_init is None and self._u_init is None:
                x_init, u_init = self._initial_guess_linear_interpolation(x0=x0, xf=x_ref.final_point, t_0=x_ref.t_0)
            else:
                x_init, u_init = self._initial_guess_shift_solution(u_ref)

        # solve optimal control problem
        x_opt, u_opt, _ = self.ocp_solver.solve(x0, x_ref, u_ref, x_init, u_init)

        # store solution as initial guess at next time step
        self._x_init = x_opt
        self._u_init = u_opt

        return u_opt.get_point_at_time_step(0)

    def _initial_guess_linear_interpolation(
        self, x0: StateInterface, xf: StateInterface, t_0: float
    ) -> Tuple[TrajectoryInterface, TrajectoryInterface]:
        """
        Computes an initial guess by linearly interpolating between the initial state x0 and the final reference state
        xf. The control inputs are set to zero.
        :param x0: initial state - StateInterface
        :param xf: desired final state - StateInterface
        :return: initial guess for the state and control inputs - TrajectoryInterface, TrajectoryInterface
        """

        # ... state: linear interpolation between x0 and xf
        x_init_interp_fun = sp.interpolate.interp1d(
            [0.0, 1.0],
            np.hstack(
                (
                    np.reshape(
                        x0.convert_to_array(),
                        (self.ocp_solver.vehicle_model.state_dimension, 1),
                    ),
                    np.reshape(
                        xf.convert_to_array().transpose(),
                        (self.ocp_solver.vehicle_model.state_dimension, 1),
                    ),
                )
            ),
            kind="linear",
        )
        x_init_np = np.zeros((self.ocp_solver.vehicle_model.state_dimension, self.ocp_solver.horizon + 1))
        time_state = [kk for kk in range(self.ocp_solver.horizon + 1)]
        for kk in range(self.ocp_solver.horizon + 1):
            x_init_np[:, kk] = x_init_interp_fun(kk / self.ocp_solver.horizon)
        x_init = self.ocp_solver.sidt_factory.trajectory_from_numpy_array(
            traj_np=x_init_np,
            mode=TrajectoryMode.State,
            time_steps=time_state,
            t_0=t_0,
            delta_t=self.ocp_solver.delta_t,
        )
        # ... control inputs: set to zero
        u_init_np = np.zeros((self.ocp_solver.vehicle_model.input_dimension, self.ocp_solver.horizon))
        time_input = time_state
        time_input.pop()
        u_init = self.ocp_solver.sidt_factory.trajectory_from_numpy_array(
            traj_np=u_init_np,
            mode=TrajectoryMode.Input,
            time_steps=time_input,
            t_0=t_0,
            delta_t=self.ocp_solver.delta_t,
        )

        return x_init, u_init

    def _initial_guess_shift_solution(
        self, u_ref: TrajectoryInterface
    ) -> Tuple[TrajectoryInterface, TrajectoryInterface]:
        """
        Computes an initial guess by shifting the solution from the previous time step and appending the last control
        input from the reference trajectory. The initial guess for the state at the end of the prediction horizon is
        obtained by simulating the system using the reference input.
        :param u_ref: input reference trajectory - TrajectoryInterface
        :return: initial guess for the state and control inputs
        """

        # extract states and input to be kept
        x_init_points = [self._x_init.get_point_at_time_step(kk) for kk in range(1, self.ocp_solver.horizon + 1)]
        u_init_points = [self._u_init.get_point_at_time_step(kk) for kk in range(1, self.ocp_solver.horizon)]

        # simulate vehicle model for num_steps using the reference control inputs
        # ... append last input from reference trajectory
        u_init_points.append(u_ref.final_point)
        # ... simulate
        x_init_points.append(
            self.ocp_solver.sidt_factory.state_from_numpy_array(
                self.ocp_solver.vehicle_model.simulate_dt_nom(x_init_points[-1], u_init_points[-1])
            )
        )

        # convert to trajectory interface
        # ... state trajectory
        time_steps = [kk for kk in range(0, self.ocp_solver.horizon + 1)]
        x_init = self.ocp_solver.sidt_factory.trajectory_from_points(
            trajectory_dict=dict(zip(time_steps, x_init_points)),
            mode=TrajectoryMode.State,
            t_0=u_ref.t_0,
            delta_t=self.ocp_solver.delta_t,
        )
        # ... input trajectory
        time_steps = [kk for kk in range(0, self.ocp_solver.horizon)]
        u_init = self.ocp_solver.sidt_factory.trajectory_from_points(
            trajectory_dict=dict(zip(time_steps, u_init_points)),
            mode=TrajectoryMode.Input,
            t_0=u_ref.t_0,
            delta_t=self.ocp_solver.delta_t,
        )

        return x_init, u_init

    def clear_initial_guess(self):
        """
        Delete stored initial guess.
        """
        self._x_init = None
        self._u_init = None

    @property
    def horizon(self) -> int:
        """
        :return: discrete prediction horizon of the OCP
        """
        return self.ocp_solver.horizon
horizon property

Returns:

Type Description
int

discrete prediction horizon of the OCP
__init__(ocp_solver)

Initialize controller.

Parameters:

Name Type Description Default
ocp_solver OptimalControlSolverInterface

OCP solver - OptimalControlSolver

 required
Source code in commonroad_control/control/model_predictive_control/model_predictive_control.py 
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
def __init__(self, ocp_solver: OptimalControlSolverInterface):
    """
    Initialize controller.
    :param ocp_solver: OCP solver - OptimalControlSolver
    """

    # init base class
    super().__init__()

    # set optimal control problem solver
    self.ocp_solver = ocp_solver

    # store initial guess
    self._x_init = None
    self._u_init = None
_initial_guess_linear_interpolation(x0, xf, t_0)

Computes an initial guess by linearly interpolating between the initial state x0 and the final reference state xf. The control inputs are set to zero.

Parameters:

Name Type Description Default
x0 StateInterface

initial state - StateInterface

 required
xf StateInterface

desired final state - StateInterface

 required

Returns:

Type Description
Tuple[TrajectoryInterface, TrajectoryInterface]

initial guess for the state and control inputs - TrajectoryInterface, TrajectoryInterface
Source code in commonroad_control/control/model_predictive_control/model_predictive_control.py 
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
def _initial_guess_linear_interpolation(
    self, x0: StateInterface, xf: StateInterface, t_0: float
) -> Tuple[TrajectoryInterface, TrajectoryInterface]:
    """
    Computes an initial guess by linearly interpolating between the initial state x0 and the final reference state
    xf. The control inputs are set to zero.
    :param x0: initial state - StateInterface
    :param xf: desired final state - StateInterface
    :return: initial guess for the state and control inputs - TrajectoryInterface, TrajectoryInterface
    """

    # ... state: linear interpolation between x0 and xf
    x_init_interp_fun = sp.interpolate.interp1d(
        [0.0, 1.0],
        np.hstack(
            (
                np.reshape(
                    x0.convert_to_array(),
                    (self.ocp_solver.vehicle_model.state_dimension, 1),
                ),
                np.reshape(
                    xf.convert_to_array().transpose(),
                    (self.ocp_solver.vehicle_model.state_dimension, 1),
                ),
            )
        ),
        kind="linear",
    )
    x_init_np = np.zeros((self.ocp_solver.vehicle_model.state_dimension, self.ocp_solver.horizon + 1))
    time_state = [kk for kk in range(self.ocp_solver.horizon + 1)]
    for kk in range(self.ocp_solver.horizon + 1):
        x_init_np[:, kk] = x_init_interp_fun(kk / self.ocp_solver.horizon)
    x_init = self.ocp_solver.sidt_factory.trajectory_from_numpy_array(
        traj_np=x_init_np,
        mode=TrajectoryMode.State,
        time_steps=time_state,
        t_0=t_0,
        delta_t=self.ocp_solver.delta_t,
    )
    # ... control inputs: set to zero
    u_init_np = np.zeros((self.ocp_solver.vehicle_model.input_dimension, self.ocp_solver.horizon))
    time_input = time_state
    time_input.pop()
    u_init = self.ocp_solver.sidt_factory.trajectory_from_numpy_array(
        traj_np=u_init_np,
        mode=TrajectoryMode.Input,
        time_steps=time_input,
        t_0=t_0,
        delta_t=self.ocp_solver.delta_t,
    )

    return x_init, u_init
_initial_guess_shift_solution(u_ref)

Computes an initial guess by shifting the solution from the previous time step and appending the last control input from the reference trajectory. The initial guess for the state at the end of the prediction horizon is obtained by simulating the system using the reference input.

Parameters:

Name Type Description Default
u_ref TrajectoryInterface

input reference trajectory - TrajectoryInterface

 required

Returns:

Type Description
Tuple[TrajectoryInterface, TrajectoryInterface]

initial guess for the state and control inputs
Source code in commonroad_control/control/model_predictive_control/model_predictive_control.py 
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
def _initial_guess_shift_solution(
    self, u_ref: TrajectoryInterface
) -> Tuple[TrajectoryInterface, TrajectoryInterface]:
    """
    Computes an initial guess by shifting the solution from the previous time step and appending the last control
    input from the reference trajectory. The initial guess for the state at the end of the prediction horizon is
    obtained by simulating the system using the reference input.
    :param u_ref: input reference trajectory - TrajectoryInterface
    :return: initial guess for the state and control inputs
    """

    # extract states and input to be kept
    x_init_points = [self._x_init.get_point_at_time_step(kk) for kk in range(1, self.ocp_solver.horizon + 1)]
    u_init_points = [self._u_init.get_point_at_time_step(kk) for kk in range(1, self.ocp_solver.horizon)]

    # simulate vehicle model for num_steps using the reference control inputs
    # ... append last input from reference trajectory
    u_init_points.append(u_ref.final_point)
    # ... simulate
    x_init_points.append(
        self.ocp_solver.sidt_factory.state_from_numpy_array(
            self.ocp_solver.vehicle_model.simulate_dt_nom(x_init_points[-1], u_init_points[-1])
        )
    )

    # convert to trajectory interface
    # ... state trajectory
    time_steps = [kk for kk in range(0, self.ocp_solver.horizon + 1)]
    x_init = self.ocp_solver.sidt_factory.trajectory_from_points(
        trajectory_dict=dict(zip(time_steps, x_init_points)),
        mode=TrajectoryMode.State,
        t_0=u_ref.t_0,
        delta_t=self.ocp_solver.delta_t,
    )
    # ... input trajectory
    time_steps = [kk for kk in range(0, self.ocp_solver.horizon)]
    u_init = self.ocp_solver.sidt_factory.trajectory_from_points(
        trajectory_dict=dict(zip(time_steps, u_init_points)),
        mode=TrajectoryMode.Input,
        t_0=u_ref.t_0,
        delta_t=self.ocp_solver.delta_t,
    )

    return x_init, u_init
clear_initial_guess()

Delete stored initial guess.

Source code in commonroad_control/control/model_predictive_control/model_predictive_control.py 
173
174
175
176
177
178
def clear_initial_guess(self):
    """
    Delete stored initial guess.
    """
    self._x_init = None
    self._u_init = None
compute_control_input(x0, x_ref, u_ref, x_init=None, u_init=None)

Computes the control input by solving an optimal control problem.

Parameters:

Name Type Description Default
x0 StateInterface

initial state of the vehicle - StateInterface

 required
x_ref TrajectoryInterface

state reference trajectory - TrajectoryInterface

 required
u_ref TrajectoryInterface

input reference trajectory - TrajectoryInterface

 required
x_init TrajectoryInterface

(optional) initial guess for the state trajectory -TrajectoryInterface

 None
u_init TrajectoryInterface

(optional) initial state for the input trajectory - TrajectoryInterface

 None

Returns:

Type Description
InputInterface

optimal control input - InputInterface
Source code in commonroad_control/control/model_predictive_control/model_predictive_control.py 
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
def compute_control_input(
    self,
    x0: StateInterface,
    x_ref: TrajectoryInterface,
    u_ref: TrajectoryInterface,
    x_init: TrajectoryInterface = None,
    u_init: TrajectoryInterface = None,
) -> InputInterface:
    """
    Computes the control input by solving an optimal control problem.
    :param x0: initial state of the vehicle - StateInterface
    :param x_ref: state reference trajectory - TrajectoryInterface
    :param u_ref: input reference trajectory - TrajectoryInterface
    :param x_init: (optional) initial guess for the state trajectory -TrajectoryInterface
    :param u_init: (optional) initial state for the input trajectory - TrajectoryInterface
    :return: optimal control input - InputInterface
    """

    # set initial guess for optimal control: if no initial guess is provided (x_init = None and or u_init = None),
    # an initial guess is computed by shifting an old solution or linearly interpolating the state between the
    # initial state and the final reference state
    if x_init is None or u_init is None:
        if self._x_init is None and self._u_init is None:
            x_init, u_init = self._initial_guess_linear_interpolation(x0=x0, xf=x_ref.final_point, t_0=x_ref.t_0)
        else:
            x_init, u_init = self._initial_guess_shift_solution(u_ref)

    # solve optimal control problem
    x_opt, u_opt, _ = self.ocp_solver.solve(x0, x_ref, u_ref, x_init, u_init)

    # store solution as initial guess at next time step
    self._x_init = x_opt
    self._u_init = u_opt

    return u_opt.get_point_at_time_step(0)

commonroad_control.control.model_predictive_control.optimal_control.optimal_control

OCPSolverParameters dataclass

Bases: ABC

Algorithm parameters for the optimal control problem solver.

Source code in commonroad_control/control/model_predictive_control/optimal_control/optimal_control.py 
17
18
19
20
21
22
23
@dataclass(frozen=True)
class OCPSolverParameters(ABC):
    """
    Algorithm parameters for the optimal control problem solver.
    """

    penalty_weight: float = 1000.0

OptimalControlSolverInterface

Bases: ABC

Base cass for solving optimal control problems (OCPs). We consider OCPs of the form:

minimize sum_{k=0}^horizon l_k(x(k),u(k)) + V_f(x(horizon))
such that
        x(0) = x_init
        for k ={0,...,horizon-1}:   x(k+1) = f(x(k),u(k),0)
                                    u_lb <= u(k) <= u_ub
        for k={0,...,horizon}:      x_lb <= x(k) <= x_ub
                                    a_comb(k) <= 1

where the stage cost function is l_k(x(k),u(k)) = (x(k) - x_ref(k))^T cost_xx (x(k) - x_ref(k)) + (u(k) - u_ref(k))^T cost_uu (u(k) - u_ref(k)) and the terminal cost function is V_f(x(horizon)) = (x(horizon) - x_ref(horizon))^T cost_final (x(horizon) - x_ref(horizon)) and a_comb(k) returns the normalized combined acceleration at each time step.

Source code in commonroad_control/control/model_predictive_control/optimal_control/optimal_control.py 
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
class OptimalControlSolverInterface(ABC):
    """
    Base cass for solving optimal control problems (OCPs). We consider OCPs of the form:

        minimize sum_{k=0}^horizon l_k(x(k),u(k)) + V_f(x(horizon))
        such that
                x(0) = x_init
                for k ={0,...,horizon-1}:   x(k+1) = f(x(k),u(k),0)
                                            u_lb <= u(k) <= u_ub
                for k={0,...,horizon}:      x_lb <= x(k) <= x_ub
                                            a_comb(k) <= 1

    where the stage cost function is
        l_k(x(k),u(k)) = (x(k) - x_ref(k))^T cost_xx (x(k) - x_ref(k)) + (u(k) - u_ref(k))^T cost_uu (u(k) - u_ref(k))
    and the terminal cost function is
        V_f(x(horizon)) = (x(horizon) - x_ref(horizon))^T cost_final (x(horizon) - x_ref(horizon))
    and a_comb(k) returns the normalized combined acceleration at each time step.
    """

    def __init__(
        self,
        vehicle_model: VehicleModelInterface,
        sidt_factory: StateInputDisturbanceTrajectoryFactoryInterface,
        horizon: int,
        delta_t: float,
        ocp_parameters: OCPSolverParameters,
    ):
        """
        Initialize OCP solver
        :param vehicle_model: vehicle model for predicting future states - VehicleModelInterface
        :param sidt_factory: factory for creating States and Inputs from the optimal solution - StateInputDisturbanceTrajectoryFactoryInterface
        :param horizon: (discrete) prediction horizon/number of time steps - int
        :param delta_t: sampling time - float
        :param ocp_parameters: algorithm parameters
        """

        self.vehicle_model: VehicleModelInterface = vehicle_model
        self.sidt_factory: StateInputDisturbanceTrajectoryFactoryInterface = sidt_factory
        self._ocp_parameters: OCPSolverParameters = ocp_parameters
        self.delta_t = delta_t

        # problem parameters
        self.horizon = horizon
        self._nx = self.vehicle_model.state_dimension
        self._nu = self.vehicle_model.input_dimension

    @abstractmethod
    def solve(
        self,
        x0: StateInterface,
        x_ref: TrajectoryInterface,
        u_ref: TrajectoryInterface,
        x_init: TrajectoryInterface,
        u_init: TrajectoryInterface,
    ) -> Tuple[TrajectoryInterface, TrajectoryInterface, List[Tuple[np.array, np.array]]]:
        """
        Solves an instance of the optimal control problem given an initial state, a reference trajectory to be tacked and an initial guess for the state and control inputs.
        :param x0: initial state of the vehicle - StateInterface
        :param x_ref: state reference trajectory - TrajectoryInterface
        :param u_ref: input reference trajectory - TrajectoryInterface
        :param x_init: (optional) initial guess for the state trajectory -TrajectoryInterface
        :param u_init: (optional) initial state for the input trajectory - TrajectoryInterface
        :return: optimal state and input trajectory (TrajectoryInterface) and solution history
        """

        pass

    def reset_ocp_parameters(self, new_ocp_parameters: OCPSolverParameters):
        """
        Updates parameters of the algorithm for solving the OCP, e.g., if one wishes to change the number of iterations.
        :param new_ocp_parameters: updated algorithm parameters - OCPSolverParameters
        """
        self._ocp_parameters = new_ocp_parameters
__init__(vehicle_model, sidt_factory, horizon, delta_t, ocp_parameters)

Initialize OCP solver

Parameters:

Name Type Description Default
vehicle_model VehicleModelInterface

vehicle model for predicting future states - VehicleModelInterface

 required
sidt_factory StateInputDisturbanceTrajectoryFactoryInterface

factory for creating States and Inputs from the optimal solution - StateInputDisturbanceTrajectoryFactoryInterface

 required
horizon int

(discrete) prediction horizon/number of time steps - int

 required
delta_t float

sampling time - float

 required
ocp_parameters OCPSolverParameters

algorithm parameters

 required
Source code in commonroad_control/control/model_predictive_control/optimal_control/optimal_control.py 
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
def __init__(
    self,
    vehicle_model: VehicleModelInterface,
    sidt_factory: StateInputDisturbanceTrajectoryFactoryInterface,
    horizon: int,
    delta_t: float,
    ocp_parameters: OCPSolverParameters,
):
    """
    Initialize OCP solver
    :param vehicle_model: vehicle model for predicting future states - VehicleModelInterface
    :param sidt_factory: factory for creating States and Inputs from the optimal solution - StateInputDisturbanceTrajectoryFactoryInterface
    :param horizon: (discrete) prediction horizon/number of time steps - int
    :param delta_t: sampling time - float
    :param ocp_parameters: algorithm parameters
    """

    self.vehicle_model: VehicleModelInterface = vehicle_model
    self.sidt_factory: StateInputDisturbanceTrajectoryFactoryInterface = sidt_factory
    self._ocp_parameters: OCPSolverParameters = ocp_parameters
    self.delta_t = delta_t

    # problem parameters
    self.horizon = horizon
    self._nx = self.vehicle_model.state_dimension
    self._nu = self.vehicle_model.input_dimension
reset_ocp_parameters(new_ocp_parameters)

Updates parameters of the algorithm for solving the OCP, e.g., if one wishes to change the number of iterations.

Parameters:

Name Type Description Default
new_ocp_parameters OCPSolverParameters

updated algorithm parameters - OCPSolverParameters

 required
Source code in commonroad_control/control/model_predictive_control/optimal_control/optimal_control.py 
93
94
95
96
97
98
def reset_ocp_parameters(self, new_ocp_parameters: OCPSolverParameters):
    """
    Updates parameters of the algorithm for solving the OCP, e.g., if one wishes to change the number of iterations.
    :param new_ocp_parameters: updated algorithm parameters - OCPSolverParameters
    """
    self._ocp_parameters = new_ocp_parameters
solve(x0, x_ref, u_ref, x_init, u_init) abstractmethod

Solves an instance of the optimal control problem given an initial state, a reference trajectory to be tacked and an initial guess for the state and control inputs.

Parameters:

Name Type Description Default
x0 StateInterface

initial state of the vehicle - StateInterface

 required
x_ref TrajectoryInterface

state reference trajectory - TrajectoryInterface

 required
u_ref TrajectoryInterface

input reference trajectory - TrajectoryInterface

 required
x_init TrajectoryInterface

(optional) initial guess for the state trajectory -TrajectoryInterface

 required
u_init TrajectoryInterface

(optional) initial state for the input trajectory - TrajectoryInterface

 required

Returns:

Type Description
Tuple[TrajectoryInterface, TrajectoryInterface, List[Tuple[array, array]]]

optimal state and input trajectory (TrajectoryInterface) and solution history
Source code in commonroad_control/control/model_predictive_control/optimal_control/optimal_control.py 
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
@abstractmethod
def solve(
    self,
    x0: StateInterface,
    x_ref: TrajectoryInterface,
    u_ref: TrajectoryInterface,
    x_init: TrajectoryInterface,
    u_init: TrajectoryInterface,
) -> Tuple[TrajectoryInterface, TrajectoryInterface, List[Tuple[np.array, np.array]]]:
    """
    Solves an instance of the optimal control problem given an initial state, a reference trajectory to be tacked and an initial guess for the state and control inputs.
    :param x0: initial state of the vehicle - StateInterface
    :param x_ref: state reference trajectory - TrajectoryInterface
    :param u_ref: input reference trajectory - TrajectoryInterface
    :param x_init: (optional) initial guess for the state trajectory -TrajectoryInterface
    :param u_init: (optional) initial state for the input trajectory - TrajectoryInterface
    :return: optimal state and input trajectory (TrajectoryInterface) and solution history
    """

    pass

commonroad_control.control.model_predictive_control.optimal_control.optimal_control_scvx

OptimalControlSCvx

Bases: OptimalControlSolverInterface

Successive convexification algorithm for solving non-convex optimal control problems (OCP) based on [1] T. P. Reynolds et al. "A Real-Time Algorithm for Non-Convex Powered Descent Guidance", AIAA Scitech Forum, 2020

We consider OCPs of the form:

minimize sum_{k=0}^horizon l_k(x(k),u(k)) + V_f(x(horizon))
such that
        x(0) = x_init
        for k ={0,...,horizon-1}:   x(k+1) = f(x(k),u(k),0)
                                    u_lb <= u(k) <= u_ub
        for k={0,...,horizon}:      x_lb <= x(k) <= x_ub
                                    a_comb(k) <= 1

where the stage cost function is l_k(x(k),u(k)) = (x(k) - x_ref(k))^T cost_xx (x(k) - x_ref(k)) + (u(k) - u_ref(k))^T cost_uu (u(k) - u_ref(k)) and the terminal cost function is V_f(x(horizon)) = (x(horizon) - x_ref(horizon))^T cost_final (x(horizon) - x_ref(horizon)) and a_comb(k) returns the normalized combined acceleration at each time step.

The algorithm successively convexifies this problem around the solution from the previous time step and solves these convex programming approximations until convergence.

Source code in commonroad_control/control/model_predictive_control/optimal_control/optimal_control_scvx.py 
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
class OptimalControlSCvx(OptimalControlSolverInterface):
    """
    Successive convexification algorithm for solving non-convex optimal control problems (OCP) based on
        [1] T. P. Reynolds et al. "A Real-Time Algorithm for Non-Convex Powered Descent Guidance", AIAA Scitech Forum, 2020

    We consider OCPs of the form:

        minimize sum_{k=0}^horizon l_k(x(k),u(k)) + V_f(x(horizon))
        such that
                x(0) = x_init
                for k ={0,...,horizon-1}:   x(k+1) = f(x(k),u(k),0)
                                            u_lb <= u(k) <= u_ub
                for k={0,...,horizon}:      x_lb <= x(k) <= x_ub
                                            a_comb(k) <= 1

    where the stage cost function is
        l_k(x(k),u(k)) = (x(k) - x_ref(k))^T cost_xx (x(k) - x_ref(k)) + (u(k) - u_ref(k))^T cost_uu (u(k) - u_ref(k))
    and the terminal cost function is
        V_f(x(horizon)) = (x(horizon) - x_ref(horizon))^T cost_final (x(horizon) - x_ref(horizon))
    and a_comb(k) returns the normalized combined acceleration at each time step.

    The algorithm successively convexifies this problem around the solution from the previous time step and solves these
    convex programming approximations until convergence.
    """

    def __init__(
        self,
        vehicle_model: VehicleModelInterface,
        sidt_factory: StateInputDisturbanceTrajectoryFactoryInterface,
        horizon: int,
        delta_t: float,
        cost_xx: np.ndarray,
        cost_uu: np.ndarray,
        cost_final: np.ndarray,
        ocp_parameters: SCvxParameters = SCvxParameters,
    ):
        """
        Initialize OCP solver
        :param vehicle_model: vehicle model for predicting future states - VehicleModelInterface
        :param sidt_factory: factory for creating States and Inputs from the optimal solution - StateInputDisturbanceTrajectoryFactoryInterface
        :param horizon: (discrete) prediction horizon/number of time steps - int
        :param delta_t: sampling time - float
        :param cost_xx: stage cost weighting matrix for the states, symmetric positive-semidefinite matrix - np.array
        :param cost_uu: stage cost weighting matrix for the inputs, symmetric positive-semidefinite matrix - np.array
        :param cost_final: terminal cost weighting matrix, symmetric positive-semidefinite matrix - np.array
        :param ocp_parameters: algorithm parameters
        """

        # init base class
        super().__init__(
            vehicle_model=vehicle_model,
            sidt_factory=sidt_factory,
            ocp_parameters=ocp_parameters,
            horizon=horizon,
            delta_t=delta_t,
        )

        # optimal control problem parameters
        self._horizon = horizon
        self._nx = self.vehicle_model.state_dimension
        self._nu = self.vehicle_model.input_dimension
        self._cost_xx = cost_xx
        self._cost_uu = cost_uu
        self._cost_final = cost_final

        # optimization variables
        # ... predicted states and control inputs
        self._x = cp.Variable((self._nx, self._horizon + 1))
        self._u = cp.Variable((self._nu, self._horizon))
        # ... virtual control inputs (see [1, Eq. (14)])
        self._virtual_control_pos = cp.Variable((self._nx, self._horizon))
        self._virtual_control_neg = cp.Variable((self._nx, self._horizon))

        # parameters of the optimal control problem
        # ... initial state
        self._par_x0 = cp.Parameter((self._nx,))
        # ... reference trajectories
        self._par_x_ref = cp.Parameter((self._nx, self._horizon + 1))
        self._par_u_ref = cp.Parameter((self._nu, self._horizon))
        # ... reference point for linearization of the vehicle dynamics
        self._par_x_lin = cp.Parameter((self._nx, self._horizon + 1))
        self._par_u_lin = cp.Parameter((self._nu, self._horizon))
        # ... linearization of vehicle dynamics (see [1, Eq. (14)])
        self._par_x_next = cp.Parameter((self._nx, self._horizon))
        self._par_A = cp.Parameter((self._nx, self._nx * self._horizon))
        self._par_B = cp.Parameter((self._nx, self._nu * self._horizon))
        # ... convexification of the (normalized) combined acceleration
        self._par_a_long = cp.Parameter(self._horizon, "a_long")
        self._par_a_lat = cp.Parameter(self._horizon, "a_lat")
        self._par_jac_a_long_x = cp.Parameter((self._horizon, self._nx), "jac_a_long_x")
        self._par_jac_a_long_u = cp.Parameter((self._horizon, self._nu), "jac_a_long_u")
        self._par_jac_a_lat_x = cp.Parameter((self._horizon, self._nx), "jac_a_lat_x")
        self._par_jac_a_lat_u = cp.Parameter((self._horizon, self._nu), "jac_a_lat_u")

        # Initialize history
        self._iteration_history = []

        # setup solver
        self._ocp = self._setup_solver()

    def _setup_solver(self):
        """
        Creates a parameterized convex programming approximation of the OCP using CVXPY.
        """

        # initialize cost function
        cost = 0.0

        # initial state constraint
        constraints = [self._x[:, 0] == self._par_x0]

        for kk in range(self._horizon):
            # cost function
            cost += cp.quad_form(self._x[:, kk] - self._par_x_ref[:, kk], self._cost_xx) + cp.quad_form(
                self._u[:, kk] - self._par_u_ref[:, kk], self._cost_uu
            )

            # dynamics constraint
            constraints += [
                self._x[:, kk + 1]
                == self._par_x_next[:, kk]
                + self._virtual_control_pos[:, kk]
                - self._virtual_control_neg[:, kk]
                + (
                    self._par_A[:, [kk * self._nx + ii for ii in range(self._nx)]]
                    @ (self._x[:, kk] - self._par_x_lin[:, kk])
                )
                + (
                    self._par_B[:, [kk * self._nu + ii for ii in range(self._nu)]]
                    @ (self._u[:, kk] - self._par_u_lin[:, kk])
                )
            ]

            # acceleration constraints
            constraints += [
                (
                    self._par_a_long[kk]
                    + self._par_jac_a_long_x[kk, :] @ (self._x[:, kk] - self._par_x_lin[:, kk])
                    + self._par_jac_a_long_u[kk, :] @ (self._u[:, kk] - self._par_u_lin[:, kk])
                )
                ** 2
                + (
                    self._par_a_lat[kk]
                    + self._par_jac_a_lat_x[kk, :] @ (self._x[:, kk] - self._par_x_lin[:, kk])
                    + self._par_jac_a_lat_u[kk, :] @ (self._u[:, kk] - self._par_u_lin[:, kk])
                )
                ** 2
                <= 1
            ]

        # terminal cost
        cost += cp.quad_form(
            self._x[:, self._horizon] - self._par_x_ref[:, self._horizon],
            self._cost_final,
        )

        # control input bounds
        u_lb, u_ub = self.vehicle_model.input_bounds()
        constraints.append(self._u >= np.tile(np.reshape(u_lb.convert_to_array(), (self._nu, 1)), (1, self._horizon)))
        constraints.append(self._u <= np.tile(np.reshape(u_ub.convert_to_array(), (self._nu, 1)), (1, self._horizon)))

        # # state bounds
        # x_mat_lb, x_lb, x_mat_ub, x_ub = self.vehicle_model.state_bounds()
        # constraints += [x_lb <= x_mat_lb @ self._x,
        #                 x_mat_ub @ self._x <= x_ub]

        # bounds on virtual control inputs
        constraints += [self._virtual_control_pos >= 0, self._virtual_control_neg >= 0]

        # penalize virtual control inputs
        cost += self.ocp_parameters.penalty_weight * (
            cp.sum(self._virtual_control_neg) + cp.sum(self._virtual_control_pos)
        )

        # (quadratic) soft trust-region penalty
        cost += self.ocp_parameters.soft_tr_penalty_weight * (
            cp.sum_squares(cp.vec(self._x - self._par_x_lin, order="F"))
            + cp.sum_squares(cp.vec(self._u - self._par_u_lin, order="F"))
        )

        # Define the optimization problem
        problem = cp.Problem(cp.Minimize(cost), constraints)

        return problem

    def solve(
        self,
        x0: StateInterface,
        x_ref: TrajectoryInterface,
        u_ref: TrajectoryInterface,
        x_init: TrajectoryInterface,
        u_init: TrajectoryInterface,
    ) -> Tuple[TrajectoryInterface, TrajectoryInterface, List[Tuple[np.array, np.array]]]:
        """
        Solves an instance of the optimal control problem given an initial state, a reference trajectory to be tracked
        and an initial guess for the state and control inputs.
        :param x0: initial state of the vehicle - StateInterface
        :param x_ref: state reference trajectory - TrajectoryInterface
        :param u_ref: input reference trajectory - TrajectoryInterface
        :param x_init: (optional) initial guess for the state trajectory -TrajectoryInterface
        :param u_init: (optional) initial state for the input trajectory - TrajectoryInterface
        :return: optimal state and input trajectory (TrajectoryInterface) and solution history
        """

        # reset iteration history
        self._iteration_history = []

        # set initial state
        self._par_x0.value = x0.convert_to_array()

        t_0 = x_ref.t_0
        time_state = [t_0 + self.delta_t * kk for kk in range(self._horizon + 1)]
        time_input = [t_0 + self.delta_t * kk for kk in range(self._horizon)]

        # set reference trajectories (cost function)
        self._par_x_ref.value = x_ref.convert_to_numpy_array(time_state)
        self._par_u_ref.value = u_ref.convert_to_numpy_array(time_input)

        # set reference trajectories for linearizing the vehicle model
        self._par_x_lin.value = x_init.convert_to_numpy_array(time_state)
        self._par_u_lin.value = u_init.convert_to_numpy_array(time_input)

        x_sol = []
        u_sol = []

        for num_iter in range(self.ocp_parameters.max_iterations):
            # linearize the nonlinear vehicle dynamics
            x_next = []
            A = []
            B = []
            for kk in range(self._horizon):
                tmp_x, tmp_A, tmp_B = self.vehicle_model.linearize_dt_nom_at(
                    self._par_x_lin.value[:, kk], self._par_u_lin.value[:, kk]
                )
                x_next.append(tmp_x)
                A.append(tmp_A)
                B.append(tmp_B)
            self._par_x_next.value = np.hstack(x_next)
            self._par_A.value = np.hstack(A)
            self._par_B.value = np.hstack(B)

            # linearize acceleration constraints
            a_long = []
            a_lat = []
            jac_a_long_x = []
            jac_a_long_u = []
            jac_a_lat_x = []
            jac_a_lat_u = []
            for kk in range(self._horizon):
                (
                    tmp_a_long,
                    tmp_a_lat,
                    tmp_jac_a_long_x,
                    tmp_jac_a_long_u,
                    tmp_jac_a_lat_x,
                    tmp_jac_a_lat_u,
                ) = self.vehicle_model.linearize_acceleration_constraints_at(
                    self._par_x_lin.value[:, kk], self._par_u_lin.value[:, kk]
                )
                a_long.append(tmp_a_long)
                a_lat.append(tmp_a_lat)
                jac_a_long_x.append(tmp_jac_a_long_x)
                jac_a_long_u.append(tmp_jac_a_long_u)
                jac_a_lat_x.append(tmp_jac_a_lat_x)
                jac_a_lat_u.append(tmp_jac_a_lat_u)
            self._par_a_long.value = np.vstack(a_long).squeeze()
            self._par_a_lat.value = np.vstack(a_lat).squeeze()
            self._par_jac_a_long_x.value = np.vstack(jac_a_long_x)
            self._par_jac_a_long_u.value = np.vstack(jac_a_long_u)
            self._par_jac_a_lat_x.value = np.vstack(jac_a_lat_x)
            self._par_jac_a_lat_u.value = np.vstack(jac_a_lat_u)

            # solve the optimal control problem
            self._ocp.solve(solver="CLARABEL", verbose=False)
            logger.debug(
                "Solver: %s | Iteration: %s | Status: %s | Optimal value: %s",
                self._ocp.solver_stats.solver_name,
                num_iter + 1,
                self._ocp.solution.status,
                self._ocp.solution.opt_val,
            )

            # check feasibility
            if "infeasible" in self._ocp.status or "unbounded" in self._ocp.status:
                logger.error(f"SCvx convex programming solver failed. Status: {self._ocp.status}")
                raise ValueError(f"SCvx convex programming solver failed. Status: {self._ocp.status}")

            # extract (candidate) solution
            x_sol = self._x.value
            u_sol = self._u.value

            # save solution
            self._iteration_history.append((x_sol.copy(), u_sol.copy()))

            # check convergence (see [1, Sec. II-E])
            conv_x = float(np.max(np.abs(x_sol - self._par_x_lin.value)))
            conv_u = float(np.max(np.abs(u_sol - self._par_u_lin.value)))
            if (
                conv_x < self.ocp_parameters.convergence_tolerance
                and conv_u < self.ocp_parameters.convergence_tolerance
            ):
                break

            # Update the initial guess
            self._par_x_lin.value = x_sol
            self._par_u_lin.value = u_sol

        # check feasibility
        # ... compute the defect (see [1, Alg. 2, l. 12-15] - for efficiency, we only compute the defect once the algorithm terminated)
        defect = self._compute_defect(x_sol, u_sol)
        if (float(np.max(defect)) > self.ocp_parameters.feasibility_tolerance) and (
            self.ocp_parameters.max_iterations > 1
        ):
            logger.debug("SCvx algorithm converged to a dynamically infeasible solution!")

        x_sol = self.sidt_factory.trajectory_from_numpy_array(
            traj_np=x_sol,
            mode=TrajectoryMode.State,
            time_steps=[kk for kk in range(self._horizon + 1)],
            t_0=t_0,
            delta_t=self.delta_t,
        )

        u_sol = self.sidt_factory.trajectory_from_numpy_array(
            traj_np=u_sol,
            mode=TrajectoryMode.Input,
            time_steps=[kk for kk in range(self._horizon)],
            t_0=t_0,
            delta_t=self.delta_t,
        )

        return x_sol, u_sol, self._iteration_history

    def _compute_defect(self, x: np.ndarray, u: np.ndarray) -> np.ndarray:
        """
        Computes the defect, which is the error in the predicted state trajectory, at each time step.
        :param x: predicted state trajectory - np.ndarray
        :param u: candidate control input trajectory - np.ndarray
        :return: array storing the defect at each time step - np.ndarray
        """
        err = x[:, 1 : self._horizon + 1] - np.column_stack(
            [self.vehicle_model.simulate_dt_nom(x[:, kk], u[:, kk]) for kk in range(self._horizon)]
        )
        return np.linalg.norm(err, axis=0)

    @property
    def ocp_parameters(self) -> SCvxParameters:
        return self._ocp_parameters
__init__(vehicle_model, sidt_factory, horizon, delta_t, cost_xx, cost_uu, cost_final, ocp_parameters=SCvxParameters)

Initialize OCP solver

Parameters:

Name Type Description Default
vehicle_model VehicleModelInterface

vehicle model for predicting future states - VehicleModelInterface

 required
sidt_factory StateInputDisturbanceTrajectoryFactoryInterface

factory for creating States and Inputs from the optimal solution - StateInputDisturbanceTrajectoryFactoryInterface

 required
horizon int

(discrete) prediction horizon/number of time steps - int

 required
delta_t float

sampling time - float

 required
cost_xx ndarray

stage cost weighting matrix for the states, symmetric positive-semidefinite matrix - np.array

 required
cost_uu ndarray

stage cost weighting matrix for the inputs, symmetric positive-semidefinite matrix - np.array

 required
cost_final ndarray

terminal cost weighting matrix, symmetric positive-semidefinite matrix - np.array

 required
ocp_parameters SCvxParameters

algorithm parameters

 SCvxParameters
Source code in commonroad_control/control/model_predictive_control/optimal_control/optimal_control_scvx.py 
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
def __init__(
    self,
    vehicle_model: VehicleModelInterface,
    sidt_factory: StateInputDisturbanceTrajectoryFactoryInterface,
    horizon: int,
    delta_t: float,
    cost_xx: np.ndarray,
    cost_uu: np.ndarray,
    cost_final: np.ndarray,
    ocp_parameters: SCvxParameters = SCvxParameters,
):
    """
    Initialize OCP solver
    :param vehicle_model: vehicle model for predicting future states - VehicleModelInterface
    :param sidt_factory: factory for creating States and Inputs from the optimal solution - StateInputDisturbanceTrajectoryFactoryInterface
    :param horizon: (discrete) prediction horizon/number of time steps - int
    :param delta_t: sampling time - float
    :param cost_xx: stage cost weighting matrix for the states, symmetric positive-semidefinite matrix - np.array
    :param cost_uu: stage cost weighting matrix for the inputs, symmetric positive-semidefinite matrix - np.array
    :param cost_final: terminal cost weighting matrix, symmetric positive-semidefinite matrix - np.array
    :param ocp_parameters: algorithm parameters
    """

    # init base class
    super().__init__(
        vehicle_model=vehicle_model,
        sidt_factory=sidt_factory,
        ocp_parameters=ocp_parameters,
        horizon=horizon,
        delta_t=delta_t,
    )

    # optimal control problem parameters
    self._horizon = horizon
    self._nx = self.vehicle_model.state_dimension
    self._nu = self.vehicle_model.input_dimension
    self._cost_xx = cost_xx
    self._cost_uu = cost_uu
    self._cost_final = cost_final

    # optimization variables
    # ... predicted states and control inputs
    self._x = cp.Variable((self._nx, self._horizon + 1))
    self._u = cp.Variable((self._nu, self._horizon))
    # ... virtual control inputs (see [1, Eq. (14)])
    self._virtual_control_pos = cp.Variable((self._nx, self._horizon))
    self._virtual_control_neg = cp.Variable((self._nx, self._horizon))

    # parameters of the optimal control problem
    # ... initial state
    self._par_x0 = cp.Parameter((self._nx,))
    # ... reference trajectories
    self._par_x_ref = cp.Parameter((self._nx, self._horizon + 1))
    self._par_u_ref = cp.Parameter((self._nu, self._horizon))
    # ... reference point for linearization of the vehicle dynamics
    self._par_x_lin = cp.Parameter((self._nx, self._horizon + 1))
    self._par_u_lin = cp.Parameter((self._nu, self._horizon))
    # ... linearization of vehicle dynamics (see [1, Eq. (14)])
    self._par_x_next = cp.Parameter((self._nx, self._horizon))
    self._par_A = cp.Parameter((self._nx, self._nx * self._horizon))
    self._par_B = cp.Parameter((self._nx, self._nu * self._horizon))
    # ... convexification of the (normalized) combined acceleration
    self._par_a_long = cp.Parameter(self._horizon, "a_long")
    self._par_a_lat = cp.Parameter(self._horizon, "a_lat")
    self._par_jac_a_long_x = cp.Parameter((self._horizon, self._nx), "jac_a_long_x")
    self._par_jac_a_long_u = cp.Parameter((self._horizon, self._nu), "jac_a_long_u")
    self._par_jac_a_lat_x = cp.Parameter((self._horizon, self._nx), "jac_a_lat_x")
    self._par_jac_a_lat_u = cp.Parameter((self._horizon, self._nu), "jac_a_lat_u")

    # Initialize history
    self._iteration_history = []

    # setup solver
    self._ocp = self._setup_solver()
_compute_defect(x, u)

Computes the defect, which is the error in the predicted state trajectory, at each time step.

Parameters:

Name Type Description Default
x ndarray

predicted state trajectory - np.ndarray

 required
u ndarray

candidate control input trajectory - np.ndarray

 required

Returns:

Type Description
ndarray

array storing the defect at each time step - np.ndarray
Source code in commonroad_control/control/model_predictive_control/optimal_control/optimal_control_scvx.py 
374
375
376
377
378
379
380
381
382
383
384
def _compute_defect(self, x: np.ndarray, u: np.ndarray) -> np.ndarray:
    """
    Computes the defect, which is the error in the predicted state trajectory, at each time step.
    :param x: predicted state trajectory - np.ndarray
    :param u: candidate control input trajectory - np.ndarray
    :return: array storing the defect at each time step - np.ndarray
    """
    err = x[:, 1 : self._horizon + 1] - np.column_stack(
        [self.vehicle_model.simulate_dt_nom(x[:, kk], u[:, kk]) for kk in range(self._horizon)]
    )
    return np.linalg.norm(err, axis=0)
_setup_solver()

Creates a parameterized convex programming approximation of the OCP using CVXPY.

Source code in commonroad_control/control/model_predictive_control/optimal_control/optimal_control_scvx.py 
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
def _setup_solver(self):
    """
    Creates a parameterized convex programming approximation of the OCP using CVXPY.
    """

    # initialize cost function
    cost = 0.0

    # initial state constraint
    constraints = [self._x[:, 0] == self._par_x0]

    for kk in range(self._horizon):
        # cost function
        cost += cp.quad_form(self._x[:, kk] - self._par_x_ref[:, kk], self._cost_xx) + cp.quad_form(
            self._u[:, kk] - self._par_u_ref[:, kk], self._cost_uu
        )

        # dynamics constraint
        constraints += [
            self._x[:, kk + 1]
            == self._par_x_next[:, kk]
            + self._virtual_control_pos[:, kk]
            - self._virtual_control_neg[:, kk]
            + (
                self._par_A[:, [kk * self._nx + ii for ii in range(self._nx)]]
                @ (self._x[:, kk] - self._par_x_lin[:, kk])
            )
            + (
                self._par_B[:, [kk * self._nu + ii for ii in range(self._nu)]]
                @ (self._u[:, kk] - self._par_u_lin[:, kk])
            )
        ]

        # acceleration constraints
        constraints += [
            (
                self._par_a_long[kk]
                + self._par_jac_a_long_x[kk, :] @ (self._x[:, kk] - self._par_x_lin[:, kk])
                + self._par_jac_a_long_u[kk, :] @ (self._u[:, kk] - self._par_u_lin[:, kk])
            )
            ** 2
            + (
                self._par_a_lat[kk]
                + self._par_jac_a_lat_x[kk, :] @ (self._x[:, kk] - self._par_x_lin[:, kk])
                + self._par_jac_a_lat_u[kk, :] @ (self._u[:, kk] - self._par_u_lin[:, kk])
            )
            ** 2
            <= 1
        ]

    # terminal cost
    cost += cp.quad_form(
        self._x[:, self._horizon] - self._par_x_ref[:, self._horizon],
        self._cost_final,
    )

    # control input bounds
    u_lb, u_ub = self.vehicle_model.input_bounds()
    constraints.append(self._u >= np.tile(np.reshape(u_lb.convert_to_array(), (self._nu, 1)), (1, self._horizon)))
    constraints.append(self._u <= np.tile(np.reshape(u_ub.convert_to_array(), (self._nu, 1)), (1, self._horizon)))

    # # state bounds
    # x_mat_lb, x_lb, x_mat_ub, x_ub = self.vehicle_model.state_bounds()
    # constraints += [x_lb <= x_mat_lb @ self._x,
    #                 x_mat_ub @ self._x <= x_ub]

    # bounds on virtual control inputs
    constraints += [self._virtual_control_pos >= 0, self._virtual_control_neg >= 0]

    # penalize virtual control inputs
    cost += self.ocp_parameters.penalty_weight * (
        cp.sum(self._virtual_control_neg) + cp.sum(self._virtual_control_pos)
    )

    # (quadratic) soft trust-region penalty
    cost += self.ocp_parameters.soft_tr_penalty_weight * (
        cp.sum_squares(cp.vec(self._x - self._par_x_lin, order="F"))
        + cp.sum_squares(cp.vec(self._u - self._par_u_lin, order="F"))
    )

    # Define the optimization problem
    problem = cp.Problem(cp.Minimize(cost), constraints)

    return problem
solve(x0, x_ref, u_ref, x_init, u_init)

Solves an instance of the optimal control problem given an initial state, a reference trajectory to be tracked and an initial guess for the state and control inputs.

Parameters:

Name Type Description Default
x0 StateInterface

initial state of the vehicle - StateInterface

 required
x_ref TrajectoryInterface

state reference trajectory - TrajectoryInterface

 required
u_ref TrajectoryInterface

input reference trajectory - TrajectoryInterface

 required
x_init TrajectoryInterface

(optional) initial guess for the state trajectory -TrajectoryInterface

 required
u_init TrajectoryInterface

(optional) initial state for the input trajectory - TrajectoryInterface

 required

Returns:

Type Description
Tuple[TrajectoryInterface, TrajectoryInterface, List[Tuple[array, array]]]

optimal state and input trajectory (TrajectoryInterface) and solution history
Source code in commonroad_control/control/model_predictive_control/optimal_control/optimal_control_scvx.py 
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
def solve(
    self,
    x0: StateInterface,
    x_ref: TrajectoryInterface,
    u_ref: TrajectoryInterface,
    x_init: TrajectoryInterface,
    u_init: TrajectoryInterface,
) -> Tuple[TrajectoryInterface, TrajectoryInterface, List[Tuple[np.array, np.array]]]:
    """
    Solves an instance of the optimal control problem given an initial state, a reference trajectory to be tracked
    and an initial guess for the state and control inputs.
    :param x0: initial state of the vehicle - StateInterface
    :param x_ref: state reference trajectory - TrajectoryInterface
    :param u_ref: input reference trajectory - TrajectoryInterface
    :param x_init: (optional) initial guess for the state trajectory -TrajectoryInterface
    :param u_init: (optional) initial state for the input trajectory - TrajectoryInterface
    :return: optimal state and input trajectory (TrajectoryInterface) and solution history
    """

    # reset iteration history
    self._iteration_history = []

    # set initial state
    self._par_x0.value = x0.convert_to_array()

    t_0 = x_ref.t_0
    time_state = [t_0 + self.delta_t * kk for kk in range(self._horizon + 1)]
    time_input = [t_0 + self.delta_t * kk for kk in range(self._horizon)]

    # set reference trajectories (cost function)
    self._par_x_ref.value = x_ref.convert_to_numpy_array(time_state)
    self._par_u_ref.value = u_ref.convert_to_numpy_array(time_input)

    # set reference trajectories for linearizing the vehicle model
    self._par_x_lin.value = x_init.convert_to_numpy_array(time_state)
    self._par_u_lin.value = u_init.convert_to_numpy_array(time_input)

    x_sol = []
    u_sol = []

    for num_iter in range(self.ocp_parameters.max_iterations):
        # linearize the nonlinear vehicle dynamics
        x_next = []
        A = []
        B = []
        for kk in range(self._horizon):
            tmp_x, tmp_A, tmp_B = self.vehicle_model.linearize_dt_nom_at(
                self._par_x_lin.value[:, kk], self._par_u_lin.value[:, kk]
            )
            x_next.append(tmp_x)
            A.append(tmp_A)
            B.append(tmp_B)
        self._par_x_next.value = np.hstack(x_next)
        self._par_A.value = np.hstack(A)
        self._par_B.value = np.hstack(B)

        # linearize acceleration constraints
        a_long = []
        a_lat = []
        jac_a_long_x = []
        jac_a_long_u = []
        jac_a_lat_x = []
        jac_a_lat_u = []
        for kk in range(self._horizon):
            (
                tmp_a_long,
                tmp_a_lat,
                tmp_jac_a_long_x,
                tmp_jac_a_long_u,
                tmp_jac_a_lat_x,
                tmp_jac_a_lat_u,
            ) = self.vehicle_model.linearize_acceleration_constraints_at(
                self._par_x_lin.value[:, kk], self._par_u_lin.value[:, kk]
            )
            a_long.append(tmp_a_long)
            a_lat.append(tmp_a_lat)
            jac_a_long_x.append(tmp_jac_a_long_x)
            jac_a_long_u.append(tmp_jac_a_long_u)
            jac_a_lat_x.append(tmp_jac_a_lat_x)
            jac_a_lat_u.append(tmp_jac_a_lat_u)
        self._par_a_long.value = np.vstack(a_long).squeeze()
        self._par_a_lat.value = np.vstack(a_lat).squeeze()
        self._par_jac_a_long_x.value = np.vstack(jac_a_long_x)
        self._par_jac_a_long_u.value = np.vstack(jac_a_long_u)
        self._par_jac_a_lat_x.value = np.vstack(jac_a_lat_x)
        self._par_jac_a_lat_u.value = np.vstack(jac_a_lat_u)

        # solve the optimal control problem
        self._ocp.solve(solver="CLARABEL", verbose=False)
        logger.debug(
            "Solver: %s | Iteration: %s | Status: %s | Optimal value: %s",
            self._ocp.solver_stats.solver_name,
            num_iter + 1,
            self._ocp.solution.status,
            self._ocp.solution.opt_val,
        )

        # check feasibility
        if "infeasible" in self._ocp.status or "unbounded" in self._ocp.status:
            logger.error(f"SCvx convex programming solver failed. Status: {self._ocp.status}")
            raise ValueError(f"SCvx convex programming solver failed. Status: {self._ocp.status}")

        # extract (candidate) solution
        x_sol = self._x.value
        u_sol = self._u.value

        # save solution
        self._iteration_history.append((x_sol.copy(), u_sol.copy()))

        # check convergence (see [1, Sec. II-E])
        conv_x = float(np.max(np.abs(x_sol - self._par_x_lin.value)))
        conv_u = float(np.max(np.abs(u_sol - self._par_u_lin.value)))
        if (
            conv_x < self.ocp_parameters.convergence_tolerance
            and conv_u < self.ocp_parameters.convergence_tolerance
        ):
            break

        # Update the initial guess
        self._par_x_lin.value = x_sol
        self._par_u_lin.value = u_sol

    # check feasibility
    # ... compute the defect (see [1, Alg. 2, l. 12-15] - for efficiency, we only compute the defect once the algorithm terminated)
    defect = self._compute_defect(x_sol, u_sol)
    if (float(np.max(defect)) > self.ocp_parameters.feasibility_tolerance) and (
        self.ocp_parameters.max_iterations > 1
    ):
        logger.debug("SCvx algorithm converged to a dynamically infeasible solution!")

    x_sol = self.sidt_factory.trajectory_from_numpy_array(
        traj_np=x_sol,
        mode=TrajectoryMode.State,
        time_steps=[kk for kk in range(self._horizon + 1)],
        t_0=t_0,
        delta_t=self.delta_t,
    )

    u_sol = self.sidt_factory.trajectory_from_numpy_array(
        traj_np=u_sol,
        mode=TrajectoryMode.Input,
        time_steps=[kk for kk in range(self._horizon)],
        t_0=t_0,
        delta_t=self.delta_t,
    )

    return x_sol, u_sol, self._iteration_history

SCvxParameters dataclass

Bases: OCPSolverParameters

Parameters for the Successive Convexification algorithm.

Source code in commonroad_control/control/model_predictive_control/optimal_control/optimal_control_scvx.py 
25
26
27
28
29
30
31
32
33
34
35
36
37
38
@dataclass(frozen=True)
class SCvxParameters(OCPSolverParameters):
    """
    Parameters for the Successive Convexification algorithm.
    """

    penalty_weight: float = 1000.0  # weight for the (exact) penalty of non-zero values of slack variables
    max_iterations: int = 10  # maximum number of convex programming iterations
    soft_tr_penalty_weight: float = 0.001  # penalty for soft trust-region terms
    convergence_tolerance: float = 1e-3  # tolerance for convergence
    feasibility_tolerance: float = 1e-4  # tolerance for dynamical feasibility

    def __post_init__(self):
        super().__init__(penalty_weight=self.penalty_weight)