@@ -38,41 +38,18 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
3838namespace ocs2 {
3939namespace LinearAlgebra {
4040
41- // forward declarations
42- void makePsdEigenvalue (matrix_t & squareMatrix, scalar_t minEigenvalue);
43-
44- void makePsdCholesky (matrix_t & A, scalar_t minEigenvalue);
45-
46- void computeConstraintProjection (const matrix_t & Dm, const matrix_t & RmInvUmUmT, matrix_t & DmDagger, matrix_t & DmDaggerTRmDmDaggerUUT,
47- matrix_t & RmInvConstrainedUUT);
48-
4941/* *
5042 * Set the eigenvalues of a triangular matrix to a minimum magnitude (maintaining the sign).
5143 */
52- inline void setTriangularMinimumEigenvalues (matrix_t & Lr, scalar_t minEigenValue = numeric_traits::weakEpsilon<scalar_t >()) {
53- for (Eigen::Index i = 0 ; i < Lr.rows (); ++i) {
54- scalar_t & eigenValue = Lr (i, i); // diagonal element is the eigenvalue
55- if (eigenValue < 0.0 ) {
56- eigenValue = std::min (-minEigenValue, eigenValue);
57- } else {
58- eigenValue = std::max (minEigenValue, eigenValue);
59- }
60- }
61- }
44+ void setTriangularMinimumEigenvalues (matrix_t & Lr, scalar_t minEigenValue = numeric_traits::weakEpsilon<scalar_t >());
6245
6346/* *
6447 * Makes the input matrix PSD using a eigenvalue decomposition.
6548 *
66- * @tparam Derived type.
67- * @param squareMatrix: The matrix to become PSD.
49+ * @param [in, out] squareMatrix: The matrix to become PSD.
6850 * @param [in] minEigenvalue: minimum eigenvalue.
6951 */
70- template <typename Derived>
71- void makePsdEigenvalue (Eigen::MatrixBase<Derived>& squareMatrix, scalar_t minEigenvalue = numeric_traits::limitEpsilon<scalar_t >()) {
72- matrix_t mat = squareMatrix;
73- makePsdEigenvalue (mat, minEigenvalue);
74- squareMatrix = mat;
75- }
52+ void makePsdEigenvalue (matrix_t & squareMatrix, scalar_t minEigenvalue = numeric_traits::limitEpsilon<scalar_t >());
7653
7754/* *
7855 * 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
8966 * (1) Aii < minEigenvalue + Ri ==> minEigenvalue < lambda < minEigenvalue + 2 Ri
9067 * (2) Aii > minEigenvalue + Ri ==> minEigenvalue < Aii - Ri < lambda < Aii + Ri
9168 *
92- * @tparam Derived type.
93- * @param squareMatrix: The matrix to become PSD.
69+ * @param [in, out] squareMatrix: The matrix to become PSD.
9470 * @param [in] minEigenvalue: minimum eigenvalue.
9571 */
96- template <typename Derived>
97- void makePsdGershgorin (Eigen::MatrixBase<Derived>& squareMatrix, scalar_t minEigenvalue = numeric_traits::limitEpsilon<scalar_t >()) {
98- assert (squareMatrix.rows () == squareMatrix.cols ());
99- squareMatrix = 0.5 * (squareMatrix + squareMatrix.transpose ()).eval ();
100- for (size_t i = 0 ; i < squareMatrix.rows (); i++) {
101- // Gershgorin radius: since the matrix is symmetric we use column sum instead of row sum
102- auto Ri = squareMatrix.col (i).cwiseAbs ().sum () - std::abs (squareMatrix (i, i));
103- squareMatrix (i, i) = std::max (squareMatrix (i, i), Ri + minEigenvalue);
104- }
105- }
72+ void makePsdGershgorin (matrix_t & squareMatrix, scalar_t minEigenvalue = numeric_traits::limitEpsilon<scalar_t >());
10673
10774/* *
10875 * Makes the input matrix PSD based on modified Cholesky decomposition.
@@ -116,32 +83,19 @@ void makePsdGershgorin(Eigen::MatrixBase<Derived>& squareMatrix, scalar_t minEig
11683 * References : C-J. Lin and J. J. Moré, Incomplete Cholesky Factorizations with Limited memory, SIAM J. Sci. Comput.
11784 * 21(1), pp. 24-45, 1999
11885 *
119- * @tparam Derived type.
120- * @param A: The matrix to become PSD.
86+ * @param [in, out] A: The matrix to become PSD.
12187 * @param [in] minEigenvalue: minimum eigenvalue.
12288 */
123- template <typename Derived>
124- void makePsdCholesky (Eigen::MatrixBase<Derived>& A, scalar_t minEigenvalue = numeric_traits::limitEpsilon<scalar_t >()) {
125- matrix_t mat = A;
126- makePsdCholesky (mat, minEigenvalue);
127- A = mat;
128- }
89+ void makePsdCholesky (matrix_t & A, scalar_t minEigenvalue = numeric_traits::limitEpsilon<scalar_t >());
12990
13091/* *
13192 * Computes the U*U^T decomposition associated to the inverse of the input matrix, where U is an upper triangular
13293 * matrix. Note that the U*U^T decomposition is different from the Cholesky decomposition (U^T*U).
13394 *
134- * @tparam Derived type.
13595 * @param [in] Am: A symmetric square positive definite matrix
13696 * @param [out] AmInvUmUmT: The upper-triangular matrix associated to the UUT decomposition of inv(Am) matrix.
13797 */
138- template <typename Derived>
139- void computeInverseMatrixUUT (const Derived& Am, Derived& AmInvUmUmT) {
140- // Am = Lm Lm^T --> inv(Am) = inv(Lm^T) inv(Lm) where Lm^T is upper triangular
141- Eigen::LLT<Derived> lltOfA (Am);
142- AmInvUmUmT.setIdentity (Am.rows (), Am.cols ()); // for dynamic size matrices
143- lltOfA.matrixU ().solveInPlace (AmInvUmUmT);
144- }
98+ void computeInverseMatrixUUT (const matrix_t & Am, matrix_t & AmInvUmUmT);
14599
146100/* *
147101 * 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) {
154108 * @param [out] RmInvConstrainedUUT: The VVT decomposition of (I-DmDagger*Dm) * inv(Rm) * (I-DmDagger*Dm)^T where V is of
155109 * the dimension n_u*(n_u-n_c) with n_u = Rm.rows() and n_c = Dm.rows().
156110 */
157- template <typename DerivedInputMatrix>
158- void computeConstraintProjection (const matrix_t & Dm, const DerivedInputMatrix& RmInvUmUmT, matrix_t & DmDagger,
159- matrix_t & DmDaggerTRmDmDaggerUUT, matrix_t & RmInvConstrainedUUT) {
160- computeConstraintProjection (Dm, matrix_t (RmInvUmUmT), DmDagger, DmDaggerTRmDmDaggerUUT, RmInvConstrainedUUT);
161- }
111+ void computeConstraintProjection (const matrix_t & Dm, const matrix_t & RmInvUmUmT, matrix_t & DmDagger, matrix_t & DmDaggerTRmDmDaggerUUT,
112+ matrix_t & RmInvConstrainedUUT);
113+
114+ /* *
115+ * Returns the linear projection
116+ * u = Pu * \tilde{u} + Px * x + Pe
117+ *
118+ * s.t. C*x + D*u + e = 0 is satisfied for any \tilde{u}
119+ *
120+ * Implementation based on the QR decomposition
121+ *
122+ * @param [in] constraint : C = dfdx, D = dfdu, e = f;
123+ * @return Projection terms Px = dfdx, Pu = dfdu, Pe = f (first) and left pseudo-inverse of D^T (second);
124+ */
125+ std::pair<VectorFunctionLinearApproximation, matrix_t > qrConstraintProjection (const VectorFunctionLinearApproximation& constraint);
126+
127+ /* *
128+ * Returns the linear projection
129+ * u = Pu * \tilde{u} + Px * x + Pe
130+ *
131+ * s.t. C*x + D*u + e = 0 is satisfied for any \tilde{u}
132+ *
133+ * Implementation based on the LU decomposition
134+ *
135+ * @param [in] constraint : C = dfdx, D = dfdu, e = f;
136+ * @param [in] extractPseudoInverse : If true, left pseudo-inverse of D^T is returned. If false, an empty matrix is returned;
137+ * @return Projection terms Px = dfdx, Pu = dfdu, Pe = f (first) and left pseudo-inverse of D^T (second);
138+ */
139+ std::pair<VectorFunctionLinearApproximation, matrix_t > luConstraintProjection (const VectorFunctionLinearApproximation& constraint,
140+ bool extractPseudoInverse = false );
162141
163142/* * Computes the rank of a matrix */
164143template <typename Derived>
0 commit comments