Skip to content

Commit

Permalink
Adds function to set neighstateoptions to use with neighstatefn in ji…
Browse files Browse the repository at this point in the history 
…tterers
  • Loading branch information
@Puttichai
Puttichai committed Nov 19, 2024
1 parent 583de4f commit 2f8c966
Show file tree
Hide file tree
Showing 2 changed files with 34 additions and 4 deletions.
19 changes: 17 additions & 2 deletions plugins/configurationcache/configurationjitterer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand Down Expand Up @@ -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));
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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

Expand Down
19 changes: 17 additions & 2 deletions plugins/configurationcache/workspaceconfigurationjitterer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand Down Expand Up @@ -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));
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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 ) {
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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

Expand Down

0 comments on commit 2f8c966

Please sign in to comment.