44#include <urdf_parser/urdf_parser.h>
45#include <geometric_shapes/shape_operations.h>
47#include <gtest/gtest.h>
57 double joint2 = -0.785;
58 double joint4 = -2.356;
59 double joint6 = 1.571;
60 double joint7 = 0.785;
68template <
class CollisionAllocatorType>
72 std::shared_ptr<CollisionAllocatorType>
value_;
77 value_ = std::make_shared<CollisionAllocatorType>();
81 acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(*
robot_model_->getSRDF());
97 collision_detection::CollisionEnvPtr
cenv_;
99 collision_detection::AllowedCollisionMatrixPtr
acm_;
109 ASSERT_TRUE(this->robot_model_ok_);
117 this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_);
125 double joint2 = 0.15;
126 double joint4 = -3.0;
127 this->robot_state_->setJointPositions(
"panda_joint2", &joint2);
128 this->robot_state_->setJointPositions(
"panda_joint4", &joint4);
129 this->robot_state_->update();
133 this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_);
143 shapes::Shape* shape =
new shapes::Box(.1, .1, .1);
144 shapes::ShapeConstPtr shape_ptr(shape);
146 Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity();
147 pos1.translation().z() = 0.3;
148 this->cenv_->getWorld()->addToObject(
"box", pos1, shape_ptr, Eigen::Isometry3d::Identity());
150 this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_);
154 this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
158 pos1.translation().z() = 0.25;
159 this->cenv_->getWorld()->moveObject(
"box", pos1);
160 this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
164 pos1.translation().z() = 0.05;
165 this->cenv_->getWorld()->moveObject(
"box", pos1);
166 this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
170 pos1.translation().z() = 0.25;
171 this->cenv_->getWorld()->moveObject(
"box", pos1);
172 this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
176 this->cenv_->getWorld()->moveObject(
"box", pos1);
189 shapes::Shape* shape =
new shapes::Box(.4, .4, .4);
190 shapes::ShapeConstPtr shape_ptr(shape);
192 Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity();
193 pos1.translation().z() = 0.3;
194 this->cenv_->getWorld()->addToObject(
"box", pos1, shape_ptr, Eigen::Isometry3d::Identity());
196 this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
210 this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
215 shapes::Shape* shape =
new shapes::Box(0.1, 0.1, 0.1);
216 shapes::ShapeConstPtr shape_ptr(shape);
218 Eigen::Isometry3d pos{ Eigen::Isometry3d::Identity() };
219 pos.translation().x() = 0.43;
220 pos.translation().y() = 0;
221 pos.translation().z() = 0.55;
222 this->cenv_->getWorld()->addToObject(
"box", pos, shape_ptr, Eigen::Isometry3d::Identity());
224 this->cenv_->setLinkPadding(
"panda_hand", 0.08);
225 this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
229 this->cenv_->setLinkPadding(
"panda_hand", 0.0);
230 this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
240 this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_);
242 EXPECT_NEAR(res.
distance, 0.022, 0.001);
252 shapes::Shape* shape =
new shapes::Box(0.1, 0.1, 0.1);
253 shapes::ShapeConstPtr shape_ptr(shape);
255 Eigen::Isometry3d pos{ Eigen::Isometry3d::Identity() };
256 pos.translation().x() = 0.43;
257 pos.translation().y() = 0;
258 pos.translation().z() = 0.55;
259 this->cenv_->getWorld()->addToObject(
"box", pos, shape_ptr, Eigen::Isometry3d::Identity());
261 this->cenv_->setLinkPadding(
"panda_hand", 0.0);
262 this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
264 EXPECT_NEAR(res.
distance, 0.029, 0.01);
267template <
class CollisionAllocatorType>
276 std::set<const moveit::core::LinkModel*> active_components{ this->robot_model_->getLinkModel(
"panda_hand") };
282 random_numbers::RandomNumberGenerator rng(0x47110815);
283 double min_distance = std::numeric_limits<double>::max();
284 for (
int i = 0; i < 10; ++i)
288 shapes::ShapeConstPtr shape = std::make_shared<const shapes::Cylinder>(rng.uniform01(), rng.uniform01());
289 Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() };
291 Eigen::Vector3d(rng.uniformReal(0.1, 2.0), rng.uniformReal(0.1, 2.0), rng.uniformReal(1.2, 1.7));
293 rng.quaternion(quat);
294 pose.linear() = Eigen::Quaterniond(quat[0], quat[1], quat[2], quat[3]).toRotationMatrix();
296 this->cenv_->getWorld()->addToObject(
"collection", Eigen::Isometry3d::Identity(), shape, pose);
297 this->cenv_->getWorld()->removeObject(
"object");
298 this->cenv_->getWorld()->addToObject(
"object", pose, shape, Eigen::Isometry3d::Identity());
300 this->cenv_->distanceRobot(req, res, *this->robot_state_);
301 auto& distances1 = res.
distances[std::pair<std::string, std::string>(
"collection",
"panda_hand")];
302 auto& distances2 = res.
distances[std::pair<std::string, std::string>(
"object",
"panda_hand")];
303 ASSERT_EQ(distances1.size(), 1u) <<
"no distance reported for collection/panda_hand";
304 ASSERT_EQ(distances2.size(), 1u) <<
"no distance reported for object/panda_hand";
306 double collection_distance = distances1[0].distance;
307 min_distance = std::min(min_distance, distances2[0].distance);
308 ASSERT_NEAR(collection_distance, min_distance, 1e-5)
309 <<
"distance to collection is greater than distance to minimum of individual objects after " << i <<
" rounds";
315template <
class CollisionAllocatorType>
326 shapes::Box* shape =
new shapes::Box(0.1, 0.1, 0.1);
327 shapes::ShapeConstPtr shape_ptr(shape);
329 Eigen::Isometry3d pos{ Eigen::Isometry3d::Identity() };
330 pos.translation().x() = 0.43;
331 pos.translation().y() = 0;
332 pos.translation().z() = 0.55;
333 this->cenv_->getWorld()->addToObject(
"box", shape_ptr, pos);
336 req.
acm = &*this->acm_;
342 this->cenv_->distanceRobot(req, res, *this->robot_state_);
345 auto check_in_box = [&](
const Eigen::Vector3d& p) {
346 Eigen::Vector3d in_box = pos.inverse() * p;
348 constexpr double eps = 1e-5;
349 EXPECT_LE(std::abs(in_box.x()), shape->size[0] + eps);
350 EXPECT_LE(std::abs(in_box.y()), shape->size[1] + eps);
351 EXPECT_LE(std::abs(in_box.z()), shape->size[2] + eps);
358 for (
auto& pair : distance.second)
360 if (pair.link_names[0] ==
"box")
362 check_in_box(pair.nearest_points[0]);
364 else if (pair.link_names[1] ==
"box")
366 check_in_box(pair.nearest_points[1]);
370 ADD_FAILURE() <<
"Unrecognized link names";
377 RobotWorldCollision_1, RobotWorldCollision_2, PaddingTest, DistanceSelf, DistanceWorld);
collision_detection::CollisionEnvPtr cenv_
moveit::core::RobotModelPtr robot_model_
std::shared_ptr< CollisionAllocatorType > value_
moveit::core::RobotStatePtr robot_state_
collision_detection::AllowedCollisionMatrixPtr acm_
Representation of a robot's state. This includes position, velocity, acceleration and effort.
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...
void setJointPositions(const std::string &joint_name, const double *position)
@ SINGLE
Find the global minimum for each pair.
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
Representation of a collision checking request.
bool verbose
Flag indicating whether information about detected collisions should be reported.
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.
bool distance
If true, compute proximity distance.
Representation of a collision checking result.
double distance
Closest distance between two bodies.
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.
Representation of a distance-reporting request.
const std::set< const moveit::core::LinkModel * > * active_components_only
The set of active components to check.
std::size_t max_contacts_per_body
Maximum number of contacts to store for bodies (multiple bodies may be within distance threshold)
bool enable_signed_distance
Indicate if a signed distance should be calculated in a collision.
const AllowedCollisionMatrix * acm
The allowed collision matrix used to filter checks.
bool enable_nearest_points
Indicate if nearest point information should be calculated.
Result of a distance request.
DistanceMap distances
A map of distance data for each link in the req.active_components_only.
TYPED_TEST_CASE_P(CollisionDetectorPandaTest)
void setToHome(moveit::core::RobotState &panda_state)
Brings the panda robot in user defined home position.
TYPED_TEST_P(CollisionDetectorPandaTest, InitOK)
Correct setup testing.
REGISTER_TYPED_TEST_SUITE_P(CollisionDetectorPandaTest, InitOK, DefaultNotInCollision, LinksInCollision, RobotWorldCollision_1, RobotWorldCollision_2, PaddingTest, DistanceSelf, DistanceWorld)