Addeed UAM files

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,62 @@ 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, rotorDistance, coefM, coefF):
+        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
+        self.d = rotorDistance
+        self.cm = coefM
+        self.cf = coefF
+
+    def calc(self, data, x, u):
+        # data.a[2:] = u
+        d, cf, cm = self.d, self.cf, self.cf
+        S = np.array(np.zeros([self.nv,self.nu]))
+        S[2:6,:4] = np.array([[1,1,1,1],[-d,d,d,-d],[-d,d,-d,d],[-cm/cf,-cm/cf,cm/cf,cm/cf]])
+        np.fill_diagonal(S[6:, 4:], 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 ActuationDataUAM(self, pinocchioData)
+
+class ActuationDataUAM:
+    def __init__(self, model, pinocchioData):
+        self.pinocchio = pinocchioData
+        ndx, nv, nu = model.ndx, model.nv, model.nu
+        d, cf, cm = model.d, model.cf, model.cf
+        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[2:6,:4] = np.array([[1,1,1,1],[-d,d,d,-d],[-d,d,-d,d],[-cm/cf,-cm/cf,cm/cf,cm/cf]])
+        np.fill_diagonal(self.Au[6:, 4:], 1)
+        #np.fill_diagonal(self.Au[2:, :], 1)
+# This is the matrix that, given a force vector representing the four motors, outputs the thrust and moment
+# [      0,      0,     0,     0]
+# [      0,      0,     0,     0]
+# [      1,      1,     1,     1]
+# [     -d,      d,    -d,     d]
+# [     -d,     -d,     d,     d]
+# [ -cm/cf, -cm/cf, cm/cf, cm/cf]
+
 
 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)
+        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:])
+        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
+
+    def quasiStatic(self, data, x):
+        nu, nq, nv = self.nu, self.nq, self.nv
+        if len(x) == nq:
+            x = np.concatenate([x, np.zeros(nv)])
+        else:
+            x[nq:] = 0
+        self.calcDiff(data, x, np.zeros(nu))
+        return np.dot(np.linalg.pinv(data.actuation.Au), -data.r)[:nu]
+
+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)
+        self.r    = np.zeros(nv)

crocoddyl/robots.py

@@ -125,3 +125,23 @@ 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
+
+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_base.py

+#!/usr/bin/env python
+# coding: utf-8
+
+# In[13]:
+
+
+from crocoddyl import *
+import pinocchio as pin
+import numpy as np
+from crocoddyl.diagnostic import displayTrajectory
+
+
+# In[14]:
+
+
+# LOAD ROBOT
+robot = loadKinton()
+robot.initViewer(loadModel=True)
+robot.display(robot.q0)
+
+robot.framesForwardKinematics(robot.q0)
+
+rmodel = robot.model
+
+
+# In[15]:
+
+
+# DEFINE TARGET POSITION
+target_pos  = np.array([0,0,1])
+target_quat = pin.Quaternion(1, 0, 0, 0)
+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()
+
+
+# In[16]:
+
+
+# ACTUATION MODEL
+distanceRotorCOG = 0.1525
+cf = 6.6e-5
+cm = 1e-6
+actModel = ActuationModelUAM(robot.model, distanceRotorCOG, cf, cm)
+
+# 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()
+
+
+wBasePos  = [1]
+wBaseOri  = [500]
+wArmPos   = [1]
+wBaseVel  = [10]
+wBaseRate = [10]
+wArmVel   = [10]
+stateWeights   = np.array(wBasePos * 3 + wBaseOri * 3 + wArmPos * (robot.model.nv - 6) + wBaseVel * robot.model.nv)
+controlWeights = np.array([0.1]*4 + [100]*6)
+
+goalTrackingCost = CostModelFramePlacement(rmodel,
+                                           frame=rmodel.getFrameId(frameName),
+                                           ref=SE3ref,
+                                           nu =actModel.nu)
+
+xRegCost = CostModelState(rmodel,
+                          state,
+                          ref=state.zero(),
+                          nu=actModel.nu,
+                          activation=ActivationModelWeightedQuad(stateWeights))
+uRegCost = CostModelControl(rmodel,
+                            nu=robot.
+                            model.nv-2,
+                            activation = ActivationModelWeightedQuad(controlWeights))
+uLimCost = CostModelControl(rmodel,
+                            nu=robot.
+                            model.nv-2,
+                            activation = ActivationModelInequality(np.array([0.1, 0.1, 0.1, 0.1, -1, -1, -1, -1, -1, -1]),
+                                                                np.array([5, 5, 5, 5, 1, 1, 1, 1, 1, 1])))
+
+# Then let's add the running and terminal cost functions
+runningCostModel.addCost(name="pos", weight=0.1, cost=goalTrackingCost)
+runningCostModel.addCost(name="regx", weight=1e-4, cost=xRegCost)
+runningCostModel.addCost(name="regu", weight=1e-6, cost=uRegCost)
+# runningCostModel.addCost(name="limu", weight=1e-3, cost=uLimCost)
+terminalCostModel.addCost(name="pos", weight=0, cost=goalTrackingCost)
+
+# DIFFERENTIAL ACTION MODEL
+runningModel = IntegratedActionModelEuler(DifferentialActionModelUAM(robot.model, actModel, runningCostModel))
+terminalModel = IntegratedActionModelEuler(DifferentialActionModelUAM(robot.model, actModel, terminalCostModel))
+
+
+# In[ ]:
+
+
+# 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
+print "----------MARK 1----------"
+T = 1000
+q0 = rmodel.referenceConfigurations["initial_pose"].copy()
+v0 = pin.utils.zero(rmodel.nv)
+x0 = m2a(np.concatenate([q0, v0]))
+rmodel.defaultState = x0.copy()
+
+print "----------MARK 2----------"
+problem = ShootingProblem(x0, [runningModel] * T, terminalModel)
+
+# Creating the DDP solver for this OC problem, defining a logger
+fddp = SolverFDDP(problem)
+fddp.callback = [CallbackDDPVerbose()]
+fddp.callback.append(CallbackDDPLogger())
+#fddp.setCallbacks([CallbackVerbose()])
+
+print "----------MARK 3----------"
+# us0 = [
+#     m.differential.quasiStatic(d.differential, rmodel.defaultState)
+#     if isinstance(m, IntegratedActionModelEuler) else np.zeros(0)
+#     for m, d in zip(fddp.problem.runningModels, fddp.problem.runningDatas)]
+# xs0 = [problem.initialState]*len(fddp.models())
+
+# Solving it with the DDP algorithm
+print "----------SOLVING----------"
+#fddp.solve(init_xs=xs0, init_us=us0)
+fddp.solve()
+
+
+# In[6]:
+
+
+displayTrajectory(robot, fddp.xs, runningModel.timeStep)
+
+
+# In[7]:
+
+
+# Control trajectory
+f1 = []
+f2 = [];
+f3 = [];
+f4 = [];
+
+for u in fddp.us:
+    f1.append(u[0])
+    f2.append(u[1])
+    f3.append(u[2])
+    f4.append(u[3])
+
+# State trajectory
+Xx = [];
+Xy = [];
+Xz = [];
+Vx = [];
+Vy = [];
+Vz = [];
+
+
+for x in fddp.xs:
+    Xx.append(x[0])
+    Xy.append(x[1])
+    Xz.append(x[2])
+    Vx.append(x[13])
+    Vy.append(x[14])
+    Vz.append(x[15])
+
+
+# In[9]:
+
+
+import matplotlib.pyplot as plt
+t = np.arange(0., 1, dt)
+
+fig, axs = plt.subplots(2,2, figsize=(15,10))
+fig.suptitle('Motor forces')
+axs[0, 0].plot(t,f1)
+axs[0, 0].set_title('Motor 1')
+axs[0, 1].plot(t,f2)
+axs[0, 1].set_title('Motor 2')
+axs[1, 0].plot(t,f3)
+axs[1, 0].set_title('Motor 3')
+axs[1, 1].plot(t,f4)
+axs[1, 1].set_title('Motor 4')
+
+plt.figure()
+t = np.append(t, 1)
+plt.plot(t,Xx,t,Xy,t,Xz)
+plt.legend(['x','y','z'])
+plt.title('State - Position')
+plt.ylabel('Position, [m]')
+plt.xlabel('[s]')
+
+plt.figure()
+plt.plot(t,Vx,t,Vy,t,Vz)
+plt.legend(['x','y','z'])
+plt.title('State - Velocity')
+plt.ylabel('Velocity, [m/s]')
+plt.xlabel('[s]')
+
+
+# In[ ]:
+
+
+
+
+
+# In[ ]: