kinematics_core_unit.cpp
Go to the documentation of this file.
1 #include <tesseract_common/macros.h>
2 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
3 #include <gtest/gtest.h>
4 TESSERACT_COMMON_IGNORE_WARNINGS_POP
5 
6 #include <tesseract_kinematics/kdl/kdl_fwd_kin_chain.h>
7 #include <tesseract_kinematics/core/utils.h>
8 #include "kinematics_test_utils.h"
9 
10 const static std::string FACTORY_NAME = "TestFactory";
11 
12 TEST(TesseractKinematicsUnit, UtilsHarmonizeTowardZeroUnit) // NOLINT
13 {
14  Eigen::VectorXd q(2);
15  q[0] = (4 * M_PI) + M_PI_4;
16  q[1] = -(4 * M_PI) - M_PI_4;
17 
18  tesseract_kinematics::harmonizeTowardZero<double>(q, { 0, 1 });
19  EXPECT_NEAR(q[0], M_PI_4, 1e-6);
20  EXPECT_NEAR(q[1], -M_PI_4, 1e-6);
21 
22  q[0] = M_PI_4;
23  q[1] = -M_PI_4;
24 
25  tesseract_kinematics::harmonizeTowardZero<double>(q, { 0, 1 });
26  EXPECT_NEAR(q[0], M_PI_4, 1e-6);
27  EXPECT_NEAR(q[1], -M_PI_4, 1e-6);
28 
29  q[0] = 5 * M_PI_4;
30  q[1] = -5 * M_PI_4;
31 
32  tesseract_kinematics::harmonizeTowardZero<double>(q, { 0, 1 });
33  EXPECT_NEAR(q[0], -3 * M_PI_4, 1e-6);
34  EXPECT_NEAR(q[1], 3 * M_PI_4, 1e-6);
35 }
36 
37 TEST(TesseractKinematicsUnit, UtilsHarmonizeTowardMedianUnit) // NOLINT
38 {
39  Eigen::MatrixX2d c(2, 2);
40  c(0, 0) = -M_PI;
41  c(0, 1) = +M_PI;
42  c(1, 0) = -M_PI;
43  c(1, 1) = +M_PI;
44  Eigen::VectorXd m(2);
45  m[0] = (c(0, 0) + c(0, 1)) / 2.0;
46  m[1] = (c(1, 0) + c(1, 1)) / 2.0;
47 
48  Eigen::VectorXd q(2);
49  q[0] = (4 * M_PI) + M_PI_4;
50  q[1] = -(4 * M_PI) - M_PI_4;
51 
52  tesseract_kinematics::harmonizeTowardMedian<double>(q, { 0, 1 }, c);
53  EXPECT_NEAR(q[0], M_PI_4, 1e-6);
54  EXPECT_NEAR(q[1], -M_PI_4, 1e-6);
55  EXPECT_TRUE(std::abs(q[0] - m[0]) < (M_PI + 1e-6));
56  EXPECT_TRUE(std::abs(q[1] - m[1]) < (M_PI + 1e-6));
57 
58  q[0] = M_PI_4;
59  q[1] = -M_PI_4;
60 
61  tesseract_kinematics::harmonizeTowardMedian<double>(q, { 0, 1 }, c);
62  EXPECT_NEAR(q[0], M_PI_4, 1e-6);
63  EXPECT_NEAR(q[1], -M_PI_4, 1e-6);
64  EXPECT_TRUE(std::abs(q[0] - m[0]) < (M_PI + 1e-6));
65  EXPECT_TRUE(std::abs(q[1] - m[1]) < (M_PI + 1e-6));
66 
67  q[0] = 5 * M_PI_4;
68  q[1] = -5 * M_PI_4;
69 
70  tesseract_kinematics::harmonizeTowardMedian<double>(q, { 0, 1 }, c);
71  EXPECT_NEAR(q[0], -3 * M_PI_4, 1e-6);
72  EXPECT_NEAR(q[1], 3 * M_PI_4, 1e-6);
73  EXPECT_TRUE(std::abs(q[0] - m[0]) < (M_PI + 1e-6));
74  EXPECT_TRUE(std::abs(q[1] - m[1]) < (M_PI + 1e-6));
75 
76  // NON Zero Positive Constant
77  c(0, 0) = (10 * M_PI) + M_PI_4 - M_PI;
78  c(0, 1) = (10 * M_PI) + M_PI_4 + M_PI;
79  c(1, 0) = (10 * M_PI) + M_PI_4 - M_PI;
80  c(1, 1) = (10 * M_PI) + M_PI_4 + M_PI;
81  m[0] = (c(0, 0) + c(0, 1)) / 2.0;
82  m[1] = (c(1, 0) + c(1, 1)) / 2.0;
83 
84  q[0] = (4 * M_PI) + M_PI_4;
85  q[1] = -(4 * M_PI) - M_PI_4;
86 
87  tesseract_kinematics::harmonizeTowardMedian<double>(q, { 0, 1 }, c);
88  EXPECT_NEAR(q[0], m[0], 1e-6);
89  EXPECT_NEAR(q[1], m[1] - M_PI_2, 1e-6);
90  EXPECT_TRUE(std::abs(q[0] - m[0]) < (M_PI + 1e-6));
91  EXPECT_TRUE(std::abs(q[1] - m[1]) < (M_PI + 1e-6));
92 
93  q[0] = M_PI_4;
94  q[1] = -M_PI_4;
95 
96  tesseract_kinematics::harmonizeTowardMedian<double>(q, { 0, 1 }, c);
97  EXPECT_NEAR(q[0], m[0], 1e-6);
98  EXPECT_NEAR(q[1], m[1] - M_PI_2, 1e-6);
99  EXPECT_TRUE(std::abs(q[0] - m[0]) < (M_PI + 1e-6));
100  EXPECT_TRUE(std::abs(q[1] - m[1]) < (M_PI + 1e-6));
101 
102  q[0] = 5 * M_PI_4;
103  q[1] = -5 * M_PI_4;
104 
105  tesseract_kinematics::harmonizeTowardMedian<double>(q, { 0, 1 }, c);
106  EXPECT_NEAR(q[0], m[0] - M_PI, 1e-6);
107  EXPECT_NEAR(q[1], m[1] + M_PI_2, 1e-6);
108  EXPECT_TRUE(std::abs(q[0] - m[0]) < (M_PI + 1e-6));
109  EXPECT_TRUE(std::abs(q[1] - m[1]) < (M_PI + 1e-6));
110 
111  // NON Zero Negative Constant
112  c(0, 0) = (-1 * ((10 * M_PI) + M_PI_4)) - M_PI;
113  c(0, 1) = (-1 * ((10 * M_PI) + M_PI_4)) + M_PI;
114  c(1, 0) = (-1 * ((10 * M_PI) + M_PI_4)) - M_PI;
115  c(1, 1) = (-1 * ((10 * M_PI) + M_PI_4)) + M_PI;
116  m[0] = (c(0, 0) + c(0, 1)) / 2.0;
117  m[1] = (c(1, 0) + c(1, 1)) / 2.0;
118 
119  q[0] = (4 * M_PI) + M_PI_4;
120  q[1] = -(4 * M_PI) - M_PI_4;
121 
122  tesseract_kinematics::harmonizeTowardMedian<double>(q, { 0, 1 }, c);
123  EXPECT_NEAR(q[0], m[0] + M_PI_2, 1e-6);
124  EXPECT_NEAR(q[1], m[1], 1e-6);
125  EXPECT_TRUE(std::abs(q[0] - m[0]) < (M_PI + 1e-6));
126  EXPECT_TRUE(std::abs(q[1] - m[1]) < (M_PI + 1e-6));
127 
128  q[0] = M_PI_4;
129  q[1] = -M_PI_4;
130 
131  tesseract_kinematics::harmonizeTowardMedian<double>(q, { 0, 1 }, c);
132  EXPECT_NEAR(q[0], m[0] + M_PI_2, 1e-6);
133  EXPECT_NEAR(q[1], m[1], 1e-6);
134  EXPECT_TRUE(std::abs(q[0] - m[0]) < (M_PI + 1e-6));
135  EXPECT_TRUE(std::abs(q[1] - m[1]) < (M_PI + 1e-6));
136 
137  q[0] = 5 * M_PI_4;
138  q[1] = -5 * M_PI_4;
139 
140  tesseract_kinematics::harmonizeTowardMedian<double>(q, { 0, 1 }, c);
141  EXPECT_NEAR(q[0], m[0] - M_PI_2, 1e-6);
142  EXPECT_NEAR(q[1], m[1] + M_PI, 1e-6);
143  EXPECT_TRUE(std::abs(q[0] - m[0]) < (M_PI + 1e-6));
144  EXPECT_TRUE(std::abs(q[1] - m[1]) < (M_PI + 1e-6));
145 }
146 
147 template <typename FloatType>
148 void runRedundantSolutionsTest()
149 {
150  // Helper function for checking if all redundant solutions are unique
151  auto expect_unique_solutions = [](const std::vector<tesseract_kinematics::VectorX<FloatType>>& solutions) {
152  for (auto sol_1 = solutions.begin(); sol_1 != solutions.end() - 1; ++sol_1)
153  {
154  for (auto sol_2 = sol_1 + 1; sol_2 != solutions.end(); ++sol_2)
155  {
156  EXPECT_FALSE(tesseract_common::almostEqualRelativeAndAbs(
157  sol_1->template cast<double>(), sol_2->template cast<double>(), 1e-6));
158  }
159  }
160  };
161 
162  {
163  double max_diff = 1e-6;
164  Eigen::MatrixX2d limits(3, 2);
165  limits << 0, 2.0 * M_PI, 0, 2.0 * M_PI, 0, 2.0 * M_PI;
166  std::vector<Eigen::Index> redundancy_capable_joints = { 0, 1, 2 };
167 
168  tesseract_kinematics::VectorX<FloatType> q(3);
169  q << 0, 0, 0;
170 
171  { // Test when initial solution is at the lower limit
172  std::vector<tesseract_kinematics::VectorX<FloatType>> solutions =
173  tesseract_kinematics::getRedundantSolutions<FloatType>(q, limits, redundancy_capable_joints);
174  if (tesseract_common::satisfiesLimits<double>(q.template cast<double>(), limits, max_diff))
175  solutions.push_back(q);
176 
177  EXPECT_EQ(solutions.size(), 8);
178  expect_unique_solutions(solutions);
179  }
180 
181  { // Test when initial solution is within the limits
182  limits << -2.0 * M_PI, 2.0 * M_PI, -2.0 * M_PI, 2.0 * M_PI, -2.0 * M_PI, 2.0 * M_PI;
183  std::vector<tesseract_kinematics::VectorX<FloatType>> solutions =
184  tesseract_kinematics::getRedundantSolutions<FloatType>(q, limits, redundancy_capable_joints);
185  if (tesseract_common::satisfiesLimits<double>(q.template cast<double>(), limits, max_diff))
186  solutions.push_back(q);
187 
188  EXPECT_EQ(solutions.size(), 27);
189  expect_unique_solutions(solutions);
190  }
191 
192  { // Test when the initial solution outside the lower and upper limit
193  limits << -2.0 * M_PI, 2.0 * M_PI, -2.0 * M_PI, 2.0 * M_PI, -2.0 * M_PI, 2.0 * M_PI;
194  q << static_cast<FloatType>(-4.0 * M_PI), static_cast<FloatType>(-4.0 * M_PI), static_cast<FloatType>(4.0 * M_PI);
195 
196  std::vector<tesseract_kinematics::VectorX<FloatType>> solutions =
197  tesseract_kinematics::getRedundantSolutions<FloatType>(q, limits, redundancy_capable_joints);
198  if (tesseract_common::satisfiesLimits<double>(q.template cast<double>(), limits, max_diff))
199  solutions.push_back(q);
200 
201  EXPECT_EQ(solutions.size(), 27);
202  expect_unique_solutions(solutions);
203  }
204  }
205 
206  { // Test case when not all joints are redundancy capable
207  double max_diff = 1.0e-6;
208  Eigen::MatrixX2d limits(4, 2);
209  limits << -2.0 * M_PI, 2.0 * M_PI, -2.0 * M_PI, 2.0 * M_PI, -2.0 * M_PI, 2.0 * M_PI, -2.0 * M_PI, 2.0 * M_PI;
210 
211  tesseract_kinematics::VectorX<FloatType> q(4);
212  q << static_cast<FloatType>(-4.0 * M_PI), static_cast<FloatType>(-4.0 * M_PI), static_cast<FloatType>(0.0),
213  static_cast<FloatType>(4.0 * M_PI);
214 
215  // Arbitrarily decide that joint 2 is not redundancy capable
216  std::vector<Eigen::Index> redundancy_capable_joints = { 0, 1, 3 };
217 
218  std::vector<tesseract_kinematics::VectorX<FloatType>> solutions =
219  tesseract_kinematics::getRedundantSolutions<FloatType>(q, limits, redundancy_capable_joints);
220  if (tesseract_common::satisfiesLimits<double>(q.template cast<double>(), limits, max_diff))
221  solutions.push_back(q);
222 
223  EXPECT_EQ(solutions.size(), 27);
224  expect_unique_solutions(solutions);
225  }
226 
227  { // Edge-case tests
228  Eigen::MatrixX2d limits(4, 2);
229  limits << -2.0 * M_PI, 2.0 * M_PI, -2.0 * M_PI, 2.0 * M_PI, -2.0 * M_PI, 2.0 * M_PI, -2.0 * M_PI, 2.0 * M_PI;
230 
231  tesseract_kinematics::VectorX<FloatType> q(4);
232  q << static_cast<FloatType>(-4.0 * M_PI), static_cast<FloatType>(-4.0 * M_PI), static_cast<FloatType>(0.0),
233  static_cast<FloatType>(4.0 * M_PI);
234 
235  std::vector<Eigen::Index> redundancy_capable_joints = {};
236  std::vector<tesseract_kinematics::VectorX<FloatType>> solutions =
237  tesseract_kinematics::getRedundantSolutions<FloatType>(q, limits, redundancy_capable_joints);
238 
239  EXPECT_EQ(solutions.size(), 0);
240 
241  redundancy_capable_joints = { 10 };
242 
243  // NOLINTNEXTLINE
244  EXPECT_THROW(tesseract_kinematics::getRedundantSolutions<FloatType>(q, limits, redundancy_capable_joints),
245  std::runtime_error);
246  }
247 
248  { // Not finit lower
249  Eigen::MatrixX2d limits(4, 2);
250  limits << -std::numeric_limits<double>::infinity(), 2.0 * M_PI, -2.0 * M_PI, 2.0 * M_PI, -2.0 * M_PI, 2.0 * M_PI,
251  -2.0 * M_PI, 2.0 * M_PI;
252 
253  tesseract_kinematics::VectorX<FloatType> q(4);
254  q << static_cast<FloatType>(-4.0 * M_PI), static_cast<FloatType>(-4.0 * M_PI), static_cast<FloatType>(0.0),
255  static_cast<FloatType>(4.0 * M_PI);
256 
257  std::vector<Eigen::Index> redundancy_capable_joints = { 0 };
258  std::vector<tesseract_kinematics::VectorX<FloatType>> solutions =
259  tesseract_kinematics::getRedundantSolutions<FloatType>(q, limits, redundancy_capable_joints);
260 
261  EXPECT_EQ(solutions.size(), 0);
262  }
263 
264  { // Not finit upper
265  Eigen::MatrixX2d limits(4, 2);
266  limits << -2.0 * M_PI, std::numeric_limits<double>::infinity(), -2.0 * M_PI, 2.0 * M_PI, -2.0 * M_PI, 2.0 * M_PI,
267  -2.0 * M_PI, 2.0 * M_PI;
268 
269  tesseract_kinematics::VectorX<FloatType> q(4);
270  q << static_cast<FloatType>(-4.0 * M_PI), static_cast<FloatType>(-4.0 * M_PI), static_cast<FloatType>(0.0),
271  static_cast<FloatType>(4.0 * M_PI);
272 
273  std::vector<Eigen::Index> redundancy_capable_joints = { 0, 1, 3 };
274  std::vector<tesseract_kinematics::VectorX<FloatType>> solutions =
275  tesseract_kinematics::getRedundantSolutions<FloatType>(q, limits, redundancy_capable_joints);
276 
277  EXPECT_EQ(solutions.size(), 0);
278  }
279 }
280 
281 TEST(TesseractKinematicsUnit, RedundantSolutionsUnit) // NOLINT
282 {
283  runRedundantSolutionsTest<float>();
284  runRedundantSolutionsTest<double>();
285 }
286 
287 TEST(TesseractKinematicsUnit, UtilsNearSingularityUnit) // NOLINT
288 {
289  tesseract_common::GeneralResourceLocator locator;
290  tesseract_scene_graph::SceneGraph::Ptr scene_graph = tesseract_kinematics::test_suite::getSceneGraphABB(locator);
291 
292  tesseract_kinematics::KDLFwdKinChain fwd_kin(*scene_graph, "base_link", "tool0");
293 
294  // First test joint 4, 5 and 6 at zero which should be in a singularity
295  Eigen::VectorXd jv = Eigen::VectorXd::Zero(6);
296  Eigen::MatrixXd jacobian = fwd_kin.calcJacobian(jv, "tool0");
297  EXPECT_TRUE(tesseract_kinematics::isNearSingularity(jacobian, 0.001));
298 
299  // Set joint 5 angle to 1 deg and it with the default threshold it should still be in singularity
300  jv[4] = 1 * M_PI / 180.0;
301  jacobian = fwd_kin.calcJacobian(jv, "tool0");
302  EXPECT_TRUE(tesseract_kinematics::isNearSingularity(jacobian));
303 
304  // Set joint 5 angle to 2 deg and it should no longer be in a singularity
305  jv[4] = 2 * M_PI / 180.0;
306  jacobian = fwd_kin.calcJacobian(jv, "tool0");
307  EXPECT_FALSE(tesseract_kinematics::isNearSingularity(jacobian));
308 
309  // Increase threshold and now with joint 5 at 2 deg it will now be considered in a singularity
310  EXPECT_TRUE(tesseract_kinematics::isNearSingularity(jacobian, 0.02));
311 }
312 
313 TEST(TesseractKinematicsUnit, UtilscalcManipulabilityUnit) // NOLINT
314 {
315  tesseract_common::GeneralResourceLocator locator;
316  tesseract_scene_graph::SceneGraph::Ptr scene_graph = tesseract_kinematics::test_suite::getSceneGraphABB(locator);
317 
318  tesseract_kinematics::KDLFwdKinChain fwd_kin(*scene_graph, "base_link", "tool0");
319 
320  // First test joint 4, 5 and 6 at zero which should be in a singularity
321  Eigen::VectorXd jv = Eigen::VectorXd::Zero(6);
322  Eigen::MatrixXd jacobian = fwd_kin.calcJacobian(jv, "tool0");
323  tesseract_kinematics::Manipulability m = tesseract_kinematics::calcManipulability(jacobian);
324  EXPECT_EQ(m.m.eigen_values.size(), 6);
325  EXPECT_NEAR(m.m.volume, 0, 1e-6);
326  EXPECT_GT(m.m.condition, 1e+20);
327 
328  EXPECT_EQ(m.m_linear.eigen_values.size(), 3);
329  EXPECT_NEAR(m.m_linear.eigen_values[0], 0.18153054745434696, 1e-6);
330  EXPECT_NEAR(m.m_linear.eigen_values[1], 0.8835999999999999, 1e-6);
331  EXPECT_NEAR(m.m_linear.eigen_values[2], 1.960719452545653, 1e-6);
332  EXPECT_NEAR(m.m_linear.condition, 10.801044122002406, 1e-6);
333  EXPECT_NEAR(m.m_linear.measure, 3.286494199295414, 1e-6);
334  EXPECT_NEAR(m.m_linear.volume, 0.5608031457314142, 1e-6);
335 
336  EXPECT_EQ(m.m_angular.eigen_values.size(), 3);
337  EXPECT_NEAR(m.m_angular.eigen_values[0], 1.0, 1e-6);
338  EXPECT_NEAR(m.m_angular.eigen_values[1], 2.0, 1e-6);
339  EXPECT_NEAR(m.m_angular.eigen_values[2], 3.0, 1e-6);
340  EXPECT_NEAR(m.m_angular.condition, 3.0, 1e-6);
341  EXPECT_NEAR(m.m_angular.measure, 1.7320508075688772, 1e-6);
342  EXPECT_NEAR(m.m_angular.volume, 2.449489742783178, 1e-6);
343 
344  EXPECT_EQ(m.f.eigen_values.size(), 6);
345  EXPECT_NEAR(m.m.volume, 0, 1e-6);
346  EXPECT_GT(m.m.condition, 1e+20);
347 
348  EXPECT_EQ(m.f_linear.eigen_values.size(), 3);
349  EXPECT_NEAR(m.f_linear.eigen_values[0], 0.5100168709509535, 1e-6);
350  EXPECT_NEAR(m.f_linear.eigen_values[1], 1.1317338162064283, 1e-6);
351  EXPECT_NEAR(m.f_linear.eigen_values[2], 5.508714726106856, 1e-6);
352  EXPECT_NEAR(m.f_linear.condition, 10.801044122002406, 1e-6);
353  EXPECT_NEAR(m.f_linear.measure, 3.286494199295414, 1e-6);
354  EXPECT_NEAR(m.f_linear.volume, 1.783156902045858, 1e-6);
355 
356  EXPECT_EQ(m.f_angular.eigen_values.size(), 3);
357  EXPECT_NEAR(m.f_angular.eigen_values[0], 0.3333333333333333, 1e-6);
358  EXPECT_NEAR(m.f_angular.eigen_values[1], 0.5, 1e-6);
359  EXPECT_NEAR(m.f_angular.eigen_values[2], 1.0, 1e-6);
360  EXPECT_NEAR(m.f_angular.condition, 3.0, 1e-6);
361  EXPECT_NEAR(m.f_angular.measure, 1.7320508075688774, 1e-6);
362  EXPECT_NEAR(m.f_angular.volume, 0.408248290463863, 1e-6);
363 }
364 
365 TEST(TesseractKinematicsUnit, solvePInv_OverdeterminedSystem)
366 {
367  Eigen::MatrixXd A(4, 2);
368  A << 1, 2, 3, 4, 5, 6, 7, 8;
369 
370  Eigen::VectorXd b(4);
371  b << 1, 2, 3, 4;
372 
373  Eigen::VectorXd x(A.cols());
374  bool success = tesseract_kinematics::solvePInv(A, b, x);
375 
376  EXPECT_TRUE(success);
377  EXPECT_EQ(x.size(), 2);
378 
379  // Check solution approximately satisfies Ax ≈ b
380  Eigen::VectorXd b_approx = A * x;
381  EXPECT_TRUE((b - b_approx).norm() < 1e-4);
382 }
383 
384 TEST(TesseractKinematicsUnit, solvePInv_UnderdeterminedSystem)
385 {
386  Eigen::MatrixXd A(2, 4);
387  A << 1, 2, 3, 4, 5, 6, 7, 8;
388 
389  Eigen::VectorXd b(2);
390  b << 1, 2;
391 
392  Eigen::VectorXd x(A.cols());
393  bool success = tesseract_kinematics::solvePInv(A, b, x);
394 
395  EXPECT_TRUE(success);
396  EXPECT_EQ(x.size(), 4);
397 
398  // Check solution approximately satisfies Ax ≈ b
399  Eigen::VectorXd b_approx = A * x;
400  EXPECT_TRUE((b - b_approx).norm() < 1e-4);
401 }
402 
403 TEST(TesseractKinematicsUnit, solvePInv_SizeMismatch)
404 {
405  Eigen::MatrixXd A(3, 3);
406  A << 1, 2, 3, 4, 5, 6, 7, 8, 9;
407 
408  Eigen::VectorXd b(2); // Wrong size
409 
410  Eigen::VectorXd x(3);
411  bool success = tesseract_kinematics::solvePInv(A, b, x);
412  EXPECT_FALSE(success);
413 }
414 
415 TEST(TesseractKinematicsUnit, solvePInv_EmptyMatrix)
416 {
417  Eigen::MatrixXd A(0, 0);
418  Eigen::VectorXd b(0);
419  Eigen::VectorXd x;
420 
421  bool success = tesseract_kinematics::solvePInv(A, b, x);
422  EXPECT_FALSE(success);
423 }
424 
425 // Helper function to validate Ax ≈ A(PA)x
426 bool isPseudoinverseValid(const Eigen::MatrixXd& A, const Eigen::MatrixXd& P, double tolerance = 1e-4)
427 {
428  Eigen::MatrixXd approx = A * P * A;
429  return (A - approx).norm() < tolerance;
430 }
431 
432 TEST(TesseractKinematicsUnit, dampedPInv_FullRankSquareMatrix)
433 {
434  Eigen::MatrixXd A(3, 3);
435  A << 1, 2, 3, 4, 5, 6, 7, 8, 10;
436 
437  Eigen::MatrixXd P(3, 3);
438  bool success = tesseract_kinematics::dampedPInv(A, P, 1e-5, 0.01);
439  EXPECT_TRUE(success);
440  EXPECT_EQ(P.rows(), 3);
441  EXPECT_EQ(P.cols(), 3);
442  EXPECT_TRUE(isPseudoinverseValid(A, P));
443 }
444 
445 TEST(TesseractKinematicsUnit, dampedPInv_RankDeficientMatrix)
446 {
447  Eigen::MatrixXd A(3, 3);
448  A << 1, 2, 3, 2, 4, 6, 3, 6, 9; // Rank deficient (linearly dependent rows)
449 
450  Eigen::MatrixXd P(3, 3);
451  bool success = tesseract_kinematics::dampedPInv(A, P, 1e-5, 0.01);
452  EXPECT_TRUE(success);
453  EXPECT_EQ(P.rows(), 3);
454  EXPECT_EQ(P.cols(), 3);
455  EXPECT_TRUE(isPseudoinverseValid(A, P, 1e-2)); // Allow more slack
456 }
457 
458 TEST(TesseractKinematicsUnit, dampedPInv_OverdeterminedMatrix)
459 {
460  Eigen::MatrixXd A(4, 2);
461  A << 1, 2, 3, 4, 5, 6, 7, 8;
462 
463  Eigen::MatrixXd P(2, 4);
464  bool success = tesseract_kinematics::dampedPInv(A, P, 1e-5, 0.01);
465  EXPECT_TRUE(success);
466  EXPECT_EQ(P.rows(), 2);
467  EXPECT_EQ(P.cols(), 4);
468  EXPECT_TRUE(isPseudoinverseValid(A, P));
469 }
470 
471 TEST(TesseractKinematicsUnit, dampedPInv_UnderdeterminedMatrix)
472 {
473  Eigen::MatrixXd A(2, 4);
474  A << 1, 2, 3, 4, 5, 6, 7, 8;
475 
476  Eigen::MatrixXd P(4, 2);
477  bool success = tesseract_kinematics::dampedPInv(A, P, 1e-5, 0.01);
478  EXPECT_TRUE(success);
479  EXPECT_EQ(P.rows(), 4);
480  EXPECT_EQ(P.cols(), 2);
481  EXPECT_TRUE(isPseudoinverseValid(A, P));
482 }
483 
484 TEST(TesseractKinematicsUnit, dampedPInv_EmptyMatrix)
485 {
486  Eigen::MatrixXd A;
487  Eigen::MatrixXd P;
488  bool success = tesseract_kinematics::dampedPInv(A, P, 1e-5, 0.01);
489  EXPECT_FALSE(success);
490 }
491 
492 int main(int argc, char** argv)
493 {
494  testing::InitGoogleTest(&argc, argv);
495 
496  return RUN_ALL_TESTS();
497 }
main
int main(int argc, char **argv)
Definition: kinematics_core_unit.cpp:492
tesseract_kinematics::isNearSingularity
bool isNearSingularity(const Eigen::Ref< const Eigen::MatrixXd > &jacobian, double threshold=0.01)
Check if the provided jacobian is near a singularity.
Definition: utils.cpp:181
kdl_fwd_kin_chain.h
Tesseract KDL forward kinematics chain implementation.
kinematics_test_utils.h
tesseract_kinematics::Manipulability::m_linear
ManipulabilityEllipsoid m_linear
Linear velocity manipulability ellipsoid.
Definition: utils.h:154
tesseract_kinematics::calcManipulability
Manipulability calcManipulability(const Eigen::Ref< const Eigen::MatrixXd > &jacobian)
Calculate manipulability data about the provided jacobian.
Definition: utils.cpp:188
tesseract_kinematics::Manipulability::f_linear
ManipulabilityEllipsoid f_linear
Linear force manipulability ellipsoid.
Definition: utils.h:163
tesseract_common::almostEqualRelativeAndAbs
bool almostEqualRelativeAndAbs(const Eigen::Ref< const Eigen::VectorXd > &v1, const Eigen::Ref< const Eigen::VectorXd > &v2, const Eigen::Ref< const Eigen::VectorXd > &max_diff, const Eigen::Ref< const Eigen::VectorXd > &max_rel_diff)
runRedundantSolutionsTest
void runRedundantSolutionsTest()
Definition: kinematics_core_unit.cpp:148
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#define TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
TEST
TEST(TesseractKinematicsUnit, UtilsHarmonizeTowardZeroUnit)
Definition: kinematics_core_unit.cpp:12
utils.h
Kinematics utility functions.
tesseract_kinematics::ManipulabilityEllipsoid::measure
double measure
The ratio of longest and shortes axes of the manipulability ellipsoid.
Definition: utils.h:130
tesseract_kinematics::VectorX
Eigen::Matrix< FloatType, Eigen::Dynamic, 1 > VectorX
Definition: utils.h:42
tesseract_kinematics::ManipulabilityEllipsoid::condition
double condition
The condition number of A.
Definition: utils.h:137
tesseract_kinematics::ManipulabilityEllipsoid::volume
double volume
This is propotial to the volume.
Definition: utils.h:140
tesseract_kinematics::Manipulability::m_angular
ManipulabilityEllipsoid m_angular
Angular velocity manipulability ellipsoid.
Definition: utils.h:157
tesseract_kinematics::Manipulability
Contains both manipulability ellipsoid and force ellipsoid data.
Definition: utils.h:144
tesseract_kinematics::ManipulabilityEllipsoid::eigen_values
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::VectorXd eigen_values
The manipulability ellipsoid eigen values.
Definition: utils.h:121
tesseract_kinematics::KDLFwdKinChain
KDL kinematic chain implementation.
Definition: kdl_fwd_kin_chain.h:49
tesseract_kinematics::Manipulability::m
EIGEN_MAKE_ALIGNED_OPERATOR_NEW ManipulabilityEllipsoid m
Full Manipulability Ellipsoid.
Definition: utils.h:151
FACTORY_NAME
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH const static TESSERACT_COMMON_IGNORE_WARNINGS_POP std::string FACTORY_NAME
Definition: kinematics_core_unit.cpp:10
tesseract_kinematics::dampedPInv
bool dampedPInv(const Eigen::Ref< const Eigen::MatrixXd > &A, Eigen::Ref< Eigen::MatrixXd > P, double eps=0.011, double lambda=0.01)
Calculate Damped Pseudoinverse Use this SVD to compute A+ (pseudoinverse of A). Weighting still TBD.
Definition: utils.cpp:148
TESSERACT_COMMON_IGNORE_WARNINGS_POP
#define TESSERACT_COMMON_IGNORE_WARNINGS_POP
isPseudoinverseValid
bool isPseudoinverseValid(const Eigen::MatrixXd &A, const Eigen::MatrixXd &P, double tolerance=1e-4)
Definition: kinematics_core_unit.cpp:426
tesseract_kinematics::Manipulability::f_angular
ManipulabilityEllipsoid f_angular
Angular momentum manipulability ellipsoid.
Definition: utils.h:166
tesseract_common::GeneralResourceLocator
tesseract_common::satisfiesLimits< double >
template bool satisfiesLimits< double >(const Eigen::Ref< const Eigen::Matrix< double, Eigen::Dynamic, 1 >> &values, const Eigen::Ref< const Eigen::Matrix< double, Eigen::Dynamic, 2 >> &limits, const Eigen::Ref< const Eigen::Matrix< double, Eigen::Dynamic, 1 >> &max_diff, const Eigen::Ref< const Eigen::Matrix< double, Eigen::Dynamic, 1 >> &max_rel_diff)
tesseract_kinematics::KDLFwdKinChain::calcJacobian
Eigen::MatrixXd calcJacobian(const Eigen::Ref< const Eigen::VectorXd > &joint_angles, const std::string &joint_link_name) const override final
Calculates the Jacobian matrix for a given joint state in the reference frame of the specified link.
Definition: kdl_fwd_kin_chain.cpp:132
macros.h
tesseract_scene_graph::SceneGraph::Ptr
std::shared_ptr< SceneGraph > Ptr
tesseract_kinematics::Manipulability::f
ManipulabilityEllipsoid f
Full Force Ellipsoid.
Definition: utils.h:160
tesseract_kinematics::solvePInv
bool solvePInv(const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b, Eigen::Ref< Eigen::VectorXd > x)
Solve equation Ax=b for x Use this SVD to compute A+ (pseudoinverse of A). Weighting still TBD.
Definition: utils.cpp:106
tesseract_kinematics::test_suite::getSceneGraphABB
tesseract_scene_graph::SceneGraph::UPtr getSceneGraphABB(const tesseract_common::ResourceLocator &locator)
Definition: kinematics_test_utils.h:77

tesseract_kinematics
Author(s): Levi Armstrong
autogenerated on Sun May 18 2025 03:02:14