Merge remote-tracking branch 'origin/reviveOrderOfGrabbed' into fixCl…

…oneAndRestoreForInvalidNonColCache

docs/source/changelog.rst

@@ -20,6 +20,11 @@ Version 0.158.0
   - Store the link pair for grabbed-grabber collision in `Grabbed` class.
   - Store the link pair for inter-grabbed collision in `KinBody` class.
 
+Version 0.157.2
+===============
+
+- Add an interface to allow users to set `NeighborStateOptions` for jitterers to supply to their `_neighstatefn` calls, which affects how neighbor configurations are computed.
+
 Version 0.157.1
 ===============
 

include/openrave/interface.h

@@ -75,7 +75,9 @@ class OPENRAVE_API InfoBase
     virtual void DeserializeJSON(const rapidjson::Value& value, dReal fUnitScale, int options) = 0;
 };
 
-struct OPENRAVE_API ReadablesContainer {
+class OPENRAVE_API ReadablesContainer
+{
+public:
     virtual ~ReadablesContainer() = default;
 
     typedef std::map<std::string, ReadablePtr, CaseInsensitiveCompare> READERSMAP;

include/openrave/logging.h

@@ -200,7 +200,14 @@ OPENRAVE_API log4cxx::LevelPtr RaveGetVerboseLogLevel();
             } \
             /* vswprintf does not tell us how much space is needed, so we need to grow until it is satisfied */ \
             wslen *= 2; \
-            wsallocated = (wchar_t*)realloc(wsallocated, wslen*sizeof(wchar_t)); \
+            wchar_t* wsnext = (wchar_t*)realloc(wsallocated, wslen*sizeof(wchar_t)); \
+            /* realloc() can return NULL, need to remember previous allocation to avoid memory leaks */ \
+            if (wsnext != NULL) {  \
+                wsallocated = wsnext;  \
+            } else {     \
+                wr = -1; \
+                break;   \
+            } \
             ws = wsallocated; \
         } \
         if (wr >= 0) { \

plugins/configurationcache/configurationjitterer.cpp

@@ -65,6 +65,8 @@ By default will sample the robot's active DOFs. Parameters part of the interface
     bias_dir is the workspace direction to bias the sampling in.\n\
     nullsampleprob, nullbiassampleprob, and deltasampleprob are in [0,1]\n\
  //");
+        RegisterCommand("SetNeighStateOptions", boost::bind(&ConfigurationJitterer::SetNeighStateOptionsCommand, this, _1, _2),
+                        "sets a flag to use with NeighStateFn");
         RegisterJSONCommand("GetFailuresCount", boost::bind(&ConfigurationJitterer::GetFailuresCountCommand, this, _1, _2, _3),
                             "Gets the numbers of failing jittered configurations from the latest call categorized based on the failure reasons.");
         RegisterJSONCommand("GetCurrentParameters", boost::bind(&ConfigurationJitterer::GetCurrentParametersCommand, this, _1, _2, _3),
@@ -157,6 +159,7 @@ By default will sample the robot's active DOFs. Parameters part of the interface
         _linkdistthresh=0.02;
         _linkdistthresh2 = _linkdistthresh*_linkdistthresh;
         _neighdistthresh = 1;
+        _neighstateoptions = 0;
 
         _UpdateLimits();
         _limitscallback = _probot->RegisterChangeCallback(RobotBase::Prop_JointLimits, boost::bind(&ConfigurationJitterer::_UpdateLimits,this));
@@ -416,6 +419,17 @@ By default will sample the robot's active DOFs. Parameters part of the interface
         _neighstatefn = neighstatefn;
     }
 
+    bool SetNeighStateOptionsCommand(std::ostream& sout, std::istream& sinput)
+    {
+        int neighstateoptions = 0;
+        sinput >> neighstateoptions;
+        if( neighstateoptions < 0 ) {
+            return false;
+        }
+        _neighstateoptions = neighstateoptions;
+        return true;
+    }
+
     virtual bool GetFailuresCountCommand(const rapidjson::Value& input, rapidjson::Value& output, rapidjson::Document::AllocatorType& alloc)
     {
         _counter.SaveToJson(output, alloc);
@@ -483,7 +497,7 @@ By default will sample the robot's active DOFs. Parameters part of the interface
         vector<AABB> newLinkAABBs;
         bool bCollision = false;
         bool bConstraintFailed = false;
-        bool bConstraint = !!_neighstatefn;
+        const bool bConstraint = !!_neighstatefn;
 
         // have to test with perturbations since very small changes in angles can produce collision inconsistencies
         std::vector<dReal> perturbations;
@@ -695,7 +709,7 @@ By default will sample the robot's active DOFs. Parameters part of the interface
                 }
                 vnewdof = _curdof;
                 _probot->SetActiveDOFValues(vnewdof); // need to set robot configuration before calling _neighstatefn
-                if( _neighstatefn(vnewdof, _deltadof, 0) == NSS_Failed) {
+                if( _neighstatefn(vnewdof, _deltadof, _neighstateoptions) == NSS_Failed) {
                     _counter.nNeighStateFailure++;
                     continue;
                 }
@@ -1116,6 +1130,7 @@ By default will sample the robot's active DOFs. Parameters part of the interface
 
     OpenRAVE::NeighStateFn _neighstatefn; ///< if initialized, then use this function to get nearest neighbor
     ///< Advantage of using neightstatefn is that user constraints can be met like maintaining a certain orientation of the gripper.
+    int _neighstateoptions; ///< a flag to supply to _neighstatefn indicating how the neighbor should be computed.
 
     UserDataPtr _limitscallback, _grabbedcallback; ///< limits,grabbed change handles
 

plugins/configurationcache/workspaceconfigurationjitterer.cpp

@@ -60,6 +60,8 @@ By default will sample the robot's active DOFs. Parameters part of the interface
                         "sets the _bResetIterationsOnSample: whether or not to reset _nNumIterations every time Sample is called.");
         RegisterCommand("SetNullSpaceSamplingProb",boost::bind(&WorkspaceConfigurationJitterer::SetNullSpaceSamplingProbCommand, this, _1, _2),
                         "sets the probability to add perturbations from the nullspace of the Jacobian.");
+        RegisterCommand("SetNeighStateOptions", boost::bind(&WorkspaceConfigurationJitterer::SetNeighStateOptionsCommand, this, _1, _2),
+                        "sets a flag to use with NeighStateFn");
         RegisterJSONCommand("GetFailuresCount", boost::bind(&WorkspaceConfigurationJitterer::GetFailuresCountCommand, this, _1, _2, _3),
                             "Gets the numbers of failing jittered configurations from the latest call categorized based on the failure reasons.");
         RegisterJSONCommand("GetCurrentParameters", boost::bind(&WorkspaceConfigurationJitterer::GetCurrentParametersCommand, this, _1, _2, _3),
@@ -154,6 +156,7 @@ By default will sample the robot's active DOFs. Parameters part of the interface
         _linkdistthresh = 0.02;
         _linkdistthresh2 = _linkdistthresh*_linkdistthresh;
         _neighdistthresh = 1;
+        _neighstateoptions = 0;
 
         _UpdateLimits();
         _limitscallback = _probot->RegisterChangeCallback(RobotBase::Prop_JointLimits, boost::bind(&WorkspaceConfigurationJitterer::_UpdateLimits, this));
@@ -411,6 +414,17 @@ By default will sample the robot's active DOFs. Parameters part of the interface
         _neighstatefn = neighstatefn;
     }
 
+    bool SetNeighStateOptionsCommand(std::ostream& sout, std::istream& sinput)
+    {
+        int neighstateoptions = 0;
+        sinput >> neighstateoptions;
+        if( neighstateoptions < 0 ) {
+            return false;
+        }
+        _neighstateoptions = neighstateoptions;
+        return true;
+    }
+
     virtual bool GetFailuresCountCommand(const rapidjson::Value& input, rapidjson::Value& output, rapidjson::Document::AllocatorType& alloc)
     {
         _counter.SaveToJson(output, alloc);
@@ -478,7 +492,7 @@ By default will sample the robot's active DOFs. Parameters part of the interface
 
         bool bCollision = false;
         bool bConstraintFailed = false;
-        bool bHasNeighStateFn = !!_neighstatefn;
+        const bool bHasNeighStateFn = !!_neighstatefn;
 
         std::vector<dReal> vPerturbations; // for testing with perturbed configurations
         if( _fPerturbation > 0 ) {
@@ -649,7 +663,7 @@ By default will sample the robot's active DOFs. Parameters part of the interface
                 }
                 vnewdof = _curdof;
                 _probot->SetActiveDOFValues(vnewdof); // need to set robot configuration before calling _neighstatefn
-                if( _neighstatefn(vnewdof, _deltadof, 0) == NSS_Failed) {
+                if( _neighstatefn(vnewdof, _deltadof, _neighstateoptions) == NSS_Failed) {
                     _counter.nNeighStateFailure++;
                     continue;
                 }
@@ -1073,6 +1087,7 @@ By default will sample the robot's active DOFs. Parameters part of the interface
 
     OpenRAVE::NeighStateFn _neighstatefn; ///< if initialized, then use this function to get nearest neighbor
     ///< Advantage of using neightstatefn is that user constraints can be met like maintaining a certain orientation of the gripper.
+    int _neighstateoptions; ///< a flag to supply to _neighstatefn indicating how the neighbor should be computed.
 
     UserDataPtr _limitscallback, _grabbedcallback; ///< limits,grabbed change handles