(self, m, eta, augment=True)
return fCm, fcv
Compute cost estimates used in the LQR backward pass.
Compute cost estimates used in the LQR backward pass.
Compute cost estimates used in the LQR backward pass .
def compute_costs(self, m, eta, augment=True):
        """ Compute cost estimates used in the LQR backward pass. """
        traj_info, traj_distr = self.cur[m].traj_info, self.cur[m].traj_distr
        if not augment:  # Whether to augment cost with term to penalize KL
            return traj_info.Cm, traj_info.cv

        pol_info = self.cur[m].pol_info
        multiplier = self._hyperparams['max_ent_traj']
        T, dU, dX = traj_distr.T, traj_distr.dU, traj_distr.dX
        Cm, cv = np.copy(traj_info.Cm), np.copy(traj_info.cv)

        # Modify policy action via Lagrange multiplier.
        cv[:, dX:] -= pol_info.lambda_k
        Cm[:, dX:, :dX] -= pol_info.lambda_K
        Cm[:, :dX, dX:] -= np.transpose(pol_info.lambda_K, [0, 2, 1])

        #Pre-process the costs with KL-divergence terms.
        TKLm = np.zeros((T, dX+dU, dX+dU))
        TKLv = np.zeros((T, dX+dU))
        PKLm = np.zeros((T, dX+dU, dX+dU))
        PKLv = np.zeros((T, dX+dU))
        fCm, fcv = np.zeros(Cm.shape), np.zeros(cv.shape)
        for t in range(T):
            K, k = traj_distr.K[t, :, :], traj_distr.k[t, :]
            inv_pol_covar = traj_distr.inv_pol_covar[t, :, :]
            # Trajectory KL-divergence terms.
            TKLm[t, :, :] = np.vstack([
                np.hstack([
                    K.T.dot(inv_pol_covar).dot(K),
                    -K.T.dot(inv_pol_covar)]),
                np.hstack([-inv_pol_covar.dot(K), inv_pol_covar])
            ])
            TKLv[t, :] = np.concatenate([
                K.T.dot(inv_pol_covar).dot(k), -inv_pol_covar.dot(k)
            ])
            # Policy KL-divergence terms.
            inv_pol_S = np.linalg.solve(
                pol_info.chol_pol_S[t, :, :],
                np.linalg.solve(pol_info.chol_pol_S[t, :, :].T, np.eye(dU))
            )
            KB, kB = pol_info.pol_K[t, :, :], pol_info.pol_k[t, :]
            PKLm[t, :, :] = np.vstack([
                np.hstack([KB.T.dot(inv_pol_S).dot(KB), -KB.T.dot(inv_pol_S)]),
                np.hstack([-inv_pol_S.dot(KB), inv_pol_S])
            ])
            PKLv[t, :] = np.concatenate([
                KB.T.dot(inv_pol_S).dot(kB), -inv_pol_S.dot(kB)
            ])
            wt = pol_info.pol_wt[t]
            fCm[t, :, :] = (Cm[t, :, :] + TKLm[t, :, :] * eta +
                            PKLm[t, :, :] * wt) / (eta + wt + multiplier)
            fcv[t, :] = (cv[t, :] + TKLv[t, :] * eta +
                         PKLv[t, :] * wt) / (eta + wt + multiplier)

        return fCm, fcv
def compute_costs ( self , m , eta , augment = True ) : traj_info , traj_distr = self . cur [ m ] . traj_info , self . cur [ m ] . traj_distr if not augment : # Whether to augment cost with term to penalize KL return traj_info . Cm , traj_info . cv pol_info = self . cur [ m ] . pol_info multiplier = self . _hyperparams [ 'max_ent_traj' ] T , dU , dX = traj_distr . T , traj_distr . dU , traj_distr . dX Cm , cv = np . copy ( traj_info . Cm ) , np . copy ( traj_info . cv ) # Modify policy action via Lagrange multiplier. cv [ : , dX : ] -= pol_info . lambda_k Cm [ : , dX : , : dX ] -= pol_info . lambda_K Cm [ : , : dX , dX : ] -= np . transpose ( pol_info . lambda_K , [ 0 , 2 , 1 ] ) #Pre-process the costs with KL-divergence terms. TKLm = np . zeros ( ( T , dX + dU , dX + dU ) ) TKLv = np . zeros ( ( T , dX + dU ) ) PKLm = np . zeros ( ( T , dX + dU , dX + dU ) ) PKLv = np . zeros ( ( T , dX + dU ) ) fCm , fcv = np . zeros ( Cm . shape ) , np . zeros ( cv . shape ) for t in range ( T ) : K , k = traj_distr . K [ t , : , : ] , traj_distr . k [ t , : ] inv_pol_covar = traj_distr . inv_pol_covar [ t , : , : ] # Trajectory KL-divergence terms. TKLm [ t , : , : ] = np . vstack ( [ np . hstack ( [ K . T . dot ( inv_pol_covar ) . dot ( K ) , - K . T . dot ( inv_pol_covar ) ] ) , np . hstack ( [ - inv_pol_covar . dot ( K ) , inv_pol_covar ] ) ] ) TKLv [ t , : ] = np . concatenate ( [ K . T . dot ( inv_pol_covar ) . dot ( k ) , - inv_pol_covar . dot ( k ) ] ) # Policy KL-divergence terms. inv_pol_S = np . linalg . solve ( pol_info . chol_pol_S [ t , : , : ] , np . linalg . solve ( pol_info . chol_pol_S [ t , : , : ] . T , np . eye ( dU ) ) ) KB , kB = pol_info . pol_K [ t , : , : ] , pol_info . pol_k [ t , : ] PKLm [ t , : , : ] = np . vstack ( [ np . hstack ( [ KB . T . dot ( inv_pol_S ) . dot ( KB ) , - KB . T . dot ( inv_pol_S ) ] ) , np . hstack ( [ - inv_pol_S . dot ( KB ) , inv_pol_S ] ) ] ) PKLv [ t , : ] = np . concatenate ( [ KB . T . dot ( inv_pol_S ) . dot ( kB ) , - inv_pol_S . dot ( kB ) ] ) wt = pol_info . pol_wt [ t ] fCm [ t , : , : ] = ( Cm [ t , : , : ] + TKLm [ t , : , : ] * eta + PKLm [ t , : , : ] * wt ) / ( eta + wt + multiplier ) fcv [ t , : ] = ( cv [ t , : ] + TKLv [ t , : ] * eta + PKLv [ t , : ] * wt ) / ( eta + wt + multiplier ) return fCm , fcv