Merge branch 'fcl-caching2' of ssh://github.com/rdiankov/openrave int…

…o production

docs/source/collada_robot_extensions.rst

@@ -454,7 +454,7 @@ Example
           <link_collision_state link="linka"><bool>true</bool></link_collision_state>
         </technique>
       </extra>
-  </library_kinematics_models>
+  </library_kinematics_models> 
 
 .. _collada_geometry_info:
 

include/openrave/collisionchecker.h

@@ -72,21 +72,21 @@ class OPENRAVE_API CollisionReport
         nKeepPrevious = 0;
         Reset();
     }
-    
+
     /// \brief resets the report structure for the next collision call
     ///
     /// depending on nKeepPrevious will keep previous data.
     virtual void Reset(int coloptions = 0);
     virtual std::string __str__() const;
-    
+
     KinBody::LinkConstPtr plink1, plink2; ///< the colliding links if a collision involves a bodies. Collisions do not always occur with 2 bodies like ray collisions, so these fields can be empty.
-    
+
     std::vector<std::pair<KinBody::LinkConstPtr, KinBody::LinkConstPtr> > vLinkColliding; ///< all link collision pairs. Set when CO_AllCollisions is enabled.
 
     std::vector<CONTACT> contacts; ///< the convention is that the normal will be "out" of plink1's surface. Filled if CO_UseContacts option is set.
 
     int options; ///< the options that the CollisionReport was called with. It is overwritten by the options set on the collision checker writing the report
-    
+
     dReal minDistance; ///< minimum distance from last query, filled if CO_Distance option is set
     int numWithinTol; ///< number of objects within tolerance of this object, filled if CO_UseTolerance option is set
 
@@ -123,14 +123,23 @@ class OPENRAVE_API CollisionCheckerBase : public InterfaceBase
 
     /// \brief Sets the geometry group that the collision checker will prefer to use (if present)
     ///
-    /// \param groupname the geometry group name. If empty, will disable the groups and use the current geometries set on the link.
+    /// Will also set the default geometry groups used for any new bodies added to the scene. groupname the geometry group name. If empty, will disable the groups and use the current geometries set on the link.
     virtual void SetGeometryGroup(const std::string& groupname) OPENRAVE_DUMMY_IMPLEMENTATION;
 
     /// \brief Gets the geometry group this collision checker is tracking.
     ///
     /// If empty, collision checker is not tracking any specific groups.
     virtual const std::string& GetGeometryGroup() const OPENRAVE_DUMMY_IMPLEMENTATION;
 
+    /// \brief Sets the geometry group that the collision checker will prefer to use for a body
+    ///
+    /// \param pbody the body to change the geometry group
+    /// \param groupname the geometry group name. If empty, will disable the groups and use the current geometries set on the link.
+    virtual void SetBodyGeometryGroup(KinBodyConstPtr pbody, const std::string& groupname) OPENRAVE_DUMMY_IMPLEMENTATION;
+
+    /// \biref Gets the geometry group that a body is currently using
+    virtual const std::string& GetBodyGeometryGroup(KinBodyConstPtr pbody) const OPENRAVE_DUMMY_IMPLEMENTATION;
+
     /// \brief initialize the checker with the current environment and gather all current bodies in the environment and put them in its collision space
     virtual bool InitEnvironment() = 0;
 
@@ -261,7 +270,7 @@ typedef boost::shared_ptr<CollisionOptionsStateSaver> CollisionOptionsStateSaver
 /** \brief Helper class to save and restore the nKeepPrevious variable in a collision report. Should be used by anyone using multiple CheckCollision calls and aggregating results.
 
     Sample code would look like:
-    
+
     bool bAllLinkCollisions = !!(pchecker->GetCollisionOptions()&CO_AllLinkCollisions);
     CollisionReportKeepSaver reportsaver(report);
     if( !!report && bAllLinkCollisions && report->nKeepPrevious == 0 ) {
@@ -282,7 +291,7 @@ class OPENRAVE_API CollisionReportKeepSaver
             _report->nKeepPrevious = _nKeepPrevious;
         }
     }
-    
+
 private:
     CollisionReportPtr _report;
     uint8_t _nKeepPrevious;

include/openrave/kinbody.h

@@ -89,13 +89,13 @@ class OPENRAVE_API KinBody : public InterfaceBase
 
         Prop_Name=0x20,     ///< name changed
         Prop_LinkDraw=0x40,     ///< toggle link geometries rendering
-        Prop_LinkGeometry=0x80,     ///< the geometry of the link changed
+        Prop_LinkGeometry=0x80,     ///< the current geometry of the link changed
         Prop_LinkTransforms=0x100, ///< if any of the link transforms changed, this implies the DOF values of the robot changed
-        // 0x200
+        Prop_LinkGeometryGroup=0x200, ///< the geometry informations of some geometry group changed
         Prop_LinkStatic=0x400,     ///< static property of link changed
         Prop_LinkEnable=0x800,     ///< enable property of link changed
         Prop_LinkDynamics=0x1000,     ///< mass/inertia properties of link changed
-        Prop_Links=Prop_LinkDraw|Prop_LinkGeometry|Prop_LinkStatic|Prop_LinkEnable|Prop_LinkDynamics,     ///< all properties of all links
+        Prop_Links=Prop_LinkDraw|Prop_LinkGeometry|Prop_LinkStatic|Prop_LinkGeometryGroup|Prop_LinkEnable|Prop_LinkDynamics,     ///< all properties of all links
         Prop_JointCustomParameters = 0x2000, ///< when Joint::SetFloatParameters(), Joint::SetIntParameters(), and Joint::SetStringParameters() are called
         Prop_LinkCustomParameters = 0x4000, ///< when Link::SetFloatParameters(), Link::SetIntParameters(), Link::SetStringParameters() are called
         Prop_BodyAttached=0x8000, ///< if attached bodies changed
@@ -1445,6 +1445,14 @@ class OPENRAVE_API KinBody : public InterfaceBase
     /// \throw If any links do not have the particular geometry, an exception will be raised.
     virtual void SetLinkGeometriesFromGroup(const std::string& name);
 
+    /// \brief Stores geometries for later retrieval for all the links at the same time.
+    ///
+    /// \param name The name of the extra geometries group to be stored in each link.
+    /// \param linkgeometries a vector containing a collection of geometry infos ptr for each links
+    /// Note that the pointers are copied and not the data, so be careful not to modify the geometries afterwards
+    /// This method is faster than Link::SetGeometriesFromGroup since it makes only one change callback.
+    virtual void SetLinkGroupGeometries(const std::string& name, const std::vector< std::vector<KinBody::GeometryInfoPtr> >& linkgeometries);
+
     /// \brief Unique name of the robot.
     virtual const std::string& GetName() const {
         return _name;
@@ -1660,6 +1668,11 @@ class OPENRAVE_API KinBody : public InterfaceBase
     /// \brief gets the enable states of all links
     virtual void GetLinkEnableStates(std::vector<uint8_t>& enablestates) const;
 
+    /// \brief gets a mask of the link enable states.
+    ///
+    /// If there are more than 64 links in the kinbody, then will give a warning. User should throw exception themselves.
+    virtual uint64_t GetLinkEnableStatesMask() const;
+
     /// queries the transfromation of the first link of the body
     virtual Transform GetTransform() const;
 
@@ -1937,6 +1950,7 @@ class OPENRAVE_API KinBody : public InterfaceBase
     ///
     /// \param setAttached fills with the attached bodies. If any bodies are already in setAttached, then ignores recursing on their attached bodies.
     virtual void GetAttached(std::set<KinBodyPtr>& setAttached) const;
+    virtual void GetAttached(std::set<KinBodyConstPtr>& setAttached) const;
 
     /// \brief return true if there are attached bodies. Used in place of GetAttached for quicker computation.
     virtual bool HasAttached() const;

plugins/fclrave/CMakeLists.txt

@@ -8,10 +8,30 @@ else()
   message("PackageConfig is supposed to be installed...")
 endif()
 
+if(NARROW_COLLISION_CACHING)
+  add_definitions(-DNARROW_COLLISION_CACHING)
+endif()
+
+
+if(FCL_USE_STATISTICS)
+  add_definitions(-DFCLUSESTATISTICS)
+  if(FCL_STATISTICS_DISPLAY_CONTINUOUSLY)
+    add_definitions(-DFCL_STATISTICS_DISPLAY_CONTINUOUSLY)
+  endif()
+endif()
+
+if(FCLRAVE_USE_COLLISION_STATISTICS)
+  add_definitions(-DFCLRAVE_COLLISION_OBJECTS_STATISTICS)
+endif()
+
+if(FCLRAVE_PERMANENT_UNSTABLE)
+  add_definitions(-DFCLRAVE_PERMANENT_UNSTABLE)
+endif()
+
 if(FCL_FOUND)
   link_directories(${OPENRAVE_LINK_DIRS} ${FCL_LIBRARY_DIRS})
   include_directories(${FCL_INCLUDE_DIRS} ${FCL_INCLUDEDIR})
-  add_library(fclrave SHARED fclrave.cpp fclcollision.h fclspace.h plugindefs.h)
+  add_library(fclrave SHARED fclrave.cpp fclcollision.h fclstatistics.h fclspace.h plugindefs.h)
   target_link_libraries(fclrave libopenrave ${FCL_LIBRARIES})
   if( CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR COMPILER_IS_CLANG)
     add_definitions("-std=c++11")