Merge remote-tracking branch 'main/devel' into release/1.0.0

loco-3d · Dec 19, 2019 · af8f826 · af8f826
2 parents 2a18746 + 8532867
commit af8f826
Show file tree

Hide file tree

Showing 159 changed files with 2,119 additions and 1,341 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -6,16 +6,17 @@ SET(PROJECT_NAME crocoddyl)
 SET(PROJECT_DESCRIPTION "Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)")
 SET(PROJECT_URL https://github.com/${PROJECT_NAMESPACE}/${PROJECT_NAME})
 
+# Check if the submodule cmake have been initialized
+IF(NOT EXISTS "${CMAKE_SOURCE_DIR}/cmake/base.cmake")
+  MESSAGE(FATAL_ERROR "\nPlease run the following command first:\ngit submodule update --init\n")
+ENDIF()
+
 # Include important cmake modules
-INCLUDE(cmake/base.cmake RESULT_VARIABLE SUBMODULE_FOUND)
+INCLUDE(cmake/base.cmake)
 INCLUDE(cmake/test.cmake)
 INCLUDE(cmake/boost.cmake)
 INCLUDE(cmake/python.cmake)
-
-# Check if the cmake (git submodule) have been initialized
-IF(SUBMODULE_FOUND STREQUAL "NOTFOUND")
-  MESSAGE(FATAL_ERROR "\nPlease run the following command first:\ngit submodule update --init\n")
-ENDIF(SUBMODULE_FOUND STREQUAL "NOTFOUND")
+INCLUDE(cmake/apple.cmake)
 
 # Print initial message
 MESSAGE("${PROJECT_DESCRIPTION}, version ${PROJECT_VERSION}")
@@ -49,6 +50,9 @@ ENDIF()
 COMPUTE_PROJECT_ARGS(PROJECT_ARGS LANGUAGES CXX)
 PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
 
+# If needed, fix CMake policy for APPLE systems
+APPLY_DEFAULT_APPLE_CONFIGURATION()
+
 # Add the different required and optional dependencies
 ADD_REQUIRED_DEPENDENCY("eigen3 >= 3.0.5")
 ADD_REQUIRED_DEPENDENCY("eigenpy")
@@ -88,8 +92,7 @@ SET(BOOST_OPTIONAL_COMPONENTS "")
 IF(BUILD_PYTHON_INTERFACE)
   SET(BOOST_OPTIONAL_COMPONENTS ${BOOST_OPTIONAL_COMPONENTS} python)
   FINDPYTHON()
-  FIND_NUMPY()
-  INCLUDE_DIRECTORIES(SYSTEM ${PYTHON_INCLUDE_DIRS} ${NUMPY_INCLUDE_DIRS})
+  INCLUDE_DIRECTORIES(SYSTEM ${PYTHON_INCLUDE_DIRS})
 ENDIF(BUILD_PYTHON_INTERFACE)
 
 SET(BOOST_COMPONENTS ${BOOST_REQUIERED_COMPONENTS} ${BOOST_OPTIONAL_COMPONENTS} ${BOOST_BUILD_COMPONENTS})

diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md
@@ -1,21 +1,63 @@
-[Carlos Mastalli](https://cmastalli.github.io/) (University of Edinburgh): main developer and manager of the project
+## I. About
+Crocoddyl has been written in C++ for efficiency and uses the  Eigen  library  for  linear  algebra  routines.  It  comes  with Python bindings for easy prototyping. Crocoddyl is currently supported  for  most  Linux  distributions,  with  plans  to  release on Mac OS X and Windows. The project is fully open-source under  the  permissive  BSD-3-Clause  license  and  is  hosted on  GitHub.
 
-[Nicolas Mansard](http://projects.laas.fr/gepetto/index.php/Members/NicolasMansard) (LAAS-CNRS): project instructor
+## II. Design pattern
+Crocoddyl is designed using virtualization pattern for easy prototyping and yet efficient implementation. In early benchmark, we have shown that virtualization is as efficient as static polymorphism (i.e. CRTP design) for system dynamics higher than 16. The real benefits of static polymorphism are in very small system (with dimension less than 6), however, virtualization is still competitive.
 
-[Rohan Budhiraja](https://scholar.google.com/citations?user=NW9Io9AAAAAJ) (LAAS-CNRS): features extension
+One of the main design concepts of Crocoddyl is the strict separation between _model_ and _data_.  A model describes a system or procedure, e.g. action, cost, and activation models. Any model has been abstracted to  easily derive new systems in both C++ and Python. By data, we refer to a container that stores computed and intermediate values used during the calculation routine performed by a model. Each model creates its own data, and with this, we avoid any temporary memory allocation produced by algebraic expressions in Eigen. To improve  efficiency, especially in systems with dimension slower than 16, the Eigen members inside a data object can be  defined with fixed dimensions. It allows for efficient use of modern CPU features by using vectorization and  Same Instruction Multiple Data (SIMD) operations.
 
-[Justin Carpentier](https://jcarpent.github.io/) (INRIA): efficient analytical rigid-body dynamics derivatives
+## III. Code Format
 
-[Maximilien Naveau](https://scholar.google.fr/citations?user=y_-cGlUAAAAJ&hl=fr) (MPI): unit-test support
+Crocoddyl CI also checks code format (for both c++ and Python code) as effort to maximize readability and maintainability. Depending on the language, c++ or Python, we use different format policies which are tested with [Gepetto/linters](https://github.com/gepetto/linters) repository. For c++ code, Crocoddyl uses Google format convention, instead for Python code it follows [flake8 rules](http://flake8.pycqa.org/en/latest/) with a maximum length of 119 characters.
 
-[Guilhem Saurel](http://projects.laas.fr/gepetto/index.php/Members/GuilhemSaurel) (LAAS-CNRS): continuous integration and deployment
+Despite that Crocoddyl follows the strict code format, you don't need to learn each specific details of these formatting rules. For that, you have two options:
+1. configuration your IDE to lint your code automatically (or check it), and
+2. let our linter to adjust your code.
 
-[Wolfgang Merkt](http://www.wolfgangmerkt.com/research/) (University of Edinburgh): feature extension and debugging
+Here we explain how to do the later. For this, you can either use the provided docker image (subsection 1), or manually setup your environment by yourself (subsection 2).
 
-[Josep Martí Saumell](https://www.iri.upc.edu/staff/jmarti) (UPC): feature extension
+### 1. Direct use of the provided docker image
 
-[Bilal Hammoud](https://scholar.google.com/citations?hl=en&user=h_4NKpsAAAAJ) (MPI): features extension
+```bash
+docker run --rm -v $PWD:/app -w /app -it gepetto/linters
+```
 
-## Acknowledgments
+This will not work if the root user doesn't have the right to go into your current working directory.
+And if your current working directory is accessed through symlinks, you might need to replace `$PWD` by `$(pwd -P)`.
 
-The development of **Pinocchio** is supported by the [EU MEMMO project](http://www.memmo-project.eu/), the [Gepetto team](http://projects.laas.fr/gepetto/) [@LAAS-CNRS](http://www.laas.fr), and the [Statistical Machine Learning and Motor Control Group](http://wcms.inf.ed.ac.uk/ipab/slmc) [@University of Edinburgh](https://www.edinburgh-robotics.org/).
+### 2. Manual use of each tool
+Before starting with it, you need to clone **Gepetto/linter** repository, i.e.
+
+```bash
+git clone [email protected]:Gepetto/linters.git
+sudo apt install clang-format-6.0
+pip install --user flake8 isort yapf
+```
+
+**Please note yapf is in a beta version, we tested our system with v0.27.0.**
+
+*Please note that with the following instructions your current code will be updated*
+
+#### 2.1. Formatting your c++ code
+
+For c++ testing through clang-format. We used the configuration file defined in the Gepetto linter. Then, you need to install the configuration files in the current working directory (beware of git if doing so) or in a parent directory. Your home might be a good choice:
+
+```bash
+cd ${YOUR_CROCCODDYL_SOURCE_PATH}
+ln -s ${YOUR_GEPETTO_LINTER_PATH}/.clang-format .clang-format
+```
+
+And for formatting your c++ code, you just need to run the follows:
+
+```bash
+cd ${YOUR_CROCCODDYL_SOURCE_PATH}
+clang-format-6.0 -i $(find . -path ./cmake -prune -o -iregex '.*\.\(h\|c\|hh\|cc\|hpp\|cpp\|hxx\|cxx\)$' -print)
+```
+
+#### 2.2. Formatting your Python code
+
+```bash
+cd ${YOUR_CROCCODDYL_SOURCE_PATH}
+flake8 .
+yapf -ri .
+```
diff --git a/bindings/python/crocoddyl/CMakeLists.txt b/bindings/python/crocoddyl/CMakeLists.txt
@@ -6,8 +6,9 @@ SET(${PROJECT_NAME}_PYTHON_BINDINGS_FILES
   __init__.py
   )
 
-
 ADD_LIBRARY(${PROJECT_NAME}_pywrap SHARED ${${PROJECT_NAME}_PYTHON_BINDINGS_SOURCES})
+SET_TARGET_PROPERTIES(${PROJECT_NAME}_pywrap PROPERTIES SUFFIX ${PYTHON_EXT_SUFFIX})
+
 PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME}_pywrap eigen3)
 PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME}_pywrap eigenpy)
 PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME}_pywrap pinocchio)
@@ -20,7 +21,6 @@ TARGET_COMPILE_OPTIONS(${PROJECT_NAME}_pywrap PRIVATE "-Wno-conversion")
 
 INSTALL(TARGETS ${PROJECT_NAME}_pywrap DESTINATION ${PYTHON_SITELIB}/${PROJECT_NAME})
 
-
 FOREACH(python ${${PROJECT_NAME}_PYTHON_BINDINGS_FILES})
   PYTHON_BUILD(. ${python})
   INSTALL(FILES

diff --git a/bindings/python/crocoddyl/core.hpp b/bindings/python/crocoddyl/core.hpp
@@ -9,11 +9,13 @@
 #ifndef BINDINGS_PYTHON_CROCODDYL_CORE_HPP_
 #define BINDINGS_PYTHON_CROCODDYL_CORE_HPP_
 
+#include "python/crocoddyl/core/data-collector-base.hpp"
 #include "python/crocoddyl/core/state-base.hpp"
 #include "python/crocoddyl/core/actuation-base.hpp"
 #include "python/crocoddyl/core/action-base.hpp"
 #include "python/crocoddyl/core/diff-action-base.hpp"
 #include "python/crocoddyl/core/activation-base.hpp"
+#include "python/crocoddyl/core/data/actuation.hpp"
 #include "python/crocoddyl/core/integrator/euler.hpp"
 #include "python/crocoddyl/core/numdiff/action.hpp"
 #include "python/crocoddyl/core/numdiff/diff-action.hpp"
@@ -37,11 +39,13 @@ namespace crocoddyl {
 namespace python {
 
 void exposeCore() {
+  exposeDataCollector();
   exposeStateAbstract();
   exposeActuationAbstract();
   exposeActionAbstract();
   exposeDifferentialActionAbstract();
   exposeActivationAbstract();
+  exposeDataCollectorActuation();
   exposeIntegratedActionEuler();
   exposeActionNumDiff();
   exposeDifferentialActionNumDiff();

diff --git a/bindings/python/crocoddyl/core/action-base.hpp b/bindings/python/crocoddyl/core/action-base.hpp
@@ -9,6 +9,7 @@
 #ifndef BINDINGS_PYTHON_CROCODDYL_CORE_ACTION_BASE_HPP_
 #define BINDINGS_PYTHON_CROCODDYL_CORE_ACTION_BASE_HPP_
 
+#include "crocoddyl/core/utils/exception.hpp"
 #include "crocoddyl/core/action-base.hpp"
 
 namespace crocoddyl {
@@ -24,21 +25,25 @@ class ActionModelAbstract_wrap : public ActionModelAbstract, public bp::wrapper<
   void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
             const Eigen::Ref<const Eigen::VectorXd>& u) {
     if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
-      throw std::invalid_argument("x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
+      throw_pretty("Invalid argument: "
+                   << "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
     }
     if (static_cast<std::size_t>(u.size()) != nu_) {
-      throw std::invalid_argument("u has wrong dimension (it should be " + std::to_string(nu_) + ")");
+      throw_pretty("Invalid argument: "
+                   << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
     }
     return bp::call<void>(this->get_override("calc").ptr(), data, (Eigen::VectorXd)x, (Eigen::VectorXd)u);
   }
 
   void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
                 const Eigen::Ref<const Eigen::VectorXd>& u, const bool& recalc = true) {
     if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
-      throw std::invalid_argument("x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
+      throw_pretty("Invalid argument: "
+                   << "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
     }
     if (static_cast<std::size_t>(u.size()) != nu_) {
-      throw std::invalid_argument("u has wrong dimension (it should be " + std::to_string(nu_) + ")");
+      throw_pretty("Invalid argument: "
+                   << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
     }
     return bp::call<void>(this->get_override("calcDiff").ptr(), data, (Eigen::VectorXd)x, (Eigen::VectorXd)u, recalc);
   }
@@ -59,22 +64,21 @@ void exposeActionAbstract() {
       "derivatives. These computations are mainly carry on inside calc() and calcDiff(),\n"
       "respectively.",
       bp::init<boost::shared_ptr<StateAbstract>, int, bp::optional<int> >(
-          bp::args(" self", " state", " nu", " nr=1"),
+          bp::args("self", "state", "nu", "nr"),
           "Initialize the action model.\n\n"
           "You can also describe autonomous systems by setting nu = 0.\n"
           ":param state: state description,\n"
           ":param nu: dimension of control vector,\n"
-          ":param nr: dimension of the cost-residual vector"))
-      .def("calc", pure_virtual(&ActionModelAbstract_wrap::calc), bp::args(" self", " data", " x", " u"),
+          ":param nr: dimension of the cost-residual vector (default 1)"))
+      .def("calc", pure_virtual(&ActionModelAbstract_wrap::calc), bp::args("self", "data", "x", "u"),
            "Compute the next state and cost value.\n\n"
            "It describes the time-discrete evolution of our dynamical system\n"
            "in which we obtain the next state. Additionally it computes the\n"
            "cost value associated to this discrete state and control pair.\n"
            ":param data: action data\n"
            ":param x: time-discrete state vector\n"
            ":param u: time-discrete control input")
-      .def("calcDiff", pure_virtual(&ActionModelAbstract_wrap::calcDiff),
-           bp::args(" self", " data", " x", " u", " recalc=True"),
+      .def("calcDiff", pure_virtual(&ActionModelAbstract_wrap::calcDiff), bp::args("self", "data", "x", "u", "recalc"),
            "Compute the derivatives of the dynamics and cost functions.\n\n"
            "It computes the partial derivatives of the dynamical system and the\n"
            "cost function. If recalc == True, it first updates the state evolution\n"
@@ -83,22 +87,22 @@ void exposeActionAbstract() {
            ":param data: action data\n"
            ":param x: time-discrete state vector\n"
            ":param u: time-discrete control input\n"
-           ":param recalc: If true, it updates the state evolution and the cost value.")
-      .def("createData", &ActionModelAbstract_wrap::createData, bp::args(" self"),
+           ":param recalc: If true, it updates the state evolution and the cost value (default True).")
+      .def("createData", &ActionModelAbstract_wrap::createData, bp::args("self"),
            "Create the action data.\n\n"
            "Each action model (AM) has its own data that needs to be allocated.\n"
            "This function returns the allocated data for a predefined AM.\n"
            ":return AM data.")
       .def("quasiStatic", &ActionModelAbstract_wrap::quasiStatic_wrap,
            ActionModel_quasiStatic_wraps(
-               bp::args(" self", " data", " x", " maxiter=100", " tol=1e-9"),
+               bp::args("self", "data", "x", "maxiter", "tol"),
                "Compute the quasic-static control given a state.\n\n"
                "It runs an iterative Newton step in order to compute the quasic-static regime\n"
                "given a state configuration.\n"
                ":param data: action data\n"
                ":param x: discrete-time state vector\n"
                ":param maxiter: maximum allowed number of iterations\n"
-               ":param tol: stopping tolerance criteria\n"
+               ":param tol: stopping tolerance criteria (default 1e-9)\n"
                ":return u: quasic-static control"))
       .add_property(
           "nu", bp::make_function(&ActionModelAbstract_wrap::get_nu, bp::return_value_policy<bp::return_by_value>()),
@@ -132,7 +136,7 @@ void exposeActionAbstract() {
       "user-defined action model. The action data typically is allocated onces by running\n"
       "model.createData() and contains the first- and second- order derivatives of the dynamics\n"
       "and cost function, respectively.",
-      bp::init<ActionModelAbstract*>(bp::args(" self", " model"),
+      bp::init<ActionModelAbstract*>(bp::args("self", "model"),
                                      "Create common data shared between AMs.\n\n"
                                      "The action data uses the model in order to first process it.\n"
                                      ":param model: action model"))