diff --git a/crocoddyl/actuation.py b/crocoddyl/actuation.py
index 1257bd0e6828a3e2bb6994d93417abde80e7ca8b..4411db63aa38fae834f64981c66fc556adde0f81 100644
--- a/crocoddyl/actuation.py
+++ b/crocoddyl/actuation.py
@@ -141,8 +141,6 @@ class DifferentialActionDataActuated:
self.costResiduals = self.costs.residuals
self.Fx = self.F[:,:ndx]
self.Fu = self.F[:,-nu:]
- self.g = self.costs.g
- self.L = self.costs.L
self.Lx = self.costs.Lx
self.Lu = self.costs.Lu
self.Lxx = self.costs.Lxx
diff --git a/crocoddyl/cost.py b/crocoddyl/cost.py
index 5c4882900e03531ab116ef62de88c529648f2ce6..3eb3b933c807c8dc78b7f28c60cc2ed8179545c6 100644
--- a/crocoddyl/cost.py
+++ b/crocoddyl/cost.py
@@ -38,15 +38,11 @@ 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.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.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.Lq = self.Lx [:nv]
self.Lqq = self.Lxx[:nv,:nv]
@@ -101,7 +97,10 @@ class CostModelNumDiff(CostModelPinocchio):
if model.withGaussApprox:
data.Ru[:,iu] = (data.datau[iu].residuals-data.data0.residuals)/h
if model.withGaussApprox:
- data.L[:,:] = np.dot(data.R.T,data.R)
+ L = np.dot(data.R.T,data.R)
+ data.Lxx[:] = L[:ndx,:ndx]
+ data.Lxu[:] = L[:ndx,ndx:]
+ data.Luu[:] = L[ndx:,ndx:]
class CostDataNumDiff(CostDataPinocchio):
def __init__(self,model,pinocchioData):
@@ -155,8 +154,11 @@ class CostModelSum(CostModelPinocchio):
return data.cost
def calcDiff(model,data,x,u,recalc=True):
if recalc: model.calc(data,x,u)
- data.g.fill(0)
- data.L.fill(0)
+ data.Lx.fill(0)
+ data.Lu.fill(0)
+ data.Lxx.fill(0)
+ data.Lxu.fill(0)
+ data.Luu.fill(0)
nr = 0
for m,d in zip(model.costs.values(),data.costs.values()):
m.cost.calcDiff(d,x,u,recalc=False)
diff --git a/crocoddyl/differential_action.py b/crocoddyl/differential_action.py
index 8e00522d5b04b296f50621a207f9f0cd8e47c387..10beb076e6b1d775c686f0233a16241c48865053 100644
--- a/crocoddyl/differential_action.py
+++ b/crocoddyl/differential_action.py
@@ -80,18 +80,14 @@ class DifferentialActionDataAbstract:
# Cost data
if costData == None:
- 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.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])
else:
self.costs = costData
self.costResiduals = self.costs.residuals
- self.g = self.costs.g
- self.L = self.costs.L
self.Lx = self.costs.Lx
self.Lu = self.costs.Lu
self.Lxx = self.costs.Lxx
diff --git a/crocoddyl/impact.py b/crocoddyl/impact.py
index 958729705cfe867ba74360414ed396176ed0177d..36491bf1989034a263659985f820e13e8edcf932 100644
--- a/crocoddyl/impact.py
+++ b/crocoddyl/impact.py
@@ -450,8 +450,6 @@ class ActionDataImpact:
self.Rv = self.Rx[:,nv:]
self.costResiduals = self.costs.residuals
- self.g = self.costs.g
- self.L = self.costs.L
self.Lx = self.costs.Lx
self.Lu = self.costs.Lu
self.Lxx = self.costs.Lxx
diff --git a/crocoddyl/integrated_action.py b/crocoddyl/integrated_action.py
index 3b596c563e2ef5abfe2bf2c2ea2e2590ead29a0d..d92ec62ba3074e402c757914d7cbba4f8de215f4 100644
--- a/crocoddyl/integrated_action.py
+++ b/crocoddyl/integrated_action.py
@@ -39,8 +39,11 @@ class IntegratedActionModelEuler:
data.Fx[:,:] = dxnext_dx + dt*np.dot(dxnext_ddx,ddx_dx)
ddx_du = np.vstack([ da_du*dt, da_du ])
data.Fu[:,:] = dt*np.dot(dxnext_ddx,ddx_du)
- data.g[:] = data.differential.g
- data.L[:] = data.differential.L
+ data.Lx[:] = data.differential.Lx
+ data.Lu[:] = data.differential.Lu
+ data.Lxx[:] = data.differential.Lxx
+ data.Lxu[:] = data.differential.Lxu
+ data.Luu[:] = data.differential.Luu
class IntegratedActionDataEuler:
""" Implement the RK4 integration scheme and its derivatives.
@@ -60,12 +63,11 @@ class IntegratedActionDataEuler:
self.cost = np.nan
self.costResiduals = np.zeros([ ncost ])
- self.Lxx = self.L[:ndx,:ndx]
- self.Lxu = self.L[:ndx,ndx:]
- self.Lux = self.L[ndx:,:ndx]
- self.Luu = self.L[ndx:,ndx:]
- self.Lx = self.g[:ndx]
- self.Lu = self.g[ndx:]
+ 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.Fx = self.F[:,:ndx]
self.Fu = self.F[:,ndx:]
self.Rx = self.R[:,:ndx]
@@ -246,18 +248,15 @@ class IntegratedActionDataRK4:
self.l = [np.nan,]*4
self.ki = [np.zeros([ndx]),]*4
- self.g = np.zeros([ ndx+nu ])
- self.L = np.zeros([ ndx+nu,ndx+nu ])
self.F = np.zeros([ ndx ,ndx+nu ])
self.xnext = np.zeros([ nx ])
self.cost = np.nan
- self.Lxx = self.L[:ndx,:ndx]
- self.Lxu = self.L[:ndx,ndx:]
- self.Lux = self.L[ndx:,:ndx]
- self.Luu = self.L[ndx:,ndx:]
- self.Lx = self.g[:ndx]
- self.Lu = self.g[ndx:]
+ 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.Fx = self.F[:,:ndx]
self.Fu = self.F[:,ndx:]
diff --git a/crocoddyl/unicycle.py b/crocoddyl/unicycle.py
index a2dcc2011e59d201f38290ec0c35d4fc8ebdb7c9..e9131ea20d26b6db322c748581a3d329f5d17049 100644
--- a/crocoddyl/unicycle.py
+++ b/crocoddyl/unicycle.py
@@ -221,8 +221,13 @@ class ActionModelUnicycleVar:
### Cost derivatives
data.R[:ndx,:ndx] = model.costWeights[0] * model.State.Jdiff(model.xref,x,'second')
data.R[ndx:,ndx:] = np.diag([ model.costWeights[1] ]*nu )
- data.g[:] = np.dot(data.R.T,data.costResiduals)
- data.L[:,:] = np.dot(data.R.T,data.R)
+ g = np.dot(data.R.T,data.costResiduals)
+ L = np.dot(data.R.T,data.R)
+ data.Lx[:] = g[:ndx]
+ data.Lu[:] = g[ndx:]
+ data.Lxx[:] = L[:ndx,:ndx]
+ data.Lxu[:] = L[:ndx,ndx:]
+ data.Luu[:] = L[ndx:,ndx:]
### Dynamic derivatives
dx = np.array([u[0],0,u[1]])*model.dt
diff --git a/unittest/test_costs.py b/unittest/test_costs.py
index cbdfa26eaf69aa8b0fece6f498edcd189cfadaec..5eedc2b283d925001f4dc69898a12d7919d7d999 100644
--- a/unittest/test_costs.py
+++ b/unittest/test_costs.py
@@ -45,8 +45,11 @@ costDataND = costModelND.createData(rdata)
costModelND.calcDiff(costDataND,x,u)
-assert( absmax(costData.g-costDataND.g) < 1e-3 )
-assert( absmax(costData.L-costDataND.L) < 1e-3 )
+assert( absmax(costData.Lx-costDataND.Lx) < 1e-4 )
+assert( absmax(costData.Lu-costDataND.Lu) < 1e-4 )
+assert( absmax(costData.Lxx-costDataND.Lxx) < 1e-4 )
+assert( absmax(costData.Lxu-costDataND.Lxu) < 1e-4 )
+assert( absmax(costData.Luu-costDataND.Luu) < 1e-4 )
@@ -77,8 +80,11 @@ costDataND = costModelND.createData(rdata)
costModelND.calcDiff(costDataND,x,u)
-assert( absmax(costData.g-costDataND.g) < 1e-4 )
-assert( absmax(costData.L-costDataND.L) < 1e-4 )
+assert( absmax(costData.Lx-costDataND.Lx) < 1e-4 )
+assert( absmax(costData.Lu-costDataND.Lu) < 1e-4 )
+assert( absmax(costData.Lxx-costDataND.Lxx) < 1e-4 )
+assert( absmax(costData.Lxu-costDataND.Lxu) < 1e-4 )
+assert( absmax(costData.Luu-costDataND.Luu) < 1e-4 )
@@ -112,8 +118,11 @@ costDataND = costModelND.createData(rdata)
costModelND.calcDiff(costDataND,x,u)
-assert( absmax(costData.g-costDataND.g) < 1e-4 )
-assert( absmax(costData.L-costDataND.L) < 1e-4 )
+assert( absmax(costData.Lx-costDataND.Lx) < 1e-4 )
+assert( absmax(costData.Lu-costDataND.Lu) < 1e-4 )
+assert( absmax(costData.Lxx-costDataND.Lxx) < 1e-4 )
+assert( absmax(costData.Lxu-costDataND.Lxu) < 1e-4 )
+assert( absmax(costData.Luu-costDataND.Luu) < 1e-4 )
@@ -142,8 +151,11 @@ costDataND = costModelND.createData(rdata)
costModelND.calcDiff(costDataND,x,u)
-assert( absmax(costData.g-costDataND.g) < 1e-4 )
-assert( absmax(costData.L-costDataND.L) < 1e-4 )
+assert( absmax(costData.Lx-costDataND.Lx) < 1e-4 )
+assert( absmax(costData.Lu-costDataND.Lu) < 1e-4 )
+assert( absmax(costData.Lxx-costDataND.Lxx) < 1e-4 )
+assert( absmax(costData.Lxu-costDataND.Lxu) < 1e-4 )
+assert( absmax(costData.Luu-costDataND.Luu) < 1e-4 )
@@ -165,8 +177,11 @@ costModelND = CostModelNumDiff(costModel,X,withGaussApprox=True,
costDataND = costModelND.createData(rdata)
costModelND.calcDiff(costDataND,x,u)
-assert( absmax(costData.g-costDataND.g) < 1e-3 )
-#assert( absmax(costData.L-costDataND.L) < 1e-3 )
+assert( absmax(costData.Lx-costDataND.Lx) < 1e-3 )
+assert( absmax(costData.Lu-costDataND.Lu) < 1e-3 )
+# assert( absmax(costData.Lxx-costDataND.Lxx) < 1e-3 )
+# assert( absmax(costData.Lxu-costDataND.Lxu) < 1e-3 )
+# assert( absmax(costData.Luu-costDataND.Luu) < 1e-3 )
# --------------------------------------------------------------
from crocoddyl import ActivationModelInequality, ActivationModelInequality
@@ -195,7 +210,8 @@ costDataND = costModelND.createData(rdata)
costModelND.calcDiff(costDataND,x,u)
-assert( absmax(costData.g-costDataND.g) < 1e-3 )
+assert( absmax(costData.Lx-costDataND.Lx) < 1e-4 )
+assert( absmax(costData.Lu-costDataND.Lu) < 1e-4 )
#Check that the cost derivative is zero if q>=lower and q<=upper
#and that cost is positive if q<lower or q>upper
lowersafe = m2a(x)>=lowerLimit; uppersafe = m2a(x)<=upperLimit
@@ -224,7 +240,8 @@ costDataND = costModelND.createData(rdata)
costModelND.calcDiff(costDataND,x,u)
-assert( absmax(costData.g-costDataND.g) < 1e-3 )
+assert( absmax(costData.Lx-costDataND.Lx) < 1e-4 )
+assert( absmax(costData.Lu-costDataND.Lu) < 1e-4 )
#Check that the cost derivative is zero if q>=lower and q<=upper
#and that cost is positive if q<lower or q>upper
lowersafe = m2a(x)>=lowerLimit; uppersafe = m2a(x)<=upperLimit
@@ -251,8 +268,11 @@ costModelND = CostModelNumDiff(costModel,StatePinocchio(rmodel),withGaussApprox=
costDataND = costModelND.createData(rdata)
costModelND.calcDiff(costDataND,x,u)
-assert( absmax(costData.g-costDataND.g) < 1e-3 )
-assert( absmax(costData.L-costDataND.L) < 1e-3 )
+assert( absmax(costData.Lx-costDataND.Lx) < 1e-4 )
+assert( absmax(costData.Lu-costDataND.Lu) < 1e-4 )
+assert( absmax(costData.Lxx-costDataND.Lxx) < 1e-4 )
+assert( absmax(costData.Lxu-costDataND.Lxu) < 1e-4 )
+assert( absmax(costData.Luu-costDataND.Luu) < 1e-4 )
@@ -291,8 +311,11 @@ costModelND = CostModelNumDiff(costModel,StatePinocchio(rmodel),withGaussApprox=
costDataND = costModelND.createData(rdata)
costModelND.calcDiff(costDataND,x,u)
-assert( absmax(costData.g-costDataND.g) < 1e-3 )
-assert( absmax(costData.L-costDataND.L) < 1e-3 )
+assert( absmax(costData.Lx-costDataND.Lx) < 1e-4 )
+assert( absmax(costData.Lu-costDataND.Lu) < 1e-4 )
+assert( absmax(costData.Lxx-costDataND.Lxx) < 1e-4 )
+assert( absmax(costData.Lxu-costDataND.Lxu) < 1e-4 )
+assert( absmax(costData.Luu-costDataND.Luu) < 1e-4 )