unittest/python/crocoddyl/kkt.py
def calc(self):
        '''
        For a given pair of candidate state and control trajectories,
        compute the KKT system.
        isFeasible should be set True if F(xs,us) = 0 (xs obtained from rollout).
        '''
        xs, us = self.xs, self.us
        problem = self.problem
        self.cost = problem.calcDiff(xs, us)
        ix = 0
        iu = 0
        cx0 = problem.runningModels[0].ndx  # offset on constraint xnext = f(x,u) due to x0 = ref.

        np.fill_diagonal(self.Fx, 1)
        for model, data, xguess in zip(problem.runningModels, problem.runningDatas, xs[1:]):
            ndx = model.ndx
            nu = model.nu
            self.Lxx[ix:ix + ndx, ix:ix + ndx] = data.Lxx
            self.Lxu[ix:ix + ndx, iu:iu + nu] = data.Lxu
            self.Lux[iu:iu + nu, ix:ix + ndx] = data.Lxu.T
            self.Luu[iu:iu + nu, iu:iu + nu] = data.Luu

            self.Lx[ix:ix + ndx] = data.Lx
            self.Lu[iu:iu + nu] = data.Lu

            self.Fx[cx0 + ix:cx0 + ix + ndx, ix:ix + ndx] = -data.Fx
            self.Fu[cx0 + ix:cx0 + ix + ndx, iu:iu + nu] = -data.Fu

            # constraint value = xnext_guess - f(x_guess,u_guess) = diff(f,xnext_guesss)
            self.cval[cx0 + ix:cx0 + ix + ndx] = model.State.diff(data.xnext, xguess)  # data.F
            ix += ndx
            iu += nu

        model, data = problem.terminalModel, problem.terminalData
        ndx = model.ndx
        nu = model.nu
        self.Lxx[ix:ix + ndx, ix:ix + ndx] = data.Lxx
        self.Lx[ix:ix + ndx] = data.Lx

        # constraint value = x_guess - x_ref = diff(x_ref,x_guess)
        self.cval[:cx0] = problem.runningModels[0].State.diff(problem.initialState, xs[0])

        self.jacT[:] = self.jac.T

        # Regularization
        ndx = sum([m.ndx for m in self.models()])
        nu = sum([m.nu for m in self.problem.runningModels])
        if self.x_reg != 0:
            self.kkt[range(ndx), range(ndx)] += self.x_reg
        if self.u_reg != 0:
            self.kkt[range(ndx, ndx + nu), range(ndx, ndx + nu)] += self.u_reg

        return self.cost

        For a given pair of candidate state and control trajectories,
        compute the KKT system.
        isFeasible should be set True if F(xs,us) = 0 (xs obtained from rollout).
        
For a given pair of candidate state and control trajectories,
compute the KKT system.
isFeasible should be set True if F(xs,us) = 0 (xs obtained from rollout).
For a given pair of candidate state and control trajectories compute the KKT system . isFeasible should be set True if F ( xs us ) = 0 ( xs obtained from rollout ) .
def calc(self):
        xs, us = self.xs, self.us
        problem = self.problem
        self.cost = problem.calcDiff(xs, us)
        ix = 0
        iu = 0
        cx0 = problem.runningModels[0].ndx  
        np.fill_diagonal(self.Fx, 1)
        for model, data, xguess in zip(problem.runningModels, problem.runningDatas, xs[1:]):
            ndx = model.ndx
            nu = model.nu
            self.Lxx[ix:ix + ndx, ix:ix + ndx] = data.Lxx
            self.Lxu[ix:ix + ndx, iu:iu + nu] = data.Lxu
            self.Lux[iu:iu + nu, ix:ix + ndx] = data.Lxu.T
            self.Luu[iu:iu + nu, iu:iu + nu] = data.Luu
            self.Lx[ix:ix + ndx] = data.Lx
            self.Lu[iu:iu + nu] = data.Lu
            self.Fx[cx0 + ix:cx0 + ix + ndx, ix:ix + ndx] = -data.Fx
            self.Fu[cx0 + ix:cx0 + ix + ndx, iu:iu + nu] = -data.Fu
            self.cval[cx0 + ix:cx0 + ix + ndx] = model.State.diff(data.xnext, xguess)  
            ix += ndx
            iu += nu
        model, data = problem.terminalModel, problem.terminalData
        ndx = model.ndx
        nu = model.nu
        self.Lxx[ix:ix + ndx, ix:ix + ndx] = data.Lxx
        self.Lx[ix:ix + ndx] = data.Lx
        self.cval[:cx0] = problem.runningModels[0].State.diff(problem.initialState, xs[0])
        self.jacT[:] = self.jac.T
        ndx = sum([m.ndx for m in self.models()])
        nu = sum([m.nu for m in self.problem.runningModels])
        if self.x_reg != 0:
            self.kkt[range(ndx), range(ndx)] += self.x_reg
        if self.u_reg != 0:
            self.kkt[range(ndx, ndx + nu), range(ndx, ndx + nu)] += self.u_reg
        return self.cost
def calc ( self ) : xs , us = self . xs , self . us problem = self . problem self . cost = problem . calcDiff ( xs , us ) ix = 0 iu = 0 cx0 = problem . runningModels [ 0 ] . ndx np . fill_diagonal ( self . Fx , 1 ) for model , data , xguess in zip ( problem . runningModels , problem . runningDatas , xs [ 1 : ] ) : ndx = model . ndx nu = model . nu self . Lxx [ ix : ix + ndx , ix : ix + ndx ] = data . Lxx self . Lxu [ ix : ix + ndx , iu : iu + nu ] = data . Lxu self . Lux [ iu : iu + nu , ix : ix + ndx ] = data . Lxu . T self . Luu [ iu : iu + nu , iu : iu + nu ] = data . Luu self . Lx [ ix : ix + ndx ] = data . Lx self . Lu [ iu : iu + nu ] = data . Lu self . Fx [ cx0 + ix : cx0 + ix + ndx , ix : ix + ndx ] = - data . Fx self . Fu [ cx0 + ix : cx0 + ix + ndx , iu : iu + nu ] = - data . Fu self . cval [ cx0 + ix : cx0 + ix + ndx ] = model . State . diff ( data . xnext , xguess ) ix += ndx iu += nu model , data = problem . terminalModel , problem . terminalData ndx = model . ndx nu = model . nu self . Lxx [ ix : ix + ndx , ix : ix + ndx ] = data . Lxx self . Lx [ ix : ix + ndx ] = data . Lx self . cval [ : cx0 ] = problem . runningModels [ 0 ] . State . diff ( problem . initialState , xs [ 0 ] ) self . jacT [ : ] = self . jac . T ndx = sum ( [ m . ndx for m in self . models ( ) ] ) nu = sum ( [ m . nu for m in self . problem . runningModels ] ) if self . x_reg != 0 : self . kkt [ range ( ndx ) , range ( ndx ) ] += self . x_reg if self . u_reg != 0 : self . kkt [ range ( ndx , ndx + nu ) , range ( ndx , ndx + nu ) ] += self . u_reg return self . cost
For a given pair of candidate state and control trajectories,
compute the KKT system.
For a given pair of candidate state and control trajectories compute the KKT system .
'''
        For a given pair of candidate state and control trajectories,
        compute the KKT system.
        isFeasible should be set True if F(xs,us) = 0 (xs obtained from rollout).
        ''' # offset on constraint xnext = f(x,u) due to x0 = ref. # constraint value = xnext_guess - f(x_guess,u_guess) = diff(f,xnext_guesss) # data.F # constraint value = x_guess - x_ref = diff(x_ref,x_guess) # Regularization
self