rep_kinematics_unit.cpp
Go to the documentation of this file.
1 #include <tesseract_common/macros.h>
2 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
3 #include <gtest/gtest.h>
4 #include <fstream>
5 #include <tesseract_urdf/urdf_parser.h>
6 TESSERACT_COMMON_IGNORE_WARNINGS_POP
7 
8 #include "kinematics_test_utils.h"
9 
10 #include <tesseract_kinematics/kdl/kdl_fwd_kin_chain.h>
11 #include <tesseract_kinematics/core/rep_inv_kin.h>
12 #include <tesseract_kinematics/opw/opw_inv_kin.h>
13 #include <tesseract_kinematics/core/utils.h>
14 #include <tesseract_kinematics/core/kinematic_group.h>
15 #include <tesseract_state_solver/kdl/kdl_state_solver.h>
16 #include <opw_kinematics/opw_parameters.h>
17 
18 using namespace tesseract_kinematics::test_suite;
19 using namespace tesseract_kinematics;
20 
21 inline opw_kinematics::Parameters<double> getOPWKinematicsParamABB()
22 {
23  opw_kinematics::Parameters<double> opw_params;
24  opw_params.a1 = (0.100);
25  opw_params.a2 = (-0.135);
26  opw_params.b = (0.000);
27  opw_params.c1 = (0.615);
28  opw_params.c2 = (0.705);
29  opw_params.c3 = (0.755);
30  opw_params.c4 = (0.085);
31 
32  opw_params.offsets[2] = -M_PI / 2.0;
33 
34  return opw_params;
35 }
36 
37 ForwardKinematics::UPtr getRobotFwdKinematics(const tesseract_scene_graph::SceneGraph& scene_graph)
38 {
39  return std::make_unique<KDLFwdKinChain>(scene_graph, "base_link", "tool0");
40 }
41 
42 ForwardKinematics::UPtr getFullFwdKinematics(const tesseract_scene_graph::SceneGraph& scene_graph)
43 {
44  return std::make_unique<KDLFwdKinChain>(scene_graph, "positioner_tool0", "tool0");
45 }
46 
47 ForwardKinematics::UPtr getPositionerFwdKinematics(const tesseract_scene_graph::SceneGraph& scene_graph)
48 {
49  return std::make_unique<KDLFwdKinChain>(scene_graph, "positioner_base_link", "positioner_tool0");
50 }
51 
52 InverseKinematics::UPtr getFullInvKinematics(const tesseract_scene_graph::SceneGraph& scene_graph)
53 {
54  auto robot_fwd_kin = getRobotFwdKinematics(scene_graph);
55 
56  tesseract_scene_graph::KDLStateSolver state_solver(scene_graph);
57  tesseract_scene_graph::SceneState scene_state = state_solver.getState();
58 
59  opw_kinematics::Parameters<double> opw_params = getOPWKinematicsParamABB();
60 
61  auto opw_kin = std::make_unique<OPWInvKin>(opw_params,
62  robot_fwd_kin->getBaseLinkName(),
63  robot_fwd_kin->getTipLinkNames()[0],
64  robot_fwd_kin->getJointNames());
65 
66  auto positioner_kin = getPositionerFwdKinematics(scene_graph);
67  Eigen::VectorXd positioner_resolution = Eigen::VectorXd::Constant(2, 1, 0.1);
68  auto rep_inv_kin = std::make_unique<REPInvKin>(
69  scene_graph, scene_state, opw_kin->clone(), 2.5, positioner_kin->clone(), positioner_resolution);
70 
71  { // Test failure
72  auto scene_graph_empty = std::make_shared<tesseract_scene_graph::SceneGraph>();
73  // NOLINTNEXTLINE
74  EXPECT_ANY_THROW(std::make_shared<REPInvKin>(
75  *scene_graph_empty, scene_state, opw_kin->clone(), 2.5, positioner_kin->clone(), positioner_resolution));
76 
77  // NOLINTNEXTLINE
78  EXPECT_ANY_THROW(std::make_shared<REPInvKin>(
79  scene_graph, scene_state, nullptr, 2.5, positioner_kin->clone(), positioner_resolution));
80 
81  // NOLINTNEXTLINE
82  EXPECT_ANY_THROW(std::make_shared<REPInvKin>(
83  scene_graph, scene_state, opw_kin->clone(), -2.5, positioner_kin->clone(), positioner_resolution));
84 
85  // NOLINTNEXTLINE
86  EXPECT_ANY_THROW(
87  std::make_shared<REPInvKin>(scene_graph, scene_state, opw_kin->clone(), 2.5, nullptr, positioner_resolution));
88 
89  positioner_resolution = Eigen::VectorXd();
90  // NOLINTNEXTLINE
91  EXPECT_ANY_THROW(std::make_shared<REPInvKin>(
92  scene_graph, scene_state, opw_kin->clone(), 2.5, positioner_kin->clone(), positioner_resolution));
93 
94  positioner_resolution = Eigen::VectorXd::Constant(2, -0.1);
95  // NOLINTNEXTLINE
96  EXPECT_ANY_THROW(std::make_shared<REPInvKin>(
97  scene_graph, scene_state, opw_kin->clone(), 2.5, positioner_kin->clone(), positioner_resolution));
98  }
99 
100  return rep_inv_kin;
101 }
102 
103 TEST(TesseractKinematicsUnit, RobotWithExternalPositionerInverseKinematicUnit) // NOLINT
104 {
105  tesseract_common::GeneralResourceLocator locator;
106  auto scene_graph = getSceneGraphABBExternalPositioner(locator);
107 
108  tesseract_scene_graph::KDLStateSolver state_solver(*scene_graph);
109  tesseract_scene_graph::SceneState scene_state = state_solver.getState();
110 
111  std::string manip_name = "robot_external_positioner";
112  std::string base_link_name = "base_link";
113  std::string working_frame = "positioner_tool0";
114  std::string tip_link_name = "tool0";
115  std::vector<std::string> joint_names{
116  "positioner_joint_1", "positioner_joint_2", "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"
117  };
118  tesseract_common::KinematicLimits target_limits = getTargetLimits(*scene_graph, joint_names);
119 
120  auto fwd_kin = getFullFwdKinematics(*scene_graph);
121  auto inv_kin = getFullInvKinematics(*scene_graph);
122  auto inv_kin2 = inv_kin->clone();
123 
124  std::vector<std::string> fwd_joint_names = fwd_kin->getJointNames();
125  std::vector<std::string> inv_joint_names = inv_kin->getJointNames();
126 
127  EXPECT_TRUE(tesseract_common::isIdentical(fwd_joint_names, inv_joint_names, false));
128 
129  Eigen::Isometry3d pose;
130  pose.setIdentity();
131  pose.translation()[0] = 0;
132  pose.translation()[1] = 0;
133  pose.translation()[2] = 0.1;
134 
135  Eigen::VectorXd seed = Eigen::VectorXd::Zero(fwd_kin->numJoints());
136 
137  EXPECT_TRUE(inv_kin != nullptr);
138  EXPECT_EQ(inv_kin->getSolverName(), DEFAULT_REP_INV_KIN_SOLVER_NAME);
139  EXPECT_EQ(inv_kin->numJoints(), 8);
140  EXPECT_EQ(inv_kin->getBaseLinkName(), base_link_name);
141  EXPECT_EQ(inv_kin->getWorkingFrame(), working_frame);
142  EXPECT_EQ(inv_kin->getTipLinkNames().size(), 1);
143  EXPECT_EQ(inv_kin->getTipLinkNames()[0], tip_link_name);
144  EXPECT_EQ(inv_kin->getJointNames(), joint_names);
145 
146  KinematicGroup kin_group(manip_name, joint_names, std::move(inv_kin), *scene_graph, scene_state);
147  KinematicGroup kin_group_copy(kin_group);
148 
149  {
150  EXPECT_EQ(kin_group.getBaseLinkName(), scene_graph->getRoot());
151  runInvKinTest(kin_group, pose, working_frame, tip_link_name, seed);
152  runKinGroupJacobianABBExternalPositionerTest(kin_group);
153  runActiveLinkNamesABBExternalPositionerTest(kin_group);
154  runKinJointLimitsTest(kin_group.getLimits(), target_limits);
155  runKinSetJointLimitsTest(kin_group);
156  EXPECT_EQ(kin_group.getName(), manip_name);
157  EXPECT_EQ(kin_group.getJointNames(), joint_names);
158 
159  auto all_tip_link_names = kin_group.getAllPossibleTipLinkNames();
160  EXPECT_GE(all_tip_link_names.size(), 1);
161  EXPECT_NE(std::find(all_tip_link_names.begin(), all_tip_link_names.end(), tip_link_name), all_tip_link_names.end());
162 
163  auto all_working_frames = kin_group.getAllValidWorkingFrames();
164  EXPECT_GE(all_working_frames.size(), 1);
165  EXPECT_NE(std::find(all_working_frames.begin(), all_working_frames.end(), working_frame), all_working_frames.end());
166  }
167 
168  // Check KinematicGroup copy
169  {
170  EXPECT_EQ(kin_group_copy.getBaseLinkName(), scene_graph->getRoot());
171  runInvKinTest(kin_group_copy, pose, working_frame, tip_link_name, seed);
172  runKinGroupJacobianABBExternalPositionerTest(kin_group_copy);
173  runActiveLinkNamesABBExternalPositionerTest(kin_group_copy);
174  runKinJointLimitsTest(kin_group_copy.getLimits(), target_limits);
175  runKinSetJointLimitsTest(kin_group_copy);
176  EXPECT_EQ(kin_group_copy.getName(), manip_name);
177  EXPECT_EQ(kin_group_copy.getJointNames(), joint_names);
178 
179  auto all_tip_link_names = kin_group_copy.getAllPossibleTipLinkNames();
180  EXPECT_GE(all_tip_link_names.size(), 1);
181  EXPECT_NE(std::find(all_tip_link_names.begin(), all_tip_link_names.end(), tip_link_name), all_tip_link_names.end());
182 
183  auto all_working_frames = kin_group_copy.getAllValidWorkingFrames();
184  EXPECT_GE(all_working_frames.size(), 1);
185  EXPECT_NE(std::find(all_working_frames.begin(), all_working_frames.end(), working_frame), all_working_frames.end());
186  }
187 
188  // Check cloned
189  EXPECT_TRUE(inv_kin2 != nullptr);
190  EXPECT_EQ(inv_kin2->getSolverName(), DEFAULT_REP_INV_KIN_SOLVER_NAME);
191  EXPECT_EQ(inv_kin2->numJoints(), 8);
192  EXPECT_EQ(inv_kin2->getBaseLinkName(), base_link_name);
193  EXPECT_EQ(inv_kin2->getWorkingFrame(), working_frame);
194  EXPECT_EQ(inv_kin2->getTipLinkNames().size(), 1);
195  EXPECT_EQ(inv_kin2->getTipLinkNames()[0], tip_link_name);
196  EXPECT_EQ(inv_kin2->getJointNames(), joint_names);
197 
198  KinematicGroup kin_group2(manip_name, joint_names, std::move(inv_kin2), *scene_graph, scene_state);
199  EXPECT_EQ(kin_group2.getBaseLinkName(), scene_graph->getRoot());
200  runInvKinTest(kin_group2, pose, working_frame, tip_link_name, seed);
201  runKinGroupJacobianABBExternalPositionerTest(kin_group2);
202  runActiveLinkNamesABBExternalPositionerTest(kin_group2);
203  runKinJointLimitsTest(kin_group2.getLimits(), target_limits);
204  runKinSetJointLimitsTest(kin_group2);
205 }
206 
207 int main(int argc, char** argv)
208 {
209  testing::InitGoogleTest(&argc, argv);
210 
211  return RUN_ALL_TESTS();
212 }
tesseract_scene_graph::KDLStateSolver::getState
SceneState getState() const override final
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:364
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;})
TEST
TEST(TesseractKinematicsUnit, RobotWithExternalPositionerInverseKinematicUnit)
Definition: rep_kinematics_unit.cpp:103
kdl_state_solver.h
kdl_fwd_kin_chain.h
Tesseract KDL forward kinematics chain implementation.
opw_kinematics::Parameters::a2
T a2
kinematics_test_utils.h
tesseract_common::KinematicLimits
opw_kinematics::Parameters::c3
T c3
rep_inv_kin.h
getPositionerFwdKinematics
ForwardKinematics::UPtr getPositionerFwdKinematics(const tesseract_scene_graph::SceneGraph &scene_graph)
Definition: rep_kinematics_unit.cpp:47
getRobotFwdKinematics
ForwardKinematics::UPtr getRobotFwdKinematics(const tesseract_scene_graph::SceneGraph &scene_graph)
Definition: rep_kinematics_unit.cpp:37
tesseract_kinematics::JointGroup::getBaseLinkName
std::string getBaseLinkName() const
Get the robot base link name.
Definition: joint_group.cpp:301
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#define TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
tesseract_scene_graph::SceneGraph
tesseract_kinematics::test_suite::getSceneGraphABBExternalPositioner
tesseract_scene_graph::SceneGraph::UPtr getSceneGraphABBExternalPositioner(const tesseract_common::ResourceLocator &locator)
Definition: kinematics_test_utils.h:62
tesseract_kinematics::InverseKinematics::UPtr
std::unique_ptr< InverseKinematics > UPtr
Definition: inverse_kinematics.h:52
tesseract_scene_graph::SceneState
tesseract_kinematics::DEFAULT_REP_INV_KIN_SOLVER_NAME
static const std::string DEFAULT_REP_INV_KIN_SOLVER_NAME
Definition: rep_inv_kin.h:40
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:226
utils.h
Kinematics utility functions.
tesseract_kinematics::JointGroup::getName
std::string getName() const
Name of the manipulator.
Definition: joint_group.cpp:303
opw_inv_kin.h
Tesseract OPW Inverse kinematics Wrapper.
getFullInvKinematics
InverseKinematics::UPtr getFullInvKinematics(const tesseract_scene_graph::SceneGraph &scene_graph)
Definition: rep_kinematics_unit.cpp:52
opw_kinematics::Parameters::c1
T c1
tesseract_kinematics::KinematicGroup::getAllPossibleTipLinkNames
std::vector< std::string > getAllPossibleTipLinkNames() const
Get the tip link name.
Definition: kinematic_group.cpp:309
opw_kinematics::Parameters::offsets
std::array< T, 6 > offsets
tesseract_kinematics::JointGroup::getJointNames
std::vector< std::string > getJointNames() const
Get list of joint names for kinematic object.
Definition: joint_group.cpp:267
tesseract_scene_graph::KDLStateSolver
opw_kinematics::Parameters::b
T b
tesseract_kinematics::test_suite::runKinGroupJacobianABBExternalPositionerTest
void runKinGroupJacobianABBExternalPositionerTest(tesseract_kinematics::KinematicGroup &kin_group)
Definition: kinematics_test_utils.h:871
tesseract_kinematics::test_suite
Definition: kinematics_test_utils.h:53
tesseract_kinematics::KinematicGroup::getAllValidWorkingFrames
std::vector< std::string > getAllValidWorkingFrames() const
Returns all possible working frames in which goal poses can be defined.
Definition: kinematic_group.cpp:307
kinematic_group.h
A kinematic group with forward and inverse kinematics methods.
main
int main(int argc, char **argv)
Definition: rep_kinematics_unit.cpp:207
tesseract_kinematics::test_suite::runActiveLinkNamesABBExternalPositionerTest
void runActiveLinkNamesABBExternalPositionerTest(const tesseract_kinematics::KinematicGroup &kin_group)
Definition: kinematics_test_utils.h:919
opw_kinematics::Parameters::a1
T a1
TESSERACT_COMMON_IGNORE_WARNINGS_POP
#define TESSERACT_COMMON_IGNORE_WARNINGS_POP
tesseract_kinematics::ForwardKinematics::UPtr
std::unique_ptr< ForwardKinematics > UPtr
Definition: forward_kinematics.h:52
tesseract_kinematics::JointGroup::getLimits
tesseract_common::KinematicLimits getLimits() const
Get the kinematic limits (joint, velocity, acceleration, etc.)
Definition: joint_group.cpp:285
tesseract_kinematics
Definition: forward_kinematics.h:40
opw_parameters.h
tesseract_common::GeneralResourceLocator
opw_kinematics::Parameters< double >
getFullFwdKinematics
ForwardKinematics::UPtr getFullFwdKinematics(const tesseract_scene_graph::SceneGraph &scene_graph)
Definition: rep_kinematics_unit.cpp:42
macros.h
getOPWKinematicsParamABB
opw_kinematics::Parameters< double > getOPWKinematicsParamABB()
Definition: rep_kinematics_unit.cpp:21
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:458
tesseract_kinematics::KinematicGroup
Definition: kinematic_group.h:75
tesseract_kinematics::test_suite::runKinSetJointLimitsTest
void runKinSetJointLimitsTest(tesseract_kinematics::KinematicGroup &kin_group)
Run kinematics setJointLimits function test.
Definition: kinematics_test_utils.h:396
opw_kinematics::Parameters::c2
T c2
opw_kinematics::Parameters::c4
T c4

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