Tesseract
Motion Planning Environment
Loading...
Searching...
No Matches
state_solver_test_suite.h
Go to the documentation of this file.
1#ifndef TESSERACT_STATE_SOLVER_STATE_SOLVER_TEST_SUITE_H
2#define TESSERACT_STATE_SOLVER_STATE_SOLVER_TEST_SUITE_H
3
4#include <tesseract_common/macros.h>
5TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
6#include <gtest/gtest.h>
7#include <algorithm>
8#include <vector>
9#include <tesseract_urdf/urdf_parser.h>
10#include <tesseract_geometry/impl/box.h>
11#include <tesseract_common/resource_locator.h>
12#include <tesseract_common/utils.h>
13TESSERACT_COMMON_IGNORE_WARNINGS_POP
14
15#include <tesseract_state_solver/kdl/kdl_state_solver.h>
16#include <tesseract_support/tesseract_support_resource_locator.h>
17
18namespace tesseract_scene_graph::test_suite
19{
20inline SceneGraph::UPtr getSceneGraph()
21{
22 std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf";
23
24 tesseract_common::TesseractSupportResourceLocator locator;
25 return tesseract_urdf::parseURDFFile(path, locator);
26}
27
28inline SceneGraph::UPtr getSubSceneGraph()
29{
30 auto subgraph = std::make_unique<SceneGraph>();
31 subgraph->setName("subgraph");
32
33 // Now add a link to empty environment
34 auto visual = std::make_shared<Visual>();
35 visual->geometry = std::make_shared<tesseract_geometry::Box>(1, 1, 1);
36 auto collision = std::make_shared<Collision>();
37 collision->geometry = std::make_shared<tesseract_geometry::Box>(1, 1, 1);
38
39 const std::string link_name1 = "subgraph_base_link";
40 const std::string link_name2 = "subgraph_link_1";
41 const std::string joint_name1 = "subgraph_joint1";
42 Link link_1(link_name1);
43 link_1.visual.push_back(visual);
44 link_1.collision.push_back(collision);
45 Link link_2(link_name2);
46
47 Joint joint_1(joint_name1);
48 joint_1.parent_to_joint_origin_transform.translation()(0) = 1.25;
49 joint_1.parent_link_name = link_name1;
50 joint_1.child_link_name = link_name2;
51 joint_1.type = JointType::FIXED;
52
53 subgraph->addLink(link_1);
54 subgraph->addLink(link_2);
55 subgraph->addJoint(joint_1);
56
57 return subgraph;
58}
59
60inline void runCompareSceneStates(const SceneState& base_state, const SceneState& compare_state)
61{
62 EXPECT_EQ(base_state.joints.size(), compare_state.joints.size());
63 EXPECT_EQ(base_state.joint_transforms.size(), compare_state.joint_transforms.size());
64 EXPECT_EQ(base_state.link_transforms.size(), compare_state.link_transforms.size());
65
66 for (const auto& pair : base_state.joints)
67 {
68 EXPECT_NEAR(pair.second, compare_state.joints.at(pair.first), 1e-6);
69 }
70
71 for (const auto& pair : base_state.joint_transforms)
72 {
73 EXPECT_TRUE(pair.second.isApprox(compare_state.joint_transforms.at(pair.first), 1e-6));
74 }
75
76 for (const auto& link_pair : base_state.link_transforms)
77 {
78 EXPECT_TRUE(link_pair.second.isApprox(compare_state.link_transforms.at(link_pair.first), 1e-6));
79 }
80}
81
82inline void runCompareStateSolver(const StateSolver& base_solver, StateSolver& comp_solver)
83{
84 EXPECT_EQ(base_solver.getBaseLinkName(), comp_solver.getBaseLinkName());
85 EXPECT_TRUE(tesseract_common::isIdentical(base_solver.getJointNames(), comp_solver.getJointNames(), false));
86 EXPECT_TRUE(
87 tesseract_common::isIdentical(base_solver.getActiveJointNames(), comp_solver.getActiveJointNames(), false));
88 EXPECT_TRUE(tesseract_common::isIdentical(base_solver.getLinkNames(), comp_solver.getLinkNames(), false));
89 EXPECT_TRUE(tesseract_common::isIdentical(base_solver.getActiveLinkNames(), comp_solver.getActiveLinkNames(), false));
90 EXPECT_TRUE(tesseract_common::isIdentical(base_solver.getStaticLinkNames(), comp_solver.getStaticLinkNames(), false));
91
92 for (const auto& active_link_name : base_solver.getActiveLinkNames())
93 {
94 EXPECT_TRUE(comp_solver.isActiveLinkName(active_link_name));
95 }
96
97 for (const auto& link_name : base_solver.getLinkNames())
98 {
99 EXPECT_TRUE(comp_solver.hasLinkName(link_name));
100 }
101
102 for (int i = 0; i < 10; ++i)
103 {
104 SceneState base_random_state;
105 SceneState comp_state_const;
106
107 if (i < 3)
108 {
109 base_random_state = comp_solver.getRandomState();
110 comp_state_const = comp_solver.getState(base_random_state.joints);
111
112 comp_solver.setState(base_random_state.joints);
113 }
114 else if (i < 6)
115 {
116 base_random_state = base_solver.getRandomState();
117 comp_state_const = comp_solver.getState(base_random_state.joints);
118
119 std::vector<std::string> joint_names(base_random_state.joints.size());
120 Eigen::VectorXd joint_values(base_random_state.joints.size());
121 std::size_t j{ 0 };
122 for (const auto& joint : base_random_state.joints)
123 {
124 joint_names.at(j) = joint.first;
125 joint_values(static_cast<Eigen::Index>(j)) = joint.second;
126 ++j;
127 }
128 comp_solver.setState(joint_names, joint_values);
129 }
130 else if (i < 10)
131 {
132 base_random_state = base_solver.getRandomState();
133 comp_state_const = comp_solver.getState(base_random_state.joints);
134
135 std::vector<std::string> joint_names = comp_solver.getActiveJointNames();
136 Eigen::VectorXd joint_values = base_random_state.getJointValues(joint_names);
137 comp_solver.setState(joint_values);
138 }
139
140 const SceneState& comp_state = comp_solver.getState();
141
142 runCompareSceneStates(base_random_state, comp_state_const);
143 runCompareSceneStates(base_random_state, comp_state);
144
145 // Test differetn link transform methods
146 for (const auto& base_link_tf : base_random_state.link_transforms)
147 {
148 EXPECT_TRUE(base_link_tf.second.isApprox(comp_solver.getLinkTransform(base_link_tf.first), 1e-6));
149 }
150
151 std::vector<std::string> comp_link_names = comp_solver.getLinkNames();
152 tesseract_common::VectorIsometry3d comp_link_tf = comp_solver.getLinkTransforms();
153 for (std::size_t j = 0; j < comp_link_names.size(); ++j)
154 {
155 EXPECT_TRUE(base_random_state.link_transforms[comp_link_names.at(j)].isApprox(comp_link_tf.at(j), 1e-6));
156 }
157
158 for (const auto& from_link_name : comp_link_names)
159 {
160 for (const auto& to_link_name : comp_link_names)
161 {
162 Eigen::Isometry3d comp_tf = comp_solver.getRelativeLinkTransform(from_link_name, to_link_name);
163 Eigen::Isometry3d base_tf = base_random_state.link_transforms[from_link_name].inverse() *
164 base_random_state.link_transforms[to_link_name];
165 EXPECT_TRUE(base_tf.isApprox(comp_tf, 1e-6));
166 }
167 }
168 }
169}
170
171inline void runCompareStateSolverLimits(const SceneGraph& scene_graph, const StateSolver& comp_solver)
172{
173 std::vector<std::string> comp_joint_names = comp_solver.getActiveJointNames();
174 tesseract_common::KinematicLimits limits = comp_solver.getLimits();
175
176 for (Eigen::Index i = 0; i < static_cast<Eigen::Index>(comp_joint_names.size()); ++i)
177 {
178 const auto& scene_joint = scene_graph.getJoint(comp_joint_names[static_cast<std::size_t>(i)]);
179 EXPECT_NEAR(limits.joint_limits(i, 0), scene_joint->limits->lower, 1e-5);
180 EXPECT_NEAR(limits.joint_limits(i, 1), scene_joint->limits->upper, 1e-5);
181 EXPECT_NEAR(limits.velocity_limits(i), scene_joint->limits->velocity, 1e-5);
182 EXPECT_NEAR(limits.acceleration_limits(i), scene_joint->limits->acceleration, 1e-5);
183 }
184}
185
194inline static void numericalJacobian(Eigen::Ref<Eigen::MatrixXd> jacobian,
195 const Eigen::Isometry3d& change_base,
196 const StateSolver& state_solver,
197 const std::vector<std::string>& joint_names,
198 const Eigen::Ref<const Eigen::VectorXd>& joint_values,
199 const std::string& link_name,
200 const Eigen::Ref<const Eigen::Vector3d>& link_point)
201{
202 Eigen::VectorXd njvals;
203 double delta = 0.001;
204 tesseract_common::TransformMap poses;
205 if (joint_names.empty())
206 poses = state_solver.getState(joint_values).link_transforms;
207 else
208 poses = state_solver.getState(joint_names, joint_values).link_transforms;
209
210 Eigen::Isometry3d pose = poses[link_name];
211 pose = change_base * pose;
212
213 for (int i = 0; i < static_cast<int>(joint_values.size()); ++i)
214 {
215 njvals = joint_values;
216 njvals[i] += delta;
217 tesseract_common::TransformMap updated_poses;
218 if (joint_names.empty())
219 updated_poses = state_solver.getState(njvals).link_transforms;
220 else
221 updated_poses = state_solver.getState(joint_names, njvals).link_transforms;
222
223 Eigen::Isometry3d updated_pose = updated_poses[link_name];
224 updated_pose = change_base * updated_pose;
225
226 Eigen::Vector3d temp = pose * link_point;
227 Eigen::Vector3d temp2 = updated_pose * link_point;
228 jacobian(0, i) = (temp2.x() - temp.x()) / delta;
229 jacobian(1, i) = (temp2.y() - temp.y()) / delta;
230 jacobian(2, i) = (temp2.z() - temp.z()) / delta;
231
232 Eigen::AngleAxisd r12(pose.rotation().transpose() * updated_pose.rotation()); // rotation from p1 -> p2
233 double theta = r12.angle();
234 theta = copysign(fmod(fabs(theta), 2.0 * M_PI), theta);
235 if (theta < -M_PI)
236 theta = theta + 2. * M_PI;
237 if (theta > M_PI)
238 theta = theta - 2. * M_PI;
239 Eigen::VectorXd omega = (pose.rotation() * r12.axis() * theta) / delta;
240 jacobian(3, i) = omega(0);
241 jacobian(4, i) = omega(1);
242 jacobian(5, i) = omega(2);
243 }
244}
245
256inline void runCompareJacobian(StateSolver& state_solver,
257 const std::vector<std::string>& joint_names,
258 const Eigen::VectorXd& jvals,
259 const std::string& link_name,
260 const Eigen::Vector3d& link_point,
261 const Eigen::Isometry3d& change_base)
262{
263 Eigen::MatrixXd jacobian, numerical_jacobian;
264 jacobian.resize(6, jvals.size());
265
266 tesseract_common::TransformMap poses;
267
268 // The numerical jacobian orders things base on the provided joint list
269 // The order needs to be calculated to compare
270 std::vector<std::string> solver_jn = state_solver.getActiveJointNames();
271 std::vector<long> order;
272 order.reserve(solver_jn.size());
273 if (joint_names.empty())
274 {
275 for (int i = 0; i < static_cast<int>(solver_jn.size()); ++i)
276 order.push_back(i);
277
278 poses = state_solver.getState(jvals).link_transforms;
279 jacobian = state_solver.getJacobian(jvals, link_name);
280 }
281 else
282 {
283 for (const auto& joint_name : solver_jn)
284 order.push_back(
285 std::distance(joint_names.begin(), std::find(joint_names.begin(), joint_names.end(), joint_name)));
286
287 poses = state_solver.getState(joint_names, jvals).link_transforms;
288 jacobian = state_solver.getJacobian(joint_names, jvals, link_name);
289 }
290
291 tesseract_common::jacobianChangeBase(jacobian, change_base);
292 tesseract_common::jacobianChangeRefPoint(jacobian, (change_base * poses[link_name]).linear() * link_point);
293
294 numerical_jacobian.resize(6, jvals.size());
295 numericalJacobian(numerical_jacobian, change_base, state_solver, joint_names, jvals, link_name, link_point);
296
297 for (int i = 0; i < 6; ++i)
298 {
299 for (int j = 0; j < static_cast<int>(jvals.size()); ++j)
300 {
301 EXPECT_NEAR(numerical_jacobian(i, order[static_cast<std::size_t>(j)]), jacobian(i, j), 1e-3);
302 }
303 }
304}
305
306inline void runCompareJacobian(StateSolver& state_solver,
307 const std::unordered_map<std::string, double>& joints_values,
308 const std::string& link_name,
309 const Eigen::Vector3d& link_point,
310 const Eigen::Isometry3d& change_base)
311{
312 Eigen::MatrixXd jacobian, numerical_jacobian;
313 jacobian.resize(6, static_cast<Eigen::Index>(joints_values.size()));
314
315 std::vector<std::string> joint_names;
316 Eigen::VectorXd jvals(joints_values.size());
317 Eigen::Index j{ 0 };
318 for (const auto& jv : joints_values)
319 {
320 joint_names.push_back(jv.first);
321 jvals(j++) = jv.second;
322 }
323
324 tesseract_common::TransformMap poses;
325
326 // The numerical jacobian orders things base on the provided joint list
327 // The order needs to be calculated to compare
328 std::vector<std::string> solver_jn = state_solver.getActiveJointNames();
329 std::vector<long> order;
330 order.reserve(solver_jn.size());
331 for (const auto& joint_name : solver_jn)
332 order.push_back(std::distance(joint_names.begin(), std::find(joint_names.begin(), joint_names.end(), joint_name)));
333
334 poses = state_solver.getState(joints_values).link_transforms;
335 jacobian = state_solver.getJacobian(joints_values, link_name);
336
337 tesseract_common::jacobianChangeBase(jacobian, change_base);
338 tesseract_common::jacobianChangeRefPoint(jacobian, (change_base * poses[link_name]).linear() * link_point);
339
340 numerical_jacobian.resize(6, static_cast<Eigen::Index>(joints_values.size()));
341 numericalJacobian(numerical_jacobian, change_base, state_solver, joint_names, jvals, link_name, link_point);
342
343 for (int i = 0; i < 6; ++i)
344 {
345 for (int j = 0; j < static_cast<int>(jvals.size()); ++j)
346 {
347 EXPECT_NEAR(numerical_jacobian(i, order[static_cast<std::size_t>(j)]), jacobian(i, j), 1e-3);
348 }
349 }
350}
351
352template <typename S>
353inline void runJacobianTest()
354{
355 // Get the scene graph
356 auto scene_graph = getSceneGraph();
357 auto state_solver = S(*scene_graph);
358 StateSolver::UPtr state_solver_clone = state_solver.clone();
359
360 std::vector<std::string> joint_names_empty;
361 std::vector<std::string> link_names = { "base_link", "link_1", "link_2", "link_3", "link_4",
362 "link_5", "link_6", "link_7", "tool0" };
363
365 // Test forward kinematics when tip link is the base of the chain
367 Eigen::VectorXd jvals;
368 jvals.resize(7);
369
370 // jvals(0) = -0.785398;
371 // jvals(1) = 0.785398;
372 // jvals(2) = -0.785398;
373 // jvals(3) = 0.785398;
374 // jvals(4) = -0.785398;
375 // jvals(5) = 0.785398;
376 // jvals(6) = -0.785398;
377 jvals(0) = -0.1;
378 jvals(1) = 0.2;
379 jvals(2) = -0.3;
380 jvals(3) = 0.4;
381 jvals(4) = -0.5;
382 jvals(5) = 0.6;
383 jvals(6) = -0.7;
384
385 std::unordered_map<std::string, double> jv_map;
386 for (Eigen::Index i = 0; i < jvals.rows(); ++i)
387 jv_map["joint_a" + std::to_string(i + 1)] = jvals(i);
388
390 // Test Jacobian
392 {
393 Eigen::Vector3d link_point(0, 0, 0);
394 for (const auto& link_name : link_names)
395 {
396 runCompareJacobian(state_solver, joint_names_empty, jvals, link_name, link_point, Eigen::Isometry3d::Identity());
397 runCompareJacobian(
398 *state_solver_clone, joint_names_empty, jvals, link_name, link_point, Eigen::Isometry3d::Identity());
399 }
400
401 // NOLINTNEXTLINE
402 EXPECT_ANY_THROW(runCompareJacobian(
403 state_solver, joint_names_empty, jvals, "", link_point, Eigen::Isometry3d::Identity())); // NOLINT
404
405 // NOLINTNEXTLINE
406 EXPECT_ANY_THROW(runCompareJacobian(
407 *state_solver_clone, joint_names_empty, jvals, "", link_point, Eigen::Isometry3d::Identity())); // NOLINT
408 }
409
411 // Test Jacobian at Point
413 for (int k = 0; k < 3; ++k)
414 {
415 Eigen::Vector3d link_point(0, 0, 0);
416 link_point[k] = 1;
417
418 for (const auto& link_name : link_names)
419 {
420 runCompareJacobian(state_solver, joint_names_empty, jvals, link_name, link_point, Eigen::Isometry3d::Identity());
421 runCompareJacobian(
422 *state_solver_clone, joint_names_empty, jvals, link_name, link_point, Eigen::Isometry3d::Identity());
423 }
424
425 // NOLINTNEXTLINE
426 EXPECT_ANY_THROW(runCompareJacobian(
427 state_solver, joint_names_empty, jvals, "", link_point, Eigen::Isometry3d::Identity())); // NOLINT
428
429 // NOLINTNEXTLINE
430 EXPECT_ANY_THROW(runCompareJacobian(
431 *state_solver_clone, joint_names_empty, jvals, "", link_point, Eigen::Isometry3d::Identity())); // NOLINT
432 }
433
435 // Test Jacobian with change base
437 for (int k = 0; k < 3; ++k)
438 {
439 Eigen::Vector3d link_point(0, 0, 0);
440 Eigen::Isometry3d change_base;
441 change_base.setIdentity();
442 change_base(0, 0) = 0;
443 change_base(1, 0) = 1;
444 change_base(0, 1) = -1;
445 change_base(1, 1) = 0;
446 change_base.translation() = Eigen::Vector3d(0, 0, 0);
447 change_base.translation()[k] = 1;
448
449 for (const auto& link_name : link_names)
450 {
451 runCompareJacobian(state_solver, joint_names_empty, jvals, link_name, link_point, change_base);
452 runCompareJacobian(*state_solver_clone, joint_names_empty, jvals, link_name, link_point, change_base);
453 }
454
455 // NOLINTNEXTLINE
456 EXPECT_ANY_THROW(
457 runCompareJacobian(state_solver, joint_names_empty, jvals, "", link_point, change_base)); // NOLINT
458
459 // NOLINTNEXTLINE
460 EXPECT_ANY_THROW(
461 runCompareJacobian(*state_solver_clone, joint_names_empty, jvals, "", link_point, change_base)); // NOLINT
462 }
463
465 // Test Jacobian at point with change base
467 for (int k = 0; k < 3; ++k)
468 {
469 Eigen::Vector3d link_point(0, 0, 0);
470 link_point[k] = 1;
471
472 Eigen::Isometry3d change_base;
473 change_base.setIdentity();
474 change_base(0, 0) = 0;
475 change_base(1, 0) = 1;
476 change_base(0, 1) = -1;
477 change_base(1, 1) = 0;
478 change_base.translation() = link_point;
479
480 for (const auto& link_name : link_names)
481 {
482 runCompareJacobian(state_solver, joint_names_empty, jvals, link_name, link_point, change_base);
483 runCompareJacobian(*state_solver_clone, joint_names_empty, jvals, link_name, link_point, change_base);
484 }
485
486 // NOLINTNEXTLINE
487 EXPECT_ANY_THROW(
488 runCompareJacobian(state_solver, joint_names_empty, jvals, "", link_point, change_base)); // NOLINT
489
490 // NOLINTNEXTLINE
491 EXPECT_ANY_THROW(
492 runCompareJacobian(*state_solver_clone, joint_names_empty, jvals, "", link_point, change_base)); // NOLINT
493 }
494
496 // Test Jacobian with joint names
498 std::vector<std::string> joint_names = state_solver.getActiveJointNames();
499 {
500 Eigen::Vector3d link_point(0, 0, 0);
501 for (const auto& link_name : link_names)
502 {
503 runCompareJacobian(state_solver, joint_names, jvals, link_name, link_point, Eigen::Isometry3d::Identity());
504 runCompareJacobian(*state_solver_clone, joint_names, jvals, link_name, link_point, Eigen::Isometry3d::Identity());
505
506 runCompareJacobian(state_solver, jv_map, link_name, link_point, Eigen::Isometry3d::Identity());
507 runCompareJacobian(*state_solver_clone, jv_map, link_name, link_point, Eigen::Isometry3d::Identity());
508 }
509
510 // NOLINTNEXTLINE
511 EXPECT_ANY_THROW(
512 runCompareJacobian(state_solver, joint_names, jvals, "", link_point, Eigen::Isometry3d::Identity())); // NOLINT
513
514 // NOLINTNEXTLINE
515 EXPECT_ANY_THROW(runCompareJacobian(
516 *state_solver_clone, joint_names, jvals, "", link_point, Eigen::Isometry3d::Identity())); // NOLINT
517 }
518
520 // Test Jacobian at Point
522 for (int k = 0; k < 3; ++k)
523 {
524 Eigen::Vector3d link_point(0, 0, 0);
525 link_point[k] = 1;
526
527 for (const auto& link_name : link_names)
528 {
529 runCompareJacobian(state_solver, joint_names, jvals, link_name, link_point, Eigen::Isometry3d::Identity());
530 runCompareJacobian(*state_solver_clone, joint_names, jvals, link_name, link_point, Eigen::Isometry3d::Identity());
531
532 runCompareJacobian(state_solver, jv_map, link_name, link_point, Eigen::Isometry3d::Identity());
533 runCompareJacobian(*state_solver_clone, jv_map, link_name, link_point, Eigen::Isometry3d::Identity());
534 }
535
536 // NOLINTNEXTLINE
537 EXPECT_ANY_THROW(
538 runCompareJacobian(state_solver, joint_names, jvals, "", link_point, Eigen::Isometry3d::Identity())); // NOLINT
539
540 // NOLINTNEXTLINE
541 EXPECT_ANY_THROW(runCompareJacobian(
542 *state_solver_clone, joint_names, jvals, "", link_point, Eigen::Isometry3d::Identity())); // NOLINT
543 }
544
546 // Test Jacobian with change base
548 for (int k = 0; k < 3; ++k)
549 {
550 Eigen::Vector3d link_point(0, 0, 0);
551 Eigen::Isometry3d change_base;
552 change_base.setIdentity();
553 change_base(0, 0) = 0;
554 change_base(1, 0) = 1;
555 change_base(0, 1) = -1;
556 change_base(1, 1) = 0;
557 change_base.translation() = Eigen::Vector3d(0, 0, 0);
558 change_base.translation()[k] = 1;
559
560 for (const auto& link_name : link_names)
561 {
562 runCompareJacobian(state_solver, joint_names, jvals, link_name, link_point, change_base);
563 runCompareJacobian(*state_solver_clone, joint_names, jvals, link_name, link_point, change_base);
564
565 runCompareJacobian(state_solver, jv_map, link_name, link_point, change_base);
566 runCompareJacobian(*state_solver_clone, jv_map, link_name, link_point, change_base);
567 }
568
569 // NOLINTNEXTLINE
570 EXPECT_ANY_THROW(runCompareJacobian(state_solver, joint_names, jvals, "", link_point, change_base)); // NOLINT
571
572 // NOLINTNEXTLINE
573 EXPECT_ANY_THROW(
574 runCompareJacobian(*state_solver_clone, joint_names, jvals, "", link_point, change_base)); // NOLINT
575 }
576
578 // Test Jacobian at point with change base
580 for (int k = 0; k < 3; ++k)
581 {
582 Eigen::Vector3d link_point(0, 0, 0);
583 link_point[k] = 1;
584
585 Eigen::Isometry3d change_base;
586 change_base.setIdentity();
587 change_base(0, 0) = 0;
588 change_base(1, 0) = 1;
589 change_base(0, 1) = -1;
590 change_base(1, 1) = 0;
591 change_base.translation() = link_point;
592
593 for (const auto& link_name : link_names)
594 {
595 runCompareJacobian(state_solver, joint_names, jvals, link_name, link_point, change_base);
596 runCompareJacobian(*state_solver_clone, joint_names, jvals, link_name, link_point, change_base);
597
598 runCompareJacobian(state_solver, jv_map, link_name, link_point, change_base);
599 runCompareJacobian(*state_solver_clone, jv_map, link_name, link_point, change_base);
600 }
601
602 // NOLINTNEXTLINE
603 EXPECT_ANY_THROW(runCompareJacobian(state_solver, joint_names, jvals, "", link_point, change_base)); // NOLINT
604
605 // NOLINTNEXTLINE
606 EXPECT_ANY_THROW(
607 runCompareJacobian(*state_solver_clone, joint_names, jvals, "", link_point, change_base)); // NOLINT
608 }
609
611 // Test Jacobian with joint names in different order
613 std::reverse(joint_names.begin(), joint_names.end());
614 jvals(0) = -0.7;
615 jvals(1) = 0.6;
616 jvals(2) = -0.5;
617 jvals(3) = 0.4;
618 jvals(4) = -0.3;
619 jvals(5) = 0.2;
620 jvals(6) = -0.1;
621
622 {
623 Eigen::Vector3d link_point(0, 0, 0);
624 for (const auto& link_name : link_names)
625 {
626 runCompareJacobian(state_solver, joint_names, jvals, link_name, link_point, Eigen::Isometry3d::Identity());
627 runCompareJacobian(*state_solver_clone, joint_names, jvals, link_name, link_point, Eigen::Isometry3d::Identity());
628
629 runCompareJacobian(state_solver, jv_map, link_name, link_point, Eigen::Isometry3d::Identity());
630 runCompareJacobian(*state_solver_clone, jv_map, link_name, link_point, Eigen::Isometry3d::Identity());
631 }
632
633 // NOLINTNEXTLINE
634 EXPECT_ANY_THROW(
635 runCompareJacobian(state_solver, joint_names, jvals, "", link_point, Eigen::Isometry3d::Identity())); // NOLINT
636
637 // NOLINTNEXTLINE
638 EXPECT_ANY_THROW(runCompareJacobian(
639 *state_solver_clone, joint_names, jvals, "", link_point, Eigen::Isometry3d::Identity())); // NOLINT
640 }
641
643 // Test Jacobian at Point
645 for (int k = 0; k < 3; ++k)
646 {
647 Eigen::Vector3d link_point(0, 0, 0);
648 link_point[k] = 1;
649
650 for (const auto& link_name : link_names)
651 {
652 runCompareJacobian(state_solver, joint_names, jvals, link_name, link_point, Eigen::Isometry3d::Identity());
653 runCompareJacobian(*state_solver_clone, joint_names, jvals, link_name, link_point, Eigen::Isometry3d::Identity());
654
655 runCompareJacobian(state_solver, jv_map, link_name, link_point, Eigen::Isometry3d::Identity());
656 runCompareJacobian(*state_solver_clone, jv_map, link_name, link_point, Eigen::Isometry3d::Identity());
657 }
658
659 // NOLINTNEXTLINE
660 EXPECT_ANY_THROW(
661 runCompareJacobian(state_solver, joint_names, jvals, "", link_point, Eigen::Isometry3d::Identity())); // NOLINT
662
663 // NOLINTNEXTLINE
664 EXPECT_ANY_THROW(runCompareJacobian(
665 *state_solver_clone, joint_names, jvals, "", link_point, Eigen::Isometry3d::Identity())); // NOLINT
666 }
667
669 // Test Jacobian with change base
671 for (int k = 0; k < 3; ++k)
672 {
673 Eigen::Vector3d link_point(0, 0, 0);
674 Eigen::Isometry3d change_base;
675 change_base.setIdentity();
676 change_base(0, 0) = 0;
677 change_base(1, 0) = 1;
678 change_base(0, 1) = -1;
679 change_base(1, 1) = 0;
680 change_base.translation() = Eigen::Vector3d(0, 0, 0);
681 change_base.translation()[k] = 1;
682
683 for (const auto& link_name : link_names)
684 {
685 runCompareJacobian(state_solver, joint_names, jvals, link_name, link_point, change_base);
686 runCompareJacobian(*state_solver_clone, joint_names, jvals, link_name, link_point, change_base);
687
688 runCompareJacobian(state_solver, jv_map, link_name, link_point, change_base);
689 runCompareJacobian(*state_solver_clone, jv_map, link_name, link_point, change_base);
690 }
691
692 EXPECT_ANY_THROW(runCompareJacobian(state_solver, joint_names, jvals, "", link_point, change_base)); // NOLINT
693
694 // NOLINTNEXTLINE
695 EXPECT_ANY_THROW(
696 runCompareJacobian(*state_solver_clone, joint_names, jvals, "", link_point, change_base)); // NOLINT
697 }
698
700 // Test Jacobian at point with change base
702 for (int k = 0; k < 3; ++k)
703 {
704 Eigen::Vector3d link_point(0, 0, 0);
705 link_point[k] = 1;
706
707 Eigen::Isometry3d change_base;
708 change_base.setIdentity();
709 change_base(0, 0) = 0;
710 change_base(1, 0) = 1;
711 change_base(0, 1) = -1;
712 change_base(1, 1) = 0;
713 change_base.translation() = link_point;
714
715 for (const auto& link_name : link_names)
716 {
717 runCompareJacobian(state_solver, joint_names, jvals, link_name, link_point, change_base);
718 runCompareJacobian(*state_solver_clone, joint_names, jvals, link_name, link_point, change_base);
719
720 runCompareJacobian(state_solver, jv_map, link_name, link_point, change_base);
721 runCompareJacobian(*state_solver_clone, jv_map, link_name, link_point, change_base);
722 }
723
724 EXPECT_ANY_THROW(runCompareJacobian(state_solver, joint_names, jvals, "", link_point, change_base)); // NOLINT
725
726 // NOLINTNEXTLINE
727 EXPECT_ANY_THROW(
728 runCompareJacobian(*state_solver_clone, joint_names, jvals, "", link_point, change_base)); // NOLINT
729 }
730}
731
732template <typename S>
733void runAddandRemoveLinkTest()
734{
735 // Get the scene graph
736 auto scene_graph = getSceneGraph();
737 auto state_solver = S(*scene_graph);
738
739 auto visual = std::make_shared<Visual>();
740 visual->geometry = std::make_shared<tesseract_geometry::Box>(1, 1, 1);
741 auto collision = std::make_shared<Collision>();
742 collision->geometry = std::make_shared<tesseract_geometry::Box>(1, 1, 1);
743
744 const std::string link_name1 = "link_n1";
745 const std::string link_name2 = "link_n2";
746 const std::string joint_name1 = "joint_n1";
747 const std::string joint_name2 = "joint_n2";
748
749 Link link_1(link_name1);
750 link_1.visual.push_back(visual);
751 link_1.collision.push_back(collision);
752
753 Joint joint_1(joint_name1);
754 joint_1.parent_link_name = scene_graph->getRoot();
755 joint_1.child_link_name = link_name1;
756 joint_1.type = JointType::FIXED;
757
758 Link link_2(link_name2);
759
760 Joint joint_2(joint_name2);
761 joint_2.parent_to_joint_origin_transform.translation()(0) = 1.25;
762 joint_2.parent_link_name = link_name1;
763 joint_2.child_link_name = link_name2;
764 joint_2.type = JointType::FIXED;
765
766 // Test adding link
767
768 EXPECT_TRUE(scene_graph->addLink(link_1, joint_1));
769 EXPECT_TRUE(state_solver.addLink(link_1, joint_1));
770
771 auto base_state_solver = std::make_shared<KDLStateSolver>(*scene_graph);
772
773 runCompareStateSolver(*base_state_solver, state_solver);
774 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
775
776 // Test Clone
777 StateSolver::UPtr state_solver_clone = state_solver.clone();
778 runCompareStateSolver(*base_state_solver, *state_solver_clone);
779
780 std::vector<std::string> joint_names = state_solver.getActiveJointNames();
781 SceneState state = state_solver.getState();
782 // Fixed joints are not listed
783 EXPECT_TRUE(std::find(joint_names.begin(), joint_names.end(), joint_name1) == joint_names.end());
784 EXPECT_TRUE(state.link_transforms.find(link_name1) != state.link_transforms.end());
785 EXPECT_TRUE(state.joint_transforms.find(joint_name1) != state.joint_transforms.end());
786 // Fixed joints are not listed
787 EXPECT_TRUE(state.joints.find(joint_name1) == state.joints.end());
788
789 EXPECT_TRUE(scene_graph->addLink(link_2, joint_2));
790 EXPECT_TRUE(state_solver.addLink(link_2, joint_2));
791
792 base_state_solver = std::make_shared<KDLStateSolver>(*scene_graph);
793
794 runCompareStateSolver(*base_state_solver, state_solver);
795 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
796
797 // Test Clone
798 state_solver_clone = state_solver.clone();
799 runCompareStateSolver(*base_state_solver, *state_solver_clone);
800
801 joint_names = state_solver.getActiveJointNames();
802 state = state_solver.getState();
803 // Fixed joints are not listed
804 EXPECT_TRUE(std::find(joint_names.begin(), joint_names.end(), joint_name2) == joint_names.end());
805 EXPECT_TRUE(state.link_transforms.find(link_name2) != state.link_transforms.end());
806 EXPECT_TRUE(state.joint_transforms.find(joint_name2) != state.joint_transforms.end());
807 // Fixed joints are not listed
808 EXPECT_TRUE(state.joints.find(joint_name2) == state.joints.end());
809
810 scene_graph->saveDOT(tesseract_common::getTempPath() + "state_solver_before_remove_link_unit.dot");
811
812 // Test removing link
813 EXPECT_TRUE(scene_graph->removeLink(link_name1, true));
814 EXPECT_TRUE(state_solver.removeLink(link_name1));
815
816 base_state_solver = std::make_shared<KDLStateSolver>(*scene_graph);
817
818 runCompareStateSolver(*base_state_solver, state_solver);
819 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
820
821 // Test Clone
822 state_solver_clone = state_solver.clone();
823 runCompareStateSolver(*base_state_solver, *state_solver_clone);
824
825 joint_names = state_solver.getActiveJointNames();
826 state = state_solver.getState();
827 EXPECT_TRUE(std::find(joint_names.begin(), joint_names.end(), joint_name1) == joint_names.end());
828 EXPECT_TRUE(state.link_transforms.find(link_name1) == state.link_transforms.end());
829 EXPECT_TRUE(state.joint_transforms.find(joint_name1) == state.joint_transforms.end());
830 EXPECT_TRUE(state.joints.find(joint_name1) == state.joints.end());
831 EXPECT_TRUE(state.link_transforms.find(link_name2) == state.link_transforms.end());
832 EXPECT_TRUE(state.joint_transforms.find(joint_name2) == state.joint_transforms.end());
833 EXPECT_TRUE(state.joints.find(joint_name2) == state.joints.end());
834
835 scene_graph->saveDOT(tesseract_common::getTempPath() + "state_solver_after_remove_link_unit.dot");
836
837 // Test against double removing
838
839 EXPECT_FALSE(state_solver.removeLink(link_name1));
840 runCompareStateSolver(*base_state_solver, state_solver);
841 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
842
843 // Test Clone
844 state_solver_clone = state_solver.clone();
845 runCompareStateSolver(*base_state_solver, *state_solver_clone);
846
847 EXPECT_FALSE(state_solver.removeLink(link_name2));
848 runCompareStateSolver(*base_state_solver, state_solver);
849 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
850
851 // Test Clone
852 state_solver_clone = state_solver.clone();
853 runCompareStateSolver(*base_state_solver, *state_solver_clone);
854
855 EXPECT_FALSE(state_solver.removeJoint(joint_name1));
856 runCompareStateSolver(*base_state_solver, state_solver);
857 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
858
859 // Test Clone
860 state_solver_clone = state_solver.clone();
861 runCompareStateSolver(*base_state_solver, *state_solver_clone);
862
863 EXPECT_FALSE(state_solver.removeJoint(joint_name2));
864 runCompareStateSolver(*base_state_solver, state_solver);
865 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
866
867 // Test Clone
868 state_solver_clone = state_solver.clone();
869 runCompareStateSolver(*base_state_solver, *state_solver_clone);
870
872
873 // Test adding link
874
875 EXPECT_TRUE(scene_graph->addLink(link_1, joint_1));
876 EXPECT_TRUE(state_solver.addLink(link_1, joint_1));
877
878 base_state_solver = std::make_shared<KDLStateSolver>(*scene_graph);
879
880 runCompareStateSolver(*base_state_solver, state_solver);
881 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
882
883 // Test Clone
884 state_solver_clone = state_solver.clone();
885 runCompareStateSolver(*base_state_solver, *state_solver_clone);
886
887 joint_names = state_solver.getActiveJointNames();
888 state = state_solver.getState();
889 // Fixed joints are not listed
890 EXPECT_TRUE(std::find(joint_names.begin(), joint_names.end(), joint_name1) == joint_names.end());
891 EXPECT_TRUE(state.link_transforms.find(link_name1) != state.link_transforms.end());
892 EXPECT_TRUE(state.joint_transforms.find(joint_name1) != state.joint_transforms.end());
893 // Fixed joints are not listed
894 EXPECT_TRUE(state.joints.find(joint_name1) == state.joints.end());
895
896 EXPECT_TRUE(scene_graph->addLink(link_2, joint_2));
897 EXPECT_TRUE(state_solver.addLink(link_2, joint_2));
898
899 base_state_solver = std::make_shared<KDLStateSolver>(*scene_graph);
900
901 runCompareStateSolver(*base_state_solver, state_solver);
902 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
903
904 // Test Clone
905 state_solver_clone = state_solver.clone();
906 runCompareStateSolver(*base_state_solver, *state_solver_clone);
907
908 joint_names = state_solver.getActiveJointNames();
909 state = state_solver.getState();
910 // Fixed joints are not listed
911 EXPECT_TRUE(std::find(joint_names.begin(), joint_names.end(), joint_name2) == joint_names.end());
912 EXPECT_TRUE(state.link_transforms.find(link_name2) != state.link_transforms.end());
913 EXPECT_TRUE(state.joint_transforms.find(joint_name2) != state.joint_transforms.end());
914 // Fixed joints are not listed
915 EXPECT_TRUE(state.joints.find(joint_name2) == state.joints.end());
916
917 scene_graph->saveDOT(tesseract_common::getTempPath() + "state_solver_before_remove_link_unit2.dot");
918
919 // Test removing link
920 EXPECT_TRUE(scene_graph->removeJoint(joint_name1, true));
921 EXPECT_TRUE(state_solver.removeJoint(joint_name1));
922
923 base_state_solver = std::make_shared<KDLStateSolver>(*scene_graph);
924
925 runCompareStateSolver(*base_state_solver, state_solver);
926 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
927
928 // Test Clone
929 state_solver_clone = state_solver.clone();
930 runCompareStateSolver(*base_state_solver, *state_solver_clone);
931
932 joint_names = state_solver.getActiveJointNames();
933 state = state_solver.getState();
934 EXPECT_TRUE(std::find(joint_names.begin(), joint_names.end(), joint_name1) == joint_names.end());
935 EXPECT_TRUE(state.link_transforms.find(link_name1) == state.link_transforms.end());
936 EXPECT_TRUE(state.joint_transforms.find(joint_name1) == state.joint_transforms.end());
937 EXPECT_TRUE(state.joints.find(joint_name1) == state.joints.end());
938 EXPECT_TRUE(state.link_transforms.find(link_name2) == state.link_transforms.end());
939 EXPECT_TRUE(state.joint_transforms.find(joint_name2) == state.joint_transforms.end());
940 EXPECT_TRUE(state.joints.find(joint_name2) == state.joints.end());
941
942 scene_graph->saveDOT(tesseract_common::getTempPath() + "state_solver_after_remove_link_unit2.dot");
943
944 EXPECT_FALSE(state_solver.removeLink(link_name1));
945 runCompareStateSolver(*base_state_solver, state_solver);
946 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
947
948 // Test Clone
949 state_solver_clone = state_solver.clone();
950 runCompareStateSolver(*base_state_solver, *state_solver_clone);
951
952 EXPECT_FALSE(state_solver.removeLink(link_name2));
953 runCompareStateSolver(*base_state_solver, state_solver);
954 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
955
956 // Test Clone
957 state_solver_clone = state_solver.clone();
958 runCompareStateSolver(*base_state_solver, *state_solver_clone);
959
960 EXPECT_FALSE(state_solver.removeJoint(joint_name1));
961 runCompareStateSolver(*base_state_solver, state_solver);
962 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
963
964 // Test Clone
965 state_solver_clone = state_solver.clone();
966 runCompareStateSolver(*base_state_solver, *state_solver_clone);
967
968 EXPECT_FALSE(state_solver.removeJoint(joint_name2));
969 runCompareStateSolver(*base_state_solver, state_solver);
970 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
971
972 // Test Clone
973 state_solver_clone = state_solver.clone();
974 runCompareStateSolver(*base_state_solver, *state_solver_clone);
975
976 // Link already exists
977 Link link_exists("link_1");
978 EXPECT_FALSE(state_solver.addLink(link_exists, joint_1));
979 runCompareStateSolver(*base_state_solver, state_solver);
980 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
981
982 // Test Clone
983 state_solver_clone = state_solver.clone();
984 runCompareStateSolver(*base_state_solver, *state_solver_clone);
985
986 // joint already exists
987 Link link_10("link_10");
988 Joint joint_exists("joint_a1");
989 joint_exists.parent_link_name = scene_graph->getRoot();
990 joint_exists.child_link_name = "link_10";
991 joint_exists.type = JointType::FIXED;
992
993 EXPECT_FALSE(state_solver.addLink(link_10, joint_exists));
994 runCompareStateSolver(*base_state_solver, state_solver);
995 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
996
997 // Test Clone
998 state_solver_clone = state_solver.clone();
999 runCompareStateSolver(*base_state_solver, *state_solver_clone);
1000}
1001
1002template <typename S>
1003void runAddSceneGraphTest()
1004{
1005 // Get the scene graph
1006 auto scene_graph = getSceneGraph();
1007 auto state_solver = S(*scene_graph);
1008
1009 auto subgraph = std::make_unique<SceneGraph>();
1010 subgraph->setName("subgraph");
1011
1012 Joint joint_1_empty("provided_subgraph_joint");
1013 joint_1_empty.parent_link_name = "base_link";
1014 joint_1_empty.child_link_name = "prefix_subgraph_base_link";
1015 joint_1_empty.type = JointType::FIXED;
1016
1017 EXPECT_FALSE(scene_graph->insertSceneGraph(*subgraph, joint_1_empty));
1018 EXPECT_FALSE(state_solver.insertSceneGraph(*subgraph, joint_1_empty));
1019
1020 auto base_state_solver = std::make_shared<KDLStateSolver>(*scene_graph);
1021
1022 runCompareStateSolver(*base_state_solver, state_solver);
1023 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
1024
1025 // Test Clone
1026 StateSolver::UPtr state_solver_clone = state_solver.clone();
1027 runCompareStateSolver(*base_state_solver, *state_solver_clone);
1028
1029 subgraph = getSubSceneGraph();
1030
1031 const std::string subgraph_joint_name = "attach_subgraph_joint";
1032
1033 Joint joint(subgraph_joint_name);
1034 joint.parent_link_name = scene_graph->getRoot();
1035 joint.child_link_name = subgraph->getRoot();
1036 joint.type = JointType::FIXED;
1037
1038 EXPECT_TRUE(scene_graph->insertSceneGraph(*subgraph, joint));
1039 EXPECT_TRUE(state_solver.insertSceneGraph(*subgraph, joint));
1040
1041 base_state_solver = std::make_shared<KDLStateSolver>(*scene_graph);
1042
1043 runCompareStateSolver(*base_state_solver, state_solver);
1044 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
1045
1046 // Test Clone
1047 state_solver_clone = state_solver.clone();
1048 runCompareStateSolver(*base_state_solver, *state_solver_clone);
1049
1050 std::vector<std::string> joint_names = state_solver.getActiveJointNames();
1051 SceneState state = state_solver.getState();
1052 EXPECT_TRUE(std::find(joint_names.begin(), joint_names.end(), subgraph_joint_name) == joint_names.end());
1053 EXPECT_TRUE(state.link_transforms.find(subgraph->getRoot()) != state.link_transforms.end());
1054 EXPECT_TRUE(state.joint_transforms.find(subgraph_joint_name) != state.joint_transforms.end());
1055 EXPECT_TRUE(state.joints.find(subgraph_joint_name) == state.joints.end());
1056
1057 // Adding twice with the same name should fail
1058 EXPECT_FALSE(scene_graph->insertSceneGraph(*subgraph, joint));
1059 EXPECT_FALSE(state_solver.insertSceneGraph(*subgraph, joint));
1060
1061 base_state_solver = std::make_shared<KDLStateSolver>(*scene_graph);
1062
1063 runCompareStateSolver(*base_state_solver, state_solver);
1064 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
1065
1066 // Test Clone
1067 state_solver_clone = state_solver.clone();
1068 runCompareStateSolver(*base_state_solver, *state_solver_clone);
1069
1070 // Add subgraph with prefix
1071 std::string prefix = "prefix_";
1072 Joint prefix_joint(prefix + subgraph_joint_name);
1073 prefix_joint.parent_link_name = scene_graph->getRoot();
1074 prefix_joint.child_link_name = prefix + subgraph->getRoot();
1075 prefix_joint.type = JointType::FIXED;
1076
1077 EXPECT_TRUE(scene_graph->insertSceneGraph(*subgraph, prefix_joint, prefix));
1078 EXPECT_TRUE(state_solver.insertSceneGraph(*subgraph, prefix_joint, prefix));
1079
1080 base_state_solver = std::make_shared<KDLStateSolver>(*scene_graph);
1081
1082 runCompareStateSolver(*base_state_solver, state_solver);
1083 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
1084
1085 // Test Clone
1086 state_solver_clone = state_solver.clone();
1087 runCompareStateSolver(*base_state_solver, *state_solver_clone);
1088
1089 joint_names = state_solver.getActiveJointNames();
1090 state = state_solver.getState();
1091 EXPECT_TRUE(std::find(joint_names.begin(), joint_names.end(), prefix + subgraph_joint_name) == joint_names.end());
1092 EXPECT_TRUE(state.link_transforms.find(prefix + subgraph->getRoot()) != state.link_transforms.end());
1093 EXPECT_TRUE(state.joint_transforms.find(prefix + subgraph_joint_name) != state.joint_transforms.end());
1094 EXPECT_TRUE(state.joints.find(prefix + subgraph_joint_name) == state.joints.end());
1095
1096 // Add subgraph with prefix and joint
1097 prefix = "prefix2_";
1098 Joint prefix_joint2(prefix + subgraph_joint_name);
1099 prefix_joint2.parent_link_name = scene_graph->getRoot();
1100 prefix_joint2.child_link_name = prefix + subgraph->getRoot();
1101 prefix_joint2.type = JointType::FIXED;
1102
1103 EXPECT_TRUE(scene_graph->insertSceneGraph(*subgraph, prefix_joint2, prefix));
1104 EXPECT_TRUE(state_solver.insertSceneGraph(*subgraph, prefix_joint2, prefix));
1105
1106 base_state_solver = std::make_shared<KDLStateSolver>(*scene_graph);
1107
1108 runCompareStateSolver(*base_state_solver, state_solver);
1109 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
1110
1111 // Test Clone
1112 state_solver_clone = state_solver.clone();
1113 runCompareStateSolver(*base_state_solver, *state_solver_clone);
1114
1115 joint_names = state_solver.getActiveJointNames();
1116
1117 EXPECT_TRUE(std::find(joint_names.begin(), joint_names.end(), prefix + subgraph_joint_name) == joint_names.end());
1118 state = state_solver.getState();
1119 EXPECT_TRUE(state.link_transforms.find(prefix + subgraph->getRoot()) != state.link_transforms.end());
1120 EXPECT_TRUE(state.joint_transforms.find(prefix + subgraph_joint_name) != state.joint_transforms.end());
1121 EXPECT_TRUE(state.joints.find(prefix + subgraph_joint_name) == state.joints.end());
1122
1123 // Add empty subgraph with prefix and joint
1124 tesseract_scene_graph::SceneGraph empty_scene_graph;
1125 prefix = "prefix3_";
1126 Joint prefix_joint3(prefix + subgraph_joint_name);
1127 prefix_joint3.parent_link_name = scene_graph->getRoot();
1128 prefix_joint3.child_link_name = "empty";
1129 prefix_joint3.type = JointType::FIXED;
1130
1131 EXPECT_FALSE(state_solver.insertSceneGraph(empty_scene_graph, prefix_joint3, prefix));
1132
1133 base_state_solver = std::make_shared<KDLStateSolver>(*scene_graph);
1134
1135 runCompareStateSolver(*base_state_solver, state_solver);
1136 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
1137
1138 // Test Clone
1139 state_solver_clone = state_solver.clone();
1140 runCompareStateSolver(*base_state_solver, *state_solver_clone);
1141}
1142
1143template <typename S>
1144void runChangeJointOriginTest()
1145{
1146 // Get the scene graph
1147 auto scene_graph = getSceneGraph();
1148 auto state_solver = S(*scene_graph);
1149
1150 const std::string link_name1 = "link_n1";
1151 const std::string joint_name1 = "joint_n1";
1152 Link link_1(link_name1);
1153
1154 Joint joint_1(joint_name1);
1155 joint_1.parent_link_name = scene_graph->getRoot();
1156 joint_1.child_link_name = link_name1;
1157 joint_1.type = JointType::FIXED;
1158
1159 EXPECT_TRUE(scene_graph->addLink(link_1, joint_1));
1160 EXPECT_TRUE(state_solver.addLink(link_1, joint_1));
1161
1162 auto base_state_solver = std::make_shared<KDLStateSolver>(*scene_graph);
1163
1164 runCompareStateSolver(*base_state_solver, state_solver);
1165 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
1166
1167 // Test Clone
1168 StateSolver::UPtr state_solver_clone = state_solver.clone();
1169 runCompareStateSolver(*base_state_solver, *state_solver_clone);
1170
1171 SceneState state = state_solver.getState();
1172 EXPECT_TRUE(state.link_transforms.find(link_name1) != state.link_transforms.end());
1173 EXPECT_TRUE(state.joint_transforms.find(joint_name1) != state.joint_transforms.end());
1174 EXPECT_TRUE(state.joints.find(joint_name1) == state.joints.end());
1175
1176 scene_graph->saveDOT(tesseract_common::getTempPath() + "state_solver_before_change_joint_origin_unit.dot");
1177
1178 Eigen::Isometry3d new_origin = Eigen::Isometry3d::Identity();
1179 new_origin.translation()(0) += 1.234;
1180
1181 EXPECT_TRUE(scene_graph->changeJointOrigin(joint_name1, new_origin));
1182 EXPECT_TRUE(state_solver.changeJointOrigin(joint_name1, new_origin));
1183
1184 base_state_solver = std::make_shared<KDLStateSolver>(*scene_graph);
1185
1186 runCompareStateSolver(*base_state_solver, state_solver);
1187 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
1188
1189 // Test Clone
1190 state_solver_clone = state_solver.clone();
1191 runCompareStateSolver(*base_state_solver, *state_solver_clone);
1192
1193 // Check that the origin got updated
1194 state = state_solver.getState();
1195 EXPECT_TRUE(state.link_transforms.at(link_name1).isApprox(new_origin));
1196 EXPECT_TRUE(state.joint_transforms.at(joint_name1).isApprox(new_origin));
1197
1198 scene_graph->saveDOT(tesseract_common::getTempPath() + "state_solver_after_change_joint_origin_unit.dot");
1199
1200 // Joint does not eixist
1201 EXPECT_FALSE(state_solver.changeJointOrigin("joint_does_not_exist", new_origin));
1202
1203 base_state_solver = std::make_shared<KDLStateSolver>(*scene_graph);
1204
1205 runCompareStateSolver(*base_state_solver, state_solver);
1206 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
1207
1208 // Test Clone
1209 state_solver_clone = state_solver.clone();
1210 runCompareStateSolver(*base_state_solver, *state_solver_clone);
1211}
1212
1213template <typename S>
1214void runMoveJointTest()
1215{
1216 // Get the scene graph
1217 auto scene_graph = getSceneGraph();
1218 auto state_solver = S(*scene_graph);
1219
1220 const std::string link_name1 = "link_n1";
1221 const std::string link_name2 = "link_n2";
1222 const std::string joint_name1 = "joint_n1";
1223 const std::string joint_name2 = "joint_n2";
1224 Link link_1(link_name1);
1225 Link link_2(link_name2);
1226
1227 Joint joint_1(joint_name1);
1228 joint_1.parent_link_name = scene_graph->getRoot();
1229 joint_1.child_link_name = link_name1;
1230 joint_1.type = JointType::FIXED;
1231
1232 Joint joint_2(joint_name2);
1233 joint_2.parent_to_joint_origin_transform.translation()(0) = 1.25;
1234 joint_2.parent_link_name = link_name1;
1235 joint_2.child_link_name = link_name2;
1236 joint_2.type = JointType::FIXED;
1237
1238 EXPECT_TRUE(scene_graph->addLink(link_1, joint_1));
1239 EXPECT_TRUE(state_solver.addLink(link_1, joint_1));
1240
1241 auto base_state_solver = std::make_shared<KDLStateSolver>(*scene_graph);
1242
1243 runCompareStateSolver(*base_state_solver, state_solver);
1244 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
1245
1246 // Test Clone
1247 StateSolver::UPtr state_solver_clone = state_solver.clone();
1248 runCompareStateSolver(*base_state_solver, *state_solver_clone);
1249
1250 SceneState state = state_solver.getState();
1251 EXPECT_TRUE(state.link_transforms.find(link_name1) != state.link_transforms.end());
1252 EXPECT_TRUE(state.joint_transforms.find(joint_name1) != state.joint_transforms.end());
1253 EXPECT_TRUE(state.joints.find(joint_name1) == state.joints.end());
1254
1255 EXPECT_TRUE(scene_graph->addLink(link_2, joint_2));
1256 EXPECT_TRUE(state_solver.addLink(link_2, joint_2));
1257
1258 base_state_solver = std::make_shared<KDLStateSolver>(*scene_graph);
1259
1260 runCompareStateSolver(*base_state_solver, state_solver);
1261
1262 // Test Clone
1263 state_solver_clone = state_solver.clone();
1264 runCompareStateSolver(*base_state_solver, *state_solver_clone);
1265
1266 std::vector<std::string> joint_names = state_solver.getActiveJointNames();
1267 state = state_solver.getState();
1268 EXPECT_TRUE(std::find(joint_names.begin(), joint_names.end(), joint_name1) == joint_names.end());
1269 EXPECT_TRUE(std::find(joint_names.begin(), joint_names.end(), joint_name2) == joint_names.end());
1270 EXPECT_TRUE(state.link_transforms.find(link_name1) != state.link_transforms.end());
1271 EXPECT_TRUE(state.joint_transforms.find(joint_name1) != state.joint_transforms.end());
1272 EXPECT_TRUE(state.joints.find(joint_name1) == state.joints.end());
1273 EXPECT_TRUE(state.link_transforms.find(link_name2) != state.link_transforms.end());
1274 EXPECT_TRUE(state.joint_transforms.find(joint_name2) != state.joint_transforms.end());
1275 EXPECT_TRUE(state.joints.find(joint_name2) == state.joints.end());
1276
1277 scene_graph->saveDOT(tesseract_common::getTempPath() + "state_solver_before_move_joint_unit.dot");
1278
1279 EXPECT_TRUE(scene_graph->moveJoint(joint_name1, "tool0"));
1280 EXPECT_TRUE(state_solver.moveJoint(joint_name1, "tool0"));
1281
1282 base_state_solver = std::make_shared<KDLStateSolver>(*scene_graph);
1283
1284 runCompareStateSolver(*base_state_solver, state_solver);
1285 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
1286
1287 // Test Clone
1288 state_solver_clone = state_solver.clone();
1289 runCompareStateSolver(*base_state_solver, *state_solver_clone);
1290
1291 joint_names = state_solver.getActiveJointNames();
1292 state = state_solver.getState();
1293 EXPECT_TRUE(std::find(joint_names.begin(), joint_names.end(), joint_name1) == joint_names.end());
1294 EXPECT_TRUE(std::find(joint_names.begin(), joint_names.end(), joint_name2) == joint_names.end());
1295 EXPECT_TRUE(state.link_transforms.find(link_name1) != state.link_transforms.end());
1296 EXPECT_TRUE(state.joint_transforms.find(joint_name1) != state.joint_transforms.end());
1297 EXPECT_TRUE(state.joints.find(joint_name1) == state.joints.end());
1298 EXPECT_TRUE(state.link_transforms.find(link_name2) != state.link_transforms.end());
1299 EXPECT_TRUE(state.joint_transforms.find(joint_name2) != state.joint_transforms.end());
1300 EXPECT_TRUE(state.joints.find(joint_name2) == state.joints.end());
1301
1302 scene_graph->saveDOT(tesseract_common::getTempPath() + "state_solver_after_move_joint_unit.dot");
1303
1304 // Joint does not exist
1305 EXPECT_FALSE(state_solver.moveJoint("joint_does_not_exist", "tool0"));
1306
1307 base_state_solver = std::make_shared<KDLStateSolver>(*scene_graph);
1308
1309 runCompareStateSolver(*base_state_solver, state_solver);
1310 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
1311
1312 // Test Clone
1313 state_solver_clone = state_solver.clone();
1314 runCompareStateSolver(*base_state_solver, *state_solver_clone);
1315
1316 // Link does not exist
1317 EXPECT_FALSE(state_solver.moveJoint(joint_name1, "link_does_not_exist"));
1318
1319 base_state_solver = std::make_shared<KDLStateSolver>(*scene_graph);
1320
1321 runCompareStateSolver(*base_state_solver, state_solver);
1322 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
1323
1324 // Test Clone
1325 state_solver_clone = state_solver.clone();
1326 runCompareStateSolver(*base_state_solver, *state_solver_clone);
1327}
1328
1329template <typename S>
1330void runMoveLinkTest()
1331{
1332 // Get the scene graph
1333 auto scene_graph = getSceneGraph();
1334 auto state_solver = S(*scene_graph);
1335
1336 const std::string link_name1 = "link_n1";
1337 const std::string link_name2 = "link_n2";
1338 const std::string joint_name1 = "joint_n1";
1339 const std::string joint_name2 = "joint_n2";
1340 Link link_1(link_name1);
1341 Link link_2(link_name2);
1342
1343 Joint joint_1(joint_name1);
1344 joint_1.parent_link_name = scene_graph->getRoot();
1345 joint_1.child_link_name = link_name1;
1346 joint_1.type = JointType::FIXED;
1347
1348 Joint joint_2(joint_name2);
1349 joint_2.parent_to_joint_origin_transform.translation()(0) = 1.25;
1350 joint_2.parent_link_name = link_name1;
1351 joint_2.child_link_name = link_name2;
1352 joint_2.type = JointType::FIXED;
1353
1354 EXPECT_TRUE(scene_graph->addLink(link_1, joint_1));
1355 EXPECT_TRUE(state_solver.addLink(link_1, joint_1));
1356
1357 auto base_state_solver = std::make_shared<KDLStateSolver>(*scene_graph);
1358
1359 runCompareStateSolver(*base_state_solver, state_solver);
1360 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
1361
1362 // Test Clone
1363 StateSolver::UPtr state_solver_clone = state_solver.clone();
1364 runCompareStateSolver(*base_state_solver, *state_solver_clone);
1365
1366 SceneState state = state_solver.getState();
1367 EXPECT_TRUE(state.link_transforms.find(link_name1) != state.link_transforms.end());
1368 EXPECT_TRUE(state.joint_transforms.find(joint_name1) != state.joint_transforms.end());
1369 EXPECT_TRUE(state.joints.find(joint_name1) == state.joints.end());
1370
1371 EXPECT_TRUE(scene_graph->addLink(link_2, joint_2));
1372 EXPECT_TRUE(state_solver.addLink(link_2, joint_2));
1373
1374 base_state_solver = std::make_shared<KDLStateSolver>(*scene_graph);
1375
1376 runCompareStateSolver(*base_state_solver, state_solver);
1377 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
1378
1379 // Test Clone
1380 state_solver_clone = state_solver.clone();
1381 runCompareStateSolver(*base_state_solver, *state_solver_clone);
1382
1383 std::vector<std::string> joint_names = state_solver.getActiveJointNames();
1384 state = state_solver.getState();
1385 EXPECT_TRUE(std::find(joint_names.begin(), joint_names.end(), joint_name1) == joint_names.end());
1386 EXPECT_TRUE(std::find(joint_names.begin(), joint_names.end(), joint_name2) == joint_names.end());
1387 EXPECT_TRUE(state.link_transforms.find(link_name1) != state.link_transforms.end());
1388 EXPECT_TRUE(state.joint_transforms.find(joint_name1) != state.joint_transforms.end());
1389 EXPECT_TRUE(state.joints.find(joint_name1) == state.joints.end());
1390 EXPECT_TRUE(state.link_transforms.find(link_name2) != state.link_transforms.end());
1391 EXPECT_TRUE(state.joint_transforms.find(joint_name2) != state.joint_transforms.end());
1392 EXPECT_TRUE(state.joints.find(joint_name2) == state.joints.end());
1393
1394 scene_graph->saveDOT(tesseract_common::getTempPath() + "state_solver_before_move_link_unit.dot");
1395
1396 std::string moved_joint_name = joint_name1 + "_moved";
1397 Joint move_link_joint = joint_1.clone(moved_joint_name);
1398 move_link_joint.parent_link_name = "tool0";
1399
1400 EXPECT_TRUE(scene_graph->moveLink(move_link_joint));
1401 EXPECT_TRUE(state_solver.moveLink(move_link_joint));
1402
1403 base_state_solver = std::make_shared<KDLStateSolver>(*scene_graph);
1404
1405 runCompareStateSolver(*base_state_solver, state_solver);
1406 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
1407
1408 // Test Clone
1409 state_solver_clone = state_solver.clone();
1410 runCompareStateSolver(*base_state_solver, *state_solver_clone);
1411
1412 joint_names = state_solver.getActiveJointNames();
1413 state = state_solver.getState();
1414 EXPECT_TRUE(std::find(joint_names.begin(), joint_names.end(), joint_name1) == joint_names.end());
1415 EXPECT_TRUE(std::find(joint_names.begin(), joint_names.end(), moved_joint_name) == joint_names.end());
1416 EXPECT_TRUE(std::find(joint_names.begin(), joint_names.end(), joint_name2) == joint_names.end());
1417
1418 EXPECT_TRUE(state.link_transforms.find(link_name1) != state.link_transforms.end());
1419 EXPECT_TRUE(state.joint_transforms.find(joint_name1) == state.joint_transforms.end());
1420 EXPECT_TRUE(state.joint_transforms.find(moved_joint_name) != state.joint_transforms.end());
1421 EXPECT_TRUE(state.joints.find(joint_name1) == state.joints.end());
1422 EXPECT_TRUE(state.link_transforms.find(link_name2) != state.link_transforms.end());
1423 EXPECT_TRUE(state.joint_transforms.find(joint_name2) != state.joint_transforms.end());
1424 EXPECT_TRUE(state.joints.find(joint_name2) == state.joints.end());
1425
1426 scene_graph->saveDOT(tesseract_common::getTempPath() + "state_solver_after_move_link_unit.dot");
1427
1428 // Child link does not exist
1429 std::string moved_joint_name_err = joint_name1 + "_err";
1430 Joint move_link_joint_err1 = joint_1.clone(moved_joint_name_err);
1431 move_link_joint_err1.child_link_name = "link_does_not_exist";
1432 move_link_joint_err1.parent_link_name = "tool0";
1433
1434 EXPECT_FALSE(state_solver.moveLink(move_link_joint_err1));
1435
1436 base_state_solver = std::make_shared<KDLStateSolver>(*scene_graph);
1437
1438 runCompareStateSolver(*base_state_solver, state_solver);
1439 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
1440
1441 // Parent link does not exist
1442 Joint move_link_joint_err2 = joint_1.clone(moved_joint_name_err);
1443 move_link_joint_err2.parent_link_name = "link_does_not_exist";
1444
1445 EXPECT_FALSE(state_solver.moveLink(move_link_joint_err2));
1446
1447 base_state_solver = std::make_shared<KDLStateSolver>(*scene_graph);
1448
1449 runCompareStateSolver(*base_state_solver, state_solver);
1450 runCompareStateSolverLimits(*scene_graph, *base_state_solver);
1451}
1452
1453template <typename S>
1454void runChangeJointLimitsTest()
1455{
1456 // Get the scene graph
1457 auto scene_graph = getSceneGraph();
1458 auto state_solver = S(*scene_graph);
1459
1460 double new_lower = 1.0;
1461 double new_upper = 2.0;
1462 double new_velocity = 3.0;
1463 double new_acceleration = 4.0;
1464
1465 scene_graph->changeJointPositionLimits("joint_a1", new_lower, new_upper);
1466 scene_graph->changeJointVelocityLimits("joint_a1", new_velocity);
1467 scene_graph->changeJointAccelerationLimits("joint_a1", new_acceleration);
1468
1469 EXPECT_TRUE(state_solver.changeJointPositionLimits("joint_a1", new_lower, new_upper));
1470 EXPECT_TRUE(state_solver.changeJointVelocityLimits("joint_a1", new_velocity));
1471 EXPECT_TRUE(state_solver.changeJointAccelerationLimits("joint_a1", new_acceleration));
1472
1473 {
1474 std::vector<std::string> joint_names = state_solver.getActiveJointNames();
1475 long idx = std::distance(joint_names.begin(), std::find(joint_names.begin(), joint_names.end(), "joint_a1"));
1476 auto limits = state_solver.getLimits();
1477 EXPECT_NEAR(limits.joint_limits(idx, 0), new_lower, 1e-5);
1478 EXPECT_NEAR(limits.joint_limits(idx, 1), new_upper, 1e-5);
1479 EXPECT_NEAR(limits.velocity_limits(idx), new_velocity, 1e-5);
1480 EXPECT_NEAR(limits.acceleration_limits(idx), new_acceleration, 1e-5);
1481 }
1482
1483 { // Test Clone
1484 StateSolver::UPtr temp = state_solver.clone();
1485 S& state_solver_clone = static_cast<S&>(*temp);
1486
1487 std::vector<std::string> joint_names = state_solver_clone.getActiveJointNames();
1488 long idx = std::distance(joint_names.begin(), std::find(joint_names.begin(), joint_names.end(), "joint_a1"));
1489 auto limits = state_solver_clone.getLimits();
1490 EXPECT_NEAR(limits.joint_limits(idx, 0), new_lower, 1e-5);
1491 EXPECT_NEAR(limits.joint_limits(idx, 1), new_upper, 1e-5);
1492 EXPECT_NEAR(limits.velocity_limits(idx), new_velocity, 1e-5);
1493 EXPECT_NEAR(limits.acceleration_limits(idx), new_acceleration, 1e-5);
1494 }
1495
1496 // Joint does not exist
1497 double new_lower_err = 1.0 * 10;
1498 double new_upper_err = 2.0 * 10;
1499 double new_velocity_err = 3.0 * 10;
1500 double new_acceleration_err = 4.0 * 10;
1501 EXPECT_FALSE(state_solver.changeJointPositionLimits("joint_does_not_exist", new_lower_err, new_upper_err));
1502 EXPECT_FALSE(state_solver.changeJointVelocityLimits("joint_does_not_exist", new_velocity_err));
1503 EXPECT_FALSE(state_solver.changeJointAccelerationLimits("joint_does_not_exist", new_acceleration_err));
1504
1505 {
1506 std::vector<std::string> joint_names = state_solver.getActiveJointNames();
1507 long idx = std::distance(joint_names.begin(), std::find(joint_names.begin(), joint_names.end(), "joint_a1"));
1508 auto limits = state_solver.getLimits();
1509 EXPECT_NEAR(limits.joint_limits(idx, 0), new_lower, 1e-5);
1510 EXPECT_NEAR(limits.joint_limits(idx, 1), new_upper, 1e-5);
1511 EXPECT_NEAR(limits.velocity_limits(idx), new_velocity, 1e-5);
1512 EXPECT_NEAR(limits.acceleration_limits(idx), new_acceleration, 1e-5);
1513 }
1514
1515 { // Test Clone
1516 StateSolver::UPtr temp = state_solver.clone();
1517 S& state_solver_clone = static_cast<S&>(*temp);
1518
1519 std::vector<std::string> joint_names = state_solver_clone.getActiveJointNames();
1520 long idx = std::distance(joint_names.begin(), std::find(joint_names.begin(), joint_names.end(), "joint_a1"));
1521 auto limits = state_solver_clone.getLimits();
1522 EXPECT_NEAR(limits.joint_limits(idx, 0), new_lower, 1e-5);
1523 EXPECT_NEAR(limits.joint_limits(idx, 1), new_upper, 1e-5);
1524 EXPECT_NEAR(limits.velocity_limits(idx), new_velocity, 1e-5);
1525 EXPECT_NEAR(limits.acceleration_limits(idx), new_acceleration, 1e-5);
1526 }
1527}
1528
1529template <typename S>
1530void runReplaceJointTest()
1531{
1532 { // Replace joint with same type
1533 // Get the scene graph
1534 auto scene_graph = getSceneGraph();
1535 auto state_solver = S(*scene_graph);
1536
1537 Joint joint_1("joint_a1");
1538 joint_1.parent_to_joint_origin_transform.translation()(0) = 1.25;
1539 joint_1.parent_link_name = "base_link";
1540 joint_1.child_link_name = "link_1";
1541 joint_1.type = JointType::FIXED;
1542
1543 EXPECT_TRUE(scene_graph->removeJoint("joint_a1"));
1544 EXPECT_TRUE(scene_graph->addJoint(joint_1));
1545 EXPECT_TRUE(state_solver.replaceJoint(joint_1));
1546
1547 KDLStateSolver base_state_solver(*scene_graph);
1548
1549 runCompareStateSolver(base_state_solver, state_solver);
1550 runCompareStateSolverLimits(*scene_graph, base_state_solver);
1551
1552 // Test Clone
1553 StateSolver::UPtr state_solver_clone = state_solver.clone();
1554 runCompareStateSolver(base_state_solver, *state_solver_clone);
1555 }
1556
1557 { // Replace joint which exist but the link does not which should fail
1558 // Get the scene graph
1559 auto scene_graph = getSceneGraph();
1560 auto state_solver = S(*scene_graph);
1561
1562 Joint joint_1("joint_a1");
1563 joint_1.parent_to_joint_origin_transform.translation()(0) = 1.25;
1564 joint_1.parent_link_name = "base_link";
1565 joint_1.child_link_name = "link_2_does_not_exist";
1566 joint_1.type = JointType::FIXED;
1567
1568 EXPECT_FALSE(state_solver.replaceJoint(joint_1));
1569
1570 KDLStateSolver base_state_solver(*scene_graph);
1571
1572 runCompareStateSolver(base_state_solver, state_solver);
1573 runCompareStateSolverLimits(*scene_graph, base_state_solver);
1574
1575 // Test Clone
1576 StateSolver::UPtr state_solver_clone = state_solver.clone();
1577 runCompareStateSolver(base_state_solver, *state_solver_clone);
1578 }
1579
1580 { // Replace joint with same type but change transform
1581 // Get the scene graph
1582 auto scene_graph = getSceneGraph();
1583 auto state_solver = S(*scene_graph);
1584
1585 Joint new_joint_a1 = scene_graph->getJoint("joint_a1")->clone();
1586 new_joint_a1.parent_to_joint_origin_transform.translation()(0) = 1.25;
1587
1588 EXPECT_TRUE(scene_graph->removeJoint("joint_a1"));
1589 EXPECT_TRUE(scene_graph->addJoint(new_joint_a1));
1590 EXPECT_TRUE(state_solver.replaceJoint(new_joint_a1));
1591
1592 KDLStateSolver base_state_solver(*scene_graph);
1593
1594 runCompareStateSolver(base_state_solver, state_solver);
1595 runCompareStateSolverLimits(*scene_graph, base_state_solver);
1596
1597 // Test Clone
1598 StateSolver::UPtr state_solver_clone = state_solver.clone();
1599 runCompareStateSolver(base_state_solver, *state_solver_clone);
1600 }
1601
1602 { // Replace joint with different type (Fixed)
1603 // Get the scene graph
1604 auto scene_graph = getSceneGraph();
1605 auto state_solver = S(*scene_graph);
1606
1607 Joint new_joint_a1 = scene_graph->getJoint("joint_a1")->clone();
1608 new_joint_a1.parent_to_joint_origin_transform.translation()(0) = 1.25;
1609 new_joint_a1.type = JointType::FIXED;
1610
1611 EXPECT_TRUE(scene_graph->removeJoint("joint_a1"));
1612 EXPECT_TRUE(scene_graph->addJoint(new_joint_a1));
1613 EXPECT_TRUE(state_solver.replaceJoint(new_joint_a1));
1614
1615 KDLStateSolver base_state_solver(*scene_graph);
1616
1617 runCompareStateSolver(base_state_solver, state_solver);
1618 runCompareStateSolverLimits(*scene_graph, base_state_solver);
1619
1620 // Test Clone
1621 StateSolver::UPtr state_solver_clone = state_solver.clone();
1622 runCompareStateSolver(base_state_solver, *state_solver_clone);
1623 }
1624
1625 { // Replace joint with different type (Continuous)
1626 // Get the scene graph
1627 auto scene_graph = getSceneGraph();
1628 auto state_solver = S(*scene_graph);
1629
1630 Joint new_joint_a1 = scene_graph->getJoint("joint_a1")->clone();
1631 new_joint_a1.parent_to_joint_origin_transform.translation()(0) = 1.25;
1632 new_joint_a1.type = JointType::CONTINUOUS;
1633
1634 EXPECT_TRUE(scene_graph->removeJoint("joint_a1"));
1635 EXPECT_TRUE(scene_graph->addJoint(new_joint_a1));
1636 EXPECT_TRUE(state_solver.replaceJoint(new_joint_a1));
1637
1638 KDLStateSolver base_state_solver(*scene_graph);
1639
1640 runCompareStateSolver(base_state_solver, state_solver);
1641 runCompareStateSolverLimits(*scene_graph, base_state_solver);
1642
1643 // Test Clone
1644 StateSolver::UPtr state_solver_clone = state_solver.clone();
1645 runCompareStateSolver(base_state_solver, *state_solver_clone);
1646 }
1647
1648 { // Replace joint with different type (Prismatic)
1649 // Get the scene graph
1650 auto scene_graph = getSceneGraph();
1651 auto state_solver = S(*scene_graph);
1652
1653 Joint new_joint_a1 = scene_graph->getJoint("joint_a1")->clone();
1654 new_joint_a1.parent_to_joint_origin_transform.translation()(0) = 1.25;
1655 new_joint_a1.type = JointType::PRISMATIC;
1656
1657 EXPECT_TRUE(scene_graph->removeJoint("joint_a1"));
1658 EXPECT_TRUE(scene_graph->addJoint(new_joint_a1));
1659 EXPECT_TRUE(state_solver.replaceJoint(new_joint_a1));
1660
1661 KDLStateSolver base_state_solver(*scene_graph);
1662
1663 runCompareStateSolver(base_state_solver, state_solver);
1664 runCompareStateSolverLimits(*scene_graph, base_state_solver);
1665
1666 // Test Clone
1667 StateSolver::UPtr state_solver_clone = state_solver.clone();
1668 runCompareStateSolver(base_state_solver, *state_solver_clone);
1669 }
1670
1671 { // Replace joint with different parent which is a replace and move
1672 // Get the scene graph
1673 auto scene_graph = getSceneGraph();
1674 auto state_solver = S(*scene_graph);
1675
1676 Joint new_joint_a3 = scene_graph->getJoint("joint_a3")->clone();
1677 new_joint_a3.parent_link_name = "base_link";
1678
1679 EXPECT_TRUE(state_solver.replaceJoint(new_joint_a3));
1680
1681 EXPECT_TRUE(scene_graph->removeJoint("joint_a3"));
1682 EXPECT_TRUE(scene_graph->addJoint(new_joint_a3));
1683
1684 KDLStateSolver base_state_solver(*scene_graph);
1685
1686 runCompareStateSolver(base_state_solver, state_solver);
1687 runCompareStateSolverLimits(*scene_graph, base_state_solver);
1688
1689 // Test Clone
1690 StateSolver::UPtr state_solver_clone = state_solver.clone();
1691 runCompareStateSolver(base_state_solver, *state_solver_clone);
1692 }
1693
1694 { // Replace joint which does not exist which should fail
1695 auto scene_graph = getSceneGraph();
1696 auto state_solver = S(*scene_graph);
1697
1698 Joint new_joint_a3 = scene_graph->getJoint("joint_a3")->clone("joint_does_not_exist");
1699
1700 EXPECT_FALSE(state_solver.replaceJoint(new_joint_a3));
1701
1702 KDLStateSolver base_state_solver(*scene_graph);
1703
1704 runCompareStateSolver(base_state_solver, state_solver);
1705 runCompareStateSolverLimits(*scene_graph, base_state_solver);
1706
1707 // Test Clone
1708 StateSolver::UPtr state_solver_clone = state_solver.clone();
1709 runCompareStateSolver(base_state_solver, *state_solver_clone);
1710 }
1711
1712 { // Replace joint where parent link does not exist
1713 auto scene_graph = getSceneGraph();
1714 auto state_solver = S(*scene_graph);
1715
1716 Joint new_joint_a3 = scene_graph->getJoint("joint_a3")->clone();
1717 new_joint_a3.parent_link_name = "link_does_not_exist";
1718
1719 EXPECT_FALSE(state_solver.replaceJoint(new_joint_a3));
1720
1721 KDLStateSolver base_state_solver(*scene_graph);
1722
1723 runCompareStateSolver(base_state_solver, state_solver);
1724 runCompareStateSolverLimits(*scene_graph, base_state_solver);
1725
1726 // Test Clone
1727 StateSolver::UPtr state_solver_clone = state_solver.clone();
1728 runCompareStateSolver(base_state_solver, *state_solver_clone);
1729 }
1730}
1731} // namespace tesseract_scene_graph::test_suite
1732
1733#endif // TESSERACT_STATE_SOLVER_
tesseract_common::TesseractSupportResourceLocator
Abstract class for resource loaders.
Definition: tesseract_support_resource_locator.h:42
tesseract_scene_graph::Joint
Definition: joint.h:281
tesseract_scene_graph::Joint::type
JointType type
The type of joint.
Definition: joint.h:301
tesseract_scene_graph::Joint::parent_to_joint_origin_transform
Eigen::Isometry3d parent_to_joint_origin_transform
transform from Parent Link frame to Joint frame
Definition: joint.h:322
tesseract_scene_graph::Joint::parent_link_name
std::string parent_link_name
Definition: joint.h:319
tesseract_scene_graph::Joint::child_link_name
std::string child_link_name
Definition: joint.h:315
tesseract_scene_graph::KDLStateSolver
Definition: kdl_state_solver.h:43
tesseract_scene_graph::Link
Definition: link.h:188
tesseract_scene_graph::SceneGraph
Definition: graph.h:125
tesseract_scene_graph::SceneGraph::UPtr
std::unique_ptr< SceneGraph > UPtr
Definition: graph.h:132
tesseract_scene_graph::StateSolver
Definition: state_solver.h:46
tesseract_scene_graph::StateSolver::getJacobian
virtual Eigen::MatrixXd getJacobian(const Eigen::Ref< const Eigen::VectorXd > &joint_values, const std::string &link_name) const =0
Get the jacobian of the solver given the joint values.
tesseract_scene_graph::StateSolver::UPtr
std::unique_ptr< StateSolver > UPtr
Definition: state_solver.h:50
tesseract_scene_graph::StateSolver::getActiveJointNames
virtual std::vector< std::string > getActiveJointNames() const =0
Get the vector of joint names which align with the limits.
tesseract_scene_graph::StateSolver::getRandomState
virtual SceneState getRandomState() const =0
Get the random state of the environment.
tesseract_scene_graph::StateSolver::getJointNames
virtual std::vector< std::string > getJointNames() const =0
Get the vector of joint names.
tesseract_scene_graph::StateSolver::hasLinkName
virtual bool hasLinkName(const std::string &link_name) const =0
Check if link name exists.
tesseract_scene_graph::StateSolver::getState
virtual SceneState getState(const Eigen::Ref< const Eigen::VectorXd > &joint_values) const =0
Get the state of the solver given the joint values.
tesseract_scene_graph::StateSolver::getLimits
virtual tesseract_common::KinematicLimits getLimits() const =0
Getter for kinematic limits.
tesseract_scene_graph::StateSolver::getLinkTransform
virtual Eigen::Isometry3d getLinkTransform(const std::string &link_name) const =0
Get the transform corresponding to the link.
tesseract_scene_graph::StateSolver::isActiveLinkName
virtual bool isActiveLinkName(const std::string &link_name) const =0
Check if link is an active link.
tesseract_scene_graph::StateSolver::getLinkNames
virtual std::vector< std::string > getLinkNames() const =0
Get the vector of link names.
tesseract_scene_graph::StateSolver::getRelativeLinkTransform
virtual Eigen::Isometry3d getRelativeLinkTransform(const std::string &from_link_name, const std::string &to_link_name) const =0
Get transform between two links using the current state.
tesseract_scene_graph::StateSolver::getBaseLinkName
virtual std::string getBaseLinkName() const =0
Get the base link name.
tesseract_scene_graph::StateSolver::getLinkTransforms
virtual tesseract_common::VectorIsometry3d getLinkTransforms() const =0
Get all of the links transforms.
tesseract_scene_graph::StateSolver::getActiveLinkNames
virtual std::vector< std::string > getActiveLinkNames() const =0
Get the vector of active link names.
tesseract_scene_graph::StateSolver::setState
virtual void setState(const Eigen::Ref< const Eigen::VectorXd > &joint_values)=0
Set the current state of the solver.
tesseract_scene_graph::StateSolver::getStaticLinkNames
virtual std::vector< std::string > getStaticLinkNames() const =0
Get a vector of static link names in the environment.
EXPECT_NEAR
EXPECT_NEAR(config.contact_manager_config.margin_data.getDefaultCollisionMargin(), 5, 1e-6)
link_names
results link_names[0]
Definition: collision_core_unit.cpp:146
EXPECT_EQ
EXPECT_EQ(results.distance, std::numeric_limits< double >::max())
EXPECT_FALSE
EXPECT_FALSE(tesseract_collision::isContactAllowed("base_link", "link_2", acm, false))
EXPECT_TRUE
EXPECT_TRUE(tesseract_common::isIdentical< tesseract_collision::ObjectPairKey >(pairs, check_pairs, false))
EXPECT_ANY_THROW
EXPECT_ANY_THROW(factory.getDefaultDiscreteContactManagerPlugin())
scene_graph
auto scene_graph
Definition: ikfast_kinematics_7dof_unit.cpp:51
kdl_state_solver.h
Tesseract Scene Graph State Solver KDL Implementation.
jv
Eigen::VectorXd jv
Definition: kinematics_core_unit.cpp:152
jacobian
Eigen::MatrixXd jacobian
Definition: kinematics_core_unit.cpp:153
macros.h
Common Tesseract Macros.
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#define TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
Definition: macros.h:71
TESSERACT_COMMON_IGNORE_WARNINGS_POP
Definition: create_convex_hull.cpp:36
tesseract_common::isIdentical
bool isIdentical(const std::vector< T > &vec1, const std::vector< T > &vec2, bool ordered=true, const std::function< bool(const T &, const T &)> &equal_pred=[](const T &v1, const T &v2) { return v1==v2;}, const std::function< bool(const T &, const T &)> &comp=[](const T &v1, const T &v2) { return v1< v2;})
Check if two vector of strings are identical.
Definition: utils.h:217
tesseract_common::jacobianChangeRefPoint
void jacobianChangeRefPoint(Eigen::Ref< Eigen::MatrixXd > jacobian, const Eigen::Ref< const Eigen::Vector3d > &ref_point)
Change the reference point of the jacobian.
Definition: utils.cpp:108
tesseract_common::getTempPath
std::string getTempPath()
Get the host temp directory path.
Definition: utils.cpp:209
tesseract_common::TransformMap
AlignedMap< std::string, Eigen::Isometry3d > TransformMap
Definition: types.h:66
tesseract_common::VectorIsometry3d
AlignedVector< Eigen::Isometry3d > VectorIsometry3d
Definition: types.h:62
tesseract_common::jacobianChangeBase
void jacobianChangeBase(Eigen::Ref< Eigen::MatrixXd > jacobian, const Eigen::Isometry3d &change_base)
Change the base coordinate system of the jacobian.
Definition: utils.cpp:101
tesseract_scene_graph::test_suite
Definition: state_solver_test_suite.h:19
tesseract_scene_graph::test_suite::runAddSceneGraphTest
void runAddSceneGraphTest()
Definition: state_solver_test_suite.h:1003
tesseract_scene_graph::test_suite::runMoveJointTest
void runMoveJointTest()
Definition: state_solver_test_suite.h:1214
tesseract_scene_graph::test_suite::getSceneGraph
SceneGraph::UPtr getSceneGraph()
Definition: state_solver_test_suite.h:20
tesseract_scene_graph::test_suite::runCompareSceneStates
void runCompareSceneStates(const SceneState &base_state, const SceneState &compare_state)
Definition: state_solver_test_suite.h:60
tesseract_scene_graph::test_suite::runReplaceJointTest
void runReplaceJointTest()
Definition: state_solver_test_suite.h:1530
tesseract_scene_graph::test_suite::runJacobianTest
void runJacobianTest()
Definition: state_solver_test_suite.h:353
tesseract_scene_graph::test_suite::numericalJacobian
static void numericalJacobian(Eigen::Ref< Eigen::MatrixXd > jacobian, const Eigen::Isometry3d &change_base, const StateSolver &state_solver, const std::vector< std::string > &joint_names, const Eigen::Ref< const Eigen::VectorXd > &joint_values, const std::string &link_name, const Eigen::Ref< const Eigen::Vector3d > &link_point)
Numerically calculate a jacobian. This is mainly used for testing.
Definition: state_solver_test_suite.h:194
tesseract_scene_graph::test_suite::runCompareStateSolver
void runCompareStateSolver(const StateSolver &base_solver, StateSolver &comp_solver)
Definition: state_solver_test_suite.h:82
tesseract_scene_graph::test_suite::getSubSceneGraph
SceneGraph::UPtr getSubSceneGraph()
Definition: state_solver_test_suite.h:28
tesseract_scene_graph::test_suite::runChangeJointLimitsTest
void runChangeJointLimitsTest()
Definition: state_solver_test_suite.h:1454
tesseract_scene_graph::test_suite::runCompareJacobian
void runCompareJacobian(StateSolver &state_solver, const std::vector< std::string > &joint_names, const Eigen::VectorXd &jvals, const std::string &link_name, const Eigen::Vector3d &link_point, const Eigen::Isometry3d &change_base)
Run a kinematic jacobian test.
Definition: state_solver_test_suite.h:256
tesseract_scene_graph::test_suite::runMoveLinkTest
void runMoveLinkTest()
Definition: state_solver_test_suite.h:1330
tesseract_scene_graph::test_suite::runAddandRemoveLinkTest
void runAddandRemoveLinkTest()
Definition: state_solver_test_suite.h:733
tesseract_scene_graph::test_suite::runCompareStateSolverLimits
void runCompareStateSolverLimits(const SceneGraph &scene_graph, const StateSolver &comp_solver)
Definition: state_solver_test_suite.h:171
tesseract_scene_graph::test_suite::runChangeJointOriginTest
void runChangeJointOriginTest()
Definition: state_solver_test_suite.h:1144
tesseract_scene_graph::JointType::PRISMATIC
@ PRISMATIC
tesseract_scene_graph::JointType::CONTINUOUS
@ CONTINUOUS
tesseract_scene_graph::JointType::FIXED
@ FIXED
tesseract_urdf::parseURDFFile
tesseract_scene_graph::SceneGraph::UPtr parseURDFFile(const std::string &path, const tesseract_common::ResourceLocator &locator)
Parse a URDF file into a Tesseract Scene Graph.
Definition: urdf_parser.cpp:160
resource_locator.h
Locate and retrieve resource data.
locator
ResourceLocator::Ptr locator
Definition: resource_locator_unit.cpp:57
tesseract_common::KinematicLimits
Store kinematic limits.
Definition: kinematic_limits.h:39
tesseract_scene_graph::SceneState
This holds a state of the scene.
Definition: scene_state.h:54
tesseract_scene_graph::SceneState::link_transforms
tesseract_common::TransformMap link_transforms
The link transforms in world coordinate system.
Definition: scene_state.h:68
tesseract_scene_graph::SceneState::getJointValues
Eigen::VectorXd getJointValues(const std::vector< std::string > &joint_names) const
Definition: scene_state.cpp:46
tesseract_scene_graph::SceneState::joints
std::unordered_map< std::string, double > joints
The joint values used for calculating the joint and link transforms.
Definition: scene_state.h:65
tesseract_scene_graph::SceneState::joint_transforms
tesseract_common::TransformMap joint_transforms
The joint transforms in world coordinate system.
Definition: scene_state.h:71
utils.h
Common Tesseract Utility Functions.
joint_names
joint_state joint_names
Definition: tesseract_common_serialization_unit.cpp:75
collision
tesseract_collision::ContactResultMap collision
Definition: tesseract_environment_collision.cpp:124
pose
auto pose
Definition: tesseract_environment_collision.cpp:118
box.h
Tesseract Box Geometry.
limits
joint_1 limits
Definition: tesseract_scene_graph_joint_unit.cpp:153
j
JointDynamics j
Definition: tesseract_scene_graph_joint_unit.cpp:15
joint_name
joint_1 mimic joint_name
Definition: tesseract_scene_graph_joint_unit.cpp:163
joint_2
Joint joint_2("joint_a2")
path
tesseract_common::fs::path path
Definition: tesseract_srdf_unit.cpp:1992
joint_1
Joint joint_1("joint_a1")
link_2
Link link_2("link_2")
tesseract_support_resource_locator.h
Locate and retrieve resource data in tesseract_support.
link_1
tesseract_scene_graph::Link::Ptr link_1
Definition: tesseract_urdf_urdf_unit.cpp:464
urdf_parser.h
A urdf parser for tesseract.
Generated on Thu Mar 30 2023 23:19:52 for Tesseract by doxygen 1.9.5