diff --git a/crocoddyl/__init__.py b/crocoddyl/__init__.py
index f128f612dffc99a6ca82388db41d6eab477dcb7f..ecca59a228e0eca8b602c8cb1b3442cc7442c218 100644
--- a/crocoddyl/__init__.py
+++ b/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
diff --git a/crocoddyl/actuation.py b/crocoddyl/actuation.py
index ae486f4e31c809e1ce96ee6006ce7a18224db405..4da7abda2299fa2aced15258d501787feae743d6 100644
--- a/crocoddyl/actuation.py
+++ b/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:
'''
diff --git a/crocoddyl/flying.py b/crocoddyl/flying.py
new file mode 100644
index 0000000000000000000000000000000000000000..3d12396adb14e0d0c66caab9bd11738f8a60bfb0
--- /dev/null
+++ b/crocoddyl/flying.py
@@ -0,0 +1,87 @@
+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)
diff --git a/crocoddyl/robots.py b/crocoddyl/robots.py
index 4b4c3774ac88074735f4d7d1283737f2721899d8..ce674425691ac1e3aa155fb8d7676f9910c3670d 100644
--- a/crocoddyl/robots.py
+++ b/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
diff --git a/examples/kinton_flying_base.py b/examples/kinton_flying_base.py
new file mode 100644
index 0000000000000000000000000000000000000000..48cbc9aed5e1bb276a825c0a478b9fc36d42dc40
--- /dev/null
+++ b/examples/kinton_flying_base.py
@@ -0,0 +1,220 @@
+#!/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[ ]: