Merge pull request #1 from loco-3d/devel

Update from the main repo

.github/workflows/industrial_ci_action.yml

@@ -11,8 +11,23 @@ jobs:
     strategy:
       matrix:
         env:
-          - {ROS_DISTRO: melodic}
-          - {ROS_DISTRO: noetic}
+          # Ubuntu 18.04, g++, Release
+          - {name: "Bionic / g++ / Release", ROS_DISTRO: melodic}
+          # Ubuntu 20.04, g++, Release
+          - {name: "Focal / g++ / Release", ROS_DISTRO: noetic}
+          # Ubuntu 20.04, clang, Release
+          - {name: "Focal / clang / Release", ROS_DISTRO: noetic, ADDITIONAL_DEBS: clang, CC: clang, CXX: clang++}
+          # Ubuntu 20.04, clang, Release, multi-threading
+          - {name: "Focal / clang / Release / Multi-threading", ROS_DISTRO: noetic, ADDITIONAL_DEBS: clang, CC: clang, CXX: clang++, CMAKE_ARGS: "-DBUILD_WITH_MULTITHREADS=ON -DBUILD_WITH_NTHREADS=2"}
+          # Format check
+          - {name: "Format check", ROS_DISTRO: noetic, CLANG_FORMAT_CHECK: file, CLANG_FORMAT_VERSION: "6.0", BEFORE_RUN_CLANG_FORMAT_CHECK: "wget https://raw.githubusercontent.com/Gepetto/linters/master/.clang-format-6.0 -O /tmp/clang_format_check/crocoddyl/.clang-format", ADDITIONAL_DEBS: wget}
+          ## Working configs for Debug mode. However, tests take too long in Debug mode (~1h on a laptop).
+          ## Hence, not active on GitHub Actions
+          # # Ubuntu 20.04, g++, Debug
+          # - {name: "Focal / g++ / Debug", ROS_DISTRO: noetic, CMAKE_ARGS: '-DCMAKE_BUILD_TYPE=Debug'}
+          # Ubuntu 20.04, clang, Debug
+          - {name: "Focal / clang / Debug", ROS_DISTRO: noetic, ADDITIONAL_DEBS: clang, CC: clang, CXX: clang++, CMAKE_ARGS: '-DCMAKE_BUILD_TYPE=Debug'}
+    name: ${{ matrix.env.name }}
     env:
       CCACHE_DIR: /github/home/.ccache             # Enable ccache
       UPSTREAM_WORKSPACE: dependencies.rosinstall  # to build example-robot-data from source as it's not released via the ROS buildfarm
@@ -36,6 +51,9 @@ jobs:
         with:
           path: ${{ env.CCACHE_DIR }}
           key: ccache-${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }}
-      # Run industrial_ci
-      - uses: 'ros-industrial/industrial_ci@master'
+      # Run industrial_ci - use fork until awaits https://github.com/ros-industrial/industrial_ci/issues/767 is resolved
+      - uses: 'wxmerkt/industrial_ci@topic/clang-format-check-in-deterministic-location'
         env: ${{ matrix.env }}
+      # # Run industrial_ci
+      # - uses: 'ros-industrial/industrial_ci@master'
+      #   env: ${{ matrix.env }}

CMakeLists.txt

@@ -66,26 +66,32 @@ SET(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake/find-external/CppAD/" ${CMAKE_M
 
 # Add the different required and optional dependencies
 ADD_PROJECT_DEPENDENCY(pinocchio 2.6.3 REQUIRED PKG_CONFIG_REQUIRES "pinocchio >= 2.6.3")
-ADD_PROJECT_DEPENDENCY(example-robot-data 3.7.0 REQUIRED PKG_CONFIG_REQUIRES "example-robot-data >= 3.7.0")
+IF(BUILD_EXAMPLES OR BUILD_TESTING OR BUILD_BENCHMARK)
+  ADD_PROJECT_DEPENDENCY(example-robot-data 3.7.0 REQUIRED PKG_CONFIG_REQUIRES "example-robot-data >= 3.7.0")
+ELSE()
+  ADD_OPTIONAL_DEPENDENCY(example-robot-data)
+ENDIF()
 ADD_OPTIONAL_DEPENDENCY("scipy")
 
 OPTION(BUILD_WITH_CODEGEN_SUPPORT "Build the library with the Code Generation support (required CppADCodeGen)" OFF)
 
 OPTION(BUILD_WITH_MULTITHREADS "Build the library with the Multithreading support (required OpenMP)" OFF)
 IF(BUILD_WITH_MULTITHREADS)
-  INCLUDE(ProcessorCount)
-  ProcessorCount(NPROCESSOR)
-  IF(NOT NPROCESSOR EQUAL 0)
-    MATH(EXPR NPROCESSOR ${NPROCESSOR}/2)
-    SET(BUILD_WITH_NTHREADS ${NPROCESSOR} CACHE STRING "Number of threads")
-  ELSE()
-    SET(BUILD_WITH_NTHREADS "4" CACHE STRING "Number of threads") # Default setting
+  # If BUILD_WITH_NTHREADS defined, use the value - otherwise detect
+  IF(NOT DEFINED BUILD_WITH_NTHREADS)
+    INCLUDE(ProcessorCount)
+    ProcessorCount(NPROCESSOR)
+    IF(NOT NPROCESSOR EQUAL 0)
+      MATH(EXPR NPROCESSOR ${NPROCESSOR}/2)
+      SET(BUILD_WITH_NTHREADS ${NPROCESSOR} CACHE STRING "Number of threads")
+    ELSE()
+      SET(BUILD_WITH_NTHREADS "4" CACHE STRING "Number of threads") # Default setting
+    ENDIF()
   ENDIF()
-
   string(REGEX MATCH "^[0-9]+$" BUILD_WITH_NTHREADS  ${BUILD_WITH_NTHREADS})
   IF(NOT BUILD_WITH_NTHREADS MATCHES "^[0-9]+$")
     SET(BUILD_WITH_NTHREADS 4) # Default setting 4.
-    MESSAGE(WARNING "the number of threads have to be an interger value, set to ${BUILD_WITH_NTHREADS}")
+    MESSAGE(WARNING "The number of threads has to be an integer value, set to ${BUILD_WITH_NTHREADS}")
   ENDIF()
 ENDIF()
 

bindings/python/crocoddyl/CMakeLists.txt

@@ -19,6 +19,8 @@ TARGET_LINK_BOOST_PYTHON(${PROJECT_NAME}_pywrap)
 # BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS spews conversion warnings from int to long unsigned int.
 # Unfortunately, using literals does not work in a macro. As such, this turns them off for the entire wrapper:
 TARGET_COMPILE_OPTIONS(${PROJECT_NAME}_pywrap PRIVATE "-Wno-conversion")
+# We are also exposing deprecated declarations until they are removed. Ignore the errors for the wrapper:
+TARGET_COMPILE_OPTIONS(${PROJECT_NAME}_pywrap PRIVATE "-Wno-deprecated-declarations")
 
 IF(UNIX AND NOT APPLE)
   SET_TARGET_PROPERTIES(${PROJECT_NAME}_pywrap PROPERTIES INSTALL_RPATH "\$ORIGIN/../../..")

bindings/python/crocoddyl/__init__.py

@@ -29,6 +29,7 @@ def rotationMatrixFromTwoVectors(a, b):
 
 
 class DisplayAbstract:
+
     def __init__(self, rate=-1, freq=1):
         self.rate = rate
         self.freq = freq
@@ -63,6 +64,7 @@ def getFrameTrajectoryFromSolver(self, solver):
 
 
 class GepettoDisplay(DisplayAbstract):
+
     def __init__(self, robot, rate=-1, freq=1, cameraTF=None, floor=True, frameNames=[], visibility=False):
         DisplayAbstract.__init__(self, rate, freq)
         self.robot = robot
@@ -182,10 +184,10 @@ def getForceTrajectoryFromSolver(self, solver):
                             R = np.eye(3)
                             mu = 0.7
                             for k, c in model.differential.costs.costs.todict().items():
-                                if isinstance(c.cost, libcrocoddyl_pywrap.CostModelContactFrictionCone):
-                                    if contact.frame == c.cost.reference.id:
-                                        R = c.cost.reference.cone.R
-                                        mu = c.cost.reference.cone.mu
+                                if isinstance(c.cost.residual, libcrocoddyl_pywrap.ResidualModelContactFrictionCone):
+                                    if contact.frame == c.cost.residual.id:
+                                        R = c.cost.residual.reference.R
+                                        mu = c.cost.residual.reference.mu
                                         continue
                             fc.append({"key": str(joint), "oMf": oMf, "f": force, "R": R, "mu": mu})
                     fs.append(fc)
@@ -200,10 +202,10 @@ def getForceTrajectoryFromSolver(self, solver):
                             R = np.eye(3)
                             mu = 0.7
                             for k, c in model.differential.costs.costs.todict().items():
-                                if isinstance(c.cost, libcrocoddyl_pywrap.CostModelContactFrictionCone):
-                                    if contact.frame == c.cost.reference.id:
-                                        R = c.cost.reference.cone.R
-                                        mu = c.cost.reference.cone.mu
+                                if isinstance(c.cost.residual, libcrocoddyl_pywrap.ResidualModelContactFrictionCone):
+                                    if contact.frame == c.cost.residual.id:
+                                        R = c.cost.residual.reference.R
+                                        mu = c.cost.residual.reference.mu
                                         continue
                             fc.append({"key": str(joint), "oMf": oMf, "f": force, "R": R, "mu": mu})
                     fs.append(fc)
@@ -218,10 +220,10 @@ def getForceTrajectoryFromSolver(self, solver):
                         R = np.eye(3)
                         mu = 0.7
                         for k, c in model.costs.costs.todict().items():
-                            if isinstance(c.cost, libcrocoddyl_pywrap.CostModelContactFrictionCone):
-                                if impulse.frame == c.cost.id:
-                                    R = c.cost.cone.R
-                                    mu = c.cost.cone.mu
+                            if isinstance(c.cost.residual, libcrocoddyl_pywrap.ResidualModelContactFrictionCone):
+                                if impulse.frame == c.cost.residual.id:
+                                    R = c.cost.residual.reference.R
+                                    mu = c.cost.residual.reference.mu
                                     continue
                         fc.append({"key": str(joint), "oMf": oMf, "f": force, "R": R, "mu": mu})
                 fs.append(fc)
@@ -336,6 +338,7 @@ def _setConeMu(self, coneName, mu):
 
 
 class MeshcatDisplay(DisplayAbstract):
+
     def __init__(self, robot, rate=-1, freq=1, openWindow=True):
         DisplayAbstract.__init__(self, rate, freq)
         self.robot = robot
@@ -362,6 +365,7 @@ def _addRobot(self, openWindow):
 
 
 class CallbackDisplay(libcrocoddyl_pywrap.CallbackAbstract):
+
     def __init__(self, display):
         libcrocoddyl_pywrap.CallbackAbstract.__init__(self)
         self.visualization = display
@@ -373,6 +377,7 @@ def __call__(self, solver):
 
 
 class CallbackLogger(libcrocoddyl_pywrap.CallbackAbstract):
+
     def __init__(self):
         libcrocoddyl_pywrap.CallbackAbstract.__init__(self)
         self.xs = []

bindings/python/crocoddyl/core/action-base.hpp

@@ -1,7 +1,7 @@
 ///////////////////////////////////////////////////////////////////////////////
 // BSD 3-Clause License
 //
-// Copyright (C) 2019-2020, LAAS-CNRS, University of Edinburgh
+// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh
 // Copyright note valid unless otherwise stated in individual files.
 // All rights reserved.
 ///////////////////////////////////////////////////////////////////////////////
@@ -47,6 +47,7 @@ class ActionModelAbstract_wrap : public ActionModelAbstract, public bp::wrapper<
   }
 
   boost::shared_ptr<ActionDataAbstract> createData() {
+    enableMultithreading() = false;
     if (boost::python::override createData = this->get_override("createData")) {
       return bp::call<boost::shared_ptr<ActionDataAbstract> >(createData.ptr());
     }

bindings/python/crocoddyl/core/activation-base.hpp

@@ -1,7 +1,7 @@
 ///////////////////////////////////////////////////////////////////////////////
 // BSD 3-Clause License
 //
-// Copyright (C) 2019-2020, LAAS-CNRS, University of Edinburgh
+// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh
 // Copyright note valid unless otherwise stated in individual files.
 // All rights reserved.
 ///////////////////////////////////////////////////////////////////////////////
@@ -37,6 +37,7 @@ class ActivationModelAbstract_wrap : public ActivationModelAbstract, public bp::
   }
 
   boost::shared_ptr<ActivationDataAbstract> createData() {
+    enableMultithreading() = false;
     if (boost::python::override createData = this->get_override("createData")) {
       return bp::call<boost::shared_ptr<ActivationDataAbstract> >(createData.ptr());
     }

bindings/python/crocoddyl/core/actuation-base.hpp

@@ -2,7 +2,7 @@
 ///////////////////////////////////////////////////////////////////////////////
 // BSD 3-Clause License
 //
-// Copyright (C) 2019-2020, LAAS-CNRS, University of Edinburgh
+// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh
 // Copyright note valid unless otherwise stated in individual files.
 // All rights reserved.
 ///////////////////////////////////////////////////////////////////////////////
@@ -49,6 +49,7 @@ class ActuationModelAbstract_wrap : public ActuationModelAbstract, public bp::wr
   }
 
   boost::shared_ptr<ActuationDataAbstract> createData() {
+    enableMultithreading() = false;
     if (boost::python::override createData = this->get_override("createData")) {
       return bp::call<boost::shared_ptr<ActuationDataAbstract> >(createData.ptr());
     }

bindings/python/crocoddyl/core/control-base.hpp

@@ -2,7 +2,7 @@
 ///////////////////////////////////////////////////////////////////////////////
 // BSD 3-Clause License
 //
-// Copyright (C) 2021, LAAS-CNRS, University of Edinburgh, University of Trento
+// Copyright (C) 2021-2022, LAAS-CNRS, University of Edinburgh, University of Trento
 // Copyright note valid unless otherwise stated in individual files.
 // All rights reserved.
 ///////////////////////////////////////////////////////////////////////////////
@@ -51,6 +51,7 @@ class ControlParametrizationModelAbstract_wrap : public ControlParametrizationMo
   }
 
   boost::shared_ptr<ControlParametrizationDataAbstract> createData() {
+    enableMultithreading() = false;
     if (boost::python::override createData = this->get_override("createData")) {
       return bp::call<boost::shared_ptr<ControlParametrizationDataAbstract> >(createData.ptr());
     }

bindings/python/crocoddyl/core/controls/poly-one.cpp

@@ -40,6 +40,8 @@ void exposeControlParametrizationPolyOne() {
           ":param data: control-parametrization data\n"
           ":param t: normalized time in [0, 1]\n"
           ":param u: control parameters (dim control.nu)")
+      .def("createData", &ControlParametrizationModelPolyOne::createData, bp::args("self"),
+           "Create the poly-one data.")
       .def<void (ControlParametrizationModelPolyOne::*)(const boost::shared_ptr<ControlParametrizationDataAbstract>&,
                                                         double, const Eigen::Ref<const Eigen::VectorXd>&) const>(
           "params", &ControlParametrizationModelPolyOne::params, bp::args("self", "data", "t", "w"),

bindings/python/crocoddyl/core/controls/poly-two-rk.cpp

@@ -43,6 +43,8 @@ void exposeControlParametrizationPolyTwoRK() {
           ":param data: poly-two-rk data\n"
           ":param t: normalized time in [0, 1]\n"
           ":param u: control parameters (dim control.nu)")
+      .def("createData", &ControlParametrizationModelPolyTwoRK::createData, bp::args("self"),
+           "Create the poly-two-rk data.")
       .def<void (ControlParametrizationModelPolyTwoRK::*)(const boost::shared_ptr<ControlParametrizationDataAbstract>&,
                                                           double, const Eigen::Ref<const Eigen::VectorXd>&) const>(
           "params", &ControlParametrizationModelPolyTwoRK::params, bp::args("self", "data", "t", "w"),

bindings/python/crocoddyl/core/controls/poly-zero.cpp

@@ -39,6 +39,8 @@ void exposeControlParametrizationPolyZero() {
           ":param data: control-parametrization data\n"
           ":param t: normalized time in [0, 1]\n"
           ":param u: control parameters (dim control.nu)")
+      .def("createData", &ControlParametrizationModelPolyZero::createData, bp::args("self"),
+           "Create the poly-zero data.")
       .def<void (ControlParametrizationModelPolyZero::*)(const boost::shared_ptr<ControlParametrizationDataAbstract>&,
                                                          double, const Eigen::Ref<const Eigen::VectorXd>&) const>(
           "params", &ControlParametrizationModelPolyZero::params, bp::args("self", "data", "t", "u"),

bindings/python/crocoddyl/core/core.cpp

@@ -36,6 +36,7 @@ void exposeCore() {
   exposeActionNumDiff();
   exposeDifferentialActionNumDiff();
   exposeActivationNumDiff();
+  exposeStateNumDiff();
   exposeShootingProblem();
   exposeSolverAbstract();
   exposeStateEuclidean();

bindings/python/crocoddyl/core/core.hpp

@@ -38,6 +38,7 @@ void exposeCostControl();
 void exposeActionNumDiff();
 void exposeDifferentialActionNumDiff();
 void exposeActivationNumDiff();
+void exposeStateNumDiff();
 void exposeShootingProblem();
 void exposeSolverAbstract();
 void exposeStateEuclidean();