| Line |
Branch |
Exec |
Source |
| 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 |
| 205 |
|
|
|