rop_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/rop_inv_kin.h>
12 #include <tesseract_kinematics/opw/opw_inv_kin.h>
13 #include <tesseract_kinematics/core/utils.h>
14 #include <opw_kinematics/opw_parameters.h>
15 #include <tesseract_state_solver/kdl/kdl_state_solver.h>
16 
17 using namespace tesseract_kinematics::test_suite;
18 using namespace tesseract_kinematics;
19 
20 inline opw_kinematics::Parameters<double> getOPWKinematicsParamABB()
21 {
22  opw_kinematics::Parameters<double> opw_params;
23  opw_params.a1 = (0.100);
24  opw_params.a2 = (-0.135);
25  opw_params.b = (0.000);
26  opw_params.c1 = (0.615);
27  opw_params.c2 = (0.705);
28  opw_params.c3 = (0.755);
29  opw_params.c4 = (0.085);
30 
31  opw_params.offsets[2] = -M_PI / 2.0;
32 
33  return opw_params;
34 }
35 
36 ForwardKinematics::UPtr getRobotFwdKinematics(const tesseract_scene_graph::SceneGraph& scene_graph)
37 {
38  return std::make_unique<KDLFwdKinChain>(scene_graph, "base_link", "tool0");
39 }
40 
41 ForwardKinematics::UPtr getFullFwdKinematics(const tesseract_scene_graph::SceneGraph& scene_graph)
42 {
43  return std::make_unique<KDLFwdKinChain>(scene_graph, "positioner_base_link", "tool0");
44 }
45 
46 ForwardKinematics::UPtr getPositionerFwdKinematics(const tesseract_scene_graph::SceneGraph& scene_graph)
47 {
48  return std::make_unique<KDLFwdKinChain>(scene_graph, "positioner_base_link", "positioner_tool0");
49 }
50 
51 InverseKinematics::UPtr getFullInvKinematics(const tesseract_scene_graph::SceneGraph& scene_graph)
52 {
53  auto robot_fwd_kin = getRobotFwdKinematics(scene_graph);
54 
55  tesseract_scene_graph::KDLStateSolver state_solver(scene_graph);
56  tesseract_scene_graph::SceneState scene_state = state_solver.getState();
57 
58  opw_kinematics::Parameters<double> opw_params = getOPWKinematicsParamABB();
59 
60  auto opw_kin = std::make_unique<OPWInvKin>(opw_params,
61  robot_fwd_kin->getBaseLinkName(),
62  robot_fwd_kin->getTipLinkNames()[0],
63  robot_fwd_kin->getJointNames());
64 
65  auto positioner_kin = getPositionerFwdKinematics(scene_graph);
66  Eigen::VectorXd positioner_resolution = Eigen::VectorXd::Constant(1, 1, 0.1);
67  auto rop_inv_kin = std::make_unique<ROPInvKin>(
68  scene_graph, scene_state, opw_kin->clone(), 2.5, positioner_kin->clone(), positioner_resolution);
69 
70  { // Test failure
71  tesseract_scene_graph::SceneGraph scene_graph_empty;
72  // NOLINTNEXTLINE
73  EXPECT_ANY_THROW(std::make_unique<ROPInvKin>(
74  scene_graph_empty, scene_state, opw_kin->clone(), 2.5, positioner_kin->clone(), positioner_resolution));
75 
76  // NOLINTNEXTLINE
77  EXPECT_ANY_THROW(std::make_unique<ROPInvKin>(
78  scene_graph, scene_state, nullptr, 2.5, positioner_kin->clone(), positioner_resolution));
79 
80  // NOLINTNEXTLINE
81  EXPECT_ANY_THROW(std::make_unique<ROPInvKin>(
82  scene_graph, scene_state, opw_kin->clone(), -2.5, positioner_kin->clone(), positioner_resolution));
83 
84  // NOLINTNEXTLINE
85  EXPECT_ANY_THROW(
86  std::make_unique<ROPInvKin>(scene_graph, scene_state, opw_kin->clone(), 2.5, nullptr, positioner_resolution));
87 
88  positioner_resolution = Eigen::VectorXd();
89  // NOLINTNEXTLINE
90  EXPECT_ANY_THROW(std::make_unique<ROPInvKin>(
91  scene_graph, scene_state, opw_kin->clone(), 2.5, positioner_kin->clone(), positioner_resolution));
92 
93  positioner_resolution = Eigen::VectorXd::Constant(1, -0.1);
94  // NOLINTNEXTLINE
95  EXPECT_ANY_THROW(std::make_unique<ROPInvKin>(
96  scene_graph, scene_state, opw_kin->clone(), 2.5, positioner_kin->clone(), positioner_resolution));
97  }
98 
99  return rop_inv_kin;
100 }
101 
102 TEST(TesseractKinematicsUnit, RobotOnPositionerInverseKinematicUnit) // NOLINT
103 {
104  tesseract_common::GeneralResourceLocator locator;
105  auto scene_graph = getSceneGraphABBOnPositioner(locator);
106 
107  tesseract_scene_graph::KDLStateSolver state_solver(*scene_graph);
108  tesseract_scene_graph::SceneState scene_state = state_solver.getState();
109 
110  std::string manip_name = "robot_on_positioner";
111  std::string base_link_name = "positioner_base_link";
112  std::string tip_link_name = "tool0";
113  std::vector<std::string> joint_names{
114  "positioner_joint_1", "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"
115  };
116  tesseract_common::KinematicLimits target_limits = getTargetLimits(*scene_graph, joint_names);
117 
118  auto fwd_kin = getFullFwdKinematics(*scene_graph);
119  auto inv_kin = getFullInvKinematics(*scene_graph);
120  auto inv_kin2 = inv_kin->clone();
121 
122  std::vector<std::string> fwd_joint_names = fwd_kin->getJointNames();
123  std::vector<std::string> inv_joint_names = inv_kin->getJointNames();
124 
125  EXPECT_TRUE(tesseract_common::isIdentical(fwd_joint_names, inv_joint_names, false));
126 
127  Eigen::Isometry3d pose;
128  pose.setIdentity();
129  pose.translation()[0] = 1;
130  pose.translation()[1] = 0;
131  pose.translation()[2] = 1.306;
132 
133  Eigen::VectorXd seed = Eigen::VectorXd::Zero(fwd_kin->numJoints());
134 
135  EXPECT_TRUE(inv_kin != nullptr);
136  EXPECT_EQ(inv_kin->getSolverName(), DEFAULT_ROP_INV_KIN_SOLVER_NAME);
137  EXPECT_EQ(inv_kin->numJoints(), 7);
138  EXPECT_EQ(inv_kin->getBaseLinkName(), base_link_name);
139  EXPECT_EQ(inv_kin->getWorkingFrame(), base_link_name);
140  EXPECT_EQ(inv_kin->getTipLinkNames().size(), 1);
141  EXPECT_EQ(inv_kin->getTipLinkNames()[0], tip_link_name);
142  EXPECT_EQ(inv_kin->getJointNames(), joint_names);
143 
144  runInvKinTest(*inv_kin, *fwd_kin, pose, tip_link_name, seed);
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, base_link_name, tip_link_name, seed);
152  runKinGroupJacobianABBOnPositionerTest(kin_group);
153  runActiveLinkNamesABBOnPositionerTest(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(), base_link_name),
166  all_working_frames.end());
167  }
168 
169  // Check KinematicGroup copy
170  {
171  EXPECT_EQ(kin_group_copy.getBaseLinkName(), scene_graph->getRoot());
172  runInvKinTest(kin_group_copy, pose, base_link_name, tip_link_name, seed);
173  runKinGroupJacobianABBOnPositionerTest(kin_group_copy);
174  runActiveLinkNamesABBOnPositionerTest(kin_group_copy);
175  runKinJointLimitsTest(kin_group_copy.getLimits(), target_limits);
176  runKinSetJointLimitsTest(kin_group_copy);
177  EXPECT_EQ(kin_group_copy.getName(), manip_name);
178  EXPECT_EQ(kin_group_copy.getJointNames(), joint_names);
179 
180  auto all_tip_link_names = kin_group.getAllPossibleTipLinkNames();
181  EXPECT_GE(all_tip_link_names.size(), 1);
182  EXPECT_NE(std::find(all_tip_link_names.begin(), all_tip_link_names.end(), tip_link_name), all_tip_link_names.end());
183 
184  auto all_working_frames = kin_group.getAllValidWorkingFrames();
185  EXPECT_GE(all_working_frames.size(), 1);
186  EXPECT_NE(std::find(all_working_frames.begin(), all_working_frames.end(), base_link_name),
187  all_working_frames.end());
188  }
189 
190  // Check cloned
191  EXPECT_TRUE(inv_kin2 != nullptr);
192  EXPECT_EQ(inv_kin2->getSolverName(), DEFAULT_ROP_INV_KIN_SOLVER_NAME);
193  EXPECT_EQ(inv_kin2->numJoints(), 7);
194  EXPECT_EQ(inv_kin2->getBaseLinkName(), base_link_name);
195  EXPECT_EQ(inv_kin2->getWorkingFrame(), base_link_name);
196  EXPECT_EQ(inv_kin2->getTipLinkNames().size(), 1);
197  EXPECT_EQ(inv_kin2->getTipLinkNames()[0], tip_link_name);
198  EXPECT_EQ(inv_kin2->getJointNames(), joint_names);
199 
200  runInvKinTest(*inv_kin2, *fwd_kin, pose, tip_link_name, seed);
201 
202  KinematicGroup kin_group2(manip_name, joint_names, std::move(inv_kin2), *scene_graph, scene_state);
203  EXPECT_EQ(kin_group2.getBaseLinkName(), scene_graph->getRoot());
204  runInvKinTest(kin_group2, pose, base_link_name, tip_link_name, seed);
205  runKinGroupJacobianABBOnPositionerTest(kin_group2);
206  runActiveLinkNamesABBOnPositionerTest(kin_group2);
207  runKinJointLimitsTest(kin_group2.getLimits(), target_limits);
208  runKinSetJointLimitsTest(kin_group2);
209 }
210 
211 int main(int argc, char** argv)
212 {
213  testing::InitGoogleTest(&argc, argv);
214 
215  return RUN_ALL_TESTS();
216 }
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
getFullInvKinematics
InverseKinematics::UPtr getFullInvKinematics(const tesseract_scene_graph::SceneGraph &scene_graph)
Definition: rop_kinematics_unit.cpp:51
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;})
kdl_state_solver.h
kdl_fwd_kin_chain.h
Tesseract KDL forward kinematics chain implementation.
tesseract_kinematics::DEFAULT_ROP_INV_KIN_SOLVER_NAME
static const std::string DEFAULT_ROP_INV_KIN_SOLVER_NAME
Definition: rop_inv_kin.h:40
opw_kinematics::Parameters::a2
T a2
kinematics_test_utils.h
tesseract_common::KinematicLimits
getPositionerFwdKinematics
ForwardKinematics::UPtr getPositionerFwdKinematics(const tesseract_scene_graph::SceneGraph &scene_graph)
Definition: rop_kinematics_unit.cpp:46
opw_kinematics::Parameters::c3
T c3
tesseract_kinematics::JointGroup::getBaseLinkName
std::string getBaseLinkName() const
Get the robot base link name.
Definition: joint_group.cpp:301
tesseract_kinematics::test_suite::runActiveLinkNamesABBOnPositionerTest
void runActiveLinkNamesABBOnPositionerTest(const tesseract_kinematics::KinematicGroup &kin_group)
Definition: kinematics_test_utils.h:830
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#define TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
tesseract_scene_graph::SceneGraph
tesseract_kinematics::InverseKinematics::UPtr
std::unique_ptr< InverseKinematics > UPtr
Definition: inverse_kinematics.h:52
getOPWKinematicsParamABB
opw_kinematics::Parameters< double > getOPWKinematicsParamABB()
Definition: rop_kinematics_unit.cpp:20
tesseract_scene_graph::SceneState
getRobotFwdKinematics
ForwardKinematics::UPtr getRobotFwdKinematics(const tesseract_scene_graph::SceneGraph &scene_graph)
Definition: rop_kinematics_unit.cpp:36
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.
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
getFullFwdKinematics
ForwardKinematics::UPtr getFullFwdKinematics(const tesseract_scene_graph::SceneGraph &scene_graph)
Definition: rop_kinematics_unit.cpp:41
tesseract_scene_graph::KDLStateSolver
main
int main(int argc, char **argv)
Definition: rop_kinematics_unit.cpp:211
opw_kinematics::Parameters::b
T b
tesseract_kinematics::test_suite
Definition: kinematics_test_utils.h:53
tesseract_kinematics::test_suite::getSceneGraphABBOnPositioner
tesseract_scene_graph::SceneGraph::UPtr getSceneGraphABBOnPositioner(const tesseract_common::ResourceLocator &locator)
Definition: kinematics_test_utils.h:70
tesseract_kinematics::test_suite::runKinGroupJacobianABBOnPositionerTest
void runKinGroupJacobianABBOnPositionerTest(tesseract_kinematics::KinematicGroup &kin_group)
Definition: kinematics_test_utils.h:783
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
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 >
macros.h
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
rop_inv_kin.h
opw_kinematics::Parameters::c2
T c2
TEST
TEST(TesseractKinematicsUnit, RobotOnPositionerInverseKinematicUnit)
Definition: rop_kinematics_unit.cpp:102
opw_kinematics::Parameters::c4
T c4

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