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
f23b2e8c
Commit
f23b2e8c
authored
Oct 02, 2015
by
Joseph Mirabel
Committed by
Joseph Mirabel
Oct 23, 2015
Browse files
Add pseudoInverse algorithm
parent
3a6dc6ab
Changes
4
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
f23b2e8c
...
...
@@ -61,6 +61,7 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/model/gripper.hh
include/hpp/model/center-of-mass-computation.hh
include/hpp/model/debug.hh
include/hpp/model/eigen.hh
)
# Declare dependencies
...
...
include/hpp/model/eigen.hh
0 → 100644
View file @
f23b2e8c
// 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
>
void
pseudoInverse
(
const
SVD
&
svd
,
Eigen
::
Ref
<
typename
SVD
::
MatrixType
>
pinvmat
,
const
value_type
tolerance
=
Eigen
::
NumTraits
<
typename
SVD
::
MatrixType
::
Scalar
>::
epsilon
())
{
eigen_assert
(
svd
.
computeU
()
&&
svd
.
computeV
()
&&
"Eigen::JacobiSVD "
"computation flags must be at least: ComputeThinU | ComputeThinV"
);
const
typename
SVD
::
SingularValuesType
&
singularValues
=
svd
.
singularValues
();
typename
SVD
::
SingularValuesType
singularValues_inv
=
(
singularValues
.
array
()
>=
tolerance
).
select
(
singularValues
.
array
().
cwiseInverse
(),
SVD
::
SingularValuesType
::
Zero
(
singularValues
.
size
())
).
matrix
();
pinvmat
=
svd
.
matrixV
()
*
singularValues_inv
.
asDiagonal
()
*
svd
.
matrixU
().
adjoint
();
}
}
}
// namespace model
}
// namespace hpp
#endif // HPP_MODEL_EIGEN_HH
tests/CMakeLists.txt
View file @
f23b2e8c
...
...
@@ -44,3 +44,4 @@ MACRO(HPP_MODEL_TEST NAME)
ENDMACRO
(
HPP_MODEL_TEST
)
HPP_MODEL_TEST
(
test-configuration
)
HPP_MODEL_TEST
(
eigen
)
tests/eigen.cc
0 → 100644
View file @
f23b2e8c
// 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
<hpp/model/eigen.hh>
using
hpp
::
model
::
pseudoInverse
;
using
hpp
::
model
::
matrix_t
;
using
hpp
::
model
::
value_type
;
BOOST_AUTO_TEST_CASE
(
testPseudoInverse
)
{
const
std
::
size_t
rows
=
4
,
cols
=
6
;
const
value_type
tol
=
1e-6
;
typedef
Eigen
::
JacobiSVD
<
matrix_t
>
SVD
;
SVD
svd
(
rows
,
cols
,
Eigen
::
ComputeThinU
|
Eigen
::
ComputeThinV
);
matrix_t
Mpinv
(
cols
,
rows
);
for
(
int
i
=
0
;
i
<
1000
;
++
i
)
{
matrix_t
M
=
matrix_t
::
Random
(
rows
,
cols
);
svd
.
compute
(
M
);
pseudoInverse
<
SVD
>
(
svd
,
Mpinv
,
tol
);
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"
);
}
}
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