diff --git a/crocoddyl/differential_action.py b/crocoddyl/differential_action.py index fa54ac2dd2cdc8e8265e305074e7a0391803f294..66f3dc2c9fb5b40faa0df8a5aa7ca9624bdf5d51 100644 --- a/crocoddyl/differential_action.py +++ b/crocoddyl/differential_action.py @@ -15,6 +15,15 @@ class DifferentialActionModelAbstract: the dynamics, cost functions and their derivatives. These computations are mainly carry on inside calc() and calcDiff(), respectively. """ + def __init__(self, nq, nv, nu): + self.nq = nq + self.nv = nv + self.nu = nu + self.nx = nq + nv + self.ndx = 2 * nv + self.nout = nv + self.unone = np.zeros(self.nu) + def createData(self): """ Created the differential action data. @@ -24,6 +33,7 @@ class DifferentialActionModelAbstract: :return DAM data. """ raise NotImplementedError("Not implemented yet.") + def calc(model, data, x, u=None): """ Compute the state evolution and cost value. @@ -38,6 +48,7 @@ class DifferentialActionModelAbstract: :param u: control input """ raise NotImplementedError("Not implemented yet.") + def calcDiff(model, data, x, u=None, recalc=True): """ Compute the derivatives of the dynamics and cost functions. @@ -55,18 +66,13 @@ class DifferentialActionModelAbstract: raise NotImplementedError("Not implemented yet.") - class DifferentialActionModelFullyActuated(DifferentialActionModelAbstract): def __init__(self, pinocchioModel, costModel): + DifferentialActionModelAbstract.__init__( + self, pinocchioModel.nq, pinocchioModel.nv, pinocchioModel.nv) self.pinocchio = pinocchioModel self.State = StatePinocchio(self.pinocchio) self.costs = costModel - self.nq,self.nv = self.pinocchio.nq, self.pinocchio.nv - self.nx = self.State.nx - self.ndx = self.State.ndx - self.nout = self.nv - self.nu = self.nv - self.unone = np.zeros(self.nu) # Use this to force the computation with ABA # Side effect is that armature is not used. self.forceAba = False @@ -157,18 +163,9 @@ class DifferentialActionModelLQR(DifferentialActionModelAbstract): On the other hand the cost function is given by l(x,u) = x^T*Q*x + u^T*U*u """ - def __init__(self,nq,nu): - self.nq,self.nv = nq, nq - self.nx = 2*self.nq - self.ndx = self.nx - self.nout = self.nv - self.nu = nu - self.unone = np.zeros(self.nu) + def __init__(self, nq, nu): + DifferentialActionModelAbstract.__init__(self, nq, nq, nu) self.State = StateVector(self.nx) - self.nx = self.State.nx - self.ndx = self.State.ndx - self.nu = nu - self.unone = np.zeros(self.nu) v1 = randomOrthonormalMatrix(self.nq) v2 = randomOrthonormalMatrix(self.nq) diff --git a/crocoddyl/floating_contact.py b/crocoddyl/floating_contact.py index 25d976e2554b99886720c8f086697600f067197e..09721e0f50ac75cb0f271499aeeebeb42f45e551 100644 --- a/crocoddyl/floating_contact.py +++ b/crocoddyl/floating_contact.py @@ -8,17 +8,13 @@ import pinocchio class DifferentialActionModelFloatingInContact(DifferentialActionModelAbstract): def __init__(self,pinocchioModel,actuationModel,contactModel,costModel): + DifferentialActionModelAbstract.__init__( + self, pinocchioModel.nq, pinocchioModel.nv, actuationModel.nu) self.pinocchio = pinocchioModel self.State = StatePinocchio(self.pinocchio) self.actuation = actuationModel self.contact = contactModel self.costs = costModel - self.nq,self.nv = self.pinocchio.nq, self.pinocchio.nv - self.nx = self.State.nx - self.ndx = self.State.ndx - self.nout = self.nv - self.nu = self.actuation.nu - self.unone = np.zeros(self.nu) @property def ncost(self): return self.costs.ncost @property