107 cenv_->checkSelfCollision(req, res1, robot_state, *acm_);
108 std::map<std::string, double> torso_val;
109 torso_val[
"torso_lift_joint"] = .15;
112 cenv_->checkSelfCollision(req, res1, robot_state, *acm_);
113 cenv_->checkSelfCollision(req, res1, robot_state, *acm_);
129 Eigen::Isometry3d offset = Eigen::Isometry3d::Identity();
130 offset.translation().x() = .01;
135 acm_->setEntry(
"base_link",
"base_bellow_link",
false);
136 cenv_->checkSelfCollision(req, res1, robot_state, *acm_);
139 acm_->setEntry(
"base_link",
"base_bellow_link",
true);
140 cenv_->checkSelfCollision(req, res2, robot_state, *acm_);
146 acm_->setEntry(
"r_gripper_palm_link",
"l_gripper_palm_link",
false);
147 cenv_->checkSelfCollision(req, res3, robot_state, *acm_);
161 Eigen::Isometry3d offset = Eigen::Isometry3d::Identity();
162 offset.translation().x() = .01;
170 acm_->setEntry(
"base_link",
"base_bellow_link",
false);
171 acm_->setEntry(
"r_gripper_palm_link",
"l_gripper_palm_link",
false);
174 cenv_->checkSelfCollision(req, res, robot_state, *acm_);
177 EXPECT_EQ(res.
contacts.begin()->second.size(), 1u);
183 cenv_->checkSelfCollision(req, res, robot_state, *acm_);
186 EXPECT_EQ(res.
contacts.begin()->second.size(), 1u);
193 acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(robot_model_->getLinkModelNames(),
false);
194 cenv_->checkSelfCollision(req, res, robot_state, *acm_);
196 EXPECT_LE(res.
contacts.size(), 10u);
210 Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity();
211 Eigen::Isometry3d pos2 = Eigen::Isometry3d::Identity();
213 pos1.translation().x() = 5.0;
214 pos2.translation().x() = 5.01;
219 acm_->setEntry(
"r_gripper_palm_link",
"l_gripper_palm_link",
false);
222 cenv_->checkSelfCollision(req, res, robot_state, *acm_);
225 ASSERT_EQ(res.
contacts.begin()->second.size(), 1u);
227 for (collision_detection::CollisionResult::ContactMap::const_iterator it = res.
contacts.begin();
230 EXPECT_NEAR(it->second[0].pos.x(), 5.0, .33);
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));
240 cenv_->checkSelfCollision(req, res2, robot_state, *acm_);
242 ASSERT_EQ(res2.
contacts.size(), 1u);
243 ASSERT_EQ(res2.
contacts.begin()->second.size(), 1u);
245 for (collision_detection::CollisionResult::ContactMap::const_iterator it = res2.
contacts.begin();
248 EXPECT_NEAR(it->second[0].pos.x(), 3.0, 0.33);
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));
258 cenv_->checkSelfCollision(req, res2, robot_state, *acm_);
269 acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(robot_model_->getLinkModelNames(),
true);
275 Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity();
276 pos1.translation().x() = 1.0;
279 cenv_->checkSelfCollision(req, res, robot_state, *acm_);
282 shapes::Shape* shape =
new shapes::Box(.25, .25, .25);
283 cenv_->getWorld()->addToObject(
"box", shapes::ShapeConstPtr(shape), pos1);
286 cenv_->checkRobotCollision(req, res, robot_state, *acm_);
290 cenv_->getWorld()->removeObject(
"box");
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;
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));
304 cenv_->checkSelfCollision(req, res, robot_state, *acm_);
310 touch_links.insert(
"r_gripper_palm_link");
311 shapes[0] = std::make_shared<shapes::Box>(.1, .1, .1);
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));
317 cenv_->checkSelfCollision(req, res, robot_state, *acm_);
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_);
327 acm_->setEntry(
"coll",
"r_gripper_palm_link",
true);
329 cenv_->checkRobotCollision(req, res, robot_state, *acm_);