(self, sample)
return l, lx, lu, lxx, luu, lux
Evaluate cost function and derivatives on a sample.
        Args:
            sample:  A single sample
Evaluate cost function and derivatives on a sample.
        Args:
            sample:  A single sample
Evaluate cost function and derivatives on a sample . Args : sample : A single sample
def eval(self, sample):
        """
        Evaluate cost function and derivatives on a sample.
        Args:
            sample:  A single sample
        """
        T, dU, dX = sample.T, sample.dU, sample.dX
        orig_A, orig_b = self._hyperparams['A'], self._hyperparams['b']

        # Discretize waypoint time steps.
        waypoint_step = np.ceil(T * self._hyperparams['waypoint_time'])
        A = np.zeros((T, dX+dU, dX+dU))
        b = np.zeros((T, dX+dU))

        if not isinstance(self._hyperparams['ramp_option'], list):
            self._hyperparams['ramp_option'] = [
                self._hyperparams['ramp_option'] for _ in waypoint_step
            ]

        # Set up time-varying matrix and vector.
        start = 0
        for i in range(len(waypoint_step)):
            wpm = get_ramp_multiplier(self._hyperparams['ramp_option'][i],
                                      waypoint_step[i] - start)
            for t in range(start, int(waypoint_step[i])):
                A[t, :, :] = wpm[t-start] * orig_A[i, :, :]
                b[t, :] = wpm[t-start] * orig_b[i, :]
            start = int(waypoint_step[i])

        # Evaluate distance function.
        XU = np.concatenate([sample.get_X(), sample.get_U()], axis=1)
        dist = np.zeros((T, dX+dU))
        for t in range(T):
            dist[t, :] = A[t, :, :].dot(XU[t, :]) + b[t, :]

        ix, iu = slice(dX), slice(dX, dX+dU)
        # Compute the loss.
        Al, Aly, Alyy = self._evalloss(dist)
        Alx = Aly[:, ix]
        Alu = Aly[:, iu]
        Alxx = Alyy[:, ix, ix]
        Aluu = Alyy[:, iu, iu]
        Alux = Alyy[:, ix, iu]

        # Compute state derivatives and value. Loss is the same.
        l = Al
        # First derivative given by A'*Aly.
        # Second derivative given by A'*Alyy*A.
        lx, lu = np.zeros((T, dX)), np.zeros((T, dU))
        lxx, luu = np.zeros((T, dX, dX)), np.zeros((T, dU, dU))
        lux = np.zeros((T, dU, dX))
        for t in range(T):
            lx[t, :] = A[t, ix, ix].T.dot(Alx[t, :])
            lu[t, :] = A[t, iu, iu].T.dot(Alu[t, :])
            lxx[t, :, :] = A[t, ix, ix].T.dot(Alxx[t, :, :]).dot(A[t, ix, ix])
            luu[t, :, :] = A[t, iu, iu].T.dot(Aluu[t, :, :]).dot(A[t, iu, iu])
            lux[t, :, :] = A[t, ix, iu].T.dot(Alux[t, :, :]).dot(A[t, iu, ix])

        return l, lx, lu, lxx, luu, lux
def eval ( self , sample ) : T , dU , dX = sample . T , sample . dU , sample . dX orig_A , orig_b = self . _hyperparams [ 'A' ] , self . _hyperparams [ 'b' ] # Discretize waypoint time steps. waypoint_step = np . ceil ( T * self . _hyperparams [ 'waypoint_time' ] ) A = np . zeros ( ( T , dX + dU , dX + dU ) ) b = np . zeros ( ( T , dX + dU ) ) if not isinstance ( self . _hyperparams [ 'ramp_option' ] , list ) : self . _hyperparams [ 'ramp_option' ] = [ self . _hyperparams [ 'ramp_option' ] for _ in waypoint_step ] # Set up time-varying matrix and vector. start = 0 for i in range ( len ( waypoint_step ) ) : wpm = get_ramp_multiplier ( self . _hyperparams [ 'ramp_option' ] [ i ] , waypoint_step [ i ] - start ) for t in range ( start , int ( waypoint_step [ i ] ) ) : A [ t , : , : ] = wpm [ t - start ] * orig_A [ i , : , : ] b [ t , : ] = wpm [ t - start ] * orig_b [ i , : ] start = int ( waypoint_step [ i ] ) # Evaluate distance function. XU = np . concatenate ( [ sample . get_X ( ) , sample . get_U ( ) ] , axis = 1 ) dist = np . zeros ( ( T , dX + dU ) ) for t in range ( T ) : dist [ t , : ] = A [ t , : , : ] . dot ( XU [ t , : ] ) + b [ t , : ] ix , iu = slice ( dX ) , slice ( dX , dX + dU ) # Compute the loss. Al , Aly , Alyy = self . _evalloss ( dist ) Alx = Aly [ : , ix ] Alu = Aly [ : , iu ] Alxx = Alyy [ : , ix , ix ] Aluu = Alyy [ : , iu , iu ] Alux = Alyy [ : , ix , iu ] # Compute state derivatives and value. Loss is the same. l = Al # First derivative given by A'*Aly. # Second derivative given by A'*Alyy*A. lx , lu = np . zeros ( ( T , dX ) ) , np . zeros ( ( T , dU ) ) lxx , luu = np . zeros ( ( T , dX , dX ) ) , np . zeros ( ( T , dU , dU ) ) lux = np . zeros ( ( T , dU , dX ) ) for t in range ( T ) : lx [ t , : ] = A [ t , ix , ix ] . T . dot ( Alx [ t , : ] ) lu [ t , : ] = A [ t , iu , iu ] . T . dot ( Alu [ t , : ] ) lxx [ t , : , : ] = A [ t , ix , ix ] . T . dot ( Alxx [ t , : , : ] ) . dot ( A [ t , ix , ix ] ) luu [ t , : , : ] = A [ t , iu , iu ] . T . dot ( Aluu [ t , : , : ] ) . dot ( A [ t , iu , iu ] ) lux [ t , : , : ] = A [ t , ix , iu ] . T . dot ( Alux [ t , : , : ] ) . dot ( A [ t , iu , ix ] ) return l , lx , lu , lxx , luu , lux