diff --git a/.travis b/.travis
index 083fa2cb0fa4ad594926d9bb3d246075e62ce9ee..cbc92f81bdd0dcfd7fc1eae56801d8111267937d 160000
--- a/.travis
+++ b/.travis
@@ -1 +1 @@
-Subproject commit 083fa2cb0fa4ad594926d9bb3d246075e62ce9ee
+Subproject commit cbc92f81bdd0dcfd7fc1eae56801d8111267937d
diff --git a/.travis.yml b/.travis.yml
index bd30eed06e51b5fcb071d4fd14851ca1e30ae2d3..6b19a7f199c3056e2814450e39ce29cd6dc45c7e 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -5,9 +5,9 @@ compiler:
   - clang
 env:
   global:
-  - secure: ECiHIh0aT5ml/MdKifvFIM2UpDWiPsJPEZpafLYM8U0VAPYThSfUe8JWhMsky8amOwm38akbSbr6C7iBKVpzjAqpgNdOdufO1RUZ6pUvtlVXiXTw2KlqPqbDVlD3QroVDhnX/rIRcg5ezEHAIb594uEaHdf8tlikhjdTc3aAgMA=
+  - secure: "ECiHIh0aT5ml/MdKifvFIM2UpDWiPsJPEZpafLYM8U0VAPYThSfUe8JWhMsky8amOwm38akbSbr6C7iBKVpzjAqpgNdOdufO1RUZ6pUvtlVXiXTw2KlqPqbDVlD3QroVDhnX/rIRcg5ezEHAIb594uEaHdf8tlikhjdTc3aAgMA="
   - APT_DEPENDENCIES="doxygen doxygen-latex libboost-all-dev libeigen3-dev liblapack-dev libblas-dev gfortran python-dev python-sphinx"
-  - GIT_DEPENDENCIES="proyan/dynamic-graph"
+  - GIT_DEPENDENCIES="stack-of-tasks/dynamic-graph"
   - LCOV_IGNORE_RULES="*unitTesting*"
   allow_failures: 
   - compiler: clang
@@ -17,11 +17,12 @@ notifications:
 branches:
   only:
   - master
-  - debian
+  - devel
 script: ./.travis/run build
+script: ./travis_custom/build_custom
 after_success: ./.travis/run after_success
 after_failure: ./.travis/run after_failure
-before_install: ./.travis/run before_install
+before_install: ./travis_custom/custom_before_install
 matrix:
   allow_failures:
     - compiler: clang
diff --git a/travis_custom/build_custom b/travis_custom/build_custom
new file mode 100755
index 0000000000000000000000000000000000000000..5614d86394c1da329c2eaa2d5692ca13e4e0ae82
--- /dev/null
+++ b/travis_custom/build_custom
@@ -0,0 +1,178 @@
+#!/bin/bash
+. `dirname $0`/common.sh
+
+# Set debug mode
+set -x
+set -v
+
+# build_package
+# -------------
+#
+# Build the package using the last Eigen release (3.2) which is not
+# available as a Debian package on Ubuntu 12.04.
+build_package()
+{
+    echo "--> Building package..."
+
+    cd "$build_dir"
+
+    if [[ ";${DO_COVERAGE_ON_BRANCH};" == *";${CI_BRANCH};"* ]]; then
+      cmake "$root_dir" -DCMAKE_INSTALL_PREFIX="$install_dir"	\
+        -DCMAKE_CXX_FLAGS="--coverage"				\
+        -DCMAKE_EXE_LINKER_FLAGS="--coverage"			\
+        -DCMAKE_MODULE_LINKER_FLAGS="--coverage"		\
+        ${CMAKE_ADDITIONAL_OPTIONS}
+    else
+      cmake "$root_dir" -DCMAKE_INSTALL_PREFIX="$install_dir"	\
+        ${CMAKE_ADDITIONAL_OPTIONS}
+    fi
+
+    ${MAKE_PREFIX} make
+    make install
+
+    ALLOW_TESTSUITE_FAILURE=${ALLOW_TESTSUITE_FAILURE:-false}
+    make test || ${ALLOW_TESTSUITE_FAILURE}
+
+    if [[ ";${DO_CPPCHECK_ON_BRANCH};" == *";${CI_BRANCH};"* ]]; then
+      cppcheck --quiet --enable=all \
+        -I $root_dir/src -I $root_dir/tests -I $root_dir/include \
+        -I $root_dir/tests/shared-tests \
+        -I $build_dir/include -I $install_dir/include \
+        -i $build_dir/CMakeFiles \
+        $root_dir || true
+    fi
+}
+
+# debian_build_package
+# --------------------
+#
+# Use git-buildpackage and pbuilder to build the package in a sid
+# sandbox.
+debian_build_package()
+{
+    export GNUPGHOME="$root_dir/.travis/.gnupg"
+    export NAME="Thomas Moulard (Travis Automatic Builds)"
+    export DEBEMAIL="thomas.moulard+travis@gmail.com"
+
+    echo "--> Building Debian package..."
+    cd "$root_dir"
+
+    buildNumber=$(git rev-list \
+      $(git describe --tags --match "debian/*" --abbrev=0)..HEAD | wc -l) \
+      || buildNumber=1
+    dch --force-distribution --distribution ${DIST} \
+        --local ppa$buildNumber+$DIST "Travis automatic build"
+
+    echo "debian/changelog first line:"
+    head -n 1 debian/changelog
+
+    git add debian/changelog
+    git commit -m "Travis automatic commit"
+
+    ${SUDO_CMD} chmod -R 777 /var/cache/pbuilder/ccache
+
+    # If orig tarball exists, delete it.
+    rm -f "$build_dir/export/*_*.orig.tar*"
+    git-buildpackage					\
+      --git-submodules				\
+      --git-no-pristine-tar				\
+      --git-ignore-branch				\
+      --git-debian-branch=HEAD			\
+      --git-export-dir="$build_dir/export"		\
+      --git-tag					\
+      --git-upstream-branch=master			\
+      --git-dist=${DIST}				\
+      --git-pbuilder					\
+      --git-force-create				\
+      --git-ignore-new				\
+      --git-retag					\
+      -p\"gpg\\ --passphrase\\ ${GNUPG_PASSPHRASE}\" \
+      -k${DEBSIGN_KEYID} || exit 1
+
+
+    git-buildpackage			\
+      --git-submodules		\
+      --git-no-pristine-tar		\
+      --git-debian-branch=HEAD	\
+      --git-ignore-branch		\
+      --git-export-dir="$build_dir/export" \
+      --git-tag			\
+      --git-upstream-branch=master	\
+      --git-dist=${DIST}		\
+      --git-ignore-new		\
+      --git-retag			\
+      -p\"gpg --passphrase ${GNUPG_PASSPHRASE}\" \
+      -k${DEBSIGN_KEYID} \
+      -S -sa || exit 1
+}
+
+
+# setup_ros_build_environment
+# ---------------------------
+#
+# Source ROS setup scripts if they exist
+setup_ros_build_environment()
+{
+  if [ -e /opt/ros/${ROS_DISTRO}/setup.sh ]; then
+    . /opt/ros/${ROS_DISTRO}/setup.sh
+  fi
+  CATKIN_DEP_WORKSPACE=/tmp/_ci/catkin_dep_ws
+  if [ -e ${CATKIN_DEP_WORKSPACE}/devel/setup.sh ]; then
+    . ${CATKIN_DEP_WORKSPACE}/devel/setup.sh
+  fi
+  # Limit the number of parallel jobs when running catkin_make
+  PARALLEL_JOBS=${PARALLEL_JOBS:-1}
+  export ROS_PARALLEL_JOBS="-j ${PARALLEL_JOBS}"
+}
+
+# build_catkin_package
+# --------------------
+#
+# build all the packages using catkin_make.
+# Also check the installation (catkin_make install)
+# and check whether the catkin package is well written (catkin_lint)
+build_catkin_package()
+{
+    # Main package workspace
+    CATKIN_WORKSPACE=$build_dir/..
+    ln -s $root_dir/.. $CATKIN_WORKSPACE/src
+    cd $CATKIN_WORKSPACE/src
+    catkin_init_workspace
+
+    cd $CATKIN_WORKSPACE
+    catkin_make
+    for pack in `ls -d ./src/*/ ./src/*/*/`; do
+    if test -f $pack/package.xml; then
+      rosdoc_lite $pack
+    fi
+    done
+    catkin_make install
+
+    # run catkin_lint on every directory.
+    ALLOW_CATKINLINT_FAILURE=${ALLOW_CATKINLINT_FAILURE:-false}
+    catkin_lint `ls -d ./src/*/ ./src/*/*/`  || ${ALLOW_CATKINLINT_FAILURE}
+}
+
+setup_ros_build_environment
+# Realize a normal build in all branches except the one containing a
+# debian/ folder.
+if [ -d debian ]; then
+    if `test x${DIST} = x`; then
+      echo "distribution is not set, skipping this build"
+      exit 0
+    fi
+    echo "Target distribution: ${DIST}"
+    debian_build_package
+else
+    if [ ! x${DIST} = x ]; then
+      echo "skipping this build"
+      exit 0
+    fi
+    # checking if it is a ros folder. Taking appropriate measure.
+    #The current repository is a package
+    build_package
+fi
+
+# End debug mode
+set +v
+set +x
diff --git a/travis_custom/custom_before_install b/travis_custom/custom_before_install
new file mode 100755
index 0000000000000000000000000000000000000000..5239bd23cc52346bd6a1aceb2126061cf388d186
--- /dev/null
+++ b/travis_custom/custom_before_install
@@ -0,0 +1,255 @@
+#!/bin/bash
+
+rm -rf "$build_dir" "$install_dir"
+
+. `dirname $0`/common.sh
+
+# Set debug mode
+set -x
+set -v
+
+# Add robotpkg
+sudo sh -c "echo \"deb [arch=amd64] http://robotpkg.openrobots.org/wip/packages/debian/pub $(lsb_release -cs) robotpkg\" >> /etc/apt/sources.list "
+sudo sh -c "echo \"deb [arch=amd64] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -cs) robotpkg\" >> /etc/apt/sources.list "
+curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | sudo apt-key add -
+
+# show memory usage before install
+sudo free -m -t
+
+# Setup environment variables.
+export APT_DEPENDENCIES="doxygen libboost-system-dev libboost-test-dev libboost-filesystem-dev libboost-program-options-dev libeigen3-dev  libtinyxml-dev libconsole-bridge-dev robotpkg-dynamic-graph-v3 robotpkg-py27-dynamic-graph-v3"
+# Add Python dependency
+export APT_DEPENDENCIES=$APT_DEPENDENCIES" libboost-python-dev robotpkg-py27-eigenpy python2.7-dev python-numpy"
+
+# Add Geometry dependencies
+if [[ $BUILD_WITH_COLLISION_SUPPORT -eq ON ]]; then 
+  export APT_DEPENDENCIES=$APT_DEPENDENCIES" robotpkg-hpp-fcl"
+fi 
+
+##############################
+#  --  Helper functions  --  #
+##############################
+
+_linux_setup_package_source()
+{
+  # Speed up apt
+  ${SUDO_CMD} sh -c "echo \"force-unsafe-io\" > /etc/dpkg/dpkg.cfg.d/02apt-speedup"
+  # Update the apt local cache.
+  ${SUDO_CMD} apt-get update -qq
+}
+
+_osx_setup_package_source()
+{
+  # Update homebrew
+  brew update
+}
+
+# setup_package_source
+# ---------------------
+#
+# Setup the package source (e.g. homebrew on osx, apt on debian-like systems)
+setup_package_source()
+{
+  if [[ ${CI_OS_NAME} = linux ]]; then
+    _linux_setup_package_source
+  fi
+  if [[ ${CI_OS_NAME} = osx ]]; then
+    _osx_setup_package_source
+  fi
+}
+
+# setup_pbuilder
+# --------------
+#
+# Setup a pbuilder environment
+setup_pbuilder()
+{
+  if `test x${DIST} = x`; then
+      echo "distribution is not set, skipping this build"
+      exit 0
+  fi
+  echo "Target distribution: ${DIST}"
+
+  # If we are, we install Debian package development tools and
+  # create a sid pbuilder. Package dependencies will be installed
+  # automatically.
+  ${SUDO_CMD} apt-get install -qq        		\
+      debootstrap devscripts			\
+      git-buildpackage debian-archive-keyring \
+      pkg-kde-tools dput eatmydata ccache
+
+  # Fix ccache use in pbuilder
+  ${SUDO_CMD} addgroup --system --gid 1234 ccache
+  ${SUDO_CMD} adduser --quiet --system --uid 1234 --ingroup ccache \
+      --home /var/cache/pbuilder --no-create-home pbuilder
+  ${SUDO_CMD} mkdir -p /var/cache/pbuilder/ccache
+  ${SUDO_CMD} chown -R pbuilder:ccache /var/cache/pbuilder/ccache
+  ${SUDO_CMD} chmod -R g+ws /var/cache/pbuilder/ccache
+
+  # Remove previous sandbox.
+  ${SUDO_CMD} rm -rf /var/cache/pbuilder/base-${DIST}.cow || true
+
+  # Create a pbuilder sandbox.
+  cp -f `dirname $0`/pbuilderrc $HOME/.pbuilderrc
+  sed -i "s|@DIST@|${DIST}|g" $HOME/.pbuilderrc
+
+  git-pbuilder create
+
+  # Speed up pbuilder.
+  echo "echo \"force-unsafe-io\" > /etc/dpkg/dpkg.cfg.d/02apt-speedup" | \
+      git-pbuilder login --save-after-exec
+
+  # Add additional PPAs
+  for ppa in ${DEBIAN_PPA}; do
+      echo "apt-add-repository ppa:${ppa}" | \
+          git-pbuilder login --save-after-exec
+  done
+
+  # Retrieve PPA package list.
+  git-pbuilder update
+
+  # FIXME There is something fishy here...
+  # ccache is not necessary in our case and may cause permission
+  # issues.
+  echo "apt-get -y remove ccache" |        		\
+      git-pbuilder login --save-after-exec
+}
+
+# catkin_git_dependency
+# --------------------
+#
+# Clone catkin package into the workspace
+# See arguments of build_git_dependency
+# Branch defaults to $ROS_DISTRO instead of master
+catkin_git_dependency()
+{
+  git_dependency_parsing $1 $ROS_DISTRO
+  echo "--> Getting $git_dep (branch $git_dep_branch)"
+  CATKIN_DEP_WORKSPACE=/tmp/_ci/catkin_dep_ws
+  cd $CATKIN_DEP_WORKSPACE/src
+  $git_clone -b $git_dep_branch "$git_dep_uri" "$git_dep"
+}
+
+# catkin_build_workspace
+# ----------------------
+#
+# Build catkin workspace
+catkin_build_workspace()
+{
+  CATKIN_DEP_WORKSPACE=/tmp/_ci/catkin_dep_ws
+  cd $CATKIN_DEP_WORKSPACE
+  catkin_make
+}
+
+# build_git_dependency
+# --------------------
+#
+# Build a dependency directly from the Git development tree.
+# First argument: repository's GitHub URL or repository's URI + optional branch
+# For example: "jrl-umi3218/jrl-travis" or "jrl-umi3218/jrl-travis#dev"
+# Or: user@host:path/to/repo or git@github.com:organization/repo#branch
+build_git_dependency()
+{
+  git_dependency_parsing $1
+  echo "--> Compiling $git_dep (branch $git_dep_branch)"
+  cd "$build_dir"
+  mkdir -p "$git_dep"
+  $git_clone -b $git_dep_branch "$git_dep_uri" "$git_dep"
+  cd "$git_dep"
+  mkdir -p build
+  cd build
+  cmake .. -DCMAKE_INSTALL_PREFIX:STRING="$install_dir" \
+          -DDISABLE_TESTS:BOOL=ON ${CMAKE_ADDITIONAL_OPTIONS}
+  make install || make
+}
+
+_osx_install_dependencies()
+{
+  # Install user-specified packages
+  brew install cppcheck ${HOMEBREW_DEPENDENCIES}
+}
+
+_linux_install_dependencies()
+{
+  # Add additional PPAs
+  for ppa in ${MASTER_PPA}; do
+    ${SUDO_CMD} add-apt-repository -y ppa:${ppa}
+  done
+  ${SUDO_CMD} apt-get update -qq
+
+  ${SUDO_CMD} apt-get install -qq curl cppcheck ${APT_DEPENDENCIES}
+
+  # Install lcov from github 
+  cd "$build_dir"
+  wget https://github.com/linux-test-project/lcov/releases/download/v1.12/lcov-1.12.tar.gz
+  tar zxvf lcov-1.12.tar.gz
+  cd lcov-1.12
+  # Reset lcov to release 1.12
+  ${SUDO_CMD} make install
+
+  gem install coveralls-lcov
+}
+
+install_dependencies()
+{
+  if [[ ${CI_OS_NAME} = linux ]]; then
+    _linux_install_dependencies
+  fi
+  if [[ ${CI_OS_NAME} = osx ]]; then
+    _osx_install_dependencies
+  fi
+  # and we build directly dependencies from the Git repository
+  for package in ${ROS_GIT_DEPENDENCIES}; do
+    catkin_git_dependency "$package"
+  done
+  if `test "x${ROS_GIT_DEPENDENCIES}" != x`; then
+    catkin_build_workspace
+  fi
+
+  for package in ${GIT_DEPENDENCIES}; do
+    build_git_dependency "$package"
+  done
+}
+
+#########################
+#  --  Main script  --  #
+#########################
+
+# Print Git version
+git --version
+
+# Setup Git identity.
+git config --global user.name "JRL/IDH Continuous Integration Tool"
+git config --global user.email "jrl-idh+ci@gmail.com"
+
+# Retrieve the submodules.
+git submodule update --quiet --init --recursive
+
+# Fetch tags to compute the distance between the last release tag
+# and us.
+git fetch --quiet --tags
+
+# Shortcuts.
+git_clone="git clone --quiet --recursive"
+
+# Display environment
+echo "Environment:"
+env
+
+
+setup_package_source
+
+# Check if we are in a debian branch...
+if [ -d debian ]; then
+  setup_pbuilder
+else
+  if [ ! x${DIST} = x ]; then
+    echo "skipping this build"
+    exit 0
+  fi
+  install_dependencies
+fi
+
+# End debug mode
+set +v
+set +x