WIP

crocoddyl/__init__.py

@@ -3,7 +3,8 @@ from .action import (ActionDataAbstract, ActionDataLQR, ActionDataNumDiff, Actio
 from .activation import (ActivationDataInequality, ActivationDataQuad, ActivationDataSmoothAbs,
                          ActivationDataWeightedQuad, ActivationModelInequality, ActivationModelQuad,
                          ActivationModelSmoothAbs, ActivationModelWeightedQuad)
-from .actuation import ActuationDataFreeFloating, ActuationDataFull, ActuationModelFreeFloating, ActuationModelFull, ActuationModelUAM
+from .actuation import (ActuationDataFreeFloating, ActuationDataFull, ActuationDataUAM, ActuationDataDoublePendulum,
+                        ActuationModelFreeFloating, ActuationModelFull, ActuationModelUAM, ActuationModelDoublePendulum)
 from .box_ddp import SolverBoxDDP
 from .box_kkt import SolverBoxKKT
 from .callbacks import CallbackDDPLogger, CallbackDDPVerbose, CallbackSolverDisplay, CallbackSolverTimer
@@ -14,7 +15,7 @@ from .cost import (CostDataCoM, CostDataControl, CostDataForce, CostDataFramePla
                    CostDataState, CostDataSum, CostModelCoM, CostModelControl, CostModelForce,
                    CostModelForceLinearCone, CostModelFramePlacement, CostModelFrameRotation,
                    CostModelFrameTranslation, CostModelFrameVelocity, CostModelFrameVelocityLinear, CostModelNumDiff,
-                   CostModelPinocchio, CostModelState, CostModelSum)
+                   CostModelPinocchio, CostModelState, CostModelSum, CostModelDoublePendulum, CostDataDoublePendulum)
 from .ddp import SolverDDP
 from .diagnostic import displayTrajectory, plotDDPConvergence, plotOCSolution
 from .differential_action import (DifferentialActionDataAbstract, DifferentialActionDataFullyActuated,

crocoddyl/actuation.py

@@ -2,6 +2,41 @@ import warnings
 
 import numpy as np
 
+class ActuationModelDoublePendulum:
+    def __init__(self, pinocchioModel):
+        self.pinocchio = pinocchioModel
+        self.nq = pinocchioModel.nq
+        self.nv = pinocchioModel.nv
+        self.nx = self.nq + self.nv
+        self.ndx = self.nv * 2
+        self.nu = 1
+
+    def calc(self, data, x, u):
+        S = np.zeros([self.nv,self.nu])
+        #S[1] = 1
+        S[0] = 1
+        data.a = np.dot(S,u)
+        return data.a
+
+    def calcDiff(self, data, x, u, recalc=True):
+        if recalc:
+            self.calc(data, x, u)
+        return data.a
+
+    def createData(self, pinocchioData):
+        return ActuationDataDoublePendulum(self, pinocchioData)
+
+class ActuationDataDoublePendulum:
+    def __init__(self, model, pinocchioData):
+        self.pinocchio = pinocchioData
+        ndx, nv, nu = model.ndx, model.nv, model.nu
+        self.a = np.zeros(nv)  # result of calc
+        self.A = np.zeros([nv, ndx + nu])  # result of calcDiff
+        self.Ax = self.A[:, :ndx]
+        self.Au = self.A[:, ndx:]
+        # self.Au[1,0] = 1
+        self.Au[0,0] = 1
+
 class ActuationModelUAM:
     '''
     This model transforms an actuation u into a joint torque tau.

crocoddyl/cost.py

@@ -560,7 +560,11 @@ class CostModelControl(CostModelPinocchio):
         # data.Ru[:,:] = np.eye(nu)
         Ax, Axx = self.activation.calcDiff(data.activation, data.residuals, recalc=recalc)
         data.Lu[:] = Ax
-        data.Luu[:, :] = np.diag(m2a(Axx))
+        if type(Axx) is np.ndarray:
+            data.Luu[:, :] = np.diag(Axx)
+        else:
+            data.Luu[:, :] = np.diag(m2a(Axx))
+        #data.Luu[:, :] = np.diag(m2a(Axx))
 
 
 class CostDataControl(CostDataPinocchio):
@@ -676,3 +680,54 @@ class CostDataForceCone(CostDataPinocchio):
         CostDataPinocchio.__init__(self, model, pinocchioData)
         self.contact = contactData
         self.activation = model.activation.createData()
+
+class CostModelDoublePendulum(CostModelPinocchio):
+    def __init__(self, pinocchioModel, frame, ref, nu=None, activation=None):
+        self.CostDataType = CostDataDoublePendulum
+        CostModelPinocchio.__init__(self, pinocchioModel, ncost=6, nu=nu)
+        self.ref = ref
+        self.frame = frame
+        self.activation = activation if activation is not None else ActivationModelQuad()
+
+    def calc(self, data, x, u):
+        c1 = np.cos(x[0])
+        c2 = np.cos(x[1])
+        s1 = np.sin(x[0])
+        s2 = np.sin(x[1])
+        data.residuals = np.array([s1, s2, 1-c1, 1-c2, x[2], x[3]])
+        data.cost = sum(self.activation.calc(data.activation, data.residuals))
+        return data.cost
+
+    def calcDiff(self, data, x, u, recalc=True):
+        if recalc:
+            self.calc(data, x, u)
+        c1 = np.cos(x[0])
+        c2 = np.cos(x[1])
+        s1 = np.sin(x[0])
+        s2 = np.sin(x[1])
+        nq = self.nq
+        Ax, Axx = self.activation.calcDiff(data.activation, data.residuals, recalc=recalc)
+        J = np.zeros([6, 4])
+        J[:2,:2] = np.diag([c1, c2])
+        J[2:4,:2] = np.diag([s1, s2])
+        J[4:6,2:4] = np.diag([1,1])
+        data.Lx[:] = np.dot(np.transpose(J), Ax)
+        H = np.zeros([6, 4])
+        H[:2,:2] = np.diag([c1**2-s1**2, c2**2-s2**2])
+        H[2:4,:2] = np.diag([s1**2+(1-c1)*c1, s2**2+(1-c2)*c2])
+        J[4:6,2:4] = np.diag([1,1])
+        data.Lxx[:, :] = np.dot(np.transpose(H), Axx)
+        return data.cost
+
+
+class CostDataDoublePendulum(CostDataPinocchio):
+    def __init__(self, model, pinocchioData):
+        CostDataPinocchio.__init__(self, model, pinocchioData)
+        self.activation = model.activation.createData()
+        self.Lu = 0
+        self.Lv = 0
+        self.Lxu = 0
+        self.Luu = 0
+        self.Lvv = 0
+        self.Ru = 0
+        self.Rv = 0

crocoddyl/double_pendulum.py

+import numpy as np
+import pinocchio
+
+from .differential_action import DifferentialActionDataAbstract, DifferentialActionModelAbstract
+from .state import StatePinocchio
+from .utils import a2m, m2a
+
+class DifferentialActionModelDoublePendulum(DifferentialActionModelAbstract):
+    def __init__(self, pinocchioModel, actuationModel, costModel):
+        DifferentialActionModelAbstract.__init__(self, pinocchioModel.nq, pinocchioModel.nv, actuationModel.nu)
+        self.DifferentialActionDataType = DifferentialActionDataDoublePendulum
+        self.pinocchio = pinocchioModel
+        self.State = StatePinocchio(self.pinocchio)
+        self.actuation = actuationModel
+        self.costs = costModel
+
+    @property
+    def ncost(self):
+        return self.costs.ncost
+
+    def calc(self, data, x, u=None):
+        if u is None:
+            u = self.unone
+        nq, nv = self.nq, self.nv
+        q = a2m(x[:nq])
+        v = a2m(x[-nv:])
+        tauq[:] = self.actuation.calc(data.actuation, x, u)
+
+        pinocchio.computeAllTerms(self.pinocchio, data.pinocchio, q, v)
+        data.M = data.pinocchio.M
+        data.Minv = np.linalg.inv(data.M)
+        data.r = data.tauq - m2a(data.pinocchio.nle)
+        data.xout[:] = np.dot(data.Minv, data.r)
+
+        # --- Cost
+        pinocchio.forwardKinematics(self.pinocchio, data.pinocchio, q, v)
+        pinocchio.updateFramePlacements(self.pinocchio, data.pinocchio)
+        data.cost = self.costs.calc(data.costs, x, u)
+        return data.xout, data.cost
+
+    def calcDiff(self, data, x, u=None, recalc=True):
+        if u is None:
+            u = self.unone
+        if recalc:
+            xout, cost = self.calc(data, x, u)
+        nq, nv = self.nq, self.nv
+        q = a2m(x[:nq])
+        v = a2m(x[-nv:])
+        tauq = a2m(u)
+        a = a2m(data.xout)
+        # --- Dynamics
+        if self.forceAba:
+            pinocchio.computeABADerivatives(self.pinocchio, data.pinocchio, q, v, tauq)
+            data.Fx[:, :nv] = data.pinocchio.ddq_dq
+            data.Fx[:, nv:] = data.pinocchio.ddq_dv
+            data.Fu[:, :] = data.pinocchio.Minv
+        else:
+            pinocchio.computeRNEADerivatives(self.pinocchio, data.pinocchio, q, v, a)
+            data.Fx[:, :nv] = -np.dot(data.Minv, data.pinocchio.dtau_dq)
+            data.Fx[:, nv:] = -np.dot(data.Minv, data.pinocchio.dtau_dv)
+            data.Fu[:, :] = data.Minv
+        # --- Cost
+        pinocchio.computeJointJacobians(self.pinocchio, data.pinocchio, q)
+        pinocchio.updateFramePlacements(self.pinocchio, data.pinocchio)
+        self.costs.calcDiff(data.costs, x, u, recalc=False)
+        return data.xout, data.cost
+
+
+class DifferentialActionDataDoublePendulum(DifferentialActionDataAbstract):
+    def __init__(self, model):
+        self.pinocchio = model.pinocchio.createData()
+        costData = model.costs.createData(self.pinocchio)
+        DifferentialActionDataAbstract.__init__(self, model, costData)

crocoddyl/flying.py

@@ -78,7 +78,6 @@ class DifferentialActionDataUAM(DifferentialActionDataAbstract):
         self.pinocchio = model.pinocchio.createData()
         self.actuation = model.actuation.createData(self.pinocchio)
         costData = model.costs.createData(self.pinocchio)
-        # print self.costs.Lu
         DifferentialActionDataAbstract.__init__(self, model, costData)
 
         nv = model.nv

crocoddyl/robots.py

@@ -145,6 +145,6 @@ def loadKintonArm(modelPath='/opt/openrobots/share/example-robot-data'):
 def loadBorinotArm(modelPath='/opt/openrobots/share/example-robot-data'):
     URDF_FILENAME = "borinot_arm.urdf"
     URDF_SUBPATH = "/borinot_arm/urdf/" + URDF_FILENAME
-    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
-    robot.q0.flat = [np.pi,0]
+    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], None)
+    robot.q0.flat = [np.pi]
     return robot

examples/notebooks/cartpole_swing_up_sol.ipynb

@@ -400,7 +400,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython2",
-   "version": "2.7.15+"
+   "version": "2.7.12"
   }
  },
  "nbformat": 4,

examples/notebooks/kinton_fixed_base.ipynb

@@ -169,7 +169,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython2",
-   "version": "2.7.16"
+   "version": "2.7.12"
   }
  },
  "nbformat": 4,

examples/notebooks/kinton_flying_ee.ipynb

@@ -499,7 +499,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython2",
-   "version": "2.7.16"
+   "version": "2.7.12"
   }
  },
  "nbformat": 4,