Merge branch 'main' into feature/ocs2_anymal_perception

# Conflicts:
#	ocs2_sqp/ocs2_sqp/CMakeLists.txt
#	ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h
#	ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp

Author: rubengrandia

.github/workflows/ros-build-test.yml

@@ -30,7 +30,7 @@ jobs:
     - name: System deps
       run: |
         apt-get update
-        apt-get install -y git ninja-build liburdfdom-dev liboctomap-dev libassimp-dev
+        apt-get install -y git ninja-build liburdfdom-dev liboctomap-dev libassimp-dev checkinstall wget rsync
 
     - uses: actions/checkout@v2
       with: 
@@ -46,14 +46,33 @@ jobs:
         git clone --recurse-submodules https://github.com/leggedrobotics/pinocchio.git src/pinocchio
         git clone --recurse-submodules https://github.com/leggedrobotics/hpp-fcl.git src/hpp-fcl
         git clone https://github.com/leggedrobotics/ocs2_robotic_assets.git src/ocs2_robotic_assets
+        
+    - name: Install RaiSim
+      run: |
+        git clone --depth 1 https://github.com/raisimTech/raisimLib.git -b v1.1.01 src/raisim_tech
+        cd src/raisim_tech
+        mkdir build
+        cd build
+        cmake .. -DPYTHON_EXECUTABLE=$(python3 -c "import sys; print(sys.executable)")
+        make -j4 && checkinstall
+        
+    - name: Install ONNX Runtime
+      run: |
+        wget https://github.com/microsoft/onnxruntime/releases/download/v1.7.0/onnxruntime-linux-x64-1.7.0.tgz -P tmp/microsoft
+        tar xf tmp/microsoft/onnxruntime-linux-x64-1.7.0.tgz -C tmp/microsoft
+        rsync -a tmp/microsoft/onnxruntime-linux-x64-1.7.0/include/ /usr/local/include/onnxruntime
+        rsync -a tmp/microsoft/onnxruntime-linux-x64-1.7.0/lib/ /usr/local/lib
+        rsync -a src/ocs2/ocs2_mpcnet/ocs2_mpcnet_core/misc/onnxruntime/cmake/ /usr/local/share/onnxruntime
+
     - name: Build (${{ matrix.build_type }})
       shell: bash
       run: |
         source /opt/ros/noetic/setup.bash
-        catkin_make_isolated --use-ninja --merge --only-pkg-with-deps ocs2 --ignore-pkg ocs2_raisim --cmake-args -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} 
+        catkin_make_isolated --use-ninja --merge --only-pkg-with-deps ocs2 --cmake-args -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} 
         
     - name: Test
       shell: bash
       run: |
         source devel_isolated/setup.bash
         catkin_make_isolated --use-ninja --merge --only-pkg-with-deps ocs2 --catkin-make-args run_tests
+        catkin_test_results

.gitignore

@@ -25,3 +25,4 @@ ocs2_ddp/test/ddp_test_generated/
 *.out
 *.synctex.gz
 .vscode/
+runs/

jenkins-pipeline

@@ -1,5 +1,5 @@
 library 'continuous_integration_pipeline'
-ciPipeline("--ros-distro noetic --publish-doxygen --recipes raisimlib\
+ciPipeline("--ros-distro noetic --publish-doxygen --recipes onnxruntime raisimlib\
             --dependencies '[email protected]:leggedrobotics/hpp-fcl.git;master;git'\
                            '[email protected]:leggedrobotics/pinocchio.git;master;git'\
                            '[email protected]:leggedrobotics/ocs2_robotic_assets.git;main;git'\

ocs2/package.xml

@@ -16,14 +16,17 @@
   <exec_depend>ocs2_oc</exec_depend>
   <exec_depend>ocs2_qp_solver</exec_depend>
   <exec_depend>ocs2_ddp</exec_depend>
+  <exec_depend>ocs2_slp</exec_depend>
   <exec_depend>ocs2_sqp</exec_depend>
   <exec_depend>ocs2_ros_interfaces</exec_depend>
   <exec_depend>ocs2_python_interface</exec_depend>
   <exec_depend>ocs2_pinocchio</exec_depend>
   <exec_depend>ocs2_robotic_tools</exec_depend>
+  <exec_depend>ocs2_perceptive</exec_depend>
   <exec_depend>ocs2_robotic_examples</exec_depend>
   <exec_depend>ocs2_thirdparty</exec_depend>
   <exec_depend>ocs2_raisim</exec_depend>
+  <exec_depend>ocs2_mpcnet</exec_depend>
 
    <export>
    	  <metapackage />

ocs2_core/cmake/ocs2_cxx_flags.cmake

@@ -22,5 +22,5 @@ list(APPEND OCS2_CXX_FLAGS
   )
 
 # Cpp standard version
-set(CMAKE_CXX_STANDARD 11)
+set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)

ocs2_core/include/ocs2_core/Types.h

@@ -71,17 +71,8 @@ using matrix_array2_t = std::vector<matrix_array_t>;
 /** Array of arrays of dynamic matrix trajectory type. */
 using matrix_array3_t = std::vector<matrix_array2_t>;
 
-/** Eigen scalar type. */
-using eigen_scalar_t = Eigen::Matrix<scalar_t, 1, 1>;
-/** Eigen scalar trajectory type. */
-using eigen_scalar_array_t = std::vector<eigen_scalar_t>;
-/** Array of eigen scalar trajectory type. */
-using eigen_scalar_array2_t = std::vector<eigen_scalar_array_t>;
-/** Array of arrays of eigen scalar trajectory type. */
-using eigen_scalar_array3_t = std::vector<eigen_scalar_array2_t>;
-
 /**
- * Defines the linear approximation
+ * Defines the linear approximation of a scalar function
  * f(x,u) = dfdx' dx + dfdu' du + f
  */
 struct ScalarFunctionLinearApproximation {
@@ -134,7 +125,7 @@ std::ostream& operator<<(std::ostream& out, const ScalarFunctionLinearApproximat
  *
  * @param[in] stateDim: Number of states.
  * @param[in] inputDim: Number of inputs.
- * @param[in] data: Given quadratic approximation.
+ * @param[in] data: Given linear approximation.
  * @param[in] dataName: The name of the data which appears in the output error message.
  * @return The description of the error. If there was no error it would be empty;
  */
@@ -148,7 +139,7 @@ inline ScalarFunctionLinearApproximation operator*(scalar_t scalar, ScalarFuncti
 }
 
 /**
- * Defines the quadratic approximation
+ * Defines the quadratic approximation of a scalar function
  * f(x,u) = 1/2 dx' dfdxx dx + du' dfdux dx + 1/2 du' dfduu du + dfdx' dx + dfdu' du + f
  */
 struct ScalarFunctionQuadraticApproximation {

ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h

@@ -49,11 +49,14 @@ class StateConstraintCollection : public Collection<StateConstraint> {
   ~StateConstraintCollection() override = default;
   StateConstraintCollection* clone() const override;
 
-  /** Returns the number of active constraints at given time. */
-  virtual size_t getNumConstraints(scalar_t time) const;
+  /** Returns the number of active constraints at a given time. */
+  size_t getNumConstraints(scalar_t time) const;
 
-  /** Get the constraint vector value */
-  virtual vector_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const;
+  /** Returns the number of active constraints at a given time for each term. If a term is inactive, its size is zero. */
+  size_array_t getTermsSize(scalar_t time) const;
+
+  /** Get an array of all constraints. If a term is inactive, the corresponding element is a vector of size zero. */
+  virtual vector_array_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const;
 
   /** Get the constraint linear approximation */
   virtual VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state,
@@ -68,4 +71,4 @@ class StateConstraintCollection : public Collection<StateConstraint> {
   StateConstraintCollection(const StateConstraintCollection& other);
 };
 
-}  // namespace ocs2
+}  // namespace ocs2

ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h

@@ -49,11 +49,14 @@ class StateInputConstraintCollection : public Collection<StateInputConstraint> {
   ~StateInputConstraintCollection() override = default;
   StateInputConstraintCollection* clone() const override;
 
-  /** Returns the number of active constraints at given time. */
-  virtual size_t getNumConstraints(scalar_t time) const;
+  /** Returns the number of active constraints at a given time. */
+  size_t getNumConstraints(scalar_t time) const;
 
-  /** Get the constraint vector value */
-  virtual vector_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const;
+  /** Returns the number of active constraints at a given time for each term. If a term is inactive, its size is zero. */
+  size_array_t getTermsSize(scalar_t time) const;
+
+  /** Get an array of all constraints. If a term is inactive, the corresponding element is a vector of size zero. */
+  virtual vector_array_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const;
 
   /** Get the constraint linear approximation */
   virtual VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const vector_t& input,
@@ -68,4 +71,4 @@ class StateInputConstraintCollection : public Collection<StateInputConstraint> {
   StateInputConstraintCollection(const StateInputConstraintCollection& other);
 };
 
-}  // namespace ocs2
+}  // namespace ocs2

ocs2_core/include/ocs2_core/control/ControllerType.h

@@ -34,6 +34,6 @@ namespace ocs2 {
 /**
  * Enum class for specifying controller type
  */
-enum class ControllerType { UNKNOWN, FEEDFORWARD, LINEAR, NEURAL_NETWORK };
+enum class ControllerType { UNKNOWN, FEEDFORWARD, LINEAR, ONNX, BEHAVIORAL };
 
 }  // namespace ocs2

ocs2_core/include/ocs2_core/integration/TrapezoidalIntegration.h

@@ -29,22 +29,32 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
+#include <cassert>
 #include <vector>
 
 namespace ocs2 {
 
-template <typename SCALAR_T>
-SCALAR_T trapezoidalIntegration(const std::vector<SCALAR_T>& timeTrajectory, const std::vector<SCALAR_T>& valueTrajectory) {
+/**
+ * Compute the trapezoidal integration of a trajectory of VALUE_T given the time stamp timeTrajectory and initial value initialValue.
+ *
+ * @note It requires that the VALUE_T has overwrite operator+(VALUE_T, VALUE_T) and define VALUE_T::operator*(SCALAR_T)
+ */
+template <typename SCALAR_T, typename VALUE_T>
+VALUE_T trapezoidalIntegration(const std::vector<SCALAR_T>& timeTrajectory, const std::vector<VALUE_T>& valueTrajectory,
+                               VALUE_T initialValue) {
+  assert(timeTrajectory.size() == valueTrajectory.size());
+
   if (timeTrajectory.size() < 2) {
-    return 0.0;
+    return initialValue;
   }
 
-  SCALAR_T areaUnderCurve = 0.0;
   for (size_t k = 1; k < timeTrajectory.size(); k++) {
-    areaUnderCurve += 0.5 * (valueTrajectory[k] + valueTrajectory[k - 1]) * (timeTrajectory[k] - timeTrajectory[k - 1]);
+    auto temp = valueTrajectory[k - 1] + valueTrajectory[k];
+    temp *= (0.5 * (timeTrajectory[k] - timeTrajectory[k - 1]));
+    initialValue += temp;
   }  // end of k loop
 
-  return areaUnderCurve;
+  return initialValue;
 }
 
 }  // namespace ocs2