diff --git a/docs/source/changelog.rst b/docs/source/changelog.rst index 278831eabb..c409d7c73c 100644 --- a/docs/source/changelog.rst +++ b/docs/source/changelog.rst @@ -11,6 +11,11 @@ Version 0.136.0 * Set correct geometry group name for fclspace +Version 0.135.1 +=============== + +* Optimize collision checking by FCL for GeometryType.Container and GeometryType.Cage. + Version 0.135.0 =============== diff --git a/plugins/fclrave/fclspace.cpp b/plugins/fclrave/fclspace.cpp index 733e6c36ee..ef5bc1f515 100644 --- a/plugins/fclrave/fclspace.cpp +++ b/plugins/fclrave/fclspace.cpp @@ -1,6 +1,7 @@ #include "plugindefs.h" #include "fclspace.h" +#include namespace fclrave { @@ -439,6 +440,21 @@ void FCLSpace::RemoveUserData(KinBodyConstPtr pbody) { } } +/// \brief helper function to initialize fcl::Container +void _AppendFclBoxCollsionObject(const OpenRAVE::Vector& fullExtents, const OpenRAVE::Vector& pos, std::vector>& contents) +{ + std::shared_ptr fclGeom = std::make_shared(fullExtents.x, fullExtents.y, fullExtents.z); + contents.emplace_back(std::make_shared(fclGeom, fcl::Transform3f(fcl::Vec3f(pos.x, pos.y, pos.z)))); +} + +/// \brief helper function to initialize fcl::Container +void _AppendFclBoxCollsionObject(const OpenRAVE::Vector& fullExtents, const OpenRAVE::Transform& trans, std::vector>& contents) +{ + std::shared_ptr fclGeom = std::make_shared(fullExtents.x, fullExtents.y, fullExtents.z); + const fcl::Transform3f fclTrans(ConvertQuaternionToFCL(trans.rot), ConvertVectorToFCL(trans.trans)); + contents.emplace_back(std::make_shared(fclGeom, fclTrans)); +} + CollisionGeometryPtr FCLSpace::_CreateFCLGeomFromGeometryInfo(const KinBody::GeometryInfo &info) { switch(info._type) { @@ -456,11 +472,65 @@ CollisionGeometryPtr FCLSpace::_CreateFCLGeomFromGeometryInfo(const KinBody::Geo case OpenRAVE::GT_Cylinder: return std::make_shared(info._vGeomData.x, info._vGeomData.y); + case OpenRAVE::GT_Container: + { + const Vector& outerextents = info._vGeomData; + const Vector& innerextents = info._vGeomData2; + const Vector& bottomcross = info._vGeomData3; + const Vector& bottom = info._vGeomData4; + OpenRAVE::dReal zoffset = 0; + if( bottom[2] > 0 ) { + if( bottom[0] > 0 && bottom[1] > 0 ) { + zoffset = bottom[2]; + } + } + std::vector> contents; + // +x wall + _AppendFclBoxCollsionObject(Vector((outerextents[0] - innerextents[0]) / 2.0, outerextents[1], outerextents[2]), Vector(+(outerextents[0] + innerextents[0]) / 4.0, 0.0, outerextents[2] / 2.0 + zoffset), contents); + // -x wall + _AppendFclBoxCollsionObject(Vector((outerextents[0] - innerextents[0]) / 2.0, outerextents[1], outerextents[2]), Vector(-(outerextents[0] + innerextents[0]) / 4.0, 0.0, outerextents[2] / 2.0 + zoffset), contents); + // +y wall + _AppendFclBoxCollsionObject(Vector(outerextents[0], (outerextents[1] - innerextents[1]) / 2.0, outerextents[2]), Vector(0.0, +(outerextents[1] + innerextents[1]) / 4.0, outerextents[2] / 2.0 + zoffset), contents); + // -y wall + _AppendFclBoxCollsionObject(Vector(outerextents[0], (outerextents[1] - innerextents[1]) / 2.0, outerextents[2]), Vector(0.0, -(outerextents[1] + innerextents[1]) / 4.0, outerextents[2] / 2.0 + zoffset), contents); + // bottom + if( outerextents[2] - innerextents[2] >= 1e-6 ) { // small epsilon error can make thin triangles appear, so test with a reasonable threshold + _AppendFclBoxCollsionObject(Vector(outerextents[0], outerextents[1], outerextents[2]-innerextents[2]), Vector(0.0, 0.0, (outerextents[2] - innerextents[2]) / 2.0 + zoffset), contents); + } + // cross + if( bottomcross[2] > 0 ) { + if( bottomcross[0] > 0 ) { + _AppendFclBoxCollsionObject(Vector(bottomcross[0], innerextents[1], bottomcross[2]), Vector(0.0, 0.0, bottomcross[2] / 2.0 + outerextents[2] - innerextents[2] + zoffset), contents); + } + if( bottomcross[1] > 0 ) { + _AppendFclBoxCollsionObject(Vector(innerextents[0], bottomcross[1], bottomcross[2]), Vector(0.0, 0.0, bottomcross[2] / 2.0 + outerextents[2] - innerextents[2] + zoffset), contents); + } + } + // bottom + if( bottom[2] > 0 ) { + if( bottom[0] > 0 && bottom[1] > 0 ) { + _AppendFclBoxCollsionObject(bottom, Vector(0.0, 0.0, bottom[2] / 2.0), contents); + } + } + return std::make_shared(contents); + } + case OpenRAVE::GT_Cage: + { + const Vector& vCageBaseExtents = info._vGeomData; + std::vector> contents; + for( const KinBody::GeometryInfo::SideWall& sideWall : info._vSideWalls ) { + Transform sideWallBoxPose; + sideWallBoxPose.rot = sideWall.transf.rot; + sideWallBoxPose.trans = sideWall.transf * Vector(0, 0, sideWall.vExtents.z); + _AppendFclBoxCollsionObject(2.0*sideWall.vExtents, sideWallBoxPose, contents); + } + // finally add the base + _AppendFclBoxCollsionObject(2.0*vCageBaseExtents, Vector(0, 0, vCageBaseExtents.z), contents); + return std::make_shared(contents); + } case OpenRAVE::GT_ConicalFrustum: case OpenRAVE::GT_Axial: - case OpenRAVE::GT_Container: case OpenRAVE::GT_TriMesh: - case OpenRAVE::GT_Cage: { const OpenRAVE::TriMesh& mesh = info._meshcollision; if (mesh.vertices.empty() || mesh.indices.empty()) {