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

.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

ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateConstraint.h

@@ -47,9 +47,10 @@ class LoopshapingStateConstraint final : public StateConstraintCollection {
 
   LoopshapingStateConstraint* clone() const override { return new LoopshapingStateConstraint(*this); }
 
-  vector_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const override;
+  vector_array_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const override;
   VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state,
                                                            const PreComputation& preComp) const override;
+
   VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state,
                                                                  const PreComputation& preComp) const override;
 

ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateInputConstraint.h

@@ -41,7 +41,7 @@ class LoopshapingStateInputConstraint : public StateInputConstraintCollection {
  public:
   ~LoopshapingStateInputConstraint() override = default;
 
-  vector_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const override;
+  vector_array_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const override;
 
  protected:
   LoopshapingStateInputConstraint(const StateInputConstraintCollection& systemConstraint,

ocs2_core/include/ocs2_core/misc/LinearAlgebra.h

@@ -38,41 +38,18 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace ocs2 {
 namespace LinearAlgebra {
 
-// forward declarations
-void makePsdEigenvalue(matrix_t& squareMatrix, scalar_t minEigenvalue);
-
-void makePsdCholesky(matrix_t& A, scalar_t minEigenvalue);
-
-void computeConstraintProjection(const matrix_t& Dm, const matrix_t& RmInvUmUmT, matrix_t& DmDagger, matrix_t& DmDaggerTRmDmDaggerUUT,
-                                 matrix_t& RmInvConstrainedUUT);
-
 /**
  *  Set the eigenvalues of a triangular matrix to a minimum magnitude (maintaining the sign).
  */
-inline void setTriangularMinimumEigenvalues(matrix_t& Lr, scalar_t minEigenValue = numeric_traits::weakEpsilon<scalar_t>()) {
-  for (Eigen::Index i = 0; i < Lr.rows(); ++i) {
-    scalar_t& eigenValue = Lr(i, i);  // diagonal element is the eigenvalue
-    if (eigenValue < 0.0) {
-      eigenValue = std::min(-minEigenValue, eigenValue);
-    } else {
-      eigenValue = std::max(minEigenValue, eigenValue);
-    }
-  }
-}
+void setTriangularMinimumEigenvalues(matrix_t& Lr, scalar_t minEigenValue = numeric_traits::weakEpsilon<scalar_t>());
 
 /**
  * Makes the input matrix PSD using a eigenvalue decomposition.
  *
- * @tparam Derived type.
- * @param squareMatrix: The matrix to become PSD.
+ * @param [in, out] squareMatrix: The matrix to become PSD.
  * @param [in] minEigenvalue: minimum eigenvalue.
  */
-template <typename Derived>
-void makePsdEigenvalue(Eigen::MatrixBase<Derived>& squareMatrix, scalar_t minEigenvalue = numeric_traits::limitEpsilon<scalar_t>()) {
-  matrix_t mat = squareMatrix;
-  makePsdEigenvalue(mat, minEigenvalue);
-  squareMatrix = mat;
-}
+void makePsdEigenvalue(matrix_t& squareMatrix, scalar_t minEigenvalue = numeric_traits::limitEpsilon<scalar_t>());
 
 /**
  * Makes the input matrix PSD based on Gershgorin circle theorem. If the input matrix is positive definite and diagonally dominant,
@@ -89,20 +66,10 @@ void makePsdEigenvalue(Eigen::MatrixBase<Derived>& squareMatrix, scalar_t minEig
  * (1) Aii < minEigenvalue + Ri ==> minEigenvalue < lambda < minEigenvalue + 2 Ri
  * (2) Aii > minEigenvalue + Ri ==> minEigenvalue < Aii - Ri < lambda < Aii + Ri
  *
- * @tparam Derived type.
- * @param squareMatrix: The matrix to become PSD.
+ * @param [in, out] squareMatrix: The matrix to become PSD.
  * @param [in] minEigenvalue: minimum eigenvalue.
  */
-template <typename Derived>
-void makePsdGershgorin(Eigen::MatrixBase<Derived>& squareMatrix, scalar_t minEigenvalue = numeric_traits::limitEpsilon<scalar_t>()) {
-  assert(squareMatrix.rows() == squareMatrix.cols());
-  squareMatrix = 0.5 * (squareMatrix + squareMatrix.transpose()).eval();
-  for (size_t i = 0; i < squareMatrix.rows(); i++) {
-    // Gershgorin radius: since the matrix is symmetric we use column sum instead of row sum
-    auto Ri = squareMatrix.col(i).cwiseAbs().sum() - std::abs(squareMatrix(i, i));
-    squareMatrix(i, i) = std::max(squareMatrix(i, i), Ri + minEigenvalue);
-  }
-}
+void makePsdGershgorin(matrix_t& squareMatrix, scalar_t minEigenvalue = numeric_traits::limitEpsilon<scalar_t>());
 
 /**
  * Makes the input matrix PSD based on modified Cholesky decomposition.
@@ -116,32 +83,19 @@ void makePsdGershgorin(Eigen::MatrixBase<Derived>& squareMatrix, scalar_t minEig
  * References : C-J. Lin and J. J. Moré, Incomplete Cholesky Factorizations with Limited memory, SIAM J. Sci. Comput.
  * 21(1), pp. 24-45, 1999
  *
- * @tparam Derived type.
- * @param A: The matrix to become PSD.
+ * @param [in, out] A: The matrix to become PSD.
  * @param [in] minEigenvalue: minimum eigenvalue.
  */
-template <typename Derived>
-void makePsdCholesky(Eigen::MatrixBase<Derived>& A, scalar_t minEigenvalue = numeric_traits::limitEpsilon<scalar_t>()) {
-  matrix_t mat = A;
-  makePsdCholesky(mat, minEigenvalue);
-  A = mat;
-}
+void makePsdCholesky(matrix_t& A, scalar_t minEigenvalue = numeric_traits::limitEpsilon<scalar_t>());
 
 /**
  * Computes the U*U^T decomposition associated to the inverse of the input matrix, where U is an upper triangular
  * matrix. Note that the U*U^T decomposition is different from the Cholesky decomposition (U^T*U).
  *
- * @tparam Derived type.
  * @param [in] Am: A symmetric square positive definite matrix
  * @param [out] AmInvUmUmT: The upper-triangular matrix associated to the UUT decomposition of inv(Am) matrix.
  */
-template <typename Derived>
-void computeInverseMatrixUUT(const Derived& Am, Derived& AmInvUmUmT) {
-  // Am = Lm Lm^T --> inv(Am) = inv(Lm^T) inv(Lm) where Lm^T is upper triangular
-  Eigen::LLT<Derived> lltOfA(Am);
-  AmInvUmUmT.setIdentity(Am.rows(), Am.cols());  // for dynamic size matrices
-  lltOfA.matrixU().solveInPlace(AmInvUmUmT);
-}
+void computeInverseMatrixUUT(const matrix_t& Am, matrix_t& AmInvUmUmT);
 
 /**
  * Computes constraint projection for linear constraints  C*x + D*u - e = 0, with the weighting inv(Rm)
@@ -154,11 +108,36 @@ void computeInverseMatrixUUT(const Derived& Am, Derived& AmInvUmUmT) {
  * @param [out] RmInvConstrainedUUT: The VVT decomposition of (I-DmDagger*Dm) * inv(Rm) * (I-DmDagger*Dm)^T where V is of
  * the dimension n_u*(n_u-n_c) with n_u = Rm.rows() and n_c = Dm.rows().
  */
-template <typename DerivedInputMatrix>
-void computeConstraintProjection(const matrix_t& Dm, const DerivedInputMatrix& RmInvUmUmT, matrix_t& DmDagger,
-                                 matrix_t& DmDaggerTRmDmDaggerUUT, matrix_t& RmInvConstrainedUUT) {
-  computeConstraintProjection(Dm, matrix_t(RmInvUmUmT), DmDagger, DmDaggerTRmDmDaggerUUT, RmInvConstrainedUUT);
-}
+void computeConstraintProjection(const matrix_t& Dm, const matrix_t& RmInvUmUmT, matrix_t& DmDagger, matrix_t& DmDaggerTRmDmDaggerUUT,
+                                 matrix_t& RmInvConstrainedUUT);
+
+/**
+ * Returns the linear projection
+ *  u = Pu * \tilde{u} + Px * x + Pe
+ *
+ * s.t. C*x + D*u + e = 0 is satisfied for any \tilde{u}
+ *
+ * Implementation based on the QR decomposition
+ *
+ * @param [in] constraint : C = dfdx, D = dfdu, e = f;
+ * @return Projection terms Px = dfdx, Pu = dfdu, Pe = f (first) and left pseudo-inverse of D^T (second);
+ */
+std::pair<VectorFunctionLinearApproximation, matrix_t> qrConstraintProjection(const VectorFunctionLinearApproximation& constraint);
+
+/**
+ * Returns the linear projection
+ *  u = Pu * \tilde{u} + Px * x + Pe
+ *
+ * s.t. C*x + D*u + e = 0 is satisfied for any \tilde{u}
+ *
+ * Implementation based on the LU decomposition
+ *
+ * @param [in] constraint : C = dfdx, D = dfdu, e = f;
+ * @param [in] extractPseudoInverse : If true, left pseudo-inverse of D^T is returned. If false, an empty matrix is returned;
+ * @return Projection terms Px = dfdx, Pu = dfdu, Pe = f (first) and left pseudo-inverse of D^T (second);
+ */
+std::pair<VectorFunctionLinearApproximation, matrix_t> luConstraintProjection(const VectorFunctionLinearApproximation& constraint,
+                                                                              bool extractPseudoInverse = false);
 
 /** Computes the rank of a matrix */
 template <typename Derived>