From 2f8c96611352e91e3a190b9988745d1f40c68f1f Mon Sep 17 00:00:00 2001 From: Puttichai Date: Tue, 19 Nov 2024 09:25:13 +0900 Subject: [PATCH] Adds function to set neighstateoptions to use with neighstatefn in jitterers --- .../configurationjitterer.cpp | 19 +++++++++++++++++-- .../workspaceconfigurationjitterer.cpp | 19 +++++++++++++++++-- 2 files changed, 34 insertions(+), 4 deletions(-) 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