diff --git a/CMakeLists.txt b/CMakeLists.txt index a6cfaf8bb0..623c66dc4e 100644 --- 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 index f1ad18eeda..8b2ed93e43 100644 --- 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 index 71de9341e5..8167cfae76 100644 --- 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 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 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 index 860cb2add8..7945f64fee 100644 --- 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 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