hpp-constraints  4.15.1
Definition of basic geometric constraints for motion planning
svd.hh
Go to the documentation of this file.
1 // Copyright (c) 2015 CNRS
2 // Author: Joseph Mirabel
3 //
4 //
5 
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are
8 // met:
9 //
10 // 1. Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 //
13 // 2. Redistributions in binary form must reproduce the above copyright
14 // notice, this list of conditions and the following disclaimer in the
15 // documentation and/or other materials provided with the distribution.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28 // DAMAGE.
29 
30 #ifndef HPP_CONSTRAINTS_SVD_HH
31 #define HPP_CONSTRAINTS_SVD_HH
32 
33 #include <Eigen/SVD>
34 #include <hpp/constraints/fwd.hh>
35 
36 namespace hpp {
37 namespace constraints {
38 template <typename SVD>
39 static Eigen::Ref<const typename SVD::MatrixUType> getU1(
40  const SVD& svd, const size_type& rank) {
41  return svd.matrixU().leftCols(rank);
42 }
43 
44 template <typename SVD>
45 static Eigen::Ref<const typename SVD::MatrixUType> getU2(
46  const SVD& svd, const size_type& rank) {
47  return svd.matrixU().rightCols(svd.matrixU().cols() - rank);
48 }
49 
50 template <typename SVD>
51 static Eigen::Ref<const typename SVD::MatrixUType> getV1(
52  const SVD& svd, const size_type& rank) {
53  return svd.matrixV().leftCols(rank);
54 }
55 
56 template <typename SVD>
57 static Eigen::Ref<const typename SVD::MatrixUType> getV2(
58  const SVD& svd, const size_type& rank) {
59  return svd.matrixV().rightCols(svd.matrixV().cols() - rank);
60 }
61 
62 template <typename SVD>
63 static void pseudoInverse(const SVD& svd,
64  Eigen::Ref<typename SVD::MatrixType> pinvmat) {
65  eigen_assert(
66  svd.computeU() && svd.computeV() &&
67  "Eigen::JacobiSVD "
68  "computation flags must be at least: ComputeThinU | ComputeThinV");
69 
70  size_type rank = svd.rank();
71  typename SVD::SingularValuesType singularValues_inv =
72  svd.singularValues().segment(0, rank).cwiseInverse();
73 
74  pinvmat.noalias() = getV1<SVD>(svd, rank) * singularValues_inv.asDiagonal() *
75  getU1<SVD>(svd, rank).adjoint();
76 }
77 
78 template <typename SVD>
79 void projectorOnSpan(const SVD& svd,
80  Eigen::Ref<typename SVD::MatrixType> projector) {
81  eigen_assert(
82  svd.computeU() && svd.computeV() &&
83  "Eigen::JacobiSVD "
84  "computation flags must be at least: ComputeThinU | ComputeThinV");
85 
86  size_type rank = svd.rank();
87  projector.noalias() = getV1<SVD>(svd, rank) * getV1<SVD>(svd, rank).adjoint();
88 }
89 
90 template <typename SVD>
91 void projectorOnSpanOfInv(const SVD& svd,
92  Eigen::Ref<typename SVD::MatrixType> projector) {
93  eigen_assert(
94  svd.computeU() && svd.computeV() &&
95  "Eigen::JacobiSVD "
96  "computation flags must be at least: ComputeThinU | ComputeThinV");
97 
98  size_type rank = svd.rank();
99  projector.noalias() = getU1<SVD>(svd, rank) * getU1<SVD>(svd, rank).adjoint();
100 }
101 
102 template <typename SVD>
103 void projectorOnKernel(const SVD& svd,
104  Eigen::Ref<typename SVD::MatrixType> projector,
105  const bool& computeFullV = false) {
106  eigen_assert(svd.computeV() &&
107  "Eigen::JacobiSVD "
108  "computation flags must be at least: ComputeThinV");
109 
110  size_type rank = svd.rank();
111  if (computeFullV)
112  projector.noalias() =
113  getV2<SVD>(svd, rank) * getV2<SVD>(svd, rank).adjoint();
114  else {
115  projector.noalias() =
116  -getV1<SVD>(svd, rank) * getV1<SVD>(svd, rank).adjoint();
117  projector.diagonal().noalias() += vector_t::Ones(svd.matrixV().rows());
118  }
119 }
120 
121 template <typename SVD>
122 void projectorOnKernelOfInv(const SVD& svd,
123  Eigen::Ref<typename SVD::MatrixType> projector,
124  const bool& computeFullU = false) {
125  eigen_assert(svd.computeU() &&
126  "Eigen::JacobiSVD "
127  "computation flags must be at least: ComputeThinU");
128 
129  size_type rank = svd.rank();
130  if (computeFullU) {
131  // U2 * U2*
132  projector.noalias() =
133  getU2<SVD>(svd, rank) * getU2<SVD>(svd, rank).adjoint();
134  } else {
135  // I - U1 * U1*
136  projector.noalias() =
137  -getU1<SVD>(svd, rank) * getU1<SVD>(svd, rank).adjoint();
138  projector.diagonal().noalias() += vector_t::Ones(svd.matrixU().rows());
139  }
140 }
141 } // namespace constraints
142 } // namespace hpp
143 
144 #endif // HPP_CONSTRAINTS_SVD_HH
Definition: active-set-differentiable-function.hh:36
void projectorOnKernel(const SVD &svd, Eigen::Ref< typename SVD::MatrixType > projector, const bool &computeFullV=false)
Definition: svd.hh:103
void projectorOnSpanOfInv(const SVD &svd, Eigen::Ref< typename SVD::MatrixType > projector)
Definition: svd.hh:91
void projectorOnKernelOfInv(const SVD &svd, Eigen::Ref< typename SVD::MatrixType > projector, const bool &computeFullU=false)
Definition: svd.hh:122
pinocchio::size_type size_type
Definition: fwd.hh:47
void projectorOnSpan(const SVD &svd, Eigen::Ref< typename SVD::MatrixType > projector)
Definition: svd.hh:79