Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Humanoid Path Planner
hpp-model
Commits
0f53ce3b
Commit
0f53ce3b
authored
Oct 27, 2015
by
Joseph Mirabel
Committed by
Joseph Mirabel
Oct 27, 2015
Browse files
Move hpp/model/eigen.hh to hpp/constraints/svd.hh
parent
879c7880
Changes
4
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
0f53ce3b
...
@@ -61,7 +61,6 @@ SET(${PROJECT_NAME}_HEADERS
...
@@ -61,7 +61,6 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/model/gripper.hh
include/hpp/model/gripper.hh
include/hpp/model/center-of-mass-computation.hh
include/hpp/model/center-of-mass-computation.hh
include/hpp/model/debug.hh
include/hpp/model/debug.hh
include/hpp/model/eigen.hh
)
)
# Declare dependencies
# Declare dependencies
...
...
include/hpp/model/eigen.hh
deleted
100644 → 0
View file @
879c7880
// Copyright (c) 2015 CNRS
// Author: Joseph Mirabel
//
//
// This file is part of hpp-model
// hpp-model is free software: you can redistribute it
// 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.
//
// hpp-model is distributed in the hope that it will be
// 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
// hpp-model If not, see
// <http://www.gnu.org/licenses/>.
#ifndef HPP_MODEL_EIGEN_HH
# define HPP_MODEL_EIGEN_HH
# include <hpp/model/fwd.hh>
# include <Eigen/SVD>
namespace
hpp
{
namespace
model
{
template
<
typename
SVD
>
static
Eigen
::
Ref
<
const
typename
SVD
::
MatrixUType
>
getU1
(
const
SVD
&
svd
)
{
return
svd
.
matrixU
().
leftCols
(
svd
.
rank
());
}
template
<
typename
SVD
>
static
Eigen
::
Ref
<
const
typename
SVD
::
MatrixUType
>
getU2
(
const
SVD
&
svd
)
{
return
svd
.
matrixU
().
rightCols
(
svd
.
matrixU
().
cols
()
-
svd
.
rank
());
}
template
<
typename
SVD
>
static
Eigen
::
Ref
<
const
typename
SVD
::
MatrixUType
>
getV1
(
const
SVD
&
svd
)
{
return
svd
.
matrixV
().
leftCols
(
svd
.
rank
());
}
template
<
typename
SVD
>
static
Eigen
::
Ref
<
const
typename
SVD
::
MatrixUType
>
getV2
(
const
SVD
&
svd
)
{
return
svd
.
matrixV
().
rightCols
(
svd
.
matrixV
().
cols
()
-
svd
.
rank
());
}
template
<
typename
SVD
>
static
void
pseudoInverse
(
const
SVD
&
svd
,
Eigen
::
Ref
<
typename
SVD
::
MatrixType
>
pinvmat
)
{
eigen_assert
(
svd
.
computeU
()
&&
svd
.
computeV
()
&&
"Eigen::JacobiSVD "
"computation flags must be at least: ComputeThinU | ComputeThinV"
);
typename
SVD
::
SingularValuesType
singularValues_inv
=
svd
.
singularValues
().
segment
(
0
,
svd
.
rank
()).
cwiseInverse
();
pinvmat
.
noalias
()
=
getV1
<
SVD
>
(
svd
)
*
singularValues_inv
.
asDiagonal
()
*
getU1
<
SVD
>
(
svd
).
adjoint
();
}
template
<
typename
SVD
>
void
projectorOnSpan
(
const
SVD
svd
,
Eigen
::
Ref
<
typename
SVD
::
MatrixType
>
projector
)
{
eigen_assert
(
svd
.
computeU
()
&&
svd
.
computeV
()
&&
"Eigen::JacobiSVD "
"computation flags must be at least: ComputeThinU | ComputeThinV"
);
projector
.
noalias
()
=
getV1
<
SVD
>
(
svd
)
*
getV1
<
SVD
>
(
svd
).
adjoint
();
}
template
<
typename
SVD
>
void
projectorOnSpanOfInv
(
const
SVD
svd
,
Eigen
::
Ref
<
typename
SVD
::
MatrixType
>
projector
)
{
eigen_assert
(
svd
.
computeU
()
&&
svd
.
computeV
()
&&
"Eigen::JacobiSVD "
"computation flags must be at least: ComputeThinU | ComputeThinV"
);
projector
.
noalias
()
=
getU1
<
SVD
>
(
svd
)
*
getU1
<
SVD
>
(
svd
).
adjoint
();
}
template
<
typename
SVD
>
void
projectorOnKernel
(
const
SVD
svd
,
Eigen
::
Ref
<
typename
SVD
::
MatrixType
>
projector
,
const
bool
&
computeFullV
=
false
)
{
eigen_assert
(
svd
.
computeV
()
&&
"Eigen::JacobiSVD "
"computation flags must be at least: ComputeThinV"
);
if
(
computeFullV
)
projector
.
noalias
()
=
getV2
<
SVD
>
(
svd
)
*
getV2
<
SVD
>
(
svd
).
adjoint
();
else
{
projector
.
noalias
()
=
-
getV1
<
SVD
>
(
svd
)
*
getV1
<
SVD
>
(
svd
).
adjoint
();
projector
.
diagonal
().
noalias
()
+=
vector_t
::
Ones
(
svd
.
matrixV
().
rows
());
}
}
template
<
typename
SVD
>
void
projectorOnKernelOfInv
(
const
SVD
svd
,
Eigen
::
Ref
<
typename
SVD
::
MatrixType
>
projector
,
const
bool
&
computeFullU
=
false
)
{
eigen_assert
(
svd
.
computeU
()
&&
"Eigen::JacobiSVD "
"computation flags must be at least: ComputeThinU"
);
if
(
computeFullU
)
{
// U2 * U2*
projector
.
noalias
()
=
getU2
<
SVD
>
(
svd
)
*
getU2
<
SVD
>
(
svd
).
adjoint
();
}
else
{
// I - U1 * U1*
projector
.
noalias
()
=
-
getU1
<
SVD
>
(
svd
)
*
getU1
<
SVD
>
(
svd
).
adjoint
();
projector
.
diagonal
().
noalias
()
+=
vector_t
::
Ones
(
svd
.
matrixU
().
rows
());
}
}
}
// namespace model
}
// namespace hpp
#endif // HPP_MODEL_EIGEN_HH
tests/CMakeLists.txt
View file @
0f53ce3b
...
@@ -44,4 +44,3 @@ MACRO(HPP_MODEL_TEST NAME)
...
@@ -44,4 +44,3 @@ MACRO(HPP_MODEL_TEST NAME)
ENDMACRO
(
HPP_MODEL_TEST
)
ENDMACRO
(
HPP_MODEL_TEST
)
HPP_MODEL_TEST
(
test-configuration
)
HPP_MODEL_TEST
(
test-configuration
)
HPP_MODEL_TEST
(
eigen
)
tests/eigen.cc
deleted
100644 → 0
View file @
879c7880
// Copyright (c) 2015, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-model.
// hpp-model is free software: you can redistribute it
// 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.
//
// hpp-model is distributed in the hope that it will be
// 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
// hpp-model. If not, see <http://www.gnu.org/licenses/>.
#define BOOST_TEST_MODULE EIGEN_FEATURE
#include
<boost/test/unit_test.hpp>
#include
<Eigen/Dense>
#include
<hpp/model/eigen.hh>
using
hpp
::
model
::
pseudoInverse
;
using
hpp
::
model
::
projectorOnKernel
;
using
hpp
::
model
::
projectorOnKernelOfInv
;
using
hpp
::
model
::
projectorOnSpan
;
using
hpp
::
model
::
projectorOnSpanOfInv
;
using
hpp
::
model
::
matrix_t
;
using
hpp
::
model
::
value_type
;
BOOST_AUTO_TEST_CASE
(
eigen_pseudoInverse_features
)
{
const
std
::
size_t
rows
=
4
,
cols
=
6
;
const
value_type
tol
=
1e-6
;
typedef
Eigen
::
JacobiSVD
<
matrix_t
>
SVD
;
const
bool
computeFullU
=
false
;
const
bool
computeFullV
=
true
;
int
computationFlags
=
(
computeFullU
?
Eigen
::
ComputeFullU
:
Eigen
::
ComputeThinU
)
|
(
computeFullV
?
Eigen
::
ComputeFullV
:
Eigen
::
ComputeThinV
);
SVD
svd
(
rows
,
cols
,
computationFlags
);
svd
.
setThreshold
(
tol
);
matrix_t
Mpinv
(
cols
,
rows
);
for
(
int
i
=
0
;
i
<
1000
;
++
i
)
{
matrix_t
M
=
matrix_t
::
Random
(
rows
,
cols
);
matrix_t
PK
(
cols
,
cols
);
matrix_t
PS
(
cols
,
cols
);
matrix_t
PKinv
(
rows
,
rows
);
matrix_t
PSinv
(
rows
,
rows
);
svd
.
compute
(
M
);
BOOST_CHECK_MESSAGE
((
svd
.
matrixV
().
adjoint
()
*
svd
.
matrixV
()
-
matrix_t
::
Identity
(
svd
.
matrixV
().
cols
(),
svd
.
matrixV
().
cols
())).
isZero
(),
svd
.
matrixV
().
adjoint
()
*
svd
.
matrixV
()
-
matrix_t
::
Identity
(
svd
.
matrixV
().
cols
(),
svd
.
matrixV
().
cols
()));
pseudoInverse
<
SVD
>
(
svd
,
Mpinv
);
projectorOnKernel
<
SVD
>
(
svd
,
PK
,
computeFullV
);
projectorOnSpan
<
SVD
>
(
svd
,
PS
);
projectorOnKernelOfInv
<
SVD
>
(
svd
,
PKinv
,
computeFullU
);
projectorOnSpanOfInv
<
SVD
>
(
svd
,
PSinv
);
matrix_t
Ir
=
M
*
Mpinv
;
matrix_t
Ic
=
Mpinv
*
M
;
matrix_t
_M
=
M
*
Ic
;
matrix_t
_Mpinv
=
Mpinv
*
Ir
;
BOOST_CHECK_MESSAGE
(
_M
.
isApprox
(
M
),
"M = M * M+ * M failed"
);
BOOST_CHECK_MESSAGE
(
_Mpinv
.
isApprox
(
Mpinv
),
"M+ = M+ * M * M+ failed"
);
BOOST_CHECK_MESSAGE
(
Ir
.
adjoint
().
isApprox
(
Ir
),
"(M * M+)* = M * M+ failed"
);
BOOST_CHECK_MESSAGE
(
Ic
.
adjoint
().
isApprox
(
Ic
),
"(M+ * M)* = M+ * M failed"
);
BOOST_CHECK_MESSAGE
(
PS
.
isApprox
(
Ic
),
"PK = M+ * M failed"
);
BOOST_CHECK_MESSAGE
(
PSinv
.
isApprox
(
Ir
),
"PKinv = M * M+ failed"
);
BOOST_CHECK_MESSAGE
((
PS
+
PK
)
.
isApprox
(
matrix_t
::
Identity
(
cols
,
cols
)),
"PS + PK = I failed"
);
BOOST_CHECK_MESSAGE
((
PSinv
+
PKinv
).
isApprox
(
matrix_t
::
Identity
(
rows
,
rows
)),
"PSinv + PKinv = I failed"
);
}
}
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment