Merge pull request #1469 from rdiankov/fix_jitterer_20241118

Add function to set neighstateoptions to use with neighstatefn in jitterer
rdiankov · Nov 27, 2024 · 2b6e18e · 2b6e18e
2 parents 0a2e82d + 7d580ca
commit 2b6e18e
Show file tree

Hide file tree

Showing 4 changed files with 40 additions and 5 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -5,7 +5,7 @@ set( CMAKE_ALLOW_LOOSE_LOOP_CONSTRUCTS TRUE )
 # Define here the needed parameters
 set (OPENRAVE_VERSION_MAJOR 0)
 set (OPENRAVE_VERSION_MINOR 157)
-set (OPENRAVE_VERSION_PATCH 1)
+set (OPENRAVE_VERSION_PATCH 2)
 set (OPENRAVE_VERSION ${OPENRAVE_VERSION_MAJOR}.${OPENRAVE_VERSION_MINOR}.${OPENRAVE_VERSION_PATCH})
 set (OPENRAVE_SOVERSION ${OPENRAVE_VERSION_MAJOR}.${OPENRAVE_VERSION_MINOR})
 message(STATUS "Compiling OpenRAVE Version ${OPENRAVE_VERSION}, soversion=${OPENRAVE_SOVERSION}")

diff --git a/docs/source/changelog.rst b/docs/source/changelog.rst
@@ -3,6 +3,11 @@
 ChangeLog
 #########
 
+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
 ===============
 

diff --git a/plugins/configurationcache/configurationjitterer.cpp b/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
 

diff --git a/plugins/configurationcache/workspaceconfigurationjitterer.cpp b/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