restructure CollisionReport to be more memory friendly

CMakeLists.txt

@@ -4,7 +4,7 @@ set( CMAKE_ALLOW_LOOSE_LOOP_CONSTRUCTS TRUE )
 
 # Define here the needed parameters
 set (OPENRAVE_VERSION_MAJOR 0)
-set (OPENRAVE_VERSION_MINOR 131)
+set (OPENRAVE_VERSION_MINOR 132)
 set (OPENRAVE_VERSION_PATCH 0)
 set (OPENRAVE_VERSION ${OPENRAVE_VERSION_MAJOR}.${OPENRAVE_VERSION_MINOR}.${OPENRAVE_VERSION_PATCH})
 set (OPENRAVE_SOVERSION ${OPENRAVE_VERSION_MAJOR}.${OPENRAVE_VERSION_MINOR})

docs/source/changelog.rst

@@ -3,6 +3,13 @@
 ChangeLog
 #########
 
+Version 0.132.0
+===============
+
+- Cleanup CollisionReport to be more memory efficient and unify single collision vs all collisions.
+
+- Add IkFilterInfo and IkFailureAccumulator to allow for fast accumulation of IK failures.
+
 Version 0.131.0
 ===============
 

include/openrave/collisionchecker.h

@@ -53,49 +53,97 @@ enum CollisionAction
     CA_Ignore = 1, ///< do nothing
 };
 
-/// \brief Holds information about a particular collision that occured.
-class OPENRAVE_API CollisionReport
+class OPENRAVE_API CONTACT
 {
 public:
-    class OPENRAVE_API CONTACT
-    {
-public:
-        CONTACT() {
-        }
-        CONTACT(const Vector &p, const Vector &n, dReal d) : pos(p), norm(n), depth(d) {
-        }
+    CONTACT() {
+    }
+    CONTACT(const Vector &p, const Vector &n, dReal d) : pos(p), norm(n), depth(d) {
+    }
 
-        Vector pos;  ///< where the contact occured
-        Vector norm; ///< the normals of the faces
-        dReal depth = 0; ///< the penetration depth, positive means the surfaces are penetrating, negative means the surfaces are not colliding (used for distance queries)
+    Vector pos;  ///< where the contact occured
+    Vector norm; ///< the normals of the faces
+    dReal depth = 0; ///< the penetration depth, positive means the surfaces are penetrating, negative means the surfaces are not colliding (used for distance queries)
 
-        void SaveToJson(rapidjson::Value& rContact, rapidjson::Document::AllocatorType& alloc) const;
-    };
+    void SaveToJson(rapidjson::Value& rContact, rapidjson::Document::AllocatorType& alloc) const;
+    void LoadFromJson(const rapidjson::Value& rContact);
+};
 
-    struct CollisionPairInfo
-    {
-        inline void Reset() {
-            bodyLinkGeom1Name.clear();
-            bodyLinkGeom2Name.clear();
-            contacts.clear();
-        }
+class OPENRAVE_API CollisionPairInfo
+{
+public:
+    inline void Reset() {
+        bodyLinkGeom1Name.clear();
+        bodyLinkGeom2Name.clear();
+        contacts.clear();
+    }
+
+    void SaveToJson(rapidjson::Value& rCollisionPair, rapidjson::Document::AllocatorType& alloc) const;
+    void LoadFromJson(const rapidjson::Value& rCollisionPair);
 
-        void SaveToJson(rapidjson::Value& rCollisionPair, rapidjson::Document::AllocatorType& alloc) const;
+    /// \brief swaps the contents
+    void Swap(CollisionPairInfo& rhs);
+
+    /// \brief swaps the first and second geometries, also changes contacts
+    void SwapFirstSecond();
+
+    void SetFirstCollision(const std::string& bodyname, const std::string& linkname, const std::string& geomname);
+    void SetSecondCollision(const std::string& bodyname, const std::string& linkname, const std::string& geomname);
+
+    /// \brief extracts the first body name
+    void ExtractFirstBodyName(string_view& bodyname) const;
+    void ExtractSecondBodyName(string_view& bodyname) const;
+    /// \brief extracts the first link name
+    void ExtractFirstLinkName(string_view& linkname) const;
+    void ExtractSecondLinkName(string_view& linkname) const;
+    /// \brief extracts the first body and link names
+    void ExtractFirstBodyLinkNames(string_view& bodyname, string_view& linkname) const;
+    void ExtractSecondBodyLinkNames(string_view& bodyname, string_view& linkname) const;
+    /// \brief extracts the first body, link, and geom names
+    void ExtractFirstBodyLinkGeomNames(string_view& bodyname, string_view& linkname, string_view& geomname) const;
+    void ExtractSecondBodyLinkGeomNames(string_view& bodyname, string_view& linkname, string_view& geomname) const;
+
+    /// \brief compares the first body name with the passed in bodyname. return value is same as strncmp
+    int CompareFirstBodyName(const std::string& bodyname) const;
+    int CompareSecondBodyName(const std::string& bodyname) const;
+
+    /// \brief return true if the first collision matches the link
+    int CompareFirstLink(const KinBody::Link& link) const;
+    int CompareSecondLink(const KinBody::Link& link) const;
 
-        void SetFirstCollision(const std::string& bodyname, const std::string& linkname, const std::string& geomname);
-        void SetSecondCollision(const std::string& bodyname, const std::string& linkname, const std::string& geomname);
+    /// \brief compares the first link name with the passed in linkname. return value is same as strncmp
+    int CompareFirstLinkName(const std::string& linkname) const;
+    int CompareSecondLinkName(const std::string& linkname) const;
 
-        // to save on dynamic allocation, only have one string per body/link/geom. Given that names cannot have spaces, can separate with spaces
-        std::string bodyLinkGeom1Name; ///< "bodyname linkname geomname" for first collision
-        std::string bodyLinkGeom2Name; ///< "bodyname linkname geomname" for second collision
+    /// \brief if first linkname matches that of any of the links passed in, then return the index (also checks body name). Return -1 if no matching
+    ///
+    /// \param vlinks list of links coming from the same kinbody
+    int FindFirstMatchingLinkIndex(const std::vector<KinBody::LinkPtr>& vlinks) const;
+    int FindSecondMatchingLinkIndex(const std::vector<KinBody::LinkPtr>& vlinks) const;
+
+    /// \brief returns a pointer to the first body
+    KinBodyPtr ExtractFirstBody(EnvironmentBase& env) const;
+    KinBodyPtr ExtractSecondBody(EnvironmentBase& env) const;
+
+    // to save on dynamic allocation, only have one string per body/link/geom. Given that names cannot have spaces, can separate with spaces
+    std::string bodyLinkGeom1Name; ///< "bodyname linkname geomname" for first collision
+    std::string bodyLinkGeom2Name; ///< "bodyname linkname geomname" for second collision
 
-        std::vector<CONTACT> contacts; ///< the convention is that the normal will be "out" of pgeom1's surface. Filled if CO_UseContacts option is set.
-    };
+    std::vector<CONTACT> contacts; ///< the convention is that the normal will be "out" of pgeom1's surface. Filled if CO_UseContacts option is set.
+};
 
+/// \brief Holds information about a particular collision that occured. Keep the class non-virtual to allow c++ to optimize more
+class OPENRAVE_API CollisionReport
+{
+public:
     CollisionReport();
+    CollisionReport(const CollisionReport& rhs);
 
-    // should be careful not to copy all of vCollisionInfos
+    // should be careful not to copy all of vCollisionInfos, only the valid ones
     CollisionReport& operator=(const CollisionReport& rhs);
+
+    /// \brief swaps the contents
+    void Swap(CollisionReport& rhs);
 
     /// \brief resets the report structure for the next collision call
     ///
@@ -107,15 +155,24 @@ class OPENRAVE_API CollisionReport
         return nNumValidCollisions > 0;
     }
 
-    void FillBodyNamesFromLinks();
     void SaveToJson(rapidjson::Value& rCollisionReport, rapidjson::Document::AllocatorType& alloc) const;
+    void LoadFromJson(const rapidjson::Value& rCollisionReport);
+
+    /// \brief return index of the collision
+    int AddCollision();
+    int AddLinkCollision(const KinBody::Link& link1);
+    int AddLinkCollision(const KinBody::Link& link1, const KinBody::Link& link2);
+    int AddLinkGeomCollision(const KinBody::LinkConstPtr& plink1, const KinBody::GeometryConstPtr& pgeom1, const KinBody::LinkConstPtr& plink2, const KinBody::GeometryConstPtr& pgeom2);
+
+    // set only one collision
+    int SetLinkGeomCollision(const KinBody::LinkConstPtr& plink1, const KinBody::GeometryConstPtr& pgeom1, const KinBody::LinkConstPtr& plink2, const KinBody::GeometryConstPtr& pgeom2);
 
     std::vector<CollisionPairInfo> vCollisionInfos; ///< all geometry collision pairs. Set when CO_AllGeometryCollisions or CO_AllLinkCollisions or CO_AllGeometryContacts is enabled. The size of the array is not indicative of how many valid collisions there are! See nNumValidCollisions instead. Due to caching and memory constraints, should not resize this vector, instead change nNumValidCollisions
     int nNumValidCollisions = 0; ///< how many infos are valid in vCollisionInfos
-    int options = 0; ///< the options that the CollisionReport was called with. It is overwritten by the options set on the collision checker writing the report
+    int options = 0; ///< mix of CO_X, the options that the CollisionReport was called with. It is overwritten by the options set on the collision checker writing the report
 
     dReal minDistance = 1e20; ///< minimum distance from last query, filled if CO_Distance option is set
-    int numWithinTol = 0; ///< number of objects within tolerance of this object, filled if CO_UseTolerance option is set
+    int16_t numWithinTol = 0; ///< number of objects within tolerance of this object, filled if CO_UseTolerance option is set
     uint8_t nKeepPrevious = 0; ///< if 1, will keep all previous data when resetting the collision checker. otherwise will reset
 };
 

include/openrave/environment.h

@@ -515,11 +515,18 @@ class OPENRAVE_API EnvironmentBase : public boost::enable_shared_from_this<Envir
     /// \return first KinBody (including robots) that matches with name
     virtual KinBodyPtr GetKinBody(const std::string& name) const =0;
 
+    /// \brief query a body from its name
+    virtual KinBodyPtr GetKinBody(const string_view name) const =0;
+
+    virtual KinBodyPtr GetKinBody(const char* pname) const =0;
+
     /// \brief Query a body from its id. <b>[multi-thread safe]</b>
     ///
     /// \return first KinBody (including robots) that matches with the id (ie KinBody::GetId). This is different from KinBody::GetEnvironmentBodyIndex!
     virtual KinBodyPtr GetKinBodyById(const std::string& id) const =0;
 
+    virtual KinBodyPtr GetKinBodyById(const string_view id) const =0;
+
     /// \brief Query the largest environment body index in this environment. <b>[multi-thread safe]</b>
     ///
     /// \return largetst environment body index among the bodies in this environment

include/openrave/iksolver.h

@@ -71,20 +71,20 @@ typedef IkReturnAction IkFilterReturn RAVE_DEPRECATED;
 
 class OPENRAVE_API IkFailureInfo : public orjson::JsonSerializable
 {
+    friend class IkFailureAccumulator;
 public:
-    IkFailureInfo()
-    {
-    }
+    IkFailureInfo() = default;
+    IkFailureInfo(const IkFailureInfo& rhs);
 
     //IkFailureInfo& operator=(const IkFailureInfo& rhs);
-
     //void Init(const IkFailureInfo& ikFailureInfo);
 
     void SaveToJson(rapidjson::Value& rIkFailureInfo, rapidjson::Document::AllocatorType& alloc) const override;
     void LoadFromJson(const rapidjson::Value& rIkFailureInfo) override;
 
-    /// \brief clears the data. _action is left unchanged.
-    void Clear();
+    /// \brief resets for use
+    void Reset();
+
     void SetDescription(const std::string& description);
 
     inline void SetIkParam(const IkParameterization& ikparam) {
@@ -103,8 +103,8 @@ class OPENRAVE_API IkFailureInfo : public orjson::JsonSerializable
         return _ikparam;
     }
 
-    int GetIndex() const {
-        return _index;
+    int GetMemoryPoolIndex() const {
+        return _memoryPoolIndex;
     }
 
     typedef std::map<std::string, std::vector<dReal> > CustomData;
@@ -113,14 +113,16 @@ class OPENRAVE_API IkFailureInfo : public orjson::JsonSerializable
     CollisionReport _report; ///< the collision report info from when some collisions were detected.
     std::string _description;      ///< a string describing the failure
     //CustomData _mapdata;           ///< stored additional information that does not fit elsewhere
-
-    int _index; // for debugging
+    rapidjson::Document _rCustomData; ///< stored additional information that does not fit elsewhere
 
 private:
     IkParameterization _ikparam;   ///< the ikparam that fails (could be different from the ikparam given to FindIKSolutions call).
     bool _bIkParamValid=false;     ///< a flag determining whether _ikparam is valid.
+
+    int _memoryPoolIndex = -1; // index into the memory pool, managed by IkFailureAccumulator
 };
 
+/// \brief maints a pool of ikFailureInfo.
 class OPENRAVE_API IkFailureAccumulator
 {
 public:
@@ -131,23 +133,27 @@ class OPENRAVE_API IkFailureAccumulator
         return _nextIndex;
     }
 
-    inline void ResetIndex(const size_t nextIndex=0)
+    inline void ResetIndex(const int nextIndex=0)
     {
         _nextIndex = nextIndex;
     }
 
     /// \brief Retrieve ikFailureInfo from the specified index. Assume the input index is valid.
-    inline const IkFailureInfo& GetIkFailureInfo(size_t index) const
+    inline IkFailureInfo& GetIkFailureInfo(int index) const
     {
-        return *_vIkFailureInfos[index];
+        int nBatchIndex = _nextIndex/_nBatchSize;
+        OPENRAVE_ASSERT_OP(nBatchIndex,<,(int)_vIkFailureInfoBatches.size());
+        return (*_vIkFailureInfoBatches[nBatchIndex])[index - nBatchIndex*_nBatchSize];
     }
 
     /// \brief Get the next available IkFailureInfo to fill in failure information.
-    IkFailureInfoPtr GetNextAvailableIkFailureInfoPtr();
+    IkFailureInfo& GetNextAvailableIkFailureInfo();
 
 private:
-    std::vector<IkFailureInfoPtr> _vIkFailureInfos;
-    size_t _nextIndex = 0;
+    static const int _nBatchSize = 256;
+    typedef boost::array<IkFailureInfo, _nBatchSize> IkFailureInfoBatch;
+    std::vector< boost::shared_ptr<IkFailureInfoBatch> > _vIkFailureInfoBatches;
+    int _nextIndex = 0; // unique index into _vIkFailureInfoBatches taking into account the batch size
 };
 
 class OPENRAVE_API IkReturn
@@ -177,7 +183,7 @@ class OPENRAVE_API IkReturn
     std::vector< dReal > _vsolution; ///< the solution
     CustomData _mapdata; ///< name/value pairs for custom data computed in the filters. Cascading filters using the same name will overwrite this until the last executed filter (with lowest priority).
     UserDataPtr _userdata; ///< if the name/value pairs are not enough, can further use a pointer to custom data. Cascading filters with valid _userdata pointers will overwrite this until the last executed filter (with lowest priority).
-    std::vector<IkFailureInfoPtr> _vIkFailureInfos;
+    std::vector<int> _vIkFailureInfoIndices; ///< index into the accumulator
 };
 
 /** \brief <b>[interface]</b> Base class for all Inverse Kinematic solvers. <b>If not specified, method is not multi-thread safe.</b> See \ref arch_iksolver.

include/openrave/kinbody.h

@@ -1267,7 +1267,9 @@ class OPENRAVE_API KinBody : public InterfaceBase
         inline const std::vector<GeometryPtr>& GetGeometries() const {
             return _vGeometries;
         }
-        GeometryPtr GetGeometry(int index);
+        GeometryPtr GetGeometry(int index) const;
+
+        GeometryPtr GetGeometry(const string_view geomame) const;
 
         /// \brief inits the current geometries with the new geometry info.
         ///
@@ -1326,12 +1328,18 @@ class OPENRAVE_API KinBody : public InterfaceBase
         /// \return true if the normal is changed to face outside of the shape
         bool ValidateContactNormal(const Vector& position, Vector& normal) const;
 
-        /// \brief returns true if plink is rigidily attahced to this link.
         bool IsRigidlyAttached(boost::shared_ptr<Link const> plink) const RAVE_DEPRECATED {
             return IsRigidlyAttached(*plink);
         }
+
+        /// \brief returns true if link is rigidily attahced to this link.
+        ///
+        /// \param link Assumes link shares the same parent as this link.
         bool IsRigidlyAttached(const Link &link) const;
 
+        /// \brief returns true if linkIndex (index into the parent's _veclinks) is rigidily attahced to this link.
+        bool IsRigidlyAttached(const int linkIndex) const;
+
         /// \brief Gets all the rigidly attached links to linkindex, also adds the link to the list.
         ///
         /// \param vattachedlinks the array to insert all links attached to linkindex with the link itself.
@@ -2400,7 +2408,7 @@ class OPENRAVE_API KinBody : public InterfaceBase
 public:
         KinBodyStateSaver(KinBodyPtr pbody, int options = Save_LinkTransformation|Save_LinkEnable);
         virtual ~KinBodyStateSaver();
-        inline KinBodyPtr GetBody() const {
+        inline const KinBodyPtr& GetBody() const {
             return _pbody;
         }
 
@@ -2795,8 +2803,15 @@ class OPENRAVE_API KinBody : public InterfaceBase
     /// \param dofindex the degree of freedom index
     bool IsDOFPrismatic(int dofindex) const;
 
-    /// return a pointer to the link with the given name
-    LinkPtr GetLink(const std::string& name) const;
+    /// \brief return a pointer to the link with the given name
+    LinkPtr GetLink(const std::string& linkname) const;
+
+    LinkPtr GetLink(const string_view linkname) const;
+
+    LinkPtr GetLink(const char* plinkname) const;
+
+    /// \brief return the link index that matches the name. -1 if did not find
+    int GetLinkIndex(const string_view name) const;
 
     /// Updates the bounding box and any other parameters that could have changed by a simulation step
     virtual void SimulationStep(dReal fElapsedTime);

include/openrave/openrave.h

@@ -55,6 +55,12 @@
 #include <set>
 #include <string>
 
+#if  __cplusplus >= 201703L
+#include <string_view>
+#else
+#include <boost/utility/string_view.hpp>
+#endif
+
 #include <iomanip>
 #include <fstream>
 #include <sstream>
@@ -105,6 +111,12 @@ namespace OpenRAVE {
 
 namespace OpenRAVE {
 
+#if  __cplusplus >= 201703L
+using string_view = std::string_view;
+#else
+using string_view = ::boost::string_view;
+#endif
+
 #if OPENRAVE_PRECISION // 1 if double precision
 typedef double dReal;
 #define g_fEpsilon 1e-15