37 #include <gtest/gtest.h>
51 #include <urdf_parser/urdf_parser.h>
60 double joint2 = -0.785;
61 double joint4 = -2.356;
62 double joint6 = 1.571;
63 double joint7 = 0.785;
79 acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(*
robot_model_->getSRDF());
97 collision_detection::CollisionEnvPtr
cenv_;
99 collision_detection::AllowedCollisionMatrixPtr
acm_;
110 Eigen::Isometry3d static_box_pose;
111 static_box_pose.setIdentity();
113 std::vector<shapes::ShapeConstPtr> obj1_shapes;
115 std::vector<cb::CollisionObjectType> obj1_types;
116 obj1_shapes.push_back(static_box);
117 obj1_poses.push_back(static_box_pose);
118 obj1_types.push_back(cb::CollisionObjectType::USE_SHAPE_TYPE);
128 Eigen::Isometry3d moving_box_pose;
130 moving_box_pose.setIdentity();
133 std::vector<shapes::ShapeConstPtr> obj2_shapes;
135 std::vector<cb::CollisionObjectType> obj2_types;
136 obj2_shapes.push_back(moving_box);
137 obj2_poses.push_back(moving_box_pose);
138 obj2_types.push_back(cb::CollisionObjectType::USE_SHAPE_TYPE);
151 Eigen::Isometry3d static_box_pose;
152 static_box_pose.setIdentity();
154 std::vector<shapes::ShapeConstPtr> obj1_shapes;
156 std::vector<cb::CollisionObjectType> obj1_types;
157 obj1_shapes.push_back(static_box);
158 obj1_poses.push_back(static_box_pose);
159 obj1_types.push_back(cb::CollisionObjectType::USE_SHAPE_TYPE);
168 std::vector<shapes::ShapeConstPtr> obj2_shapes;
170 std::vector<cb::CollisionObjectType> obj2_types;
172 obj1_poses.push_back(static_box_pose);
173 obj1_types.push_back(cb::CollisionObjectType::USE_SHAPE_TYPE);
175 Eigen::Isometry3d s_pose;
176 s_pose.setIdentity();
178 std::string kinect =
"package://moveit_resources_panda_description/meshes/collision/hand.stl";
180 obj2_shapes.push_back(
s);
181 obj2_types.push_back(cb::CollisionObjectType::CONVEX_HULL);
182 obj2_poses.push_back(s_pose);
190 std::vector<collision_detection::Contact>& result_vector, Eigen::Isometry3d& start_pos,
191 Eigen::Isometry3d& end_pos)
207 checker.
contactTest(result, request,
nullptr,
false);
209 for (
const auto& contacts_all : result.
contacts)
211 for (
const auto& contact : contacts_all.second)
213 result_vector.push_back(contact);
229 double joint2 = 0.15;
230 double joint4 = -3.0;
231 double joint5 = -0.8;
232 double joint7 = -0.785;
239 cenv_->checkSelfCollision(req, res, state1, *acm_);
244 double joint_5_other = 0.8;
251 cenv_->checkSelfCollision(req, res, state2, *acm_);
255 ROS_INFO_STREAM(
"Continous to continous collisions are not supported yet, therefore fail here.");
275 double joint_2{ 0.05 };
276 double joint_4{ -1.6 };
281 cenv_->checkRobotCollision(req, res, state1, state2, *acm_);
289 Eigen::Isometry3d pos{ Eigen::Isometry3d::Identity() };
290 pos.translation().x() = 0.43;
291 pos.translation().y() = 0;
292 pos.translation().z() = 0.55;
293 cenv_->getWorld()->addToObject(
"box", shape_ptr, pos);
295 cenv_->checkRobotCollision(req, res, state1, *acm_);
299 cenv_->checkRobotCollision(req, res, state2, *acm_);
303 cenv_->checkRobotCollision(req, res, state1, state2, *acm_);
307 for (
auto& contact_pair : res.
contacts)
324 TEST(ContinuousCollisionUnit, BulletCastBVHCollisionBoxBoxUnit)
327 std::vector<collision_detection::Contact> result_vector;
329 Eigen::Isometry3d start_pos, end_pos;
330 start_pos.setIdentity();
331 start_pos.translation().x() = -2;
332 end_pos.setIdentity();
333 end_pos.translation().x() = 2;
337 runTest(checker, result, result_vector, start_pos, end_pos);
341 EXPECT_NEAR(result_vector[0].percent_interpolation, 0.6, 0.001);
344 TEST(ContinuousCollisionUnit, BulletCastMeshVsBox)
349 Eigen::Isometry3d start_pos, end_pos;
350 start_pos.setIdentity();
351 start_pos.translation().x() = -1.9;
352 end_pos.setIdentity();
353 end_pos.translation().x() = 1.9;
356 std::vector<collision_detection::Contact> result_vector;
358 runTest(checker, result, result_vector, start_pos, end_pos);
363 int main(
int argc,
char** argv)
365 testing::InitGoogleTest(&argc, argv);
366 return RUN_ALL_TESTS();