1 |
|
|
// |
2 |
|
|
// Copyright (c) 2017 CNRS |
3 |
|
|
// |
4 |
|
|
// This file is part of tsid |
5 |
|
|
// tsid is free software: you can redistribute it |
6 |
|
|
// and/or modify it under the terms of the GNU Lesser General Public |
7 |
|
|
// License as published by the Free Software Foundation, either version |
8 |
|
|
// 3 of the License, or (at your option) any later version. |
9 |
|
|
// tsid is distributed in the hope that it will be |
10 |
|
|
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty |
11 |
|
|
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
12 |
|
|
// General Lesser Public License for more details. You should have |
13 |
|
|
// received a copy of the GNU Lesser General Public License along with |
14 |
|
|
// tsid If not, see |
15 |
|
|
// <http://www.gnu.org/licenses/>. |
16 |
|
|
// |
17 |
|
|
|
18 |
|
|
#include <tsid/math/utils.hpp> |
19 |
|
|
|
20 |
|
|
namespace tsid { |
21 |
|
|
namespace math { |
22 |
|
|
|
23 |
|
|
void SE3ToXYZQUAT(const pinocchio::SE3 &M, RefVector xyzQuat) { |
24 |
|
|
PINOCCHIO_CHECK_INPUT_ARGUMENT( |
25 |
|
|
xyzQuat.size() == 7, "The size of the xyzQuat vector needs to equal 7"); |
26 |
|
|
xyzQuat.head<3>() = M.translation(); |
27 |
|
|
xyzQuat.tail<4>() = Eigen::Quaterniond(M.rotation()).coeffs(); |
28 |
|
|
} |
29 |
|
|
|
30 |
|
|
void SE3ToVector(const pinocchio::SE3 &M, RefVector vec) { |
31 |
|
|
PINOCCHIO_CHECK_INPUT_ARGUMENT( |
32 |
|
|
vec.size() == 12, "The size of the vec vector needs to equal 12"); |
33 |
|
|
vec.head<3>() = M.translation(); |
34 |
|
|
typedef Eigen::Matrix<double, 9, 1> Vector9; |
35 |
|
|
vec.tail<9>() = Eigen::Map<const Vector9>(&M.rotation()(0), 9); |
36 |
|
|
} |
37 |
|
|
|
38 |
|
|
void vectorToSE3(RefVector vec, pinocchio::SE3 &M) { |
39 |
|
|
PINOCCHIO_CHECK_INPUT_ARGUMENT(vec.size() == 12, |
40 |
|
|
"vec needs to contain 12 rows"); |
41 |
|
|
M.translation(vec.head<3>()); |
42 |
|
|
typedef Eigen::Matrix<double, 3, 3> Matrix3; |
43 |
|
|
M.rotation(Eigen::Map<const Matrix3>(&vec(3), 3, 3)); |
44 |
|
|
} |
45 |
|
|
|
46 |
|
|
void errorInSE3(const pinocchio::SE3 &M, const pinocchio::SE3 &Mdes, |
47 |
|
|
pinocchio::Motion &error) { |
48 |
|
|
// error = pinocchio::log6(Mdes.inverse() * M); |
49 |
|
|
// pinocchio::SE3 M_err = Mdes.inverse() * M; |
50 |
|
|
pinocchio::SE3 M_err = M.inverse() * Mdes; |
51 |
|
|
error.linear() = M_err.translation(); |
52 |
|
|
error.angular() = pinocchio::log3(M_err.rotation()); |
53 |
|
|
} |
54 |
|
|
|
55 |
|
|
void solveWithDampingFromSvd(Eigen::JacobiSVD<Eigen::MatrixXd> &svd, |
56 |
|
|
ConstRefVector b, RefVector sol, double damping) { |
57 |
|
|
assert(svd.rows() == b.size()); |
58 |
|
|
const double d2 = damping * damping; |
59 |
|
|
const long int nzsv = svd.nonzeroSingularValues(); |
60 |
|
|
Eigen::VectorXd tmp(svd.cols()); |
61 |
|
|
tmp.noalias() = svd.matrixU().leftCols(nzsv).adjoint() * b; |
62 |
|
|
double sv; |
63 |
|
|
for (long int i = 0; i < nzsv; i++) { |
64 |
|
|
sv = svd.singularValues()(i); |
65 |
|
|
tmp(i) *= sv / (sv * sv + d2); |
66 |
|
|
} |
67 |
|
|
sol = svd.matrixV().leftCols(nzsv) * tmp; |
68 |
|
|
// cout<<"sing val = "+toString(svd.singularValues(),3); |
69 |
|
|
// cout<<"solution with damp "+toString(damping)+" = "+toString(res.norm()); |
70 |
|
|
// cout<<"solution without damping ="+toString(svd.solve(b).norm()); |
71 |
|
|
} |
72 |
|
|
|
73 |
|
|
void svdSolveWithDamping(ConstRefMatrix A, ConstRefVector b, RefVector sol, |
74 |
|
|
double damping) { |
75 |
|
|
assert(A.rows() == b.size()); |
76 |
|
|
Eigen::JacobiSVD<Eigen::MatrixXd> svd(A.rows(), A.cols()); |
77 |
|
|
svd.compute(A, Eigen::ComputeThinU | Eigen::ComputeThinV); |
78 |
|
|
|
79 |
|
|
solveWithDampingFromSvd(svd, b, sol, damping); |
80 |
|
|
} |
81 |
|
|
|
82 |
|
|
void pseudoInverse(ConstRefMatrix A, RefMatrix Apinv, double tolerance, |
83 |
|
|
unsigned int computationOptions) |
84 |
|
|
|
85 |
|
|
{ |
86 |
|
|
Eigen::JacobiSVD<Eigen::MatrixXd> svdDecomposition(A.rows(), A.cols()); |
87 |
|
|
pseudoInverse(A, svdDecomposition, Apinv, tolerance, computationOptions); |
88 |
|
|
} |
89 |
|
|
|
90 |
|
|
void pseudoInverse(ConstRefMatrix A, |
91 |
|
|
Eigen::JacobiSVD<Eigen::MatrixXd> &svdDecomposition, |
92 |
|
|
RefMatrix Apinv, double tolerance, |
93 |
|
|
unsigned int computationOptions) { |
94 |
|
|
using namespace Eigen; |
95 |
|
|
int nullSpaceRows = -1, nullSpaceCols = -1; |
96 |
|
|
pseudoInverse(A, svdDecomposition, Apinv, tolerance, (double *)0, |
97 |
|
|
nullSpaceRows, nullSpaceCols, computationOptions); |
98 |
|
|
} |
99 |
|
|
|
100 |
|
|
void pseudoInverse(ConstRefMatrix A, |
101 |
|
|
Eigen::JacobiSVD<Eigen::MatrixXd> &svdDecomposition, |
102 |
|
|
RefMatrix Apinv, double tolerance, double *nullSpaceBasisOfA, |
103 |
|
|
int &nullSpaceRows, int &nullSpaceCols, |
104 |
|
|
unsigned int computationOptions) { |
105 |
|
|
using namespace Eigen; |
106 |
|
|
|
107 |
|
|
if (computationOptions == 0) |
108 |
|
|
return; // if no computation options we cannot compute the pseudo inverse |
109 |
|
|
svdDecomposition.compute(A, computationOptions); |
110 |
|
|
|
111 |
|
|
JacobiSVD<MatrixXd>::SingularValuesType singularValues = |
112 |
|
|
svdDecomposition.singularValues(); |
113 |
|
|
long int singularValuesSize = singularValues.size(); |
114 |
|
|
int rank = 0; |
115 |
|
|
for (long int idx = 0; idx < singularValuesSize; idx++) { |
116 |
|
|
if (tolerance > 0 && singularValues(idx) > tolerance) { |
117 |
|
|
singularValues(idx) = 1.0 / singularValues(idx); |
118 |
|
|
rank++; |
119 |
|
|
} else { |
120 |
|
|
singularValues(idx) = 0.0; |
121 |
|
|
} |
122 |
|
|
} |
123 |
|
|
|
124 |
|
|
// equivalent to this U/V matrix in case they are computed full |
125 |
|
|
Apinv = svdDecomposition.matrixV().leftCols(singularValuesSize) * |
126 |
|
|
singularValues.asDiagonal() * |
127 |
|
|
svdDecomposition.matrixU().leftCols(singularValuesSize).adjoint(); |
128 |
|
|
|
129 |
|
|
if (nullSpaceBasisOfA && (computationOptions & ComputeFullV)) { |
130 |
|
|
// we can compute the null space basis for A |
131 |
|
|
nullSpaceBasisFromDecomposition(svdDecomposition, rank, nullSpaceBasisOfA, |
132 |
|
|
nullSpaceRows, nullSpaceCols); |
133 |
|
|
} |
134 |
|
|
} |
135 |
|
|
|
136 |
|
|
void dampedPseudoInverse(ConstRefMatrix A, |
137 |
|
|
Eigen::JacobiSVD<Eigen::MatrixXd> &svdDecomposition, |
138 |
|
|
RefMatrix Apinv, double tolerance, |
139 |
|
|
double dampingFactor, unsigned int computationOptions, |
140 |
|
|
double *nullSpaceBasisOfA, int *nullSpaceRows, |
141 |
|
|
int *nullSpaceCols) { |
142 |
|
|
using namespace Eigen; |
143 |
|
|
|
144 |
|
|
if (computationOptions == 0) |
145 |
|
|
return; // if no computation options we cannot compute the pseudo inverse |
146 |
|
|
svdDecomposition.compute(A, computationOptions); |
147 |
|
|
|
148 |
|
|
JacobiSVD<MatrixXd>::SingularValuesType singularValues = |
149 |
|
|
svdDecomposition.singularValues(); |
150 |
|
|
|
151 |
|
|
// rank will be used for the null space basis. |
152 |
|
|
// not sure if this is correct |
153 |
|
|
const long int singularValuesSize = singularValues.size(); |
154 |
|
|
const double d2 = dampingFactor * dampingFactor; |
155 |
|
|
int rank = 0; |
156 |
|
|
for (int idx = 0; idx < singularValuesSize; idx++) { |
157 |
|
|
if (singularValues(idx) > tolerance) rank++; |
158 |
|
|
singularValues(idx) = singularValues(idx) / |
159 |
|
|
((singularValues(idx) * singularValues(idx)) + d2); |
160 |
|
|
} |
161 |
|
|
|
162 |
|
|
// equivalent to this U/V matrix in case they are computed full |
163 |
|
|
Apinv = svdDecomposition.matrixV().leftCols(singularValuesSize) * |
164 |
|
|
singularValues.asDiagonal() * |
165 |
|
|
svdDecomposition.matrixU().leftCols(singularValuesSize).adjoint(); |
166 |
|
|
|
167 |
|
|
if (nullSpaceBasisOfA && nullSpaceRows && nullSpaceCols && |
168 |
|
|
(computationOptions & ComputeFullV)) { |
169 |
|
|
// we can compute the null space basis for A |
170 |
|
|
nullSpaceBasisFromDecomposition(svdDecomposition, rank, nullSpaceBasisOfA, |
171 |
|
|
*nullSpaceRows, *nullSpaceCols); |
172 |
|
|
} |
173 |
|
|
} |
174 |
|
|
|
175 |
|
|
void nullSpaceBasisFromDecomposition( |
176 |
|
|
const Eigen::JacobiSVD<Eigen::MatrixXd> &svdDecomposition, double tolerance, |
177 |
|
|
double *nullSpaceBasisMatrix, int &rows, int &cols) { |
178 |
|
|
using namespace Eigen; |
179 |
|
|
JacobiSVD<MatrixXd>::SingularValuesType singularValues = |
180 |
|
|
svdDecomposition.singularValues(); |
181 |
|
|
int rank = 0; |
182 |
|
|
for (int idx = 0; idx < singularValues.size(); idx++) { |
183 |
|
|
if (tolerance > 0 && singularValues(idx) > tolerance) { |
184 |
|
|
rank++; |
185 |
|
|
} |
186 |
|
|
} |
187 |
|
|
nullSpaceBasisFromDecomposition(svdDecomposition, rank, nullSpaceBasisMatrix, |
188 |
|
|
rows, cols); |
189 |
|
|
} |
190 |
|
|
|
191 |
|
|
void nullSpaceBasisFromDecomposition( |
192 |
|
|
const Eigen::JacobiSVD<Eigen::MatrixXd> &svdDecomposition, int rank, |
193 |
|
|
double *nullSpaceBasisMatrix, int &rows, int &cols) { |
194 |
|
|
using namespace Eigen; |
195 |
|
|
const MatrixXd &vMatrix = svdDecomposition.matrixV(); |
196 |
|
|
// A \in \mathbb{R}^{uMatrix.rows() \times vMatrix.cols()} |
197 |
|
|
rows = (int)vMatrix.cols(); |
198 |
|
|
cols = (int)vMatrix.cols() - rank; |
199 |
|
|
Map<MatrixXd> map(nullSpaceBasisMatrix, rows, cols); |
200 |
|
|
map = vMatrix.rightCols(vMatrix.cols() - rank); |
201 |
|
|
} |
202 |
|
|
|
203 |
|
|
} // namespace math |
204 |
|
|
} // namespace tsid |