Repository: stack-of-tasks/tsid (language: python)

Issue:
Posture
This PR solves #168 


Discussion / hints:

Fix patch:
diff --git a/.git-blame-ignore-revs b/.git-blame-ignore-revs
new file mode 100644
index 00000000..6cdad43e
--- /dev/null
+++ b/.git-blame-ignore-revs
@@ -0,0 +1,2 @@
+# pre-commit run -a (Guilhem Saurel, 2022-09-05)
+1d932e2d389a6205e6d3a7490b5c826fe01fea47
diff --git a/.gitignore b/.gitignore
index c00f1862..ec97ed79 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,5 +1,7 @@
 *~
 *.pyc
 Xcode/
+.vscode/
 build*/
 _build*/
+.vscode
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
new file mode 100644
index 00000000..56bfbe3c
--- /dev/null
+++ b/.pre-commit-config.yaml
@@ -0,0 +1,14 @@
+ci:
+    autoupdate_branch: 'devel'
+repos:
+-   repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v4.3.0
+    hooks:
+    -   id: check-json
+    -   id: check-symlinks
+    -   id: check-toml
+    -   id: check-yaml
+-   repo: https://github.com/cheshirekow/cmake-format-precommit
+    rev: v0.6.13
+    hooks:
+    - id: cmake-format
diff --git a/CMakeLists.txt b/CMakeLists.txt
index b84f2155..5c6737c9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,160 +1,161 @@
 #
 # Copyright (c) 2017-2021 CNRS
 #
-# This file is part of tsid
-# tsid is free software: you can redistribute it
-# and/or modify it under the terms of the GNU Lesser General Public
-# License as published by the Free Software Foundation, either version
-# 3 of the License, or (at your option) any later version.
-# tsid is distributed in the hope that it will be
-# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
-# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
-# General Lesser Public License for more details. You should have
-# received a copy of the GNU Lesser General Public License along with
-# tsid If not, see
+# This file is part of tsid tsid is free software: you can redistribute it
+# and/or modify it under the terms of the GNU Lesser General Public License as
+# published by the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version. tsid is distributed in the hope that it
+# will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+# Lesser Public License for more details. You should have received a copy of the
+# GNU Lesser General Public License along with tsid If not, see
 # <http://www.gnu.org/licenses/>.
 
-CMAKE_MINIMUM_REQUIRED(VERSION 3.1)
+cmake_minimum_required(VERSION 3.1)
 
 # Project properties
-SET(PROJECT_ORG stack-of-tasks)
-SET(PROJECT_NAME tsid)
-SET(PROJECT_DESCRIPTION "Efficient Task Space Inverse Dynamics for Multi-body Systems based on Pinocchio")
-SET(PROJECT_URL "https://github.com/${PROJECT_ORG}/${PROJECT_NAME}")
+set(PROJECT_ORG stack-of-tasks)
+set(PROJECT_NAME tsid)
+set(PROJECT_DESCRIPTION
+    "Efficient Task Space Inverse Dynamics for Multi-body Systems based on Pinocchio"
+)
+set(PROJECT_URL "https://github.com/${PROJECT_ORG}/${PROJECT_NAME}")
 
 # Project options
-OPTION(BUILD_PYTHON_INTERFACE "Build the python bindings" ON)
-OPTION(INSTALL_PYTHON_INTERFACE_ONLY "Install *ONLY* the python bindings" OFF)
-OPTION(SUFFIX_SO_VERSION "Suffix library name with its version" ON)
-OPTION(INITIALIZE_WITH_NAN "Initialize Eigen entries with NaN" OFF)
-OPTION(EIGEN_RUNTIME_NO_MALLOC "If ON, it can assert in case of runtime allocation" ON)
-OPTION(EIGEN_NO_AUTOMATIC_RESIZING "If ON, it forbids automatic resizing of dynamics arrays and matrices" OFF)
+option(BUILD_PYTHON_INTERFACE "Build the python bindings" ON)
+option(INSTALL_PYTHON_INTERFACE_ONLY "Install *ONLY* the python bindings" OFF)
+option(SUFFIX_SO_VERSION "Suffix library name with its version" ON)
+option(INITIALIZE_WITH_NAN "Initialize Eigen entries with NaN" OFF)
+option(EIGEN_RUNTIME_NO_MALLOC
+       "If ON, it can assert in case of runtime allocation" ON)
+option(EIGEN_NO_AUTOMATIC_RESIZING
+       "If ON, it forbids automatic resizing of dynamics arrays and matrices"
+       OFF)
 
 # Project configuration
-IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
-  SET(PROJECT_USE_CMAKE_EXPORT TRUE)
-ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
-SET(CXX_DISABLE_WERROR TRUE)
-SET(DOXYGEN_USE_MATHJAX YES)
-SET(CMAKE_VERBOSE_MAKEFILE TRUE)
+if(NOT INSTALL_PYTHON_INTERFACE_ONLY)
+  set(PROJECT_USE_CMAKE_EXPORT TRUE)
+endif(NOT INSTALL_PYTHON_INTERFACE_ONLY)
+set(CXX_DISABLE_WERROR TRUE)
+set(DOXYGEN_USE_MATHJAX YES)
+set(CMAKE_VERBOSE_MAKEFILE TRUE)
 
 # JRL-cmakemodule setup
-INCLUDE(cmake/base.cmake)
-INCLUDE(cmake/boost.cmake)
-INCLUDE(cmake/python.cmake)
-INCLUDE(cmake/ide.cmake)
-INCLUDE(cmake/apple.cmake)
+include(cmake/base.cmake)
+include(cmake/ide.cmake)
+include(cmake/apple.cmake)
 
 # Handle APPLE Cmake policy
-IF(APPLE)
-  APPLY_DEFAULT_APPLE_CONFIGURATION()
-ENDIF(APPLE)
+if(APPLE)
+  apply_default_apple_configuration()
+endif(APPLE)
 
 # Project definition
-COMPUTE_PROJECT_ARGS(PROJECT_ARGS LANGUAGES CXX)
-PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
+compute_project_args(PROJECT_ARGS LANGUAGES CXX)
+project(${PROJECT_NAME} ${PROJECT_ARGS})
 
-IF(INITIALIZE_WITH_NAN)
-  MESSAGE(STATUS "Initialize with NaN all the Eigen entries.")
-  ADD_DEFINITIONS(-DEIGEN_INITIALIZE_MATRICES_BY_NAN)
-ENDIF(INITIALIZE_WITH_NAN)
+if(INITIALIZE_WITH_NAN)
+  message(STATUS "Initialize with NaN all the Eigen entries.")
+  add_definitions(-DEIGEN_INITIALIZE_MATRICES_BY_NAN)
+endif(INITIALIZE_WITH_NAN)
 
-IF(EIGEN_RUNTIME_NO_MALLOC)
-  MESSAGE(STATUS "Option EIGEN_RUNTIME_NO_MALLOC on.")
-  ADD_DEFINITIONS(-DEIGEN_RUNTIME_NO_MALLOC)
-ENDIF(EIGEN_RUNTIME_NO_MALLOC)
+if(EIGEN_RUNTIME_NO_MALLOC)
+  message(STATUS "Option EIGEN_RUNTIME_NO_MALLOC on.")
+  add_definitions(-DEIGEN_RUNTIME_NO_MALLOC)
+endif(EIGEN_RUNTIME_NO_MALLOC)
 
-IF(EIGEN_NO_AUTOMATIC_RESIZING)
-  MESSAGE(STATUS "Option EIGEN_NO_AUTOMATIC_RESIZING on.")
-  ADD_DEFINITIONS(-DEIGEN_NO_AUTOMATIC_RESIZING)
-ENDIF(EIGEN_NO_AUTOMATIC_RESIZING)
+if(EIGEN_NO_AUTOMATIC_RESIZING)
+  message(STATUS "Option EIGEN_NO_AUTOMATIC_RESIZING on.")
+  add_definitions(-DEIGEN_NO_AUTOMATIC_RESIZING)
+endif(EIGEN_NO_AUTOMATIC_RESIZING)
 
-CHECK_MINIMAL_CXX_STANDARD(11 ENFORCE)
+check_minimal_cxx_standard(11 ENFORCE)
 
 # Project dependencies
-ADD_PROJECT_DEPENDENCY(pinocchio 2.3.1 REQUIRED)
-ADD_PROJECT_DEPENDENCY(eiquadprog 1.1.3 REQUIRED)
+if(BUILD_PYTHON_INTERFACE)
+  set(PYWRAP ${PROJECT_NAME}_pywrap)
+  add_project_dependency(eigenpy 2.7.12 REQUIRED)
+endif(BUILD_PYTHON_INTERFACE)
 
-IF(BUILD_TESTING)
-  FIND_PACKAGE(Boost REQUIRED COMPONENTS unit_test_framework)
-ENDIF(BUILD_TESTING)
+add_project_dependency(pinocchio 2.3.1 REQUIRED)
+add_project_dependency(eiquadprog 1.1.3 REQUIRED)
 
-IF(BUILD_PYTHON_INTERFACE)
-  FINDPYTHON()
-  SET(PYWRAP ${PROJECT_NAME}_pywrap)
-  ADD_PROJECT_DEPENDENCY(eigenpy REQUIRED)
-  SEARCH_FOR_BOOST_PYTHON(REQUIRED)
-ENDIF(BUILD_PYTHON_INTERFACE)
+find_package(qpmad QUIET) # optional
+if(qpmad_FOUND)
+  add_project_dependency(qpmad QUIET)
+endif()
+
+if(BUILD_TESTING)
+  find_package(Boost REQUIRED COMPONENTS unit_test_framework)
+endif(BUILD_TESTING)
 
 # Main Library
-SET(${PROJECT_NAME}_MATH_HEADERS
-  include/tsid/math/fwd.hpp
-  include/tsid/math/utils.hpp
-  include/tsid/math/constraint-base.hpp
-  include/tsid/math/constraint-equality.hpp
-  include/tsid/math/constraint-inequality.hpp
-  include/tsid/math/constraint-bound.hpp
-  )
-
-SET(${PROJECT_NAME}_TASKS_HEADERS
-  include/tsid/tasks/fwd.hpp
-  include/tsid/tasks/task-base.hpp
-  include/tsid/tasks/task-motion.hpp
-  include/tsid/tasks/task-actuation.hpp
-  include/tsid/tasks/task-contact-force.hpp
-  include/tsid/tasks/task-com-equality.hpp
-  include/tsid/tasks/task-se3-equality.hpp
-  include/tsid/tasks/task-contact-force-equality.hpp
-  include/tsid/tasks/task-cop-equality.hpp
-  include/tsid/tasks/task-actuation-equality.hpp
-  include/tsid/tasks/task-actuation-bounds.hpp
-  include/tsid/tasks/task-joint-bounds.hpp
-  include/tsid/tasks/task-joint-posture.hpp
-  include/tsid/tasks/task-joint-posVelAcc-bounds.hpp
-  include/tsid/tasks/task-capture-point-inequality.hpp
-  include/tsid/tasks/task-angular-momentum-equality.hpp
-  )
-
-SET(${PROJECT_NAME}_CONTACTS_HEADERS
-  include/tsid/contacts/fwd.hpp
-  include/tsid/contacts/contact-base.hpp
-  include/tsid/contacts/contact-6d.hpp
-  include/tsid/contacts/contact-point.hpp
-  )
-
-SET(${PROJECT_NAME}_TRAJECTORIES_HEADERS
-  include/tsid/trajectories/fwd.hpp
-  include/tsid/trajectories/trajectory-base.hpp
-  include/tsid/trajectories/trajectory-se3.hpp
-  include/tsid/trajectories/trajectory-euclidian.hpp
-  )
-
-SET(${PROJECT_NAME}_SOLVERS_HEADERS
-  include/tsid/solvers/fwd.hpp
-  include/tsid/solvers/utils.hpp
-  include/tsid/solvers/solver-HQP-output.hpp
-  include/tsid/solvers/solver-HQP-base.hpp
-  include/tsid/solvers/solver-HQP-factory.hpp
-  include/tsid/solvers/solver-HQP-factory.hxx
-  include/tsid/solvers/solver-HQP-qpoases.hpp
-  include/tsid/solvers/solver-HQP-eiquadprog.hpp
-  include/tsid/solvers/solver-HQP-eiquadprog-rt.hpp
-  include/tsid/solvers/solver-HQP-eiquadprog-rt.hxx
-  include/tsid/solvers/solver-HQP-eiquadprog-fast.hpp
-  )
-
-SET(${PROJECT_NAME}_ROBOTS_HEADERS
-  include/tsid/robots/fwd.hpp
-  include/tsid/robots/robot-wrapper.hpp
-  )
-
-SET(${PROJECT_NAME}_FORMULATIONS_HEADERS
-  include/tsid/formulations/contact-level.hpp
-  include/tsid/formulations/inverse-dynamics-formulation-base.hpp
-  include/tsid/formulations/inverse-dynamics-formulation-acc-force.hpp
-  )
-
-FILE(GLOB ${PYWRAP}_HEADERS
+set(${PROJECT_NAME}_MATH_HEADERS
+    include/tsid/math/fwd.hpp
+    include/tsid/math/utils.hpp
+    include/tsid/math/constraint-base.hpp
+    include/tsid/math/constraint-equality.hpp
+    include/tsid/math/constraint-inequality.hpp
+    include/tsid/math/constraint-bound.hpp)
+
+set(${PROJECT_NAME}_TASKS_HEADERS
+    include/tsid/tasks/fwd.hpp
+    include/tsid/tasks/task-base.hpp
+    include/tsid/tasks/task-motion.hpp
+    include/tsid/tasks/task-actuation.hpp
+    include/tsid/tasks/task-contact-force.hpp
+    include/tsid/tasks/task-com-equality.hpp
+    include/tsid/tasks/task-se3-equality.hpp
+    include/tsid/tasks/task-contact-force-equality.hpp
+    include/tsid/tasks/task-cop-equality.hpp
+    include/tsid/tasks/task-actuation-equality.hpp
+    include/tsid/tasks/task-actuation-bounds.hpp
+    include/tsid/tasks/task-joint-bounds.hpp
+    include/tsid/tasks/task-joint-posture.hpp
+    include/tsid/tasks/task-joint-posVelAcc-bounds.hpp
+    include/tsid/tasks/task-capture-point-inequality.hpp
+    include/tsid/tasks/task-angular-momentum-equality.hpp)
+
+set(${PROJECT_NAME}_CONTACTS_HEADERS
+    include/tsid/contacts/fwd.hpp include/tsid/contacts/contact-base.hpp
+    include/tsid/contacts/contact-6d.hpp
+    include/tsid/contacts/contact-point.hpp)
+
+set(${PROJECT_NAME}_TRAJECTORIES_HEADERS
+    include/tsid/trajectories/fwd.hpp
+    include/tsid/trajectories/trajectory-base.hpp
+    include/tsid/trajectories/trajectory-se3.hpp
+    include/tsid/trajectories/trajectory-euclidian.hpp)
+
+set(${PROJECT_NAME}_SOLVERS_HEADERS
+    include/tsid/solvers/fwd.hpp
+    include/tsid/solvers/utils.hpp
+    include/tsid/solvers/solver-HQP-output.hpp
+    include/tsid/solvers/solver-HQP-base.hpp
+    include/tsid/solvers/solver-HQP-factory.hpp
+    include/tsid/solvers/solver-HQP-factory.hxx
+    include/tsid/solvers/solver-HQP-qpoases.hpp
+    include/tsid/solvers/solver-HQP-eiquadprog.hpp
+    include/tsid/solvers/solver-HQP-eiquadprog-rt.hpp
+    include/tsid/solvers/solver-HQP-eiquadprog-rt.hxx
+    include/tsid/solvers/solver-HQP-eiquadprog-fast.hpp)
+
+if(qpmad_FOUND)
+  list(APPEND ${PROJECT_NAME}_SOLVERS_HEADERS
+       include/tsid/solvers/solver-HQP-qpmad.hpp)
+endif()
+
+set(${PROJECT_NAME}_ROBOTS_HEADERS include/tsid/robots/fwd.hpp
+                                   include/tsid/robots/robot-wrapper.hpp)
+
+set(${PROJECT_NAME}_FORMULATIONS_HEADERS
+    include/tsid/formulations/contact-level.hpp
+    include/tsid/formulations/inverse-dynamics-formulation-base.hpp
+    include/tsid/formulations/inverse-dynamics-formulation-acc-force.hpp)
+
+file(
+  GLOB
+  ${PYWRAP}_HEADERS
   include/tsid/bindings/python/fwd.hpp
   include/tsid/bindings/python/constraint/*.hpp
   include/tsid/bindings/python/contacts/*.hpp
@@ -163,119 +164,114 @@ FILE(GLOB ${PYWRAP}_HEADERS
   include/tsid/bindings/python/solvers/*.hpp
   include/tsid/bindings/python/tasks/*.hpp
   include/tsid/bindings/python/trajectories/*.hpp
-  include/tsid/bindings/python/utils/*.hpp
-  )
-
-SET(${PROJECT_NAME}_HEADERS
-  include/tsid/config.hpp
-  include/tsid/macros.hpp
-  include/tsid/deprecation.hpp
-  include/tsid/utils/statistics.hpp
-  include/tsid/utils/stop-watch.hpp
-  include/tsid/utils/Stdafx.hh
-  ${${PROJECT_NAME}_MATH_HEADERS}
-  ${${PROJECT_NAME}_TASKS_HEADERS}
-  ${${PROJECT_NAME}_CONTACTS_HEADERS}
-  ${${PROJECT_NAME}_TRAJECTORIES_HEADERS}
-  ${${PROJECT_NAME}_SOLVERS_HEADERS}
-  ${${PROJECT_NAME}_ROBOTS_HEADERS}
-  ${${PROJECT_NAME}_FORMULATIONS_HEADERS}
-  )
-
-LIST(REMOVE_DUPLICATES ${PROJECT_NAME}_HEADERS)
-
-SET(${PROJECT_NAME}_MATH_SOURCES
-  src/math/constraint-base.cpp
-  src/math/constraint-equality.cpp
-  src/math/constraint-inequality.cpp
-  src/math/constraint-bound.cpp
-  src/math/utils.cpp
-  )
-
-SET(${PROJECT_NAME}_TASKS_SOURCES
-  src/tasks/task-base.cpp
-  src/tasks/task-actuation-bounds.cpp
-  src/tasks/task-actuation-equality.cpp
-  src/tasks/task-actuation.cpp
-  src/tasks/task-com-equality.cpp
-  src/tasks/task-contact-force-equality.cpp
-  src/tasks/task-contact-force.cpp
-  src/tasks/task-cop-equality.cpp
-  src/tasks/task-joint-bounds.cpp
-  src/tasks/task-joint-posture.cpp
-  src/tasks/task-joint-posVelAcc-bounds.cpp
-  src/tasks/task-capture-point-inequality.cpp
-  src/tasks/task-motion.cpp
-  src/tasks/task-se3-equality.cpp
-  src/tasks/task-angular-momentum-equality.cpp
-  )
-
-SET(${PROJECT_NAME}_CONTACTS_SOURCES
-  src/contacts/contact-base.cpp
-  src/contacts/contact-6d.cpp
-  src/contacts/contact-point.cpp
-  )
-
-SET(${PROJECT_NAME}_TRAJECTORIES_SOURCES
-  src/trajectories/trajectory-se3.cpp
-  src/trajectories/trajectory-euclidian.cpp
-  )
-
-SET(${PROJECT_NAME}_SOLVERS_SOURCES
-  src/solvers/solver-HQP-base.cpp
-  src/solvers/solver-HQP-factory.cpp
-  src/solvers/solver-HQP-eiquadprog.cpp
-  src/solvers/solver-HQP-eiquadprog-fast.cpp
-  src/solvers/solver-HQP-qpoases.cpp
-  src/solvers/utils.cpp
-  )
-
-SET(${PROJECT_NAME}_ROBOTS_SOURCES
-  src/robots/robot-wrapper.cpp
-  )
-
-SET(${PROJECT_NAME}_FORMULATIONS_SOURCES
-  src/formulations/contact-level.cpp
-  src/formulations/inverse-dynamics-formulation-base.cpp
-  src/formulations/inverse-dynamics-formulation-acc-force.cpp
-  )
-
-SET(${PROJECT_NAME}_SOURCES
-  src/utils/statistics.cpp
-  src/utils/stop-watch.cpp
-  ${${PROJECT_NAME}_MATH_SOURCES}
-  ${${PROJECT_NAME}_TASKS_SOURCES}
-  ${${PROJECT_NAME}_CONTACTS_SOURCES}
-  ${${PROJECT_NAME}_TRAJECTORIES_SOURCES}
-  ${${PROJECT_NAME}_SOLVERS_SOURCES}
-  ${${PROJECT_NAME}_ROBOTS_SOURCES}
-  ${${PROJECT_NAME}_FORMULATIONS_SOURCES}
-  )
-
-ADD_HEADER_GROUP(${PROJECT_NAME}_HEADERS)
-ADD_SOURCE_GROUP(${PROJECT_NAME}_SOURCES)
-
-ADD_LIBRARY(${PROJECT_NAME} SHARED
-  ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})
-TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} PUBLIC $<INSTALL_INTERFACE:include>)
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} PUBLIC
-  pinocchio::pinocchio eiquadprog::eiquadprog)
-
-
-IF(SUFFIX_SO_VERSION)
-  SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES SOVERSION ${PROJECT_VERSION})
-ENDIF(SUFFIX_SO_VERSION)
-
-IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
-  INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib)
-ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
-
-ADD_SUBDIRECTORY(bindings)
-IF(BUILD_TESTING)
-  ADD_SUBDIRECTORY(tests)
-ENDIF(BUILD_TESTING)
+  include/tsid/bindings/python/utils/*.hpp)
+
+set(${PROJECT_NAME}_HEADERS
+    include/tsid/macros.hpp
+    include/tsid/utils/statistics.hpp
+    include/tsid/utils/stop-watch.hpp
+    include/tsid/utils/Stdafx.hh
+    ${${PROJECT_NAME}_MATH_HEADERS}
+    ${${PROJECT_NAME}_TASKS_HEADERS}
+    ${${PROJECT_NAME}_CONTACTS_HEADERS}
+    ${${PROJECT_NAME}_TRAJECTORIES_HEADERS}
+    ${${PROJECT_NAME}_SOLVERS_HEADERS}
+    ${${PROJECT_NAME}_ROBOTS_HEADERS}
+    ${${PROJECT_NAME}_FORMULATIONS_HEADERS})
+
+list(REMOVE_DUPLICATES ${PROJECT_NAME}_HEADERS)
+
+set(${PROJECT_NAME}_MATH_SOURCES
+    src/math/constraint-base.cpp src/math/constraint-equality.cpp
+    src/math/constraint-inequality.cpp src/math/constraint-bound.cpp
+    src/math/utils.cpp)
+
+set(${PROJECT_NAME}_TASKS_SOURCES
+    src/tasks/task-base.cpp
+    src/tasks/task-actuation-bounds.cpp
+    src/tasks/task-actuation-equality.cpp
+    src/tasks/task-actuation.cpp
+    src/tasks/task-com-equality.cpp
+    src/tasks/task-contact-force-equality.cpp
+    src/tasks/task-contact-force.cpp
+    src/tasks/task-cop-equality.cpp
+    src/tasks/task-joint-bounds.cpp
+    src/tasks/task-joint-posture.cpp
+    src/tasks/task-joint-posVelAcc-bounds.cpp
+    src/tasks/task-capture-point-inequality.cpp
+    src/tasks/task-motion.cpp
+    src/tasks/task-se3-equality.cpp
+    src/tasks/task-angular-momentum-equality.cpp)
+
+set(${PROJECT_NAME}_CONTACTS_SOURCES
+    src/contacts/contact-base.cpp src/contacts/contact-6d.cpp
+    src/contacts/contact-point.cpp)
+
+set(${PROJECT_NAME}_TRAJECTORIES_SOURCES
+    src/trajectories/trajectory-se3.cpp
+    src/trajectories/trajectory-euclidian.cpp)
+
+set(${PROJECT_NAME}_SOLVERS_SOURCES
+    src/solvers/solver-HQP-base.cpp
+    src/solvers/solver-HQP-factory.cpp
+    src/solvers/solver-HQP-eiquadprog.cpp
+    src/solvers/solver-HQP-eiquadprog-fast.cpp
+    src/solvers/solver-HQP-qpoases.cpp
+    src/solvers/utils.cpp)
+
+if(qpmad_FOUND)
+  list(APPEND ${PROJECT_NAME}_SOLVERS_SOURCES src/solvers/solver-HQP-qpmad.cpp)
+endif()
+
+set(${PROJECT_NAME}_ROBOTS_SOURCES src/robots/robot-wrapper.cpp)
+
+set(${PROJECT_NAME}_FORMULATIONS_SOURCES
+    src/formulations/contact-level.cpp
+    src/formulations/inverse-dynamics-formulation-base.cpp
+    src/formulations/inverse-dynamics-formulation-acc-force.cpp)
+
+set(${PROJECT_NAME}_SOURCES
+    src/utils/statistics.cpp
+    src/utils/stop-watch.cpp
+    ${${PROJECT_NAME}_MATH_SOURCES}
+    ${${PROJECT_NAME}_TASKS_SOURCES}
+    ${${PROJECT_NAME}_CONTACTS_SOURCES}
+    ${${PROJECT_NAME}_TRAJECTORIES_SOURCES}
+    ${${PROJECT_NAME}_SOLVERS_SOURCES}
+    ${${PROJECT_NAME}_ROBOTS_SOURCES}
+    ${${PROJECT_NAME}_FORMULATIONS_SOURCES})
+
+add_header_group(${PROJECT_NAME}_HEADERS)
+add_source_group(${PROJECT_NAME}_SOURCES)
+
+add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES}
+                                   ${${PROJECT_NAME}_HEADERS})
+target_include_directories(${PROJECT_NAME} PUBLIC $<INSTALL_INTERFACE:include>)
+target_link_libraries(${PROJECT_NAME} PUBLIC pinocchio::pinocchio
+                                             eiquadprog::eiquadprog)
+
+if(qpmad_FOUND)
+  target_include_directories(${PROJECT_NAME} PUBLIC ${qpmad_INCLUDE_DIRS})
+  target_compile_definitions(${PROJECT_NAME} PUBLIC -DTSID_QPMAD_FOUND)
+endif()
+
+if(SUFFIX_SO_VERSION)
+  set_target_properties(${PROJECT_NAME} PROPERTIES SOVERSION ${PROJECT_VERSION})
+endif(SUFFIX_SO_VERSION)
+
+if(NOT INSTALL_PYTHON_INTERFACE_ONLY)
+  install(
+    TARGETS ${PROJECT_NAME}
+    EXPORT ${TARGETS_EXPORT_NAME}
+    DESTINATION lib)
+endif(NOT INSTALL_PYTHON_INTERFACE_ONLY)
+
+add_subdirectory(bindings)
+if(BUILD_TESTING)
+  add_subdirectory(tests)
+endif(BUILD_TESTING)
 
 # --- PACKAGING ----------------------------------------------------------------
-IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
-  INSTALL(FILES package.xml DESTINATION share/${PROJECT_NAME})
-ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
+if(NOT INSTALL_PYTHON_INTERFACE_ONLY)
+  install(FILES package.xml DESTINATION share/${PROJECT_NAME})
+endif(NOT INSTALL_PYTHON_INTERFACE_ONLY)
diff --git a/bindings/CMakeLists.txt b/bindings/CMakeLists.txt
index a3384883..f39df2f2 100644
--- a/bindings/CMakeLists.txt
+++ b/bindings/CMakeLists.txt
@@ -1,19 +1,16 @@
 #
 # Copyright (c) 2017-2018 CNRS
 #
-# This file is part of tsid
-# tsid is free software: you can redistribute it
-# and/or modify it under the terms of the GNU Lesser General Public
-# License as published by the Free Software Foundation, either version
-# 3 of the License, or (at your option) any later version.
-# tsid is distributed in the hope that it will be
-# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
-# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
-# General Lesser Public License for more details. You should have
-# received a copy of the GNU Lesser General Public License along with
-# tsid If not, see
+# This file is part of tsid tsid is free software: you can redistribute it
+# and/or modify it under the terms of the GNU Lesser General Public License as
+# published by the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version. tsid is distributed in the hope that it
+# will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+# Lesser Public License for more details. You should have received a copy of the
+# GNU Lesser General Public License along with tsid If not, see
 # <http://www.gnu.org/licenses/>.
 
-IF(BUILD_PYTHON_INTERFACE)
-  ADD_SUBDIRECTORY(python)
-ENDIF(BUILD_PYTHON_INTERFACE)
+if(BUILD_PYTHON_INTERFACE)
+  add_subdirectory(python)
+endif(BUILD_PYTHON_INTERFACE)
diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt
index 005935d6..a699d403 100644
--- a/bindings/python/CMakeLists.txt
+++ b/bindings/python/CMakeLists.txt
@@ -1,21 +1,20 @@
 #
 # Copyright (c) 2017-2021 CNRS
 #
-# This file is part of tsid
-# tsid is free software: you can redistribute it
-# and/or modify it under the terms of the GNU Lesser General Public
-# License as published by the Free Software Foundation, either version
-# 3 of the License, or (at your option) any later version.
-# tsid is distributed in the hope that it will be
-# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
-# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
-# General Lesser Public License for more details. You should have
-# received a copy of the GNU Lesser General Public License along with
-# tsid If not, see
+# This file is part of tsid tsid is free software: you can redistribute it
+# and/or modify it under the terms of the GNU Lesser General Public License as
+# published by the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version. tsid is distributed in the hope that it
+# will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+# Lesser Public License for more details. You should have received a copy of the
+# GNU Lesser General Public License along with tsid If not, see
 # <http://www.gnu.org/licenses/>.
 
 # --- LIBRARY --- #
-FILE(GLOB ${PYWRAP}_SOURCES
+file(
+  GLOB
+  ${PYWRAP}_SOURCES
   *.cpp
   constraint/*.cpp
   contacts/*.cpp
@@ -24,36 +23,36 @@ FILE(GLOB ${PYWRAP}_SOURCES
   solvers/*.cpp
   tasks/*.cpp
   trajectories/*.cpp
-  utils/*.cpp
-  )
+  utils/*.cpp)
 
-ADD_LIBRARY(${PYWRAP} SHARED ${${PYWRAP}_SOURCES} ${${PYWRAP}_HEADERS})
-TARGET_LINK_LIBRARIES(${PYWRAP} PUBLIC ${PROJECT_NAME} eigenpy::eigenpy)
-TARGET_LINK_BOOST_PYTHON(${PYWRAP} PRIVATE)
+add_library(${PYWRAP} SHARED ${${PYWRAP}_SOURCES} ${${PYWRAP}_HEADERS})
+target_link_libraries(${PYWRAP} PUBLIC ${PROJECT_NAME} eigenpy::eigenpy)
 
-IF(BUILD_WITH_COMMIT_VERSION)
-  TAG_LIBRARY_VERSION(${PYWRAP})
-ENDIF(BUILD_WITH_COMMIT_VERSION)
-ADD_HEADER_GROUP(${PYWRAP}_HEADERS)
-ADD_SOURCE_GROUP(${PYWRAP}_SOURCES)
+if(BUILD_WITH_COMMIT_VERSION)
+  tag_library_version(${PYWRAP})
+endif(BUILD_WITH_COMMIT_VERSION)
+add_header_group(${PYWRAP}_HEADERS)
+add_source_group(${PYWRAP}_SOURCES)
 
-IF(APPLE)
+if(APPLE)
   # We need to change the extension for python bindings
-  SET_TARGET_PROPERTIES(${PYWRAP} PROPERTIES SUFFIX ".so")
-ENDIF(APPLE)
+  set_target_properties(${PYWRAP} PROPERTIES SUFFIX ".so")
+endif(APPLE)
 
-SET_TARGET_PROPERTIES(${PYWRAP} PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${PROJECT_NAME})
+set_target_properties(${PYWRAP} PROPERTIES LIBRARY_OUTPUT_DIRECTORY
+                                           ${PROJECT_NAME})
 
-IF(UNIX AND NOT APPLE)
-  SET_TARGET_PROPERTIES(${PYWRAP} PROPERTIES INSTALL_RPATH "\$ORIGIN/../../..")
-ENDIF()
+if(UNIX AND NOT APPLE)
+  set_target_properties(${PYWRAP} PROPERTIES INSTALL_RPATH "\$ORIGIN/../../..")
+endif()
 
-INSTALL(TARGETS ${PYWRAP} EXPORT ${TARGETS_EXPORT_NAME} DESTINATION ${PYTHON_SITELIB}/${PROJECT_NAME})
+install(
+  TARGETS ${PYWRAP}
+  EXPORT ${TARGETS_EXPORT_NAME}
+  DESTINATION ${PYTHON_SITELIB}/${PROJECT_NAME})
 
-SET(PYTHON_FILES
-  __init__.py
-  )
+set(PYTHON_FILES __init__.py)
 
-FOREACH(python ${PYTHON_FILES})
-  PYTHON_INSTALL_ON_SITE(${PROJECT_NAME} ${python})
-ENDFOREACH(python)
+foreach(python ${PYTHON_FILES})
+  python_install_on_site(${PROJECT_NAME} ${python})
+endforeach(python)
diff --git a/cmake b/cmake
index fb9ee969..a558ff94 160000
--- a/cmake
+++ b/cmake
@@ -1,1 +1,1 @@
-Subproject commit fb9ee969c8119a24fd280e2fcd7f81c31704e8f2
+Subproject commit a558ff948ce65384b3ccd9e5cd9a295a2c0016bc
diff --git a/demo/demo_romeo.py b/demo/demo_romeo.py
deleted file mode 100644
index 4c5cfe85..00000000
--- a/demo/demo_romeo.py
+++ /dev/null
@@ -1,180 +0,0 @@
-import pinocchio as se3
-from pinocchio import libpinocchio_pywrap as pin 
-import tsid
-import numpy as np
-from numpy.linalg import norm as norm
-import os
-
-import gepetto.corbaserver
-import time
-import subprocess
-
-np.set_printoptions(precision=3, linewidth=200)
-
-print("".center(100,'#'))
-print(" Test Task Space Inverse Dynamics ".center(100, '#'))
-print("".center(100,'#'), '\n')
-
-lxp = 0.14                          # foot length in positive x direction
-lxn = 0.077                         # foot length in negative x direction
-lyp = 0.069                         # foot length in positive y direction
-lyn = 0.069                         # foot length in negative y direction
-lz = 0.105                          # foot sole height with respect to ankle joint
-mu = 0.3                            # friction coefficient
-fMin = 5.0                          # minimum normal force
-fMax = 1000.0                       # maximum normal force
-rf_frame_name = "RAnkleRoll"        # right foot frame name
-lf_frame_name = "LAnkleRoll"        # left foot frame name
-contactNormal = np.array([0., 0., 1.])   # direction of the normal to the contact surface
-w_com = 1.0                     # weight of center of mass task
-w_posture = 1e-3                # weight of joint posture task
-w_forceReg = 1e-5               # weight of force regularization task
-w_RF = 1.0                      # weight of right foot motion task
-kp_contact = 30.0               # proportional gain of contact constraint
-kp_com = 30.0                   # proportional gain of center of mass task
-kp_posture = 30.0               # proportional gain of joint posture task
-kp_RF = 30.0                    # proportional gain of right foot motion task
-REMOVE_CONTACT_N = 100          # remove right foot contact constraint after REMOVE_CONTACT_N time steps
-CONTACT_TRANSITION_TIME = 1.0   # duration of the contact transition (to smoothly get a zero contact force before removing a contact constraint)
-DELTA_COM_Y = 0.1               # distance between initial and desired center of mass position in y direction
-DELTA_FOOT_Z = 0.1              # desired elevation of right foot in z direction
-dt = 0.001                      # controller time step
-PRINT_N = 500                   # print every PRINT_N time steps
-DISPLAY_N = 25                  # update robot configuration in viwewer every DISPLAY_N time steps
-N_SIMULATION = 4000             # number of time steps simulated
-
-filename = str(os.path.dirname(os.path.abspath(__file__)))
-path = filename + '/../models/romeo'
-urdf = path + '/urdf/romeo.urdf'
-vector = se3.StdVec_StdString()
-vector.extend(item for item in path)
-robot = tsid.RobotWrapper(urdf, vector, se3.JointModelFreeFlyer(), False)
-srdf = path + '/srdf/romeo_collision.srdf'
-
-# for gepetto viewer .. but Fix me!!
-robot_display = se3.RobotWrapper.BuildFromURDF(urdf, [path, ], se3.JointModelFreeFlyer())
-
-l = subprocess.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
-if int(l[1]) == 0:
-    os.system('gepetto-gui &')
-time.sleep(1)
-cl = gepetto.corbaserver.Client()
-gui = cl.gui
-robot_display.initViewer(loadModel=True)
-
-model = robot.model()
-pin.loadReferenceConfigurations(model, srdf, False)
-q = model.referenceConfigurations["half_sitting"]
-q[2] += 0.84
-v = np.array(np.zeros(robot.nv))
-
-robot_display.displayCollisions(False)
-robot_display.displayVisuals(True)
-robot_display.display(q)
-
-assert robot.model().existFrame(rf_frame_name)
-assert robot.model().existFrame(lf_frame_name)
-
-t = 0.0                         # time
-invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
-invdyn.computeProblemData(t, q, v)
-data = invdyn.data()
-contact_Point = np.array(np.ones((3,4)) * lz)
-contact_Point[0, :] = [-lxn, -lxn, lxp, lxp]
-contact_Point[1, :] = [-lyn, lyp, -lyn, lyp]
-
-contactRF =tsid.Contact6d("contact_rfoot", robot, rf_frame_name, contact_Point, contactNormal, mu, fMin, fMax)
-contactRF.setKp(kp_contact * np.array(np.ones(6)))
-contactRF.setKd(2.0 * np.sqrt(kp_contact) * np.array(np.ones(6)))
-H_rf_ref = robot.position(data, robot.model().getFrameId(rf_frame_name))
-contactRF.setReference(H_rf_ref)
-invdyn.addRigidContact(contactRF, w_forceReg)
-
-contactLF =tsid.Contact6d("contact_lfoot", robot, lf_frame_name, contact_Point, contactNormal, mu, fMin, fMax)
-contactLF.setKp(kp_contact * np.array(np.ones(6)))
-contactLF.setKd(2.0 * np.sqrt(kp_contact) * np.array(np.ones(6)))
-H_lf_ref = robot.position(data, robot.model().getFrameId(lf_frame_name))
-contactLF.setReference(H_lf_ref)
-invdyn.addRigidContact(contactLF, w_forceReg)
-
-comTask = tsid.TaskComEquality("task-com", robot)
-comTask.setKp(kp_com * np.array(np.ones(3)))
-comTask.setKd(2.0 * np.sqrt(kp_com) * np.array(np.ones(3)))
-invdyn.addMotionTask(comTask, w_com, 1, 0.0)
-
-postureTask = tsid.TaskJointPosture("task-posture", robot)
-postureTask.setKp(kp_posture * np.array(np.ones(robot.nv-6)))
-postureTask.setKd(2.0 * np.sqrt(kp_posture) * np.array(np.ones(robot.nv-6)))
-invdyn.addMotionTask(postureTask, w_posture, 1, 0.0)
-
-rightFootTask = tsid.TaskSE3Equality("task-right-foot", robot, rf_frame_name)
-rightFootTask.setKp(kp_RF * np.array(np.ones(6)))
-rightFootTask.setKd(2.0 * np.sqrt(kp_com) * np.array(np.ones(6)))
-invdyn.addMotionTask(rightFootTask, w_RF, 1, 0.0)
-
-H_rf_ref.translation += np.array([0., 0., DELTA_FOOT_Z])
-rightFootTraj = tsid.TrajectorySE3Constant("traj-right-foot", H_rf_ref)
-
-com_ref = robot.com(data)
-com_ref[1] += DELTA_COM_Y
-trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref)
-
-q_ref = q[7:]
-trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref)
-
-solver = tsid.SolverHQuadProg("qp solver")
-solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn)
-
-for i in range(0, N_SIMULATION):
-    time_start = time.time()
-    
-    if i == REMOVE_CONTACT_N:
-        print("\nTime %.3f Start breaking contact %s\n"%(t, contactRF.name))
-        invdyn.removeRigidContact(contactRF.name, CONTACT_TRANSITION_TIME)
-
-    sampleCom = trajCom.computeNext()
-    comTask.setReference(sampleCom)
-    samplePosture = trajPosture.computeNext()
-    postureTask.setReference(samplePosture)
-    sampleRightFoot = rightFootTraj.computeNext()
-    rightFootTask.setReference(sampleRightFoot)
-
-    HQPData = invdyn.computeProblemData(t, q, v)
-    if i == 0:
-        HQPData.print_all()
-
-    sol = solver.solve(HQPData)
-    tau = invdyn.getActuatorForces(sol)
-    dv = invdyn.getAccelerations(sol)
-
-    if i%PRINT_N == 0:
-        print("Time %.3f"%(t))
-        if invdyn.checkContact(contactRF.name, sol):
-            f = invdyn.getContactForce(contactRF.name, sol)
-            print("\tnormal force %s: %.1f"%(contactRF.name.ljust(20,'.'),contactRF.getNormalForce(f)))
-
-        if invdyn.checkContact(contactLF.name, sol):
-            f = invdyn.getContactForce(contactLF.name, sol)
-            print("\tnormal force %s: %.1f"%(contactLF.name.ljust(20,'.'),contactLF.getNormalForce(f)))
-
-        print("\ttracking err %s: %.3f"%(comTask.name.ljust(20,'.'),       norm(comTask.position_error, 2)))
-        print("\ttracking err %s: %.3f"%(rightFootTask.name.ljust(20,'.'), norm(rightFootTask.position_error, 2)))
-        print("\t||v||: %.3f\t ||dv||: %.3f"%(norm(v, 2), norm(dv)))
-
-    v_mean = v + 0.5*dt*dv
-    v += dt*dv
-    q = se3.integrate(robot.model(), q, dt*v_mean)
-    t += dt
-    
-    if i%DISPLAY_N == 0:
-        robot_display.display(q)
-
-    time_spent = time.time() - time_start
-    if(time_spent < dt):
-        time.sleep(dt-time_spent)
-        
-    assert norm(dv) < 1e6
-    assert norm(v) < 1e6
-
-print("\nFinal COM Position  ", robot.com(invdyn.data()).T)
-print("Desired COM Position", com_ref.T)
diff --git a/include/tsid/bindings/python/robots/robot-wrapper.hpp b/include/tsid/bindings/python/robots/robot-wrapper.hpp
index f386df91..4d01fbed 100644
--- a/include/tsid/bindings/python/robots/robot-wrapper.hpp
+++ b/include/tsid/bindings/python/robots/robot-wrapper.hpp
@@ -41,12 +41,15 @@ namespace tsid
       {
         cl
         .def(bp::init<std::string, std_vec, bool>((bp::arg("filename"), bp::arg("package_dir"), bp::arg("verbose")), "Default constructor without RootJoint."))
-        .def(bp::init<pinocchio::Model, bool>((bp::arg("Pinocchio Model"), bp::arg("verbose")), "Default constructor from pinocchio model"))
-        .def(bp::init<std::string, std_vec, pinocchio::JointModelVariant &, bool>((bp::arg("filename"), bp::arg("package_dir"), bp::arg("roottype"), bp::arg("verbose")), "Default constructor without RootJoint."))
+        .def(bp::init<std::string, std_vec, pinocchio::JointModelVariant &, bool>((bp::arg("filename"), bp::arg("package_dir"), bp::arg("roottype"), bp::arg("verbose")), "Default constructor with RootJoint."))
+        .def(bp::init<pinocchio::Model, bool>((bp::arg("Pinocchio Model"), bp::arg("verbose")), "Default constructor from pinocchio model without RootJoint."))
+        .def(bp::init<pinocchio::Model, robots::RobotWrapper::RootJointType, bool>((bp::arg("Pinocchio Model"), bp::arg("rootJoint"), bp::arg("verbose")), "Default constructor from pinocchio model with RootJoint."))
         .def("__init__",bp::make_constructor(RobotPythonVisitor<Robot> ::makeClass))
         .add_property("nq", &Robot::nq)
         .add_property("nv", &Robot::nv)
         .add_property("na", &Robot::na)
+        .add_property("nq_actuated", &Robot::nq_actuated)
+        .add_property("is_fixed_base", &Robot::is_fixed_base)
 
         .def("model", &RobotPythonVisitor::model)
         .def("data", &RobotPythonVisitor::data)
@@ -179,6 +182,11 @@ namespace tsid
                           bp::no_init)
         .def(RobotPythonVisitor<Robot>());
         ;
+        bp::enum_<robots::RobotWrapper::RootJointType>("RootJointType")
+            .value("FIXED_BASE_SYSTEM", robots::RobotWrapper::FIXED_BASE_SYSTEM)
+            .value("FLOATING_BASE_SYSTEM", robots::RobotWrapper::FLOATING_BASE_SYSTEM)
+            .export_values()
+            ;
       }
     };
   }
diff --git a/include/tsid/config.hpp b/include/tsid/config.hpp
deleted file mode 100644
index aa40a182..00000000
--- a/include/tsid/config.hpp
+++ /dev/null
@@ -1,69 +0,0 @@
-//
-// Copyright (c) 2017 CNRS
-//
-// This file is part of tsid
-// tsid is free software: you can redistribute it
-// and/or modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation, either version
-// 3 of the License, or (at your option) any later version.
-// tsid is distributed in the hope that it will be
-// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
-// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
-// General Lesser Public License for more details. You should have
-// received a copy of the GNU Lesser General Public License along with
-// tsid If not, see
-// <http://www.gnu.org/licenses/>.
-//
-
-#ifndef _invdyn_lib_config_hpp
-#define _invdyn_lib_config_hpp
-
-// Package version (header).
-# define TSID_VERSION "UNKNOWN"
-
-// Handle portable symbol export.
-// Defining manually which symbol should be exported is required
-// under Windows whether MinGW or MSVC is used.
-//
-// The headers then have to be able to work in two different modes:
-// - dllexport when one is building the library,
-// - dllimport for clients using the library.
-//
-// On Linux, set the visibility accordingly. If C++ symbol visibility
-// is handled by the compiler, see: http://gcc.gnu.org/wiki/Visibility
-# if defined _WIN32 || defined __CYGWIN__
-// On Microsoft Windows, use dllimport and dllexport to tag symbols.
-#  define TSID_DLLIMPORT __declspec(dllimport)
-#  define TSID_DLLEXPORT __declspec(dllexport)
-#  define TSID_DLLLOCAL
-# else
-// On Linux, for GCC >= 4, tag symbols using GCC extension.
-#  if __GNUC__ >= 4
-#   define TSID_DLLIMPORT __attribute__ ((visibility("default")))
-#   define TSID_DLLEXPORT __attribute__ ((visibility("default")))
-#   define TSID_DLLLOCAL  __attribute__ ((visibility("hidden")))
-#  else
-// Otherwise (GCC < 4 or another compiler is used), export everything.
-#   define TSID_DLLIMPORT
-#   define TSID_DLLEXPORT
-#   define TSID_DLLLOCAL
-#  endif // __GNUC__ >= 4
-# endif // defined _WIN32 || defined __CYGWIN__
-
-# ifdef TSID_STATIC
-// If one is using the library statically, get rid of
-// extra information.
-#  define TSID_DLLAPI
-#  define TSID_LOCAL
-# else
-// Depending on whether one is building or using the
-// library define DLLAPI to import or export.
-#  ifdef TSID_EXPORTS
-#   define TSID_DLLAPI TSID_DLLEXPORT
-#  else
-#   define TSID_DLLAPI TSID_DLLIMPORT
-#  endif // TSID_EXPORTS
-#  define TSID_LOCAL TSID_DLLLOCAL
-# endif // TSID_STATIC
-
-#endif //_invdyn_lib_config_hpp
diff --git a/include/tsid/contacts/contact-6d.hpp b/include/tsid/contacts/contact-6d.hpp
index 06b238d5..45685475 100644
--- a/include/tsid/contacts/contact-6d.hpp
+++ b/include/tsid/contacts/contact-6d.hpp
@@ -18,7 +18,7 @@
 #ifndef __invdyn_contact_6d_hpp__
 #define __invdyn_contact_6d_hpp__
 
-#include "tsid/deprecation.hpp"
+#include "tsid/deprecated.hh"
 #include "tsid/contacts/contact-base.hpp"
 #include "tsid/tasks/task-se3-equality.hpp"
 #include "tsid/math/constraint-inequality.hpp"
@@ -53,7 +53,7 @@ namespace tsid
                 const double minNormalForce,
                 const double maxNormalForce);
 
-      DEPRECATED Contact6d(const std::string & name,
+      TSID_DEPRECATED Contact6d(const std::string & name,
                 RobotWrapper & robot,
                 const std::string & frameName,
                 ConstRefMatrix contactPoints,
diff --git a/include/tsid/deprecation.hpp b/include/tsid/deprecation.hpp
deleted file mode 100644
index 684658c8..00000000
--- a/include/tsid/deprecation.hpp
+++ /dev/null
@@ -1,30 +0,0 @@
-//
-// Copyright (c) 2019 University of Trento
-//
-// This file is part of tsid
-// tsid is free software: you can redistribute it
-// and/or modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation, either version
-// 3 of the License, or (at your option) any later version.
-// tsid is distributed in the hope that it will be
-// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
-// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
-// General Lesser Public License for more details. You should have
-// received a copy of the GNU Lesser General Public License along with
-// tsid If not, see
-// <http://www.gnu.org/licenses/>.
-//
-
-#ifndef _tsid_lib_deprecation_hpp
-#define _tsid_lib_deprecation_hpp
-
-#if defined(__GNUC__) || defined(__clang__)
-#define DEPRECATED __attribute__((deprecated))
-#elif defined(_MSC_VER)
-#define DEPRECATED __declspec(deprecated)
-#else
-#pragma message("WARNING: You need to implement DEPRECATED for this compiler")
-#define DEPRECATED
-#endif
-
-#endif //_tsid_lib_deprecation_hpp
diff --git a/include/tsid/formulations/inverse-dynamics-formulation-acc-force.hpp b/include/tsid/formulations/inverse-dynamics-formulation-acc-force.hpp
index 92b34164..4b73b299 100644
--- a/include/tsid/formulations/inverse-dynamics-formulation-acc-force.hpp
+++ b/include/tsid/formulations/inverse-dynamics-formulation-acc-force.hpp
@@ -87,7 +87,7 @@ namespace tsid
     bool addRigidContact(ContactBase & contact, double force_regularization_weight,
                          double motion_weight=1.0, unsigned int motion_priority_level=0);
 
-    DEPRECATED bool addRigidContact(ContactBase & contact);
+    TSID_DEPRECATED bool addRigidContact(ContactBase & contact);
 
     bool updateRigidContactWeights(const std::string & contact_name,
                                    double force_regularization_weight,
diff --git a/include/tsid/formulations/inverse-dynamics-formulation-base.hpp b/include/tsid/formulations/inverse-dynamics-formulation-base.hpp
index 12bc4c0b..605772f1 100644
--- a/include/tsid/formulations/inverse-dynamics-formulation-base.hpp
+++ b/include/tsid/formulations/inverse-dynamics-formulation-base.hpp
@@ -18,7 +18,7 @@
 #ifndef __invdyn_inverse_dynamics_formulation_base_hpp__
 #define __invdyn_inverse_dynamics_formulation_base_hpp__
 
-#include "tsid/deprecation.hpp"
+#include "tsid/deprecated.hh"
 #include "tsid/math/fwd.hpp"
 #include "tsid/robots/robot-wrapper.hpp"
 #include "tsid/tasks/task-actuation.hpp"
@@ -118,7 +118,7 @@ namespace tsid
     virtual bool addRigidContact(ContactBase & contact, double force_regularization_weight,
                                  double motion_weight=1.0, unsigned int motion_priority_level=0) = 0;
 
-    DEPRECATED virtual bool addRigidContact(ContactBase & contact);
+    TSID_DEPRECATED virtual bool addRigidContact(ContactBase & contact);
 
     /**
      * @brief Update the weights associated to the specified contact
diff --git a/include/tsid/robots/robot-wrapper.hpp b/include/tsid/robots/robot-wrapper.hpp
index e22aac5f..f29a657a 100644
--- a/include/tsid/robots/robot-wrapper.hpp
+++ b/include/tsid/robots/robot-wrapper.hpp
@@ -18,6 +18,7 @@
 #ifndef __invdyn_robot_wrapper_hpp__
 #define __invdyn_robot_wrapper_hpp__
 
+#include "tsid/deprecated.hh"
 #include "tsid/math/fwd.hpp"
 #include "tsid/robots/fwd.hpp"
 
@@ -54,22 +55,35 @@ namespace tsid
       typedef math::Matrix3x Matrix3x;
       typedef math::RefVector RefVector;
       typedef math::ConstRefVector ConstRefVector;
-      
-      
-      RobotWrapper(const std::string & filename,
-                   const std::vector<std::string> & package_dirs,
-                   bool verbose=false);
 
-      RobotWrapper(const Model & m, bool verbose=false);
+      /* Possible root joints */
+      typedef enum e_RootJointType
+      {
+        FIXED_BASE_SYSTEM=0,
+        FLOATING_BASE_SYSTEM=1,
+      } RootJointType;
 
       RobotWrapper(const std::string & filename,
-                   const std::vector<std::string> & package_dirs,
-                   const pinocchio::JointModelVariant & rootJoint,
+                                   const std::vector<std::string> & package_dirs,
+                                   bool verbose=false);
+
+      RobotWrapper(const std::string & filename,
+                                   const std::vector<std::string> & package_dirs,
+                                   const pinocchio::JointModelVariant & rootJoint,
+                                   bool verbose=false);
+
+      TSID_DEPRECATED RobotWrapper(const Model & m,
+                                   bool verbose=false);
+
+      RobotWrapper(const Model & m,
+                   RootJointType rootJoint,
                    bool verbose=false);
       
       virtual int nq() const;
+      virtual int nq_actuated() const;
       virtual int nv() const;
       virtual int na() const;
+      virtual bool is_fixed_base() const;
       
       ///
       /// \brief Accessor to model.
@@ -184,7 +198,9 @@ namespace tsid
       std::string m_model_filename;
       bool m_verbose;
       
+      int m_nq_actuated;     /// dimension of the configuration space of the actuated DoF (nq for fixed-based, nq-7 for floating-base robots)
       int m_na;     /// number of actuators (nv for fixed-based, nv-6 for floating-base robots)
+      bool m_is_fixed_base;
       Vector m_rotor_inertias;
       Vector m_gear_ratios;
       Vector m_Md;  /// diagonal part of inertia matrix due to rotor inertias
diff --git a/include/tsid/solvers/fwd.hpp b/include/tsid/solvers/fwd.hpp
index f48c56ec..052c77c0 100644
--- a/include/tsid/solvers/fwd.hpp
+++ b/include/tsid/solvers/fwd.hpp
@@ -20,7 +20,7 @@
 
 #include <memory>
 
-#include "tsid/config.hpp"
+#include "tsid/config.hh"
 #include "tsid/math/fwd.hpp"
 #include <pinocchio/container/aligned-vector.hpp>
 
@@ -39,8 +39,11 @@ namespace tsid
       SOLVER_HQP_EIQUADPROG = 0,
       SOLVER_HQP_EIQUADPROG_FAST = 1,
       SOLVER_HQP_EIQUADPROG_RT = 2
+#ifdef TSID_QPMAD_FOUND
+      ,SOLVER_HQP_QPMAD
+#endif
 #ifdef QPOASES_FOUND
-      ,SOLVER_HQP_OASES = 3
+      ,SOLVER_HQP_OASES
 #endif
     };
     
diff --git a/include/tsid/solvers/solver-HQP-qpmad.hpp b/include/tsid/solvers/solver-HQP-qpmad.hpp
new file mode 100644
index 00000000..b0fd3e71
--- /dev/null
+++ b/include/tsid/solvers/solver-HQP-qpmad.hpp
@@ -0,0 +1,84 @@
+//
+// Copyright (c) 2022 Inria
+//
+// This file is part of tsid
+// tsid is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+// tsid is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// tsid If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+#ifndef __invdyn_solvers_hqp_qpmad_hpp__
+#define __invdyn_solvers_hqp_qpmad_hpp__
+
+#include <tsid/solvers/solver-HQP-base.hpp>
+
+#include <qpmad/solver.h>
+
+namespace tsid
+{
+  namespace solvers
+  {
+    /**
+     * @brief Implementation of Quadratic Program (HQP) solver using qpmad.
+     */
+    class TSID_DLLAPI SolverHQpmad:
+        public SolverHQPBase
+    {
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+      
+      typedef math::Matrix Matrix;
+      typedef math::Vector Vector;
+      typedef math::RefVector RefVector;
+      typedef math::ConstRefVector ConstRefVector;
+      typedef math::ConstRefMatrix ConstRefMatrix;
+
+      typedef qpmad::SolverParameters Settings;
+
+      SolverHQpmad(const std::string & name);
+
+      void resize(unsigned int n, unsigned int neq, unsigned int nin);
+
+      /** Solve the given Hierarchical Quadratic Program
+       */
+      const HQPOutput & solve(const HQPData & problemData);
+
+      /** Get the objective value of the last solved problem. */
+      double getObjectiveValue();
+
+      Settings& settings() { return m_settings; }
+
+    protected:
+
+      void sendMsg(const std::string & s);
+
+      qpmad::Solver m_solver;
+      Settings m_settings;
+
+      bool m_has_bounds;
+      
+      Matrix m_H;   // hessian matrix
+      Vector m_g;   // gradient vector
+      Vector m_lb;  // gradient vector
+      Vector m_ub;  // gradient vector
+      Matrix m_C;   // constraint matrix
+      Vector m_cl;  // constraints lower bound
+      Vector m_cu;  // constraints upper bound
+
+      double m_hessian_regularization;
+
+      unsigned int m_nc;  /// number of equality-inequality constraints
+      unsigned int m_n;    /// number of variables
+    };
+  }
+}
+
+#endif // ifndef __invdyn_solvers_hqp_qpmad_hpp__
diff --git a/include/tsid/tasks/task-actuation-equality.hpp b/include/tsid/tasks/task-actuation-equality.hpp
index 4e1100f4..3e5291e5 100644
--- a/include/tsid/tasks/task-actuation-equality.hpp
+++ b/include/tsid/tasks/task-actuation-equality.hpp
@@ -1,5 +1,5 @@
 //
-// Copyright (c) 2017 CNRS
+// Copyright (c) 2021 CNRS INRIA LORIA
 //
 // This file is part of tsid
 // tsid is free software: you can redistribute it
@@ -15,15 +15,56 @@
 // <http://www.gnu.org/licenses/>.
 //
 
-#ifndef __invdyn_tasks_fwd_hpp__
-#define __invdyn_tasks_fwd_hpp__
+#ifndef __invdyn_task_actuation_equality_hpp__
+#define __invdyn_task_actuation_equality_hpp__
+
+#include <tsid/tasks/task-actuation.hpp>
+#include <tsid/math/constraint-equality.hpp>
 
 namespace tsid
 {
   namespace tasks
   {
-    
+    class TaskActuationEquality : public TaskActuation
+    {
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+      typedef math::Index Index;
+      typedef math::Vector Vector;
+      typedef math::VectorXi VectorXi;
+      typedef math::ConstraintEquality ConstraintEquality;
+      typedef pinocchio::Data Data;
+
+      TaskActuationEquality(const std::string & name,
+                            RobotWrapper & robot);
+
+      int dim() const;
+
+      const ConstraintBase & compute(const double t,
+                                     ConstRefVector q,
+                                     ConstRefVector v,
+                                     Data & data);
+
+      const ConstraintBase & getConstraint() const;
+
+      void setReference(math::ConstRefVector ref);
+      const Vector & getReference() const;
+
+      void setWeightVector(math::ConstRefVector weights);
+      const Vector & getWeightVector() const;
+
+      const Vector & mask() const;
+      void mask(const Vector & mask);
+
+    protected:
+      Vector m_mask;
+      VectorXi m_activeAxes;
+      Vector m_ref;
+      Vector m_weights;
+      ConstraintEquality m_constraint;
+    };
   }
 }
 
-#endif // ifndef __invdyn_tasks_fwd_hpp__
+#endif // ifndef __invdyn_task_actuation_equality_hpp__
diff --git a/include/tsid/tasks/task-joint-posVelAcc-bounds.hpp b/include/tsid/tasks/task-joint-posVelAcc-bounds.hpp
index 4f6aac60..9d17c557 100644
--- a/include/tsid/tasks/task-joint-posVelAcc-bounds.hpp
+++ b/include/tsid/tasks/task-joint-posVelAcc-bounds.hpp
@@ -21,7 +21,7 @@
 #include <tsid/tasks/task-motion.hpp>
 #include <tsid/math/constraint-bound.hpp>
 #include <tsid/math/constraint-inequality.hpp>
-#include <tsid/deprecation.hpp>
+#include <tsid/deprecated.hh>
 
 /** This class has been implemented following :
 * Andrea del Prete. Joint Position and Velocity Bounds in Discrete-Time
@@ -107,8 +107,8 @@ namespace tsid
       */
       void computeAccLimits(ConstRefVector q,ConstRefVector dq,bool verbose=true);
 
-      DEPRECATED const Vector & mask() const; // deprecated
-      DEPRECATED void mask(const Vector & mask); // deprecated
+      TSID_DEPRECATED const Vector & mask() const; // deprecated
+      TSID_DEPRECATED void mask(const Vector & mask); // deprecated
       virtual void setMask(math::ConstRefVector mask);
 
 
diff --git a/include/tsid/tasks/task-joint-posture.hpp b/include/tsid/tasks/task-joint-posture.hpp
index e39c26bd..b60016f5 100644
--- a/include/tsid/tasks/task-joint-posture.hpp
+++ b/include/tsid/tasks/task-joint-posture.hpp
@@ -21,7 +21,7 @@
 #include <tsid/tasks/task-motion.hpp>
 #include <tsid/trajectories/trajectory-base.hpp>
 #include <tsid/math/constraint-equality.hpp>
-#include <tsid/deprecation.hpp>
+#include <tsid/deprecated.hh>
 
 namespace tsid
 {
@@ -58,8 +58,8 @@ namespace tsid
       const Vector & getDesiredAcceleration() const;
       Vector getAcceleration(ConstRefVector dv) const;
 
-      DEPRECATED const Vector & mask() const; // deprecated
-      DEPRECATED void mask(const Vector & mask); // deprecated
+      TSID_DEPRECATED const Vector & mask() const; // deprecated
+      TSID_DEPRECATED void mask(const Vector & mask); // deprecated
       virtual void setMask(math::ConstRefVector mask);
 
       const Vector & position_error() const;
@@ -82,6 +82,7 @@ namespace tsid
       Vector m_a_des;
       VectorXi m_activeAxes;
       TrajectorySample m_ref;
+      Vector m_ref_q_augmented;
       ConstraintEquality m_constraint;
     };
     
diff --git a/include/tsid/trajectories/trajectory-base.hpp b/include/tsid/trajectories/trajectory-base.hpp
index 98e009b3..18151c02 100644
--- a/include/tsid/trajectories/trajectory-base.hpp
+++ b/include/tsid/trajectories/trajectory-base.hpp
@@ -18,7 +18,7 @@
 #ifndef __invdyn_trajectory_base_hpp__
 #define __invdyn_trajectory_base_hpp__
 
-#include "tsid/deprecation.hpp"
+#include "tsid/deprecated.hh"
 #include "tsid/macros.hpp"
 #include "tsid/math/fwd.hpp"
 #include "tsid/math/utils.hpp"
@@ -38,7 +38,7 @@ namespace tsid
       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
       // TODO rename pos, vel, acc → value, derivative, second_derivative
-      DEPRECATED math::Vector pos, vel, acc;
+      TSID_DEPRECATED math::Vector pos, vel, acc;
 
 TSID_DISABLE_WARNING_PUSH
 TSID_DISABLE_WARNING_DEPRECATED
diff --git a/models/arm/generator.py b/models/arm/generator.py
new file mode 100644
index 00000000..dce65e9e
--- /dev/null
+++ b/models/arm/generator.py
@@ -0,0 +1,249 @@
+import pinocchio as pin
+import hppfcl as fcl
+import numpy as np
+from math import pi
+
+def create_7dof_arm(revoluteOnly = False):
+    '''
+    Create a 7 DoF robot arm (with spherical joint for shoulder and wrist and revolute joint for elbow)
+
+    Optionnal parameters:
+        revoluteOnly (default=False): if True, the arm is created with only revolute joints. (Spherical joints are replaced by 3 revolute joints)
+
+    Return:
+        model: pinocchio model of the robot
+        geom_model: pinocchio geometric model of the robot
+    '''
+    # Some useful values
+    geom_radius = 0.1 # radius of the arm geometries
+    geom_length = 0.66 # length of the lower or upper arm geometries
+
+    def set_limit(model, joint_id, pos_absmax, vel_absmax, eff_absmax):
+        idx_q = model.joints[joint_id].idx_q
+        nq =  model.joints[joint_id].nq
+        for idx in range(idx_q, idx_q+nq):
+            model.upperPositionLimit[idx] = pos_absmax
+            model.lowerPositionLimit[idx] = -pos_absmax
+
+        idx_v = model.joints[joint_id].idx_v
+        nv =  model.joints[joint_id].nv
+        for idx in range(idx_v, idx_v+nv):
+            model.velocityLimit[idx] = vel_absmax
+            model.effortLimit[idx] = eff_absmax
+
+    # Create models
+    model = pin.Model()
+    geom_model = pin.GeometryModel()
+
+    # Base
+    id_base = 0
+
+    color_base = np.array([1.,0.1,0.1,1.])
+
+    geom_model.addGeometryObject(pin.GeometryObject(
+                                                    "base",
+                                                    id_base, # parent
+                                                    fcl.Box(0.5,0.5,0.02),
+                                                    pin.SE3(np.identity(3), np.array([0,0,-0.01-geom_radius])),
+                                                    "", np.ones(3), False, # Default values
+                                                    color_base
+                                                    ))
+
+    geom_model.addGeometryObject(pin.GeometryObject(
+                                                    "shoulderGeom",
+                                                    id_base,
+                                                    fcl.Sphere(geom_radius),
+                                                    pin.SE3.Identity(),
+                                                    "", np.ones(3), False, # Default values
+                                                    color_base
+                                                    ))
+
+
+    # Upper arm
+    placement_upper = pin.SE3(np.identity(3), np.array([0,0, geom_length/2.]))
+    if(not revoluteOnly):
+        id_upper = model.addJoint(
+                                id_base,
+                                pin.JointModelSpherical(),
+                                pin.SE3.Identity(),
+                                "shoulder"
+                                )
+        set_limit(model, id_upper, pi, 3.0, 100.)
+    else:
+        id_upper = model.addJoint(
+                                id_base,
+                                pin.JointModelRX(),
+                                pin.SE3.Identity(),
+                                "shoulderX"
+                                )
+        set_limit(model, id_upper, pi, 3.0, 100.)
+        id_upper = model.addJoint(
+                                id_upper,
+                                pin.JointModelRY(),
+                                pin.SE3.Identity(),
+                                "shoulderY"
+                                )
+        set_limit(model, id_upper, pi, 3.0, 100.)
+        id_upper = model.addJoint(
+                                id_upper,
+                                pin.JointModelRZ(),
+                                pin.SE3.Identity(),
+                                "shoulderZ"
+                                )
+        set_limit(model, id_upper, pi, 3.0, 100.)
+    model.appendBodyToJoint(
+                            id_upper,
+                            pin.Inertia.FromCylinder(1.0 , geom_radius, geom_length),
+                            placement_upper
+                            )
+
+    color_upper = np.array([0.1,0.8,0.1,.8])
+
+    geom_model.addGeometryObject(pin.GeometryObject(
+                                                    "elbowGeom",
+                                                    id_upper,
+                                                    fcl.Sphere(geom_radius),
+                                                    pin.SE3(np.identity(3), np.array([0,0, geom_length])),
+                                                    "", np.ones(3), False, # Default values
+                                                    color_upper
+                                                    ))
+
+    geom_model.addGeometryObject(pin.GeometryObject(
+                                                    "upperGeom",
+                                                    id_upper,
+                                                    fcl.Cylinder(geom_radius, geom_length),
+                                                    placement_upper,
+                                                    "", np.ones(3), False, # Default values
+                                                    color_upper
+                                                    ))
+
+    # Lower arm
+    placement_lower = pin.SE3(np.identity(3), np.array([0,0, geom_length/2.]))
+    id_lower = model.addJoint(
+                            id_upper,
+                            pin.JointModelRX(),
+                            pin.SE3(np.identity(3), np.array([0,0, geom_length])),
+                            "elbow"
+                            )
+    model.appendBodyToJoint(
+                            id_lower,
+                            pin.Inertia.FromCylinder(1.0 , geom_radius, geom_length),
+                            placement_lower
+                            )
+    set_limit(model, id_lower, pi, 3.0, 100.)
+
+    color_lower = np.array([0.1,0.1,0.8,.8])
+
+    geom_model.addGeometryObject(pin.GeometryObject(
+                                                    "wristGeom",
+                                                    id_lower,
+                                                    fcl.Sphere(geom_radius),
+                                                    pin.SE3(np.identity(3), np.array([0,0, geom_length])),
+                                                    "", np.ones(3), False, # Default values
+                                                    color_lower
+                                                    ))
+
+    geom_model.addGeometryObject(pin.GeometryObject(
+                                                    "lowerGeom",
+                                                    id_lower,
+                                                    fcl.Cylinder(geom_radius, geom_length),
+                                                    placement_lower,
+                                                    "", np.ones(3), False, # Default values
+                                                    color_lower
+                                                    ))
+
+
+    # Hand
+    placement_hand = pin.SE3(np.identity(3), np.array([0,0, geom_length]))
+    if(not revoluteOnly):
+        id_hand = model.addJoint(
+                                id_lower,
+                                pin.JointModelSpherical(),
+                                pin.SE3(np.identity(3), np.array([0,0, geom_length])),
+                                "wrist"
+                                )
+        set_limit(model, id_hand, pi, 3.0, 100.)
+    else:
+        id_hand = model.addJoint(
+                                id_lower,
+                                pin.JointModelRX(),
+                                pin.SE3(np.identity(3), np.array([0,0, geom_length])),
+                                "wristX"
+                                )
+        set_limit(model, id_hand, pi, 3.0, 100.)
+        id_hand = model.addJoint(
+                                id_hand,
+                                pin.JointModelRY(),
+                                pin.SE3.Identity(),
+                                "wristY"
+                                )
+        set_limit(model, id_hand, pi, 3.0, 100.)
+        id_hand = model.addJoint(
+                                id_hand,
+                                pin.JointModelRZ(),
+                                pin.SE3.Identity(),
+                                "wristZ"
+                                )
+        set_limit(model, id_hand, pi, 3.0, 100.)
+        model.appendBodyToJoint(
+                                id_hand,
+                                pin.Inertia.FromSphere(1.0 , geom_radius),
+                                placement_hand
+                                )
+
+    color_hand = np.array([0.7,0.7,0.7,.8])
+
+    geom_model.addGeometryObject(pin.GeometryObject(
+                                                    "palmGeom",
+                                                    id_hand,
+                                                    fcl.Sphere(geom_radius),
+                                                    pin.SE3(np.identity(3), np.array([0,0, 2*geom_radius])),
+                                                    "", np.ones(3), False, # Default values
+                                                    color_hand
+                                                    ))
+
+    geom_model.addGeometryObject(pin.GeometryObject(
+                                                    "finger1Geom",
+                                                    id_hand,
+                                                    fcl.Cylinder(0.02, 0.1),
+                                                    pin.SE3(np.identity(3), np.array([0.05, 0, 3*geom_radius])),
+                                                    "", np.ones(3), False, # Default values
+                                                    color_hand
+                                                    ))
+
+    geom_model.addGeometryObject(pin.GeometryObject(
+                                                    "finger2Geom",
+                                                    id_hand,
+                                                    fcl.Cylinder(0.02, 0.1),
+                                                    pin.SE3(np.identity(3), np.array([0, 0, 3*geom_radius])),
+                                                    "", np.ones(3), False, # Default values
+                                                    color_hand
+                                                    ))
+
+    geom_model.addGeometryObject(pin.GeometryObject(
+                                                    "finger3Geom",
+                                                    id_hand,
+                                                    fcl.Cylinder(0.02, 0.1),
+                                                    pin.SE3(np.identity(3), np.array([-0.05, 0, 3*geom_radius])),
+                                                    "", np.ones(3), False, # Default values
+                                                    color_hand
+                                                    ))
+
+    geom_model.addGeometryObject(pin.GeometryObject(
+                                                    "thumbGeom",
+                                                    id_hand,
+                                                    fcl.Cylinder(0.02, 0.1),
+                                                    pin.XYZQUATToSE3(np.array([geom_radius, 0, 2*geom_radius] + [0,.707,0,.707])),
+                                                    "", np.ones(3), False, # Default values
+                                                    color_hand
+                                                    ))
+
+    # Add end-effector frame
+    model.addFrame( pin.Frame('eeFrame',
+                                id_hand,
+                                0,
+                                pin.SE3(np.identity(3), np.array([0, 0, 2*geom_radius]))
+                                ,pin.FrameType.OP_FRAME)
+                                )
+
+    return model, geom_model
\ No newline at end of file
diff --git a/package.xml b/package.xml
index 6ab8eec5..8d67fdae 100644
--- a/package.xml
+++ b/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>tsid</name>
-  <version>1.6.1</version>
+  <version>1.6.2</version>
   <description>
     Efficient Task Space Inverse Dynamics (TSID) based on Pinocchio
   </description>
diff --git a/src/robots/robot-wrapper.cpp b/src/robots/robot-wrapper.cpp
index 1b6243de..b10a4d03 100644
--- a/src/robots/robot-wrapper.cpp
+++ b/src/robots/robot-wrapper.cpp
@@ -41,18 +41,11 @@ namespace tsid
       pinocchio::urdf::buildModel(filename, m_model, m_verbose);
       m_model_filename = filename;
       m_na = m_model.nv;
+      m_nq_actuated = m_model.nq;
+      m_is_fixed_base = true;
       init();
     }
 
-    RobotWrapper::RobotWrapper(const pinocchio::Model& m, bool verbose)
-      : m_verbose(verbose)
-    {
-      m_model = m;
-      m_model_filename = "";
-      m_na = m_model.nv-6;  // for backward compatibility assume the robot has a floating base
-      init();
-    }
-    
     RobotWrapper::RobotWrapper(const std::string & filename,
                                const std::vector<std::string> & ,
                                const pinocchio::JointModelVariant & rootJoint,
@@ -62,6 +55,43 @@ namespace tsid
       pinocchio::urdf::buildModel(filename, rootJoint, m_model, m_verbose);
       m_model_filename = filename;
       m_na = m_model.nv-6;
+      m_nq_actuated = m_model.nq-7;
+      m_is_fixed_base = false;
+      init();
+    }
+
+    RobotWrapper::RobotWrapper(const pinocchio::Model& m,
+                               bool verbose)
+      : m_verbose(verbose)
+    {
+      m_model = m;
+      m_model_filename = "";
+      m_na = m_model.nv-6;
+      m_nq_actuated = m_model.nq-7;
+      m_is_fixed_base = false;
+      init();
+    }
+
+    RobotWrapper::RobotWrapper(const pinocchio::Model& m,
+                               RootJointType rootJoint,
+                               bool verbose)
+      : m_verbose(verbose)
+    {
+      m_model = m;
+      m_model_filename = "";
+      m_na = m_model.nv;
+      m_nq_actuated = m_model.nq;
+      m_is_fixed_base = true;
+      switch(rootJoint) {
+        case FIXED_BASE_SYSTEM:
+          break;
+        case FLOATING_BASE_SYSTEM:
+          m_na -= 6;
+          m_nq_actuated = m_model.nq-7;
+          m_is_fixed_base = false;
+        default:
+          break;
+      }
       init();
     }
 
@@ -76,6 +106,8 @@ namespace tsid
     int RobotWrapper::nq() const { return m_model.nq; }
     int RobotWrapper::nv() const { return m_model.nv; }
     int RobotWrapper::na() const { return m_na; }
+    int RobotWrapper::nq_actuated() const { return m_nq_actuated; }
+    bool RobotWrapper::is_fixed_base() const { return m_is_fixed_base; }
     
     const Model & RobotWrapper::model() const { return m_model; }
     Model & RobotWrapper::model() { return m_model; }
diff --git a/src/solvers/solver-HQP-factory.cpp b/src/solvers/solver-HQP-factory.cpp
index f7f6def1..14bbcae6 100644
--- a/src/solvers/solver-HQP-factory.cpp
+++ b/src/solvers/solver-HQP-factory.cpp
@@ -19,6 +19,10 @@
 #include <tsid/solvers/solver-HQP-eiquadprog.hpp>
 #include <tsid/solvers/solver-HQP-eiquadprog-fast.hpp>
 
+#ifdef TSID_QPMAD_FOUND
+  #include <tsid/solvers/solver-HQP-qpmad.hpp>
+#endif
+
 #ifdef QPOASES_FOUND
   #include <tsid/solvers/solver-HQP-qpoases.hh>
 #endif
@@ -36,7 +40,12 @@ namespace tsid
       
       if(solverType==SOLVER_HQP_EIQUADPROG_FAST)
         return new SolverHQuadProgFast(name);
-      
+
+#ifdef TSID_QPMAD_FOUND
+      if(solverType==SOLVER_HQP_QPMAD)
+        return new SolverHQpmad(name);
+#endif
+
 #ifdef QPOASES_FOUND
       if(solverType==SOLVER_HQP_QPOASES)
         return new Solver_HQP_qpoases(name);
diff --git a/src/solvers/solver-HQP-qpmad.cpp b/src/solvers/solver-HQP-qpmad.cpp
new file mode 100644
index 00000000..998e776d
--- /dev/null
+++ b/src/solvers/solver-HQP-qpmad.cpp
@@ -0,0 +1,224 @@
+//
+// Copyright (c) 2022 Inria
+//
+// This file is part of tsid
+// tsid is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+// tsid is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// tsid If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+#include "tsid/solvers/solver-HQP-qpmad.hpp"
+#include "tsid/math/utils.hpp"
+#include "tsid/utils/stop-watch.hpp"
+
+#include <limits>
+
+using namespace tsid::math;
+using namespace tsid::solvers;
+using namespace Eigen;
+
+namespace tsid
+{
+  namespace solvers
+  {
+
+    SolverHQpmad::SolverHQpmad(const std::string & name):
+      SolverHQPBase(name),
+      m_hessian_regularization(DEFAULT_HESSIAN_REGULARIZATION)
+    {
+      m_n = 0;
+      m_nc = 0;
+    }
+
+    void SolverHQpmad::sendMsg(const std::string & s)
+    {
+      std::cout<<"[SolverHQpmad."<<m_name<<"] "<<s<<std::endl;
+    }
+
+    void SolverHQpmad::resize(unsigned int n, unsigned int neq, unsigned int nin)
+    {
+      unsigned int nc = neq+nin;
+
+      const bool resizeVar = n!=m_n;
+      const bool resizeEqIn = (resizeVar || nc!=m_nc);
+
+      if(resizeEqIn)
+      {
+    #ifndef NDEBUG
+        sendMsg("Resizing equality-inequality constraints from "+toString(m_nc)+" to "+toString(nc));
+    #endif
+        m_C.resize(nc, n);
+        m_cl.resize(nc);
+        m_cu.resize(nc);
+      }
+      if(resizeVar)
+      {
+    #ifndef NDEBUG
+        sendMsg("Resizing Hessian from "+toString(m_n)+" to "+toString(n));
+    #endif
+        m_H.resize(n, n);
+        m_g.resize(n);
+        m_output.x.resize(n);
+
+        if(m_has_bounds)
+        {
+          m_lb.resize(n);
+          m_ub.resize(n);
+        }
+      }
+
+      m_n = n;
+      m_nc = nc;
+    }
+
+    const HQPOutput & SolverHQpmad::solve(const HQPData & problemData)
+    {
+      if(problemData.size()>2)
+      {
+        assert(false && "Solver not implemented for more than 2 hierarchical levels.");
+      }
+
+      // Compute the constraint matrix sizes
+      m_has_bounds = false;
+      unsigned int nin = 0, neq=0;
+      const ConstraintLevel & cl0 = problemData[0];
+      if(cl0.size()>0)
+      {
+        const unsigned int n = cl0[0].second->cols();
+        for(ConstraintLevel::const_iterator it=cl0.begin(); it!=cl0.end(); it++)
+        {
+          auto constr = it->second;
+          assert(n==constr->cols());
+          if(constr->isEquality())
+            neq += constr->rows();
+          else if(constr->isInequality())
+            nin += constr->rows();
+          else if(constr->isBound())
+            m_has_bounds = true;
+        }
+        // If necessary, resize the constraint matrices
+        resize(n, neq, nin);
+
+        if(m_has_bounds)
+        {
+          m_lb.setConstant(std::numeric_limits<double>::min());
+          m_ub.setConstant(std::numeric_limits<double>::max());
+        }
+
+        int i_eq_in=0;
+        for(ConstraintLevel::const_iterator it=cl0.begin(); it!=cl0.end(); it++)
+        {
+          auto constr = it->second;
+          if(constr->isEquality())
+          {
+            m_C.middleRows(i_eq_in, constr->rows()) = constr->matrix();
+            m_cl.segment(i_eq_in, constr->rows())   = constr->vector();
+            m_cu.segment(i_eq_in, constr->rows())   = constr->vector();
+            i_eq_in += constr->rows();
+          }
+          else if(constr->isInequality())
+          {
+            m_C.middleRows(i_eq_in, constr->rows()) = constr->matrix();
+            m_cl.segment(i_eq_in, constr->rows())   = constr->lowerBound();
+            m_cu.segment(i_eq_in, constr->rows())   = constr->upperBound();
+            i_eq_in += constr->rows();
+          }
+          else if(constr->isBound())
+          {
+            // not considering masks
+            m_lb = m_lb.cwiseMax(constr->lowerBound());
+            m_ub = m_ub.cwiseMin(constr->upperBound());
+          }
+        }
+      }
+      else
+        resize(m_n, neq, nin);
+
+      if(problemData.size()>1)
+      {
+        const ConstraintLevel & cl1 = problemData[1];
+        m_H.setZero();
+        m_g.setZero();
+        for(ConstraintLevel::const_iterator it=cl1.begin(); it!=cl1.end(); it++)
+        {
+          const double & w = it->first;
+          auto constr = it->second;
+          if(!constr->isEquality())
+            assert(false && "Inequalities in the cost function are not implemented yet");
+
+          m_H.noalias() += w*constr->matrix().transpose()*constr->matrix();
+          m_g.noalias() -= w*(constr->matrix().transpose()*constr->vector());
+        }
+        m_H.diagonal().array() += m_hessian_regularization;
+      }
+
+      //  min 0.5 * x H x + g x
+      //  s.t.
+      //  cl <= C^T x <= cu
+      //  lb <= x <= ub
+
+      qpmad::Solver::ReturnStatus solve_status;
+
+      if(m_has_bounds)
+        solve_status = m_solver.solve(m_output.x, m_H, m_g, m_lb, m_ub, m_C, m_cl, m_cu, m_settings);
+      else
+        solve_status = m_solver.solve(m_output.x, m_H, m_g, Eigen::VectorXd(), Eigen::VectorXd(), m_C, m_cl, m_cu, m_settings);
+
+      if(solve_status != qpmad::Solver::OK)
+        m_output.status = HQP_STATUS_INFEASIBLE;
+      else
+      {
+        m_output.status = HQP_STATUS_OPTIMAL;
+
+    #ifndef NDEBUG
+        const Vector & x = m_output.x;
+
+        if(cl0.size()>0)
+        {
+          for(ConstraintLevel::const_iterator it=cl0.begin(); it!=cl0.end(); it++)
+          {
+            auto constr = it->second;
+            if(constr->checkConstraint(x)==false)
+            {
+              if(constr->isEquality())
+              {
+                sendMsg("Equality "+constr->name()+" violated: "+
+                        toString((constr->matrix()*x-constr->vector()).norm()));
+              }
+              else if(constr->isInequality())
+              {
+                sendMsg("Inequality "+constr->name()+" violated: "+
+                        toString((constr->matrix()*x-constr->lowerBound()).minCoeff())+"\n"+
+                        toString((constr->upperBound()-constr->matrix()*x).minCoeff()));
+              }
+              else if(constr->isBound())
+              {
+                sendMsg("Bound "+constr->name()+" violated: "+
+                        toString((x-constr->lowerBound()).minCoeff())+"\n"+
+                        toString((constr->upperBound()-x).minCoeff()));
+              }
+            }
+          }
+        }
+    #endif
+      }
+
+      return m_output;
+    }
+
+    double SolverHQpmad::getObjectiveValue()
+    {
+      // objective values is not stored by qpmad
+      return std::numeric_limits<double>::infinity();
+    }
+
+  } // namespace solvers
+} // namespace tsid
\ No newline at end of file
diff --git a/src/tasks/task-actuation-equality.cpp b/src/tasks/task-actuation-equality.cpp
index 74603e89..b40d1369 100644
--- a/src/tasks/task-actuation-equality.cpp
+++ b/src/tasks/task-actuation-equality.cpp
@@ -1,5 +1,5 @@
 //
-// Copyright (c) 2017 CNRS
+// Copyright (c) 2021 CNRS INRIA LORIA
 //
 // This file is part of tsid
 // tsid is free software: you can redistribute it
@@ -15,11 +15,106 @@
 // <http://www.gnu.org/licenses/>.
 //
 
+#include <tsid/tasks/task-actuation-equality.hpp>
+#include "tsid/robots/robot-wrapper.hpp"
+
 namespace tsid
 {
   namespace tasks
   {
-    
-    
+    using namespace math;
+    using namespace pinocchio;
+
+    TaskActuationEquality::TaskActuationEquality(const std::string & name,
+                                             RobotWrapper & robot):
+     TaskActuation(name, robot),
+     m_constraint(name, robot.na(), robot.na())
+   {
+      m_ref = Vector::Zero(robot.na());
+      m_weights = Vector::Ones(robot.na());
+      Vector m = Vector::Ones(robot.na());
+      mask(m);
+    }
+
+    const Vector & TaskActuationEquality::mask() const
+    {
+      return m_mask;
+    }
+
+    void TaskActuationEquality::mask(const Vector & m)
+    {
+      assert(m.size()==m_robot.na());
+      m_mask = m;
+
+      const Vector::Index dim = static_cast<Vector::Index>(m.sum());
+      Matrix S = Matrix::Zero(dim, m_robot.na());
+      m_activeAxes.resize(dim);
+      unsigned int j=0;
+      for(unsigned int i=0; i<m.size(); i++)
+        if(m(i)!=0.0)
+        {
+          assert(m(i)==1.0);
+          S(j,i) = m_weights(i);
+          m_activeAxes(j) = i;
+          j++;
+        }
+      m_constraint.resize((unsigned int)dim, m_robot.na());
+      m_constraint.setMatrix(S);
+
+      for(unsigned int i=0; i<m_activeAxes.size(); i++)
+        m_constraint.vector()(i) = m_ref(m_activeAxes(i)) * m_weights(m_activeAxes(i));
+    }
+
+    int TaskActuationEquality::dim() const
+    {
+      return (int)m_mask.sum();
+    }
+
+    // Reference should be the same size as robot.na(), even if a mask is used (masked dof values will just be ignored)
+    void TaskActuationEquality::setReference(ConstRefVector ref)
+    {
+      assert(ref.size()==m_robot.na());
+      m_ref = ref;
+
+      for(unsigned int i=0; i<m_activeAxes.size(); i++)
+        m_constraint.vector()(i) = m_ref(m_activeAxes(i)) * m_weights(m_activeAxes(i));
+    }
+
+    const Vector & TaskActuationEquality::getReference() const
+    {
+      return m_ref;
+    }
+
+    // Weighting vector should be the same size as robot.na(), even if a mask is used (masked dof values will just be ignored)
+    void TaskActuationEquality::setWeightVector(ConstRefVector weights)
+    {
+      assert(weights.size()==m_robot.na());
+      m_weights = weights;
+
+      for(unsigned int i=0; i<m_activeAxes.size(); i++)
+      {
+        m_constraint.matrix()(i,m_activeAxes(i)) = m_weights(m_activeAxes(i));
+        m_constraint.vector()(i) = m_ref(m_activeAxes(i)) * m_weights(m_activeAxes(i));
+      }
+    }
+
+    const Vector & TaskActuationEquality::getWeightVector() const
+    {
+      return m_weights;
+    }
+
+    const ConstraintBase & TaskActuationEquality::getConstraint() const
+    {
+      return m_constraint;
+    }
+
+    const ConstraintBase & TaskActuationEquality::compute(const double ,
+                                                        ConstRefVector ,
+                                                        ConstRefVector ,
+                                                        Data & )
+    {
+      return m_constraint;
+    }
+
   }
 }
diff --git a/src/tasks/task-joint-posVelAcc-bounds.cpp b/src/tasks/task-joint-posVelAcc-bounds.cpp
index 589bfde2..933c6683 100644
--- a/src/tasks/task-joint-posVelAcc-bounds.cpp
+++ b/src/tasks/task-joint-posVelAcc-bounds.cpp
@@ -20,7 +20,7 @@
 // #include <tsid/utils/stop-watch.hpp>
 
 /** This class has been implemented following :
-* Andrea del Prete. Joint Position and Velocity Bounds in Discrete-Time 
+* Andrea del Prete. Joint Position and Velocity Bounds in Discrete-Time
 * Acceleration/Torque Control of Robot Manipulators. IEEE Robotics and Automation
 * Letters, IEEE 2018, 3 (1), pp.281-288.￿10.1109/LRA.2017.2738321￿. hal-01356989v3
 * And
@@ -47,8 +47,8 @@ namespace tsid
     {
       assert(dt>0.0);
       m_eps = 1e-10;
-      m_qMin=Vector::Constant(m_na,1,1e10);
-      m_qMax=Vector::Constant(m_na,1,-1e10);
+      m_qMin=Vector::Constant(m_na,1,-1e10);
+      m_qMax=Vector::Constant(m_na,1,1e10);
       m_dqMax=Vector::Constant(m_na,1,1e10);
       m_ddqMax=Vector::Constant(m_na,1,1e10);
       m_impose_position_bounds=false;
@@ -207,7 +207,7 @@ namespace tsid
       computeAccLimits(q,v,m_verbose);
       m_constraint.upperBound()= m_ddqUB;
       m_constraint.lowerBound()= m_ddqLB;
-      // Eigen::internal::set_is_malloc_allowed(true); 
+      // Eigen::internal::set_is_malloc_allowed(true);
       // getProfiler().stop("TaskJointPosVelAccBounds");
       // getProfiler().report_all(9, std::cout);
       return m_constraint;
@@ -374,7 +374,7 @@ namespace tsid
           m_ddq_1[i] = m_minus_dq_over_dt[i];
           if(verbose==true)
           {
-            std::cout << "Error: state (" << qa[i] <<"," << dqa[i] <<") of joint "<< 
+            std::cout << "Error: state (" << qa[i] <<"," << dqa[i] <<") of joint "<<
             i <<  "not viable because delta is negative: "<< m_delta_1 << std::endl;
           }
         }
@@ -387,7 +387,7 @@ namespace tsid
           m_ddq_2[i] = m_minus_dq_over_dt[i];
           if(verbose==true)
           {
-            std::cout << "Error: state (" << qa[i] <<"," << dqa[i] <<") of joint "<< 
+            std::cout << "Error: state (" << qa[i] <<"," << dqa[i] <<") of joint "<<
               i <<  "not viable because delta is negative: "<< m_delta_2 << std::endl;
           }
         }
@@ -461,7 +461,7 @@ namespace tsid
 
         m_ddqLB[i]=m_lb.maxCoeff();
         m_ddqUB[i]=m_ub.minCoeff();
-        
+
         if(m_ddqUB[i] < m_ddqLB[i])
         {
           if(verbose==true)
diff --git a/src/tasks/task-joint-posture.cpp b/src/tasks/task-joint-posture.cpp
index 730d7db7..e062d5b8 100644
--- a/src/tasks/task-joint-posture.cpp
+++ b/src/tasks/task-joint-posture.cpp
@@ -17,6 +17,7 @@
 
 #include <tsid/tasks/task-joint-posture.hpp>
 #include "tsid/robots/robot-wrapper.hpp"
+#include <pinocchio/algorithm/joint-configuration.hpp>
 
 namespace tsid
 {
@@ -29,9 +30,10 @@ namespace tsid
     TaskJointPosture::TaskJointPosture(const std::string & name,
                                      RobotWrapper & robot):
       TaskMotion(name, robot),
-      m_ref(robot.na()),
+      m_ref(robot.nq_actuated(), robot.na()),
       m_constraint(name, robot.na(), robot.nv())
     {
+      m_ref_q_augmented = pinocchio::neutral(robot.model());
       m_Kp.setZero(robot.na());
       m_Kd.setZero(robot.na());
       Vector m = Vector::Ones(robot.na());
@@ -92,7 +94,7 @@ namespace tsid
 
     void TaskJointPosture::setReference(const TrajectorySample & ref)
     {
-      assert(ref.getValue().size()==m_robot.na());
+      assert(ref.getValue().size()==m_robot.nq_actuated());
       assert(ref.getDerivative().size()==m_robot.na());
       assert(ref.getSecondDerivative().size()==m_robot.na());
       m_ref = ref;
@@ -153,10 +155,12 @@ namespace tsid
                                                     ConstRefVector v,
                                                     Data & )
     {
+      m_ref_q_augmented.tail(m_robot.nq_actuated()) = m_ref.getValue();
+
       // Compute errors
-      m_p = q.tail(m_robot.na());
+      m_p_error = pinocchio::difference(m_robot.model(), m_ref_q_augmented, q).tail(m_robot.na());
+
       m_v = v.tail(m_robot.na());
-      m_p_error = m_p - m_ref.getValue();
       m_v_error = m_v - m_ref.getDerivative();
       m_a_des = - m_Kp.cwiseProduct(m_p_error)
                 - m_Kd.cwiseProduct(m_v_error)


Test patch:
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index efc6693a..dfb67a7a 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -1,63 +1,68 @@
 #
 # Copyright (c) 2017-2020 CNRS
 #
-# This file is part of tsid
-# tsid is free software: you can redistribute it
-# and/or modify it under the terms of the GNU Lesser General Public
-# License as published by the Free Software Foundation, either version
-# 3 of the License, or (at your option) any later version.
-# tsid is distributed in the hope that it will be
-# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
-# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
-# General Lesser Public License for more details. You should have
-# received a copy of the GNU Lesser General Public License along with
-# tsid. If not, see
+# This file is part of tsid tsid is free software: you can redistribute it
+# and/or modify it under the terms of the GNU Lesser General Public License as
+# published by the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version. tsid is distributed in the hope that it
+# will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+# Lesser Public License for more details. You should have received a copy of the
+# GNU Lesser General Public License along with tsid. If not, see
 # <http://www.gnu.org/licenses/>.
 
 # --- MACROS ------------------------------------------------------------------
 # --- MACROS ------------------------------------------------------------------
 # --- MACROS ------------------------------------------------------------------
 
-MACRO(ADD_TEST_CFLAGS target flag)
-  SET_PROPERTY(TARGET ${target} APPEND_STRING PROPERTY COMPILE_FLAGS " ${flag}")
-ENDMACRO(ADD_TEST_CFLAGS)
+macro(ADD_TEST_CFLAGS target flag)
+  set_property(
+    TARGET ${target}
+    APPEND_STRING
+    PROPERTY COMPILE_FLAGS " ${flag}")
+endmacro(ADD_TEST_CFLAGS)
 
-MACRO(ADD_TESTCASE NAME)
-  ADD_UNIT_TEST(${NAME} ${NAME}.cpp)
+macro(ADD_TESTCASE NAME)
+  add_unit_test(${NAME} ${NAME}.cpp)
 
-  ADD_TEST_CFLAGS(${NAME} "-DBOOST_TEST_DYN_LINK")
-  SET(MODULE_NAME "${NAME}Test")
-  STRING(REGEX REPLACE "-" "_" MODULE_NAME ${MODULE_NAME})
-  ADD_TEST_CFLAGS(${NAME} "-DBOOST_TEST_MODULE=${MODULE_NAME}")
+  add_test_cflags(${NAME} "-DBOOST_TEST_DYN_LINK")
+  set(MODULE_NAME "${NAME}Test")
+  string(REGEX REPLACE "-" "_" MODULE_NAME ${MODULE_NAME})
+  add_test_cflags(${NAME} "-DBOOST_TEST_MODULE=${MODULE_NAME}")
 
-  TARGET_LINK_LIBRARIES(${NAME} PRIVATE ${PROJECT_NAME} Boost::unit_test_framework)
-ENDMACRO(ADD_TESTCASE)
+  target_link_libraries(${NAME} PRIVATE ${PROJECT_NAME}
+                                        Boost::unit_test_framework)
+endmacro(ADD_TESTCASE)
 
 # --- RULES -------------------------------------------------------------------
 # --- RULES -------------------------------------------------------------------
 # --- RULES -------------------------------------------------------------------
 
-ADD_TESTCASE(constraints)
-ADD_TESTCASE(trajectories)
+add_testcase(constraints)
+add_testcase(trajectories)
 
-ADD_TESTCASE(robot-wrapper)
-ADD_TEST_CFLAGS(robot-wrapper '-DTSID_SOURCE_DIR=\\\"${${PROJECT_NAME}_SOURCE_DIR}\\\"')
+add_testcase(robot-wrapper)
+add_test_cflags(robot-wrapper
+                '-DTSID_SOURCE_DIR=\\\"${${PROJECT_NAME}_SOURCE_DIR}\\\"')
 
-ADD_TESTCASE(tasks)
-ADD_TEST_CFLAGS(tasks '-DTSID_SOURCE_DIR=\\\"${${PROJECT_NAME}_SOURCE_DIR}\\\"')
+add_testcase(tasks)
+add_test_cflags(tasks '-DTSID_SOURCE_DIR=\\\"${${PROJECT_NAME}_SOURCE_DIR}\\\"')
 
-ADD_TESTCASE(contacts)
-ADD_TEST_CFLAGS(contacts '-DTSID_SOURCE_DIR=\\\"${${PROJECT_NAME}_SOURCE_DIR}\\\"')
+add_testcase(contacts)
+add_test_cflags(contacts
+                '-DTSID_SOURCE_DIR=\\\"${${PROJECT_NAME}_SOURCE_DIR}\\\"')
 
-ADD_TESTCASE(tsid-formulation)
-ADD_TEST_CFLAGS(tsid-formulation '-DTSID_SOURCE_DIR=\\\"${${PROJECT_NAME}_SOURCE_DIR}\\\"')
+add_testcase(tsid-formulation)
+add_test_cflags(tsid-formulation
+                '-DTSID_SOURCE_DIR=\\\"${${PROJECT_NAME}_SOURCE_DIR}\\\"')
 
-ADD_TESTCASE(math_utils)
-ADD_TESTCASE(hqp_solvers)
+add_testcase(math_utils)
+add_testcase(hqp_solvers)
 
-ADD_TESTCASE(set_gravity)
-ADD_TEST_CFLAGS(set_gravity '-DTSID_SOURCE_DIR=\\\"${${PROJECT_NAME}_SOURCE_DIR}\\\"')
+add_testcase(set_gravity)
+add_test_cflags(set_gravity
+                '-DTSID_SOURCE_DIR=\\\"${${PROJECT_NAME}_SOURCE_DIR}\\\"')
 
-IF(BUILD_PYTHON_INTERFACE)
-  ADD_SUBDIRECTORY(python)
-ENDIF(BUILD_PYTHON_INTERFACE)
+if(BUILD_PYTHON_INTERFACE)
+  add_subdirectory(python)
+endif(BUILD_PYTHON_INTERFACE)
diff --git a/tests/hqp_solvers.cpp b/tests/hqp_solvers.cpp
index daec9df2..bd5e6ff7 100644
--- a/tests/hqp_solvers.cpp
+++ b/tests/hqp_solvers.cpp
@@ -23,6 +23,11 @@
 #include <tsid/solvers/solver-HQP-factory.hxx>
 #include <tsid/solvers/solver-HQP-eiquadprog.hpp>
 #include <tsid/solvers/solver-HQP-eiquadprog-rt.hpp>
+
+#ifdef TSID_QPMAD_FOUND
+  #include <tsid/solvers/solver-HQP-qpmad.hpp>
+#endif
+
 #include <tsid/math/utils.hpp>
 #include <tsid/math/constraint-equality.hpp>
 #include <tsid/math/constraint-inequality.hpp>
@@ -164,6 +169,7 @@ BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
 #define PROFILE_EIQUADPROG "Eiquadprog"
 #define PROFILE_EIQUADPROG_RT "Eiquadprog Real Time"
 #define PROFILE_EIQUADPROG_FAST "Eiquadprog Fast"
+#define PROFILE_QPMAD "QPMAD"
 
 BOOST_AUTO_TEST_CASE ( test_eiquadprog_classic_vs_rt_vs_fast)
 {
@@ -208,6 +214,12 @@ BOOST_AUTO_TEST_CASE ( test_eiquadprog_classic_vs_rt_vs_fast)
                                                              "eiquadprog");
   solver->resize(n, neq, nin);
 
+#ifdef TSID_QPMAD_FOUND
+  SolverHQPBase * solver_qpmad = SolverHQPFactory::createNewSolver(SOLVER_HQP_QPMAD,
+                                                             "qpmad");
+  solver_qpmad->resize(n, neq, nin);
+#endif
+
   // CREATE PROBLEM DATA
   HQPData HQPData(2);
 
@@ -274,6 +286,12 @@ BOOST_AUTO_TEST_CASE ( test_eiquadprog_classic_vs_rt_vs_fast)
     const HQPOutput & output    = solver->solve(HQPData);
     getProfiler().stop(PROFILE_EIQUADPROG);
 
+  #ifdef TSID_QPMAD_FOUND
+    getProfiler().start(PROFILE_QPMAD);
+    const HQPOutput & output_qpmad = solver_qpmad->solve(HQPData);
+    getProfiler().stop(PROFILE_QPMAD);
+  #endif
+
     getStatistics().store("active inequalities", (double)output_rt.activeSet.size());
     getStatistics().store("solver iterations", output_rt.iterations);
 
@@ -303,6 +321,11 @@ BOOST_AUTO_TEST_CASE ( test_eiquadprog_classic_vs_rt_vs_fast)
   //                        "Sol RT"+toString(output_rt.x.transpose())+
   //                        "\nSol FAST "+toString(output_fast.x.transpose())+
                           "\nDiff FAST: "+toString((output_rt.x-output_fast.x).norm()));
+
+    #ifdef TSID_QPMAD_FOUND
+      BOOST_CHECK_MESSAGE(output.x.isApprox(output_qpmad.x, EPS),
+                          "\nDiff QPMAD: "+toString((output.x-output_qpmad.x).norm()));
+    #endif
     }
   }
 
diff --git a/tests/python/CMakeLists.txt b/tests/python/CMakeLists.txt
index 9eed5c98..d5e7e654 100644
--- a/tests/python/CMakeLists.txt
+++ b/tests/python/CMakeLists.txt
@@ -1,16 +1,12 @@
-SET(${PYWRAP}_TESTS
-  #Constraint
-  #ContactPoint
-  #Contact
-  Formulation
-  Gravity
-  RobotWrapper
-  #Solvers
-  Tasks
-  Trajectories
-  Deprecations
-  )
+set(${PYWRAP}_TESTS
+    # Constraint
+    # ContactPoint
+    # Contact
+    Formulation Gravity RobotWrapper
+    # Solvers
+    Tasks Trajectories Deprecations)
 
-FOREACH(test ${${PYWRAP}_TESTS})
-  ADD_PYTHON_UNIT_TEST("py-${test}" "tests/python/test_${test}.py" "bindings/python")
-ENDFOREACH(test ${${PYWRAP}_TESTS})
+foreach(test ${${PYWRAP}_TESTS})
+  add_python_unit_test("py-${test}" "tests/python/test_${test}.py"
+                       "bindings/python")
+endforeach(test ${${PYWRAP}_TESTS})
diff --git a/tests/python/test_Constraint.py b/tests/python/test_Constraint.py
index 332eccb1..db6dcac5 100644
--- a/tests/python/test_Constraint.py
+++ b/tests/python/test_Constraint.py
@@ -7,12 +7,12 @@
 print("Test Constraint Bound")
 print("")
 
-se3.switchToNumpyMatrix()
+#se3.switchToNumpyMatrix()
 
 tol = 1e-5
 n = 5
-lb = np.matrix(-1.0 * np.ones(n)).transpose()
-ub = np.matrix(np.ones(n)).transpose()
+lb = -1.0 * np.ones(n)
+ub = np.ones(n)
 ConstBound = tsid.ConstraintBound("bounds", lb, ub)
 
 assert ConstBound.isBound
@@ -26,12 +26,12 @@
 assert ub.all() == ConstBound.upperBound.all()
 
 lb *= 2.0
-assert np.linalg.norm(lb - ConstBound.lowerBound, 2) is not 0
+assert np.linalg.norm(lb - ConstBound.lowerBound, 2) != 0
 ConstBound.setLowerBound(lb)
 assert lb.all() == ConstBound.lowerBound.all()
 
 ub *= 2.0
-assert np.linalg.norm(ub - ConstBound.upperBound, 2) is not 0
+assert np.linalg.norm(ub - ConstBound.upperBound, 2) != 0
 ConstBound.setUpperBound(ub)
 assert ub.all() == ConstBound.upperBound.all()
 assert np.linalg.norm(ub - ConstBound.upperBound, 2) < tol
@@ -41,8 +41,8 @@
 print("")
 n = 5
 m = 2
-A = np.matrix(np.ones((m, n)))
-b = np.matrix(np.ones(m)).transpose()
+A = np.ones((m, n))
+b = np.ones(m)
 equality = tsid.ConstraintEquality("equality", A, b)
 
 assert not equality.isBound
@@ -56,12 +56,12 @@
 assert np.linalg.norm(b - equality.vector, 2) < tol
 
 b *= 2.0
-assert np.linalg.norm(b - equality.vector, 2) is not 0
+assert np.linalg.norm(b - equality.vector, 2) != 0
 equality.setVector(b)
 assert np.linalg.norm(b - equality.vector, 2) < tol
 
 A *= 2.0
-assert np.linalg.norm(A - equality.matrix, 2) is not 0
+assert np.linalg.norm(A - equality.matrix, 2) != 0
 equality.setMatrix(A)
 assert np.linalg.norm(A - equality.matrix, 2) < tol
 
@@ -71,9 +71,9 @@
 
 n = 5
 m = 2
-A = np.matrix(np.ones((m, n)))
-lb = np.matrix(-np.ones(m)).transpose()
-ub = np.matrix(np.ones(m)).transpose()
+A = np.ones((m, n))
+lb = -np.ones(m)
+ub = np.ones(m)
 inequality = tsid.ConstraintInequality("inequality", A, lb, ub)
 
 assert not inequality.isBound
@@ -84,12 +84,12 @@
 assert inequality.cols == n
 
 lb *= 2.0
-assert np.linalg.norm(lb - inequality.lowerBound, 2) is not 0
+assert np.linalg.norm(lb - inequality.lowerBound, 2) != 0
 inequality.setLowerBound(lb)
 assert np.linalg.norm(lb - inequality.lowerBound, 2) < tol
 
 A *= 2.0
-assert np.linalg.norm(A - inequality.matrix, 2) is not 0
+assert np.linalg.norm(A - inequality.matrix, 2) != 0
 inequality.setMatrix(A)
 assert np.linalg.norm(A - inequality.matrix, 2) < tol
 
diff --git a/tests/python/test_Solvers.py b/tests/python/test_Solvers.py
index 50668d77..60fdc2e9 100644
--- a/tests/python/test_Solvers.py
+++ b/tests/python/test_Solvers.py
@@ -1,16 +1,10 @@
-import copy
-
 import numpy as np
-import pinocchio as se3
-
 import tsid
 
 print("")
 print("Test Solvers")
 print("")
 
-se3.switchToNumpyMatrix()
-
 EPS = 1e-3
 nTest = 100
 n = 60
@@ -29,15 +23,15 @@
 solver.resize(n, neq, nin)
 
 HQPData = tsid.HQPData()
-A1 = np.matrix(np.random.rand(n, n)) + 0.001 * np.matrix(np.eye(n))
-b1 = np.matrix(np.random.rand(n)).transpose()
+A1 = np.random.rand(n, n) + 0.001 * np.eye(n)
+b1 = np.random.rand(n)
 cost = tsid.ConstraintEquality("c1", A1, b1)
 
-x = np.linalg.inv(A1) * b1
-A_in = np.matrix(np.random.rand(nin, n))
-A_lb = np.matrix(np.random.rand(nin)).transpose() * NORMAL_DISTR_VAR
-A_ub = np.matrix(np.random.rand(nin)).transpose() * NORMAL_DISTR_VAR
-constrVal = A_in * x
+x = np.linalg.inv(A1) @ b1
+A_in = np.random.rand(nin, n)
+A_lb = np.random.rand(nin) * NORMAL_DISTR_VAR
+A_ub = np.random.rand(nin) * NORMAL_DISTR_VAR
+constrVal = A_in @ x
 
 for i in range(0, nin):
     if A_ub[i] <= A_lb[i]:
@@ -50,8 +44,8 @@
         A_lb[i] = constrVal[i] - MARGIN_PERC * np.abs(constrVal[i])
 
 in_const = tsid.ConstraintInequality("ini1", A_in, A_lb, A_ub)
-A_eq = np.matrix(np.random.rand(neq, n))
-b_eq = A_eq * x
+A_eq = np.random.rand(neq, n)
+b_eq = A_eq @ x
 eq_const = tsid.ConstraintEquality("eq1", A_eq, b_eq)
 
 const1 = tsid.ConstraintLevel()
@@ -73,8 +67,8 @@
 gradientPerturbations = []
 hessianPerturbations = []
 for i in range(0, nTest):
-    gradientPerturbations.append(np.matrix(np.random.rand(n) * GRADIENT_PERTURBATION_VARIANCE).transpose())
-    hessianPerturbations.append(np.matrix(np.random.rand(n, n) * HESSIAN_PERTURBATION_VARIANCE))
+    gradientPerturbations.append(np.random.rand(n) * GRADIENT_PERTURBATION_VARIANCE)
+    hessianPerturbations.append(np.random.rand(n, n) * HESSIAN_PERTURBATION_VARIANCE)
 
 for i in range(0, nTest):
     cost.setMatrix(cost.matrix + hessianPerturbations[i])
@@ -82,6 +76,6 @@
 
     HQPoutput = solver.solve(HQPData)
 
-    assert np.linalg.norm(A_eq * HQPoutput.x - b_eq, 2) < EPS
-    # assert (A_in * HQPoutput.x <= A_ub + EPS).all()
-    # assert (A_in * HQPoutput.x > A_lb - EPS).all()
+    assert np.linalg.norm(A_eq @ HQPoutput.x - b_eq, 2) < EPS
+    # assert (A_in @ HQPoutput.x <= A_ub + EPS).all()
+    # assert (A_in @ HQPoutput.x > A_lb - EPS).all()
diff --git a/tests/python/test_Tasks.py b/tests/python/test_Tasks.py
index ed963a6f..590895a6 100644
--- a/tests/python/test_Tasks.py
+++ b/tests/python/test_Tasks.py
@@ -257,4 +257,68 @@
     if i % 100 == 0:
         print("Time :", t, "Momentum error :", error)
 
+print("")
+print("Test Task Joint Posture (Uncommon joints)")
+print("")
+
+# Get robot model generator module
+import sys
+filepath = os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), '../../'))
+sys.path.append(str(filepath))
+from models.arm.generator import create_7dof_arm
+
+# Get robot model
+model, geom_model = create_7dof_arm() # A robot containing sperical joints where nq != nv
+
+# Initialize problem
+robot = tsid.RobotWrapper(model, tsid.FIXED_BASE_SYSTEM, False)
+data = robot.data()
+
+q = pin.neutral(model)
+v = np.matrix(np.zeros(robot.nv)).transpose()
+
+task_joint = tsid.TaskJointPosture("task-posture-uncommon", robot)
+
+Kp = 100 * np.matrix(np.ones(robot.nv)).transpose()
+Kd = 20.0 * np.matrix(np.ones(robot.na)).transpose()
+task_joint.setKp(Kp)
+task_joint.setKd(Kd)
+
+assert np.linalg.norm(Kp - task_joint.Kp, 2) < tol
+assert np.linalg.norm(Kd - task_joint.Kd, 2) < tol
+
+q_ref = pin.randomConfiguration(model)
+
+sample = tsid.TrajectorySample(robot.nq, robot.nv)
+sample.value(q_ref)
+sample.derivative(np.zeros(robot.nv))
+sample.second_derivative(np.zeros(robot.nv))
+
+error_past = 1e100
+t = 0.0
+max_it = 1000
+
+for i in range(0, max_it):
+    robot.computeAllTerms(data, q, v)
+    task_joint.setReference(sample)
+    const = task_joint.compute(t, q, v, data)
+
+    Jpinv = np.linalg.pinv(const.matrix, 1e-5)
+    dv = Jpinv * const.vector
+
+    assert np.linalg.norm(Jpinv * const.matrix, 2) - 1.0 < tol
+    v += dt * dv
+    q = pin.integrate(model, q, dt * v)
+    t += dt
+
+    error = np.linalg.norm(task_joint.position_error, 2)
+    assert error - error_past < 1e-4
+    error_past = error
+    if error < 1e-8:
+        print("Success Convergence")
+        break
+    if i % 100 == 0:
+        print("Time :", t, "Joint pos error :", error, "Joint vel error :",
+              np.linalg.norm(task_joint.velocity_error, 2))
+
 print("All test is done")