Commit 5f5ab6df authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub

Merge pull request #954 from jmirabel/urdf_convex

[URDF][Experimental] Enable convex representation of meshes for collision detection
parents d0a84f4e fdf192b2
......@@ -4,6 +4,8 @@ git:
depth: false
compiler:
- gcc
cache:
ccache: true
env:
global:
- secure: "SnIBG/xLIHX3CSvUbqqsX8xTVqIqQ7fFS6HWO6KZQVBsT6yugTwYHbyhNiU531JejYJ/I3ZrDhXfYH3qFZiYxnH1sifvwV+fnTtMXpPN7qPZwIymkjcmm6gJF51e0C7VOfUbvKFv0ngwj+ul21rgZSMuoEvxPK0WxtE3/ZSfn9c="
......
......@@ -17,10 +17,16 @@
<cylinder radius="1" length="1"/>
</geometry>
</collision>
<collision name="box">
<geometry>
<mesh filename="package://simple_humanoid_description/box.stl" />
</geometry>
</collision>
<collision_checking>
<!--- This tells to pinocchio to replace the cylinder called "test"
by a capsule with the same radius and length -->
<capsule name="test"/>
<convex name="box"/>
</collision_checking>
</link>
......
solid Exported from Blender-2.78 (sub 0)
facet normal -1.000000 0.000000 0.000000
outer loop
vertex -0.500000 -0.500000 -0.500000
vertex -0.500000 -0.500000 0.500000
vertex -0.500000 0.500000 0.500000
endloop
endfacet
facet normal -1.000000 0.000000 0.000000
outer loop
vertex -0.500000 0.500000 0.500000
vertex -0.500000 0.500000 -0.500000
vertex -0.500000 -0.500000 -0.500000
endloop
endfacet
facet normal 0.000000 1.000000 0.000000
outer loop
vertex -0.500000 0.500000 -0.500000
vertex -0.500000 0.500000 0.500000
vertex 0.500000 0.500000 0.500000
endloop
endfacet
facet normal 0.000000 1.000000 0.000000
outer loop
vertex 0.500000 0.500000 0.500000
vertex 0.500000 0.500000 -0.500000
vertex -0.500000 0.500000 -0.500000
endloop
endfacet
facet normal 1.000000 0.000000 0.000000
outer loop
vertex 0.500000 0.500000 -0.500000
vertex 0.500000 0.500000 0.500000
vertex 0.500000 -0.500000 0.500000
endloop
endfacet
facet normal 1.000000 0.000000 0.000000
outer loop
vertex 0.500000 -0.500000 0.500000
vertex 0.500000 -0.500000 -0.500000
vertex 0.500000 0.500000 -0.500000
endloop
endfacet
facet normal 0.000000 -1.000000 0.000000
outer loop
vertex -0.500000 -0.500000 0.500000
vertex -0.500000 -0.500000 -0.500000
vertex 0.500000 -0.500000 -0.500000
endloop
endfacet
facet normal 0.000000 -1.000000 0.000000
outer loop
vertex 0.500000 -0.500000 -0.500000
vertex 0.500000 -0.500000 0.500000
vertex -0.500000 -0.500000 0.500000
endloop
endfacet
facet normal 0.000000 0.000000 -1.000000
outer loop
vertex 0.500000 -0.500000 -0.500000
vertex -0.500000 -0.500000 -0.500000
vertex -0.500000 0.500000 -0.500000
endloop
endfacet
facet normal 0.000000 0.000000 -1.000000
outer loop
vertex -0.500000 0.500000 -0.500000
vertex 0.500000 0.500000 -0.500000
vertex 0.500000 -0.500000 -0.500000
endloop
endfacet
facet normal 0.000000 0.000000 1.000000
outer loop
vertex 0.500000 0.500000 0.500000
vertex -0.500000 0.500000 0.500000
vertex -0.500000 -0.500000 0.500000
endloop
endfacet
facet normal 0.000000 0.000000 1.000000
outer loop
vertex -0.500000 -0.500000 0.500000
vertex 0.500000 -0.500000 0.500000
vertex 0.500000 0.500000 0.500000
endloop
endfacet
endsolid Exported from Blender-2.78 (sub 0)
......@@ -56,8 +56,8 @@ namespace pinocchio
} // BOOST_FOREACH
}
bool replaceCylinderByCapsule (const std::string & linkName,
const std::string & geomName) const
bool isCapsule (const std::string & linkName,
const std::string & geomName) const
{
LinkMap_t::const_iterator _link = links_.find(linkName);
assert (_link != links_.end());
......@@ -66,8 +66,36 @@ namespace pinocchio
return false;
BOOST_FOREACH(const ptree::value_type & cc, link.get_child("collision_checking")) {
if (cc.first == "capsule") {
#ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
std::cerr << "Warning: support for tag link/collision_checking/capsule"
" is not available for URDFDOM < 0.3.0" << std::endl;
#else
std::string name = cc.second.get<std::string>("<xmlattr>.name");
if (geomName == name) return true;
#endif
}
} // BOOST_FOREACH
return false;
}
bool isMeshConvex (const std::string & linkName,
const std::string & geomName) const
{
LinkMap_t::const_iterator _link = links_.find(linkName);
assert (_link != links_.end());
const ptree& link = _link->second;
if (link.count ("collision_checking") == 0)
return false;
BOOST_FOREACH(const ptree::value_type & cc, link.get_child("collision_checking")) {
if (cc.first == "convex") {
#ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
std::cerr << "Warning: support for tag link/collision_checking/convex"
" is not available for URDFDOM < 0.3.0" << std::endl;
#else
std::string name = cc.second.get<std::string>("<xmlattr>.name");
if (geomName == name) return true;
#endif
}
} // BOOST_FOREACH
......@@ -93,7 +121,13 @@ namespace pinocchio
mesh->scale.z;
}
#ifdef PINOCCHIO_WITH_HPP_FCL
#ifdef PINOCCHIO_WITH_HPP_FCL
# if ( HPP_FCL_MAJOR_VERSION>1 || ( HPP_FCL_MAJOR_VERSION==1 && \
( HPP_FCL_MINOR_VERSION>1 || ( HPP_FCL_MINOR_VERSION==1 && \
HPP_FCL_PATCH_VERSION>3))))
# define PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
# endif
/**
* @brief Get a fcl::CollisionObject from an urdf geometry, searching
* for it in specified package directories
......@@ -138,14 +172,24 @@ namespace pinocchio
retrieveMeshScale(urdf_mesh, meshScale);
// Create FCL mesh by parsing Collada file.
#ifdef PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
hpp::fcl::BVHModelPtr_t bvh = meshLoader->load (meshPath, scale);
bool convex = tree.isMeshConvex (linkName, geomName);
if (convex) {
bvh->buildConvexRepresentation (false);
geometry = bvh->convex;
} else
geometry = bvh;
#else
geometry = meshLoader->load (meshPath, scale);
#endif
}
// Handle the case where collision geometry is a cylinder
// Use FCL capsules for cylinders
else if (urdf_geometry->type == ::urdf::Geometry::CYLINDER)
{
bool capsule = tree.replaceCylinderByCapsule(linkName, geomName);
bool capsule = tree.isCapsule(linkName, geomName);
meshScale << 1,1,1;
const ::urdf::CylinderSharedPtr collisionGeometry = ::urdf::dynamic_pointer_cast< ::urdf::Cylinder> (urdf_geometry);
......@@ -475,4 +519,8 @@ namespace pinocchio
} // namespace urdf
} // namespace pinocchio
#ifdef PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
# undef PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
#endif
#endif // ifndef __pinocchio_multibody_parsers_urdf_geometry_hxx__
......@@ -9,6 +9,10 @@
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/parsers/urdf.hpp"
#ifdef PINOCCHIO_WITH_HPP_FCL
#include <hpp/fcl/collision_object.h>
#endif // PINOCCHIO_WITH_HPP_FCL
#include <boost/test/unit_test.hpp>
#include <urdf_parser/urdf_parser.h>
......@@ -32,11 +36,39 @@ BOOST_AUTO_TEST_CASE ( build_model )
BOOST_AUTO_TEST_CASE ( build_model_simple_humanoid )
{
const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
const std::string dir = PINOCCHIO_MODEL_DIR;
pinocchio::Model model;
pinocchio::urdf::buildModel(filename, model);
BOOST_CHECK(model.nq == 29);
BOOST_CHECK_EQUAL(model.nq, 29);
// Check that parsing collision_checking works.
pinocchio::GeometryModel geomModel;
pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel, dir);
BOOST_CHECK_EQUAL(geomModel.ngeoms, 2);
#ifdef PINOCCHIO_WITH_HPP_FCL
// Check that cylinder is converted into capsule.
#ifdef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
BOOST_CHECK_EQUAL(geomModel.geometryObjects[0].fcl->getNodeType(), hpp::fcl::GEOM_CYLINDER);
#else // PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
BOOST_CHECK_EQUAL(geomModel.geometryObjects[0].fcl->getNodeType(), hpp::fcl::GEOM_CAPSULE);
#endif
#if ( HPP_FCL_MAJOR_VERSION>1 || ( HPP_FCL_MAJOR_VERSION==1 && \
( HPP_FCL_MINOR_VERSION>1 || ( HPP_FCL_MINOR_VERSION==1 && \
HPP_FCL_PATCH_VERSION>3))))
# define PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
#endif
#if defined(PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3) && !defined(PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME)
BOOST_CHECK_EQUAL(geomModel.geometryObjects[1].fcl->getNodeType(), hpp::fcl::GEOM_CONVEX);
#undef PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3
#else // PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3 && !PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
BOOST_CHECK_EQUAL(geomModel.geometryObjects[1].fcl->getObjectType(), hpp::fcl::OT_BVH);
#endif // PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3 && !PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME
#endif // PINOCCHIO_WITH_HPP_FCL
pinocchio::Model model_ff;
pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model_ff);
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment