utils.cpp 8.53 KB
Newer Older
1
2
3
//
// Copyright (c) 2017 CNRS
//
jcarpent's avatar
jcarpent committed
4
5
// This file is part of tsid
// tsid is free software: you can redistribute it
6
7
8
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
jcarpent's avatar
jcarpent committed
9
// tsid is distributed in the hope that it will be
10
11
12
13
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
jcarpent's avatar
jcarpent committed
14
// tsid If not, see
15
16
17
// <http://www.gnu.org/licenses/>.
//

jcarpent's avatar
jcarpent committed
18
#include <tsid/math/utils.hpp>
19

jcarpent's avatar
jcarpent committed
20
namespace tsid
21
{
22
  namespace math
23
  {
24
    
25
    void SE3ToXYZQUAT(const pinocchio::SE3 & M, RefVector xyzQuat)
26
27
28
29
    {
      assert(xyzQuat.size()==7);
      xyzQuat.head<3>() = M.translation();
      xyzQuat.tail<4>() = Eigen::Quaterniond(M.rotation()).coeffs();
30
    }
31

32
    void SE3ToVector(const pinocchio::SE3 & M, RefVector vec)
33
34
35
36
37
38
    {
      assert(vec.size()==12);
      vec.head<3>() = M.translation();
      typedef Eigen::Matrix<double,9,1> Vector9;
      vec.tail<9>() = Eigen::Map<const Vector9>(&M.rotation()(0), 9);
    }
39

40
    void vectorToSE3(RefVector vec, pinocchio::SE3 & M)
41
42
43
44
45
46
47
    {
      assert(vec.size()==12);
      M.translation( vec.head<3>() );
      typedef Eigen::Matrix<double,3,3> Matrix3;
      M.rotation( Eigen::Map<const Matrix3>(&vec(3), 3, 3) );
    }
    
48
49
50
    void errorInSE3 (const pinocchio::SE3 & M,
                     const pinocchio::SE3 & Mdes,
                     pinocchio::Motion & error)
51
    {
52
      // error = pinocchio::log6(Mdes.inverse() * M);
53
54
      // pinocchio::SE3 M_err = Mdes.inverse() * M;
      pinocchio::SE3 M_err = M.inverse() * Mdes;
55
56
      error.linear() = M_err.translation();
      error.angular() = pinocchio::log3(M_err.rotation());
57
58
    }
    
jcarpent's avatar
jcarpent committed
59
    void solveWithDampingFromSvd(Eigen::JacobiSVD<Eigen::MatrixXd> & svd,
60
61
                                 ConstRefVector b,
                                 RefVector sol, double damping)
62
    {
63
64
65
66
      assert(svd.rows()==b.size());
      const double d2 = damping*damping;
      const long int nzsv = svd.nonzeroSingularValues();
      Eigen::VectorXd tmp(svd.cols());
67
      tmp.noalias() = svd.matrixU().leftCols(nzsv).adjoint() * b;
68
69
      double sv;
      for(long int i=0; i<nzsv; i++)
70
71
72
73
74
75
76
77
78
      {
        sv = svd.singularValues()(i);
        tmp(i) *= sv/(sv*sv + d2);
      }
      sol = svd.matrixV().leftCols(nzsv) * tmp;
      //  cout<<"sing val = "+toString(svd.singularValues(),3);
      //  cout<<"solution with damp "+toString(damping)+" = "+toString(res.norm());
      //  cout<<"solution without damping  ="+toString(svd.solve(b).norm());
    }
79
80
81
82
83

    void svdSolveWithDamping(ConstRefMatrix A, ConstRefVector b,
                             RefVector sol, double damping)
    {
      assert(A.rows()==b.size());
jcarpent's avatar
jcarpent committed
84
      Eigen::JacobiSVD<Eigen::MatrixXd> svd(A.rows(), A.cols());
85
86
87
88
      svd.compute(A, Eigen::ComputeThinU | Eigen::ComputeThinV);

      solveWithDampingFromSvd(svd, b, sol, damping);
    }
89
90
91
92
93
94
95
    
    void pseudoInverse(ConstRefMatrix A,
                       RefMatrix Apinv,
                       double tolerance,
                       unsigned int computationOptions)
    
    {
jcarpent's avatar
jcarpent committed
96
      Eigen::JacobiSVD<Eigen::MatrixXd> svdDecomposition(A.rows(), A.cols());
97
98
99
100
      pseudoInverse(A, svdDecomposition, Apinv, tolerance, computationOptions);
    }
    
    void pseudoInverse(ConstRefMatrix A,
jcarpent's avatar
jcarpent committed
101
                       Eigen::JacobiSVD<Eigen::MatrixXd> & svdDecomposition,
102
103
104
105
106
107
108
109
110
111
112
                       RefMatrix Apinv,
                       double tolerance,
                       unsigned int computationOptions)
    {
      using namespace Eigen;
      int nullSpaceRows = -1, nullSpaceCols = -1;
      pseudoInverse(A, svdDecomposition, Apinv, tolerance,
                    (double*)0, nullSpaceRows, nullSpaceCols, computationOptions);
    }
    
    void pseudoInverse(ConstRefMatrix A,
jcarpent's avatar
jcarpent committed
113
                       Eigen::JacobiSVD<Eigen::MatrixXd> & svdDecomposition,
114
115
116
117
118
119
120
121
122
123
124
                       RefMatrix Apinv,
                       double tolerance,
                       double * nullSpaceBasisOfA,
                       int &nullSpaceRows, int &nullSpaceCols,
                       unsigned int computationOptions)
    {
      using namespace Eigen;
      
      if (computationOptions == 0) return; //if no computation options we cannot compute the pseudo inverse
      svdDecomposition.compute(A, computationOptions);
      
jcarpent's avatar
jcarpent committed
125
      JacobiSVD<MatrixXd>::SingularValuesType singularValues = svdDecomposition.singularValues();
126
      long int singularValuesSize = singularValues.size();
127
      int rank = 0;
128
      for (long int idx = 0; idx < singularValuesSize; idx++) {
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
        if (tolerance > 0 && singularValues(idx) > tolerance) {
          singularValues(idx) = 1.0 / singularValues(idx);
          rank++;
        } else {
          singularValues(idx) = 0.0;
        }
      }
      
      //equivalent to this U/V matrix in case they are computed full
      Apinv = svdDecomposition.matrixV().leftCols(singularValuesSize) * singularValues.asDiagonal() * svdDecomposition.matrixU().leftCols(singularValuesSize).adjoint();
      
      if (nullSpaceBasisOfA && (computationOptions & ComputeFullV)) {
        //we can compute the null space basis for A
        nullSpaceBasisFromDecomposition(svdDecomposition, rank, nullSpaceBasisOfA, nullSpaceRows, nullSpaceCols);
      }
144
    }
145
146
    
    void dampedPseudoInverse(ConstRefMatrix A,
jcarpent's avatar
jcarpent committed
147
                             Eigen::JacobiSVD<Eigen::MatrixXd>& svdDecomposition,
148
149
150
151
152
153
154
155
156
157
158
159
                             RefMatrix Apinv,
                             double tolerance,
                             double dampingFactor,
                             unsigned int computationOptions,
                             double * nullSpaceBasisOfA,
                             int *nullSpaceRows, int *nullSpaceCols)
    {
      using namespace Eigen;
      
      if (computationOptions == 0) return; //if no computation options we cannot compute the pseudo inverse
      svdDecomposition.compute(A, computationOptions);
      
jcarpent's avatar
jcarpent committed
160
      JacobiSVD<MatrixXd>::SingularValuesType singularValues = svdDecomposition.singularValues();
161
162
163
      
      //rank will be used for the null space basis.
      //not sure if this is correct
164
      const long int singularValuesSize = singularValues.size();
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
      const double d2 = dampingFactor * dampingFactor;
      int rank = 0;
      for (int idx = 0; idx < singularValuesSize; idx++)
      {
        if(singularValues(idx) > tolerance)
          rank++;
        singularValues(idx) = singularValues(idx) / ((singularValues(idx) * singularValues(idx)) + d2);
      }
      
      //equivalent to this U/V matrix in case they are computed full
      Apinv = svdDecomposition.matrixV().leftCols(singularValuesSize) * singularValues.asDiagonal() * svdDecomposition.matrixU().leftCols(singularValuesSize).adjoint();
      
      if (nullSpaceBasisOfA && nullSpaceRows && nullSpaceCols
          && (computationOptions & ComputeFullV))
      {
        //we can compute the null space basis for A
        nullSpaceBasisFromDecomposition(svdDecomposition, rank, nullSpaceBasisOfA, *nullSpaceRows, *nullSpaceCols);
      }
    }
    
jcarpent's avatar
jcarpent committed
185
    void nullSpaceBasisFromDecomposition(const Eigen::JacobiSVD<Eigen::MatrixXd>& svdDecomposition,
186
187
188
189
190
                                         double tolerance,
                                         double * nullSpaceBasisMatrix,
                                         int &rows, int &cols)
    {
      using namespace Eigen;
jcarpent's avatar
jcarpent committed
191
      JacobiSVD<MatrixXd>::SingularValuesType singularValues = svdDecomposition.singularValues();
192
193
194
195
196
197
198
199
200
201
      int rank = 0;
      for (int idx = 0; idx < singularValues.size(); idx++) {
        if (tolerance > 0 && singularValues(idx) > tolerance) {
          rank++;
        }
      }
      nullSpaceBasisFromDecomposition(svdDecomposition, rank, nullSpaceBasisMatrix, rows, cols);
      
    }
    
jcarpent's avatar
jcarpent committed
202
    void nullSpaceBasisFromDecomposition(const Eigen::JacobiSVD<Eigen::MatrixXd> & svdDecomposition,
203
204
205
206
207
208
209
                                         int rank,
                                         double * nullSpaceBasisMatrix,
                                         int &rows, int &cols)
    {
      using namespace Eigen;
      const MatrixXd &vMatrix = svdDecomposition.matrixV();
      //A \in \mathbb{R}^{uMatrix.rows() \times vMatrix.cols()}
210
211
      rows = (int) vMatrix.cols();
      cols = (int) vMatrix.cols() - rank;
212
213
214
215
      Map<MatrixXd> map(nullSpaceBasisMatrix, rows, cols);
      map = vMatrix.rightCols(vMatrix.cols() - rank);
    }
    
216
217
218
  }
}

219