| Directory: | ./ |
|---|---|
| File: | include/ndcurves/MathDefs.h |
| Date: | 2025-03-05 17:18:30 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 19 | 19 | 100.0% |
| Branches: | 19 | 34 | 55.9% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /** | ||
| 2 | * \file Math.h | ||
| 3 | * \brief Linear algebra and other maths definitions. Based on Eigen 3 or more | ||
| 4 | * \author Steve T. | ||
| 5 | * \version 0.1 | ||
| 6 | * \date 06/17/2013 | ||
| 7 | * | ||
| 8 | * This file contains math definitions used | ||
| 9 | * used throughout the library. | ||
| 10 | * Preprocessors definition are used to use eitheir float | ||
| 11 | * or double values, and 3 dimensional vectors for | ||
| 12 | * the Point structure. | ||
| 13 | */ | ||
| 14 | |||
| 15 | #ifndef _SPLINEMATH | ||
| 16 | #define _SPLINEMATH | ||
| 17 | |||
| 18 | #include <Eigen/Dense> | ||
| 19 | #include <Eigen/SVD> | ||
| 20 | #include <utility> | ||
| 21 | #include <vector> | ||
| 22 | namespace ndcurves { | ||
| 23 | /// \brief An inverse kinematics architecture enforcing an arbitrary number of | ||
| 24 | /// strict priority levels (Reference : Boulic et Al. 2003) | ||
| 25 | template <typename _Matrix_Type_> | ||
| 26 | 13 | void PseudoInverse(_Matrix_Type_& pinvmat) { | |
| 27 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | Eigen::JacobiSVD<_Matrix_Type_> svd( |
| 28 | pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV); | ||
| 29 |
2/4✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
|
13 | _Matrix_Type_ m_sigma = svd.singularValues(); |
| 30 | 13 | double pinvtoler = 1.e-6; // choose your tolerance widely! | |
| 31 |
2/4✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
|
13 | _Matrix_Type_ m_sigma_inv = |
| 32 | _Matrix_Type_::Zero(pinvmat.cols(), pinvmat.rows()); | ||
| 33 |
2/2✓ Branch 1 taken 36 times.
✓ Branch 2 taken 13 times.
|
49 | for (long i = 0; i < m_sigma.rows(); ++i) { |
| 34 |
3/4✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 10 times.
✓ Branch 4 taken 26 times.
|
36 | if (m_sigma(i) > pinvtoler) { |
| 35 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
10 | m_sigma_inv(i, i) = 1.0 / m_sigma(i); |
| 36 | } | ||
| 37 | } | ||
| 38 |
6/12✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 13 times.
✗ Branch 17 not taken.
|
13 | pinvmat = (svd.matrixV() * m_sigma_inv * svd.matrixU().transpose()); |
| 39 | 13 | } | |
| 40 | |||
| 41 | template <typename Matrix3, typename Point> | ||
| 42 | 32 | Matrix3 skew(const Point& x) { | |
| 43 |
1/2✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
|
32 | Matrix3 res = Matrix3::Zero(3, 3); |
| 44 | 32 | res(0, 1) = -x(2); | |
| 45 | 32 | res(0, 2) = x(1); | |
| 46 | 32 | res(1, 0) = x(2); | |
| 47 | 32 | res(1, 2) = -x(0); | |
| 48 | 32 | res(2, 0) = -x(1); | |
| 49 | 32 | res(2, 1) = x(0); | |
| 50 | 32 | return res; | |
| 51 | } | ||
| 52 | |||
| 53 | static const double MARGIN(0.001); | ||
| 54 | |||
| 55 | } // namespace ndcurves | ||
| 56 | #endif //_SPLINEMATH | ||
| 57 |