Tesseract
Motion Planning Environment
Loading...
Searching...
No Matches
kinematics_test_utils.h
Go to the documentation of this file.
1
26#ifndef TESSERACT_KINEMATICS_KIN_TEST_SUITE_H
27#define TESSERACT_KINEMATICS_KIN_TEST_SUITE_H
28
29#include <tesseract_common/macros.h>
30TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
31#include <gtest/gtest.h>
32#include <fstream>
33#include <yaml-cpp/yaml.h>
34TESSERACT_COMMON_IGNORE_WARNINGS_POP
35
36#include <tesseract_kinematics/core/forward_kinematics.h>
37#include <tesseract_kinematics/core/inverse_kinematics.h>
38#include <tesseract_kinematics/core/utils.h>
39#include <tesseract_kinematics/core/types.h>
40#include <tesseract_kinematics/core/kinematic_group.h>
41#include <tesseract_kinematics/core/kinematics_plugin_factory.h>
42#include <tesseract_kinematics/core/validate.h>
43
44#include <tesseract_scene_graph/graph.h>
45#include <tesseract_state_solver/kdl/kdl_state_solver.h>
46
47#include <tesseract_urdf/urdf_parser.h>
48#include <tesseract_common/utils.h>
49#include <tesseract_support/tesseract_support_resource_locator.h>
50
51namespace tesseract_kinematics::test_suite
52{
53inline tesseract_scene_graph::SceneGraph::UPtr getSceneGraphIIWA()
54{
55 std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf";
56
57 tesseract_common::TesseractSupportResourceLocator locator;
58 return tesseract_urdf::parseURDFFile(path, locator);
59}
60
61inline tesseract_scene_graph::SceneGraph::UPtr getSceneGraphABBExternalPositioner()
62{
63 std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/abb_irb2400_external_positioner.urdf";
64
65 tesseract_common::TesseractSupportResourceLocator locator;
66 return tesseract_urdf::parseURDFFile(path, locator);
67}
68
69inline tesseract_scene_graph::SceneGraph::UPtr getSceneGraphABBOnPositioner()
70{
71 std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/abb_irb2400_on_positioner.urdf";
72
73 tesseract_common::TesseractSupportResourceLocator locator;
74 return tesseract_urdf::parseURDFFile(path, locator);
75}
76
77inline tesseract_scene_graph::SceneGraph::UPtr getSceneGraphABB()
78{
79 std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/abb_irb2400.urdf";
80
81 tesseract_common::TesseractSupportResourceLocator locator;
82 return tesseract_urdf::parseURDFFile(path, locator);
83}
84
85inline tesseract_scene_graph::SceneGraph::UPtr getSceneGraphIIWA7()
86{
87 std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/iiwa7.urdf";
88
89 tesseract_common::TesseractSupportResourceLocator locator;
90 return tesseract_urdf::parseURDFFile(path, locator);
91}
92
93inline tesseract_scene_graph::SceneGraph::UPtr getSceneGraphUR(const tesseract_kinematics::URParameters& params,
94 double shoulder_offset,
95 double elbow_offset)
96{
97 using namespace tesseract_scene_graph;
98
99 auto sg = std::make_unique<SceneGraph>("universal_robot");
100 sg->addLink(Link("base_link"));
101 sg->addLink(Link("shoulder_link"));
102 sg->addLink(Link("upper_arm_link"));
103 sg->addLink(Link("forearm_link"));
104 sg->addLink(Link("wrist_1_link"));
105 sg->addLink(Link("wrist_2_link"));
106 sg->addLink(Link("wrist_3_link"));
107 sg->addLink(Link("ee_link"));
108 sg->addLink(Link("tool0"));
109
110 {
111 Joint j("shoulder_pan_joint");
112 j.type = JointType::REVOLUTE;
113 j.parent_link_name = "base_link";
114 j.child_link_name = "shoulder_link";
115 j.axis = Eigen::Vector3d::UnitZ();
116 j.parent_to_joint_origin_transform.translation() = Eigen::Vector3d(0, 0, params.d1);
117 j.limits = std::make_shared<JointLimits>();
118 j.limits->lower = -2.0 * M_PI;
119 j.limits->upper = 2.0 * M_PI;
120 j.limits->velocity = 2.16;
121 j.limits->acceleration = 0.5 * j.limits->velocity;
122 sg->addJoint(j);
123 }
124
125 {
126 Joint j("shoulder_lift_joint");
127 j.type = JointType::REVOLUTE;
128 j.parent_link_name = "shoulder_link";
129 j.child_link_name = "upper_arm_link";
130 j.axis = Eigen::Vector3d::UnitY();
131 j.parent_to_joint_origin_transform.translation() = Eigen::Vector3d(0, shoulder_offset, 0);
132 j.parent_to_joint_origin_transform =
133 j.parent_to_joint_origin_transform * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitY());
134 j.limits = std::make_shared<JointLimits>();
135 j.limits->lower = -2.0 * M_PI;
136 j.limits->upper = 2.0 * M_PI;
137 j.limits->velocity = 2.16;
138 j.limits->acceleration = 0.5 * j.limits->velocity;
139 sg->addJoint(j);
140 }
141
142 {
143 Joint j("elbow_joint");
144 j.type = JointType::REVOLUTE;
145 j.parent_link_name = "upper_arm_link";
146 j.child_link_name = "forearm_link";
147 j.axis = Eigen::Vector3d::UnitY();
148 j.parent_to_joint_origin_transform.translation() = Eigen::Vector3d(0, elbow_offset, -params.a2);
149 j.limits = std::make_shared<JointLimits>();
150 j.limits->lower = -2.0 * M_PI;
151 j.limits->upper = 2.0 * M_PI;
152 j.limits->velocity = 2.16;
153 j.limits->acceleration = 0.5 * j.limits->velocity;
154 sg->addJoint(j);
155 }
156
157 {
158 Joint j("wrist_1_joint");
159 j.type = JointType::REVOLUTE;
160 j.parent_link_name = "forearm_link";
161 j.child_link_name = "wrist_1_link";
162 j.axis = Eigen::Vector3d::UnitY();
163 j.parent_to_joint_origin_transform.translation() = Eigen::Vector3d(0, 0, -params.a3);
164 j.parent_to_joint_origin_transform =
165 j.parent_to_joint_origin_transform * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitY());
166 j.limits = std::make_shared<JointLimits>();
167 j.limits->lower = -2.0 * M_PI;
168 j.limits->upper = 2.0 * M_PI;
169 j.limits->velocity = 2.16;
170 j.limits->acceleration = 0.5 * j.limits->velocity;
171 sg->addJoint(j);
172 }
173
174 {
175 Joint j("wrist_2_joint");
176 j.type = JointType::REVOLUTE;
177 j.parent_link_name = "wrist_1_link";
178 j.child_link_name = "wrist_2_link";
179 j.axis = Eigen::Vector3d::UnitZ();
180 j.parent_to_joint_origin_transform.translation() =
181 Eigen::Vector3d(0, params.d4 - elbow_offset - shoulder_offset, 0);
182 j.limits = std::make_shared<JointLimits>();
183 j.limits->lower = -2.0 * M_PI;
184 j.limits->upper = 2.0 * M_PI;
185 j.limits->velocity = 2.16;
186 j.limits->acceleration = 0.5 * j.limits->velocity;
187 sg->addJoint(j);
188 }
189
190 {
191 Joint j("wrist_3_joint");
192 j.type = JointType::REVOLUTE;
193 j.parent_link_name = "wrist_2_link";
194 j.child_link_name = "wrist_3_link";
195 j.axis = Eigen::Vector3d::UnitY();
196 j.parent_to_joint_origin_transform.translation() = Eigen::Vector3d(0, 0, params.d5);
197 j.limits = std::make_shared<JointLimits>();
198 j.limits->lower = -2.0 * M_PI;
199 j.limits->upper = 2.0 * M_PI;
200 j.limits->velocity = 2.16;
201 j.limits->acceleration = 0.5 * j.limits->velocity;
202 sg->addJoint(j);
203 }
204
205 {
206 Joint j("ee_fixed_joint");
207 j.type = JointType::FIXED;
208 j.parent_link_name = "wrist_3_link";
209 j.child_link_name = "ee_link";
210 j.parent_to_joint_origin_transform.translation() = Eigen::Vector3d(0, params.d6, 0);
211 j.parent_to_joint_origin_transform =
212 j.parent_to_joint_origin_transform * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitZ());
213 sg->addJoint(j);
214 }
215
216 {
217 Joint j("wrist_3_link-tool0_fixed_joint");
218 j.type = JointType::FIXED;
219 j.parent_link_name = "wrist_3_link";
220 j.child_link_name = "tool0";
221 j.parent_to_joint_origin_transform.translation() = Eigen::Vector3d(0, params.d6, 0);
222 j.parent_to_joint_origin_transform =
223 j.parent_to_joint_origin_transform * Eigen::AngleAxisd(-M_PI_2, Eigen::Vector3d::UnitX());
224 sg->addJoint(j);
225 }
226
227 return sg;
228}
229
230inline tesseract_common::KinematicLimits getTargetLimits(const tesseract_scene_graph::SceneGraph& scene_graph,
231 const std::vector<std::string>& joint_names)
232{
233 auto s = static_cast<Eigen::Index>(joint_names.size());
234
235 tesseract_common::KinematicLimits limits;
236 limits.resize(s);
237
238 for (Eigen::Index i = 0; i < s; ++i)
239 {
240 auto joint = scene_graph.getJoint(joint_names[static_cast<std::size_t>(i)]);
241 limits.joint_limits(i, 0) = joint->limits->lower;
242 limits.joint_limits(i, 1) = joint->limits->upper;
243 limits.velocity_limits(i) = joint->limits->velocity;
244 limits.acceleration_limits(i) = joint->limits->acceleration;
245 }
246
247 return limits;
248}
249
260inline void runJacobianTest(tesseract_kinematics::ForwardKinematics& kin,
261 const Eigen::VectorXd& jvals,
262 const std::string& link_name,
263 const Eigen::Vector3d& link_point,
264 const Eigen::Isometry3d& change_base)
265{
266 Eigen::MatrixXd jacobian, numerical_jacobian;
267 tesseract_common::TransformMap poses;
268
269 jacobian.resize(6, kin.numJoints());
270
271 poses = kin.calcFwdKin(jvals);
272 jacobian = kin.calcJacobian(jvals, link_name);
273 tesseract_common::jacobianChangeBase(jacobian, change_base);
274 tesseract_common::jacobianChangeRefPoint(jacobian, (change_base * poses[link_name]).linear() * link_point);
275
276 numerical_jacobian.resize(6, kin.numJoints());
277 tesseract_kinematics::numericalJacobian(numerical_jacobian, change_base, kin, jvals, link_name, link_point);
278
279 for (int i = 0; i < 6; ++i)
280 for (int j = 0; j < static_cast<int>(kin.numJoints()); ++j)
281 EXPECT_NEAR(numerical_jacobian(i, j), jacobian(i, j), 1e-3);
282}
283
293inline void runJacobianTest(tesseract_kinematics::KinematicGroup& kin_group,
294 const Eigen::VectorXd& jvals,
295 const std::string& link_name,
296 const Eigen::Vector3d& link_point)
297{
298 Eigen::MatrixXd jacobian, numerical_jacobian;
299 jacobian.resize(6, kin_group.numJoints());
300
301 tesseract_common::TransformMap poses = kin_group.calcFwdKin(jvals);
302 { // Test with all information
303 jacobian = kin_group.calcJacobian(jvals, link_name, link_point);
304
305 numerical_jacobian.resize(6, kin_group.numJoints());
306 tesseract_kinematics::numericalJacobian(numerical_jacobian, kin_group, jvals, link_name, link_point);
307
308 for (int i = 0; i < 6; ++i)
309 {
310 for (int j = 0; j < static_cast<int>(kin_group.numJoints()); ++j)
311 {
312 EXPECT_NEAR(numerical_jacobian(i, j), jacobian(i, j), 1e-3);
313 }
314 }
315 }
316
317 { // Test don't use link_point
318 jacobian = kin_group.calcJacobian(jvals, link_name);
319
320 numerical_jacobian.resize(6, kin_group.numJoints());
321 tesseract_kinematics::numericalJacobian(numerical_jacobian, kin_group, jvals, link_name, Eigen::Vector3d::Zero());
322
323 for (int i = 0; i < 6; ++i)
324 {
325 for (int j = 0; j < static_cast<int>(kin_group.numJoints()); ++j)
326 {
327 EXPECT_NEAR(numerical_jacobian(i, j), jacobian(i, j), 1e-3);
328 }
329 }
330 }
331}
332
338inline void runKinJointLimitsTest(const tesseract_common::KinematicLimits& limits,
339 const tesseract_common::KinematicLimits& target_limits)
340{
342 // Test forward kinematics joint limits
344 EXPECT_EQ(limits.joint_limits.rows(), target_limits.joint_limits.rows());
345 EXPECT_EQ(limits.velocity_limits.rows(), target_limits.velocity_limits.rows());
346 EXPECT_EQ(limits.acceleration_limits.rows(), target_limits.acceleration_limits.rows());
347
348 // Check limits
349 for (Eigen::Index i = 0; i < limits.joint_limits.rows(); ++i)
350 {
351 EXPECT_NEAR(limits.joint_limits(i, 0), target_limits.joint_limits(i, 0), 1e-6);
352 EXPECT_NEAR(limits.joint_limits(i, 1), target_limits.joint_limits(i, 1), 1e-6);
353 EXPECT_NEAR(limits.velocity_limits(i), target_limits.velocity_limits(i), 1e-6);
354 EXPECT_NEAR(limits.acceleration_limits(i), target_limits.acceleration_limits(i), 1e-6);
355 }
356}
357
362inline void runKinSetJointLimitsTest(tesseract_kinematics::KinematicGroup& kin_group)
363{
365 // Test setting kinematic group joint limits
367 tesseract_common::KinematicLimits limits = kin_group.getLimits();
368 EXPECT_TRUE(limits.joint_limits.rows() > 0);
369 EXPECT_TRUE(limits.velocity_limits.rows() > 0);
370 EXPECT_TRUE(limits.acceleration_limits.rows() > 0);
371
372 // Check limits
373 for (Eigen::Index i = 0; i < limits.joint_limits.rows(); ++i)
374 {
375 limits.joint_limits(i, 0) = -5.0 - double(i);
376 limits.joint_limits(i, 1) = 5.0 + double(i);
377 limits.velocity_limits(i) = 10.0 + double(i);
378 limits.acceleration_limits(i) = 5.0 + double(i);
379 }
380
381 kin_group.setLimits(limits);
382 runKinJointLimitsTest(kin_group.getLimits(), limits);
383
384 // Test failure
385 tesseract_common::KinematicLimits limits_empty;
386 EXPECT_ANY_THROW(kin_group.setLimits(limits_empty)); // NOLINT
387}
388
394inline void runStringVectorEqualTest(const std::vector<std::string>& names,
395 const std::vector<std::string>& target_names)
396{
397 EXPECT_EQ(names.size(), target_names.size());
398 EXPECT_FALSE(names.empty());
399 EXPECT_FALSE(target_names.empty());
400
401 std::vector<std::string> v1 = names;
402 std::vector<std::string> v2 = target_names;
403 std::sort(v1.begin(), v1.end());
404 std::sort(v2.begin(), v2.end());
405 EXPECT_TRUE(std::equal(v1.begin(), v1.end(), v2.begin()));
406 // EXPECT_TRUE(tesseract_common::isIdentical(names, target_names, false));
407}
408
416inline void runInvKinTest(const tesseract_kinematics::InverseKinematics& inv_kin,
417 const tesseract_kinematics::ForwardKinematics& fwd_kin,
418 const Eigen::Isometry3d& target_pose,
419 const std::string& tip_link_name,
420 const Eigen::VectorXd& seed)
421{
423 // Test Inverse kinematics
425 EXPECT_TRUE(inv_kin.getBaseLinkName() == fwd_kin.getBaseLinkName()); // This only works if they are equal
426 tesseract_common::TransformMap input{ std::make_pair(tip_link_name, target_pose) };
427 IKSolutions solutions = inv_kin.calcInvKin(input, seed);
428 EXPECT_TRUE(!solutions.empty());
429
430 for (const auto& sol : solutions)
431 {
432 tesseract_common::TransformMap result_poses = fwd_kin.calcFwdKin(sol);
433 Eigen::Isometry3d result = result_poses[tip_link_name];
434 EXPECT_TRUE(target_pose.translation().isApprox(result.translation(), 1e-4));
435
436 Eigen::Quaterniond rot_pose(target_pose.rotation());
437 Eigen::Quaterniond rot_result(result.rotation());
438 EXPECT_TRUE(rot_pose.isApprox(rot_result, 1e-3));
439 }
440}
441
448inline void runInvKinTest(const tesseract_kinematics::KinematicGroup& kin_group,
449 const Eigen::Isometry3d& target_pose,
450 const std::string& working_frame,
451 const std::string& tip_link_name,
452 const Eigen::VectorXd& seed)
453{
455 // Test Inverse kinematics
457 KinGroupIKInputs inputs{ KinGroupIKInput(target_pose, working_frame, tip_link_name) };
458 IKSolutions solutions = kin_group.calcInvKin(inputs, seed);
459 EXPECT_TRUE(!solutions.empty());
460
461 for (const auto& sol : solutions)
462 {
463 tesseract_common::TransformMap result_poses = kin_group.calcFwdKin(sol);
464 Eigen::Isometry3d result = result_poses.at(working_frame).inverse() * result_poses[tip_link_name];
465 EXPECT_TRUE(target_pose.translation().isApprox(result.translation(), 1e-4));
466
467 Eigen::Quaterniond rot_pose(target_pose.rotation());
468 Eigen::Quaterniond rot_result(result.rotation());
469 EXPECT_TRUE(rot_pose.isApprox(rot_result, 1e-3));
470 }
471
472 EXPECT_TRUE(checkKinematics(kin_group));
473}
474
475inline void runFwdKinIIWATest(tesseract_kinematics::ForwardKinematics& kin)
476{
478 // Test forward kinematics when tip link is the base of the chain
480
481 Eigen::VectorXd jvals;
482 jvals.resize(7);
483 jvals.setZero();
484
485 tesseract_common::TransformMap poses = kin.calcFwdKin(jvals);
486
487 EXPECT_EQ(poses.size(), 1);
488 Eigen::Isometry3d pose = poses.at("tool0");
489 Eigen::Isometry3d result;
490 result.setIdentity();
491 result.translation()[0] = 0;
492 result.translation()[1] = 0;
493 result.translation()[2] = 1.306;
494 EXPECT_TRUE(pose.isApprox(result));
495}
496
497inline void runJacobianIIWATest(tesseract_kinematics::ForwardKinematics& kin, bool is_kin_tree = false)
498{
499 UNUSED(is_kin_tree);
500 std::string tip_link = "tool0";
501 std::string base_link = "base_link";
502
504 // Test forward kinematics when tip link is the base of the chain
506 Eigen::VectorXd jvals;
507 jvals.resize(7);
508
509 jvals(0) = -0.785398;
510 jvals(1) = 0.785398;
511 jvals(2) = -0.785398;
512 jvals(3) = 0.785398;
513 jvals(4) = -0.785398;
514 jvals(5) = 0.785398;
515 jvals(6) = -0.785398;
516
518 // Test Jacobian
520 Eigen::Vector3d link_point(0, 0, 0);
521 runJacobianTest(kin, jvals, tip_link, link_point, Eigen::Isometry3d::Identity());
522
523 EXPECT_ANY_THROW(runJacobianTest(kin, jvals, "", link_point, Eigen::Isometry3d::Identity())); // NOLINT
524
526 // Test Jacobian at Point
528 for (int k = 0; k < 3; ++k)
529 {
530 Eigen::Vector3d link_point(0, 0, 0);
531 link_point[k] = 1;
532
533 runJacobianTest(kin, jvals, tip_link, link_point, Eigen::Isometry3d::Identity());
534
535 EXPECT_ANY_THROW(runJacobianTest(kin, jvals, "", link_point, Eigen::Isometry3d::Identity())); // NOLINT
536 }
537
539 // Test Jacobian with change base
541 for (int k = 0; k < 3; ++k)
542 {
543 link_point = Eigen::Vector3d(0, 0, 0);
544 Eigen::Isometry3d change_base;
545 change_base.setIdentity();
546 change_base(0, 0) = 0;
547 change_base(1, 0) = 1;
548 change_base(0, 1) = -1;
549 change_base(1, 1) = 0;
550 change_base.translation() = Eigen::Vector3d(0, 0, 0);
551 change_base.translation()[k] = 1;
552
553 runJacobianTest(kin, jvals, tip_link, link_point, change_base);
554
555 EXPECT_ANY_THROW(runJacobianTest(kin, jvals, "", link_point, change_base)); // NOLINT
556 }
557
559 // Test Jacobian at point with change base
561 for (int k = 0; k < 3; ++k)
562 {
563 Eigen::Vector3d link_point(0, 0, 0);
564 link_point[k] = 1;
565
566 Eigen::Isometry3d change_base;
567 change_base.setIdentity();
568 change_base(0, 0) = 0;
569 change_base(1, 0) = 1;
570 change_base(0, 1) = -1;
571 change_base(1, 1) = 0;
572 change_base.translation() = link_point;
573
574 runJacobianTest(kin, jvals, tip_link, link_point, change_base);
575
576 EXPECT_ANY_THROW(runJacobianTest(kin, jvals, "", link_point, change_base)); // NOLINT
577 }
578}
579
580inline void runKinGroupJacobianIIWATest(tesseract_kinematics::KinematicGroup& kin_group)
581{
582 std::string base_link_name = "base_link";
583 std::vector<std::string> link_names = { "base_link", "link_1", "link_2", "link_3", "link_4",
584 "link_5", "link_6", "link_7", "tool0" };
585
587 // Test forward kinematics when tip link is the base of the chain
589 Eigen::VectorXd jvals;
590 jvals.resize(7);
591
592 jvals(0) = -0.785398;
593 jvals(1) = 0.785398;
594 jvals(2) = -0.785398;
595 jvals(3) = 0.785398;
596 jvals(4) = -0.785398;
597 jvals(5) = 0.785398;
598 jvals(6) = -0.785398;
599
601 // Test Jacobian at Point
602 // This also runs a test without the link point
604 for (int k = 0; k < 3; ++k)
605 {
606 Eigen::Vector3d link_point(0, 0, 0);
607 link_point[k] = 1;
608
609 for (const auto& link_name : link_names)
610 runJacobianTest(kin_group, jvals, link_name, link_point);
611
612 // NOLINTNEXTLINE
613 EXPECT_ANY_THROW(runJacobianTest(kin_group, jvals, "", link_point));
614 }
615}
616
617inline void runActiveLinkNamesIIWATest(const tesseract_kinematics::KinematicGroup& kin_group)
618{
619 EXPECT_FALSE(kin_group.checkJoints(Eigen::VectorXd::Zero(8)));
620 EXPECT_FALSE(kin_group.checkJoints(Eigen::VectorXd::Constant(7, std::numeric_limits<double>::max())));
621 EXPECT_FALSE(kin_group.checkJoints(Eigen::VectorXd::Constant(7, -std::numeric_limits<double>::max())));
622 EXPECT_TRUE(kin_group.checkJoints(Eigen::VectorXd::Zero(7)));
623
624 std::vector<std::string> target_active_link_names = { "link_1", "link_2", "link_3", "link_4",
625 "link_5", "link_6", "link_7", "tool0" };
626
627 std::vector<std::string> target_static_link_names = { "base_link", "base" };
628
629 std::vector<std::string> target_link_names = target_active_link_names;
630 target_link_names.insert(target_link_names.end(), target_static_link_names.begin(), target_static_link_names.end());
631
632 for (const auto& l : target_active_link_names)
633 {
634 EXPECT_TRUE(kin_group.isActiveLinkName(l));
635 }
636
637 for (const auto& l : target_link_names)
638 {
639 EXPECT_TRUE(kin_group.hasLinkName(l));
640 }
641
642 {
643 std::vector<std::string> link_names = kin_group.getActiveLinkNames();
644 runStringVectorEqualTest(link_names, target_active_link_names);
645 }
646
647 {
648 std::vector<std::string> link_names = kin_group.getStaticLinkNames();
649 runStringVectorEqualTest(link_names, target_static_link_names);
650 }
651
652 {
653 std::vector<std::string> link_names = kin_group.getLinkNames();
654 runStringVectorEqualTest(link_names, target_link_names);
655 }
656}
657
658inline void runActiveLinkNamesABBTest(const tesseract_kinematics::KinematicGroup& kin_group)
659{
660 EXPECT_FALSE(kin_group.checkJoints(Eigen::VectorXd::Zero(7)));
661 EXPECT_FALSE(kin_group.checkJoints(Eigen::VectorXd::Constant(6, std::numeric_limits<double>::max())));
662 EXPECT_FALSE(kin_group.checkJoints(Eigen::VectorXd::Constant(6, -std::numeric_limits<double>::max())));
663 EXPECT_TRUE(kin_group.checkJoints(Eigen::VectorXd::Zero(6)));
664
665 std::vector<std::string> target_active_link_names = { "link_1", "link_2", "link_3", "link_4",
666 "link_5", "link_6", "tool0" };
667
668 std::vector<std::string> target_static_link_names = { "base_link" };
669
670 std::vector<std::string> target_link_names = target_active_link_names;
671 target_link_names.insert(target_link_names.end(), target_static_link_names.begin(), target_static_link_names.end());
672
673 for (const auto& l : target_active_link_names)
674 {
675 EXPECT_TRUE(kin_group.isActiveLinkName(l));
676 }
677
678 for (const auto& l : target_link_names)
679 {
680 EXPECT_TRUE(kin_group.hasLinkName(l));
681 }
682
683 {
684 std::vector<std::string> link_names = kin_group.getActiveLinkNames();
685 runStringVectorEqualTest(link_names, target_active_link_names);
686 }
687
688 {
689 std::vector<std::string> link_names = kin_group.getStaticLinkNames();
690 runStringVectorEqualTest(link_names, target_static_link_names);
691 }
692
693 {
694 std::vector<std::string> link_names = kin_group.getLinkNames();
695 runStringVectorEqualTest(link_names, target_link_names);
696 }
697}
698
699inline void runActiveLinkNamesURTest(const tesseract_kinematics::KinematicGroup& kin_group)
700{
701 EXPECT_FALSE(kin_group.checkJoints(Eigen::VectorXd::Zero(7)));
702 EXPECT_FALSE(kin_group.checkJoints(Eigen::VectorXd::Constant(6, std::numeric_limits<double>::max())));
703 EXPECT_FALSE(kin_group.checkJoints(Eigen::VectorXd::Constant(6, -std::numeric_limits<double>::max())));
704 EXPECT_TRUE(kin_group.checkJoints(Eigen::VectorXd::Zero(6)));
705
706 std::vector<std::string> target_active_link_names = { "shoulder_link", "upper_arm_link", "forearm_link",
707 "wrist_1_link", "wrist_2_link", "wrist_3_link",
708 "tool0" };
709
710 std::vector<std::string> target_static_link_names = { "base_link" };
711
712 std::vector<std::string> target_link_names = target_active_link_names;
713 target_link_names.insert(target_link_names.end(), target_static_link_names.begin(), target_static_link_names.end());
714
715 for (const auto& l : target_active_link_names)
716 {
717 EXPECT_TRUE(kin_group.isActiveLinkName(l));
718 }
719
720 for (const auto& l : target_link_names)
721 {
722 EXPECT_TRUE(kin_group.hasLinkName(l));
723 }
724
725 {
726 std::vector<std::string> link_names = kin_group.getActiveLinkNames();
727 runStringVectorEqualTest(link_names, target_active_link_names);
728 }
729
730 {
731 std::vector<std::string> link_names = kin_group.getStaticLinkNames();
732 runStringVectorEqualTest(link_names, target_static_link_names);
733 }
734
735 {
736 std::vector<std::string> link_names = kin_group.getLinkNames();
737 runStringVectorEqualTest(link_names, target_link_names);
738 }
739}
740
741inline void runKinGroupJacobianABBOnPositionerTest(tesseract_kinematics::KinematicGroup& kin_group)
742{
743 std::string base_link_name = "positioner_base_link";
744 std::vector<std::string> link_names{ "positioner_base_link",
745 "positioner_tool0",
746 "base",
747 "base_link",
748 "link_1",
749 "link_2",
750 "link_3",
751 "link_4",
752 "link_5",
753 "link_6",
754 "tool0" };
755
757 // Test forward kinematics when tip link is the base of the chain
759 Eigen::MatrixXd jacobian, numerical_jacobian;
760 Eigen::VectorXd jvals;
761 jvals.resize(7);
762
763 jvals(0) = -0.785398;
764 jvals(1) = 0.785398;
765 jvals(2) = -0.785398;
766 jvals(3) = 0.785398;
767 jvals(4) = -0.785398;
768 jvals(5) = 0.785398;
769 jvals(6) = -0.785398;
770
772 // Test Jacobian at Point
773 // Note: Also tests without the link point
775 for (int k = 0; k < 3; ++k)
776 {
777 Eigen::Vector3d link_point(0, 0, 0);
778 link_point[k] = 1;
779
780 for (const auto& link_name : link_names)
781 runJacobianTest(kin_group, jvals, link_name, link_point);
782
783 // NOLINTNEXTLINE
784 EXPECT_ANY_THROW(runJacobianTest(kin_group, jvals, "", link_point));
785 }
786}
787
788inline void runActiveLinkNamesABBOnPositionerTest(const tesseract_kinematics::KinematicGroup& kin_group)
789{
790 EXPECT_FALSE(kin_group.checkJoints(Eigen::VectorXd::Zero(8)));
791 EXPECT_FALSE(kin_group.checkJoints(Eigen::VectorXd::Constant(7, std::numeric_limits<double>::max())));
792 EXPECT_FALSE(kin_group.checkJoints(Eigen::VectorXd::Constant(7, -std::numeric_limits<double>::max())));
793 EXPECT_TRUE(kin_group.checkJoints(Eigen::VectorXd::Zero(7)));
794
795 std::vector<std::string> target_active_link_names = { "positioner_tool0", "base_link", "base", "link_1", "link_2",
796 "link_3", "link_4", "link_5", "link_6", "tool0" };
797
798 std::vector<std::string> target_static_link_names = { "positioner_base_link" };
799
800 std::vector<std::string> target_link_names = target_active_link_names;
801 target_link_names.insert(target_link_names.end(), target_static_link_names.begin(), target_static_link_names.end());
802
803 for (const auto& l : target_active_link_names)
804 {
805 EXPECT_TRUE(kin_group.isActiveLinkName(l));
806 }
807
808 for (const auto& l : target_link_names)
809 {
810 EXPECT_TRUE(kin_group.hasLinkName(l));
811 }
812
813 {
814 std::vector<std::string> link_names = kin_group.getActiveLinkNames();
815 runStringVectorEqualTest(link_names, target_active_link_names);
816 }
817
818 {
819 std::vector<std::string> link_names = kin_group.getStaticLinkNames();
820 runStringVectorEqualTest(link_names, target_static_link_names);
821 }
822
823 {
824 std::vector<std::string> link_names = kin_group.getLinkNames();
825 runStringVectorEqualTest(link_names, target_link_names);
826 }
827}
828
829inline void runKinGroupJacobianABBExternalPositionerTest(tesseract_kinematics::KinematicGroup& kin_group)
830{
831 std::string base_link_name = "positioner_base_link";
832 std::vector<std::string> link_names{ "positioner_base_link",
833 "positioner_tool0",
834 "positioner_link_1",
835 "base_link",
836 "link_1",
837 "link_2",
838 "link_3",
839 "link_4",
840 "link_5",
841 "link_6",
842 "tool0" };
843
845 // Test forward kinematics when tip link is the base of the chain
847 Eigen::MatrixXd jacobian, numerical_jacobian;
848 Eigen::VectorXd jvals;
849 jvals.resize(8);
850
851 jvals(0) = -0.785398;
852 jvals(1) = 0.785398;
853 jvals(2) = -0.785398;
854 jvals(3) = 0.785398;
855 jvals(4) = -0.785398;
856 jvals(5) = 0.785398;
857 jvals(6) = -0.785398;
858 jvals(7) = 0.785398;
859
861 // Test Jacobian at Point
862 // Note: Also tests without the link point
864 for (int k = 0; k < 3; ++k)
865 {
866 Eigen::Vector3d link_point(0, 0, 0);
867 link_point[k] = 1;
868
869 for (const auto& link_name : link_names)
870 runJacobianTest(kin_group, jvals, link_name, link_point);
871
872 // NOLINTNEXTLINE
873 EXPECT_ANY_THROW(runJacobianTest(kin_group, jvals, "", link_point));
874 }
875}
876
877inline void runActiveLinkNamesABBExternalPositionerTest(const tesseract_kinematics::KinematicGroup& kin_group)
878{
879 EXPECT_FALSE(kin_group.checkJoints(Eigen::VectorXd::Zero(9)));
880 EXPECT_FALSE(kin_group.checkJoints(Eigen::VectorXd::Constant(8, std::numeric_limits<double>::max())));
881 EXPECT_FALSE(kin_group.checkJoints(Eigen::VectorXd::Constant(8, -std::numeric_limits<double>::max())));
882 EXPECT_TRUE(kin_group.checkJoints(Eigen::VectorXd::Zero(8)));
883
884 std::vector<std::string> target_active_link_names = {
885 "positioner_tool0", "positioner_link_1", "link_1", "link_2", "link_3", "link_4", "link_5", "link_6", "tool0"
886 };
887
888 std::vector<std::string> target_static_link_names = { "world", "base_link", "base", "positioner_base_link" };
889
890 std::vector<std::string> target_link_names = target_active_link_names;
891 target_link_names.insert(target_link_names.end(), target_static_link_names.begin(), target_static_link_names.end());
892
893 for (const auto& l : target_active_link_names)
894 {
895 EXPECT_TRUE(kin_group.isActiveLinkName(l));
896 }
897
898 for (const auto& l : target_link_names)
899 {
900 EXPECT_TRUE(kin_group.hasLinkName(l));
901 }
902
903 {
904 std::vector<std::string> link_names = kin_group.getActiveLinkNames();
905 runStringVectorEqualTest(link_names, target_active_link_names);
906 }
907
908 {
909 std::vector<std::string> link_names = kin_group.getStaticLinkNames();
910 runStringVectorEqualTest(link_names, target_static_link_names);
911 }
912
913 {
914 std::vector<std::string> link_names = kin_group.getLinkNames();
915 runStringVectorEqualTest(link_names, target_link_names);
916 }
917}
918
919inline void runInvKinIIWATest(const tesseract_kinematics::KinematicsPluginFactory& factory,
920 const std::string& inv_factory_name,
921 const std::string& fwd_factory_name)
922{
923 tesseract_scene_graph::SceneGraph::Ptr scene_graph = getSceneGraphIIWA();
924 std::string manip_name = "manip";
925 std::string base_link_name = "base_link";
926 std::string tip_link_name = "tool0";
927 std::vector<std::string> joint_names{ "joint_a1", "joint_a2", "joint_a3", "joint_a4",
928 "joint_a5", "joint_a6", "joint_a7" };
929 std::vector<std::string> joint_link_names{ "link_1", "link_2", "link_3", "link_4", "link_5", "link_6", "link_7" };
930 tesseract_common::KinematicLimits target_limits = getTargetLimits(*scene_graph, joint_names);
931
932 tesseract_scene_graph::KDLStateSolver state_solver(*scene_graph);
933 tesseract_scene_graph::SceneState scene_state = state_solver.getState();
934
935 tesseract_common::PluginInfo fwd_plugin_info;
936 fwd_plugin_info.class_name = fwd_factory_name;
937 fwd_plugin_info.config["base_link"] = base_link_name;
938 fwd_plugin_info.config["tip_link"] = tip_link_name;
939
940 tesseract_common::PluginInfo inv_plugin_info;
941 inv_plugin_info.class_name = inv_factory_name;
942 inv_plugin_info.config = fwd_plugin_info.config;
943
944 // Inverse target pose and seed
945 Eigen::Isometry3d pose;
946 pose.setIdentity();
947 pose.translation()[0] = 0;
948 pose.translation()[1] = 0;
949 pose.translation()[2] = 1.306;
950
951 Eigen::VectorXd seed;
952 seed.resize(7);
953 seed(0) = -0.785398;
954 seed(1) = 0.785398;
955 seed(2) = -0.785398;
956 seed(3) = 0.785398;
957 seed(4) = -0.785398;
958 seed(5) = 0.785398;
959 seed(6) = -0.785398;
960
961 // Check create method with empty scene graph
962 tesseract_scene_graph::SceneGraph scene_graph_empty;
963 auto kin_empty = factory.createInvKin(inv_factory_name, inv_plugin_info, scene_graph_empty, scene_state);
964 EXPECT_TRUE(kin_empty == nullptr);
965
966 { // Check create method using base_link and tool0
967 auto fwd_kin = factory.createFwdKin(fwd_factory_name, fwd_plugin_info, *scene_graph, scene_state);
968 EXPECT_TRUE(fwd_kin != nullptr);
969 // EXPECT_EQ(fwd_kin->getSolverName(), fwd_solver_name);
970 EXPECT_EQ(fwd_kin->numJoints(), 7);
971 EXPECT_EQ(fwd_kin->getBaseLinkName(), base_link_name);
972 EXPECT_EQ(fwd_kin->getTipLinkNames().size(), 1);
973 EXPECT_EQ(fwd_kin->getTipLinkNames()[0], tip_link_name);
974 EXPECT_EQ(fwd_kin->getJointNames(), joint_names);
975
976 runJacobianIIWATest(*fwd_kin);
977 runFwdKinIIWATest(*fwd_kin);
978
979 auto inv_kin = factory.createInvKin(inv_factory_name, inv_plugin_info, *scene_graph, scene_state);
980 EXPECT_TRUE(inv_kin != nullptr);
981 // EXPECT_EQ(inv_kin->getSolverName(), inv_solver_name);
982 EXPECT_EQ(inv_kin->numJoints(), 7);
983 EXPECT_EQ(inv_kin->getBaseLinkName(), base_link_name);
984 EXPECT_EQ(inv_kin->getWorkingFrame(), base_link_name);
985 EXPECT_EQ(inv_kin->getTipLinkNames().size(), 1);
986 EXPECT_EQ(inv_kin->getTipLinkNames()[0], tip_link_name);
987 EXPECT_EQ(inv_kin->getJointNames(), joint_names);
988
989 runInvKinTest(*inv_kin, *fwd_kin, pose, tip_link_name, seed);
990
991 KinematicGroup kin_group(manip_name, joint_names, std::move(inv_kin), *scene_graph, scene_state);
992 EXPECT_EQ(kin_group.getBaseLinkName(), scene_graph->getRoot());
993 runInvKinTest(kin_group, pose, base_link_name, tip_link_name, seed);
994 runKinGroupJacobianIIWATest(kin_group);
995 runActiveLinkNamesIIWATest(kin_group);
996 runKinJointLimitsTest(kin_group.getLimits(), target_limits);
997 runKinSetJointLimitsTest(kin_group);
998 }
999
1000 { // Check cloned
1001 auto fwd_kin = factory.createFwdKin(fwd_factory_name, fwd_plugin_info, *scene_graph, scene_state);
1002 EXPECT_TRUE(fwd_kin != nullptr);
1003 auto fwd_kin3 = fwd_kin->clone();
1004 // EXPECT_EQ(fwd_kin3->getSolverName(), fwd_solver_name);
1005 EXPECT_EQ(fwd_kin3->numJoints(), 7);
1006 EXPECT_EQ(fwd_kin3->getBaseLinkName(), base_link_name);
1007 EXPECT_EQ(fwd_kin3->getTipLinkNames().size(), 1);
1008 EXPECT_EQ(fwd_kin3->getTipLinkNames()[0], tip_link_name);
1009 EXPECT_EQ(fwd_kin3->getJointNames(), joint_names);
1010
1011 runJacobianIIWATest(*fwd_kin3);
1012 runFwdKinIIWATest(*fwd_kin3);
1013
1014 auto inv_kin = factory.createInvKin(inv_factory_name, inv_plugin_info, *scene_graph, scene_state);
1015 auto inv_kin3 = inv_kin->clone();
1016 EXPECT_TRUE(inv_kin3 != nullptr);
1017 // EXPECT_EQ(inv_kin3->getSolverName(), inv_solver_name);
1018 EXPECT_EQ(inv_kin3->numJoints(), 7);
1019 EXPECT_EQ(inv_kin3->getBaseLinkName(), base_link_name);
1020 EXPECT_EQ(inv_kin3->getWorkingFrame(), base_link_name);
1021 EXPECT_EQ(inv_kin3->getTipLinkNames().size(), 1);
1022 EXPECT_EQ(inv_kin3->getTipLinkNames()[0], tip_link_name);
1023 EXPECT_EQ(inv_kin3->getJointNames(), joint_names);
1024
1025 runInvKinTest(*inv_kin3, *fwd_kin3, pose, tip_link_name, seed);
1026
1027 KinematicGroup kin_group(manip_name, joint_names, std::move(inv_kin3), *scene_graph, scene_state);
1028 EXPECT_EQ(kin_group.getBaseLinkName(), scene_graph->getRoot());
1029 runInvKinTest(kin_group, pose, base_link_name, tip_link_name, seed);
1030 runKinGroupJacobianIIWATest(kin_group);
1031 runActiveLinkNamesIIWATest(kin_group);
1032 runKinJointLimitsTest(kin_group.getLimits(), target_limits);
1033 runKinSetJointLimitsTest(kin_group);
1034 }
1035
1036 fwd_plugin_info.config["base_link"] = "missing_link";
1037 fwd_plugin_info.config["tip_link"] = tip_link_name;
1038 inv_plugin_info.config = fwd_plugin_info.config;
1039
1040 { // Test forward kinematics failure
1041
1042 auto fwd_kin = factory.createFwdKin(fwd_factory_name, fwd_plugin_info, *scene_graph, scene_state);
1043 EXPECT_TRUE(fwd_kin == nullptr);
1044 }
1045
1046 { // Inverse Kinematics Test failure
1047 auto inv_kin = factory.createInvKin(inv_factory_name, inv_plugin_info, *scene_graph, scene_state);
1048 EXPECT_TRUE(inv_kin == nullptr);
1049 }
1050}
1051
1052} // namespace tesseract_kinematics::test_suite
1053#endif // TESSERACT_KINEMATICS_KIN_TEST_SUITE_H
UNUSED
#define UNUSED(x)
Definition: VHACD.cpp:58
tesseract_common::TesseractSupportResourceLocator
Abstract class for resource loaders.
Definition: tesseract_support_resource_locator.h:42
tesseract_kinematics::ForwardKinematics
Forward kinematics functions.
Definition: forward_kinematics.h:47
tesseract_kinematics::InverseKinematics
Inverse kinematics functions.
Definition: inverse_kinematics.h:47
tesseract_kinematics::KinematicGroup
Definition: kinematic_group.h:78
tesseract_kinematics::KinematicsPluginFactory
Definition: kinematics_plugin_factory.h:114
tesseract_scene_graph::Joint
Definition: joint.h:281
tesseract_scene_graph::KDLStateSolver
Definition: kdl_state_solver.h:43
tesseract_scene_graph::KDLStateSolver::getState
SceneState getState(const Eigen::Ref< const Eigen::VectorXd > &joint_values) const override final
Get the state of the solver given the joint values.
Definition: kdl_state_solver.cpp:109
tesseract_scene_graph::Link
Definition: link.h:188
tesseract_scene_graph::SceneGraph
Definition: graph.h:125
tesseract_scene_graph::SceneGraph::Ptr
std::shared_ptr< SceneGraph > Ptr
Definition: graph.h:130
tesseract_scene_graph::SceneGraph::UPtr
std::unique_ptr< SceneGraph > UPtr
Definition: graph.h:132
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))
factory
ContactManagersPluginFactory factory(config)
Definition: kdl_kinematics_unit.cpp:31
EXPECT_ANY_THROW
EXPECT_ANY_THROW(factory.getDefaultDiscreteContactManagerPlugin())
forward_kinematics.h
Forward kinematics functions.
graph.h
A basic scene graph using boost.
seed
Eigen::VectorXd seed
Definition: ikfast_kinematics_7dof_unit.cpp:48
base_link_name
std::string base_link_name
Definition: ikfast_kinematics_7dof_unit.cpp:52
tip_link_name
std::string tip_link_name
Definition: ikfast_kinematics_7dof_unit.cpp:53
scene_graph
auto scene_graph
Definition: ikfast_kinematics_7dof_unit.cpp:51
inv_kin
auto inv_kin
Definition: ikfast_kinematics_unit.cpp:59
manip_name
std::string manip_name
Definition: ikfast_kinematics_unit.cpp:52
inverse_kinematics.h
Inverse kinematics functions.
kdl_state_solver.h
Tesseract Scene Graph State Solver KDL Implementation.
kinematic_group.h
A kinematic group with forward and inverse kinematics methods.
jacobian
Eigen::MatrixXd jacobian
Definition: kinematics_core_unit.cpp:153
fwd_kin
auto fwd_kin
Definition: kinematics_factory_unit.cpp:1140
kin
auto kin
Definition: kinematics_factory_unit.cpp:1165
scene_state
tesseract_scene_graph::SceneState scene_state
Definition: kinematics_factory_unit.cpp:265
kinematics_plugin_factory.h
Kinematics Plugin Factory.
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::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::TransformMap
AlignedMap< std::string, Eigen::Isometry3d > TransformMap
Definition: types.h:66
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_kinematics::test_suite
Definition: kinematics_test_utils.h:52
tesseract_kinematics::test_suite::runStringVectorEqualTest
void runStringVectorEqualTest(const std::vector< std::string > &names, const std::vector< std::string > &target_names)
Check if two vectors of strings are equal but ignore order.
Definition: kinematics_test_utils.h:394
tesseract_kinematics::test_suite::runKinGroupJacobianABBExternalPositionerTest
void runKinGroupJacobianABBExternalPositionerTest(tesseract_kinematics::KinematicGroup &kin_group)
Definition: kinematics_test_utils.h:829
tesseract_kinematics::test_suite::runActiveLinkNamesABBExternalPositionerTest
void runActiveLinkNamesABBExternalPositionerTest(const tesseract_kinematics::KinematicGroup &kin_group)
Definition: kinematics_test_utils.h:877
tesseract_kinematics::test_suite::runKinGroupJacobianABBOnPositionerTest
void runKinGroupJacobianABBOnPositionerTest(tesseract_kinematics::KinematicGroup &kin_group)
Definition: kinematics_test_utils.h:741
tesseract_kinematics::test_suite::getSceneGraphABBOnPositioner
tesseract_scene_graph::SceneGraph::UPtr getSceneGraphABBOnPositioner()
Definition: kinematics_test_utils.h:69
tesseract_kinematics::test_suite::runKinSetJointLimitsTest
void runKinSetJointLimitsTest(tesseract_kinematics::KinematicGroup &kin_group)
Run kinematics setJointLimits function test.
Definition: kinematics_test_utils.h:362
tesseract_kinematics::test_suite::runInvKinTest
void runInvKinTest(const tesseract_kinematics::InverseKinematics &inv_kin, const tesseract_kinematics::ForwardKinematics &fwd_kin, const Eigen::Isometry3d &target_pose, const std::string &tip_link_name, const Eigen::VectorXd &seed)
Run inverse kinematics test comparing the inverse solution to the forward solution.
Definition: kinematics_test_utils.h:416
tesseract_kinematics::test_suite::runActiveLinkNamesURTest
void runActiveLinkNamesURTest(const tesseract_kinematics::KinematicGroup &kin_group)
Definition: kinematics_test_utils.h:699
tesseract_kinematics::test_suite::runActiveLinkNamesABBOnPositionerTest
void runActiveLinkNamesABBOnPositionerTest(const tesseract_kinematics::KinematicGroup &kin_group)
Definition: kinematics_test_utils.h:788
tesseract_kinematics::test_suite::getSceneGraphABBExternalPositioner
tesseract_scene_graph::SceneGraph::UPtr getSceneGraphABBExternalPositioner()
Definition: kinematics_test_utils.h:61
tesseract_kinematics::test_suite::runActiveLinkNamesIIWATest
void runActiveLinkNamesIIWATest(const tesseract_kinematics::KinematicGroup &kin_group)
Definition: kinematics_test_utils.h:617
tesseract_kinematics::test_suite::runJacobianTest
void runJacobianTest(tesseract_kinematics::ForwardKinematics &kin, 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: kinematics_test_utils.h:260
tesseract_kinematics::test_suite::runJacobianIIWATest
void runJacobianIIWATest(tesseract_kinematics::ForwardKinematics &kin, bool is_kin_tree=false)
Definition: kinematics_test_utils.h:497
tesseract_kinematics::test_suite::runKinJointLimitsTest
void runKinJointLimitsTest(const tesseract_common::KinematicLimits &limits, const tesseract_common::KinematicLimits &target_limits)
Run kinematic limits test.
Definition: kinematics_test_utils.h:338
tesseract_kinematics::test_suite::runFwdKinIIWATest
void runFwdKinIIWATest(tesseract_kinematics::ForwardKinematics &kin)
Definition: kinematics_test_utils.h:475
tesseract_kinematics::test_suite::getSceneGraphUR
tesseract_scene_graph::SceneGraph::UPtr getSceneGraphUR(const tesseract_kinematics::URParameters &params, double shoulder_offset, double elbow_offset)
Definition: kinematics_test_utils.h:93
tesseract_kinematics::test_suite::getTargetLimits
tesseract_common::KinematicLimits getTargetLimits(const tesseract_scene_graph::SceneGraph &scene_graph, const std::vector< std::string > &joint_names)
Definition: kinematics_test_utils.h:230
tesseract_kinematics::test_suite::getSceneGraphIIWA7
tesseract_scene_graph::SceneGraph::UPtr getSceneGraphIIWA7()
Definition: kinematics_test_utils.h:85
tesseract_kinematics::test_suite::runKinGroupJacobianIIWATest
void runKinGroupJacobianIIWATest(tesseract_kinematics::KinematicGroup &kin_group)
Definition: kinematics_test_utils.h:580
tesseract_kinematics::test_suite::runActiveLinkNamesABBTest
void runActiveLinkNamesABBTest(const tesseract_kinematics::KinematicGroup &kin_group)
Definition: kinematics_test_utils.h:658
tesseract_kinematics::test_suite::getSceneGraphABB
tesseract_scene_graph::SceneGraph::UPtr getSceneGraphABB()
Definition: kinematics_test_utils.h:77
tesseract_kinematics::test_suite::runInvKinIIWATest
void runInvKinIIWATest(const tesseract_kinematics::KinematicsPluginFactory &factory, const std::string &inv_factory_name, const std::string &fwd_factory_name)
Definition: kinematics_test_utils.h:919
tesseract_kinematics::test_suite::getSceneGraphIIWA
tesseract_scene_graph::SceneGraph::UPtr getSceneGraphIIWA()
Definition: kinematics_test_utils.h:53
tesseract_kinematics::numericalJacobian
static void numericalJacobian(Eigen::Ref< Eigen::MatrixXd > jacobian, const Eigen::Isometry3d &change_base, const tesseract_kinematics::ForwardKinematics &kin, 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: utils.h:55
tesseract_kinematics::checkKinematics
bool checkKinematics(const KinematicGroup &manip, double tol=1e-3)
This compares calcFwdKin to calcInvKin for a KinematicGroup.
Definition: validate.cpp:40
tesseract_kinematics::KinGroupIKInputs
tesseract_common::AlignedVector< KinGroupIKInput > KinGroupIKInputs
Definition: kinematic_group.h:75
tesseract_kinematics::IKSolutions
std::vector< Eigen::VectorXd > IKSolutions
The inverse kinematics solutions container.
Definition: types.h:41
tesseract_scene_graph
Definition: graph.h:82
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
s
std::set< std::string > s
Definition: plugin_loader_unit.cpp:45
target_limits
tesseract_common::KinematicLimits target_limits
Definition: rep_kinematics_unit.cpp:117
kin_group
KinematicGroup kin_group(manip_name, joint_names, std::move(inv_kin), *scene_graph, scene_state)
locator
ResourceLocator::Ptr locator
Definition: resource_locator_unit.cpp:57
tesseract_common::KinematicLimits
Store kinematic limits.
Definition: kinematic_limits.h:39
tesseract_common::KinematicLimits::joint_limits
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::MatrixX2d joint_limits
The position limits.
Definition: kinematic_limits.h:45
tesseract_common::KinematicLimits::acceleration_limits
Eigen::VectorXd acceleration_limits
The acceleration limits.
Definition: kinematic_limits.h:51
tesseract_common::KinematicLimits::velocity_limits
Eigen::VectorXd velocity_limits
The velocity limits.
Definition: kinematic_limits.h:48
tesseract_common::PluginInfo
The Plugin Information struct.
Definition: types.h:104
tesseract_common::PluginInfo::class_name
std::string class_name
The plugin class name.
Definition: types.h:106
tesseract_common::PluginInfo::config
YAML::Node config
The plugin config data.
Definition: types.h:109
tesseract_kinematics::KinGroupIKInput
Structure containing the data required to solve inverse kinematics.
Definition: kinematic_group.h:48
tesseract_kinematics::URParameters
The Universal Robot kinematic parameters.
Definition: types.h:45
tesseract_kinematics::URParameters::d1
double d1
Definition: types.h:52
tesseract_kinematics::URParameters::d6
double d6
Definition: types.h:57
tesseract_kinematics::URParameters::a2
double a2
Definition: types.h:53
tesseract_kinematics::URParameters::d5
double d5
Definition: types.h:56
tesseract_kinematics::URParameters::a3
double a3
Definition: types.h:54
tesseract_kinematics::URParameters::d4
double d4
Definition: types.h:55
tesseract_scene_graph::SceneState
This holds a state of the scene.
Definition: scene_state.h:54
utils.h
Common Tesseract Utility Functions.
joint_names
joint_state joint_names
Definition: tesseract_common_serialization_unit.cpp:75
v2
std::vector< std::string > v2
Definition: tesseract_common_unit.cpp:418
working_frame
manip_info_override working_frame
Definition: tesseract_common_unit.cpp:267
v1
v1["1"]
Definition: tesseract_common_unit.cpp:434
pose
auto pose
Definition: tesseract_environment_collision.cpp:118
types.h
Kinematics types.
utils.h
Kinematics utility functions.
limits
joint_1 limits
Definition: tesseract_scene_graph_joint_unit.cpp:153
j
JointDynamics j
Definition: tesseract_scene_graph_joint_unit.cpp:15
l
Link l("test_link")
path
tesseract_common::fs::path path
Definition: tesseract_srdf_unit.cpp:1992
base_link
Link base_link("base_link")
tesseract_support_resource_locator.h
Locate and retrieve resource data in tesseract_support.
sg
tesseract_scene_graph::SceneGraph::Ptr sg
Definition: tesseract_urdf_urdf_unit.cpp:31
elbow_offset
double elbow_offset
Definition: ur_kinematics_unit.cpp:92
shoulder_offset
double shoulder_offset
Definition: ur_kinematics_unit.cpp:91
urdf_parser.h
A urdf parser for tesseract.
validate.h
This contains utility function validate things like forward kinematics match inverse kinematics.
Generated on Thu Mar 30 2023 23:19:52 for Tesseract by doxygen 1.9.5