(self, traj_distr, traj_info, pol_info, m)
return predicted_cost, predicted_kl
Compute Laplace approximation to expected cost.
        Args:
            traj_distr: A linear Gaussian policy object.
            traj_info: A TrajectoryInfo object.
            pol_info: Policy linearization info.
            m: Condition number.
Compute Laplace approximation to expected cost.
        Args:
            traj_distr: A linear Gaussian policy object.
            traj_info: A TrajectoryInfo object.
            pol_info: Policy linearization info.
            m: Condition number.
Compute Laplace approximation to expected cost . Args : traj_distr : A linear Gaussian policy object . traj_info : A TrajectoryInfo object . pol_info : Policy linearization info . m : Condition number .
def _estimate_cost(self, traj_distr, traj_info, pol_info, m):
        """
        Compute Laplace approximation to expected cost.
        Args:
            traj_distr: A linear Gaussian policy object.
            traj_info: A TrajectoryInfo object.
            pol_info: Policy linearization info.
            m: Condition number.
        """
        # Constants.
        T, dU, dX = self.T, self.dU, self.dX

        # Perform forward pass (note that we repeat this here, because
        # traj_info may have different dynamics from the ones that were
        # used to compute the distribution already saved in traj).
        mu, sigma = self.traj_opt.forward(traj_distr, traj_info)

        # Compute cost.
        predicted_cost = np.zeros(T)
        for t in range(T):
            predicted_cost[t] = traj_info.cc[t] + 0.5 * \
                    (np.sum(sigma[t, :, :] * traj_info.Cm[t, :, :]) +
                     mu[t, :].T.dot(traj_info.Cm[t, :, :]).dot(mu[t, :])) + \
                    mu[t, :].T.dot(traj_info.cv[t, :])

        # Compute KL divergence.
        predicted_kl = np.zeros(T)
        for t in range(T):
            inv_pS = np.linalg.solve(
                pol_info.chol_pol_S[t, :, :],
                np.linalg.solve(pol_info.chol_pol_S[t, :, :].T, np.eye(dU))
            )
            Ufb = pol_info.pol_K[t, :, :].dot(mu[t, :dX].T) + \
                    pol_info.pol_k[t, :]
            diff = mu[t, dX:] - Ufb
            Kbar = traj_distr.K[t, :, :] - pol_info.pol_K[t, :, :]
            predicted_kl[t] = 0.5 * (diff).dot(inv_pS).dot(diff) + \
                    0.5 * np.sum(traj_distr.pol_covar[t, :, :] * inv_pS) + \
                    0.5 * np.sum(
                        sigma[t, :dX, :dX] * Kbar.T.dot(inv_pS).dot(Kbar)
                    ) + np.sum(
                        np.log(np.diag(pol_info.chol_pol_S[t, :, :]))
                    ) - np.sum(
                        np.log(np.diag(traj_distr.chol_pol_covar[t, :, :]))
                    ) + 0.5 * dU

        return predicted_cost, predicted_kl
def _estimate_cost ( self , traj_distr , traj_info , pol_info , m ) : # Constants. T , dU , dX = self . T , self . dU , self . dX # Perform forward pass (note that we repeat this here, because # traj_info may have different dynamics from the ones that were # used to compute the distribution already saved in traj). mu , sigma = self . traj_opt . forward ( traj_distr , traj_info ) # Compute cost. predicted_cost = np . zeros ( T ) for t in range ( T ) : predicted_cost [ t ] = traj_info . cc [ t ] + 0.5 * ( np . sum ( sigma [ t , : , : ] * traj_info . Cm [ t , : , : ] ) + mu [ t , : ] . T . dot ( traj_info . Cm [ t , : , : ] ) . dot ( mu [ t , : ] ) ) + mu [ t , : ] . T . dot ( traj_info . cv [ t , : ] ) # Compute KL divergence. predicted_kl = np . zeros ( T ) for t in range ( T ) : inv_pS = np . linalg . solve ( pol_info . chol_pol_S [ t , : , : ] , np . linalg . solve ( pol_info . chol_pol_S [ t , : , : ] . T , np . eye ( dU ) ) ) Ufb = pol_info . pol_K [ t , : , : ] . dot ( mu [ t , : dX ] . T ) + pol_info . pol_k [ t , : ] diff = mu [ t , dX : ] - Ufb Kbar = traj_distr . K [ t , : , : ] - pol_info . pol_K [ t , : , : ] predicted_kl [ t ] = 0.5 * ( diff ) . dot ( inv_pS ) . dot ( diff ) + 0.5 * np . sum ( traj_distr . pol_covar [ t , : , : ] * inv_pS ) + 0.5 * np . sum ( sigma [ t , : dX , : dX ] * Kbar . T . dot ( inv_pS ) . dot ( Kbar ) ) + np . sum ( np . log ( np . diag ( pol_info . chol_pol_S [ t , : , : ] ) ) ) - np . sum ( np . log ( np . diag ( traj_distr . chol_pol_covar [ t , : , : ] ) ) ) + 0.5 * dU return predicted_cost , predicted_kl