moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
test_collision_distance_field.cpp
Go to the documentation of this file.
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2012, 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
43
44#include <geometric_shapes/shape_operations.h>
45#include <urdf_parser/urdf_parser.h>
46
47#include <fstream>
48#include <gtest/gtest.h>
49#include <sstream>
50#include <algorithm>
51#include <ctype.h>
52
54
55class DistanceFieldCollisionDetectionTester : public testing::Test
56{
57protected:
58 void SetUp() override
59 {
61
62 acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(robot_model_->getLinkModelNames(), true);
63
64 std::map<std::string, std::vector<collision_detection::CollisionSphere>> link_body_decompositions;
65 cenv_ = std::make_shared<DefaultCEnvType>(robot_model_, link_body_decompositions);
66 }
67
68 void TearDown() override
69 {
70 }
71
72protected:
73 moveit::core::RobotModelPtr robot_model_;
74
75 moveit::core::TransformsPtr ftf_;
76 moveit::core::TransformsConstPtr ftf_const_;
77
78 collision_detection::CollisionEnvPtr cenv_;
79
80 collision_detection::AllowedCollisionMatrixPtr acm_;
81};
82
84{
85 moveit::core::RobotState robot_state(robot_model_);
86 robot_state.setToDefaultValues();
87 robot_state.update();
88
91 req.group_name = "whole_body";
92 cenv_->checkSelfCollision(req, res, robot_state, *acm_);
93 ASSERT_FALSE(res.collision);
94}
95
97{
98 moveit::core::RobotState robot_state(robot_model_);
99 robot_state.setToDefaultValues();
100 robot_state.update();
101
105
106 req.group_name = "right_arm";
107 cenv_->checkSelfCollision(req, res1, robot_state, *acm_);
108 std::map<std::string, double> torso_val;
109 torso_val["torso_lift_joint"] = .15;
110 robot_state.setVariablePositions(torso_val);
111 robot_state.update();
112 cenv_->checkSelfCollision(req, res1, robot_state, *acm_);
113 cenv_->checkSelfCollision(req, res1, robot_state, *acm_);
114}
115
117{
122 // req.contacts = true;
123 // req.max_contacts = 100;
124 req.group_name = "whole_body";
125
126 moveit::core::RobotState robot_state(robot_model_);
127 robot_state.setToDefaultValues();
128
129 Eigen::Isometry3d offset = Eigen::Isometry3d::Identity();
130 offset.translation().x() = .01;
131
132 robot_state.updateStateWithLinkAt("base_link", Eigen::Isometry3d::Identity());
133 robot_state.updateStateWithLinkAt("base_bellow_link", offset);
134
135 acm_->setEntry("base_link", "base_bellow_link", false);
136 cenv_->checkSelfCollision(req, res1, robot_state, *acm_);
137 ASSERT_TRUE(res1.collision);
138
139 acm_->setEntry("base_link", "base_bellow_link", true);
140 cenv_->checkSelfCollision(req, res2, robot_state, *acm_);
141 ASSERT_FALSE(res2.collision);
142
143 robot_state.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Isometry3d::Identity());
144 robot_state.updateStateWithLinkAt("l_gripper_palm_link", offset);
145
146 acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
147 cenv_->checkSelfCollision(req, res3, robot_state, *acm_);
148 ASSERT_TRUE(res3.collision);
149}
150
152{
154 req.contacts = true;
155 req.max_contacts = 1;
156 req.group_name = "whole_body";
157
158 moveit::core::RobotState robot_state(robot_model_);
159 robot_state.setToDefaultValues();
160
161 Eigen::Isometry3d offset = Eigen::Isometry3d::Identity();
162 offset.translation().x() = .01;
163
164 robot_state.updateStateWithLinkAt("base_link", Eigen::Isometry3d::Identity());
165 robot_state.updateStateWithLinkAt("base_bellow_link", offset);
166
167 robot_state.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Isometry3d::Identity());
168 robot_state.updateStateWithLinkAt("l_gripper_palm_link", offset);
169
170 acm_->setEntry("base_link", "base_bellow_link", false);
171 acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
172
174 cenv_->checkSelfCollision(req, res, robot_state, *acm_);
175 ASSERT_TRUE(res.collision);
176 EXPECT_EQ(res.contacts.size(), 1u);
177 EXPECT_EQ(res.contacts.begin()->second.size(), 1u);
178
179 res.clear();
180 req.max_contacts = 2;
181 req.max_contacts_per_pair = 1;
182 // req.verbose = true;
183 cenv_->checkSelfCollision(req, res, robot_state, *acm_);
184 ASSERT_TRUE(res.collision);
185 EXPECT_EQ(res.contact_count, 2u);
186 EXPECT_EQ(res.contacts.begin()->second.size(), 1u);
187
188 res.contacts.clear();
189 res.contact_count = 0;
190
191 req.max_contacts = 10;
192 req.max_contacts_per_pair = 2;
193 acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(robot_model_->getLinkModelNames(), false);
194 cenv_->checkSelfCollision(req, res, robot_state, *acm_);
195 ASSERT_TRUE(res.collision);
196 EXPECT_LE(res.contacts.size(), 10u);
197 EXPECT_LE(res.contact_count, 10u);
198}
199
201{
203 req.contacts = true;
204 req.max_contacts = 1;
205 req.group_name = "whole_body";
206
207 moveit::core::RobotState robot_state(robot_model_);
208 robot_state.setToDefaultValues();
209
210 Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity();
211 Eigen::Isometry3d pos2 = Eigen::Isometry3d::Identity();
212
213 pos1.translation().x() = 5.0;
214 pos2.translation().x() = 5.01;
215
216 robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1);
217 robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2);
218
219 acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
220
222 cenv_->checkSelfCollision(req, res, robot_state, *acm_);
223 ASSERT_TRUE(res.collision);
224 ASSERT_EQ(res.contacts.size(), 1u);
225 ASSERT_EQ(res.contacts.begin()->second.size(), 1u);
226
227 for (collision_detection::CollisionResult::ContactMap::const_iterator it = res.contacts.begin();
228 it != res.contacts.end(); ++it)
229 {
230 EXPECT_NEAR(it->second[0].pos.x(), 5.0, .33);
231 }
232
233 pos1 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity());
234 pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond(0.965, 0.0, 0.258, 0.0));
235
236 robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1);
237 robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2);
238
240 cenv_->checkSelfCollision(req, res2, robot_state, *acm_);
241 ASSERT_TRUE(res2.collision);
242 ASSERT_EQ(res2.contacts.size(), 1u);
243 ASSERT_EQ(res2.contacts.begin()->second.size(), 1u);
244
245 for (collision_detection::CollisionResult::ContactMap::const_iterator it = res2.contacts.begin();
246 it != res2.contacts.end(); ++it)
247 {
248 EXPECT_NEAR(it->second[0].pos.x(), 3.0, 0.33);
249 }
250
251 pos1 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity());
252 pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond(M_PI / 4.0, 0.0, M_PI / 4.0, 0.0));
253
254 robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1);
255 robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2);
256
258 cenv_->checkSelfCollision(req, res2, robot_state, *acm_);
259 ASSERT_FALSE(res3.collision);
260}
261
263{
266
267 req.group_name = "right_arm";
268
269 acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(robot_model_->getLinkModelNames(), true);
270
271 moveit::core::RobotState robot_state(robot_model_);
272 robot_state.setToDefaultValues();
273 robot_state.update();
274
275 Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity();
276 pos1.translation().x() = 1.0;
277
278 robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1);
279 cenv_->checkSelfCollision(req, res, robot_state, *acm_);
280 ASSERT_FALSE(res.collision);
281
282 shapes::Shape* shape = new shapes::Box(.25, .25, .25);
283 cenv_->getWorld()->addToObject("box", shapes::ShapeConstPtr(shape), pos1);
284
286 cenv_->checkRobotCollision(req, res, robot_state, *acm_);
287 ASSERT_TRUE(res.collision);
288
289 // deletes shape
290 cenv_->getWorld()->removeObject("box");
291
292 const auto identity = Eigen::Isometry3d::Identity();
293 std::vector<shapes::ShapeConstPtr> shapes;
294 EigenSTL::vector_Isometry3d poses;
295 shapes.push_back(std::make_shared<const shapes::Box>(.25, .25, .25));
296 poses.push_back(identity);
297 std::set<std::string> touch_links;
298 trajectory_msgs::msg::JointTrajectory empty_state;
299
300 robot_state.attachBody(std::make_unique<moveit::core::AttachedBody>(
301 robot_state.getLinkModel("r_gripper_palm_link"), "box", identity, shapes, poses, touch_links, empty_state));
302
304 cenv_->checkSelfCollision(req, res, robot_state, *acm_);
305 ASSERT_TRUE(res.collision);
306
307 // deletes shape
308 robot_state.clearAttachedBody("box");
309
310 touch_links.insert("r_gripper_palm_link");
311 shapes[0] = std::make_shared<shapes::Box>(.1, .1, .1);
312
313 robot_state.attachBody(std::make_unique<moveit::core::AttachedBody>(
314 robot_state.getLinkModel("r_gripper_palm_link"), "box", identity, shapes, poses, touch_links, empty_state));
315
317 cenv_->checkSelfCollision(req, res, robot_state, *acm_);
318 // ASSERT_FALSE(res.collision);
319
320 pos1.translation().x() = 1.01;
321 shapes::Shape* coll = new shapes::Box(.1, .1, .1);
322 cenv_->getWorld()->addToObject("coll", shapes::ShapeConstPtr(coll), pos1);
324 cenv_->checkRobotCollision(req, res, robot_state, *acm_);
325 ASSERT_TRUE(res.collision);
326
327 acm_->setEntry("coll", "r_gripper_palm_link", true);
329 cenv_->checkRobotCollision(req, res, robot_state, *acm_);
330 ASSERT_TRUE(res.collision);
331}
332
333int main(int argc, char** argv)
334{
335 testing::InitGoogleTest(&argc, argv);
336 return RUN_ALL_TESTS();
337}
collision_detection::AllowedCollisionMatrixPtr acm_
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition robot_state.h:90
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state....
void attachBody(std::unique_ptr< AttachedBody > attached_body)
Add an attached body to this state.
void updateStateWithLinkAt(const std::string &link_name, const Eigen::Isometry3d &transform, bool backward=false)
Update the state after setting a particular link to the input global transform pose.
bool clearAttachedBody(const std::string &id)
Remove the attached body named id. Return false if the object was not found (and thus not removed)....
const LinkModel * getLinkModel(const std::string &link) const
Get the model of a particular link.
void update(bool force=false)
Update all transforms.
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &package_name, const std::string &urdf_relative_path, const std::string &srdf_relative_path)
Loads a robot model given a URDF and SRDF file in a package.
Definition world.h:49
Representation of a collision checking request.
std::string group_name
The group name to check collisions for (optional; if empty, assume the complete robot)....
bool contacts
If true, compute contacts. Otherwise only a binary collision yes/no is reported.
std::size_t max_contacts
Overall maximum number of contacts to compute.
std::size_t max_contacts_per_pair
Maximum number of contacts to compute per pair of bodies (multiple bodies may be in contact at differ...
Representation of a collision checking result.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW void clear()
Clear a previously stored result.
bool collision
True if collision was found, false otherwise.
std::size_t contact_count
Number of contacts returned.
int main(int argc, char **argv)
TEST_F(DistanceFieldCollisionDetectionTester, DefaultNotInCollision)
collision_detection::CollisionEnvDistanceField DefaultCEnvType