hpp-constraints: include/hpp/constraints/svd.hh Source File

hpp-constraints 9.0.2 Definition of basic geometric constraints for motion planning

Loading...
Searching...
No Matches
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
fwd.hh
hpp::constraints::projectorOnSpan
void projectorOnSpan(const SVD &svd, Eigen::Ref< typename SVD::MatrixType > projector)
Definition svd.hh:79
hpp::constraints::projectorOnKernelOfInv
void projectorOnKernelOfInv(const SVD &svd, Eigen::Ref< typename SVD::MatrixType > projector, const bool &computeFullU=false)
Definition svd.hh:122
hpp::constraints::size_type
pinocchio::size_type size_type
Definition fwd.hh:48
hpp::constraints::projectorOnSpanOfInv
void projectorOnSpanOfInv(const SVD &svd, Eigen::Ref< typename SVD::MatrixType > projector)
Definition svd.hh:91
hpp::constraints::projectorOnKernel
void projectorOnKernel(const SVD &svd, Eigen::Ref< typename SVD::MatrixType > projector, const bool &computeFullV=false)
Definition svd.hh:103
hpp