43 #include <moveit_resources/config.h> 46 #include <urdf_parser/urdf_parser.h> 49 #include <gtest/gtest.h> 53 #include <boost/filesystem.hpp> 65 std::fstream xml_file(MOVEIT_TEST_RESOURCES_DIR
"/pr2_description/urdf/robot.xml", std::fstream::in);
66 if (xml_file.is_open())
68 while (xml_file.good())
71 std::getline(xml_file, line);
72 xml_string += (line +
"\n");
86 std::map<std::string, std::vector<collision_detection::CollisionSphere>> link_body_decompositions;
104 robot_state::TransformsPtr
ftf_;
107 collision_detection::CollisionRobotPtr
crobot_;
108 collision_detection::CollisionWorldPtr
cworld_;
110 collision_detection::AllowedCollisionMatrixPtr
acm_;
124 crobot_->checkSelfCollision(req, res, kstate, *
acm_);
139 crobot_->checkSelfCollision(req, res1, kstate, *
acm_);
140 std::map<std::string, double> torso_val;
141 torso_val[
"torso_lift_joint"] = .15;
144 crobot_->checkSelfCollision(req, res1, kstate, *
acm_);
145 crobot_->checkSelfCollision(req, res1, kstate, *
acm_);
161 Eigen::Affine3d offset = Eigen::Affine3d::Identity();
162 offset.translation().x() = .01;
167 acm_->setEntry(
"base_link",
"base_bellow_link",
false);
168 crobot_->checkSelfCollision(req, res1, kstate, *
acm_);
171 acm_->setEntry(
"base_link",
"base_bellow_link",
true);
172 crobot_->checkSelfCollision(req, res2, kstate, *
acm_);
178 acm_->setEntry(
"r_gripper_palm_link",
"l_gripper_palm_link",
false);
179 crobot_->checkSelfCollision(req, res3, kstate, *
acm_);
193 Eigen::Affine3d offset = Eigen::Affine3d::Identity();
194 offset.translation().x() = .01;
202 acm_->setEntry(
"base_link",
"base_bellow_link",
false);
203 acm_->setEntry(
"r_gripper_palm_link",
"l_gripper_palm_link",
false);
206 crobot_->checkSelfCollision(req, res, kstate, *
acm_);
215 crobot_->checkSelfCollision(req, res, kstate, *
acm_);
226 crobot_->checkSelfCollision(req, res, kstate, *
acm_);
242 Eigen::Affine3d pos1 = Eigen::Affine3d::Identity();
243 Eigen::Affine3d pos2 = Eigen::Affine3d::Identity();
245 pos1.translation().x() = 5.0;
246 pos2.translation().x() = 5.01;
251 acm_->setEntry(
"r_gripper_palm_link",
"l_gripper_palm_link",
false);
254 crobot_->checkSelfCollision(req, res, kstate, *
acm_);
257 ASSERT_EQ(res.
contacts.begin()->second.size(), 1);
259 for (collision_detection::CollisionResult::ContactMap::const_iterator it = res.
contacts.begin();
265 pos1 = Eigen::Affine3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity());
266 pos2 = Eigen::Affine3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond(0.965, 0.0, 0.258, 0.0));
272 crobot_->checkSelfCollision(req, res2, kstate, *
acm_);
275 ASSERT_EQ(res2.
contacts.begin()->second.size(), 1);
277 for (collision_detection::CollisionResult::ContactMap::const_iterator it = res2.
contacts.begin();
283 pos1 = Eigen::Affine3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity());
284 pos2 = Eigen::Affine3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond(
M_PI / 4.0, 0.0,
M_PI / 4.0, 0.0));
290 crobot_->checkSelfCollision(req, res2, kstate, *
acm_);
307 Eigen::Affine3d pos1 = Eigen::Affine3d::Identity();
308 pos1.translation().x() = 1.0;
311 crobot_->checkSelfCollision(req, res, kstate, *
acm_);
322 cworld_->getWorld()->removeObject(
"box");
324 std::vector<shapes::ShapeConstPtr>
shapes;
327 poses.push_back(Eigen::Affine3d::Identity());
328 std::set<std::string> touch_links;
329 trajectory_msgs::JointTrajectory empty_state;
331 kstate.
getLinkModel(
"r_gripper_palm_link"),
"box", shapes, poses, touch_links, empty_state);
336 crobot_->checkSelfCollision(req, res, kstate, *
acm_);
342 touch_links.insert(
"r_gripper_palm_link");
346 kstate.
getLinkModel(
"r_gripper_palm_link"),
"box", shapes, poses, touch_links, empty_state);
350 crobot_->checkSelfCollision(req, res, kstate, *
acm_);
353 pos1.translation().x() = 1.01;
360 acm_->setEntry(
"coll",
"r_gripper_palm_link",
true);
366 int main(
int argc,
char** argv)
368 testing::InitGoogleTest(&argc, argv);
369 return RUN_ALL_TESTS();
robot_state::TransformsPtr ftf_
Representation of a collision checking request.
urdf::ModelInterfaceSharedPtr urdf_model_
int main(int argc, char **argv)
collision_detection::CollisionRobotDistanceField DefaultCRobotType
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
void attachBody(AttachedBody *attached_body)
Add an attached body to this state. Ownership of the memory for the attached body is assumed by the s...
ContactMap contacts
A map returning the pairs of ids of the bodies in contact, plus information about the contacts themse...
Representation of a collision checking result.
TEST_F(DistanceFieldCollisionDetectionTester, DefaultNotInCollision)
robot_state::TransformsConstPtr ftf_const_
srdf::ModelSharedPtr srdf_model_
#define EXPECT_NEAR(a, b, prec)
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state...
EIGEN_MAKE_ALIGNED_OPERATOR_NEW void clear()
Clear a previously stored result.
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
collision_detection::CollisionRobotPtr crobot_
std::size_t contact_count
Number of contacts returned.
bool collision
True if collision was found, false otherwise.
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
std::size_t max_contacts
Overall maximum number of contacts to compute.
void update(bool force=false)
Update all transforms.
collision_detection::CollisionWorldPtr cworld_
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
void updateStateWithLinkAt(const std::string &link_name, const Eigen::Affine3d &transform, bool backward=false)
Update the state after setting a particular link to the input global transform pose.
robot_model::RobotModelPtr robot_model_
std::string group_name
The group name to check collisions for (optional; if empty, assume the complete robot) ...
bool contacts
If true, compute contacts.
def xml_string(rootXml, addHeader=True)
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...
const LinkModel * getLinkModel(const std::string &link) const
Get the model of a particular link.
Representation of a robot's state. This includes position, velocity, acceleration and effort...
collision_detection::CollisionWorldDistanceField DefaultCWorldType
Object defining bodies that can be attached to robot links. This is useful when handling objects pick...
collision_detection::AllowedCollisionMatrixPtr acm_
bool clearAttachedBody(const std::string &id)
Remove the attached body named id. Return false if the object was not found (and thus not removed)...
std::shared_ptr< const Shape > ShapeConstPtr