[cost] Reverted back the g and L variables since they're usefull here

crocoddyl/cost.py

@@ -38,11 +38,14 @@ class CostDataPinocchio:
         ncost,nq,nv,nx,ndx,nu = model.ncost,model.nq,model.nv,model.nx,model.ndx,model.nu
         self.pinocchio = pinocchioData
         self.cost = np.nan
-        self.Lx = np.zeros(ndx)
-        self.Lu = np.zeros(nu)
-        self.Lxx = np.zeros([ndx,ndx])
-        self.Lxu = np.zeros([ndx,nu])
-        self.Luu = np.zeros([nu,nu])
+        self.g = np.zeros( ndx+nu)
+        self.L = np.zeros([ndx+nu,ndx+nu])
+
+        self.Lx = self.g[:ndx]
+        self.Lu = self.g[ndx:]
+        self.Lxx = self.L[:ndx,:ndx]
+        self.Lxu = self.L[:ndx,ndx:]
+        self.Luu = self.L[ndx:,ndx:]
 
         self.Lq  = self.Lx [:nv]
         self.Lqq = self.Lxx[:nv,:nv]
@@ -97,10 +100,7 @@ class CostModelNumDiff(CostModelPinocchio):
             if model.withGaussApprox:
                 data.Ru[:,iu] = (data.datau[iu].residuals-data.data0.residuals)/h
         if model.withGaussApprox:
-            L = np.dot(data.R.T,data.R)
-            data.Lxx[:] = L[:ndx,:ndx]
-            data.Lxu[:] = L[:ndx,ndx:]
-            data.Luu[:] = L[ndx:,ndx:]
+            data.L[:,:] = np.dot(data.R.T,data.R)
 
 class CostDataNumDiff(CostDataPinocchio):
     def __init__(self,model,pinocchioData):
@@ -154,11 +154,8 @@ class CostModelSum(CostModelPinocchio):
         return data.cost
     def calcDiff(model,data,x,u,recalc=True):
         if recalc: model.calc(data,x,u)
-        data.Lx.fill(0)
-        data.Lu.fill(0)
-        data.Lxx.fill(0)
-        data.Lxu.fill(0)
-        data.Luu.fill(0)
+        data.g.fill(0)
+        data.L.fill(0)
         nr = 0
         for m,d in zip(model.costs.values(),data.costs.values()):
             m.cost.calcDiff(d,x,u,recalc=False)