moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
test_pseudoinverse.cpp
Go to the documentation of this file.
1#include "pseudoinverse.hpp"
2
3#include <Eigen/QR>
4#include <Eigen/SVD>
5#include <gtest/gtest.h>
6
7#include <algorithm>
8#include <cmath>
9#include <vector>
10
11namespace
12{
13Eigen::MatrixXd matrixWithSingularValues(const std::vector<double>& values, Eigen::Index columns)
14{
15 const Eigen::Index rows = values.size();
16 Eigen::MatrixXd left(rows, rows);
17 Eigen::MatrixXd right(columns, rows);
18 for (Eigen::Index row = 0; row < rows; ++row)
19 {
20 for (Eigen::Index column = 0; column < rows; ++column)
21 left(row, column) = std::sin((row + 1) * (column + 2));
22 for (Eigen::Index column = 0; column < columns; ++column)
23 right(column, row) = std::cos((row + 2) * (column + 1));
24 }
25 const Eigen::MatrixXd u = left.householderQr().householderQ() * Eigen::MatrixXd::Identity(rows, rows);
26 const Eigen::MatrixXd v = right.householderQr().householderQ() * Eigen::MatrixXd::Identity(columns, rows);
27 return u * Eigen::Map<const Eigen::VectorXd>(values.data(), rows).asDiagonal() * v.transpose();
28}
29
30void expectSameSolution(const std::vector<double>& singular_values)
31{
32 constexpr double threshold = 0.001;
33 constexpr Eigen::Index columns = 7;
34 const Eigen::MatrixXd jacobian = matrixWithSingularValues(singular_values, columns);
35 const Eigen::VectorXd input = Eigen::VectorXd::LinSpaced(jacobian.rows(), -0.7, 0.9);
36 Eigen::JacobiSVD<Eigen::MatrixXd> svd(jacobian.rows(), jacobian.cols(), Eigen::ComputeThinU | Eigen::ComputeThinV);
37 svd.setThreshold(threshold);
38 svd.compute(jacobian);
39 const Eigen::VectorXd expected = svd.solve(input);
40 Eigen::MatrixXd jacobian_storage = Eigen::MatrixXd::Zero(jacobian.rows() + 1, jacobian.cols());
41 Eigen::VectorXd input_storage = Eigen::VectorXd::Zero(input.size() + 1);
42 jacobian_storage.topRows(jacobian.rows()) = jacobian;
43 input_storage.topRows(input.size()) = input;
44 const Eigen::VectorXd actual = kdl_kinematics_plugin::internal::solvePseudoinverse(
45 jacobian_storage.topRows(jacobian.rows()), input_storage.topRows(input.size()), threshold);
46 EXPECT_LT((expected - actual).norm(), 1e-8 * std::max(1.0, expected.norm()));
47 EXPECT_NEAR((jacobian * expected - input).norm(), (jacobian * actual - input).norm(), 1e-9);
48}
49} // namespace
50
51TEST(Pseudoinverse, FullRank)
52{
53 expectSameSolution({ 1.0, 0.7, 0.4, 0.2, 0.1, 0.02 });
54 expectSameSolution({ 1.0, 0.3, 0.02 });
55}
56
57TEST(Pseudoinverse, RankDeficient)
58{
59 expectSameSolution({ 1.0, 0.4, 0.1, 0.0, 0.0, 0.0 });
60}
61
62TEST(Pseudoinverse, ThresholdBoundary)
63{
64 expectSameSolution({ 1.0, 0.4, 0.1, 0.01, 0.001001, 0.0005 });
65 expectSameSolution({ 1.0, 0.4, 0.1, 0.01, 0.000999, 0.0005 });
66}
67
68TEST(Pseudoinverse, IllConditioned)
69{
70 expectSameSolution({ 1.0, 0.01, 0.0001, 1e-6, 1e-8, 0.0 });
71}
Eigen::VectorXd solvePseudoinverse(const Eigen::Ref< const Eigen::MatrixXd > &jacobian, const Eigen::Ref< const Eigen::VectorXd > &input, double threshold)
KeyboardReader input
TEST(Pseudoinverse, FullRank)