Merge branch 'main' into feature/constraint_array

Author: farbod-farshidian

.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 checkinstall
+        apt-get install -y git ninja-build liburdfdom-dev liboctomap-dev libassimp-dev checkinstall wget rsync
 
     - uses: actions/checkout@v2
       with: 
@@ -56,6 +56,14 @@ jobs:
         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: |

.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,6 +16,7 @@
   <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>
@@ -25,6 +26,7 @@
   <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/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/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>

ocs2_core/include/ocs2_core/model_data/Metrics.h

@@ -58,6 +58,8 @@ struct LagrangianMetricsConstRef {
  * cost : The total cost in a particular time point.
  * stateEqConstraint : An array of all state equality constraints.
  * stateInputEqConstraint : An array of all state-input equality constraints.
+ * stateIneqConstraint : An array of all state inequality constraints.
+ * stateInputIneqConstraint : An array of all state-input inequality constraints.
  * stateEqLagrangian : An array of state equality constraint terms handled by Lagrangian method.
  * stateIneqLagrangian : An array of state inequality constraint terms handled by Lagrangian method.
  * stateInputEqLagrangian : An array of state-input equality constraint terms handled by Lagrangian method.
@@ -74,6 +76,10 @@ struct Metrics {
   vector_array_t stateEqConstraint;
   vector_array_t stateInputEqConstraint;
 
+  // Inequality constraints
+  vector_array_t stateIneqConstraint;
+  vector_array_t stateInputIneqConstraint;
+
   // Lagrangians
   std::vector<LagrangianMetrics> stateEqLagrangian;
   std::vector<LagrangianMetrics> stateIneqLagrangian;
@@ -89,6 +95,9 @@ struct Metrics {
     // Equality constraints
     stateEqConstraint.swap(other.stateEqConstraint);
     stateInputEqConstraint.swap(other.stateInputEqConstraint);
+    // Inequality constraints
+    stateIneqConstraint.swap(other.stateIneqConstraint);
+    stateInputIneqConstraint.swap(other.stateInputIneqConstraint);
     // Lagrangians
     stateEqLagrangian.swap(other.stateEqLagrangian);
     stateIneqLagrangian.swap(other.stateIneqLagrangian);
@@ -105,6 +114,9 @@ struct Metrics {
     // Equality constraints
     stateEqConstraint.clear();
     stateInputEqConstraint.clear();
+    // Inequality constraints
+    stateIneqConstraint.clear();
+    stateInputIneqConstraint.clear();
     // Lagrangians
     stateEqLagrangian.clear();
     stateIneqLagrangian.clear();
@@ -185,7 +197,7 @@ std::vector<LagrangianMetrics> toLagrangianMetrics(const size_array_t& termsSize
  * @param [in] termsSize : An array of constraint terms size. It as the same size as the output array.
  * @param [in] vec : Serialized array of constraint terms of the format :
  *                   (..., constraintArray[i], ...)
- * @return An array of LagrangianMetrics structures associated to an array of constraint terms
+ * @return An array of constraint terms.
  */
 vector_array_t toConstraintArray(const size_array_t& termsSize, const vector_t& vec);
 

ocs2_core/include/ocs2_core/penalties/augmented/ModifiedRelaxedBarrierPenalty.h

@@ -76,7 +76,7 @@ class ModifiedRelaxedBarrierPenalty final : public AugmentedPenaltyBase {
 
   /** Factory function */
   static std::unique_ptr<ModifiedRelaxedBarrierPenalty> create(Config config) {
-    return std::unique_ptr<ModifiedRelaxedBarrierPenalty>(new ModifiedRelaxedBarrierPenalty(std::move(config)));
+    return std::make_unique<ModifiedRelaxedBarrierPenalty>(std::move(config));
   }
 
   ~ModifiedRelaxedBarrierPenalty() override = default;

ocs2_core/include/ocs2_core/penalties/augmented/QuadraticPenalty.h

@@ -72,9 +72,7 @@ class QuadraticPenalty final : public AugmentedPenaltyBase {
   explicit QuadraticPenalty(Config config) : config_(std::move(config)) {}
 
   /** Factory function */
-  static std::unique_ptr<QuadraticPenalty> create(Config config) {
-    return std::unique_ptr<QuadraticPenalty>(new QuadraticPenalty(std::move(config)));
-  }
+  static std::unique_ptr<QuadraticPenalty> create(Config config) { return std::make_unique<QuadraticPenalty>(std::move(config)); }
 
   ~QuadraticPenalty() override = default;
   QuadraticPenalty* clone() const override { return new QuadraticPenalty(*this); }