Merge branch 'UAM_devel' of ssh://gitlab.iri.upc.edu:2202/jmarti/crocoddyl into UAM_devel

034decd1 · Pep Martí Saumell · f1b84540 · c5bc015e · 034decd1 · 034decd1
Commit 034decd1 authored 5 years ago by Pep Martí Saumell
--- a/crocoddyl/__init__.py
+++ b/crocoddyl/__init__.py
@@ -30,7 +30,7 @@ from .impact import (ActionDataImpact, ActionModelImpact, CostModelImpactCoM, Co
 from .integrated_action import (IntegratedActionDataEuler, IntegratedActionDataRK4, IntegratedActionModelEuler,
                                IntegratedActionModelRK4)
 from .kkt import SolverKKT
-from .robots import getTalosPathFromRos, loadHyQ, loadTalos, loadTalosArm, loadTalosLegs, loadKinton, loadKintonArm, loadBorinotArm
+from .robots import getTalosPathFromRos, loadHyQ, loadTalos, loadTalosArm, loadTalosLegs, loadKinton, loadKintonArm, load2dofPlanar, loadHector
 from .shooting import ShootingProblem
 from .solver import SolverAbstract
 from .state import StateAbstract, StateNumDiff, StatePinocchio, StateVector

--- a/crocoddyl/robots.py
+++ b/crocoddyl/robots.py
@@ -142,9 +142,15 @@ def loadKintonArm(modelPath='/opt/openrobots/share/example-robot-data'):
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
    return robot
-def loadBorinotArm(modelPath='/opt/openrobots/share/example-robot-data'):
+def load2dofPlanar(modelPath='/home/pepms/robotics/other-tools/robot-data'):
-    URDF_FILENAME = "borinot_arm.urdf"
+    URDF_FILENAME = "2dof_planar.urdf"
-    URDF_SUBPATH = "/borinot_arm/urdf/" + URDF_FILENAME
+    URDF_SUBPATH = "/2dof_planar/urdf/" + URDF_FILENAME
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], None)
    robot.q0.flat = [np.pi]
    return robot
+def loadHector(modelPath='/home/pepms/robotics/other-tools/robot-data'):
+    URDF_FILENAME = "quadrotor_base.urdf"
+    URDF_SUBPATH = "/hector-description/urdf/" + URDF_FILENAME
+    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
+    return robot
--- a/examples/notebooks/2DOFs.ipynb
+++ b/examples/notebooks/2DOFs.ipynb
--- a/examples/notebooks/acrobot.ipynb
+++ b/examples/notebooks/acrobot.ipynb
@@ -10,7 +10,7 @@
    "import pinocchio as pin\n",
    "import numpy as np\n",
    "\n",
-    "robot = loadBorinotArm()\n",
+    "robot = load2dofPlanar()\n",
    "robot.initViewer(loadModel=True)\n",
    "\n",
    "q0 = [3.14, 0]\n",

 %% Cell type:code id: tags:
 ``` python
 from crocoddyl import *
 import pinocchio as pin
 import numpy as np
-robot = loadBorinotArm()
+robot = load2dofPlanar()
 robot.initViewer(loadModel=True)
 q0 = [3.14, 0]
 robot.q0.flat = q0
 robot.framesForwardKinematics(robot.q0)
 robot.display(robot.q0)
 IDX_LINK1 = robot.model.getFrameId('link1', pin.FrameType.BODY)
 IDX_LINK2 = robot.model.getFrameId('link2', pin.FrameType.BODY)
 Mlink1 = robot.data.oMf[IDX_LINK1]
 Mlink2 = robot.data.oMf[IDX_LINK2]
 target_pos  = np.array([0,0,0.3])
 target_quat = pin.Quaternion(1, 0, 0, 0)
 target_quat.normalize()
 Mref = pin.SE3()
 Mref.translation = target_pos.reshape(3,1)
 Mref.rotation = target_quat.matrix()
 robot.viewer.gui.refresh()
 ```
 %% Cell type:code id: tags:
 ``` python
 state = StatePinocchio(robot.model)
 xRegCost = CostModelState(robot.model, state, ref=state.zero(), nu=1)
 uRegCost = CostModelControl(robot.model, nu = 1)
 xPendCost = CostModelDoublePendulum(robot.model,
                                    frame=state,
                                    ref=state.zero,
                                    nu=1,
                                    activation=ActivationModelWeightedQuad(np.array([1,1,1,1]+[0.1]*2)))
 runningCostModel = CostModelSum(robot.model, nu=1)
 terminalCostModel = CostModelSum(robot.model, nu=1)
 # runningCostModel.addCost(name="regx", weight=1e-6, cost=xRegCost)
 runningCostModel.addCost(name="regu", weight=1e-3, cost=uRegCost)
 runningCostModel.addCost(name="pend", weight=1, cost=xPendCost)
 terminalCostModel.addCost(name="ori2", weight=1e5, cost=xPendCost)
 ```
 %% Cell type:code id: tags:
 ``` python
 actModel = ActuationModelDoublePendulum(robot.model, actLink=2)
 runningModel = IntegratedActionModelEuler(DifferentialActionModelActuated(robot.model, actModel, runningCostModel))
 terminalModel = IntegratedActionModelEuler(DifferentialActionModelActuated(robot.model, actModel, terminalCostModel))
 ```
 %% Cell type:code id: tags:
 ``` python
 # Defining the time duration for running action models and the terminal one
 dt = 1e-2
 runningModel.timeStep = dt
 T = 100
 x0 = np.array([3.14, 0, 0., 0. ])
 problem = ShootingProblem(x0, [runningModel] * T, terminalModel)
 ```
 %% Cell type:code id: tags:
 ``` python
 # Creating the DDP solver for this OC problem, defining a logger
 ddp = SolverFDDP(problem)
 ddp.callback = [CallbackDDPVerbose()]
 ddp.callback.append(CallbackDDPLogger())
 us0 = np.zeros([T,1])
 xs0 = [problem.initialState+0.1]*len(ddp.models())
 ddp.solve(init_xs=xs0,init_us=us0,maxiter=150)
 ```
 %% Output
    iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
       0  2.00201e+05  1.16096e+09  9.21968e+01  1.00000e+00  1.00000e+00   1.0000     1
       1  2.00200e+05  1.54461e+07  3.14734e+00  1.00000e+00  1.00000e+00   1.0000     1
       2  2.00199e+05  1.79399e+03  3.09626e-01  1.00000e+00  1.00000e+00   1.0000     1
       3  2.00199e+05  3.39328e+03  5.89887e-01  1.00000e+00  1.00000e+00   1.0000     1
       4  2.00197e+05  6.47120e+03  1.12549e+00  1.00000e+00  1.00000e+00   1.0000     1
       5  2.00195e+05  1.23441e+04  2.14841e+00  1.00000e+00  1.00000e+00   1.0000     1
       6  2.00190e+05  2.35492e+04  4.10323e+00  1.00000e+00  1.00000e+00   1.0000     1
       7  2.00180e+05  4.49270e+04  7.84407e+00  1.00000e+00  1.00000e+00   1.0000     1
       8  2.00162e+05  8.57134e+04  1.50217e+01  1.00000e+00  1.00000e+00   1.0000     1
       9  2.00128e+05  1.63545e+05  2.88639e+01  1.00000e+00  1.00000e+00   1.0000     1
    iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
      10  2.00061e+05  3.12250e+05  5.58218e+01  1.00000e+00  1.00000e+00   1.0000     1
      11  1.99929e+05  5.97981e+05  1.09351e+02  1.00000e+00  1.00000e+00   1.0000     1
      12  1.99660e+05  1.16043e+06  2.19929e+02  1.00000e+00  1.00000e+00   1.0000     1
      13  1.99065e+05  2.38449e+06  4.68916e+02  1.00000e+00  1.00000e+00   1.0000     1
      14  1.97416e+05  6.33753e+06  1.16250e+03  1.00000e+00  1.00000e+00   1.0000     1
      15  1.82461e+05  4.99746e+07  5.10514e+03  1.00000e+00  1.00000e+00   1.0000     1
      16  1.68743e+05  7.51342e+10  5.15698e+04  1.00000e+01  1.00000e+01   1.0000     1
      17  1.26802e+05  3.99501e+09  3.44668e+05  1.00000e+01  1.00000e+01   0.5000     1
      18  6.98759e+04  9.82853e+11  2.26947e+05  1.00000e+01  1.00000e+01   0.5000     1
      19  2.02725e+04  5.71522e+11  1.17776e+05  1.00000e+00  1.00000e+00   1.0000     1
    iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
      20  1.02926e+04  1.14975e+11  2.59392e+04  1.00000e+00  1.00000e+00   0.5000     1
      21  6.50756e+03  2.62667e+10  6.55275e+03  1.00000e-01  1.00000e-01   1.0000     1
      22  5.34085e+03  4.07997e+09  4.17908e+03  1.00000e-01  1.00000e-01   0.2500     1
      23  4.70136e+03  1.88124e+09  2.62343e+03  1.00000e-01  1.00000e-01   0.2500     1
      24  3.99760e+03  9.09931e+08  1.84358e+03  1.00000e-01  1.00000e-01   0.5000     1
      25  3.32742e+03  7.81105e+07  1.11239e+03  1.00000e-02  1.00000e-02   1.0000     1
      26  3.06404e+03  1.43324e+08  9.14533e+02  1.00000e-02  1.00000e-02   0.5000     1
      27  2.85334e+03  9.68882e+07  5.65338e+02  1.00000e-02  1.00000e-02   0.5000     1
      28  2.79953e+03  3.04477e+07  2.66526e+02  1.00000e-03  1.00000e-03   1.0000     1
      29  2.70925e+03  2.13333e+08  2.80254e+02  1.00000e-03  1.00000e-03   0.5000     1
    iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
      30  2.50004e+03  2.92098e+08  3.27901e+02  1.00000e-04  1.00000e-04   1.0000     1
      31  2.02035e+03  3.52509e+05  1.26929e+03  1.00000e-04  1.00000e-04   0.5000     1
      32  1.80103e+03  2.08929e+07  1.97848e+03  1.00000e-04  1.00000e-04   0.1250     1
      33  1.60557e+03  1.60392e+07  1.42325e+03  1.00000e-04  1.00000e-04   0.2500     1
      34  1.41725e+03  3.85125e+07  9.65434e+02  1.00000e-04  1.00000e-04   0.2500     1
      35  1.24798e+03  1.64564e+07  4.53406e+02  1.00000e-04  1.00000e-04   0.5000     1
      36  1.05211e+03  5.38901e+06  2.75515e+02  1.00000e-05  1.00000e-05   1.0000     1
      37  8.44057e+02  1.99724e+05  4.26936e+02  1.00000e-06  1.00000e-06   1.0000     1
      38  8.02529e+02  4.66864e+04  1.73189e+02  1.00000e-06  1.00000e-06   0.5000     1
      39  7.77646e+02  8.81186e+02  1.00350e+02  1.00000e-06  1.00000e-06   0.5000     1
    iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
      40  7.62149e+02  3.22748e+03  6.09360e+01  1.00000e-06  1.00000e-06   0.5000     1
      41  7.52141e+02  1.11324e+03  3.83520e+01  1.00000e-06  1.00000e-06   0.5000     1
      42  7.45253e+02  3.50300e+03  2.53379e+01  1.00000e-06  1.00000e-06   0.5000     1
      43  7.40273e+02  2.73430e+03  1.76460e+01  1.00000e-06  1.00000e-06   0.5000     1
      44  7.36427e+02  1.39565e+03  1.31192e+01  1.00000e-06  1.00000e-06   0.5000     1
      45  7.33270e+02  6.20272e+02  1.03678e+01  1.00000e-06  1.00000e-06   0.5000     1
      46  7.30557e+02  2.80257e+02  8.59548e+00  1.00000e-06  1.00000e-06   0.5000     1
      47  7.28152e+02  1.42955e+02  7.37005e+00  1.00000e-06  1.00000e-06   0.5000     1
      48  7.25979e+02  8.63697e+01  6.45943e+00  1.00000e-06  1.00000e-06   0.5000     1
      49  7.23994e+02  6.07746e+01  5.73694e+00  1.00000e-06  1.00000e-06   0.5000     1
    iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
      50  7.22170e+02  4.73969e+01  5.13265e+00  1.00000e-06  1.00000e-06   0.5000     1
      51  7.20493e+02  3.90506e+01  4.60615e+00  1.00000e-06  1.00000e-06   0.5000     1
      52  7.18951e+02  3.29214e+01  4.13491e+00  1.00000e-06  1.00000e-06   0.5000     1
      53  7.17537e+02  2.79148e+01  3.70581e+00  1.00000e-06  1.00000e-06   0.5000     1
      54  7.16245e+02  2.36222e+01  3.31178e+00  1.00000e-06  1.00000e-06   0.5000     1
      55  7.15889e+02  1.99076e+01  2.94888e+00  1.00000e-07  1.00000e-07   1.0000     1
      56  7.12990e+02  2.01704e+01  6.55642e+00  1.00000e-08  1.00000e-08   1.0000     1
      57  7.10845e+02  6.67999e+02  4.07708e+00  1.00000e-09  1.00000e-09   1.0000     1
      58  7.09461e+02  3.80830e+01  2.26922e+00  1.00000e-09  1.00000e-09   1.0000     1
      59  7.08542e+02  6.82361e+00  1.34000e+00  1.00000e-09  1.00000e-09   1.0000     1
    iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
      60  7.07909e+02  4.03002e+00  8.47089e-01  1.00000e-09  1.00000e-09   1.0000     1
      61  7.07469e+02  2.73417e+00  5.52833e-01  1.00000e-09  1.00000e-09   1.0000     1
      62  7.07165e+02  1.84088e+00  3.65584e-01  1.00000e-09  1.00000e-09   1.0000     1
      63  7.06956e+02  1.23860e+00  2.43972e-01  1.00000e-09  1.00000e-09   1.0000     1
      64  7.06812e+02  8.37368e-01  1.63849e-01  1.00000e-09  1.00000e-09   1.0000     1
      65  7.06713e+02  5.67882e-01  1.10468e-01  1.00000e-09  1.00000e-09   1.0000     1
      66  7.06646e+02  3.85585e-01  7.46353e-02  1.00000e-09  1.00000e-09   1.0000     1
      67  7.06600e+02  2.61781e-01  5.04698e-02  1.00000e-09  1.00000e-09   1.0000     1
      68  7.06569e+02  1.77570e-01  3.41303e-02  1.00000e-09  1.00000e-09   1.0000     1
      69  7.06548e+02  1.20293e-01  2.30698e-02  1.00000e-09  1.00000e-09   1.0000     1
    iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
      70  7.06533e+02  8.13736e-02  1.55814e-02  1.00000e-09  1.00000e-09   1.0000     1
      71  7.06524e+02  5.49677e-02  1.05140e-02  1.00000e-09  1.00000e-09   1.0000     1
      72  7.06517e+02  3.70811e-02  7.08778e-03  1.00000e-09  1.00000e-09   1.0000     1
      73  7.06513e+02  2.49848e-02  4.77356e-03  1.00000e-09  1.00000e-09   1.0000     1
      74  7.06510e+02  1.68167e-02  3.21213e-03  1.00000e-09  1.00000e-09   1.0000     1
      75  7.06508e+02  1.13087e-02  2.15973e-03  1.00000e-09  1.00000e-09   1.0000     1
      76  7.06507e+02  7.59883e-03  1.45112e-03  1.00000e-09  1.00000e-09   1.0000     1
      77  7.06506e+02  5.10267e-03  9.74412e-04  1.00000e-09  1.00000e-09   1.0000     1
      78  7.06505e+02  3.42460e-03  6.53968e-04  1.00000e-09  1.00000e-09   1.0000     1
      79  7.06505e+02  2.29732e-03  4.38710e-04  1.00000e-09  1.00000e-09   1.0000     1
    iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
      80  7.06505e+02  1.54053e-03  2.94196e-04  1.00000e-09  1.00000e-09   1.0000     1
      81  7.06504e+02  1.03271e-03  1.97224e-04  1.00000e-09  1.00000e-09   1.0000     1
      82  7.06504e+02  6.92113e-04  1.32181e-04  1.00000e-09  1.00000e-09   1.0000     1
      83  7.06504e+02  4.63746e-04  8.85696e-05  1.00000e-09  1.00000e-09   1.0000     1
      84  7.06504e+02  3.10676e-04  5.93365e-05  1.00000e-09  1.00000e-09   1.0000     1
      85  7.06504e+02  2.08099e-04  3.97461e-05  1.00000e-09  1.00000e-09   1.0000     1
      86  7.06504e+02  1.39374e-04  2.66203e-05  1.00000e-09  1.00000e-09   1.0000     1
      87  7.06504e+02  9.33361e-05  1.78274e-05  1.00000e-09  1.00000e-09   1.0000     1
      88  7.06504e+02  6.25004e-05  1.19379e-05  1.00000e-09  1.00000e-09   1.0000     1
      89  7.06504e+02  4.18492e-05  7.99351e-06  1.00000e-09  1.00000e-09   1.0000     1
    iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
      90  7.06504e+02  2.80200e-05  5.35208e-06  1.00000e-09  1.00000e-09   1.0000     1
      91  7.06504e+02  1.87599e-05  3.58333e-06  1.00000e-09  1.00000e-09   1.0000     1
      92  7.06504e+02  1.25596e-05  2.39903e-06  1.00000e-09  1.00000e-09   1.0000     1
      93  7.06504e+02  8.40829e-06  1.60609e-06  1.00000e-09  1.00000e-09   1.0000     1
      94  7.06504e+02  5.62898e-06  1.07521e-06  1.00000e-09  1.00000e-09   1.0000     1
      95  7.06504e+02  3.76828e-06  7.19797e-07  1.00000e-09  1.00000e-09   1.0000     1
      96  7.06504e+02  2.52261e-06  4.81856e-07  1.00000e-09  1.00000e-09   1.0000     1
      97  7.06504e+02  1.68869e-06  3.22566e-07  1.00000e-09  1.00000e-09   1.0000     1
      98  7.06504e+02  1.13043e-06  2.15931e-07  1.00000e-09  1.00000e-09   1.0000     1
      99  7.06504e+02  7.56723e-07  1.44547e-07  1.00000e-09  1.00000e-09   1.0000     1
    iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
     100  7.06504e+02  5.06554e-07  9.67603e-08  1.00000e-09  1.00000e-09   1.0000     1
     101  7.06504e+02  3.39087e-07  6.47715e-08  1.00000e-09  1.00000e-09   1.0000     1
     102  7.06504e+02  2.26984e-07  4.33579e-08  1.00000e-09  1.00000e-09   1.0000     1
     103  7.06504e+02  1.51942e-07  2.90235e-08  1.00000e-09  1.00000e-09   1.0000     1
     104  7.06504e+02  1.01709e-07  1.94281e-08  1.00000e-09  1.00000e-09   1.0000     1
     105  7.06504e+02  6.80828e-08  1.30050e-08  1.00000e-09  1.00000e-09   1.0000     1
     106  7.06504e+02  4.55739e-08  8.70541e-09  1.00000e-09  1.00000e-09   1.0000     1
     107  7.06504e+02  3.05066e-08  5.82730e-09  1.00000e-09  1.00000e-09   1.0000     1
     108  7.06504e+02  2.04207e-08  3.90072e-09  1.00000e-09  1.00000e-09   1.0000     1
     109  7.06504e+02  1.36693e-08  2.61109e-09  1.00000e-09  1.00000e-09   1.0000     1
    iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
     110  7.06504e+02  9.15005e-09  1.74782e-09  1.00000e-09  1.00000e-09   1.0000     1
     111  7.06504e+02  6.12490e-09  1.16997e-09  1.00000e-09  1.00000e-09   1.0000     1
     112  7.06504e+02  4.09991e-09  7.83156e-10  1.00000e-09  1.00000e-09   1.0000     1
     113  7.06504e+02  2.74441e-09  5.24232e-10  1.00000e-09  1.00000e-09   1.0000     1
     114  7.06504e+02  1.83707e-09  3.50913e-10  1.00000e-09  1.00000e-09   1.0000     1
     115  7.06504e+02  1.22970e-09  2.34895e-10  1.00000e-09  1.00000e-09   1.0000     1
     116  7.06504e+02  8.23141e-10  1.57235e-10  1.00000e-09  1.00000e-09   1.0000     1
    ([array([3.14, 0.  , 0.  , 0.  ]),
      array([ 3.18493482, -0.08713096,  4.49348194, -8.71309552]),
      array([ 3.2298292 , -0.17422292,  4.48943827, -8.70919683]),
      array([ 3.27463366, -0.26123502,  4.48044598, -8.70120942]),
      array([ 3.31930113, -0.34813226,  4.46674667, -8.68972446]),
      array([ 3.36378637, -0.43488521,  4.44852367, -8.67529474]),
      array([ 3.4080461 , -0.52147097,  4.42597344, -8.65857575]),
      array([ 3.45203903, -0.60787403,  4.39929332, -8.64030581]),
      array([ 3.4957257 , -0.69408685,  4.36866653, -8.62128281]),
      array([ 3.53906814, -0.78011023,  4.33424423, -8.60233717]),
      array([ 3.58202938, -0.86595323,  4.29612405, -8.58430083]),
      array([ 3.62457263, -0.95163295,  4.2543245 , -8.56797163]),
      array([ 3.66666017, -1.03717368,  4.20875472, -8.55407264]),
      array([ 3.70825196, -1.12260574,  4.15917857, -8.54320601]),
      array([ 3.74930368, -1.20796374,  4.10517248, -8.53580074]),
      array([ 3.78976444, -1.29328428,  4.04607602, -8.53205407]),
      array([ 3.82957379, -1.37860295,  3.98093443, -8.53186644]),
      array([ 3.86865812, -1.46395065,  3.90843271, -8.5347701 ]),
      array([ 3.90692633, -1.54934918,  3.82682107, -8.53985257]),
      array([ 3.94426465, -1.63480594,  3.73383282, -8.54567695]),
      array([ 3.98053063, -1.72030798,  3.62659768, -8.55020316]),
      array([ 4.01554619, -1.80581515,  3.50155634, -8.55071692]),
      array([ 4.04909007, -1.89125292,  3.35438748, -8.54377767]),
      array([ 4.08088973, -1.97650495,  3.17996605, -8.52520265]),
      array([ 4.11061355, -2.06140607,  2.9723825 , -8.49011177]),
      array([ 4.13786422, -2.14573673,  2.72506642, -8.43306615]),
      array([ 4.16217494, -2.22922011,  2.43107181, -8.34833785]),
      array([ 4.18301081, -2.31152354,  2.08358752, -8.23034349]),
      array([ 4.19977803, -2.39226603,  1.67672214, -8.07424895]),
      array([ 4.21184361, -2.47103299,  1.2065574 , -7.87669617]),
      array([ 4.21856715, -2.54739816,  0.67235412, -7.63651698]),
      array([ 4.21934356, -2.62095029,  0.07764073, -7.35521284]),
      array([ 4.21365137, -2.69131985, -0.5692183 , -7.03695599]),
      array([ 4.20109761, -2.75819961, -1.25537597, -6.68797565]),
      array([ 4.18144808, -2.82135392, -1.96495324, -6.31543089]),
      array([ 4.15463456, -2.88061511, -2.68135242, -5.92611935]),
      array([ 4.12073641, -2.93586965, -3.38981479, -5.525454  ]),
      array([ 4.07994156, -2.98703951, -4.07948487, -5.11698614]),
      array([ 4.03249694, -3.03406423, -4.74446208, -4.70247228]),
      array([ 3.97865966, -3.0768869 , -5.38372748, -4.28226633]),
      array([ 3.91865772, -3.11544464, -6.00019469, -3.85577402]),
      array([ 3.85266466, -3.14966263, -6.59930552, -3.42179966]),
      array([ 3.78078902, -3.17945009, -7.18756462, -2.97874593]),
      array([ 3.70307634, -3.20469726, -7.77126796, -2.52471678]),
      array([ 3.61952115, -3.22527328, -8.3555189 , -2.05760147]),
      array([ 3.5300861 , -3.24102531, -8.94350507, -1.57520325]),
      array([ 3.43472662, -3.25177975, -9.53594786, -1.07544446]),
      array([  3.33342033,  -3.25734622, -10.13062875,  -0.55664642]),
      array([  3.22620107,  -3.25752473, -10.72192632,  -0.01785156]),
      array([  3.11319752,  -3.25211597, -11.30035464,   0.54087625]),
      array([  2.99467581,  -3.24093327, -11.85217104,   1.11826978]),
      array([  2.87108366,  -3.22381409, -12.35921487,   1.71191798]),
      array([  2.74309134,  -3.2006272 , -12.79923182,   2.31868886]),
      array([  2.61162144,  -3.17127146, -13.14699083,   2.93557402]),
      array([  2.47785696,  -3.13566294, -13.3764471 ,   3.56085211]),
      array([  2.34321717,  -3.09371085, -13.46397986,   4.19520901]),
      array([  2.20929372,  -3.0452891 , -13.39234466,   4.84217551]),
      array([  2.07774808,  -2.99021784, -13.1545638 ,   5.50712605]),
      array([  1.95018035,  -2.92827437, -12.75677323,   6.19434675]),
      array([  1.82798839,  -2.85924962, -12.2191963 ,   6.90247462]),
      array([  1.71224041,  -2.78305121, -11.57479785,   7.61984101]),
      array([  1.60358529,  -2.6998276 , -10.86551226,   8.32236121]),
      array([  1.50222254,  -2.61006306, -10.13627404,   8.976454  ]),
      array([ 1.40794413, -2.51459222, -9.42784116,  9.54708392]),
      array([ 1.32023892, -2.41451805, -8.77052111, 10.0074172 ]),
      array([ 1.23842745, -2.31106757, -8.18114766, 10.34504814]),
      array([ 1.16178642, -2.20544538, -7.6641023 , 10.5622186 ]),
      array([ 1.0896362 , -2.09873042, -7.21502244, 10.67149584]),
      array([ 1.02138612, -1.99182797, -6.82500812, 10.69024557]),
      array([ 0.95654761, -1.8854647 , -6.48385065, 10.63632652]),
      array([ 0.8947285 , -1.78020782, -6.18191078, 10.52568849]),
      array([ 0.83561923, -1.67649297, -5.91092732, 10.37148461]),
      array([ 0.77897738, -1.5746524 , -5.66418506, 10.18405693]),
      array([ 0.72461361, -1.4749394 , -5.43637709,  9.9713008 ]),
      array([ 0.67238   , -1.37754814, -5.22336057,  9.73912544]),
      array([ 0.62216096, -1.28262932, -5.02190441,  9.49188186]),
      array([ 0.57386627, -1.19030217, -4.8294683 ,  9.23271545]),
      array([ 0.52742604, -1.10066377, -4.64402354,  8.96383942]),
      array([ 0.48278691, -1.01379637, -4.46391333,  8.68674066]),
      array([ 0.43990944, -0.92977304, -4.28774644,  8.40233236]),
      array([ 0.39876627, -0.84866238, -4.11431701,  8.11106667]),
      array([ 0.35934083, -0.7705322 , -3.94254454,  7.81301792]),
      array([ 0.32162654, -0.69545275, -3.77142905,  7.50794454]),
      array([ 0.28562636, -0.6234994 , -3.60001745,  7.1953355 ]),
      array([ 0.25135258, -0.55475494, -3.42737839,  6.87444539]),
      array([ 0.21882675, -0.48931173, -3.2525831 ,  6.54432136]),
      array([ 0.18807984, -0.42727349, -3.0746908 ,  6.20382372]),
      array([ 0.15915247, -0.36875707, -2.8927373 ,  5.85164182]),
      array([ 0.13209521, -0.31389401, -2.70572581,  5.48630617]),
      array([ 0.10696901, -0.26283204, -2.51261927,  5.1061972 ]),
      array([ 0.08384568, -0.21573653, -2.31233348,  4.70955131]),
      array([ 0.06280837, -0.17279189, -2.10373072,  4.29446411]),
      array([ 0.04395224, -0.13420298, -1.88561332,  3.85889105]),
      array([ 0.02738507, -0.10019652, -1.65671694,  3.40064542]),
      array([ 0.01322804, -0.07102258, -1.41570347,  2.91739391]),
      array([ 1.61650040e-03, -4.69560835e-02, -1.16115350e+00,  2.40664990e+00]),
      array([-0.00729909, -0.02829843, -0.89155866,  1.86576554]),
      array([-0.01335223, -0.01537919, -0.60531455,  1.29192372]),
      array([-0.01635938, -0.00855786, -0.30071529,  0.6821328 ]),
      array([-0.01611896, -0.00822546,  0.02404208,  0.03323985]),
      array([-0.01586976, -0.00809705,  0.02492077,  0.01284125])],
     [array([-0.41707639]),
      array([-0.02170736]),
      array([-0.04339121]),
      array([-0.06488339]),
      array([-0.08608214]),
      array([-0.10690605]),
      array([-0.12728863]),
      array([-0.14717918]),
      array([-0.16654239]),
      array([-0.18535658]),
      array([-0.20361058]),
      array([-0.22129932]),
      array([-0.23841813]),
      array([-0.25495595]),
      array([-0.27088756]),
      array([-0.28616505]),
      array([-0.30070894]),
      array([-0.31439939]),
      array([-0.32706843]),
      array([-0.33849405]),
      array([-0.34839822]),
      array([-0.3564508]),
      array([-0.36228264]),
      array([-0.36551154]),
      array([-0.36578462]),
      array([-0.36283921]),
      array([-0.35657909]),
      array([-0.34715357]),
      array([-0.33501288]),
      array([-0.32090036]),
      array([-0.30574326]),
      array([-0.29043471]),
      array([-0.27556091]),
      array([-0.26118713]),
      array([-0.24681691]),
      array([-0.23155717]),
      array([-0.21440566]),
      array([-0.19451738]),
      array([-0.17134323]),
      array([-0.14462508]),
      array([-0.11430488]),
      array([-0.08042383]),
      array([-0.04306259]),
      array([-0.00233759]),
      array([0.04155727]),
      array([0.0882841]),
      array([0.13729043]),
      array([0.18775348]),
      array([0.23855829]),
      array([0.28833105]),
      array([0.3355495]),
      array([0.37874419]),
      array([0.41677769]),
      array([0.44913947]),
      array([0.47613113]),
      array([0.49877609]),
      array([0.51832359]),
      array([0.53537352]),
      array([0.54891866]),
      array([0.55588481]),
      array([0.55180844]),
      array([0.53280124]),
      array([0.49790594]),
      array([0.4502027]),
      array([0.39572102]),
      array([0.34097891]),
      array([0.2908931]),
      array([0.2480415]),
      array([0.21303487]),
      array([0.18528336]),
      array([0.16366376]),
      array([0.14693799]),
      array([0.13396036]),
      array([0.12375473]),
      array([0.115526]),
      array([0.10864418]),
      array([0.10262018]),
      array([0.09708141]),
      array([0.09175015]),
      array([0.0864254]),
      array([0.08096782]),
      array([0.07528741]),
      array([0.06933347]),
      array([0.06308651]),
      array([0.05655164]),
      array([0.04975356]),
      array([0.04273256]),
      array([0.03554161]),
      array([0.02824437]),
      array([0.02091379]),
      array([0.01363135]),
      array([0.00648661]),
      array([-0.00042301]),
      array([-0.00699249]),
      array([-0.01310974]),
      array([-0.0186566]),
      array([-0.02351091]),
      array([-0.02754985]),
      array([-0.03065489]),
      array([-0.00059537])],
     True)
 %% Cell type:code id: tags:
 ``` python
 displayTrajectory(robot, ddp.xs, runningModel.timeStep)
 ```
 %% Cell type:code id: tags:
 ``` python
 import time
 dt = 0.01
 t = np.arange(0,1,dt)
 q0 = np.array([[3.14,0]]).T
 q = q0
 q_d = np.zeros([2,1])
 q_dd = np.zeros([2,1])
 ```
 %% Cell type:code id: tags:
 ``` python
 nle = np.zeros([2,1])
 for i in range(len(t)):
    pin.computeAllTerms(robot.model, robot.data, q, q_d)
    M = robot.data.M
    Minv = np.linalg.inv(M)
    r = np.zeros([2,1])
    tau = np.zeros([2,1])
    tau[1,0] = ddp.us[i]
    nle[:,0] = m2a(robot.data.nle)
    r[:] = tau - nle
    q_dd = np.dot(Minv, r)
    q = q + q_d*dt + q_dd*dt**2
    q_d = q_d + q_dd*dt
    #pin.forwardKinematics(robot.model, robot.data, q, q_d)
    robot.display(q)
    time.sleep(dt)
 ```
 %% Cell type:code id: tags:
 ``` python
 import numpy as np
 import matplotlib.pyplot as plt
 control = np.vstack(ddp.us)
 t = np.arange(0,T*dt, dt)
 fig, axs = plt.subplots(1,2, figsize=(15,10))
 fig.suptitle('Motor torques')
 # axs[0].plot(t,control[:,0], t,control[:,1])
 axs[0].plot(t,control[:,0])
 axs[0].set_title('Moments')
 axs[0].legend(['M1','M2'])
 t_state = np.append(t, t[-1]+dt)
 state = np.vstack(ddp.xs)
 fig, axs = plt.subplots(1,2, figsize=(15,10))
 fig.suptitle('States')
 axs[0].plot(t_state,state[:,0], t_state, state[:,1])
 axs[0].set_title('Position')
 axs[0].legend(['Link 1','Link2'])
 axs[1].plot(t_state,state[:,2], t_state, state[:,3])
 axs[1].set_title('Vels')
 axs[1].legend(['Link 1','Link2'])
 ```
 %% Output
    <matplotlib.legend.Legend at 0x7fa1936d3490>
 %% Cell type:code id: tags:
 ``` python
 ```

--- a/examples/notebooks/kinton_flying_mission.ipynb
+++ b/examples/notebooks/kinton_flying_mission.ipynb