Source code for nnabla_rl.algorithms.ilqr

# Copyright 2022,2023 Sony Group Corporation.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# See the License for the specific language governing permissions and
# limitations under the License.

from dataclasses import dataclass
from typing import List

import numpy as np

from nnabla_rl.algorithms.ddp import DDP, DDPConfig
from nnabla_rl.numpy_models.cost_function import CostFunction
from nnabla_rl.numpy_models.dynamics import Dynamics

[docs]@dataclass class iLQRConfig(DDPConfig): # same as DDP pass
[docs]class iLQR(DDP): """Iterative LQR (Linear Quadratic Regulator) algorithm. This class implements the iterative Linear Quadratic Requlator (iLQR) algorithm proposed by Y. Tassa, et al. in the paper: "Synthesis and Stabilization of Complex Behaviors through Online Trajectory Optimization" For details see: Args: env_or_env_info\ (gym.Env or :py:class:`EnvironmentInfo <nnabla_rl.environments.environment_info.EnvironmentInfo>`): the environment to train or environment info dynamics (:py:class:`Dynamics <nnabla_rl.non_nn_models.dynamics.Dynamics>`): dynamics of the system to control cost_function (:py:class:`Dynamics <nnabla_rl.non_nn_models.cost_function.CostFunction>`): cost function to optimize the trajectory config (:py:class:`iLQRConfig <nnabla_rl.algorithmss.ilqr.iLQRConfig>`): the parameter for iLQR controller """ _config: iLQRConfig def __init__(self, env_or_env_info, dynamics: Dynamics, cost_function: CostFunction, config=iLQRConfig()): super(iLQR, self).__init__(env_or_env_info, dynamics, cost_function, config=config) def _backward_pass(self, trajectory, dynamics, cost_function, mu): x_last, u_last = trajectory[-1] # Initialize Vx and Vxx to the gradient/hessian of value function of the final state of the trajectory Vx, *_ = cost_function.gradient(x_last, u_last, self._config.T_max, final_state=True) Vxx, *_ = cost_function.hessian(x_last, u_last, self._config.T_max, final_state=True) E = np.identity(n=Vxx.shape[0]) ks: List[np.ndarray] = [] Ks: List[np.ndarray] = [] Qus: List[np.ndarray] = [] Quus: List[np.ndarray] = [] Quu_invs: List[np.ndarray] = [] for t in reversed(range(self._config.T_max - 1)): (x, u) = trajectory[t] Cx, Cu = cost_function.gradient(x, u, self._config.T_max - t - 1) Cxx, Cxu, Cux, Cuu = cost_function.hessian(x, u, self._config.T_max - t - 1) Fx, Fu = dynamics.gradient(x, u, self._config.T_max - t - 1) # iLQR ignore dynamics' hessian Quu = Cuu + + mu * E).dot(Fu) if not self._is_positive_definite(Quu): return ks, Ks, Qus, Quus, Quu_invs, False Qx = Cx + Qu = Cu + Qxx = Cxx + # NOTE: Qxu and Qux should be symmetric and same matrix. i.e. Qxu = Qux and Qxu.T = Qux Qxu = Cxu + + mu * E).dot(Fx).T Qux = Cux + + mu * E).dot(Fx) assert np.allclose(Qxu, Qux.T) Quu_inv = np.linalg.inv(Quu) k = K = ks.append(k) Ks.append(K) Qus.append(Qu) Quus.append(Quu) Quu_invs.append(Quu_inv) Vx = Qx + + + Vxx = Qxx + + + ks = list(reversed(ks)) Ks = list(reversed(Ks)) Qus = list(reversed(Qus)) Quus = list(reversed(Quus)) Quu_invs = list(reversed(Quu_invs)) return ks, Ks, Qus, Quus, Quu_invs, True