diff --git a/crocoddyl/__init__.py b/crocoddyl/__init__.py index cd04ca23e3d608531d381efd18b84a67609c6a32..7794b6e2ab2b2f9a7f7c4e38907228d543e45c4e 100644 --- a/crocoddyl/__init__.py +++ b/crocoddyl/__init__.py @@ -19,7 +19,7 @@ from action import ActionDataLQR, ActionModelLQR from action import ActionDataNumDiff, ActionModelNumDiff from integrated_action import IntegratedActionDataEuler, IntegratedActionModelEuler from integrated_action import IntegratedActionDataRK4, IntegratedActionModelRK4 -from differential_action import DifferentialActionModelAbstract +from differential_action import DifferentialActionDataAbstract, DifferentialActionModelAbstract from differential_action import DifferentialActionDataFullyActuated, DifferentialActionModelFullyActuated from differential_action import DifferentialActionDataLQR, DifferentialActionModelLQR from differential_action import DifferentialActionDataNumDiff, DifferentialActionModelNumDiff diff --git a/crocoddyl/differential_action.py b/crocoddyl/differential_action.py index c2ab14e52821b5ef4d1cc9b25f9dd3c1a00af711..8e00522d5b04b296f50621a207f9f0cd8e47c387 100644 --- a/crocoddyl/differential_action.py +++ b/crocoddyl/differential_action.py @@ -66,6 +66,42 @@ class DifferentialActionModelAbstract: raise NotImplementedError("Not implemented yet.") +class DifferentialActionDataAbstract: + def __init__(self, model, costData = None): + nx,nu,ndx,nv,nout = model.nx,model.nu,model.ndx,model.nv,model.nout + # State evolution and cost data + self.cost = np.nan + self.xout = np.zeros(nout) + + # Dynamics data + self.F = np.zeros([ nout,ndx+nu ]) + self.Fx = self.F[:,:ndx] + self.Fu = self.F[:,-nu:] + + # 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:] + 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 + self.Lxu = self.costs.Lxu + self.Luu = self.costs.Luu + self.Rx = self.costs.Rx + self.Ru = self.costs.Ru + + + class DifferentialActionModelFullyActuated(DifferentialActionModelAbstract): def __init__(self, pinocchioModel, costModel): DifferentialActionModelAbstract.__init__( @@ -126,26 +162,11 @@ class DifferentialActionModelFullyActuated(DifferentialActionModelAbstract): model.costs.calcDiff(data.costs,x,u,recalc=False) return data.xout,data.cost -class DifferentialActionDataFullyActuated: - def __init__(self,model): +class DifferentialActionDataFullyActuated(DifferentialActionDataAbstract): + def __init__(self, model): self.pinocchio = model.pinocchio.createData() - self.costs = model.costs.createData(self.pinocchio) - self.cost = np.nan - nu,ndx,nout = model.nu,model.State.ndx,model.nout - self.xout = np.zeros(nout) - self.F = np.zeros([ nout,ndx+nu ]) - 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 - self.Lxu = self.costs.Lxu - self.Luu = self.costs.Luu - self.Rx = self.costs.Rx - self.Ru = self.costs.Ru + costData = model.costs.createData(self.pinocchio) + DifferentialActionDataAbstract.__init__(self, model, costData) class DifferentialActionModelLQR(DifferentialActionModelAbstract): @@ -195,34 +216,17 @@ class DifferentialActionModelLQR(DifferentialActionModelAbstract): return data.xout,data.cost -class DifferentialActionDataLQR: - def __init__(self,model): - nx,nu,ndx,nv,nout = model.nx,model.nu,model.State.ndx,model.nv,model.nout - # State evolution and cost data - self.cost = np.nan - self.xout = np.zeros(nout) - - # Dynamics data - self.F = np.zeros([ nout,ndx+nu ]) - self.Fx = self.F[:,:ndx] - self.Fu = self.F[:,-nu:] - - # Cost data - 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:] - - # Setting the linear model and quadratic cost here because they are constant - self.Fx[:,:model.nv] = model.B - self.Fx[:,model.nv:] = model.A - self.Fu[:,:] = model.C - np.copyto(self.Lxx, model.Q+model.Q.T) - np.copyto(self.Lxu, np.zeros((nx, nu))) - np.copyto(self.Luu, model.U+model.U.T) +class DifferentialActionDataLQR(DifferentialActionDataAbstract): + def __init__(self,model): + DifferentialActionDataAbstract.__init__(self, model) + + # Setting the linear model and quadratic cost here because they are constant + self.Fx[:,:model.nv] = model.B + self.Fx[:,model.nv:] = model.A + self.Fu[:,:] = model.C + np.copyto(self.Lxx, model.Q+model.Q.T) + np.copyto(self.Lxu, np.zeros((model.nx, model.nu))) + np.copyto(self.Luu, model.U+model.U.T) class DifferentialActionModelNumDiff(DifferentialActionModelAbstract): diff --git a/crocoddyl/floating_contact.py b/crocoddyl/floating_contact.py index 006e3c58ba265ba3dc9ceb7ae39448e4bf786093..bf411fb40dbd6e9ce020a912d12bdfbae086f6d1 100644 --- a/crocoddyl/floating_contact.py +++ b/crocoddyl/floating_contact.py @@ -1,4 +1,4 @@ -from differential_action import DifferentialActionModelAbstract +from differential_action import DifferentialActionDataAbstract, DifferentialActionModelAbstract from state import StatePinocchio from utils import a2m, m2a import numpy as np @@ -107,36 +107,24 @@ class DifferentialActionModelFloatingInContact(DifferentialActionModelAbstract): return data.xout,data.cost def quasiStatic(self,data,x): - nx,ndx,nu,nq,nv,nc = self.nx,self.State.ndx,self.nu,self.nq,self.nv,self.ncontact - if len(x)==self.nq: x = np.concatenate([x,np.zero(nv)]) + nu,nq,nv = self.nx,self.nu,self.nq,self.nv + if len(x)==nq: x = np.concatenate([x,np.zero(nv)]) else: x[nq:] = 0 self.calcDiff(data,x,np.zeros(nu)) return np.dot( np.linalg.pinv(np.hstack([data.actuation.Au,data.contact.J.T])), -data.r[:nv])[:nu] -class DifferentialActionDataFloatingInContact: + +class DifferentialActionDataFloatingInContact(DifferentialActionDataAbstract): def __init__(self,model): self.pinocchio = model.pinocchio.createData() self.actuation = model.actuation.createData(self.pinocchio) self.contact = model.contact.createData(self.pinocchio) - self.costs = model.costs.createData(self.pinocchio) - self.cost = np.nan - nu,ndx,nv,nout,nc = model.nu,model.State.ndx,model.nv,model.nout,model.ncontact - self.F = np.zeros([ nout,ndx+nu ]) - 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 - self.Lxu = self.costs.Lxu - self.Luu = self.costs.Luu - self.Rx = self.costs.Rx - self.Ru = self.costs.Ru + costData = model.costs.createData(self.pinocchio) + DifferentialActionDataAbstract.__init__(self, model, costData) + nu,ndx,nv,nc = model.nu,model.ndx,model.nv,model.ncontact self.tauq = np.zeros(nv) self.K = np.zeros([nv+nc, nv+nc]) # KKT matrix = [ MJ.T ; J0 ] self.r = np.zeros( nv+nc ) # NLE effects = [ tau-b ; -gamma ]