add UAM compatibility

crocoddyl/__init__.py

@@ -3,7 +3,7 @@ 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
+from .actuation import ActuationDataFreeFloating, ActuationDataFull, ActuationModelFreeFloating, ActuationModelFull, ActuationModelUAM
 from .box_ddp import SolverBoxDDP
 from .box_kkt import SolverBoxKKT
 from .callbacks import CallbackDDPLogger, CallbackDDPVerbose, CallbackSolverDisplay, CallbackSolverTimer
@@ -23,12 +23,13 @@ from .differential_action import (DifferentialActionDataAbstract, DifferentialAc
                                   DifferentialActionModelLQR, DifferentialActionModelNumDiff)
 from .fddp import SolverFDDP
 from .floating_contact import DifferentialActionDataFloatingInContact, DifferentialActionModelFloatingInContact
+from .flying import DifferentialActionModelUAM, DifferentialActionDataUAM
 from .impact import (ActionDataImpact, ActionModelImpact, CostModelImpactCoM, CostModelImpactWholeBody, ImpulseData6D,
                      ImpulseDataPinocchio, ImpulseModel3D, ImpulseModel6D, ImpulseModelMultiple, ImpulseModelPinocchio)
 from .integrated_action import (IntegratedActionDataEuler, IntegratedActionDataRK4, IntegratedActionModelEuler,
                                 IntegratedActionModelRK4)
 from .kkt import SolverKKT
-from .robots import getTalosPathFromRos, loadHyQ, loadTalos, loadTalosArm, loadTalosLegs
+from .robots import getTalosPathFromRos, loadHyQ, loadTalos, loadTalosArm, loadTalosLegs, loadKinton, loadKintonArm
 from .shooting import ShootingProblem
 from .solver import SolverAbstract
 from .state import StateAbstract, StateNumDiff, StatePinocchio, StateVector

crocoddyl/actuation.py

@@ -2,6 +2,44 @@ import warnings
 
 import numpy as np
 
+class ActuationModelUAM:
+    '''
+    This model transforms an actuation u into a joint torque tau.
+    We implement here the simplest model: tau = S.T*u, where S is constant.
+    '''
+
+    def __init__(self, pinocchioModel):
+        self.pinocchio = pinocchioModel
+        if (pinocchioModel.joints[1].shortname() != 'JointModelFreeFlyer'):
+            warnings.warn('Strange that the first joint is not a freeflyer')
+        self.nq = pinocchioModel.nq
+        self.nv = pinocchioModel.nv
+        self.nx = self.nq + self.nv
+        self.ndx = self.nv * 2
+        self.nu = self.nv - 2
+
+    def calc(self, data, x, u):
+        data.a[2:] = 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 ActuationDataUAM(self, pinocchioData)
+
+
+class ActuationDataUAM:
+    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:]
+        np.fill_diagonal(self.Au[2:, :], 1)
 
 class ActuationModelFreeFloating:
     '''

crocoddyl/flying.py

+import numpy as np
+import pinocchio
+
+from .differential_action import DifferentialActionDataAbstract, DifferentialActionModelAbstract
+from .state import StatePinocchio
+from .utils import a2m, m2a
+
+
+class DifferentialActionModelUAM(DifferentialActionModelAbstract):
+    def __init__(self, pinocchioModel, actuationModel, costModel):
+        DifferentialActionModelAbstract.__init__(self, pinocchioModel.nq, pinocchioModel.nv, actuationModel.nu)
+        self.DifferentialActionDataType = DifferentialActionDataUAM
+        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:])
+
+        data.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)
+        # print data.Minv
+        # print data.tauq
+        # print data.pinocchio.nle
+        data.xout[:] = data.Minv * (a2m(data.tauq) - data.pinocchio.nle).flat
+
+        # --- 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:])
+        a = a2m(data.xout)
+
+        dtau_dx = data.actuation.Ax
+        dtau_du = data.actuation.Au
+
+        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.Fx += np.dot(data.Minv, dtau_dx)
+        data.Fu[:, :] = np.dot(data.Minv, dtau_du)
+        # 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 DifferentialActionDataUAM(DifferentialActionDataAbstract):
+    def __init__(self, model):
+        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
+        self.tauq = np.zeros(nv)
+        self.xout = np.zeros(nv)

crocoddyl/robots.py

@@ -125,3 +125,17 @@ def loadHyQ(modelPath='/opt/openrobots/share/example-robot-data'):
     robot.q0[2] = 0.57750958
     robot.model.referenceConfigurations["half_sitting"] = robot.q0
     return robot
+
+def loadKinton(modelPath='/opt/openrobots/share/example-robot-data'):
+    URDF_FILENAME = "kinton_arm.urdf"
+    URDF_SUBPATH = "/kinton_description/urdf/" + URDF_FILENAME
+    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
+    robot.q0.flat[7:] = [0, 0, 0, 0, 0, 0]
+    robot.model.referenceConfigurations["initial_pose"] = robot.q0
+    return robot
+
+def loadKintonArm(modelPath='/opt/openrobots/share/example-robot-data'):
+    URDF_FILENAME = "kinton_arm.urdf"
+    URDF_SUBPATH = "/kinton_description/urdf/" + URDF_FILENAME
+    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
+    return robot

examples/kinton_flying.py

+from crocoddyl import *
+import pinocchio as pin
+import numpy as np
+from crocoddyl.diagnostic import displayTrajectory
+
+# LOAD ROBOT
+robot = loadKinton()
+robot.initDisplay(loadModel=True)
+robot.display(robot.q0)
+
+robot.framesForwardKinematics(robot.q0)
+
+# DEFINE TARGET POSITION
+target_pos  = np.array([0,0,3])
+target_quat = pin.Quaternion(0, 0, 0, 1)
+target_quat.normalize()
+
+# Plot goal frame
+robot.viewer.gui.addXYZaxis('world/framegoal', [1., 0., 0., 1.], .015, 4)
+robot.viewer.gui.applyConfiguration('world/framegoal', target_pos.tolist() + [target_quat[0], target_quat[1], target_quat[2], target_quat[3]])
+robot.viewer.gui.refresh()
+
+# ACTUATION MODEL
+actModel = ActuationModelUAM(robot.model)
+
+# COST MODEL
+# Create a cost model per the running and terminal action model.
+runningCostModel = CostModelSum(robot.model, actModel.nu)
+terminalCostModel = CostModelSum(robot.model, actModel.nu)
+
+frameName = 'base_link'
+state = StatePinocchio(robot.model)
+SE3ref = pin.SE3()
+SE3ref.translation = target_pos.reshape(3,1)
+SE3ref.rotation = target_quat.matrix()
+
+
+stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] * (robot.model.nv - 2) + [10] * robot.model.nv)
+goalTrackingCost = CostModelFramePlacement(robot.model,
+                                           frame=robot.model.getFrameId(frameName),
+                                           ref=SE3ref,
+                                           nu =actModel.nu)
+xRegCost = CostModelState(robot.model, state, ref=state.zero(), nu=actModel.nu)
+uRegCost = CostModelControl(robot.model, nu=robot.model.nv-2)
+
+# Then let's add the running and terminal cost functions
+runningCostModel.addCost(name="pos", weight=1e-3, cost=goalTrackingCost)
+runningCostModel.addCost(name="regx", weight=1e-7, cost=xRegCost)
+runningCostModel.addCost(name="regu", weight=1e-7, cost=uRegCost)
+terminalCostModel.addCost(name="pos", weight=50, cost=goalTrackingCost)
+
+# DIFFERENTIAL ACTION MODEL
+runningModel = IntegratedActionModelEuler(DifferentialActionModelUAM(robot.model, actModel, runningCostModel))
+terminalModel = IntegratedActionModelEuler(DifferentialActionModelUAM(robot.model, actModel, terminalCostModel))
+
+# DEFINING THE SHOOTING PROBLEM & SOLVING
+
+# Defining the time duration for running action models and the terminal one
+dt = 1e-3
+runningModel.timeStep = dt
+
+# For this optimal control problem, we define 250 knots (or running action
+# models) plus a terminal knot
+T = 250
+q0 = [0., 0., 0., 0., 0., 0.]
+q0 = robot.model.referenceConfigurations["initial_pose"]
+
+x0 = np.hstack([m2a(q0), np.zeros(robot.model.nv)])
+problem = ShootingProblem(x0, [runningModel] * T, terminalModel)
+
+# Creating the DDP solver for this OC problem, defining a logger
+ddp = SolverDDP(problem)
+ddp.callback = [CallbackDDPVerbose()]
+ddp.callback.append(CallbackDDPLogger())
+
+# Solving it with the DDP algorithm
+ddp.solve()
+
+displayTrajectory(robot, ddp.xs, runningModel.timeStep)