moveit2
The MoveIt Motion Planning Framework for ROS 2.
test.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Willow Garage, Inc.
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 Willow Garage 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: Ioan Sucan */
36 
38 #include <urdf_parser/urdf_parser.h>
39 #include <fstream>
40 #include <gtest/gtest.h>
41 
43 
44 class LoadPlanningModelsPr2 : public testing::Test
45 {
46 protected:
47  void SetUp() override
48  {
50  };
51 
52  void TearDown() override
53  {
54  }
55 
56 protected:
57  moveit::core::RobotModelConstPtr robot_model_;
58 };
59 
61 {
62  ASSERT_EQ(robot_model_->getURDF()->getName(), "pr2");
63  ASSERT_EQ(robot_model_->getSRDF()->getName(), "pr2");
64 }
65 
67 {
68  // robot_model_->printModelInfo(std::cout);
69 
70  const std::vector<const moveit::core::JointModel*>& joints = robot_model_->getJointModels();
71  for (std::size_t i = 0; i < joints.size(); ++i)
72  {
73  ASSERT_EQ(joints[i]->getJointIndex(), i);
74  ASSERT_EQ(robot_model_->getJointModel(joints[i]->getName()), joints[i]);
75  }
76  const std::vector<const moveit::core::LinkModel*>& links = robot_model_->getLinkModels();
77  for (std::size_t i = 0; i < links.size(); ++i)
78  {
79  ASSERT_EQ(links[i]->getLinkIndex(), i);
80  }
81 
82  // This joint has effort and velocity limits defined in the URDF. Nothing else.
83  const std::string joint_name = "fl_caster_rotation_joint";
84  const auto& joint = robot_model_->getJointModel(joint_name);
85  const auto& bounds = joint->getVariableBounds(joint->getName());
86 
87  EXPECT_TRUE(bounds.velocity_bounded_);
88  EXPECT_EQ(bounds.max_velocity_, 10.0);
89 
90  EXPECT_FALSE(bounds.position_bounded_);
91  EXPECT_FALSE(bounds.acceleration_bounded_);
92  EXPECT_FALSE(bounds.jerk_bounded_);
93 }
94 
95 TEST(SiblingAssociateLinks, SimpleYRobot)
96 {
97  // base_link - a - b - c //
98  // \ //
99  // - d ~ e //
100  moveit::core::RobotModelBuilder builder("one_robot", "base_link");
101  builder.addChain("base_link->a", "continuous");
102  builder.addChain("a->b->c", "fixed");
103  builder.addChain("a->d", "fixed");
104  builder.addChain("d->e", "continuous");
105  builder.addVirtualJoint("odom", "base_link", "planar", "base_joint");
106  builder.addGroup({}, { "base_joint" }, "base_joint");
107  ASSERT_TRUE(builder.isValid());
108  moveit::core::RobotModelConstPtr robot_model = builder.build();
109 
110  const std::string a = "a", b = "b", c = "c", d = "d";
111  auto connected = { a, b, c, d }; // these are rigidly connected with each other
113 
114  for (const std::string& root : connected)
115  {
116  SCOPED_TRACE("root: " + root);
117  std::set<std::string> expected_set(connected);
118  expected_set.erase(root);
119  std::set<std::string> actual_set;
120  for (const auto& item : robot_model->getLinkModel(root)->getAssociatedFixedTransforms())
121  actual_set.insert(item.first->getName());
122 
123  std::ostringstream expected, actual;
124  std::copy(expected_set.begin(), expected_set.end(), std::ostream_iterator<std::string>(expected, " "));
125  std::copy(actual_set.begin(), actual_set.end(), std::ostream_iterator<std::string>(actual, " "));
126 
127  EXPECT_EQ(expected.str(), actual.str());
128  }
129 }
130 
131 TEST(FloatingJointTest, interpolation_test)
132 {
133  // Create a simple floating joint model with some dummy parameters (these are not used by the test)
134  moveit::core::FloatingJointModel fjm("joint", 0, 0);
135 
136  // We set some bounds where the joint position's translation component is bounded between -1 and 1 in all
137  // dimensions. This is necessary, otherwise we just get (0,0,0) translations.
139  bounds = fjm.getVariableBounds();
140  bounds[0].min_position_ = -1.0;
141  bounds[0].max_position_ = 1.0;
142  bounds[1].min_position_ = -1.0;
143  bounds[1].max_position_ = 1.0;
144  bounds[2].min_position_ = -1.0;
145  bounds[2].max_position_ = 1.0;
146 
147  double jv1[7];
148  double jv2[7];
149  double intp[7];
150  random_numbers::RandomNumberGenerator rng;
151 
152  for (size_t i = 0; i < 1000; ++i)
153  {
154  // Randomize the joint settings.
155  fjm.getVariableRandomPositions(rng, jv1, bounds);
156  fjm.getVariableRandomPositions(rng, jv2, bounds);
157 
158  // Pick a random interpolation value
159  double t = rng.uniformReal(0.0, 1.0);
160 
161  // Apply the interpolation
162  fjm.interpolate(jv1, jv2, t, intp);
163 
164  // Get the distances between the two joint configurations
165  double d1 = fjm.distance(jv1, intp);
166  double d2 = fjm.distance(jv2, intp);
167  double t_total = fjm.distance(jv1, jv2);
168 
169  // Check that the resulting distances match with the interpolation value
170  EXPECT_NEAR(d1, t_total * t, 1e-6);
171  EXPECT_NEAR(d2, t_total * (1.0 - t), 1e-6);
172  }
173 }
174 
175 int main(int argc, char** argv)
176 {
177  testing::InitGoogleTest(&argc, argv);
178  return RUN_ALL_TESTS();
179 }
moveit::core::RobotModelPtr robot_model_
moveit::core::RobotModelConstPtr robot_model_
Definition: test.cpp:57
void TearDown() override
Definition: test.cpp:52
void SetUp() override
Definition: test.cpp:47
void interpolate(const double *from, const double *to, const double t, double *state) const override
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state....
double distance(const double *values1, const double *values2) const override
Compute the distance between two joint states of the same model (represented by the variable values)
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values, const Bounds &other_bounds) const override
Provide random values for the joint variables (within specified bounds). Enough memory is assumed to ...
std::vector< VariableBounds > Bounds
The datatype for the joint bounds.
Definition: joint_model.h:131
const VariableBounds & getVariableBounds(const std::string &variable) const
Get the bounds for a variable. Throw an exception if the variable was not found.
Easily build different robot models for testing. Essentially a programmer-friendly light wrapper arou...
void addChain(const std::string &section, const std::string &type, const std::vector< geometry_msgs::msg::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...
bool isValid()
Returns true if the building process so far has been valid.
void addGroup(const std::vector< std::string > &links, const std::vector< std::string > &joints, const std::string &name)
Adds a new group using a list of links and a list of joints.
moveit::core::RobotModelPtr build()
Builds and returns the robot model added to the builder.
void addVirtualJoint(const std::string &parent_frame, const std::string &child_link, const std::string &type, const std::string &name="")
Adds a virtual joint to the SRDF.
std::map< const LinkModel *, Eigen::Isometry3d, std::less< const LinkModel * >, Eigen::aligned_allocator< std::pair< const LinkModel *const, Eigen::Isometry3d > > > LinkTransformMap
Map from link model instances to Eigen transforms.
Definition: link_model.h:68
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
a
Definition: plan.py:54
d
Definition: setup.py:4
int main(int argc, char **argv)
Definition: test.cpp:175
TEST_F(LoadPlanningModelsPr2, InitOK)
Definition: test.cpp:60
TEST(SiblingAssociateLinks, SimpleYRobot)
Definition: test.cpp:95