test_cartesian_interpolator.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, PickNik LLC.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the PickNik nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Michael Lautman */
36 
42 
43 using namespace moveit::core;
44 
45 class SimpleRobot : public testing::Test
46 {
47 protected:
48  void SetUp() override
49  {
50  RobotModelBuilder builder("simple", "a");
51  builder.addChain("a->b", "continuous");
52  builder.addChain("b->c", "prismatic");
53  builder.addGroupChain("a", "c", "group");
54  robot_model_ = builder.build();
55  }
56 
57  void TearDown() override
58  {
59  }
60 
61 protected:
62  RobotModelConstPtr robot_model_;
63 
64  static std::size_t generateTestTraj(std::vector<std::shared_ptr<RobotState>>& traj,
65  const RobotModelConstPtr& robot_model_);
66 };
67 
68 std::size_t SimpleRobot::generateTestTraj(std::vector<std::shared_ptr<RobotState>>& traj,
69  const RobotModelConstPtr& robot_model_)
70 {
71  traj.clear();
72 
73  std::shared_ptr<RobotState> robot_state(new RobotState(robot_model_));
74  robot_state->setToDefaultValues();
75  double ja, jc;
76 
77  // 3 waypoints with default joints
78  for (std::size_t traj_ix = 0; traj_ix < 3; ++traj_ix)
79  {
80  traj.push_back(std::make_shared<RobotState>(*robot_state));
81  }
82 
83  ja = robot_state->getVariablePosition("a-b-joint"); // revolute joint
84  jc = robot_state->getVariablePosition("b-c-joint"); // prismatic joint
85 
86  // 4th waypoint with a small jump of 0.01 in revolute joint and prismatic joint. This should not be considered a jump
87  ja = ja - 0.01;
88  robot_state->setVariablePosition("a-b-joint", ja);
89  jc = jc - 0.01;
90  robot_state->setVariablePosition("b-c-joint", jc);
91  traj.push_back(std::make_shared<RobotState>(*robot_state));
92 
93  // 5th waypoint with a large jump of 1.01 in first revolute joint
94  ja = ja + 1.01;
95  robot_state->setVariablePosition("a-b-joint", ja);
96  traj.push_back(std::make_shared<RobotState>(*robot_state));
97 
98  // 6th waypoint with a large jump of 1.01 in first prismatic joint
99  jc = jc + 1.01;
100  robot_state->setVariablePosition("b-c-joint", jc);
101  traj.push_back(std::make_shared<RobotState>(*robot_state));
102 
103  // 7th waypoint with no jump
104  traj.push_back(std::make_shared<RobotState>(*robot_state));
105 
106  return traj.size();
107 }
108 
109 TEST_F(SimpleRobot, testGenerateTrajectory)
110 {
111  std::vector<std::shared_ptr<RobotState>> traj;
112 
113  // The full trajectory should be of length 7
114  const std::size_t expected_full_traj_len = 7;
115 
116  // Generate a test trajectory
117  std::size_t full_traj_len = generateTestTraj(traj, robot_model_);
118 
119  // Test the generateTestTraj still generates a trajectory of length 7
120  EXPECT_EQ(full_traj_len, expected_full_traj_len); // full traj should be 7 waypoints long
121 }
122 
123 TEST_F(SimpleRobot, checkAbsoluteJointSpaceJump)
124 {
125  const JointModelGroup* joint_model_group = robot_model_->getJointModelGroup("group");
126  std::vector<std::shared_ptr<RobotState>> traj;
127 
128  // A revolute joint jumps 1.01 at the 5th waypoint and a prismatic joint jumps 1.01 at the 6th waypoint
129  const std::size_t expected_revolute_jump_traj_len = 4;
130  const std::size_t expected_prismatic_jump_traj_len = 5;
131 
132  // Pre-compute expected results for tests
133  std::size_t full_traj_len = generateTestTraj(traj, robot_model_);
134  const double expected_revolute_jump_fraction = (double)expected_revolute_jump_traj_len / (double)full_traj_len;
135  const double expected_prismatic_jump_fraction = (double)expected_prismatic_jump_traj_len / (double)full_traj_len;
136 
137  // Container for results
138  double fraction;
139 
140  // Direct call of absolute version
141  fraction = CartesianInterpolator::checkAbsoluteJointSpaceJump(joint_model_group, traj, 1.0, 1.0);
142  EXPECT_EQ(expected_revolute_jump_traj_len, traj.size()); // traj should be cut
143  EXPECT_NEAR(expected_revolute_jump_fraction, fraction, 0.01);
144 
145  // Indirect call using checkJointSpaceJumps
146  generateTestTraj(traj, robot_model_);
147  fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold(1.0, 1.0));
148  EXPECT_EQ(expected_revolute_jump_traj_len, traj.size()); // traj should be cut before the revolute jump
149  EXPECT_NEAR(expected_revolute_jump_fraction, fraction, 0.01);
150 
151  // Test revolute joints
152  generateTestTraj(traj, robot_model_);
153  fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold(1.0, 0.0));
154  EXPECT_EQ(expected_revolute_jump_traj_len, traj.size()); // traj should be cut before the revolute jump
155  EXPECT_NEAR(expected_revolute_jump_fraction, fraction, 0.01);
156 
157  // Test prismatic joints
158  generateTestTraj(traj, robot_model_);
159  fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold(0.0, 1.0));
160  EXPECT_EQ(expected_prismatic_jump_traj_len, traj.size()); // traj should be cut before the prismatic jump
161  EXPECT_NEAR(expected_prismatic_jump_fraction, fraction, 0.01);
162 
163  // Ignore all absolute jumps
164  generateTestTraj(traj, robot_model_);
165  fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold(0.0, 0.0));
166  EXPECT_EQ(full_traj_len, traj.size()); // traj should not be cut
167  EXPECT_NEAR(1.0, fraction, 0.01);
168 }
169 
170 TEST_F(SimpleRobot, checkRelativeJointSpaceJump)
171 {
172  const JointModelGroup* joint_model_group = robot_model_->getJointModelGroup("group");
173  std::vector<std::shared_ptr<RobotState>> traj;
174 
175  // The first large jump of 1.01 occurs at the 5th waypoint so the test should trim the trajectory to length 4
176  const std::size_t expected_relative_jump_traj_len = 4;
177 
178  // Pre-compute expected results for tests
179  std::size_t full_traj_len = generateTestTraj(traj, robot_model_);
180  const double expected_relative_jump_fraction = (double)expected_relative_jump_traj_len / (double)full_traj_len;
181 
182  // Container for results
183  double fraction;
184 
185  // Direct call of relative version: 1.01 > 2.97 * (0.01 * 2 + 1.01 * 2)/6.
186  fraction = CartesianInterpolator::checkRelativeJointSpaceJump(joint_model_group, traj, 2.97);
187  EXPECT_EQ(expected_relative_jump_traj_len, traj.size()); // traj should be cut before the first jump of 1.01
188  EXPECT_NEAR(expected_relative_jump_fraction, fraction, 0.01);
189 
190  // Indirect call of relative version using checkJointSpaceJumps
191  generateTestTraj(traj, robot_model_);
192  fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold(2.97));
193  EXPECT_EQ(expected_relative_jump_traj_len, traj.size()); // traj should be cut before the first jump of 1.01
194  EXPECT_NEAR(expected_relative_jump_fraction, fraction, 0.01);
195 
196  // Trajectory should not be cut: 1.01 < 2.98 * (0.01 * 2 + 1.01 * 2)/6.
197  generateTestTraj(traj, robot_model_);
198  fraction = CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj, JumpThreshold(2.98));
199  EXPECT_EQ(full_traj_len, traj.size()); // traj should not be cut
200  EXPECT_NEAR(1.0, fraction, 0.01);
201 }
202 
203 // Gracefully handle gtest 1.8 (Melodic)
204 #ifndef INSTANTIATE_TEST_SUITE_P
205 #define _STATIC
206 #define _OLD_GTEST
207 #else
208 #define _STATIC static
209 #endif
210 
211 class PandaRobot : public testing::Test
212 {
213 protected:
214  _STATIC void SetUpTestSuite() // setup resources shared between tests
215  {
216  robot_model_ = loadTestingRobotModel("panda");
217  jmg_ = robot_model_->getJointModelGroup("panda_arm");
218  link_ = robot_model_->getLinkModel("panda_link8");
219  ASSERT_TRUE(link_);
220  loadIKPluginForGroup(jmg_, "panda_link0", link_->getName());
221  }
222 
224  {
225  robot_model_.reset();
226  }
227 
228  void SetUp() override
229  {
230 #ifdef _OLD_GTEST
231  SetUpTestSuite();
232 #endif
233  start_state_ = std::make_shared<RobotState>(robot_model_);
234  ASSERT_TRUE(start_state_->setToDefaultValues(jmg_, "ready"));
235  start_pose_ = start_state_->getGlobalLinkTransform(link_);
236  }
237 
238 #ifdef _OLD_GTEST
239  void TearDown() override
240  {
241  TearDownTestSuite();
242  }
243 #endif
244 
245  double computeCartesianPath(std::vector<std::shared_ptr<RobotState>>& result, const Eigen::Vector3d& translation,
246  bool global)
247  {
248  return CartesianInterpolator::computeCartesianPath(start_state_.get(), jmg_, result, link_, translation, global,
251  }
252  double computeCartesianPath(std::vector<std::shared_ptr<RobotState>>& result, const Eigen::Isometry3d& target,
253  bool global, const Eigen::Isometry3d& offset = Eigen::Isometry3d::Identity())
254  {
255  return CartesianInterpolator::computeCartesianPath(start_state_.get(), jmg_, result, link_, target, global,
258  }
259 
260 protected:
261  _STATIC RobotModelPtr robot_model_;
264 
265  double prec_ = 1e-8;
266  RobotStatePtr start_state_;
267  Eigen::Isometry3d start_pose_;
268  std::vector<std::shared_ptr<RobotState>> result_;
269 };
270 #ifndef _OLD_GTEST
271 RobotModelPtr PandaRobot::robot_model_;
273 const LinkModel* PandaRobot::link_ = nullptr;
274 #endif
275 
276 TEST_F(PandaRobot, testVectorGlobal)
277 {
278  Eigen::Vector3d translation(0.2, 0, 0); // move by 0.2 along world's x axis
279  ASSERT_DOUBLE_EQ(computeCartesianPath(result_, translation, true), 0.2); // moved full distance of 0.2
280  // first pose of trajectory should be identical to start_pose
281  EXPECT_EIGEN_EQ(result_.front()->getGlobalLinkTransform(link_), start_pose_);
282  // last pose of trajectory should have same orientation, and offset of 0.2 along world's x-axis
283  EXPECT_EIGEN_NEAR(result_.back()->getGlobalLinkTransform(link_).linear(), start_pose_.linear(), prec_);
284  EXPECT_EIGEN_NEAR(result_.back()->getGlobalLinkTransform(link_).translation(),
285  start_pose_.translation() + translation, prec_);
286 }
287 TEST_F(PandaRobot, testVectorLocal)
288 {
289  Eigen::Vector3d translation(0.2, 0, 0); // move by 0.2 along link's x axis
290  ASSERT_DOUBLE_EQ(computeCartesianPath(result_, translation, false), 0.2); // moved full distance of 0.2
291  // first pose of trajectory should be identical to start_pose
292  EXPECT_EIGEN_EQ(result_.front()->getGlobalLinkTransform(link_), start_pose_);
293  // last pose of trajectory should have same orientation, and offset of 0.2 along link's x-axis
294  EXPECT_EIGEN_NEAR(result_.back()->getGlobalLinkTransform(link_).linear(), start_pose_.linear(), prec_);
295  EXPECT_EIGEN_NEAR(result_.back()->getGlobalLinkTransform(link_).translation(), start_pose_ * translation, prec_);
296 }
297 
298 TEST_F(PandaRobot, testTranslationGlobal)
299 {
300  Eigen::Isometry3d goal = start_pose_;
301  goal.translation().x() += 0.2; // move by 0.2 along world's x-axis
302 
303  ASSERT_DOUBLE_EQ(computeCartesianPath(result_, goal, true), 1.0); // 100% of distance generated
304  // first pose of trajectory should be identical to start_pose
305  EXPECT_EIGEN_EQ(result_.front()->getGlobalLinkTransform(link_), start_pose_);
306  // last pose of trajectory should have same orientation, but offset of 0.2 along world's x-axis
307  EXPECT_EIGEN_NEAR(result_.back()->getGlobalLinkTransform(link_).linear(), start_pose_.linear(), prec_);
308  EXPECT_EIGEN_NEAR(result_.back()->getGlobalLinkTransform(link_).translation(), goal.translation(), prec_);
309 }
310 TEST_F(PandaRobot, testTranslationLocal)
311 {
312  Eigen::Isometry3d offset(Eigen::Translation3d(0.2, 0, 0)); // move along link's x-axis
313  ASSERT_DOUBLE_EQ(computeCartesianPath(result_, offset, false), 1.0); // 100% of distance generated
314  // first pose of trajectory should be identical to start_pose
315  EXPECT_EIGEN_EQ(result_.front()->getGlobalLinkTransform(link_), start_pose_);
316  // last pose of trajectory should have same orientation, but offset of 0.2 along link's x-axis
317  EXPECT_EIGEN_NEAR(result_.back()->getGlobalLinkTransform(link_).linear(), start_pose_.linear(), prec_);
318  EXPECT_EIGEN_NEAR(result_.back()->getGlobalLinkTransform(link_).translation(), start_pose_ * offset.translation(),
319  prec_);
320 }
321 
322 TEST_F(PandaRobot, testRotationLocal)
323 {
324  // 45° rotation about links's x-axis
325  Eigen::Isometry3d rot(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitX()));
326  Eigen::Isometry3d goal = start_pose_ * rot;
327 
328  ASSERT_DOUBLE_EQ(computeCartesianPath(result_, rot, false), 1.0); // 100% of distance generated
329  // first pose of trajectory should be identical to start_pose
330  EXPECT_EIGEN_EQ(result_.front()->getGlobalLinkTransform(link_), start_pose_);
331  // last pose of trajectory should have same position,
332  EXPECT_EIGEN_NEAR(result_.back()->getGlobalLinkTransform(link_).translation(), start_pose_.translation(), prec_);
333  EXPECT_EIGEN_NEAR(result_.back()->getGlobalLinkTransform(link_), goal, prec_);
334 }
335 TEST_F(PandaRobot, testRotationGlobal)
336 {
337  // 45° rotation about links's x-axis
338  Eigen::Isometry3d rot(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitX()));
339  Eigen::Isometry3d goal = start_pose_ * rot;
340 
341  ASSERT_DOUBLE_EQ(computeCartesianPath(result_, goal, true), 1.0); // 100% of distance generated
342  // first pose of trajectory should be identical to start_pose
343  EXPECT_EIGEN_NEAR(result_.front()->getGlobalLinkTransform(link_), start_pose_, prec_);
344  // last pose of trajectory should have same position,
345  EXPECT_EIGEN_NEAR(result_.back()->getGlobalLinkTransform(link_).translation(), start_pose_.translation(), prec_);
346  EXPECT_EIGEN_NEAR(result_.back()->getGlobalLinkTransform(link_), goal, prec_);
347 }
348 TEST_F(PandaRobot, testRotationOffset)
349 {
350  // define offset to virtual center frame
351  Eigen::Isometry3d offset = Eigen::Translation3d(0, 0, 0.2) * Eigen::AngleAxisd(-M_PI / 4, Eigen::Vector3d::UnitZ());
352  // 45° rotation about center's x-axis
353  Eigen::Isometry3d rot(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitX()));
354  Eigen::Isometry3d goal = start_pose_ * offset * rot;
355 
356  ASSERT_DOUBLE_EQ(computeCartesianPath(result_, goal, true, offset), 1.0); // 100% of distance generated
357  // first pose of trajectory should be identical to start_pose
358  EXPECT_EIGEN_NEAR(result_.front()->getGlobalLinkTransform(link_), start_pose_, prec_);
359 
360  // All waypoints of trajectory should have same position in virtual frame
361  for (const auto& waypoint : result_)
362  EXPECT_EIGEN_NEAR((waypoint->getGlobalLinkTransform(link_) * offset).translation(),
363  (start_pose_ * offset).translation(), prec_);
364  // goal should be reached by virtual frame
365  EXPECT_EIGEN_NEAR(result_.back()->getGlobalLinkTransform(link_) * offset, goal, prec_);
366 }
367 
368 int main(int argc, char** argv)
369 {
370  testing::InitGoogleTest(&argc, argv);
371  ros::init(argc, argv, "test_cartesian_interpolator");
372  return RUN_ALL_TESTS();
373 }
moveit::core::LinkModel
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition: link_model.h:71
moveit::core
Core components of MoveIt.
Definition: kinematics_base.h:83
PandaRobot
Definition: test_cartesian_interpolator.cpp:211
moveit::core::GroupStateValidityCallbackFn
boost::function< bool(RobotState *robot_state, const JointModelGroup *joint_group, const double *joint_group_variable_values)> GroupStateValidityCallbackFn
Signature for functions that can verify that if the group joint_group in robot_state is set to joint_...
Definition: robot_state.h:136
PandaRobot::SetUp
void SetUp() override
Definition: test_cartesian_interpolator.cpp:228
PandaRobot::link_
const _STATIC LinkModel * link_
Definition: test_cartesian_interpolator.cpp:263
moveit::core::RobotModelBuilder::addGroupChain
RobotModelBuilder & addGroupChain(const std::string &base_link, const std::string &tip_link, const std::string &name="")
Adds a new group using a chain of links. The group is the parent joint of each link in the chain.
Definition: robot_model_test_utils.cpp:436
SimpleRobot::SetUp
void SetUp() override
Definition: test_cartesian_interpolator.cpp:48
moveit::core::JumpThreshold
Struct for containing jump_threshold.
Definition: cartesian_interpolator.h:122
moveit::core::JointModelGroup
Definition: joint_model_group.h:134
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
moveit::core::CartesianInterpolator::computeCartesianPath
static double computeCartesianPath(RobotState *start_state, const JointModelGroup *group, std::vector< std::shared_ptr< RobotState >> &traj, const LinkModel *link, const Eigen::Vector3d &translation, bool global_reference_frame, const MaxEEFStep &max_step, const JumpThreshold &jump_threshold, const GroupStateValidityCallbackFn &validCallback=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Compute the sequence of joint values that correspond to a straight Cartesian path for a particular li...
moveit::core::CartesianInterpolator::checkRelativeJointSpaceJump
static double checkRelativeJointSpaceJump(const JointModelGroup *group, std::vector< std::shared_ptr< RobotState >> &traj, double jump_threshold_factor)
Tests for relative joint space jumps of the trajectory traj.
Definition: cartesian_interpolator.cpp:300
PandaRobot::TearDown
void TearDown() override
Definition: test_cartesian_interpolator.cpp:239
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
robot_model.h
PandaRobot::computeCartesianPath
double computeCartesianPath(std::vector< std::shared_ptr< RobotState >> &result, const Eigen::Vector3d &translation, bool global)
Definition: test_cartesian_interpolator.cpp:245
SimpleRobot::generateTestTraj
static std::size_t generateTestTraj(std::vector< std::shared_ptr< RobotState >> &traj, const RobotModelConstPtr &robot_model_)
Definition: test_cartesian_interpolator.cpp:68
PandaRobot::SetUpTestSuite
_STATIC void SetUpTestSuite()
Definition: test_cartesian_interpolator.cpp:214
PandaRobot::result_
std::vector< std::shared_ptr< RobotState > > result_
Definition: test_cartesian_interpolator.cpp:268
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
PandaRobot::computeCartesianPath
double computeCartesianPath(std::vector< std::shared_ptr< RobotState >> &result, const Eigen::Isometry3d &target, bool global, const Eigen::Isometry3d &offset=Eigen::Isometry3d::Identity())
Definition: test_cartesian_interpolator.cpp:252
_STATIC
#define _STATIC
Definition: test_cartesian_interpolator.cpp:205
PandaRobot::start_pose_
Eigen::Isometry3d start_pose_
Definition: test_cartesian_interpolator.cpp:267
cartesian_interpolator.h
moveit::core::MaxEEFStep
Struct for containing max_step for computeCartesianPath.
Definition: cartesian_interpolator.h:149
moveit::core::RobotModelBuilder::build
moveit::core::RobotModelPtr build()
Builds and returns the robot model added to the builder.
Definition: robot_model_test_utils.cpp:484
moveit::core::CartesianInterpolator::checkJointSpaceJump
static double checkJointSpaceJump(const JointModelGroup *group, std::vector< std::shared_ptr< RobotState >> &traj, const JumpThreshold &jump_threshold)
Tests joint space jumps of a trajectory.
Definition: cartesian_interpolator.cpp:282
SimpleRobot
Definition: test_cartesian_interpolator.cpp:45
PandaRobot::robot_model_
_STATIC RobotModelPtr robot_model_
Definition: test_cartesian_interpolator.cpp:261
main
int main(int argc, char **argv)
Definition: test_cartesian_interpolator.cpp:368
moveit::core::CartesianInterpolator::checkAbsoluteJointSpaceJump
static double checkAbsoluteJointSpaceJump(const JointModelGroup *group, std::vector< std::shared_ptr< RobotState >> &traj, double revolute_jump_threshold, double prismatic_jump_threshold)
Tests for absolute joint space jumps of the trajectory traj.
Definition: cartesian_interpolator.cpp:337
robot_model_test_utils.h
SimpleRobot::robot_model_
RobotModelConstPtr robot_model_
Definition: test_cartesian_interpolator.cpp:62
EXPECT_EIGEN_NEAR
#define EXPECT_EIGEN_NEAR(val1, val2, prec_)
Definition: eigen_test_utils.h:76
kinematics::KinematicsQueryOptions
A set of options for the kinematics solver.
Definition: kinematics_base.h:109
moveit::core::RobotModelBuilder
Easily build different robot models for testing. Essentially a programmer-friendly light wrapper arou...
Definition: robot_model_test_utils.h:158
M_PI
#define M_PI
moveit::core::RobotModelBuilder::addChain
RobotModelBuilder & addChain(const std::string &section, const std::string &type, const std::vector< geometry_msgs::Pose > &joint_origins={}, urdf::Vector3 joint_axis=urdf::Vector3(1.0, 0.0, 0.0))
Adds a chain of links and joints to the builder. The joint names are generated automatically as "<par...
Definition: robot_model_test_utils.cpp:204
EXPECT_EIGEN_EQ
#define EXPECT_EIGEN_EQ(val1, val2)
Definition: eigen_test_utils.h:72
SimpleRobot::TearDown
void TearDown() override
Definition: test_cartesian_interpolator.cpp:57
PandaRobot::start_state_
RobotStatePtr start_state_
Definition: test_cartesian_interpolator.cpp:266
PandaRobot::TearDownTestSuite
_STATIC void TearDownTestSuite()
Definition: test_cartesian_interpolator.cpp:223
moveit::core::loadIKPluginForGroup
void loadIKPluginForGroup(JointModelGroup *jmg, const std::string &base_link, const std::string &tip_link, std::string plugin="KDL", double timeout=0.1)
Load an IK solver plugin for the given joint model group.
Definition: robot_model_test_utils.cpp:165
eigen_test_utils.h
robot_state.h
EXPECT_EQ
#define EXPECT_EQ(a, b)
PandaRobot::jmg_
_STATIC JointModelGroup * jmg_
Definition: test_cartesian_interpolator.cpp:262
TEST_F
TEST_F(SimpleRobot, testGenerateTrajectory)
Definition: test_cartesian_interpolator.cpp:109
fcl::Vector3d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
moveit::core::loadTestingRobotModel
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
Definition: robot_model_test_utils.cpp:117


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat Jul 6 2024 02:26:32