diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8f7fb523bfcffe35c9be4cd90aace9d4a63585a1..9cc1f6d78a600e3b7581bcdc123470b45a4d16ac 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -41,6 +41,7 @@ OPTION(ENABLE_VECTORIZATION "Enable vectorization and futhers processor-related
 OPTION(BUILD_PYTHON_INTERFACE "Build the python binding" ON)
 OPTION(BUILD_UNIT_TESTS "Build the unitary tests" ON)
 OPTION(BUILD_BENCHMARK "Build the benchmark" ON)
+OPTION(BUILD_EXAMPLES "Build the examples" ON)
 
 
 IF(ENABLE_VECTORIZATION)
@@ -120,5 +121,10 @@ IF(BUILD_BENCHMARK)
   ADD_SUBDIRECTORY(benchmark)
 ENDIF(BUILD_BENCHMARK)
 
+# Build the examples
+IF(BUILD_EXAMPLES)
+  ADD_SUBDIRECTORY(examples)
+ENDIF(BUILD_EXAMPLES)
+
 
 SETUP_PROJECT_FINALIZE()
diff --git a/README.md b/README.md
index 12d2ff2096d460edb8a07e9b73c8fff463ec1cc4..63e8382665aeda69b5c8e92e0f8913e922fea269 100644
--- a/README.md
+++ b/README.md
@@ -8,7 +8,7 @@ Contact RObot COntrol by Differential DYnamic programming Library (crocoddyl)
 The source code is released under the [BSD 3-Clause license](LICENSE).
 
 **Authors:** [Carlos Mastalli](https://cmastalli.github.io/) and Rohan Budhiraja <br />
-**Instructors:** Justin Carpentier and Nicolas Mansard <br />
+**Instructors:** Nicolas Mansard <br />
 **With additional support from the Gepetto team at LAAS-CNRS.**
 
 [![License BSD-3-Clause](https://img.shields.io/badge/license-BSD--3--Clause-blue.svg?style=flat)](https://tldrlegal.com/license/bsd-3-clause-license-%28revised%29#fulltext)
@@ -25,74 +25,94 @@ If you want to follow the current developments, you can directly refer to the [d
 
 
 ## <img align="center" height="20" src="https://i.imgur.com/x1morBF.png"/> Installation
-**Crocoddyl** can be easily installed on various Linux (Ubuntu, Fedora, etc.) and Unix distributions (Mac OS X, BSD, etc.). Please refer to 
+**Crocoddyl** can be easily installed on various Linux (Ubuntu, Fedora, etc.) and Unix distributions (Mac OS X, BSD, etc.). Please refer to
 
 ### Installation through robotpkg
 
-You can install this package throught robotpkg. robotpkg is a package manager tailored for robotics softwares. It greatly simplifies the release of new versions along with the management of their dependencies. You just need to add the robotpkg apt repository to your sources.list and then use `sudo apt install robotpkg-py27-crocoddyl`:
+You can install this package through robotpkg. robotpkg is a package manager tailored for robotics softwares. It greatly simplifies the release of new versions along with the management of their dependencies. You just need to add the robotpkg apt repository to your sources.list and then use `sudo apt install robotpkg-py27-crocoddyl`:
 
 If you have never added robotpkg as a softwares repository, please follow first the instructions from 1 to 3. Otherwise, go directly to instruction 4. Those instructions are similar to the installation procedures presented in [http://robotpkg.openrobots.org/debian.html](http://robotpkg.openrobots.org/debian.html).
 
 1. Add robotpkg as source repository to apt:
 
-		sudo tee /etc/apt/sources.list.d/robotpkg.list <<EOF
-		deb [arch=amd64] http://robotpkg.openrobots.org/wip/packages/debian/pub $(lsb_release -sc) robotpkg
-		deb [arch=amd64] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -sc) robotpkg
-		EOF
+```bash
+sudo tee /etc/apt/sources.list.d/robotpkg.list <<EOF
+deb [arch=amd64] http://robotpkg.openrobots.org/wip/packages/debian/pub $(lsb_release -sc) robotpkg
+deb [arch=amd64] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -sc) robotpkg
+EOF
+```
 
 2. Register the authentication certificate of robotpkg:
 
-		curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | sudo apt-key add -
+```bash
+curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | sudo apt-key add -
+```
 
 3. You need to run at least once apt update to fetch the package descriptions:
 
-		sudo apt-get update
+```bash
+sudo apt-get update
+```
 
 4. The installation of Crocoddyl:
 
-		sudo apt install robotpkg-py27-crocoddyl # for Python 2
+```bash
+sudo apt install robotpkg-py27-crocoddyl # for Python 2
 
-		sudo apt install robotpkg-py35-crocoddyl # for Python 3
+sudo apt install robotpkg-py35-crocoddyl # for Python 3
+```
 
 Finally you will need to configure your environment variables, e.g.:
 
-		export PATH=/opt/openrobots/bin:$PATH
-		export PKG_CONFIG_PATH=/opt/openrobots/lib/pkgconfig:$PKG_CONFIG_PATH
-		export LD_LIBRARY_PATH=/opt/openrobots/lib:$LD_LIBRARY_PATH
-		export PYTHONPATH=/opt/openrobots/lib/python2.7/site-packages:$PYTHONPATH
+```bash
+export PATH=/opt/openrobots/bin:$PATH
+export PKG_CONFIG_PATH=/opt/openrobots/lib/pkgconfig:$PKG_CONFIG_PATH
+export LD_LIBRARY_PATH=/opt/openrobots/lib:$LD_LIBRARY_PATH
+export PYTHONPATH=/opt/openrobots/lib/python2.7/site-packages:$PYTHONPATH
+```
 
 
 ### Building from source
 
-**Crocoddyl** is c++ library with Python bindings for versatible and fast prototyping. It has the following dependecies:
+**Crocoddyl** is c++ library with Python bindings for versatile and fast prototyping. It has the following dependencies:
 
 * [pinocchio](https://github.com/stack-of-tasks/pinocchio)
 * [quadprog](https://pypi.org/project/quadprog/)
 * [multicontact-api](https://gepgitlab.laas.fr/loco-3d/multicontact-api)
-* [example-robot-data](https://gepgitlab.laas.fr/gepetto/example-robot-data) (optional for running examples, install Python loaders)
-* [gepetto-viewer-corba](https://github.com/Gepetto/gepetto-viewer-corba) (optional for running examples and notebooks)
-* [jupyter](https://jupyter.org/) (optional for running notebooks)
-* [matplotlib](https://matplotlib.org/) (optional for running examples)
+* [example-robot-data](https://gepgitlab.laas.fr/gepetto/example-robot-data) (optional for examples, install Python loaders)
+* [gepetto-viewer-corba](https://github.com/Gepetto/gepetto-viewer-corba) (optional for display)
+* [jupyter](https://jupyter.org/) (optional for notebooks)
+* [matplotlib](https://matplotlib.org/) (optional for examples)
 
 
-You can run examples and tests from the root dir of the repository:
+You can run examples, unit-tests and benchmarks from your build dir:
 
-		cd PATH_TO_CROCODDYL
-		python examples/talos_arm.py
-		python unittest/all.py
+```bash
+cd build
+make test
+make examples-bipedal_walk
+make benchmarks-bipedal_walk
+```
+
+If you want to see the 3D result and/or graphs of your run examples, you can use
+
+```bash
+export CROCODDYL_DISPLAY=1
+export CROCODDYL_PLOT=1
+```
 
 If you want to learn about Crocoddyl, take a look at the Jupyter notebooks. Start in the following order.
 - [examples/notebooks/unicycle_towards_origin.ipynb](https://gepgitlab.laas.fr/loco-3d/crocoddyl/blob/devel/examples/notebooks/unicycle_towards_origin.ipynb)
 - [examples/notebooks/cartpole_swing_up.ipynb](https://gepgitlab.laas.fr/loco-3d/crocoddyl/blob/devel/examples/notebooks/cartpole_swing_up.py)
-- [examples/notebooks/manipulator.ipynb](https://gepgitlab.laas.fr/loco-3d/crocoddyl/blob/devel/examples/notebooks/manipulator.ipynb)
-- [examples/notebooks/bipedal_walking_from_foot_traj.ipynb](https://gepgitlab.laas.fr/loco-3d/crocoddyl/blob/devel/examples/notebooks/bipedal_walking_from_foot_traj.ipynb)
+- [examples/notebooks/arm_manipulation.ipynb](https://gepgitlab.laas.fr/loco-3d/crocoddyl/blob/devel/examples/notebooks/arm_manipulation.ipynb)
+- [examples/notebooks/bipedal_walking.ipynb](https://gepgitlab.laas.fr/loco-3d/crocoddyl/blob/devel/examples/notebooks/bipedal_walking.ipynb)
 - [examples/notebooks/introduction_to_crocoddyl.ipynb](https://gepgitlab.laas.fr/loco-3d/crocoddyl/blob/devel/examples/notebooks/introduction_to_crocoddyl.ipynb)
 
 
 ## Citing Crocoddyl
 
 To cite **Crocoddyl** in your academic research, please use the following bibtex lines:
-```
+```tex
 @misc{crocoddylweb,
    author = {Carlos Mastalli, Rohan Budhiraja and Nicolas Mansard and others},
    title = {Crocoddyl: a fast and flexible optimal control library for robot control under contact sequence},
@@ -102,7 +122,7 @@ To cite **Crocoddyl** in your academic research, please use the following bibtex
 ```
 
 and the following one for the reference to the paper introducing **Crocoddyl**:
-```
+```tex
 @unpublished{mastalli2020crocoddyl,
   author={Mastalli, Carlos and Budhiraja, Rohan and Merkt, Wolfgang and Saurel, Guilhem and Hammoud, Bilal
   and Naveau, Maximilien and Carpentier, Justin and Vijayakumar, Sethu and Mansard, Nicolas},
@@ -125,3 +145,19 @@ The rest of the publications describes different component of **Crocoddyl**:
 
 You have a question or an issue? You may either directly open a [new issue](https://gepgitlab.laas.fr/loco-3d/crocoddyl/issues) or use the mailing list <crocoddyl@laas.fr>.
 
+
+## Credits
+
+The following people have been involved in the development of **Crocoddyl**:
+
+- [Carlos Mastalli](https://cmastalli.github.io/) (LAAS-CNRS): main developer and manager of the project
+- [Nicolas Mansard](http://projects.laas.fr/gepetto/index.php/Members/NicolasMansard) (LAAS-CNRS): project instructor
+- [Rohan Budhiraja](https://scholar.google.com/citations?user=NW9Io9AAAAAJ) (LAAS-CNRS): features extension
+- [Maximilien Naveau](https://scholar.google.fr/citations?user=y_-cGlUAAAAJ&hl=fr) (MPI): unit-test support
+- [Guilhem Saurel](http://projects.laas.fr/gepetto/index.php/Members/GuilhemSaurel) (LAAS-CNRS): continuous integration and deployment
+- [Bilal Hammoud](https://scholar.google.com/citations?hl=en&user=h_4NKpsAAAAJ) (MPI): features extension
+
+
+## Acknowledgments
+
+The development of **Pinocchio** is supported by the [EU MEMMO project](http://www.memmo-project.eu/), and the [Gepetto team](http://projects.laas.fr/gepetto/) [@LAAS-CNRS](http://www.laas.fr).
\ No newline at end of file
diff --git a/benchmark/CMakeLists.txt b/benchmark/CMakeLists.txt
index 88be29bb393bca4bc197febc185b6605228ea9fc..f110e1f19dc937fe9edcb6f3a70f1f692062694e 100644
--- a/benchmark/CMakeLists.txt
+++ b/benchmark/CMakeLists.txt
@@ -11,16 +11,16 @@ FOREACH(BENCHMARK_NAME ${${PROJECT_NAME}_BENCHMARK})
 ENDFOREACH(BENCHMARK_NAME ${${PROJECT_NAME}_BENCHMARK})
 
 SET(${PROJECT_NAME}_BENCHMARK_PYTHON
-  hyq
-  lqr
-  talos_arm
-  talos_legs
   unicycle
+  lqr
+  arm_manipulation
+  bipedal_walk
+  quadrupedal_gaits
   )
 
 FOREACH(benchmark ${${PROJECT_NAME}_BENCHMARK_PYTHON})
   PYTHON_BUILD(. "${benchmark}.py")
   ADD_CUSTOM_TARGET("benchmark-${benchmark}"
-    ${CMAKE_COMMAND} -E env PYTHONPATH=${PROJECT_BINARY_DIR}/bindings/python
+    ${CMAKE_COMMAND} -E env PYTHONPATH=$ENV{PYTHONPATH}:${PROJECT_BINARY_DIR}/bindings/python
     ${PYTHON_EXECUTABLE} -c "from ${benchmark} import *")
 ENDFOREACH(benchmark ${${PROJECT_NAME}_BENCHMARK_PYTHON})
diff --git a/benchmark/talos_arm.py b/benchmark/arm_manipulation.py
similarity index 100%
rename from benchmark/talos_arm.py
rename to benchmark/arm_manipulation.py
diff --git a/benchmark/talos_legs.py b/benchmark/bipedal_walk.py
similarity index 100%
rename from benchmark/talos_legs.py
rename to benchmark/bipedal_walk.py
diff --git a/benchmark/lqr.cpp b/benchmark/lqr.cpp
index b0463fe810741201e92b6a967a8bbefb8c843b4e..d71b9bc4c0a46c4f6d98c9a2cdf033b956e55774 100644
--- a/benchmark/lqr.cpp
+++ b/benchmark/lqr.cpp
@@ -55,8 +55,8 @@ int main() {
     clock_gettime(CLOCK_MONOTONIC, &start);
     ddp.solve(xs, us, MAXITER);
     clock_gettime(CLOCK_MONOTONIC, &finish);
-    elapsed = (finish.tv_sec - start.tv_sec) * 1000000.0;
-    elapsed += (finish.tv_nsec - start.tv_nsec) / 1000.0;
+    elapsed = static_cast<double>(finish.tv_sec - start.tv_sec) * 1000000;
+    elapsed += static_cast<double>(finish.tv_nsec - start.tv_nsec) / 1000;
     duration[i] = elapsed / 1000.;
   }
 
@@ -70,8 +70,8 @@ int main() {
     clock_gettime(CLOCK_MONOTONIC, &start);
     problem.calc(xs, us);
     clock_gettime(CLOCK_MONOTONIC, &finish);
-    elapsed = (finish.tv_sec - start.tv_sec) * 1000000.0;
-    elapsed += (finish.tv_nsec - start.tv_nsec) / 1000.0;
+    elapsed = static_cast<double>(finish.tv_sec - start.tv_sec) * 1000000;
+    elapsed += static_cast<double>(finish.tv_nsec - start.tv_nsec) / 1000;
     duration[i] = elapsed / 1000.;
   }
 
@@ -86,8 +86,8 @@ int main() {
     clock_gettime(CLOCK_MONOTONIC, &start);
     problem.calcDiff(xs, us);
     clock_gettime(CLOCK_MONOTONIC, &finish);
-    elapsed = (finish.tv_sec - start.tv_sec) * 1000000.0;
-    elapsed += (finish.tv_nsec - start.tv_nsec) / 1000.0;
+    elapsed = static_cast<double>(finish.tv_sec - start.tv_sec) * 1000000;
+    elapsed += static_cast<double>(finish.tv_nsec - start.tv_nsec) / 1000;
     duration[i] = elapsed / 1000.;
   }
 
diff --git a/benchmark/hyq.py b/benchmark/quadrupedal_gaits.py
similarity index 80%
rename from benchmark/hyq.py
rename to benchmark/quadrupedal_gaits.py
index 1f6a3e1fa35b689f8e35ea95f14b06f8a6b850cf..9c934f825e108d8f9e4c89bf5b15001e398587e9 100644
--- a/benchmark/hyq.py
+++ b/benchmark/quadrupedal_gaits.py
@@ -1,3 +1,4 @@
+import sys
 import time
 
 import example_robot_data
@@ -9,11 +10,28 @@ from crocoddyl.utils.quadruped import SimpleQuadrupedalGaitProblem
 
 T = int(5e3)  # number of trials
 MAXITER = 1
-GAIT = "walking"  # 115 nodes
-# GAIT = "trotting"  # 63 nodes
-# GAIT = "pacing"  # 63 nodes
-# GAIT = "bounding"  # 63 nodes
-# GAIT = "jumping"  # 61 nodes
+WALKING = 'walk' in sys.argv
+TROTTING = 'trot' in sys.argv
+PACING = 'pace' in sys.argv
+BOUNDING = 'bound' in sys.argv
+JUMPING = 'jump' in sys.argv
+
+GAIT = "walking"  # 104 nodes
+if WALKING:
+    print('running walking benchmark ...')
+    GAIT = "walking"  # 104 nodes
+if TROTTING:
+    print('running trotting benchmark ...')
+    GAIT = "trotting"  # 54 nodes
+if PACING:
+    print('running pacing benchmark ...')
+    GAIT = "pacing"  # 54 nodes
+if BOUNDING:
+    print('running bounding benchmark ...')
+    GAIT = "bounding"  # 54 nodes
+if JUMPING:
+    print('running jumping benchmark ...')
+    GAIT = "jumping"  # 61 nodes
 
 
 def runBenchmark(gait_phase):
@@ -69,11 +87,11 @@ def runBenchmark(gait_phase):
 if GAIT == 'walking':
     GAITPHASE = {
         'walking': {
-            'stepLength': 0.15,
-            'stepHeight': 0.2,
+            'stepLength': 0.25,
+            'stepHeight': 0.25,
             'timeStep': 1e-2,
             'stepKnots': 25,
-            'supportKnots': 5
+            'supportKnots': 2
         }
     }
 elif GAIT == 'trotting':
@@ -83,7 +101,7 @@ elif GAIT == 'trotting':
             'stepHeight': 0.2,
             'timeStep': 1e-2,
             'stepKnots': 25,
-            'supportKnots': 5
+            'supportKnots': 2
         }
     }
 elif GAIT == 'pacing':
@@ -93,17 +111,17 @@ elif GAIT == 'pacing':
             'stepHeight': 0.2,
             'timeStep': 1e-2,
             'stepKnots': 25,
-            'supportKnots': 5
+            'supportKnots': 2
         }
     }
 elif GAIT == 'bounding':
     GAITPHASE = {
         'bounding': {
-            'stepLength': 0.15,
-            'stepHeight': 0.2,
+            'stepLength': 0.007,
+            'stepHeight': 0.05,
             'timeStep': 1e-2,
             'stepKnots': 25,
-            'supportKnots': 5
+            'supportKnots': 12
         }
     }
 elif GAIT == 'jumping':
diff --git a/benchmark/unicycle.cpp b/benchmark/unicycle.cpp
index 970b9a53fdaf536cf29ee5039b81f50e6e8815c7..d1ec001ab9066e59832c3c472bd377448ba1f17c 100644
--- a/benchmark/unicycle.cpp
+++ b/benchmark/unicycle.cpp
@@ -56,8 +56,8 @@ int main() {
     clock_gettime(CLOCK_MONOTONIC, &start);
     ddp.solve(xs, us, MAXITER);
     clock_gettime(CLOCK_MONOTONIC, &finish);
-    elapsed = (finish.tv_sec - start.tv_sec) * 1000000.0;
-    elapsed += (finish.tv_nsec - start.tv_nsec) / 1000.0;
+    elapsed = static_cast<double>(finish.tv_sec - start.tv_sec) * 1000000;
+    elapsed += static_cast<double>(finish.tv_nsec - start.tv_nsec) / 1000;
     duration[i] = elapsed / 1000.;
   }
 
@@ -72,8 +72,8 @@ int main() {
     clock_gettime(CLOCK_MONOTONIC, &start);
     problem.calc(xs, us);
     clock_gettime(CLOCK_MONOTONIC, &finish);
-    elapsed = (finish.tv_sec - start.tv_sec) * 1000000.0;
-    elapsed += (finish.tv_nsec - start.tv_nsec) / 1000.0;
+    elapsed = static_cast<double>(finish.tv_sec - start.tv_sec) * 1000000;
+    elapsed += static_cast<double>(finish.tv_nsec - start.tv_nsec) / 1000;
     duration[i] = elapsed / 1000.;
   }
 
@@ -88,8 +88,8 @@ int main() {
     clock_gettime(CLOCK_MONOTONIC, &start);
     problem.calcDiff(xs, us);
     clock_gettime(CLOCK_MONOTONIC, &finish);
-    elapsed = (finish.tv_sec - start.tv_sec) * 1000000.0;
-    elapsed += (finish.tv_nsec - start.tv_nsec) / 1000.0;
+    elapsed = static_cast<double>(finish.tv_sec - start.tv_sec) * 1000000;
+    elapsed += static_cast<double>(finish.tv_nsec - start.tv_nsec) / 1000;
     duration[i] = elapsed / 1000.;
   }
 
diff --git a/bindings/python/crocoddyl/__init__.py b/bindings/python/crocoddyl/__init__.py
index 908fcad3ba6a9768ee83d699714a9ac45321b345..0727003b9e848b960855fd24626a5abdcef3e243 100644
--- a/bindings/python/crocoddyl/__init__.py
+++ b/bindings/python/crocoddyl/__init__.py
@@ -38,7 +38,7 @@ def displayTrajectory(robot, xs, dt=0.1, rate=-1, cameraTF=None):
             time.sleep(dt)
 
 
-class CallbackSolverDisplay(libcrocoddyl_pywrap.CallbackAbstract):
+class CallbackDisplay(libcrocoddyl_pywrap.CallbackAbstract):
     def __init__(self, robotwrapper, rate=-1, freq=1, cameraTF=None):
         libcrocoddyl_pywrap.CallbackAbstract.__init__(self)
         self.robotwrapper = robotwrapper
@@ -53,7 +53,7 @@ class CallbackSolverDisplay(libcrocoddyl_pywrap.CallbackAbstract):
         displayTrajectory(self.robotwrapper, solver.xs, dt, self.rate, self.cameraTF)
 
 
-class CallbackSolverLogger(libcrocoddyl_pywrap.CallbackAbstract):
+class CallbackLogger(libcrocoddyl_pywrap.CallbackAbstract):
     def __init__(self):
         libcrocoddyl_pywrap.CallbackAbstract.__init__(self)
         self.steps = []
@@ -77,6 +77,70 @@ class CallbackSolverLogger(libcrocoddyl_pywrap.CallbackAbstract):
         self.costs.append(solver.cost)
         self.control_regs.append(solver.u_reg)
         self.state_regs.append(solver.x_reg)
-        self.th_stops.append(solver.stoppingCriteria)
+        self.th_stops.append(solver.stoppingCriteria())
         self.gm_stops.append(-np.asscalar(solver.expectedImprovement()[1]))
-        self.gaps.append(copy.copy(solver.gaps))
\ No newline at end of file
+        self.gaps.append(copy.copy(solver.gaps))
+
+
+def plotOCSolution(xs, us, figIndex=1, show=True):
+    import matplotlib.pyplot as plt
+    import numpy as np
+    # Getting the state and control trajectories
+    nx, nu = xs[0].shape[0], us[0].shape[0]
+    X = [0.] * nx
+    U = [0.] * nu
+    for i in range(nx):
+        X[i] = [np.asscalar(x[i]) for x in xs]
+    for i in range(nu):
+        U[i] = [np.asscalar(u[i]) for u in us]
+
+    plt.figure(figIndex)
+
+    # Plotting the state trajectories
+    plt.subplot(211)
+    [plt.plot(X[i], label='x' + str(i)) for i in range(nx)]
+    plt.legend()
+
+    # Plotting the control commands
+    plt.subplot(212)
+    [plt.plot(U[i], label='u' + str(i)) for i in range(nu)]
+    plt.legend()
+    plt.xlabel('knots')
+    if show:
+        plt.show()
+
+
+def plotConvergence(costs, muLM, muV, gamma, theta, alpha, figIndex=1, show=True, figTitle=""):
+    import matplotlib.pyplot as plt
+    import numpy as np
+
+    plt.figure(figIndex, figsize=(6.4, 8))
+    plt.suptitle(figTitle, fontsize=14)
+    # Plotting the total cost sequence
+    plt.subplot(511)
+    plt.ylabel('cost')
+    plt.plot(costs)
+
+    # Ploting mu sequences
+    plt.subplot(512)
+    plt.ylabel('mu')
+    plt.plot(muLM, label='LM')
+    plt.plot(muV, label='V')
+    plt.legend()
+
+    # Plotting the gradient sequence (gamma and theta)
+    plt.subplot(513)
+    plt.ylabel('gamma')
+    plt.plot(gamma)
+    plt.subplot(514)
+    plt.ylabel('theta')
+    plt.plot(theta)
+
+    # Plotting the alpha sequence
+    plt.subplot(515)
+    plt.ylabel('alpha')
+    ind = np.arange(len(alpha))
+    plt.bar(ind, alpha)
+    plt.xlabel('iteration')
+    if show:
+        plt.show()
diff --git a/bindings/python/crocoddyl/core/solver-base.hpp b/bindings/python/crocoddyl/core/solver-base.hpp
index 0b4f3239a7df850641ea8d2b14f64f1ced9e9615..94bb47d2a059569cdd64d34d43370c09b25fb887 100644
--- a/bindings/python/crocoddyl/core/solver-base.hpp
+++ b/bindings/python/crocoddyl/core/solver-base.hpp
@@ -158,6 +158,10 @@ void exposeSolverAbstract() {
            "Each iteration, the solver calls these set of functions in order to\n"
            "allowed user the diagnostic of the solver's performance.\n"
            ":param callbacks: set of callback functions.")
+      .def("getCallbacks", &SolverAbstract_wrap::getCallbacks, bp::return_value_policy<bp::return_by_value>(),
+           bp::args(" self"),
+           "Return the list of callback functions using for diagnostic.\n\n"
+           ":return set of callback functions.")
       .add_property("problem", bp::make_function(&SolverAbstract_wrap::get_problem, bp::return_internal_reference<>()),
                     "shooting problem")
       .def("models", &SolverAbstract_wrap::get_models, bp::return_value_policy<bp::return_by_value>(), "models")
diff --git a/bindings/python/crocoddyl/core/solvers/ddp.hpp b/bindings/python/crocoddyl/core/solvers/ddp.hpp
index 70778bc59f153206b5075aeeaa6cd1c14d15aeeb..368cff8754bd9905cfb5ad419e4ed70fc8e6ad3c 100644
--- a/bindings/python/crocoddyl/core/solvers/ddp.hpp
+++ b/bindings/python/crocoddyl/core/solvers/ddp.hpp
@@ -98,7 +98,29 @@ void exposeSolverDDP() {
       .add_property("K", make_function(&SolverDDP::get_K, bp::return_value_policy<bp::copy_const_reference>()), "K")
       .add_property("k", make_function(&SolverDDP::get_k, bp::return_value_policy<bp::copy_const_reference>()), "k")
       .add_property("gaps", make_function(&SolverDDP::get_gaps, bp::return_value_policy<bp::copy_const_reference>()),
-                    "gaps");
+                    "gaps")
+      .add_property("regFactor",
+                    bp::make_function(&SolverDDP::get_regfactor, bp::return_value_policy<bp::copy_const_reference>()),
+                    bp::make_function(&SolverDDP::set_regfactor),
+                    "regularization factor used for increasing or decreasing the value.")
+      .add_property("regMin",
+                    bp::make_function(&SolverDDP::get_regmin, bp::return_value_policy<bp::copy_const_reference>()),
+                    bp::make_function(&SolverDDP::set_regmin), "minimum regularization value.")
+      .add_property("regMax",
+                    bp::make_function(&SolverDDP::get_regmax, bp::return_value_policy<bp::copy_const_reference>()),
+                    bp::make_function(&SolverDDP::set_regmax), "maximum regularization value.")
+      .add_property("th_step",
+                    bp::make_function(&SolverDDP::get_th_step, bp::return_value_policy<bp::copy_const_reference>()),
+                    bp::make_function(&SolverDDP::set_th_step),
+                    "threshold for decreasing the regularization after approving a step (higher values decreases the "
+                    "regularization)")
+      .add_property("th_grad",
+                    bp::make_function(&SolverDDP::get_th_grad, bp::return_value_policy<bp::copy_const_reference>()),
+                    bp::make_function(&SolverDDP::set_th_grad),
+                    "threshold for accepting step which gradients is lower than this value")
+      .add_property("alphas",
+                    bp::make_function(&SolverDDP::get_alphas, bp::return_value_policy<bp::copy_const_reference>()),
+                    bp::make_function(&SolverDDP::set_alphas), "list of step length (alpha) values");
 }
 
 }  // namespace python
diff --git a/bindings/python/crocoddyl/core/solvers/fddp.hpp b/bindings/python/crocoddyl/core/solvers/fddp.hpp
index 0223d34e5833ce2a68a759543831af51b31fcd17..4fee4488529cab29b1b0f59602d49f7326f72eb6 100644
--- a/bindings/python/crocoddyl/core/solvers/fddp.hpp
+++ b/bindings/python/crocoddyl/core/solvers/fddp.hpp
@@ -17,13 +17,13 @@ namespace python {
 namespace bp = boost::python;
 
 BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(SolverFDDP_solves, SolverFDDP::solve, 0, 5)
-BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(SolverFDDP_computeDirections, SolverFDDP::computeDirection, 0, 1)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(SolverFDDP_computeDirections, SolverDDP::computeDirection, 0, 1)
 BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(SolverFDDP_trySteps, SolverFDDP::tryStep, 0, 1)
 
 void exposeSolverFDDP() {
   bp::class_<SolverFDDP, bp::bases<SolverDDP> >(
       "SolverFDDP",
-      "FDDP solver.\n\n"
+      "Feasibility-prone DDP (FDDP) solver.\n\n"
       "The FDDP solver computes an optimal trajectory and control commands by iterates\n"
       "running backward and forward passes. The backward-pass updates locally the\n"
       "quadratic approximation of the problem and computes descent direction,\n"
@@ -48,30 +48,47 @@ void exposeSolverFDDP() {
                ":param regInit: initial guess for the regularization value. Very low values are typical\n"
                "                used with very good guess points (init_xs, init_us).\n"
                ":returns the optimal trajectory xopt, uopt and a boolean that describes if convergence was reached."))
-
+      .def("computeDirection", &SolverFDDP::computeDirection,
+           SolverFDDP_computeDirections(
+               bp::args(" self", " recalc=True"),
+               "Compute the search direction (dx, du) for the current guess (xs, us).\n\n"
+               "You must call setCandidate first in order to define the current\n"
+               "guess. A current guess defines a state and control trajectory\n"
+               "(xs, us) of T+1 and T elements, respectively.\n"
+               ":params recalc: true for recalculating the derivatives at current state and control.\n"
+               ":returns the search direction dx, du and the dual lambdas as lists of T+1, T and T+1 lengths."))
+      .def("tryStep", &SolverFDDP::tryStep,
+           SolverFDDP_trySteps(bp::args(" self", " stepLength=1"),
+                               "Rollout the system with a predefined step length.\n\n"
+                               ":param stepLength: step length\n"
+                               ":returns the cost improvement."))
       .def("expectedImprovement", &SolverFDDP::expectedImprovement,
            bp::return_value_policy<bp::copy_const_reference>(), bp::args(" self"),
            "Return two scalars denoting the quadratic improvement model\n\n"
            "For computing the expected improvement, you need to compute first\n"
            "the search direction by running computeDirection. The quadratic\n"
-           "improvement model is described as dV = f_0 - f_+ = d1*a + d2*a**2/2.")
+           "improvement model is described as dV = f_0 - f_+ = d1*a + d2*a**2/2.\n"
+           "Additionally, you need to update the expected model by running\n"
+           "updateExpectedImprovement.")
+      .def("updateExpectedImprovement", &SolverFDDP::updateExpectedImprovement,
+           bp::return_value_policy<bp::copy_const_reference>(), bp::args(" self"),
+           "Update the expected improvement model\n\n")
       .def("calc", &SolverFDDP::calc, bp::args(" self"),
            "Update the Jacobian and Hessian of the optimal control problem\n\n"
            "These derivatives are computed around the guess state and control\n"
            "trajectory. These trajectory can be set by using setCandidate.\n"
            ":return the total cost around the guess trajectory.")
-      .def("backwardPass", &SolverFDDP::backwardPass, bp::args(" self"),
-           "Run the backward pass (Riccati sweep)\n\n"
-           "It assumes that the Jacobian and Hessians of the optimal control problem have been\n"
-           "compute. These terms are computed by running calc.")
       .def("forwardPass", &SolverFDDP::forwardPass, bp::args(" self", " stepLength=1"),
            "Run the forward pass or rollout\n\n"
            "It rollouts the action model give the computed policy (feedfoward terns and feedback\n"
            "gains) by the backwardPass. We can define different step lengths\n"
-           ":param stepLength: applied step length (<= 1. and >= 0.)");
+           ":param stepLength: applied step length (<= 1. and >= 0.)")
+      .add_property("th_acceptNegStep", bp::make_function(&SolverFDDP::get_th_acceptnegstep),
+                    bp::make_function(&SolverFDDP::set_th_acceptnegstep),
+                    "threshold for step acceptance in ascent direction");
 }
 
 }  // namespace python
 }  // namespace crocoddyl
 
-#endif  // BINDINGS_PYTHON_CROCODDYL_CORE_SOLVERS_DDP_HPP_
+#endif  // BINDINGS_PYTHON_CROCODDYL_CORE_SOLVERS_FDDP_HPP_
diff --git a/bindings/python/crocoddyl/crocoddyl.cpp b/bindings/python/crocoddyl/crocoddyl.cpp
index 2deaf62ff45b7d4c08a68657f4b4a015a8abbb2d..f7d0a54746129c2c3c817a21cc2aae85180ae95c 100644
--- a/bindings/python/crocoddyl/crocoddyl.cpp
+++ b/bindings/python/crocoddyl/crocoddyl.cpp
@@ -37,10 +37,13 @@ BOOST_PYTHON_MODULE(libcrocoddyl_pywrap) {
   eigenpy::enableEigenPySpecific<VectorX>();
   eigenpy::enableEigenPySpecific<MatrixX>();
 
-  // Register Eigen converters between std::vector and Python list
+  // Register converters between std::vector and Python list
+  // TODO(cmastalli): figure out how to convert std::vector<double> to Python list
+  // bp::to_python_converter<std::vector<double, std::allocator<double> >, vector_to_list<double, false>, true>();
   bp::to_python_converter<std::vector<VectorX, std::allocator<VectorX> >, vector_to_list<VectorX, false>, true>();
   bp::to_python_converter<std::vector<MatrixX, std::allocator<MatrixX> >, vector_to_list<MatrixX, false>, true>();
   list_to_vector()
+      .from_python<std::vector<double, std::allocator<double> > >()
       .from_python<std::vector<VectorX, std::allocator<VectorX> > >()
       .from_python<std::vector<MatrixX, std::allocator<MatrixX> > >();
 
diff --git a/bindings/python/crocoddyl/utils.hpp b/bindings/python/crocoddyl/utils.hpp
index 0039a64aae093e49b700bad4a13c98a832fab0d8..96198af666086d115e96135981c46f7c70e849fc 100644
--- a/bindings/python/crocoddyl/utils.hpp
+++ b/bindings/python/crocoddyl/utils.hpp
@@ -25,12 +25,13 @@ namespace bp = boost::python;
 template <class T, bool NoProxy = true>
 struct vector_to_list {
   static PyObject* convert(const std::vector<T>& vec) {
+    typedef typename std::vector<T>::const_iterator const_iter;
     bp::list* l = new boost::python::list();
-    for (size_t i = 0; i < vec.size(); i++) {
+    for (const_iter it = vec.begin(); it != vec.end(); ++it) {
       if (NoProxy) {
-        l->append(boost::ref(vec[i]));
+        l->append(boost::ref(*it));
       } else {
-        l->append(vec[i]);
+        l->append(*it);
       }
     }
     return l->ptr();
diff --git a/bindings/python/crocoddyl/utils/__init__.py b/bindings/python/crocoddyl/utils/__init__.py
index aacf275ef38c8a260ecde5e3ab8bf6ff97059530..c25c914d4a03d6abe1bd59790bb402991ee5c240 100644
--- a/bindings/python/crocoddyl/utils/__init__.py
+++ b/bindings/python/crocoddyl/utils/__init__.py
@@ -640,7 +640,7 @@ class DDPDerived(crocoddyl.SolverAbstract):
         crocoddyl.SolverAbstract.__init__(self, shootingProblem)
         self.allocateData()  # TODO remove it?
 
-        self.isFeasible = False  # Change it to true if you know that datas[t].xnext = xs[t+1]
+        self.isFeasible = False
         self.alphas = [2**(-n) for n in range(10)]
         self.th_grad = 1e-12
 
@@ -654,8 +654,6 @@ class DDPDerived(crocoddyl.SolverAbstract):
     def calc(self):
         self.cost = self.problem.calcDiff(self.xs, self.us)
         if not self.isFeasible:
-            # Gap store the state defect from the guess to feasible (rollout) trajectory, i.e.
-            #   gap = x_rollout [-] x_guess = DIFF(x_guess, x_rollout)
             self.gaps[0] = self.problem.runningModels[0].state.diff(self.xs[0], self.problem.x0)
             for i, (m, d, x) in enumerate(zip(self.problem.runningModels, self.problem.runningDatas, self.xs[1:])):
                 self.gaps[i + 1] = m.state.diff(x, d.xnext)
@@ -728,8 +726,6 @@ class DDPDerived(crocoddyl.SolverAbstract):
 
             if self.wasFeasible and self.stop < self.th_stop:
                 return self.xs, self.us, True
-
-        # Warning: no convergence in max iterations
         return self.xs, self.us, False
 
     def increaseRegularization(self):
@@ -744,7 +740,6 @@ class DDPDerived(crocoddyl.SolverAbstract):
             self.x_reg = self.regMin
         self.u_reg = self.x_reg
 
-    # DDP Specific
     def allocateData(self):
         self.Vxx = [a2m(np.zeros([m.state.ndx, m.state.ndx])) for m in self.models()]
         self.Vx = [a2m(np.zeros([m.state.ndx])) for m in self.models()]
@@ -774,17 +769,16 @@ class DDPDerived(crocoddyl.SolverAbstract):
             ndx = self.problem.terminalModel.state.ndx
             self.Vxx[-1][range(ndx), range(ndx)] += self.x_reg
 
+        # Compute and store the Vx gradient at end of the interval (rollout state)
+        if not self.isFeasible:
+            self.Vx[-1] += np.dot(self.Vxx[-1], self.gaps[-1])
+
         for t, (model, data) in rev_enumerate(zip(self.problem.runningModels, self.problem.runningDatas)):
             self.Qxx[t][:, :] = data.Lxx + data.Fx.T * self.Vxx[t + 1] * data.Fx
             self.Qxu[t][:, :] = data.Lxu + data.Fx.T * self.Vxx[t + 1] * data.Fu
             self.Quu[t][:, :] = data.Luu + data.Fu.T * self.Vxx[t + 1] * data.Fu
             self.Qx[t][:] = data.Lx + data.Fx.T * self.Vx[t + 1]
             self.Qu[t][:] = data.Lu + data.Fu.T * self.Vx[t + 1]
-            if not self.isFeasible:
-                # In case the xt+1 are not f(xt,ut) i.e warm start not obtained from roll-out.
-                relinearization = self.Vxx[t + 1] * self.gaps[t + 1]
-                self.Qx[t][:] += data.Fx.T * relinearization
-                self.Qu[t][:] += data.Fu.T * relinearization
 
             if self.u_reg != 0:
                 self.Quu[t][range(model.nu), range(model.nu)] += self.u_reg
@@ -800,6 +794,11 @@ class DDPDerived(crocoddyl.SolverAbstract):
 
             if self.x_reg != 0:
                 self.Vxx[t][range(model.state.ndx), range(model.state.ndx)] += self.x_reg
+
+            # Compute and store the Vx gradient at end of the interval (rollout state)
+            if not self.isFeasible:
+                self.Vx[t] += np.dot(self.Vxx[t], self.gaps[t])
+
             raiseIfNan(self.Vxx[t], ArithmeticError('backward error'))
             raiseIfNan(self.Vx[t], ArithmeticError('backward error'))
 
@@ -815,8 +814,6 @@ class DDPDerived(crocoddyl.SolverAbstract):
             raise ArithmeticError('backward error')
 
     def forwardPass(self, stepLength, warning='ignore'):
-        # Argument warning is also introduce for debug: by default, it masks the numpy warnings
-        #    that can be reactivated during debug.
         xs, us = self.xs, self.us
         xtry, utry = self.xs_try, self.us_try
         ctry = 0
@@ -837,3 +834,145 @@ class DDPDerived(crocoddyl.SolverAbstract):
         raiseIfNan(ctry, ArithmeticError('forward error'))
         self.cost_try = ctry
         return xtry, utry, ctry
+
+
+class FDDPDerived(DDPDerived):
+    def __init__(self, shootingProblem):
+        DDPDerived.__init__(self, shootingProblem)
+
+        self.th_acceptNegStep = 2.
+        self.dg = 0.
+        self.dq = 0.
+        self.dv = 0.
+
+    def solve(self, init_xs=[], init_us=[], maxiter=100, isFeasible=False, regInit=None):
+        self.setCandidate(init_xs, init_us, isFeasible)
+        self.x_reg = regInit if regInit is not None else self.regMin
+        self.u_reg = regInit if regInit is not None else self.regMin
+        self.wasFeasible = False
+        for i in range(maxiter):
+            recalc = True
+            while True:
+                try:
+                    self.computeDirection(recalc=recalc)
+                except ArithmeticError:
+                    recalc = False
+                    self.increaseRegularization()
+                    if self.x_reg == self.regMax:
+                        return self.xs, self.us, False
+                    else:
+                        continue
+                break
+            self.updateExpectedImprovement()
+
+            for a in self.alphas:
+                try:
+                    self.dV = self.tryStep(a)
+                except ArithmeticError:
+                    continue
+                d1, d2 = self.expectedImprovement()
+
+                self.dV_exp = a * (d1 + .5 * d2 * a)
+                if self.dV_exp > 0.:  # descend direction
+                    if d1 < self.th_grad or self.dV > self.th_acceptStep * self.dV_exp:
+                        self.wasFeasible = self.isFeasible
+                        self.setCandidate(self.xs_try, self.us_try, (self.wasFeasible or a == 1))
+                        self.cost = self.cost_try
+                        break
+                else:  # reducing the gaps by allowing a small increment in the cost value
+                    if d1 < self.th_grad or self.dV < self.th_acceptNegStep * self.dV_exp:
+                        self.wasFeasible = self.isFeasible
+                        self.setCandidate(self.xs_try, self.us_try, (self.wasFeasible or a == 1))
+                        self.cost = self.cost_try
+                        break
+            if a > self.th_step:
+                self.decreaseRegularization()
+            if a == self.alphas[-1]:
+                self.increaseRegularization()
+                if self.x_reg == self.regMax:
+                    return self.xs, self.us, False
+            self.stepLength = a
+            self.iter = i
+            self.stop = self.stoppingCriteria()
+            # TODO @Carlos bind the callbacks
+            # if self.callback is not None:
+            #     [c(self) for c in self.callback]
+
+            if self.wasFeasible and self.stop < self.th_stop:
+                return self.xs, self.us, True
+        return self.xs, self.us, False
+
+    def computeDirection(self, recalc=True):
+        if recalc:
+            self.calc()
+        self.backwardPass()
+        return [np.nan] * (self.problem.T + 1), self.k, self.Vx
+
+    def tryStep(self, stepLength=1):
+        self.forwardPass(stepLength)
+        return self.cost - self.cost_try
+
+    def updateExpectedImprovement(self):
+        self.dg = 0.
+        self.dq = 0.
+        if not self.isFeasible:
+            self.dg -= np.asscalar(self.Vx[-1].T * self.gaps[-1])
+            self.dq += np.asscalar(self.gaps[-1].T * self.Vxx[-1] * self.gaps[-1])
+        for t in range(self.problem.T):
+            self.dg += np.asscalar(self.Qu[t].T * self.k[t])
+            self.dq -= np.asscalar(self.k[t].T * self.Quu[t] * self.k[t])
+            if not self.isFeasible:
+                self.dg -= np.asscalar(self.Vx[t].T * self.gaps[t])
+                self.dq += np.asscalar(self.gaps[t].T * self.Vxx[t] * self.gaps[t])
+
+    def expectedImprovement(self):
+        self.dv = 0.
+        if not self.isFeasible:
+            dx = self.problem.runningModels[-1].state.diff(self.xs_try[-1], self.xs[-1])
+            self.dv -= np.asscalar(self.gaps[-1].T * self.Vxx[-1] * dx)
+            for t in range(self.problem.T):
+                dx = self.problem.runningModels[t].state.diff(self.xs_try[t], self.xs[t])
+                self.dv -= np.asscalar(self.gaps[t].T * self.Vxx[t] * dx)
+        d1 = self.dg + self.dv
+        d2 = self.dq - 2 * self.dv
+        return np.matrix([d1, d2]).T
+
+    def calc(self):
+        self.cost = self.problem.calcDiff(self.xs, self.us)
+        if not self.isFeasible:
+            self.gaps[0] = self.problem.runningModels[0].state.diff(self.xs[0], self.problem.x0)
+            for i, (m, d, x) in enumerate(zip(self.problem.runningModels, self.problem.runningDatas, self.xs[1:])):
+                self.gaps[i + 1] = m.state.diff(x, d.xnext)
+        elif not self.wasFeasible:
+            self.gaps[:] = [np.zeros_like(f) for f in self.gaps]
+        return self.cost
+
+    def forwardPass(self, stepLength, warning='ignore'):
+        xs, us = self.xs, self.us
+        xtry, utry = self.xs_try, self.us_try
+        ctry = 0
+        xnext = self.problem.x0
+        for t, (m, d) in enumerate(zip(self.problem.runningModels, self.problem.runningDatas)):
+            if self.isFeasible or stepLength == 1:
+                xtry[t] = xnext.copy()
+            else:
+                xtry[t] = m.state.integrate(xnext, self.gaps[t] * (stepLength - 1))
+            utry[t] = us[t] - self.k[t] * stepLength - self.K[t] * m.state.diff(xs[t], xtry[t])
+            with np.warnings.catch_warnings():
+                np.warnings.simplefilter(warning)
+                m.calc(d, xtry[t], utry[t])
+                xnext, cost = d.xnext, d.cost
+            ctry += cost
+            raiseIfNan([ctry, cost], ArithmeticError('forward error'))
+            raiseIfNan(xnext, ArithmeticError('forward error'))
+        if self.isFeasible or stepLength == 1:
+            xtry[-1] = xnext.copy()
+        else:
+            xtry[-1] = self.problem.terminalModel.state.integrate(xnext, self.gaps[-1] * (stepLength - 1))
+        with np.warnings.catch_warnings():
+            np.warnings.simplefilter(warning)
+            self.problem.terminalModel.calc(self.problem.terminalData, xtry[-1])
+            ctry += self.problem.terminalData.cost
+        raiseIfNan(ctry, ArithmeticError('forward error'))
+        self.cost_try = ctry
+        return xtry, utry, ctry
diff --git a/bindings/python/crocoddyl/utils/biped.py b/bindings/python/crocoddyl/utils/biped.py
index ffb92cfb87cebffa9d056ed6e827456609e73781..6274c10d95960cad49ba8371b4c50e2a2993e03e 100644
--- a/bindings/python/crocoddyl/utils/biped.py
+++ b/bindings/python/crocoddyl/utils/biped.py
@@ -39,7 +39,7 @@ class SimpleBipedGaitProblem:
         rfPos0 = self.rdata.oMf[self.rfId].translation
         lfPos0 = self.rdata.oMf[self.lfId].translation
         comRef = (rfPos0 + lfPos0) / 2
-        comRef[2] = 0.6185
+        comRef[2] = np.asscalar(pinocchio.centerOfMass(self.rmodel, self.rdata, q0)[2])
 
         # Defining the action models along the time instances
         loco3dModel = []
@@ -63,6 +63,56 @@ class SimpleBipedGaitProblem:
         problem = crocoddyl.ShootingProblem(x0, loco3dModel, loco3dModel[-1])
         return problem
 
+    def createJumpingProblem(self, x0, jumpHeight, jumpLength, timeStep, groundKnots, flyingKnots, final=False):
+        q0 = x0[:self.rmodel.nq]
+        pinocchio.forwardKinematics(self.rmodel, self.rdata, q0)
+        pinocchio.updateFramePlacements(self.rmodel, self.rdata)
+        rfFootPos0 = self.rdata.oMf[self.rfId].translation
+        lfFootPos0 = self.rdata.oMf[self.lfId].translation
+        df = jumpLength[2] - rfFootPos0[2]
+        rfFootPos0[2] = 0.
+        lfFootPos0[2] = 0.
+        comRef = (rfFootPos0 + lfFootPos0) / 2
+        comRef[2] = np.asscalar(pinocchio.centerOfMass(self.rmodel, self.rdata, q0)[2])
+
+        self.rWeight = 1e1
+        loco3dModel = []
+        takeOff = [self.createSwingFootModel(
+            timeStep,
+            [self.lfId, self.rfId],
+        ) for k in range(groundKnots)]
+        flyingUpPhase = [
+            self.createSwingFootModel(
+                timeStep, [],
+                np.matrix([jumpLength[0], jumpLength[1], jumpLength[2] + jumpHeight]).T * (k + 1) / flyingKnots +
+                comRef) for k in range(flyingKnots)
+        ]
+        flyingDownPhase = []
+        for k in range(flyingKnots):
+            flyingDownPhase += [self.createSwingFootModel(timeStep, [])]
+
+        f0 = np.matrix(jumpLength).T
+        footTask = [
+            crocoddyl.FramePlacement(self.lfId, pinocchio.SE3(np.eye(3), self.lfId + f0)),
+            crocoddyl.FramePlacement(self.rfId, pinocchio.SE3(np.eye(3), self.rfId + f0))
+        ]
+        landingPhase = [self.createFootSwitchModel([self.lfId, self.rfId], footTask, False)]
+        f0[2] = df
+        if final is True:
+            self.rWeight = 1e4
+        landed = [
+            self.createSwingFootModel(timeStep, [self.lfId, self.rfId], comTask=comRef + f0)
+            for k in range(groundKnots)
+        ]
+        loco3dModel += takeOff
+        loco3dModel += flyingUpPhase
+        loco3dModel += flyingDownPhase
+        loco3dModel += landingPhase
+        loco3dModel += landed
+
+        problem = crocoddyl.ShootingProblem(x0, loco3dModel, loco3dModel[-1])
+        return problem
+
     def createFootstepModels(self, comPos0, feetPos0, stepLength, stepHeight, timeStep, numKnots, supportFootIds,
                              swingFootIds):
         """ Action models for a footstep phase.
@@ -108,8 +158,7 @@ class SimpleBipedGaitProblem:
             ]
 
         # Action model for the foot switch
-        footSwitchModel = \
-            self.createFootSwitchModel(supportFootIds, swingFootTask)
+        footSwitchModel = self.createFootSwitchModel(supportFootIds, swingFootTask)
 
         # Updating the current foot position for next step
         comPos0 += np.matrix([stepLength * comPercentage, 0., 0.]).T
@@ -160,13 +209,28 @@ class SimpleBipedGaitProblem:
         model = crocoddyl.IntegratedActionModelEuler(dmodel, timeStep)
         return model
 
-    def createFootSwitchModel(self, supportFootIds, swingFootTask):
+    def createFootSwitchModel(self, supportFootIds, swingFootTask, pseudoImpulse=True):
         """ Action model for a foot switch phase.
 
         :param supportFootIds: Ids of the constrained feet
         :param swingFootTask: swinging foot task
+        :param pseudoImpulse: true for pseudo impulse models, otherwise it uses the impulse model
         :return action model for a foot switch phase
         """
+        if pseudoImpulse:
+            return self.createPseudoImpulseModel(supportFootIds, swingFootTask)
+        else:
+            return self.createImpulseModel(supportFootIds, swingFootTask)
+
+    def createPseudoImpulseModel(self, supportFootIds, swingFootTask):
+        """ Action model for pseudo-impulse models.
+
+      A pseudo-impulse model consists of adding high-penalty cost for the contact velocities.
+        :param supportFootIds: Ids of the constrained feet
+        :param swingFootTask: swinging foot task
+        :return pseudo-impulse differential action model
+        """
+
         # Creating a 6D multi-contact model, and then including the supporting
         # foot
         contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu)
@@ -182,8 +246,8 @@ class SimpleBipedGaitProblem:
                 footTrack = crocoddyl.CostModelFramePlacement(self.state, i, self.actuation.nu)
                 costModel.addCost("footTrack_" + str(i), footTrack, 1e8)
                 footVel = crocoddyl.FrameMotion(i.frame, pinocchio.Motion.Zero())
-                impactFootVelCost = crocoddyl.CostModelFrameVelocity(self.state, footVel, self.actuation.nu)
-                costModel.addCost('impactVel_' + str(i.frame), impactFootVelCost, 1e6)
+                impulseFootVelCost = crocoddyl.CostModelFrameVelocity(self.state, footVel, self.actuation.nu)
+                costModel.addCost('impulseVel_' + str(i.frame), impulseFootVelCost, 1e6)
 
         stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] * (self.state.nv - 6) + [10] * self.state.nv)
         stateReg = crocoddyl.CostModelState(self.state,
@@ -200,6 +264,38 @@ class SimpleBipedGaitProblem:
         model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.)
         return model
 
+    def createImpulseModel(self, supportFootIds, swingFootTask):
+        """ Action model for impulse models.
+
+        An impulse model consists of describing the impulse dynamics against a set of contacts.
+        :param supportFootIds: Ids of the constrained feet
+        :param swingFootTask: swinging foot task
+        :return impulse action model
+        """
+        # Creating a 6D multi-contact model, and then including the supporting foot
+        impulseModel = crocoddyl.ImpulseModelMultiple(self.state)
+        for i in supportFootIds:
+            supportContactModel = crocoddyl.ImpulseModel6D(self.state, i)
+            impulseModel.addImpulse("impulse_" + str(i), supportContactModel)
+
+        # Creating the cost model for a contact phase
+        costModel = crocoddyl.CostModelSum(self.state, 0)
+        if swingFootTask is not None:
+            for i in swingFootTask:
+                xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation)
+                footTrack = crocoddyl.CostModelFrameTranslation(self.state, xref, 0)
+                costModel.addCost("footTrack_" + str(i), footTrack, 1e8)
+        stateWeights = np.array([1.] * 6 + [0.1] * (self.rmodel.nv - 6) + [10] * self.rmodel.nv)
+        stateReg = crocoddyl.CostModelState(self.state,
+                                            crocoddyl.ActivationModelWeightedQuad(np.matrix(stateWeights**2).T),
+                                            self.rmodel.defaultState, 0)
+        costModel.addCost("stateReg", stateReg, 1e1)
+
+        # Creating the action model for the KKT dynamics with simpletic Euler
+        # integration scheme
+        model = crocoddyl.ActionModelImpulseFwdDynamics(self.state, impulseModel, costModel)
+        return model
+
 
 def plotSolution(rmodel, xs, us, figIndex=1, show=True):
     import matplotlib.pyplot as plt
@@ -210,7 +306,7 @@ def plotSolution(rmodel, xs, us, figIndex=1, show=True):
     for i in range(nx):
         X[i] = [np.asscalar(x[i]) for x in xs]
     for i in range(nu):
-        U[i] = [np.asscalar(u[i]) for u in us]
+        U[i] = [np.asscalar(u[i]) if u.shape[0] != 0 else 0 for u in us]
 
     # Plotting the joint positions, velocities and torques
     plt.figure(figIndex)
diff --git a/bindings/python/crocoddyl/utils/quadruped.py b/bindings/python/crocoddyl/utils/quadruped.py
index eb9dd4841d015baeaf9e648285f27c8e634f82e4..f2a64ae9c25d5b616957718a5cb4295ffce3eabb 100644
--- a/bindings/python/crocoddyl/utils/quadruped.py
+++ b/bindings/python/crocoddyl/utils/quadruped.py
@@ -33,10 +33,6 @@ class SimpleQuadrupedalGaitProblem:
         pinocchio.forwardKinematics(self.rmodel, self.rdata, q0)
         pinocchio.updateFramePlacements(self.rmodel, self.rdata)
         com0 = pinocchio.centerOfMass(self.rmodel, self.rdata, q0)
-        # lfFootPos0 = self.rdata.oMf[self.lfFootId].translation
-        # rfFootPos0 = self.rdata.oMf[self.rfFootId].translation
-        # lhFootPos0 = self.rdata.oMf[self.lhFootId].translation
-        # rhFootPos0 = self.rdata.oMf[self.rhFootId].translation
 
         # Defining the action models along the time instances
         comModels = []
@@ -92,7 +88,7 @@ class SimpleQuadrupedalGaitProblem:
         lfFootPos0 = self.rdata.oMf[self.lfFootId].translation
         lhFootPos0 = self.rdata.oMf[self.lhFootId].translation
         comRef = (rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4
-        comRef[2] = 0.5325
+        comRef[2] = np.asscalar(pinocchio.centerOfMass(self.rmodel, self.rdata, q0)[2])
 
         # Defining the action models along the time instances
         loco3dModel = []
@@ -143,7 +139,7 @@ class SimpleQuadrupedalGaitProblem:
         lfFootPos0 = self.rdata.oMf[self.lfFootId].translation
         lhFootPos0 = self.rdata.oMf[self.lhFootId].translation
         comRef = (rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4
-        comRef[2] = 0.5325
+        comRef[2] = np.asscalar(pinocchio.centerOfMass(self.rmodel, self.rdata, q0)[2])
 
         # Defining the action models along the time instances
         loco3dModel = []
@@ -191,7 +187,7 @@ class SimpleQuadrupedalGaitProblem:
         lfFootPos0 = self.rdata.oMf[self.lfFootId].translation
         lhFootPos0 = self.rdata.oMf[self.lhFootId].translation
         comRef = (rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4
-        comRef[2] = 0.5325
+        comRef[2] = np.asscalar(pinocchio.centerOfMass(self.rmodel, self.rdata, q0)[2])
 
         # Defining the action models along the time instances
         loco3dModel = []
@@ -240,7 +236,7 @@ class SimpleQuadrupedalGaitProblem:
         lfFootPos0 = self.rdata.oMf[self.lfFootId].translation
         lhFootPos0 = self.rdata.oMf[self.lhFootId].translation
         comRef = (rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4
-        comRef[2] = 0.5325
+        comRef[2] = np.asscalar(pinocchio.centerOfMass(self.rmodel, self.rdata, q0)[2])
 
         # Defining the action models along the time instances
         loco3dModel = []
@@ -261,32 +257,59 @@ class SimpleQuadrupedalGaitProblem:
         problem = crocoddyl.ShootingProblem(x0, loco3dModel, loco3dModel[-1])
         return problem
 
-    def createJumpingProblem(self, x0, jumpHeight, timeStep):
+    def createJumpingProblem(self, x0, jumpHeight, jumpLength, timeStep, groundKnots, flyingKnots):
+        q0 = x0[:self.rmodel.nq]
+        pinocchio.forwardKinematics(self.rmodel, self.rdata, q0)
+        pinocchio.updateFramePlacements(self.rmodel, self.rdata)
         rfFootPos0 = self.rdata.oMf[self.rfFootId].translation
         rhFootPos0 = self.rdata.oMf[self.rhFootId].translation
         lfFootPos0 = self.rdata.oMf[self.lfFootId].translation
         lhFootPos0 = self.rdata.oMf[self.lhFootId].translation
+        df = jumpLength[2] - rfFootPos0[2]
+        rfFootPos0[2] = 0.
+        rhFootPos0[2] = 0.
+        lfFootPos0[2] = 0.
+        lhFootPos0[2] = 0.
         comRef = (rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4
-        comRef[2] = 0.5325
-
-        takeOffKnots = 30
-        flyingKnots = 30
+        comRef[2] = np.asscalar(pinocchio.centerOfMass(self.rmodel, self.rdata, q0)[2])
 
         loco3dModel = []
         takeOff = [
             self.createSwingFootModel(
                 timeStep,
                 [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
-            ) for k in range(takeOffKnots)
+            ) for k in range(groundKnots)
         ]
-        flyingPhase = [
-            self.createSwingFootModel(timeStep, [],
-                                      np.matrix([0., 0., jumpHeight * (k + 1) / flyingKnots]).T + comRef)
-            for k in range(flyingKnots)
+        flyingUpPhase = [
+            self.createSwingFootModel(
+                timeStep, [],
+                np.matrix([jumpLength[0], jumpLength[1], jumpLength[2] + jumpHeight]).T * (k + 1) / flyingKnots +
+                comRef) for k in range(flyingKnots)
+        ]
+        flyingDownPhase = []
+        for k in range(flyingKnots):
+            flyingDownPhase += [self.createSwingFootModel(timeStep, [])]
+
+        f0 = np.matrix(jumpLength).T
+        footTask = [
+            crocoddyl.FramePlacement(self.lfFootId, pinocchio.SE3(np.eye(3), lfFootPos0 + f0)),
+            crocoddyl.FramePlacement(self.rfFootId, pinocchio.SE3(np.eye(3), rfFootPos0 + f0)),
+            crocoddyl.FramePlacement(self.lhFootId, pinocchio.SE3(np.eye(3), lhFootPos0 + f0)),
+            crocoddyl.FramePlacement(self.rhFootId, pinocchio.SE3(np.eye(3), rhFootPos0 + f0))
+        ]
+        landingPhase = [
+            self.createFootSwitchModel([self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId], footTask, False)
+        ]
+        f0[2] = df
+        landed = [
+            self.createSwingFootModel(timeStep, [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
+                                      comTask=comRef + f0) for k in range(groundKnots)
         ]
-
         loco3dModel += takeOff
-        loco3dModel += flyingPhase
+        loco3dModel += flyingUpPhase
+        loco3dModel += flyingDownPhase
+        loco3dModel += landingPhase
+        loco3dModel += landed
 
         problem = crocoddyl.ShootingProblem(x0, loco3dModel, loco3dModel[-1])
         return problem
@@ -327,11 +350,11 @@ class SimpleQuadrupedalGaitProblem:
 
                 swingFootTask += [crocoddyl.FramePlacement(i, pinocchio.SE3(np.eye(3), tref))]
 
-            # Adding an action model for this knot
             comTask = np.matrix([stepLength * (k + 1) / numKnots, 0., 0.]).T * comPercentage + comPos0
             footSwingModel += [
                 self.createSwingFootModel(timeStep, supportFootIds, comTask=comTask, swingFootTask=swingFootTask)
             ]
+
         # Action model for the foot switch
         footSwitchModel = self.createFootSwitchModel(supportFootIds, swingFootTask)
 
@@ -355,7 +378,7 @@ class SimpleQuadrupedalGaitProblem:
         contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu)
         for i in supportFootIds:
             xref = crocoddyl.FrameTranslation(i, np.matrix([0., 0., 0.]).T)
-            supportContactModel = crocoddyl.ContactModel3D(self.state, xref, self.actuation.nu, np.matrix([0., 0.]).T)
+            supportContactModel = crocoddyl.ContactModel3D(self.state, xref, self.actuation.nu, np.matrix([0., 50.]).T)
             contactModel.addContact('contact_' + str(i), supportContactModel)
 
         # Creating the cost model for a contact phase
@@ -369,7 +392,7 @@ class SimpleQuadrupedalGaitProblem:
                 footTrack = crocoddyl.CostModelFrameTranslation(self.state, xref, self.actuation.nu)
                 costModel.addCost("footTrack_" + str(i), footTrack, 1e4)
 
-        stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10] * self.rmodel.nv)
+        stateWeights = np.array([0.] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10.] * self.rmodel.nv)
         stateReg = crocoddyl.CostModelState(self.state,
                                             crocoddyl.ActivationModelWeightedQuad(np.matrix(stateWeights**2).T),
                                             self.rmodel.defaultState, self.actuation.nu)
@@ -384,19 +407,33 @@ class SimpleQuadrupedalGaitProblem:
         model = crocoddyl.IntegratedActionModelEuler(dmodel, timeStep)
         return model
 
-    def createFootSwitchModel(self, supportFootIds, swingFootTask):
+    def createFootSwitchModel(self, supportFootIds, swingFootTask, pseudoImpulse=True):
         """ Action model for a foot switch phase.
 
         :param supportFootIds: Ids of the constrained feet
         :param swingFootTask: swinging foot task
+        :param pseudoImpulse: true for pseudo-impulse models, otherwise it uses the impulse model
         :return action model for a foot switch phase
         """
+        if pseudoImpulse:
+            return self.createPseudoImpulseModel(supportFootIds, swingFootTask)
+        else:
+            return self.createImpulseModel(supportFootIds, swingFootTask)
+
+    def createPseudoImpulseModel(self, supportFootIds, swingFootTask):
+        """ Action model for pseudo-impulse models.
+
+        A pseudo-impulse model consists of adding high-penalty cost for the contact velocities.
+        :param supportFootIds: Ids of the constrained feet
+        :param swingFootTask: swinging foot task
+        :return pseudo-impulse differential action model
+        """
         # Creating a 3D multi-contact model, and then including the supporting
         # foot
         contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu)
         for i in supportFootIds:
             xref = crocoddyl.FrameTranslation(i, np.matrix([0., 0., 0.]).T)
-            supportContactModel = crocoddyl.ContactModel3D(self.state, xref, self.actuation.nu, np.matrix([0., 0.]).T)
+            supportContactModel = crocoddyl.ContactModel3D(self.state, xref, self.actuation.nu, np.matrix([0., 50.]).T)
             contactModel.addContact('contact_' + str(i), supportContactModel)
 
         # Creating the cost model for a contact phase
@@ -404,9 +441,12 @@ class SimpleQuadrupedalGaitProblem:
         if swingFootTask is not None:
             for i in swingFootTask:
                 xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation)
+                vref = crocoddyl.FrameMotion(i.frame, pinocchio.Motion.Zero())
                 footTrack = crocoddyl.CostModelFrameTranslation(self.state, xref, self.actuation.nu)
+                impulseFootVelCost = crocoddyl.CostModelFrameVelocity(self.state, vref, self.actuation.nu)
                 costModel.addCost("footTrack_" + str(i), footTrack, 1e7)
-        stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10] * self.rmodel.nv)
+                costModel.addCost('impulseVel_' + str(i.frame), impulseFootVelCost, 1e6)
+        stateWeights = np.array([0.] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10.] * self.rmodel.nv)
         stateReg = crocoddyl.CostModelState(self.state,
                                             crocoddyl.ActivationModelWeightedQuad(np.matrix(stateWeights**2).T),
                                             self.rmodel.defaultState, self.actuation.nu)
@@ -414,11 +454,6 @@ class SimpleQuadrupedalGaitProblem:
         costModel.addCost("stateReg", stateReg, 1e1)
         costModel.addCost("ctrlReg", ctrlReg, 1e-3)
 
-        for i in swingFootTask:
-            vref = crocoddyl.FrameMotion(i.frame, pinocchio.Motion.Zero())
-            impactFootVelCost = crocoddyl.CostModelFrameVelocity(self.state, vref, self.actuation.nu)
-            costModel.addCost('impactVel_' + str(i.frame), impactFootVelCost, 1e6)
-
         # Creating the action model for the KKT dynamics with simpletic Euler
         # integration scheme
         dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics(self.state, self.actuation, contactModel,
@@ -426,8 +461,40 @@ class SimpleQuadrupedalGaitProblem:
         model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.)
         return model
 
+    def createImpulseModel(self, supportFootIds, swingFootTask):
+        """ Action model for impulse models.
+
+        An impulse model consists of describing the impulse dynamics against a set of contacts.
+        :param supportFootIds: Ids of the constrained feet
+        :param swingFootTask: swinging foot task
+        :return impulse action model
+        """
+        # Creating a 3D multi-contact model, and then including the supporting foot
+        impulseModel = crocoddyl.ImpulseModelMultiple(self.state)
+        for i in supportFootIds:
+            supportContactModel = crocoddyl.ImpulseModel3D(self.state, i)
+            impulseModel.addImpulse("impulse_" + str(i), supportContactModel)
+
+        # Creating the cost model for a contact phase
+        costModel = crocoddyl.CostModelSum(self.state, 0)
+        if swingFootTask is not None:
+            for i in swingFootTask:
+                xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation)
+                footTrack = crocoddyl.CostModelFrameTranslation(self.state, xref, 0)
+                costModel.addCost("footTrack_" + str(i), footTrack, 1e7)
+        stateWeights = np.array([1.] * 6 + [10.] * (self.rmodel.nv - 6) + [10.] * self.rmodel.nv)
+        stateReg = crocoddyl.CostModelState(self.state,
+                                            crocoddyl.ActivationModelWeightedQuad(np.matrix(stateWeights**2).T),
+                                            self.rmodel.defaultState, 0)
+        costModel.addCost("stateReg", stateReg, 1e1)
+
+        # Creating the action model for the KKT dynamics with simpletic Euler
+        # integration scheme
+        model = crocoddyl.ActionModelImpulseFwdDynamics(self.state, impulseModel, costModel)
+        return model
+
 
-def plotSolution(rmodel, xs, us):
+def plotSolution(rmodel, xs, us, figIndex=1, show=True):
     import matplotlib.pyplot as plt
     # Getting the state and control trajectories
     nx, nq, nu = xs[0].shape[0], rmodel.nq, us[0].shape[0]
@@ -436,10 +503,10 @@ def plotSolution(rmodel, xs, us):
     for i in range(nx):
         X[i] = [np.asscalar(x[i]) for x in xs]
     for i in range(nu):
-        U[i] = [np.asscalar(u[i]) for u in us]
+        U[i] = [np.asscalar(u[i]) if u.shape[0] != 0 else 0 for u in us]
 
     # Plotting the joint positions, velocities and torques
-    plt.figure(1)
+    plt.figure(figIndex)
     legJointNames = ['HAA', 'HFE', 'KFE']
     # LF foot
     plt.subplot(4, 3, 1)
@@ -502,9 +569,8 @@ def plotSolution(rmodel, xs, us):
     plt.ylabel('RH')
     plt.legend()
     plt.xlabel('knots')
-    plt.show()
 
-    plt.figure(2)
+    plt.figure(figIndex + 1)
     rdata = rmodel.createData()
     Cx = []
     Cy = []
@@ -518,4 +584,5 @@ def plotSolution(rmodel, xs, us):
     plt.xlabel('x [m]')
     plt.ylabel('y [m]')
     plt.grid(True)
-    plt.show()
+    if show:
+        plt.show()
diff --git a/cmake b/cmake
index 0fb087eb8e78db616329de0e4ab55abbe015bbe8..bbdade93ad2a4b7818cb5d5b9f0bf131627e0b71 160000
--- a/cmake
+++ b/cmake
@@ -1 +1 @@
-Subproject commit 0fb087eb8e78db616329de0e4ab55abbe015bbe8
+Subproject commit bbdade93ad2a4b7818cb5d5b9f0bf131627e0b71
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..88fa4e3e071a1d9cb846bca09d342eacee763de3
--- /dev/null
+++ b/examples/CMakeLists.txt
@@ -0,0 +1,12 @@
+SET(${PROJECT_NAME}_EXAMPLES_PYTHON
+  arm_manipulation
+  bipedal_walk
+  quadrupedal_gaits
+  )
+
+FOREACH(examples ${${PROJECT_NAME}_EXAMPLES_PYTHON})
+  PYTHON_BUILD(. "${examples}.py")
+  ADD_CUSTOM_TARGET("examples-${examples}"
+    ${CMAKE_COMMAND} -E env PYTHONPATH=$ENV{PYTHONPATH}:${PROJECT_BINARY_DIR}/bindings/python
+    ${PYTHON_EXECUTABLE} -c "import ${examples}")
+ENDFOREACH(examples ${${PROJECT_NAME}_EXAMPLES_PYTHON})
diff --git a/examples/talos_arm.py b/examples/arm_manipulation.py
similarity index 77%
rename from examples/talos_arm.py
rename to examples/arm_manipulation.py
index fd23d5f91b0f890ed7677d1a60179ee31b0b2943..c804e41cd4efed2b02956e3755f0c9cc9782b3f1 100644
--- a/examples/talos_arm.py
+++ b/examples/arm_manipulation.py
@@ -1,3 +1,4 @@
+import os
 import sys
 
 import crocoddyl
@@ -5,8 +6,8 @@ import pinocchio
 import numpy as np
 import example_robot_data
 
-WITHDISPLAY = 'disp' in sys.argv
-WITHPLOT = 'plot' in sys.argv
+WITHDISPLAY = 'display' in sys.argv or 'CROCODDYL_DISPLAY' in os.environ
+WITHPLOT = 'plot' in sys.argv or 'CROCODDYL_PLOT' in os.environ
 
 # In this example test, we will solve the reaching-goal task with the Talos arm.
 # For that, we use the forward dynamics (with its analytical derivatives)
@@ -60,8 +61,19 @@ problem = crocoddyl.ShootingProblem(x0, [runningModel] * T, terminalModel)
 # Creating the DDP solver for this OC problem, defining a logger
 ddp = crocoddyl.SolverDDP(problem)
 cameraTF = [2., 2.68, 0.54, 0.2, 0.62, 0.72, 0.22]
-if WITHDISPLAY:
-    ddp.setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackSolverDisplay(talos_arm, 4, 4, cameraTF)])
+if WITHDISPLAY and WITHPLOT:
+    ddp.setCallbacks([
+        crocoddyl.CallbackLogger(),
+        crocoddyl.CallbackVerbose(),
+        crocoddyl.CallbackDisplay(talos_arm, 4, 4, cameraTF)
+    ])
+elif WITHDISPLAY:
+    ddp.setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackVerbose()])
+elif WITHPLOT:
+    ddp.setCallbacks([
+        crocoddyl.CallbackLogger(),
+        crocoddyl.CallbackVerbose(),
+    ])
 else:
     ddp.setCallbacks([crocoddyl.CallbackVerbose()])
 
@@ -69,11 +81,16 @@ else:
 ddp.solve()
 
 # Plotting the solution and the DDP convergence
-# if WITHPLOT:
-#     log = ddp.callback[1]
-#     plotOCSolution(log.xs, log.us, figIndex=1, show=False)
-#     plotDDPConvergence(log.costs, log.control_regs, log.state_regs, log.gm_stops, log.th_stops, log.steps,
-#                        figIndex=2)
+if WITHPLOT:
+    log = ddp.getCallbacks()[0]
+    crocoddyl.plotOCSolution(log.xs, log.us, figIndex=1, show=False)
+    crocoddyl.plotConvergence(log.costs,
+                              log.control_regs,
+                              log.state_regs,
+                              log.gm_stops,
+                              log.th_stops,
+                              log.steps,
+                              figIndex=2)
 
 # Visualizing the solution in gepetto-viewer
 if WITHDISPLAY:
diff --git a/examples/bipedal_walking_from_foot_traj.py b/examples/bipedal_walk.py
similarity index 66%
rename from examples/bipedal_walking_from_foot_traj.py
rename to examples/bipedal_walk.py
index 5dc018defba3ed3e7293b8caa172752d39921c54..70f40a7fa694886f0d4a5c3bcef26ee4101d0075 100644
--- a/examples/bipedal_walking_from_foot_traj.py
+++ b/examples/bipedal_walk.py
@@ -1,3 +1,4 @@
+import os
 import sys
 
 import numpy as np
@@ -7,8 +8,8 @@ import example_robot_data
 import pinocchio
 from crocoddyl.utils.biped import SimpleBipedGaitProblem, plotSolution
 
-WITHDISPLAY = 'display' in sys.argv
-WITHPLOT = 'plot' in sys.argv
+WITHDISPLAY = 'display' in sys.argv or 'CROCODDYL_DISPLAY' in os.environ
+WITHPLOT = 'plot' in sys.argv or 'CROCODDYL_PLOT' in os.environ
 
 # Creating the lower-body part of Talos
 talos_legs = example_robot_data.loadTalosLegs()
@@ -46,8 +47,19 @@ for i, phase in enumerate(GAITPHASES):
 
     # Added the callback functions
     print('*** SOLVE ' + key + ' ***')
-    if WITHDISPLAY:
-        ddp[i].setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackSolverDisplay(talos_legs, 4, 4, cameraTF)])
+    if WITHDISPLAY and WITHPLOT:
+        ddp[i].setCallbacks([
+            crocoddyl.CallbackLogger(),
+            crocoddyl.CallbackVerbose(),
+            crocoddyl.CallbackDisplay(talos_legs, 4, 4, cameraTF)
+        ])
+    elif WITHDISPLAY:
+        ddp[i].setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackVerbose()])
+    elif WITHPLOT:
+        ddp[i].setCallbacks([
+            crocoddyl.CallbackLogger(),
+            crocoddyl.CallbackVerbose(),
+        ])
     else:
         ddp[i].setCallbacks([crocoddyl.CallbackVerbose()])
 
@@ -70,6 +82,20 @@ if WITHPLOT:
     xs = []
     us = []
     for i, phase in enumerate(GAITPHASES):
-        xs.extend(ddp[i].xs)
+        xs.extend(ddp[i].xs[:-1])
         us.extend(ddp[i].us)
-    plotSolution(talos_legs.model, xs, us)
+    log = ddp[0].getCallbacks()[0]
+    plotSolution(talos_legs.model, xs, us, figIndex=1, show=False)
+
+    for i, phase in enumerate(GAITPHASES):
+        title = phase.keys()[0] + " (phase " + str(i) + ")"
+        log = ddp[i].getCallbacks()[0]
+        crocoddyl.plotConvergence(log.costs,
+                                  log.control_regs,
+                                  log.state_regs,
+                                  log.gm_stops,
+                                  log.th_stops,
+                                  log.steps,
+                                  figTitle=title,
+                                  figIndex=i + 3,
+                                  show=True if i == len(GAITPHASES) - 1 else False)
diff --git a/examples/log/talos_arm.log b/examples/log/arm_manipulation.log
similarity index 78%
rename from examples/log/talos_arm.log
rename to examples/log/arm_manipulation.log
index ae7d84894dc23827b688b8edecf4f931394a5146..6bec912e443bb0741984f8e6e275edc82b8e3395 100644
--- a/examples/log/talos_arm.log
+++ b/examples/log/arm_manipulation.log
@@ -6,10 +6,3 @@ iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
    4  1.97477e-01  1.20824e-08  1.03534e-01  1.00000e-09  1.00000e-09   1.0000     1
    5  1.92250e-01  1.00371e-09  9.15511e-03  1.00000e-09  1.00000e-09   1.0000     1
    6  1.89677e-01  2.26074e-10  2.14801e-03  1.00000e-09  1.00000e-09   1.0000     1
-
-The reached pose by the wrist is
-  R =
-   0.999966    0.005158  0.00649231
--0.00513193    0.999979 -0.00402692
--0.00651294  0.00399347    0.999971
-  p =  0.0102162 0.00402174   0.304815
diff --git a/examples/log/bipedal_walk.log b/examples/log/bipedal_walk.log
new file mode 100644
index 0000000000000000000000000000000000000000..5641d0a611387c3ada881ad2f8a1409be5695a08
--- /dev/null
+++ b/examples/log/bipedal_walk.log
@@ -0,0 +1,56 @@
+*** SOLVE walking ***
+iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
+   0  3.79882e+06  5.92143e+11  8.10615e+07  1.00000e-02  1.00000e-02   1.0000     1
+   1  3.32210e+05  2.59218e+14  7.59517e+06  1.00000e-03  1.00000e-03   1.0000     1
+   2  3.52779e+04  4.05441e+12  6.62876e+05  1.00000e-04  1.00000e-04   1.0000     1
+   3  1.10639e+03  3.51410e+12  6.90638e+04  1.00000e-05  1.00000e-05   1.0000     1
+   4  7.45973e+02  3.22925e+10  7.20883e+02  1.00000e-06  1.00000e-06   1.0000     1
+   5  7.45953e+02  1.98486e+06  4.06963e-02  1.00000e-07  1.00000e-07   1.0000     1
+   6  7.45953e+02  8.50158e-01  6.98867e-06  1.00000e-08  1.00000e-08   1.0000     1
+   7  7.45953e+02  7.41961e-05  1.04647e-07  1.00000e-09  1.00000e-09   1.0000     1
+   8  7.45953e+02  2.13529e-07  1.64307e-09  1.00000e-09  1.00000e-09   1.0000     1
+   9  7.45953e+02  2.13138e-09  3.44634e-11  1.00000e-09  1.00000e-09   1.0000     1
+iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
+  10  7.45953e+02  6.81549e-11  8.17711e-12  1.00000e-09  1.00000e-09   1.0000     1
+*** SOLVE walking ***
+iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
+   0  8.72974e+06  2.47561e+12  3.69452e+08  1.00000e-02  1.00000e-02   1.0000     1
+   1  4.51365e+05  9.34606e+14  1.74565e+07  1.00000e-03  1.00000e-03   1.0000     1
+   2  4.70803e+04  3.50339e+12  9.00802e+05  1.00000e-04  1.00000e-04   1.0000     1
+   3  4.66196e+03  4.96423e+12  9.23052e+04  1.00000e-05  1.00000e-05   1.0000     1
+   4  9.27458e+02  3.46862e+11  7.46924e+03  1.00000e-06  1.00000e-06   1.0000     1
+   5  9.27341e+02  1.26462e+07  2.34127e-01  1.00000e-07  1.00000e-07   1.0000     1
+   6  9.27341e+02  4.71575e+00  1.29549e-05  1.00000e-08  1.00000e-08   1.0000     1
+   7  9.27341e+02  1.92036e-04  1.93143e-07  1.00000e-09  1.00000e-09   1.0000     1
+   8  9.27341e+02  5.65419e-07  3.15528e-09  1.00000e-09  1.00000e-09   1.0000     1
+   9  9.27341e+02  4.11015e-09  9.22155e-11  1.00000e-09  1.00000e-09   1.0000     1
+iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
+  10  9.27341e+02  1.04428e-10  1.37345e-11  1.00000e-09  1.00000e-09   0.5000     1
+*** SOLVE walking ***
+iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
+   0  1.23185e+07  5.69122e+12  8.74288e+08  1.00000e-02  1.00000e-02   1.0000     1
+   1  6.32257e+05  1.39715e+15  2.46338e+07  1.00000e-03  1.00000e-03   1.0000     1
+   2  3.23009e+04  1.58785e+13  1.26257e+06  1.00000e-04  1.00000e-04   1.0000     1
+   3  1.16817e+04  3.04744e+12  6.27451e+04  1.00000e-05  1.00000e-05   1.0000     1
+   4  9.28160e+02  1.01141e+12  2.15075e+04  1.00000e-06  1.00000e-06   1.0000     1
+   5  9.27954e+02  1.53852e+07  4.12158e-01  1.00000e-07  1.00000e-07   1.0000     1
+   6  9.27954e+02  1.49702e+00  1.12149e-05  1.00000e-08  1.00000e-08   1.0000     1
+   7  9.27954e+02  1.35106e-04  1.65220e-07  1.00000e-09  1.00000e-09   1.0000     1
+   8  9.27954e+02  4.98415e-07  2.79919e-09  1.00000e-09  1.00000e-09   1.0000     1
+   9  9.27954e+02  3.66849e-09  1.66261e-10  1.00000e-09  1.00000e-09   1.0000     1
+iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
+  10  9.27954e+02  2.51946e-10  5.23493e-11  1.00000e-09  1.00000e-09   0.0625     1
+*** SOLVE walking ***
+iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
+   0  1.34228e+07  1.02317e+13  1.59557e+09  1.00000e-02  1.00000e-02   1.0000     1
+   1  4.68564e+05  1.48838e+15  2.68423e+07  1.00000e-03  1.00000e-03   1.0000     1
+   2  3.56579e+04  1.48909e+13  9.35190e+05  1.00000e-04  1.00000e-04   1.0000     1
+   3  9.72632e+03  3.29361e+12  6.94578e+04  1.00000e-05  1.00000e-05   1.0000     1
+   4  9.28986e+02  7.97376e+11  1.75953e+04  1.00000e-06  1.00000e-06   1.0000     1
+   5  9.28678e+02  2.96602e+07  6.15953e-01  1.00000e-07  1.00000e-07   1.0000     1
+   6  9.28678e+02  1.72808e+01  1.55403e-05  1.00000e-08  1.00000e-08   1.0000     1
+   7  9.28678e+02  3.98616e-04  2.25356e-07  1.00000e-09  1.00000e-09   1.0000     1
+   8  9.28678e+02  7.57571e-07  3.56856e-09  1.00000e-09  1.00000e-09   1.0000     1
+   9  9.28678e+02  4.64317e-09  6.66206e-11  1.00000e-09  1.00000e-09   0.5000     1
+iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
+  10  9.28678e+02  9.39527e-10  2.23473e-11  1.00000e-09  1.00000e-09   1.0000     1
\ No newline at end of file
diff --git a/examples/log/bipedal_walking_from_foot_traj.log b/examples/log/bipedal_walking_from_foot_traj.log
deleted file mode 100644
index 8252ec5e3642427132735b5a8f5b6808472d5792..0000000000000000000000000000000000000000
--- a/examples/log/bipedal_walking_from_foot_traj.log
+++ /dev/null
@@ -1,43 +0,0 @@
-*** SOLVE walking ***
-iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
-   0  1.15825e+06  3.08454e+09  8.10612e+07  1.00000e-02  1.00000e-02   1.0000     1
-   1  1.20312e+05  1.18690e+08  2.31390e+06  1.00000e-03  1.00000e-03   1.0000     1
-   2  1.73531e+03  3.46775e+07  2.39055e+05  1.00000e-04  1.00000e-04   1.0000     1
-   3  7.65753e+02  3.94554e+06  1.94737e+03  1.00000e-05  1.00000e-05   1.0000     1
-   4  7.61391e+02  1.31205e+04  8.72483e+00  1.00000e-06  1.00000e-06   1.0000     1
-   5  7.61391e+02  2.51348e-02  6.88732e-05  1.00000e-07  1.00000e-07   1.0000     1
-   6  7.61391e+02  3.59102e-07  5.86261e-07  1.00000e-08  1.00000e-08   1.0000     1
-   7  7.61391e+02  8.32838e-10  8.94723e-09  1.00000e-09  1.00000e-09   1.0000     1
-*** SOLVE walking ***
-iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
-   0  1.23256e+06  1.32614e+10  3.69455e+08  1.00000e-02  1.00000e-02   1.0000     1
-   1  1.25284e+05  1.25947e+08  2.46199e+06  1.00000e-03  1.00000e-03   1.0000     1
-   2  1.96372e+03  3.80122e+07  2.48604e+05  1.00000e-04  1.00000e-04   1.0000     1
-   3  9.57185e+02  3.93920e+06  2.02273e+03  1.00000e-05  1.00000e-05   1.0000     1
-   4  9.51989e+02  1.42955e+04  1.03902e+01  1.00000e-06  1.00000e-06   1.0000     1
-   5  9.51989e+02  1.31179e-01  1.73358e-04  1.00000e-07  1.00000e-07   1.0000     1
-   6  9.51989e+02  1.04272e-06  1.49411e-06  1.00000e-08  1.00000e-08   1.0000     1
-   7  9.51989e+02  3.09588e-09  2.47886e-08  1.00000e-09  1.00000e-09   1.0000     1
-   8  9.51989e+02  8.67607e-11  4.54661e-10  1.00000e-09  1.00000e-09   1.0000     1
-*** SOLVE walking ***
-iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
-   0  1.10575e+06  3.07089e+10  8.74294e+08  1.00000e-02  1.00000e-02   1.0000     1
-   1  9.82289e+04  1.04452e+08  2.20834e+06  1.00000e-03  1.00000e-03   1.0000     1
-   2  2.29493e+03  3.32464e+07  1.94494e+05  1.00000e-04  1.00000e-04   1.0000     1
-   3  9.59187e+02  4.83906e+06  2.68405e+03  1.00000e-05  1.00000e-05   1.0000     1
-   4  9.52600e+02  1.63914e+04  1.31751e+01  1.00000e-06  1.00000e-06   1.0000     1
-   5  9.52600e+02  3.84877e-01  4.84418e-04  1.00000e-07  1.00000e-07   1.0000     1
-   6  9.52600e+02  3.93861e-06  3.80077e-06  1.00000e-08  1.00000e-08   1.0000     1
-   7  9.52600e+02  1.09978e-08  6.38147e-08  1.00000e-09  1.00000e-09   1.0000     1
-   8  9.52600e+02  2.13246e-10  1.09478e-09  1.00000e-09  1.00000e-09   1.0000     1
-*** SOLVE walking ***
-iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
-   0  9.97256e+05  5.54162e+10  1.59558e+09  1.00000e-02  1.00000e-02   1.0000     1
-   1  7.26949e+04  8.87034e+07  1.99131e+06  1.00000e-03  1.00000e-03   1.0000     1
-   2  2.91463e+03  2.87425e+07  1.43426e+05  1.00000e-04  1.00000e-04   1.0000     1
-   3  9.63182e+02  6.07916e+06  3.92224e+03  1.00000e-05  1.00000e-05   1.0000     1
-   4  9.53306e+02  2.02322e+04  1.97555e+01  1.00000e-06  1.00000e-06   1.0000     1
-   5  9.53305e+02  1.11507e+00  1.41303e-03  1.00000e-07  1.00000e-07   1.0000     1
-   6  9.53305e+02  1.68794e-05  8.18140e-06  1.00000e-08  1.00000e-08   1.0000     1
-   7  9.53305e+02  2.79748e-08  1.36146e-07  1.00000e-09  1.00000e-09   1.0000     1
-   8  9.53305e+02  4.50847e-10  2.30846e-09  1.00000e-09  1.00000e-09   1.0000     1
diff --git a/examples/log/quadrupedal_gaits.log b/examples/log/quadrupedal_gaits.log
new file mode 100644
index 0000000000000000000000000000000000000000..9cf31cd48419bdcef6992e3d9f8f645b23457cca
--- /dev/null
+++ b/examples/log/quadrupedal_gaits.log
@@ -0,0 +1,80 @@
+*** SOLVE walking ***
+iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
+   0  4.71060e+04  5.00529e+07  2.19805e+06  1.00000e-02  1.00000e-02   1.0000     1
+   1  2.71985e+04  1.23011e+10  7.49126e+04  1.00000e-03  1.00000e-03   1.0000     1
+   2  8.52084e+03  9.98222e+09  3.76239e+04  1.00000e-04  1.00000e-04   1.0000     1
+   3  8.29487e+03  3.15308e+07  8.46511e+02  1.00000e-05  1.00000e-05   1.0000     1
+   4  8.06767e+03  1.21786e+08  4.55753e+02  1.00000e-06  1.00000e-06   1.0000     1
+   5  8.06650e+03  6.70379e+05  2.31832e+00  1.00000e-07  1.00000e-07   1.0000     1
+   6  8.06650e+03  5.30707e+00  3.96525e-03  1.00000e-08  1.00000e-08   1.0000     1
+   7  8.06650e+03  3.35865e-04  2.12454e-04  1.00000e-09  1.00000e-09   1.0000     1
+   8  8.06650e+03  4.67649e-06  1.19350e-05  1.00000e-09  1.00000e-09   1.0000     1
+   9  8.06650e+03  2.55334e-07  6.85312e-07  1.00000e-09  1.00000e-09   1.0000     1
+iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
+  10  8.06650e+03  1.53850e-08  3.98917e-08  1.00000e-09  1.00000e-09   1.0000     1
+  11  8.06650e+03  9.23475e-10  2.34280e-09  1.00000e-09  1.00000e-09   1.0000     1
+*** SOLVE trotting ***
+iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
+   0  5.06631e+04  3.31465e+08  7.11857e+06  1.00000e-02  1.00000e-02   1.0000     1
+   1  2.11975e+04  8.28258e+09  8.89572e+04  1.00000e-03  1.00000e-03   1.0000     1
+   2  5.47426e+03  5.44889e+09  3.21343e+04  1.00000e-04  1.00000e-04   1.0000     1
+   3  4.93778e+03  1.93635e+08  1.09837e+03  1.00000e-05  1.00000e-05   1.0000     1
+   4  4.91204e+03  1.26305e+06  5.11245e+01  1.00000e-06  1.00000e-06   1.0000     1
+   5  4.91198e+03  9.12812e+03  1.17757e-01  1.00000e-07  1.00000e-07   1.0000     1
+   6  4.91198e+03  9.36311e-01  1.65643e-03  1.00000e-08  1.00000e-08   1.0000     1
+   7  4.91198e+03  3.73906e-04  6.12648e-05  1.00000e-09  1.00000e-09   1.0000     1
+   8  4.91198e+03  1.40822e-06  2.31119e-06  1.00000e-09  1.00000e-09   1.0000     1
+   9  4.91198e+03  5.96580e-08  8.83018e-08  1.00000e-09  1.00000e-09   1.0000     1
+iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
+  10  4.91198e+03  2.43644e-09  3.42869e-09  1.00000e-09  1.00000e-09   1.0000     1
+  11  4.91198e+03  9.87581e-11  1.41995e-10  1.00000e-09  1.00000e-09   1.0000     1
+*** SOLVE pacing ***
+iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
+   0  5.00587e+06  7.29811e+08  1.62092e+07  1.00000e-02  1.00000e-02   1.0000     1
+   1  5.45480e+05  5.44671e+12  9.97788e+06  1.00000e-03  1.00000e-03   1.0000     1
+   2  2.39173e+05  4.21088e+11  1.07453e+06  1.00000e-04  1.00000e-04   1.0000     1
+   3  9.17900e+03  1.52753e+11  4.63455e+05  1.00000e-05  1.00000e-05   1.0000     1
+   4  7.35430e+03  9.75817e+08  3.65511e+03  1.00000e-06  1.00000e-06   1.0000     1
+   5  7.35003e+03  1.99372e+06  8.55166e+00  1.00000e-07  1.00000e-07   1.0000     1
+   6  7.35002e+03  1.97563e+02  8.29399e-03  1.00000e-08  1.00000e-08   1.0000     1
+   7  7.35002e+03  2.42706e-01  2.12813e-04  1.00000e-09  1.00000e-09   1.0000     1
+   8  7.35002e+03  3.39558e-04  6.86314e-06  1.00000e-09  1.00000e-09   1.0000     1
+   9  7.35002e+03  1.02090e-06  2.34919e-07  1.00000e-09  1.00000e-09   1.0000     1
+iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
+  10  7.35002e+03  4.68927e-08  8.29480e-09  1.00000e-09  1.00000e-09   1.0000     1
+  11  7.35002e+03  1.53040e-09  2.99125e-10  1.00000e-09  1.00000e-09   1.0000     1
+  12  7.35002e+03  5.39239e-11  1.09810e-11  1.00000e-09  1.00000e-09   1.0000     1
+*** SOLVE bounding ***
+iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
+   0  6.30222e+05  5.11011e+09  2.46895e+07  1.00000e-02  1.00000e-02   1.0000     1
+   1  3.56791e+04  3.10021e+11  1.22924e+06  1.00000e-03  1.00000e-03   1.0000     1
+   2  1.53406e+04  8.43694e+09  5.32585e+04  1.00000e-04  1.00000e-04   1.0000     1
+   3  8.18536e+03  4.19892e+09  1.43797e+04  1.00000e-05  1.00000e-05   1.0000     1
+   4  8.01180e+03  6.67017e+07  3.41062e+02  1.00000e-06  1.00000e-06   1.0000     1
+   5  8.01115e+03  1.11178e+05  1.27823e+00  1.00000e-07  1.00000e-07   1.0000     1
+   6  8.01114e+03  2.69712e+02  1.00216e-02  1.00000e-08  1.00000e-08   1.0000     1
+   7  8.01114e+03  9.99891e-02  8.11450e-04  1.00000e-09  1.00000e-09   1.0000     1
+   8  8.01114e+03  2.12973e-03  1.04521e-04  1.00000e-09  1.00000e-09   1.0000     1
+   9  8.01114e+03  2.23460e-04  1.37404e-05  1.00000e-09  1.00000e-09   1.0000     1
+iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
+  10  8.01114e+03  2.86775e-05  1.80826e-06  1.00000e-09  1.00000e-09   1.0000     1
+  11  8.01114e+03  3.76104e-06  2.37988e-07  1.00000e-09  1.00000e-09   1.0000     1
+  12  8.01114e+03  4.95058e-07  3.13220e-08  1.00000e-09  1.00000e-09   1.0000     1
+  13  8.01114e+03  6.51341e-08  4.12247e-09  1.00000e-09  1.00000e-09   1.0000     1
+  14  8.01114e+03  8.57345e-09  5.42632e-10  1.00000e-09  1.00000e-09   1.0000     1
+  15  8.01114e+03  1.12839e-09  7.14990e-11  1.00000e-09  1.00000e-09   1.0000     1
+  16  8.01114e+03  1.48545e-10  9.56772e-12  1.00000e-09  1.00000e-09   1.0000     1
+*** SOLVE jumping ***
+iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
+   0  4.84990e+05  1.96114e+08  2.00583e+07  1.00000e-02  1.00000e-02   1.0000     1
+   1  2.55518e+05  5.45932e+06  9.44614e+05  1.00000e-02  1.00000e-02   0.5000     1
+   2  3.21623e+04  6.08478e+06  4.86889e+05  1.00000e-03  1.00000e-03   1.0000     1
+   3  8.84222e+03  8.03098e+05  4.63662e+04  1.00000e-04  1.00000e-04   1.0000     1
+   4  8.21026e+03  2.37356e+04  1.22753e+03  1.00000e-05  1.00000e-05   1.0000     1
+   5  8.18969e+03  5.29795e+02  4.04915e+01  1.00000e-06  1.00000e-06   1.0000     1
+   6  8.18963e+03  1.75809e-01  9.89714e-02  1.00000e-07  1.00000e-07   1.0000     1
+   7  8.18963e+03  1.79513e-04  1.19506e-03  1.00000e-08  1.00000e-08   1.0000     1
+   8  8.18963e+03  1.44032e-06  2.62011e-05  1.00000e-09  1.00000e-09   1.0000     1
+   9  8.18963e+03  3.98758e-08  6.24980e-07  1.00000e-09  1.00000e-09   1.0000     1
+iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
+  10  8.18963e+03  9.15627e-10  1.56982e-08  1.00000e-09  1.00000e-09   1.0000     1
\ No newline at end of file
diff --git a/examples/log/quadrupedal_walking_balancing.log b/examples/log/quadrupedal_walking_balancing.log
deleted file mode 100644
index fdb151b755219b2d84c803e0d1f161366e407e8d..0000000000000000000000000000000000000000
--- a/examples/log/quadrupedal_walking_balancing.log
+++ /dev/null
@@ -1,84 +0,0 @@
-*** SOLVE walking ***
-iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
-   0  1.37485e+04  1.85164e+07  7.91406e+05  1.00000e-02  1.00000e-02   1.0000     1
-   1  6.98715e+03  2.91164e+09  1.61165e+04  1.00000e-03  1.00000e-03   1.0000     1
-   2  5.00494e+03  9.61535e+08  3.87659e+03  1.00000e-04  1.00000e-04   1.0000     1
-   3  4.87427e+03  1.62571e+06  2.83792e+02  1.00000e-05  1.00000e-05   1.0000     1
-   4  4.83018e+03  1.66146e+07  8.69937e+01  1.00000e-06  1.00000e-06   1.0000     1
-   5  4.83003e+03  4.89373e+04  3.02862e-01  1.00000e-07  1.00000e-07   1.0000     1
-   6  4.83003e+03  3.25805e-01  5.72241e-04  1.00000e-08  1.00000e-08   1.0000     1
-   7  4.83003e+03  6.23636e-05  2.00015e-05  1.00000e-09  1.00000e-09   1.0000     1
-   8  4.83003e+03  9.24953e-07  7.58347e-07  1.00000e-09  1.00000e-09   1.0000     1
-   9  4.83003e+03  3.04042e-08  2.94413e-08  1.00000e-09  1.00000e-09   1.0000     1
-iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
-  10  4.83003e+03  1.10821e-09  1.16218e-09  1.00000e-09  1.00000e-09   1.0000     1
-  11  4.83003e+03  4.21539e-11  4.65054e-11  1.00000e-09  1.00000e-09   1.0000     1
-*** SOLVE trotting ***
-iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
-   0  2.68468e+04  4.69817e+09  4.25814e+06  1.00000e-02  1.00000e-02   1.0000     1
-   1  1.14296e+04  1.52301e+09  4.16464e+04  1.00000e-03  1.00000e-03   1.0000     1
-   2  5.59099e+03  2.13557e+09  1.26334e+04  1.00000e-04  1.00000e-04   1.0000     1
-   3  4.93701e+03  2.51384e+08  1.32067e+03  1.00000e-05  1.00000e-05   1.0000     1
-   4  4.91198e+03  1.73224e+06  4.93214e+01  1.00000e-06  1.00000e-06   1.0000     1
-   5  4.91188e+03  1.22601e+04  1.90307e-01  1.00000e-07  1.00000e-07   1.0000     1
-   6  4.91188e+03  8.56271e-01  1.51564e-03  1.00000e-08  1.00000e-08   1.0000     1
-   7  4.91188e+03  4.61460e-04  6.35166e-05  1.00000e-09  1.00000e-09   1.0000     1
-   8  4.91188e+03  4.32643e-06  2.75983e-06  1.00000e-09  1.00000e-09   1.0000     1
-   9  4.91188e+03  1.81072e-07  1.22024e-07  1.00000e-09  1.00000e-09   1.0000     1
-iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
-  10  4.91188e+03  7.98564e-09  5.47036e-09  1.00000e-09  1.00000e-09   1.0000     1
-  11  4.91188e+03  3.45652e-10  2.66409e-10  1.00000e-09  1.00000e-09   1.0000     1
-*** SOLVE pacing ***
-iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
-   0  7.15118e+05  4.40102e+11  1.92790e+07  1.00000e-02  1.00000e-02   1.0000     1
-   1  4.31583e+05  4.96153e+11  1.40778e+06  1.00000e-03  1.00000e-03   1.0000     1
-   2  8.39776e+03  3.16015e+11  8.47409e+05  1.00000e-04  1.00000e-04   1.0000     1
-   3  7.29461e+03  3.68041e+08  2.49227e+03  1.00000e-05  1.00000e-05   1.0000     1
-   4  7.05636e+03  1.07475e+08  4.75721e+02  1.00000e-06  1.00000e-06   1.0000     1
-   5  7.05482e+03  6.63227e+05  3.05895e+00  1.00000e-07  1.00000e-07   1.0000     1
-   6  7.05482e+03  8.99408e+00  3.95375e-03  1.00000e-08  1.00000e-08   1.0000     1
-   7  7.05482e+03  1.05298e-03  8.52418e-05  1.00000e-09  1.00000e-09   1.0000     1
-   8  7.05482e+03  6.22669e-06  2.21452e-06  1.00000e-09  1.00000e-09   1.0000     1
-   9  7.05482e+03  3.78027e-07  6.75324e-08  1.00000e-09  1.00000e-09   1.0000     1
-iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
-  10  7.05482e+03  2.17786e-09  2.35971e-09  1.00000e-09  1.00000e-09   1.0000     1
-  11  7.05482e+03  4.50072e-10  9.12655e-11  1.00000e-09  1.00000e-09   1.0000     1
-*** SOLVE bounding ***
-iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
-   0  8.18722e+05  1.08687e+11  2.00020e+07  1.00000e-02  1.00000e-02   1.0000     1
-   1  7.79387e+04  6.04524e+11  1.60133e+06  1.00000e-03  1.00000e-03   1.0000     1
-   2  1.84906e+04  2.28056e+10  1.32379e+05  1.00000e-04  1.00000e-04   1.0000     1
-   3  1.08699e+04  2.65616e+09  1.52839e+04  1.00000e-05  1.00000e-05   1.0000     1
-   4  1.07031e+04  6.33626e+07  3.27233e+02  1.00000e-06  1.00000e-06   1.0000     1
-   5  1.07025e+04  8.21707e+04  1.43045e+00  1.00000e-07  1.00000e-07   1.0000     1
-   6  1.07025e+04  2.70740e+03  8.84330e-02  1.00000e-08  1.00000e-08   1.0000     1
-   7  1.07025e+04  2.55970e+00  1.16696e-02  1.00000e-09  1.00000e-09   1.0000     1
-   8  1.07025e+04  1.15859e-01  1.93454e-03  1.00000e-09  1.00000e-09   1.0000     1
-   9  1.07025e+04  6.44470e-03  3.24309e-04  1.00000e-09  1.00000e-09   1.0000     1
-iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
-  10  1.07025e+04  7.54970e-04  5.44404e-05  1.00000e-09  1.00000e-09   1.0000     1
-  11  1.07025e+04  1.30345e-04  9.14194e-06  1.00000e-09  1.00000e-09   1.0000     1
-  12  1.07025e+04  2.10326e-05  1.53515e-06  1.00000e-09  1.00000e-09   1.0000     1
-  13  1.07025e+04  3.57494e-06  2.57797e-07  1.00000e-09  1.00000e-09   1.0000     1
-  14  1.07025e+04  5.96885e-07  4.32913e-08  1.00000e-09  1.00000e-09   1.0000     1
-  15  1.07025e+04  1.00464e-07  7.26986e-09  1.00000e-09  1.00000e-09   1.0000     1
-  16  1.07025e+04  1.68545e-08  1.22086e-09  1.00000e-09  1.00000e-09   1.0000     1
-  17  1.07025e+04  2.83150e-09  2.05014e-10  1.00000e-09  1.00000e-09   1.0000     1
-  18  1.07025e+04  4.75414e-10  3.44277e-11  1.00000e-09  1.00000e-09   0.5000     1
-*** SOLVE jumping ***
-iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
-   0  1.18138e+05  1.07909e+04  8.62193e+04  1.00000e-02  1.00000e-02   1.0000     1
-   1  2.18963e+04  8.42151e+05  2.15921e+05  1.00000e-03  1.00000e-03   1.0000     1
-   2  1.65014e+04  1.09570e+04  3.18265e+04  1.00000e-03  1.00000e-03   0.5000     1
-   3  6.98607e+03  4.92609e+03  2.01180e+04  1.00000e-04  1.00000e-04   1.0000     1
-   4  5.22205e+03  8.08190e+02  3.43545e+03  1.00000e-05  1.00000e-05   1.0000     1
-   5  5.19468e+03  6.44125e+00  4.90813e+01  1.00000e-06  1.00000e-06   1.0000     1
-   6  5.19331e+03  1.22269e-02  2.09810e+00  1.00000e-07  1.00000e-07   1.0000     1
-   7  5.19316e+03  2.12864e-04  2.26843e-01  1.00000e-08  1.00000e-08   1.0000     1
-   8  5.19314e+03  2.02209e-05  2.62528e-02  1.00000e-09  1.00000e-09   1.0000     1
-   9  5.19314e+03  3.25291e-06  3.05574e-03  1.00000e-09  1.00000e-09   1.0000     1
-iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
-  10  5.19314e+03  3.51994e-07  3.56664e-04  1.00000e-09  1.00000e-09   1.0000     1
-  11  5.19314e+03  4.48866e-08  4.16925e-05  1.00000e-09  1.00000e-09   1.0000     1
-  12  5.19314e+03  5.00807e-09  4.87737e-06  1.00000e-09  1.00000e-09   1.0000     1
-  13  5.19314e+03  6.08521e-10  5.70796e-07  1.00000e-09  1.00000e-09   1.0000     1
diff --git a/examples/notebooks/manipulator.ipynb b/examples/notebooks/arm_manipulation.ipynb
similarity index 98%
rename from examples/notebooks/manipulator.ipynb
rename to examples/notebooks/arm_manipulation.ipynb
index 7c9d4d82aaac8fd544df1143501a02ded21edfab..cb03cb8a0869e7a95ca33453d7d320906d82ee05 100644
--- a/examples/notebooks/manipulator.ipynb
+++ b/examples/notebooks/arm_manipulation.ipynb
@@ -180,7 +180,7 @@
     "## IV. Callbacks\n",
     "\n",
     "Callback functions are needed for analysing and debugging the performance of the solver for your specific problem.\n",
-    "For problems defined with Pinocchio, you can display the robot trajectory per each iterate by including CallbackSolverDisplay. With this callback, you can display robot motions with different rates. Additionally, CallbackVerbose prints a message that allows us to understand the behaviour of the solver.\n",
+    "For problems defined with Pinocchio, you can display the robot trajectory per each iterate by including CallbackDisplay. With this callback, you can display robot motions with different rates. Additionally, CallbackVerbose prints a message that allows us to understand the behaviour of the solver.\n",
     "\n",
     "Generally speaking, an user is able to describe any callback function. This function will be run once per iterate and it has access to all data."
    ]
diff --git a/examples/notebooks/bipedal_walking.ipynb b/examples/notebooks/bipedal_walking.ipynb
new file mode 100644
index 0000000000000000000000000000000000000000..8c15bf9f9915f5f95efd3ca78c65162885d2fab1
--- /dev/null
+++ b/examples/notebooks/bipedal_walking.ipynb
@@ -0,0 +1,186 @@
+{
+ "cells": [
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "##  Multi-Contact dynamics for biped walking (Talos legs)\n",
+    "In this example, we describe the multi-contact dynamics through holonomic constraints for the support legs. From the Gauss principle, we have derived the model as:\n",
+    "$$\n",
+    "\\left[\\begin{matrix}\n",
+    " \\mathbf{M} & \\mathbf{J}^{\\top}_c \\\\\n",
+    " {\\mathbf{J}_{c}} & \\mathbf{0} \\\\\n",
+    "\\end{matrix}\\right]\n",
+    "\\left[\\begin{matrix}\n",
+    " \\dot{\\mathbf{v}} \\\\ -\\boldsymbol{\\lambda}\n",
+    "\\end{matrix}\\right]\n",
+    " = \n",
+    "\\left[\\begin{matrix}\n",
+    "  \\boldsymbol{\\tau} - \\mathbf{h} \\\\\n",
+    "  -\\dot{\\mathbf{J}}_c \\mathbf{v} \\\\\n",
+    "\\end{matrix}\\right]$$.\n",
+    "\n",
+    "\n",
+    "Base on a predefined walking gait, we build per each phase a specific multi-contact dynamics. Indeed we need to describe multi-phase optimal control problem. One can formulate the multi-contact optimal control problem (MCOP) as follows:\n",
+    "\n",
+    "\n",
+    "$$\\mathbf{X}^*,\\mathbf{U}^*=\n",
+    "\\begin{Bmatrix} \\mathbf{x}^*_0,\\cdots,\\mathbf{x}^*_N \\\\\n",
+    "\t\t\t\t  \\mathbf{u}^*_0,\\cdots,\\mathbf{u}^*_N\n",
+    "\\end{Bmatrix} =\n",
+    "\\arg\\min_{\\mathbf{X},\\mathbf{U}} \\sum_{p=0}^P \\sum_{k=1}^{N(p)} \\int_{t_k}^{t_k+\\Delta t} l_p(\\mathbf{x},\\mathbf{u})dt$$\n",
+    "subject to\n",
+    "$$ \\mathbf{\\dot{x}} = \\mathbf{f}_p(\\mathbf{x},\\mathbf{u}), \\text{for } t \\in [\\tau_p,\\tau_{p+1}]$$\n",
+    "\n",
+    "$$ \\mathbf{g}(\\mathbf{v}^{p+1},\\mathbf{v}^p) = \\mathbf{0}$$\n",
+    "\n",
+    "$$ \\mathbf{x}\\in\\mathcal{X}_p, \\mathbf{u}\\in\\mathcal{U}_p, \\boldsymbol{\\lambda}\\in\\mathcal{K}_p.$$\n",
+    "\n",
+    "where $\\mathbf{g}(\\cdot,\\cdot,\\cdot)$ describes the contact dynamics, and they represents terminal constraints in each walking phase. In this example we use the following impact model:\n",
+    "\n",
+    "$$\\mathbf{M}(\\mathbf{v}_{next}-\\mathbf{v}) = \\mathbf{J}_{impulse}^T$$\n",
+    "\n",
+    "$$\\mathbf{J}_{impulse} \\mathbf{v}_{next} = \\mathbf{0}$$\n",
+    "\n",
+    "$$\\mathbf{J}_{c} \\mathbf{v}_{next} = \\mathbf{J}_{c} \\mathbf{v}$$"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "SimpleBipedGaitProblem class builds action models for each locomotion phase:\n",
+    "  - createSwingFootModel: defines an action model for the swing phase\n",
+    "  - createFootSwitchModel: defines an action model for switch knots between phases\n",
+    "  \n",
+    "Then we build a walking by combining a set of contact phases and their contact switches. This is defined by createFootstepModel\n",
+    "\n",
+    "Now let's create a walking OC problem for the Talos legs."
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "from crocoddyl.utils.biped import SimpleBipedGaitProblem\n",
+    "\n",
+    "# Creating the lower-body part of Talos\n",
+    "import example_robot_data\n",
+    "talos_legs = example_robot_data.loadTalosLegs()\n",
+    "\n",
+    "# Setting up the 3d walking problem\n",
+    "rightFoot = 'right_sole_link'\n",
+    "leftFoot = 'left_sole_link'\n",
+    "gait = SimpleBipedGaitProblem(talos_legs.model, rightFoot, leftFoot)\n",
+    "\n",
+    "\n",
+    "# Create the initial state\n",
+    "q0 = talos_legs.q0.copy()\n",
+    "v0 = zero(talos_legs.model.nv)\n",
+    "x0 = np.concatenate([q0,v0])\n",
+    "\n",
+    "\n",
+    "# Creating the walking problem\n",
+    "stepLength = 0.6 # meters\n",
+    "stepHeight = 0.1 # meters\n",
+    "timeStep = 0.0375 # seconds\n",
+    "stepKnots = 20\n",
+    "supportKnots = 10\n",
+    "problem = gait.createWalkingProblem(x0, stepLength, stepHeight, timeStep, stepKnots, supportKnots)\n",
+    "\n",
+    "\n",
+    "# Solving the 3d walking problem using Feasibility-prone DDP\n",
+    "ddp = crocoddyl.SolverFDDP(problem)\n",
+    "cameraTF = [3., 3.68, 0.84, 0.2, 0.62, 0.72, 0.22]\n",
+    "ddp.setCallbacks([crocoddyl.CallbackLogger(),\n",
+    "                  crocoddyl.CallbackVerbose(),\n",
+    "                  crocoddyl.CallbackDisplay(talos_legs, 4, 4,cameraTF)])\n",
+    "ddp.th_stop = 1e-9\n",
+    "init_xs = [talos_legs.model.defaultState]*problem.T\n",
+    "init_us = []\n",
+    "maxiter = 1000\n",
+    "regInit = 0.1\n",
+    "ddp.solve(init_xs, init_us, maxiter, False, regInit)"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "With the following commands we can plot \n",
+    " - the state and control trajectories, and\n",
+    " - the DDP performance"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "%matplotlib inline\n",
+    "\n",
+    "# Plotting the solution and the DDP convergence\n",
+    "log = ddp.getCallbacks()[0]\n",
+    "crocoddyl.plotOCSolution(log.xs, log.us)\n",
+    "crocoddyl.plotConvergence(log.costs,log.control_regs,\n",
+    "                   log.state_regs,log.gm_stops,\n",
+    "                   log.th_stops,log.steps)\n"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "Finally we can visualize the solution using gepetto-viewer."
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Visualization of the DDP solution in gepetto-viewer\n",
+    "ddp.getCallbacks()[2](ddp)\n",
+    "CallbackDisplay(talos_legs)(ddp)"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "## VI. Understanding the walking problem\n",
+    "\n",
+    "In this problem we pre-defined a 20 and 10 knots for the step and double support phases with $dt=$3.75e-2, repectively. \n",
+    "\n",
+    " 1. Could you tell us how much is the foot step and double support duration?\n",
+    " 2. What happens when do we change $dt$ (e.g. 2e-2 secs)?\n",
+    " 3. What happens when do we change the number of step knots (e.g. 10)?\n"
+   ]
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "Python 2",
+   "language": "python",
+   "name": "python2"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 2
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython2",
+   "version": "2.7.12"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}
diff --git a/examples/notebooks/bipedal_walking_from_foot_traj.ipynb b/examples/notebooks/bipedal_walking_from_foot_traj.ipynb
deleted file mode 100644
index e1b64484f15dd704d2148f8b4f9746b6505678a1..0000000000000000000000000000000000000000
--- a/examples/notebooks/bipedal_walking_from_foot_traj.ipynb
+++ /dev/null
@@ -1,371 +0,0 @@
-{
- "cells": [
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "##  Multi-Contact dynamics for biped walking (Talos legs)\n",
-    "In this example, we describe the multi-contact dynamics through holonomic constraints for the support legs. From the Gauss principle, we have derived the model as:\n",
-    "$$\n",
-    "\\left[\\begin{matrix}\n",
-    " \\mathbf{M} & \\mathbf{J}^{\\top}_c \\\\\n",
-    " {\\mathbf{J}_{c}} & \\mathbf{0} \\\\\n",
-    "\\end{matrix}\\right]\n",
-    "\\left[\\begin{matrix}\n",
-    " \\dot{\\mathbf{v}} \\\\ -\\boldsymbol{\\lambda}\n",
-    "\\end{matrix}\\right]\n",
-    " = \n",
-    "\\left[\\begin{matrix}\n",
-    "  \\boldsymbol{\\tau} - \\mathbf{h} \\\\\n",
-    "  -\\dot{\\mathbf{J}}_c \\mathbf{v} \\\\\n",
-    "\\end{matrix}\\right]$$.\n",
-    "\n",
-    "\n",
-    "Base on a predefined walking gait, we build per each phase a specific multi-contact dynamics. Indeed we need to describe multi-phase optimal control problem. One can formulate the multi-contact optimal control problem (MCOP) as follows:\n",
-    "\n",
-    "\n",
-    "$$\\mathbf{X}^*,\\mathbf{U}^*=\n",
-    "\\begin{Bmatrix} \\mathbf{x}^*_0,\\cdots,\\mathbf{x}^*_N \\\\\n",
-    "\t\t\t\t  \\mathbf{u}^*_0,\\cdots,\\mathbf{u}^*_N\n",
-    "\\end{Bmatrix} =\n",
-    "\\arg\\min_{\\mathbf{X},\\mathbf{U}} \\sum_{p=0}^P \\sum_{k=1}^{N(p)} \\int_{t_k}^{t_k+\\Delta t} l_p(\\mathbf{x},\\mathbf{u})dt$$\n",
-    "subject to\n",
-    "$$ \\mathbf{\\dot{x}} = \\mathbf{f}_p(\\mathbf{x},\\mathbf{u}), \\text{for } t \\in [\\tau_p,\\tau_{p+1}]$$\n",
-    "\n",
-    "$$ \\mathbf{g}(\\mathbf{v}^{p+1},\\mathbf{v}^p) = \\mathbf{0}$$\n",
-    "\n",
-    "$$ \\mathbf{x}\\in\\mathcal{X}_p, \\mathbf{u}\\in\\mathcal{U}_p, \\boldsymbol{\\lambda}\\in\\mathcal{K}_p.$$\n",
-    "\n",
-    "where $\\mathbf{g}(\\cdot,\\cdot,\\cdot)$ describes the contact dynamics, and they represents terminal constraints in each walking phase. In this example we use the following impact model:\n",
-    "\n",
-    "$$\\mathbf{M}(\\mathbf{v}_{next}-\\mathbf{v}) = \\mathbf{J}_{impulse}^T$$\n",
-    "\n",
-    "$$\\mathbf{J}_{impulse} \\mathbf{v}_{next} = \\mathbf{0}$$\n",
-    "\n",
-    "$$\\mathbf{J}_{c} \\mathbf{v}_{next} = \\mathbf{J}_{c} \\mathbf{v}$$\n",
-    "\n",
-    "\n",
-    "\n",
-    "First, let's define walking shooting problem:"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "from crocoddyl import *\n",
-    "import pinocchio\n",
-    "from pinocchio.utils import *\n",
-    "\n",
-    "\n",
-    "class TaskSE3:\n",
-    "    def __init__(self, oXf, frameId):\n",
-    "        self.oXf = oXf\n",
-    "        self.frameId = frameId\n",
-    "\n",
-    "class SimpleBipedWalkingProblem:\n",
-    "    \"\"\" Defines a simple 3d locomotion problem\n",
-    "    \"\"\"\n",
-    "    def __init__(self, rmodel, rightFoot, leftFoot):\n",
-    "        self.rmodel = rmodel\n",
-    "        self.rdata = rmodel.createData()\n",
-    "        self.state = StatePinocchio(self.rmodel)\n",
-    "        self.rightFoot = rightFoot\n",
-    "        self.leftFoot = leftFoot\n",
-    "        # Defining default state\n",
-    "        self.rmodel.defaultState = \\\n",
-    "            np.concatenate([m2a(self.rmodel.neutralConfiguration),\n",
-    "                            np.zeros(self.rmodel.nv)])\n",
-    "        # Remove the armature\n",
-    "        self.rmodel.armature[6:] = 1.\n",
-    "    \n",
-    "    def createProblem(self, x0, stepLength, timeStep, stepKnots, supportKnots):\n",
-    "        \"\"\" Create a shooting problem for a simple walking.\n",
-    "\n",
-    "        :param x0: initial state\n",
-    "        :param stepLength: step lenght\n",
-    "        :param timeStep: step time for each knot\n",
-    "        :param stepKnots: number of knots for step phases\n",
-    "        :param supportKnots: number of knots for double support phases\n",
-    "        :return shooting problem\n",
-    "        \"\"\"\n",
-    "        # Getting the frame id for the right and left foot\n",
-    "        rightFootId = self.rmodel.getFrameId(self.rightFoot)\n",
-    "        leftFootId = self.rmodel.getFrameId(self.leftFoot)\n",
-    "\n",
-    "        # Compute the current foot positions\n",
-    "        q0 = a2m(x0[:self.rmodel.nq])\n",
-    "        pinocchio.forwardKinematics(self.rmodel,self.rdata,q0)\n",
-    "        pinocchio.updateFramePlacements(self.rmodel,self.rdata)\n",
-    "        rightFootPos0 = self.rdata.oMf[rightFootId].translation\n",
-    "        leftFootPos0 = self.rdata.oMf[leftFootId].translation\n",
-    "\n",
-    "        # Defining the action models along the time instances\n",
-    "        loco3dModel = []\n",
-    "        # Creating the action models for three steps\n",
-    "        firstStep = self.createFootstepModels(rightFootId, leftFootId,\n",
-    "                                              0.5*stepLength, leftFootPos0,\n",
-    "                                              stepKnots)\n",
-    "        secondStep = self.createFootstepModels(leftFootId, rightFootId,\n",
-    "                                               stepLength, rightFootPos0,\n",
-    "                                               stepKnots)\n",
-    "        thirdStep = self.createFootstepModels(rightFootId, leftFootId,\n",
-    "                                              stepLength, leftFootPos0,\n",
-    "                                              stepKnots)\n",
-    "\n",
-    "        # Creating the action model for the double support phase\n",
-    "        doubleSupport = \\\n",
-    "            [ self.createSwingFootModel(\n",
-    "                timeStep,\n",
-    "                [ rightFootId, leftFootId ]\n",
-    "                ) for k in range(supportKnots) ]\n",
-    "\n",
-    "        # We defined the problem as:\n",
-    "        #  STEP 1 - DS - STEP 2 - DS - STEP 3 - DS\n",
-    "        loco3dModel += firstStep + doubleSupport\n",
-    "        loco3dModel += secondStep + doubleSupport\n",
-    "        loco3dModel += thirdStep + doubleSupport\n",
-    "\n",
-    "        problem = ShootingProblem(x0, loco3dModel, loco3dModel[-1])\n",
-    "        return problem\n",
-    "\n",
-    "    def createFootstepModels(self, supportFootId, swingFootId, stepLength, footPos0, numKnots):\n",
-    "        \"\"\" Action models for a footstep phase.\n",
-    "\n",
-    "        :param supportFootId: Id of the supporting foot\n",
-    "        :param swingFootId: Id of the swinging foot\n",
-    "        :param stepLength: step length\n",
-    "        :param footPos0: initial position of the swinging foot\n",
-    "        :param numKnots: number of knots for the footstep phase\n",
-    "        :return footstep action models\n",
-    "        \"\"\"\n",
-    "        # Action models for the foot swing\n",
-    "        footSwingModel = \\\n",
-    "            [ self.createSwingFootModel(\n",
-    "                timeStep,\n",
-    "                [ supportFootId ],\n",
-    "                TaskSE3(\n",
-    "                    pinocchio.SE3(np.eye(3),\n",
-    "                                  np.asmatrix(a2m([ [(stepLength*k)/numKnots, 0., 0.] ]) +\n",
-    "                                  footPos0)),\n",
-    "                    swingFootId)\n",
-    "                ) for k in range(numKnots) ]\n",
-    "        # Action model for the foot switch\n",
-    "        footSwitchModel = \\\n",
-    "            self.createFootSwitchModel(\n",
-    "                [ supportFootId ],\n",
-    "                TaskSE3(\n",
-    "                    pinocchio.SE3(np.eye(3),\n",
-    "                                  np.asmatrix(a2m([ stepLength, 0., 0. ]) +\n",
-    "                                  footPos0)),\n",
-    "                    swingFootId)\n",
-    "                )\n",
-    "        # Updating the current foot position for next step\n",
-    "        footPos0 += np.asmatrix(a2m([ stepLength, 0., 0. ]))\n",
-    "        return footSwingModel + [ footSwitchModel ]\n",
-    "\n",
-    "    def createSwingFootModel(self, timeStep, supportFootIds, swingFootTask = None):\n",
-    "        \"\"\" Action model for a swing foot phase.\n",
-    "\n",
-    "        :param timeStep: step duration of the action model\n",
-    "        :param supportFootIds: Ids of the constrained feet\n",
-    "        :param swingFootTask: swinging foot task\n",
-    "        :return action model for a swing foot phase\n",
-    "        \"\"\"\n",
-    "        # Creating the action model for floating-base systems. A walker system \n",
-    "        # is by default a floating-base system\n",
-    "        actModel = ActuationModelFreeFloating(self.rmodel)\n",
-    "\n",
-    "        # Creating a 6D multi-contact model, and then including the supporting\n",
-    "        # foot\n",
-    "        contactModel = ContactModelMultiple(self.rmodel)\n",
-    "        for i in supportFootIds:\n",
-    "            supportContactModel = \\\n",
-    "                ContactModel6D(self.rmodel, i, ref=pinocchio.SE3.Identity(), gains=[0.,0.])\n",
-    "            contactModel.addContact('contact_'+str(i), supportContactModel)\n",
-    "\n",
-    "        # Creating the cost model for a contact phase\n",
-    "        costModel = CostModelSum(self.rmodel, actModel.nu)\n",
-    "        if swingFootTask != None:\n",
-    "            footTrack = CostModelFramePlacement(self.rmodel,\n",
-    "                                                swingFootTask.frameId,\n",
-    "                                                swingFootTask.oXf,\n",
-    "                                                actModel.nu)\n",
-    "            costModel.addCost(\"footTrack\", footTrack, 100.)\n",
-    "\n",
-    "        stateWeights = \\\n",
-    "            np.array([0]*6 + [0.01]*(self.rmodel.nv-6) + [10]*self.rmodel.nv)\n",
-    "        stateReg = CostModelState(self.rmodel,\n",
-    "                                  self.state,\n",
-    "                                  self.rmodel.defaultState,\n",
-    "                                  actModel.nu,\n",
-    "                                  activation=ActivationModelWeightedQuad(stateWeights**2))\n",
-    "        ctrlReg = CostModelControl(self.rmodel, actModel.nu)\n",
-    "        costModel.addCost(\"stateReg\", stateReg, 0.1)\n",
-    "        costModel.addCost(\"ctrlReg\", ctrlReg, 0.001)\n",
-    "\n",
-    "        # Creating the action model for the KKT dynamics with simpletic Euler\n",
-    "        # integration scheme\n",
-    "        dmodel = \\\n",
-    "            DifferentialActionModelFloatingInContact(self.rmodel,\n",
-    "                                                     actModel,\n",
-    "                                                     contactModel,\n",
-    "                                                     costModel)\n",
-    "        model = IntegratedActionModelEuler(dmodel)\n",
-    "        model.timeStep = timeStep\n",
-    "        return model\n",
-    "\n",
-    "    def createFootSwitchModel(self, supportFootId, swingFootTask):\n",
-    "        \"\"\" Action model for a foot switch phase.\n",
-    "\n",
-    "        :param timeStep: step duration of the action model\n",
-    "        :param supportFootIds: Ids of the constrained feet\n",
-    "        :param swingFootTask: swinging foot task\n",
-    "        :return action model for a foot switch phase\n",
-    "        \"\"\"\n",
-    "        model = self.createSwingFootModel(0., supportFootId, swingFootTask)\n",
-    "\n",
-    "        impactFootVelCost = \\\n",
-    "            CostModelFrameVelocity(self.rmodel, swingFootTask.frameId)\n",
-    "        model.differential.costs.addCost('impactVel', impactFootVelCost, 10000.)\n",
-    "        model.differential.costs['impactVel' ].weight = 100000\n",
-    "        model.differential.costs['footTrack' ].weight = 100000\n",
-    "        model.differential.costs['stateReg'].weight = 10\n",
-    "        model.differential.costs['ctrlReg'].weight = 0.001\n",
-    "        return model"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "This class builds action models for each locomotion phase:\n",
-    "  - createSwingFootModel: defines an action model for the swing phase\n",
-    "  - createFootSwitchModel: defines an action model for switch knots between phases\n",
-    "  \n",
-    "Then we build a walking by combining a set of contact phases and their contact switches. This is defined by createFootstepModel\n",
-    "\n",
-    "Now let's create a walking OC problem for the Talos legs."
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "# Creating the lower-body part of Talos\n",
-    "talos_legs = loadTalosLegs()\n",
-    "\n",
-    "# Setting up the 3d walking problem\n",
-    "rightFoot = 'right_sole_link'\n",
-    "leftFoot = 'left_sole_link'\n",
-    "walk = SimpleBipedWalkingProblem(talos_legs.model, rightFoot, leftFoot)\n",
-    "\n",
-    "\n",
-    "# Create the initial state\n",
-    "q0 = talos_legs.q0.copy()\n",
-    "v0 = zero(talos_legs.model.nv)\n",
-    "x0 = m2a(np.concatenate([q0,v0]))\n",
-    "\n",
-    "\n",
-    "# Creating the walking problem\n",
-    "stepLength = 0.6 # meters\n",
-    "timeStep = 0.0375 # seconds\n",
-    "stepKnots = 20\n",
-    "supportKnots = 10\n",
-    "walkProblem = walk.createProblem(x0, stepLength, timeStep, stepKnots, supportKnots)\n",
-    "\n",
-    "\n",
-    "\n",
-    "# Solving the 3d walking problem using DDP\n",
-    "ddp = SolverDDP(walkProblem)\n",
-    "cameraTF = [3., 3.68, 0.84, 0.2, 0.62, 0.72, 0.22]\n",
-    "ddp.callback = [CallbackDDPLogger(), CallbackDDPVerbose(),\n",
-    "                CallbackSolverDisplay(talos_legs,4,1,cameraTF)]\n",
-    "ddp.th_stop = 1e-9\n",
-    "ddp.solve(maxiter=1000,regInit=.1,init_xs=[talos_legs.model.defaultState]*len(ddp.models()))"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "With the following commands we can plot \n",
-    " - the state and control trajectories, and\n",
-    " - the DDP performance"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "%matplotlib inline\n",
-    "from crocoddyl import plotOCSolution, plotDDPConvergence\n",
-    "\n",
-    "# Plotting the solution and the DDP convergence\n",
-    "log = ddp.callback[0]\n",
-    "plotOCSolution(log.xs, log.us)\n",
-    "plotDDPConvergence(log.costs,log.control_regs,\n",
-    "                   log.state_regs,log.gm_stops,\n",
-    "                   log.th_stops,log.steps)\n"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "Finally we can visualize the solution using gepetto-viewer."
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "# Visualization of the DDP solution in gepetto-viewer\n",
-    "ddp.callback[2](ddp)\n",
-    "CallbackSolverDisplay(talos_legs)(ddp)"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "## VI. Understanding the walking problem\n",
-    "\n",
-    "In this problem we pre-defined a 20 and 10 knots for the step and double support phases with $dt=$3.75e-2, repectively. \n",
-    "\n",
-    " 1. Could you tell us how much is the foot step and double support duration?\n",
-    " 2. What happens when do we change $dt$ (e.g. 2e-2 secs)?\n",
-    " 3. What happens when do we change the number of step knots (e.g. 10)?\n"
-   ]
-  }
- ],
- "metadata": {
-  "kernelspec": {
-   "display_name": "Python 2",
-   "language": "python",
-   "name": "python2"
-  },
-  "language_info": {
-   "codemirror_mode": {
-    "name": "ipython",
-    "version": 2
-   },
-   "file_extension": ".py",
-   "mimetype": "text/x-python",
-   "name": "python",
-   "nbconvert_exporter": "python",
-   "pygments_lexer": "ipython2",
-   "version": "2.7.12"
-  }
- },
- "nbformat": 4,
- "nbformat_minor": 2
-}
diff --git a/examples/notebooks/cartpole_swing_up.ipynb b/examples/notebooks/cartpole_swing_up.ipynb
index a18845ffa8fc4f03faf8b1a37500f96111991354..832be40234092de4fb58933b51d225f21aa6c524 100644
--- a/examples/notebooks/cartpole_swing_up.ipynb
+++ b/examples/notebooks/cartpole_swing_up.ipynb
@@ -66,7 +66,7 @@
     "    def calc(self, data, x, u=None):\n",
     "        if u is None: u=model.unone\n",
     "        # Getting the state and control variables\n",
-    "        x, th, xdot, thdot = np.asscalar(x[0]), np.asscalar(x[1]), np.asscalar(x[2]), np.asscalar(x[3])\n",
+    "        y, th, ydot, thdot = np.asscalar(x[0]), np.asscalar(x[1]), np.asscalar(x[2]), np.asscalar(x[3])\n",
     "        f = np.asscalar(u[0])\n",
     "\n",
     "        # Shortname for system parameters\n",
@@ -85,7 +85,7 @@
     "        data.xout = np.matrix([ xddot, thddot ]).T\n",
     "        \n",
     "        # Computing the cost residual and value\n",
-    "        data.r = np.matrix(self.costWeights * np.array([ s, 1-c, x, xdot, thdot, f ])).T\n",
+    "        data.r = np.matrix(self.costWeights * np.array([ s, 1-c, y, ydot, thdot, f ])).T\n",
     "        data.cost = .5* np.asscalar(sum(np.asarray(data.r)**2))\n",
     "\n",
     "    def calcDiff(model,data,x,u=None,recalc=True):\n",
@@ -126,7 +126,7 @@
     "cartpoleData = cartpoleDAM.createData()\n",
     "x = cartpoleDAM.State.rand()\n",
     "u = np.zeros(1)\n",
-    "cartpoleDAM.calc(cartpoleData,x,u)"
+    "cartpoleDAM.calc(cartpoleData, x, u)"
    ]
   },
   {
diff --git a/examples/notebooks/cartpole_swing_up_sol.ipynb b/examples/notebooks/cartpole_swing_up_sol.ipynb
index 03be544176658bc7f5d61b55a8c9c2bdd8a907b1..1be4758c79a86daf2000f3e99fe46a89631cf09d 100644
--- a/examples/notebooks/cartpole_swing_up_sol.ipynb
+++ b/examples/notebooks/cartpole_swing_up_sol.ipynb
@@ -66,7 +66,7 @@
     "    def calc(self, data, x, u=None):\n",
     "        if u is None: u=model.unone\n",
     "        # Getting the state and control variables\n",
-    "        x, th, xdot, thdot = np.asscalar(x[0]), np.asscalar(x[1]), np.asscalar(x[2]), np.asscalar(x[3])\n",
+    "        y, th, ydot, thdot = np.asscalar(x[0]), np.asscalar(x[1]), np.asscalar(x[2]), np.asscalar(x[3])\n",
     "        f = np.asscalar(u[0])\n",
     "\n",
     "        # Shortname for system parameters\n",
@@ -81,7 +81,7 @@
     "        data.xout = np.matrix([ xddot,thddot ]).T\n",
     "\n",
     "        # Computing the cost residual and value\n",
-    "        data.r = np.matrix(self.costWeights * np.array([ s, 1-c, x, xdot, thdot, f ])).T\n",
+    "        data.r = np.matrix(self.costWeights * np.array([ s, 1-c, y, ydot, thdot, f ])).T\n",
     "        data.cost = .5* np.asscalar(sum(np.asarray(data.r)**2))\n",
     "\n",
     "    def calcDiff(self,data,x,u=None,recalc=True):\n",
@@ -106,7 +106,7 @@
     "cartpoleData = cartpoleDAM.createData()\n",
     "x = cartpoleDAM.state.rand()\n",
     "u = np.zeros(1)\n",
-    "cartpoleDAM.calc(cartpoleData,x,u)"
+    "cartpoleDAM.calc(cartpoleData, x, u)"
    ]
   },
   {
diff --git a/examples/notebooks/introduction_to_crocoddyl.ipynb b/examples/notebooks/introduction_to_crocoddyl.ipynb
index deea9e4b56035b1642be7f87a7be089cef7369e5..1f8069bef6ee645c891cbf9b95ca4f1db9c4a171 100644
--- a/examples/notebooks/introduction_to_crocoddyl.ipynb
+++ b/examples/notebooks/introduction_to_crocoddyl.ipynb
@@ -149,18 +149,20 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 1,
+   "execution_count": null,
    "metadata": {},
    "outputs": [],
    "source": [
-    "from crocoddyl import *\n",
+    "import crocoddyl\n",
     "import numpy as np\n",
+    "import example_robot_data\n",
     "\n",
-    "talos_arm = loadTalosArm()\n",
+    "talos_arm = example_robot_data.loadTalosArm()\n",
+    "robot_model = talos_arm.model # getting the Pinocchio model\n",
     "\n",
     "# Defining a initial state\n",
-    "q0 = [0.173046, 1., -0.52366, 0., 0., 0.1, -0.005]\n",
-    "x0 = np.hstack([q0, np.zeros(talos_arm.model.nv)])"
+    "q0 = np.matrix([0.173046, 1., -0.52366, 0., 0., 0.1, -0.005]).T\n",
+    "x0 = np.vstack([q0, np.matrix(np.zeros(talos_arm.model.nv)).T])"
    ]
   },
   {
@@ -186,93 +188,68 @@
    "metadata": {},
    "outputs": [],
    "source": [
-    "import numpy as np\n",
     "import pinocchio\n",
     "\n",
     "\n",
-    "class DifferentialActionModelABA:\n",
-    "    def __init__(self,pinocchioModel):\n",
-    "        # The forward dynamics and its derivatives are computed with Pinocchio\n",
-    "        self.pinocchio = pinocchioModel\n",
-    "        self.nq,self.nv = self.pinocchio.nq, self.pinocchio.nv\n",
-    "        \n",
-    "        # Describes integrate, difference, Jacobian integrate and Jacobian difference\n",
-    "        # for any Pinocchio model\n",
-    "        self.State = StatePinocchio(self.pinocchio)\n",
-    "        \n",
-    "        # Keeps a stack of cost functions\n",
-    "        self.costs = CostModelSum(self.pinocchio)\n",
-    "        \n",
-    "        # Dimension of the state, and its tangent space, and control\n",
-    "        self.nx = self.State.nx\n",
-    "        self.ndx = self.State.ndx\n",
-    "        self.nout = self.nv\n",
-    "        self.nu = self.nv\n",
-    "        self.unone = np.zeros(self.nu)\n",
-    "    @property\n",
-    "    def ncost(self): return self.costs.ncost\n",
-    "    def createData(self): return DifferentialActionDataABA(self) # create the data needed for this model\n",
-    "    def calc(model,data,x,u=None):\n",
-    "        if u is None: u=model.unone\n",
-    "        nx,nu,nq,nv,nout = model.nx,model.nu,model.nq,model.nv,model.nout\n",
-    "        q = a2m(x[:nq]) # from np array to matrix\n",
-    "        v = a2m(x[-nv:]) # from np array to matrix\n",
-    "        tauq = a2m(u) # from np array to matrix\n",
-    "        \n",
-    "        # Computes the next state through ABA\n",
-    "        data.xout[:] = pinocchio.aba(model.pinocchio,data.pinocchio,q,v,tauq).flat\n",
-    "        \n",
-    "        # Updates the kinematics needed for cost computation\n",
-    "        pinocchio.forwardKinematics(model.pinocchio,data.pinocchio,q,v)\n",
-    "        pinocchio.updateFramePlacements(model.pinocchio,data.pinocchio)\n",
-    "        \n",
-    "        # Computes the cost from a set of single cost functions\n",
-    "        data.cost = model.costs.calc(data.costs,x,u)\n",
-    "        return data.xout,data.cost\n",
-    "\n",
-    "    def calcDiff(model,data,x,u=None,recalc=True):\n",
-    "        if u is None: u=model.unone\n",
-    "        if recalc: xout,cost = model.calc(data,x,u)\n",
-    "        nx,ndx,nu,nq,nv,nout = model.nx,model.State.ndx,model.nu,model.nq,model.nv,model.nout\n",
-    "        q = a2m(x[:nq]) # from np array to matrix\n",
-    "        v = a2m(x[-nv:]) # from np array to matrix\n",
-    "        tauq = a2m(u) # from np array to matrix\n",
-    "        \n",
-    "        # Computes the ABA derivatives (dynamics), and keeps them inside data\n",
-    "        pinocchio.computeABADerivatives(model.pinocchio,data.pinocchio,q,v,tauq)\n",
-    "        data.Fx[:,:nv] = data.pinocchio.ddq_dq\n",
-    "        data.Fx[:,nv:] = data.pinocchio.ddq_dv\n",
-    "        data.Fu[:,:]   = data.Minv\n",
-    "\n",
-    "        # Updates the kinematics Jacobians needed for getting the derivatives of the cost function\n",
-    "        pinocchio.computeJointJacobians(model.pinocchio,data.pinocchio,q)\n",
-    "        pinocchio.updateFramePlacements(model.pinocchio,data.pinocchio)\n",
-    "        \n",
-    "        # Computes all derivatives of cost function\n",
-    "        model.costs.calcDiff(data.costs,x,u,recalc=False)\n",
-    "        return data.xout,data.cost\n",
-    "\n",
-    "\n",
-    "class DifferentialActionDataABA:\n",
-    "    def __init__(self,model):\n",
-    "        self.pinocchio = model.pinocchio.createData()\n",
-    "        self.costs = model.costs.createData(self.pinocchio)\n",
-    "        self.cost = np.nan\n",
-    "        self.xout = np.zeros(model.nout)\n",
-    "        nx,nu,ndx,nq,nv,nout = model.nx,model.nu,model.State.ndx,model.nq,model.nv,model.nout\n",
-    "        self.F = np.zeros([ nout,ndx+nu ])\n",
-    "        self.costResiduals = self.costs.residuals\n",
-    "        self.Fx = self.F[:,:ndx]\n",
-    "        self.Fu = self.F[:,-nu:]\n",
-    "        self.g   = self.costs.g\n",
-    "        self.L   = self.costs.L\n",
-    "        self.Lx  = self.costs.Lx\n",
-    "        self.Lu  = self.costs.Lu\n",
-    "        self.Lxx = self.costs.Lxx\n",
-    "        self.Lxu = self.costs.Lxu\n",
-    "        self.Luu = self.costs.Luu\n",
-    "        self.Rx  = self.costs.Rx\n",
-    "        self.Ru  = self.costs.Ru"
+    "class DifferentialFwdDynamics(crocoddyl.DifferentialActionModelAbstract):\n",
+    "    def __init__(self, state, costModel):\n",
+    "        crocoddyl.DifferentialActionModelAbstract.__init__(self, state, state.nv, costModel.nr)\n",
+    "        self.costs = costModel\n",
+    "        self.enable_force = True\n",
+    "        self.armature = np.matrix(np.zeros(0))\n",
+    "\n",
+    "    def calc(self, data, x, u=None):\n",
+    "        if u is None:\n",
+    "            u = self.unone\n",
+    "        q, v = x[:self.state.nq], x[-self.state.nv:]\n",
+    "        # Computing the dynamics using ABA or manually for armature case\n",
+    "        if self.enable_force:\n",
+    "            data.xout = pinocchio.aba(self.state.pinocchio, data.pinocchio, q, v, u)\n",
+    "        else:\n",
+    "            pinocchio.computeAllTerms(self.state.pinocchio, data.pinocchio, q, v)\n",
+    "            data.M = data.pinocchio.M\n",
+    "            if self.armature.size == self.state.nv:\n",
+    "                data.M[range(self.state.nv), range(self.state.nv)] += self.armature\n",
+    "            data.Minv = np.linalg.inv(data.M)\n",
+    "            data.xout = data.Minv * (u - data.pinocchio.nle)\n",
+    "        # Computing the cost value and residuals\n",
+    "        pinocchio.forwardKinematics(self.state.pinocchio, data.pinocchio, q, v)\n",
+    "        pinocchio.updateFramePlacements(self.state.pinocchio, data.pinocchio)\n",
+    "        self.costs.calc(data.costs, x, u)\n",
+    "        data.cost = data.costs.cost\n",
+    "\n",
+    "    def calcDiff(self, data, x, u=None, recalc=True):\n",
+    "        q, v = x[:self.state.nq], x[-self.state.nv:]\n",
+    "        if u is None:\n",
+    "            u = self.unone\n",
+    "        if recalc:\n",
+    "            self.calc(data, x, u)\n",
+    "            pinocchio.computeJointJacobians(self.state.pinocchio, data.pinocchio, q)\n",
+    "        # Computing the dynamics derivatives\n",
+    "        if self.enable_force:\n",
+    "            pinocchio.computeABADerivatives(self.state.pinocchio, data.pinocchio, q, v, u)\n",
+    "            data.Fx = np.hstack([data.pinocchio.ddq_dq, data.pinocchio.ddq_dv])\n",
+    "            data.Fu = data.pinocchio.Minv\n",
+    "        else:\n",
+    "            pinocchio.computeRNEADerivatives(self.state.pinocchio, data.pinocchio, q, v, data.xout)\n",
+    "            data.Fx = -np.hstack([data.Minv * data.pinocchio.dtau_dq, data.Minv * data.pinocchio.dtau_dv])\n",
+    "            data.Fu = data.Minv\n",
+    "        # Computing the cost derivatives\n",
+    "        self.costs.calcDiff(data.costs, x, u, False)\n",
+    "\n",
+    "    def set_armature(self, armature):\n",
+    "        if armature.size is not self.state.nv:\n",
+    "            print('The armature dimension is wrong, we cannot set it.')\n",
+    "        else:\n",
+    "            self.enable_force = False\n",
+    "            self.armature = armature.T\n",
+    "\n",
+    "    def createData(self):\n",
+    "        data = crocoddyl.DifferentialActionModelAbstract.createData(self)\n",
+    "        data.pinocchio = pinocchio.Data(self.state.pinocchio)\n",
+    "        data.costs = self.costs.createData(data.pinocchio)\n",
+    "        data.costs.shareMemory(data) # this allows us to share the memory of cost-terms of action model\n",
+    "        return data"
    ]
   },
   {
@@ -308,39 +285,21 @@
     "\n",
     "## III.a ABA dynamics for reaching a goal with Talos arm\n",
     "\n",
-    "Our optimal control solver interacts with a defined ShootingProblem. A shooting problem represents a stack of action models in which an action model defins a specific knot along the OC problem.\n",
+    "Our optimal control solver interacts with a defined ShootingProblem. A shooting problem represents a stack of action models in which an action model defines a specific node along the OC problem.\n",
     "\n",
-    "First we need to create an action model from DifferentialActionModelABA. We use it for building terminal and running action models. In this example, we employ an simpletic Euler integration rule as follows:"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "# Running and terminal action models\n",
-    "runningModel = IntegratedActionModelEuler(DifferentialActionModelABA(talos_arm.model))\n",
-    "terminalModel = IntegratedActionModelEuler(DifferentialActionModelABA(talos_arm.model))\n",
+    "First we need to create an action model from DifferentialFwdDynamics. We use it for building terminal and running action models. In this example, we employ an simpletic Euler integration rule.\n",
     "\n",
-    "# Defining the time duration for running action models and the terminal one\n",
-    "dt = 1e-3\n",
-    "runningModel.timeStep = dt"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "Next we define the set of cost functions for this problem. For this particular example, we formulate three running-cost functions: \n",
-    " - goal-tracking cost, $log(^fXd_o \\,^oX_f)$\n",
-    "   \n",
-    " - state and control regularization; and $\\|\\mathbf{x}-\\mathbf{x}_{ref}\\|, \\|\\mathbf{u}\\|$\n",
+    "Next we define the set of cost functions for this problem. For this particular example, we formulate three running-cost functions:\n",
+    "\n",
+    "    goal-tracking cost, 𝑙𝑜𝑔(𝑓𝑋𝑑𝑜𝑜𝑋𝑓)\n",
+    "\n",
+    "state and control regularization; and ‖𝐱−𝐱𝑟𝑒𝑓‖,‖𝐮‖\n",
     "\n",
     "one terminal-cost:\n",
-    " - goal cost. $\\|\\mathbf{u}_T\\|$\n",
-    " \n",
-    " First, let's create the common cost functions."
+    "\n",
+    "    goal cost. ‖𝐮𝑇‖\n",
+    "\n",
+    "First, let's create the common cost functions."
    ]
   },
   {
@@ -349,54 +308,32 @@
    "metadata": {},
    "outputs": [],
    "source": [
-    "from pinocchio.utils import *\n",
-    "\n",
-    "# Goal tracking cost\n",
-    "frameName = 'gripper_left_joint' #gripper_left_fingertip_2_link'\n",
-    "state = StatePinocchio(talos_arm.model)\n",
-    "SE3ref = pinocchio.SE3(np.eye(3), np.array([ [.0],[.0],[.4] ]))\n",
-    "goalTrackingCost = CostModelFramePlacement(talos_arm.model,\n",
-    "                                       nu=talos_arm.model.nv,\n",
-    "                                       frame=talos_arm.model.getFrameId(frameName),\n",
-    "                                       ref=SE3ref)\n",
-    "\n",
-    "# State and control regularization\n",
-    "xRegCost = CostModelState(talos_arm.model,\n",
-    "                          state,\n",
-    "                          ref=state.zero(),\n",
-    "                          nu=talos_arm.model.nv)\n",
-    "uRegCost = CostModelControl(talos_arm.model,nu=talos_arm.model.nv)\n",
-    "\n",
-    "# Adds the running and terminal cost functions\n",
-    "runningCostModel = runningModel.differential.costs\n",
-    "runningCostModel.addCost( name=\"pos\", weight = 1e-3, cost = goalTrackingCost)\n",
-    "runningCostModel.addCost( name=\"regx\", weight = 1e-7, cost = xRegCost) \n",
-    "runningCostModel.addCost( name=\"regu\", weight = 1e-7, cost = uRegCost)\n",
-    "terminalCostModel = terminalModel.differential.costs\n",
-    "terminalCostModel.addCost( name=\"pos\", weight = 1, cost = goalTrackingCost)\n",
-    "\n",
-    "\n",
-    "# Let's compute the cost and its derivatives\n",
-    "robot_data = talos_arm.model.createData() # Pinocchio data\n",
-    "data = goalTrackingCost.createData(robot_data)\n",
-    "\n",
-    "# Update kinematics\n",
-    "q = pinocchio.randomConfiguration(talos_arm.model)\n",
-    "v = rand(talos_arm.model.nv)\n",
-    "x = m2a(np.concatenate([q,v]))\n",
-    "u = m2a(rand(talos_arm.model.nv))\n",
-    "pinocchio.forwardKinematics(talos_arm.model,robot_data,q,v)\n",
-    "pinocchio.computeJointJacobians(talos_arm.model,robot_data,q)\n",
-    "pinocchio.updateFramePlacements(talos_arm.model,robot_data)\n",
-    "\n",
-    "print 'cost =', goalTrackingCost.calc(data, x, u)\n",
-    "print 'cost =', goalTrackingCost.calcDiff(data, x, u)\n",
-    "print\n",
-    "print 'lx =', data.Lx\n",
-    "print 'lu =', data.Lu\n",
-    "print\n",
-    "print 'lxx =', data.Lxx\n",
-    "print 'luu =', data.Luu"
+    "# Create the cost functions\n",
+    "target = np.array([0.4, 0., .4])\n",
+    "Mref = crocoddyl.FrameTranslation(robot_model.getFrameId(\"gripper_left_joint\"), np.matrix(target).T)\n",
+    "state = crocoddyl.StateMultibody(robot_model)\n",
+    "goalTrackingCost = crocoddyl.CostModelFrameTranslation(state, Mref)\n",
+    "xRegCost = crocoddyl.CostModelState(state)\n",
+    "uRegCost = crocoddyl.CostModelControl(state)\n",
+    "\n",
+    "# Create cost model per each action model\n",
+    "runningCostModel = crocoddyl.CostModelSum(state)\n",
+    "terminalCostModel = crocoddyl.CostModelSum(state)\n",
+    "\n",
+    "# Then let's added the running and terminal cost functions\n",
+    "runningCostModel.addCost(\"gripperPose\", goalTrackingCost, 1.)\n",
+    "runningCostModel.addCost(\"stateReg\", xRegCost, 1e-4)\n",
+    "runningCostModel.addCost(\"ctrlReg\", uRegCost, 1e-7)\n",
+    "terminalCostModel.addCost(\"gripperPose\", goalTrackingCost, 1000.)\n",
+    "terminalCostModel.addCost(\"stateReg\", xRegCost, 1e-4)\n",
+    "terminalCostModel.addCost(\"ctrlReg\", uRegCost, 1e-7)\n",
+    "\n",
+    "# Running and terminal action models\n",
+    "DT = 1e-3\n",
+    "runningModel = crocoddyl.IntegratedActionModelEuler(\n",
+    "    crocoddyl.DifferentialActionModelFreeFwdDynamics(state, runningCostModel), DT)\n",
+    "terminalModel = crocoddyl.IntegratedActionModelEuler(\n",
+    "    crocoddyl.DifferentialActionModelFreeFwdDynamics(state, terminalCostModel))"
    ]
   },
   {
@@ -415,7 +352,7 @@
     "# For this optimal control problem, we define 250 knots (or running action\n",
     "# models) plus a terminal knot\n",
     "T = 250\n",
-    "problem = ShootingProblem(x0, [ runningModel ]*T, terminalModel)"
+    "problem = crocoddyl.ShootingProblem(x0, [runningModel] * T, terminalModel)"
    ]
   },
   {
@@ -427,7 +364,7 @@
     "Please note that:\n",
     "- CallbackDDPLogger: store the solution information.\n",
     "- CallbackDDPVerbose(level): printing message during the iterates.\n",
-    "- CallbackSolverDisplay(robot,rate): display the state trajectory using Gepetto viewer."
+    "- CallbackDisplay(robot,rate): display the state trajectory using Gepetto viewer."
    ]
   },
   {
@@ -437,18 +374,20 @@
    "outputs": [],
    "source": [
     "# Creating the DDP solver for this OC problem, defining a logger\n",
-    "ddp = SolverDDP(problem)\n",
+    "ddp = crocoddyl.SolverDDP(problem)\n",
     "cameraTF = [2., 2.68, 0.54, 0.2, 0.62, 0.72, 0.22]\n",
-    "ddp.callback = [CallbackDDPLogger(), CallbackDDPVerbose(1), CallbackSolverDisplay(talos_arm,4,1,cameraTF)]\n",
+    "log = crocoddyl.CallbackLogger()\n",
+    "ddp.setCallbacks([log,\n",
+    "                  crocoddyl.CallbackVerbose(),\n",
+    "                  crocoddyl.CallbackDisplay(talos_arm,4,4,cameraTF)])\n",
     "\n",
     "# Solving it with the DDP algorithm\n",
     "ddp.solve()\n",
     "\n",
     "# Printing the reached position\n",
-    "log = ddp.callback[0]\n",
-    "frame_idx = talos_arm.model.getFrameId(frameName)\n",
-    "xT = log.xs[-1]\n",
-    "qT = np.asmatrix(xT[:talos_arm.model.nq]).T\n",
+    "frame_idx = talos_arm.model.getFrameId(\"gripper_left_joint\")\n",
+    "xT = ddp.xs[-1]\n",
+    "qT = xT[:talos_arm.model.nq]\n",
     "print\n",
     "print \"The reached pose by the wrist is\"\n",
     "print talos_arm.framePlacement(qT, frame_idx)"
@@ -468,15 +407,12 @@
    "outputs": [],
    "source": [
     "%matplotlib inline\n",
-    "# Plotting the solution and the DDP convergence\n",
-    "log = ddp.callback[0]\n",
-    "plotOCSolution(log.xs, log.us)\n",
-    "plotDDPConvergence(log.costs,log.control_regs,\n",
-    "                   log.state_regs,log.gm_stops,\n",
-    "                   log.th_stops,log.steps)\n",
+    "# # Plotting the solution and the DDP convergence\n",
+    "crocoddyl.plotOCSolution(log.xs, log.us)\n",
+    "crocoddyl.plotConvergence(log.costs, log.control_regs, log.state_regs, log.gm_stops, log.th_stops, log.steps)\n",
     "\n",
     "# Visualizing the solution in gepetto-viewer\n",
-    "CallbackSolverDisplay(talos_arm)(ddp)"
+    "crocoddyl.CallbackDisplay(talos_arm)(ddp)"
    ]
   },
   {
diff --git a/examples/notebooks/solutions/cartpole_dyn.py b/examples/notebooks/solutions/cartpole_dyn.py
index 390a3ef87bdcda0a9580ee979294d795a002d969..863bd3dc234415b56b432193ed15a260841e5ea1 100644
--- a/examples/notebooks/solutions/cartpole_dyn.py
+++ b/examples/notebooks/solutions/cartpole_dyn.py
@@ -2,7 +2,7 @@
 #      xddot, thddot = cartpole_dynamics(self, data, x, u)
 def cartpole_dynamics(model, data, x, u):
     # Getting the state and control variables
-    x, th, xdot, thdot = np.asscalar(x[0]), np.asscalar(x[1]), np.asscalar(x[2]), np.asscalar(x[3])
+    th, thdot = np.asscalar(x[1]), np.asscalar(x[3])
     f = np.asscalar(u[0])
 
     # Shortname for system parameters
diff --git a/examples/notebooks/unicycle_towards_origin.py b/examples/notebooks/unicycle_towards_origin.py
index 8c8006386fb24183d572dc95b52eada10e17d6cd..2a41fb6ff222bcd08e13fa5dd8547fdb983c3598 100644
--- a/examples/notebooks/unicycle_towards_origin.py
+++ b/examples/notebooks/unicycle_towards_origin.py
@@ -1,31 +1,31 @@
 import numpy as np
 
-from crocoddyl import *
+import crocoddyl
 from unicycle_utils import plotUnicycleSolution
 
 # Creating an action model for the unicycle system
-model = ActionModelUnicycle()
+model = crocoddyl.ActionModelUnicycle()
 
 # Setting up the cost weights
-model.costWeights = [
+model.r = [
     10.,  # state weight
     1.  # control weight
 ]
 
 # Formulating the optimal control problem
 T = 20  # number of knots
-x0 = np.array([-1., -1., 1.])  #x,y,theta
-problem = ShootingProblem(x0, [model] * T, model)
+x0 = np.matrix([-1., -1., 1.]).T  #x,y,theta
+problem = crocoddyl.ShootingProblem(x0, [model] * T, model)
 
 # Creating the DDP solver for this OC problem, defining a logger
-ddp = SolverDDP(problem)
-ddp.callback = [CallbackDDPLogger(), CallbackDDPVerbose()]
+ddp = crocoddyl.SolverDDP(problem)
+ddp.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose()])
 
 # Solving it with the DDP algorithm
 ddp.solve()
 
-# Plotting the solution, solver convergence and unycicle motion
-log = ddp.callback[0]
-plotOCSolution(log.xs, log.us)
-plotDDPConvergence(log.costs, log.control_regs, log.state_regs, log.gm_stops, log.th_stops, log.steps)
+# Plotting the solution, solver convergence and unicycle motion
+log = ddp.getCallbacks()[0]
+crocoddyl.plotOCSolution(log.xs, log.us)
+crocoddyl.plotConvergence(log.costs, log.control_regs, log.state_regs, log.gm_stops, log.th_stops, log.steps)
 plotUnicycleSolution(log.xs)
diff --git a/examples/quadrupedal_walking_balancing.py b/examples/quadrupedal_gaits.py
similarity index 63%
rename from examples/quadrupedal_walking_balancing.py
rename to examples/quadrupedal_gaits.py
index 0274bd02634b02003f27849212a839fa52b02acf..9fe1d10054cccec11e16f6e534a25490b35a35de 100644
--- a/examples/quadrupedal_walking_balancing.py
+++ b/examples/quadrupedal_gaits.py
@@ -1,3 +1,4 @@
+import os
 import sys
 
 import numpy as np
@@ -7,8 +8,8 @@ import example_robot_data
 import pinocchio
 from crocoddyl.utils.quadruped import SimpleQuadrupedalGaitProblem, plotSolution
 
-WITHDISPLAY = 'display' in sys.argv
-WITHPLOT = 'plot' in sys.argv
+WITHDISPLAY = 'display' in sys.argv or 'CROCODDYL_DISPLAY' in os.environ
+WITHPLOT = 'plot' in sys.argv or 'CROCODDYL_PLOT' in os.environ
 
 # Loading the HyQ model
 hyq = example_robot_data.loadHyQ()
@@ -28,11 +29,11 @@ gait = SimpleQuadrupedalGaitProblem(hyq.model, lfFoot, rfFoot, lhFoot, rhFoot)
 # Setting up all tasks
 GAITPHASES = [{
     'walking': {
-        'stepLength': 0.15,
-        'stepHeight': 0.2,
+        'stepLength': 0.25,
+        'stepHeight': 0.25,
         'timeStep': 1e-2,
         'stepKnots': 25,
-        'supportKnots': 5
+        'supportKnots': 2
     }
 }, {
     'trotting': {
@@ -40,7 +41,7 @@ GAITPHASES = [{
         'stepHeight': 0.2,
         'timeStep': 1e-2,
         'stepKnots': 25,
-        'supportKnots': 5
+        'supportKnots': 2
     }
 }, {
     'pacing': {
@@ -53,15 +54,18 @@ GAITPHASES = [{
 }, {
     'bounding': {
         'stepLength': 0.15,
-        'stepHeight': 0.2,
+        'stepHeight': 0.1,
         'timeStep': 1e-2,
         'stepKnots': 25,
         'supportKnots': 5
     }
 }, {
     'jumping': {
-        'jumpHeight': 0.5,
-        'timeStep': 1e-2
+        'jumpHeight': 0.15,
+        'jumpLength': [0.0, 0.3, 0.],
+        'timeStep': 1e-2,
+        'groundKnots': 10,
+        'flyingKnots': 20
     }
 }]
 cameraTF = [2., 2.68, 0.84, 0.2, 0.62, 0.72, 0.22]
@@ -71,37 +75,48 @@ for i, phase in enumerate(GAITPHASES):
     for key, value in phase.items():
         if key == 'walking':
             # Creating a walking problem
-            ddp[i] = crocoddyl.SolverDDP(
+            ddp[i] = crocoddyl.SolverFDDP(
                 gait.createWalkingProblem(x0, value['stepLength'], value['stepHeight'], value['timeStep'],
                                           value['stepKnots'], value['supportKnots']))
         elif key == 'trotting':
             # Creating a trotting problem
-            ddp[i] = crocoddyl.SolverDDP(
+            ddp[i] = crocoddyl.SolverFDDP(
                 gait.createTrottingProblem(x0, value['stepLength'], value['stepHeight'], value['timeStep'],
                                            value['stepKnots'], value['supportKnots']))
         elif key == 'pacing':
             # Creating a pacing problem
-            ddp[i] = crocoddyl.SolverDDP(
+            ddp[i] = crocoddyl.SolverFDDP(
                 gait.createPacingProblem(x0, value['stepLength'], value['stepHeight'], value['timeStep'],
                                          value['stepKnots'], value['supportKnots']))
         elif key == 'bounding':
             # Creating a bounding problem
-            ddp[i] = crocoddyl.SolverDDP(
+            ddp[i] = crocoddyl.SolverFDDP(
                 gait.createBoundingProblem(x0, value['stepLength'], value['stepHeight'], value['timeStep'],
                                            value['stepKnots'], value['supportKnots']))
         elif key == 'jumping':
             # Creating a jumping problem
-            ddp[i] = crocoddyl.SolverDDP(gait.createJumpingProblem(x0, value['jumpHeight'], value['timeStep']))
+            ddp[i] = crocoddyl.SolverFDDP(
+                gait.createJumpingProblem(x0, value['jumpHeight'], value['jumpLength'], value['timeStep'],
+                                          value['groundKnots'], value['flyingKnots']))
 
     # Added the callback functions
     print('*** SOLVE ' + key + ' ***')
-    if WITHDISPLAY:
-        ddp[i].setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackSolverDisplay(hyq, 4, 4, cameraTF)])
+    if WITHDISPLAY and WITHPLOT:
+        ddp[i].setCallbacks(
+            [crocoddyl.CallbackLogger(),
+             crocoddyl.CallbackVerbose(),
+             crocoddyl.CallbackDisplay(hyq, 4, 4, cameraTF)])
+    elif WITHDISPLAY:
+        ddp[i].setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackVerbose()])
+    elif WITHPLOT:
+        ddp[i].setCallbacks([
+            crocoddyl.CallbackLogger(),
+            crocoddyl.CallbackVerbose(),
+        ])
     else:
         ddp[i].setCallbacks([crocoddyl.CallbackVerbose()])
 
     # Solving the problem with the DDP solver
-    ddp[i].th_stop = 1e-9
     xs = [hyq.model.defaultState] * len(ddp[i].models())
     us = [m.quasicStatic(d, hyq.model.defaultState) for m, d in list(zip(ddp[i].models(), ddp[i].datas()))[:-1]]
     ddp[i].solve(xs, us, 100, False, 0.1)
@@ -119,6 +134,20 @@ if WITHPLOT:
     xs = []
     us = []
     for i, phase in enumerate(GAITPHASES):
-        xs.extend(ddp[i].xs)
+        xs.extend(ddp[i].xs[:-1])
         us.extend(ddp[i].us)
-    plotSolution(hyq.model, xs, us)
+    log = ddp[0].getCallbacks()[0]
+    plotSolution(hyq.model, xs, us, figIndex=1, show=False)
+
+    for i, phase in enumerate(GAITPHASES):
+        title = phase.keys()[0] + " (phase " + str(i) + ")"
+        log = ddp[i].getCallbacks()[0]
+        crocoddyl.plotConvergence(log.costs,
+                                  log.control_regs,
+                                  log.state_regs,
+                                  log.gm_stops,
+                                  log.th_stops,
+                                  log.steps,
+                                  figTitle=title,
+                                  figIndex=i + 3,
+                                  show=True if i == len(GAITPHASES) - 1 else False)
diff --git a/include/crocoddyl/core/solver-base.hpp b/include/crocoddyl/core/solver-base.hpp
index 85e17ce48e278cc8b5815c08a840b8babe3f0d53..06f7817fdfecbc6e0b98986223c46f8eaf2480a4 100644
--- a/include/crocoddyl/core/solver-base.hpp
+++ b/include/crocoddyl/core/solver-base.hpp
@@ -36,6 +36,7 @@ class SolverAbstract {
                     const std::vector<Eigen::VectorXd>& us_warm = DEFAULT_VECTOR, const bool& is_feasible = false);
 
   void setCallbacks(const std::vector<CallbackAbstract*>& callbacks);
+  const std::vector<CallbackAbstract*>& getCallbacks() const;
 
   const ShootingProblem& get_problem() const;
   const std::vector<ActionModelAbstract*>& get_models() const;
diff --git a/include/crocoddyl/core/solvers/ddp.hpp b/include/crocoddyl/core/solvers/ddp.hpp
index 1cfa952fe08ff03f393541f61a9b78dd6bae278b..688794b042e9b89b071a2154c8cfab06002f75ec 100644
--- a/include/crocoddyl/core/solvers/ddp.hpp
+++ b/include/crocoddyl/core/solvers/ddp.hpp
@@ -33,6 +33,17 @@ class SolverDDP : public SolverAbstract {
   void backwardPass();
   void forwardPass(const double& stepLength);
 
+  void computeGains(unsigned int const& t);
+  void increaseRegularization();
+  void decreaseRegularization();
+  void allocateData();
+
+  const double& get_regfactor() const;
+  const double& get_regmin() const;
+  const double& get_regmax() const;
+  const std::vector<double>& get_alphas() const;
+  const double& get_th_step() const;
+  const double& get_th_grad() const;
   const std::vector<Eigen::MatrixXd>& get_Vxx() const;
   const std::vector<Eigen::VectorXd>& get_Vx() const;
   const std::vector<Eigen::MatrixXd>& get_Qxx() const;
@@ -44,10 +55,12 @@ class SolverDDP : public SolverAbstract {
   const std::vector<Eigen::VectorXd>& get_k() const;
   const std::vector<Eigen::VectorXd>& get_gaps() const;
 
-  void computeGains(unsigned int const& t);
-  void increaseRegularization();
-  void decreaseRegularization();
-  void allocateData();
+  void set_regfactor(double reg_factor);
+  void set_regmin(double regmin);
+  void set_regmax(double regmax);
+  void set_alphas(const std::vector<double>& alphas);
+  void set_th_step(double th_step);
+  void set_th_grad(double th_grad);
 
  protected:
   double regfactor_;
diff --git a/include/crocoddyl/core/solvers/fddp.hpp b/include/crocoddyl/core/solvers/fddp.hpp
index 372d75240619581cb6ea8ddd12678b505c810a02..0e83cbf7c2ee2445580b43d56f21c4138b81cafe 100644
--- a/include/crocoddyl/core/solvers/fddp.hpp
+++ b/include/crocoddyl/core/solvers/fddp.hpp
@@ -21,14 +21,19 @@ class SolverFDDP : public SolverDDP {
 
   explicit SolverFDDP(ShootingProblem& problem);
   ~SolverFDDP();
-  double calc();
-  void backwardPass();
-  void forwardPass(const double& stepLength);
-  const Eigen::Vector2d& expectedImprovement();
-  void updateExpectedImprovement();
+
   bool solve(const std::vector<Eigen::VectorXd>& init_xs = DEFAULT_VECTOR,
              const std::vector<Eigen::VectorXd>& init_us = DEFAULT_VECTOR, unsigned int const& maxiter = 100,
              const bool& is_feasible = false, const double& regInit = 1e-9);
+  void computeDirection(const bool& recalc = true);
+  double tryStep(const double& steplength = 1);
+  const Eigen::Vector2d& expectedImprovement();
+  void updateExpectedImprovement();
+  double calc();
+  void forwardPass(const double& stepLength);
+
+  double get_th_acceptnegstep() const;
+  void set_th_acceptnegstep(double th_acceptnegstep);
 
  protected:
   double dg_;
@@ -36,7 +41,7 @@ class SolverFDDP : public SolverDDP {
   double dv_;
 
  private:
-  double th_acceptNegStep_;
+  double th_acceptnegstep_;
 };
 
 }  // namespace crocoddyl
diff --git a/src/core/solver-base.cpp b/src/core/solver-base.cpp
index f39d309f6a23da4ca02f86342b5797cb1f82c38d..aeaac18b76aaaacd16d8576986c78a160706c9fc 100644
--- a/src/core/solver-base.cpp
+++ b/src/core/solver-base.cpp
@@ -74,6 +74,8 @@ void SolverAbstract::setCandidate(const std::vector<Eigen::VectorXd>& xs_warm,
 
 void SolverAbstract::setCallbacks(const std::vector<CallbackAbstract*>& callbacks) { callbacks_ = callbacks; }
 
+const std::vector<CallbackAbstract*>& SolverAbstract::getCallbacks() const { return callbacks_; }
+
 const ShootingProblem& SolverAbstract::get_problem() const { return problem_; }
 
 const std::vector<ActionModelAbstract*>& SolverAbstract::get_models() const { return models_; }
diff --git a/src/core/solvers/ddp.cpp b/src/core/solvers/ddp.cpp
index 0fd09beefcb92b395dfbc973f91bb6da3a60a65d..ee02437d307ed5b26234402d05167f9bf43a7abb 100644
--- a/src/core/solvers/ddp.cpp
+++ b/src/core/solvers/ddp.cpp
@@ -163,27 +163,23 @@ void SolverDDP::backwardPass() {
     Vxx_.back().diagonal() += x_reg_;
   }
 
+  if (!is_feasible_) {
+    Vx_.back().noalias() += Vxx_.back() * gaps_.back();
+  }
+
   for (int t = static_cast<int>(problem_.get_T()) - 1; t >= 0; --t) {
     ActionModelAbstract* m = problem_.running_models_[t];
     boost::shared_ptr<ActionDataAbstract>& d = problem_.running_datas_[t];
     const Eigen::MatrixXd& Vxx_p = Vxx_[t + 1];
     const Eigen::VectorXd& Vx_p = Vx_[t + 1];
-    const Eigen::VectorXd& gap_p = gaps_[t + 1];
 
     FxTVxx_p_.noalias() = d->get_Fx().transpose() * Vxx_p;
     FuTVxx_p_[t].noalias() = d->get_Fu().transpose() * Vxx_p;
     Qxx_[t].noalias() = d->get_Lxx() + FxTVxx_p_ * d->get_Fx();
     Qxu_[t].noalias() = d->get_Lxu() + FxTVxx_p_ * d->get_Fu();
     Quu_[t].noalias() = d->get_Luu() + FuTVxx_p_[t] * d->get_Fu();
-    if (!is_feasible_) {
-      // In case the xt+1 are not f(xt,ut) i.e warm start not obtained from roll-out.
-      fTVxx_p_.noalias() = Vxx_p * gap_p;
-      Qx_[t].noalias() = d->get_Lx() + d->get_Fx().transpose() * Vx_p + d->get_Fx().transpose() * fTVxx_p_;
-      Qu_[t].noalias() = d->get_Lu() + d->get_Fu().transpose() * Vx_p + d->get_Fu().transpose() * fTVxx_p_;
-    } else {
-      Qx_[t].noalias() = d->get_Lx() + d->get_Fx().transpose() * Vx_p;
-      Qu_[t].noalias() = d->get_Lu() + d->get_Fu().transpose() * Vx_p;
-    }
+    Qx_[t].noalias() = d->get_Lx() + d->get_Fx().transpose() * Vx_p;
+    Qu_[t].noalias() = d->get_Lu() + d->get_Fu().transpose() * Vx_p;
 
     if (!std::isnan(ureg_)) {
       unsigned int const& nu = m->get_nu();
@@ -205,6 +201,11 @@ void SolverDDP::backwardPass() {
       Vxx_[t].diagonal() += x_reg_;
     }
 
+    // Compute and store the Vx gradient at end of the interval (rollout state)
+    if (!is_feasible_) {
+      Vx_[t].noalias() += Vxx_[t] * gaps_[t];
+    }
+
     if (raiseIfNaN(Vx_[t].lpNorm<Eigen::Infinity>())) {
       throw "backward_error";
     }
@@ -248,11 +249,13 @@ void SolverDDP::forwardPass(const double& steplength) {
 }
 
 void SolverDDP::computeGains(const unsigned int& t) {
-  Quu_llt_[t].compute(Quu_[t]);
-  K_[t] = Qxu_[t].transpose();
-  Quu_llt_[t].solveInPlace(K_[t]);
-  k_[t] = Qu_[t];
-  Quu_llt_[t].solveInPlace(k_[t]);
+  if (problem_.running_models_[t]->get_nu() > 0) {
+    Quu_llt_[t].compute(Quu_[t]);
+    K_[t] = Qxu_[t].transpose();
+    Quu_llt_[t].solveInPlace(K_[t]);
+    k_[t] = Qu_[t];
+    Quu_llt_[t].solveInPlace(k_[t]);
+  }
 }
 
 void SolverDDP::increaseRegularization() {
@@ -332,6 +335,18 @@ void SolverDDP::allocateData() {
   fTVxx_p_ = Eigen::VectorXd::Zero(ndx);
 }
 
+const double& SolverDDP::get_regfactor() const { return regfactor_; }
+
+const double& SolverDDP::get_regmin() const { return regmin_; }
+
+const double& SolverDDP::get_regmax() const { return regmax_; }
+
+const std::vector<double>& SolverDDP::get_alphas() const { return alphas_; }
+
+const double& SolverDDP::get_th_step() const { return th_step_; }
+
+const double& SolverDDP::get_th_grad() const { return th_grad_; }
+
 const std::vector<Eigen::MatrixXd>& SolverDDP::get_Vxx() const { return Vxx_; }
 
 const std::vector<Eigen::VectorXd>& SolverDDP::get_Vx() const { return Vx_; }
@@ -352,4 +367,16 @@ const std::vector<Eigen::VectorXd>& SolverDDP::get_k() const { return k_; }
 
 const std::vector<Eigen::VectorXd>& SolverDDP::get_gaps() const { return gaps_; }
 
+void SolverDDP::set_regfactor(double regfactor) { regfactor_ = regfactor; }
+
+void SolverDDP::set_regmin(double regmin) { regmin_ = regmin; }
+
+void SolverDDP::set_regmax(double regmax) { regmax_ = regmax; }
+
+void SolverDDP::set_alphas(const std::vector<double>& alphas) { alphas_ = alphas; }
+
+void SolverDDP::set_th_step(double th_step) { th_step_ = th_step; }
+
+void SolverDDP::set_th_grad(double th_grad) { th_grad_ = th_grad; }
+
 }  // namespace crocoddyl
diff --git a/src/core/solvers/fddp.cpp b/src/core/solvers/fddp.cpp
index 55e65b2ef28f4df4d8f115bdadc328ff75320aa6..525d59dc00eba09fada90c17626eab0c990a4cfc 100644
--- a/src/core/solvers/fddp.cpp
+++ b/src/core/solvers/fddp.cpp
@@ -10,7 +10,7 @@
 
 namespace crocoddyl {
 
-SolverFDDP::SolverFDDP(ShootingProblem& problem) : SolverDDP(problem), dg_(0), dq_(0), dv_(0), th_acceptNegStep_(2) {}
+SolverFDDP::SolverFDDP(ShootingProblem& problem) : SolverDDP(problem), dg_(0), dq_(0), dv_(0), th_acceptnegstep_(2) {}
 
 SolverFDDP::~SolverFDDP() {}
 
@@ -60,17 +60,14 @@ bool SolverFDDP::solve(const std::vector<Eigen::VectorXd>& init_xs, const std::v
 
       if (dVexp_ > 0) {  // descend direction
         if (d_[0] < th_grad_ || dV_ > th_acceptstep_ * dVexp_) {
-          // Accept step
-
           was_feasible_ = is_feasible_;
           setCandidate(xs_try_, us_try_, (was_feasible_) || (steplength_ == 1));
           cost_ = cost_try_;
           recalc = true;
           break;
         }
-      } else {
-        if (d_[0] < th_grad_ || dV_ < th_acceptNegStep_ * dVexp_) {
-          // accept step
+      } else {  // reducing the gaps by allowing a small increment in the cost value
+        if (d_[0] < th_grad_ || dV_ < th_acceptnegstep_ * dVexp_) {
           was_feasible_ = is_feasible_;
           setCandidate(xs_try_, us_try_, (was_feasible_) || (steplength_ == 1));
           cost_ = cost_try_;
@@ -79,6 +76,7 @@ bool SolverFDDP::solve(const std::vector<Eigen::VectorXd>& init_xs, const std::v
         }
       }
     }
+
     if (steplength_ > th_step_) {
       decreaseRegularization();
     }
@@ -103,37 +101,29 @@ bool SolverFDDP::solve(const std::vector<Eigen::VectorXd>& init_xs, const std::v
   return false;
 }
 
-void SolverFDDP::updateExpectedImprovement() {
-  dg_ = 0;
-  dq_ = 0;
-  unsigned int const& T = this->problem_.get_T();
-  if (!is_feasible_) {
-    dg_ -= Vx_.back().transpose() * gaps_.back();
-    dq_ += gaps_.back().transpose() * Vxx_.back() * gaps_.back();
-  }
-  for (unsigned int t = 0; t < T; ++t) {
-    dg_ += Qu_[t].transpose() * k_[t];
-    dq_ -= k_[t].transpose() * Quuk_[t];
-    if (!is_feasible_) {
-      dg_ -= Vx_[t].transpose() * gaps_[t];
-      fTVxx_p_.noalias() = Vxx_[t] * gaps_[t];
-      dq_ += gaps_[t].transpose() * fTVxx_p_;
-    }
+void SolverFDDP::computeDirection(const bool& recalc) {
+  if (recalc) {
+    calc();
   }
+  backwardPass();
+}
+
+double SolverFDDP::tryStep(const double& steplength) {
+  forwardPass(steplength);
+  return cost_ - cost_try_;
 }
 
 const Eigen::Vector2d& SolverFDDP::expectedImprovement() {
   dv_ = 0;
-  d_.fill(0);
   unsigned int const& T = this->problem_.get_T();
   if (!is_feasible_) {
     problem_.running_models_.back()->get_state().diff(xs_try_.back(), xs_.back(), dx_.back());
     fTVxx_p_.noalias() = Vxx_.back() * dx_.back();
-    dv_ -= gaps_.back().transpose() * fTVxx_p_;
+    dv_ -= gaps_.back().dot(fTVxx_p_);
     for (unsigned int t = 0; t < T; ++t) {
       problem_.running_models_[t]->get_state().diff(xs_try_[t], xs_[t], dx_[t]);
       fTVxx_p_.noalias() = Vxx_[t] * dx_[t];
-      dv_ -= gaps_[t].transpose() * fTVxx_p_;
+      dv_ -= gaps_[t].dot(fTVxx_p_);
     }
   }
   d_[0] = dg_ + dv_;
@@ -141,6 +131,26 @@ const Eigen::Vector2d& SolverFDDP::expectedImprovement() {
   return d_;
 }
 
+void SolverFDDP::updateExpectedImprovement() {
+  dg_ = 0;
+  dq_ = 0;
+  unsigned int const& T = this->problem_.get_T();
+  if (!is_feasible_) {
+    dg_ -= Vx_.back().dot(gaps_.back());
+    fTVxx_p_.noalias() = Vxx_.back() * gaps_.back();
+    dq_ += gaps_.back().dot(fTVxx_p_);
+  }
+  for (unsigned int t = 0; t < T; ++t) {
+    dg_ += Qu_[t].dot(k_[t]);
+    dq_ -= k_[t].dot(Quuk_[t]);
+    if (!is_feasible_) {
+      dg_ -= Vx_[t].dot(gaps_[t]);
+      fTVxx_p_.noalias() = Vxx_[t] * gaps_[t];
+      dq_ += gaps_[t].dot(fTVxx_p_);
+    }
+  }
+}
+
 double SolverFDDP::calc() {
   cost_ = problem_.calcDiff(xs_, us_);
   if (!is_feasible_) {
@@ -161,69 +171,6 @@ double SolverFDDP::calc() {
   return cost_;
 }
 
-void SolverFDDP::backwardPass() {
-  boost::shared_ptr<ActionDataAbstract>& d_T = problem_.terminal_data_;
-  Vxx_.back() = d_T->get_Lxx();
-  Vx_.back() = d_T->get_Lx();
-
-  x_reg_.fill(xreg_);
-  if (!std::isnan(xreg_)) {
-    Vxx_.back().diagonal() += x_reg_;
-  }
-
-  if (!is_feasible_) {
-    Vx_.back() += Vxx_.back() * gaps_.back();
-  }
-
-  for (int t = static_cast<int>(problem_.get_T()) - 1; t >= 0; --t) {
-    ActionModelAbstract* m = problem_.running_models_[t];
-    boost::shared_ptr<ActionDataAbstract>& d = problem_.running_datas_[t];
-    const Eigen::MatrixXd& Vxx_p = Vxx_[t + 1];
-    const Eigen::VectorXd& Vx_p = Vx_[t + 1];
-
-    FxTVxx_p_.noalias() = d->get_Fx().transpose() * Vxx_p;
-    FuTVxx_p_[t].noalias() = d->get_Fu().transpose() * Vxx_p;
-    Qxx_[t].noalias() = d->get_Lxx() + FxTVxx_p_ * d->get_Fx();
-    Qxu_[t].noalias() = d->get_Lxu() + FxTVxx_p_ * d->get_Fu();
-    Quu_[t].noalias() = d->get_Luu() + FuTVxx_p_[t] * d->get_Fu();
-    Qx_[t].noalias() = d->get_Lx() + d->get_Fx().transpose() * Vx_p;
-    Qu_[t].noalias() = d->get_Lu() + d->get_Fu().transpose() * Vx_p;
-
-    if (!std::isnan(ureg_)) {
-      unsigned int const& nu = m->get_nu();
-      Quu_[t].diagonal() += Eigen::VectorXd::Constant(nu, ureg_);
-    }
-
-    computeGains(t);
-
-    if (std::isnan(ureg_)) {
-      Vx_[t].noalias() = Qx_[t] - K_[t].transpose() * Qu_[t];
-    } else {
-      Quuk_[t].noalias() = Quu_[t] * k_[t];
-      Vx_[t].noalias() = Qx_[t] + K_[t].transpose() * Quuk_[t] - 2 * K_[t].transpose() * Qu_[t];
-    }
-    Vxx_[t].noalias() = Qxx_[t] - Qxu_[t] * K_[t];
-    Vxx_[t] = 0.5 * (Vxx_[t] + Vxx_[t].transpose()).eval();
-    // TODO(cmastalli): as suggested by Nicolas
-
-    if (!std::isnan(xreg_)) {
-      Vxx_[t].diagonal() += x_reg_;
-    }
-
-    // Compute and store the Vx gradient at end of the interval (rollout state)
-    if (!is_feasible_) {
-      Vx_[t].noalias() += Vxx_[t] * gaps_[t];
-    }
-
-    if (raiseIfNaN(Vx_[t].lpNorm<Eigen::Infinity>())) {
-      throw "backward_error";
-    }
-    if (raiseIfNaN(Vxx_[t].lpNorm<Eigen::Infinity>())) {
-      throw "backward_error";
-    }
-  }
-}
-
 void SolverFDDP::forwardPass(const double& steplength) {
   assert(steplength <= 1. && "Step length has to be <= 1.");
   assert(steplength >= 0. && "Step length has to be >= 0.");
@@ -241,12 +188,13 @@ void SolverFDDP::forwardPass(const double& steplength) {
     m->get_state().diff(xs_[t], xs_try_[t], dx_[t]);
     us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t];
     m->calc(d, xs_try_[t], us_try_[t]);
+    xnext_ = d->get_xnext();
     cost_try_ += d->cost;
 
     if (raiseIfNaN(cost_try_)) {
       throw "forward_error";
     }
-    if (raiseIfNaN(d->get_xnext().lpNorm<Eigen::Infinity>())) {
+    if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
       throw "forward_error";
     }
   }
@@ -255,10 +203,9 @@ void SolverFDDP::forwardPass(const double& steplength) {
   boost::shared_ptr<ActionDataAbstract>& d = problem_.terminal_data_;
 
   if ((is_feasible_) || (steplength == 1)) {
-    xs_try_.back() = problem_.running_datas_.back()->get_xnext();
+    xs_try_.back() = xnext_;
   } else {
-    m->get_state().integrate(problem_.running_datas_.back()->get_xnext(), gaps_.back() * (steplength - 1),
-                             xs_try_.back());
+    m->get_state().integrate(xnext_, gaps_.back() * (steplength - 1), xs_try_.back());
   }
   m->calc(d, xs_try_.back());
   cost_try_ += d->cost;
@@ -268,4 +215,8 @@ void SolverFDDP::forwardPass(const double& steplength) {
   }
 }
 
+double SolverFDDP::get_th_acceptnegstep() const { return th_acceptnegstep_; }
+
+void SolverFDDP::set_th_acceptnegstep(double th_acceptnegstep) { th_acceptnegstep_ = th_acceptnegstep; }
+
 }  // namespace crocoddyl
diff --git a/unittest/bindings/test_solvers.py b/unittest/bindings/test_solvers.py
index 5dd124032916773054b0c44612a1f0450889d849..1e82027acb7dbb7bc627697aa2c3bf14299e5934 100644
--- a/unittest/bindings/test_solvers.py
+++ b/unittest/bindings/test_solvers.py
@@ -6,7 +6,7 @@ import numpy as np
 
 import crocoddyl
 import pinocchio
-from crocoddyl.utils import DDPDerived
+from crocoddyl.utils import DDPDerived, FDDPDerived
 
 
 class SolverAbstractTestCase(unittest.TestCase):
@@ -101,6 +101,12 @@ class UnicycleDDPTest(SolverAbstractTestCase):
     SOLVER_DER = DDPDerived
 
 
+class UnicycleFDDPTest(SolverAbstractTestCase):
+    MODEL = crocoddyl.ActionModelUnicycle()
+    SOLVER = crocoddyl.SolverFDDP
+    SOLVER_DER = FDDPDerived
+
+
 class ManipulatorDDPTest(SolverAbstractTestCase):
     ROBOT_MODEL = pinocchio.buildSampleModelManipulator()
     STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
@@ -118,8 +124,25 @@ class ManipulatorDDPTest(SolverAbstractTestCase):
     SOLVER_DER = DDPDerived
 
 
+class ManipulatorFDDPTest(SolverAbstractTestCase):
+    ROBOT_MODEL = pinocchio.buildSampleModelManipulator()
+    STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
+    COST_SUM = crocoddyl.CostModelSum(STATE, ROBOT_MODEL.nv)
+    COST_SUM.addCost('xReg', crocoddyl.CostModelState(STATE), 1e-7)
+    COST_SUM.addCost('uReg', crocoddyl.CostModelControl(STATE), 1e-7)
+    COST_SUM.addCost(
+        'frTrack',
+        crocoddyl.CostModelFramePlacement(
+            STATE, crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId("effector_body"), pinocchio.SE3.Random())), 1.)
+    DIFF_MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, COST_SUM)
+    MODEL = crocoddyl.IntegratedActionModelEuler(crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, COST_SUM),
+                                                 1e-3)
+    SOLVER = crocoddyl.SolverFDDP
+    SOLVER_DER = FDDPDerived
+
+
 if __name__ == '__main__':
-    test_classes_to_run = [UnicycleDDPTest, ManipulatorDDPTest]
+    test_classes_to_run = [UnicycleDDPTest, UnicycleFDDPTest, ManipulatorDDPTest, ManipulatorFDDPTest]
     loader = unittest.TestLoader()
     suites_list = []
     for test_class in test_classes_to_run: