Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix check function #1367

Merged
merged 4 commits into from
Apr 3, 2024
Merged

Fix check function #1367

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
2aecde0
Adds missing check of _vtempconfig from the last iteration for IT_Def…
Puttichai Mar 29, 2024
ceb1092
Adds fixes for the remaining case
Puttichai Mar 29, 2024
a7cb1ab
Updates changelog and bumps version
Puttichai Apr 2, 2024
7f059ac
Fixes velocity computation
Puttichai Apr 2, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 141)
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}")
Expand Down
5 changes: 5 additions & 0 deletions docs/source/changelog.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,11 @@
ChangeLog
#########

Version 0.141.2
===============

* Fix the issue that second-to-last configuration along the given path segment may not be checked in `Check` function.

Version 0.141.1
===============

Expand Down
135 changes: 75 additions & 60 deletions src/libopenrave/planningutils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3234,33 +3234,29 @@ int DynamicsCollisionConstraint::Check(const std::vector<dReal>& q0, const std::
}
}

if( bCheckEnd && bHasNewTempConfigToAdd && bHasRampDeviatedFromInterpolation ) {
dReal dist = params->_distmetricfn(_vtempconfig, q1);
if( bHasNewTempConfigToAdd ) {
// At this point, _vtempconfig is expected to have reached q1. But if not, then need to check _vtempconfig.
const dReal dist = params->_distmetricfn(_vtempconfig, q1);
if( dist > 1e-7 ) {
RAVELOG_DEBUG_FORMAT("env=%d, ramp has deviated, so most likely q1 is not following constraints and there's a difference dist=%f", _listCheckBodies.front()->GetEnv()->GetId()%dist);
bCheckEnd = false; // to prevent adding the last point

if( !!filterreturn ) {
if( options & CFO_FillCheckedConfiguration ) {
int nstateret = 0;
if( istep >= start ) {
nstateret = _SetAndCheckState(params, _vtempconfig, _vtempvelconfig, _vtempaccelconfig, maskoptions, filterreturn);
bHasNewTempConfigToAdd = false;
if( !!params->_getstatefn ) {
params->_getstatefn(_vtempconfig); // query again in order to get normalizations/joint limits
}
if( !!filterreturn && (options & CFO_FillCheckedConfiguration) ) {
filterreturn->_configurations.insert(filterreturn->_configurations.end(), _vtempconfig.begin(), _vtempconfig.end());
filterreturn->_configurationtimes.push_back(timestep);
}
}
if( nstateret != 0 ) {
if( !!filterreturn ) {
filterreturn->_returncode = nstateret;
}
return nstateret;
}
bCheckEnd = false; // _vtempconfig ends up far from q1 and we haven't checked the segment connecting _vtempconfig and q1 so prevent adding q1 to the list of checked configuration to tell the caller that the checked segment ends here.
int nstateret = 0;
nstateret = _SetAndCheckState(params, _vtempconfig, _vtempvelconfig, _vtempaccelconfig, maskoptions, filterreturn);
bHasNewTempConfigToAdd = false;
if( !!params->_getstatefn ) {
params->_getstatefn(_vtempconfig); // query again in order to get normalizations/joint limits
}
if( !!filterreturn && (options & CFO_FillCheckedConfiguration) ) {
filterreturn->_configurations.insert(filterreturn->_configurations.end(), _vtempconfig.begin(), _vtempconfig.end());
filterreturn->_configurationtimes.push_back(timestep);
}
if( nstateret != 0 ) {
if( !!filterreturn ) {
filterreturn->_returncode = nstateret;
filterreturn->_invalidvalues = _vtempconfig;
filterreturn->_invalidvelocities = _vtempvelconfig;
filterreturn->_fTimeWhenInvalid = timestep;
}
return nstateret;
}
}
}
Expand All @@ -3277,13 +3273,7 @@ int DynamicsCollisionConstraint::Check(const std::vector<dReal>& q0, const std::
*it *= fisteps;
}

// just in case, have to set the current values to _vtempconfig since neighstatefn expects the state to be set.
if( params->SetStateValues(_vtempconfig, 0) != 0 ) {
if( !!filterreturn ) {
filterreturn->_returncode = CFO_StateSettingError;
}
return CFO_StateSettingError;
}
const bool validVelocities = (timeelapsed > 0) && (dq0.size() == _vtempconfig.size()) && (dq1.size() == _vtempconfig.size());

_vdiffconfig.resize(dQ.size());
_vstepconfig.resize(dQ.size());
Expand Down Expand Up @@ -3366,8 +3356,13 @@ int DynamicsCollisionConstraint::Check(const std::vector<dReal>& q0, const std::
}
} // end checking configurations between _vtempconfig2 (the previous _vtempconfig) and _vtempconfig (the new one)
} // end if maxnumsteps > 1
} // end check neighstatus
if( validVelocities ) {
// Compute the next velocity
for( size_t idof = 0; idof < dQ.size(); ++idof ) {
_vtempvelconfig.at(idof) = dq0.at(idof) + _vtempveldelta.at(idof);
}
}
// Else, _neighstatefn returns _vtempconfig + dQ.
}

_vprevtempconfig.resize(dQ.size());
Expand Down Expand Up @@ -3485,11 +3480,40 @@ int DynamicsCollisionConstraint::Check(const std::vector<dReal>& q0, const std::
}
} // end collision checking
} // end if maxnumsteps > 1
} // end check neighstatus
if( validVelocities ) {
// Compute the next velocity
for( size_t idof = 0; idof < dQ.size(); ++idof ) {
_vtempvelconfig.at(idof) = dq0.at(idof) + dReal(f + 1)*_vtempveldelta.at(idof);
}
}
} // end for

// At this point, _vtempconfig is not checked yet!
const dReal dist = params->_distmetricfn(_vtempconfig, q1);
if( dist > 1e-7 ) {
// _vtempconfig is different from q1 so must check it.
int nstateret = 0;
nstateret = _SetAndCheckState(params, _vtempconfig, _vtempvelconfig, _vtempaccelconfig, maskoptions, filterreturn);
if( !!params->_getstatefn ) {
params->_getstatefn(_vtempconfig); // query again in order to get normalizations/joint limits
}
if( !!filterreturn && (options & CFO_FillCheckedConfiguration) ) {
filterreturn->_configurations.insert(filterreturn->_configurations.end(), _vtempconfig.begin(), _vtempconfig.end());
filterreturn->_configurationtimes.push_back(1.0);
}
if( nstateret != 0 ) {
if( !!filterreturn ) {
filterreturn->_returncode = nstateret;
filterreturn->_invalidvalues = _vtempconfig;
if( validVelocities ) {
filterreturn->_invalidvelocities = _vtempvelconfig;
}
filterreturn->_fTimeWhenInvalid = 1.0;
}
return nstateret;
}
}

// check if _vtempconfig is close to q1!
{
// the neighbor function could be a constraint function and might move _vtempconfig by more than the specified dQ! so double check the straight light distance between them justin case?
// TODO check if acceleration limits are satisfied between _vtempconfig, _vprevtempconfig, and _vprevtempvelconfig
int numPostNeighSteps = 1;
Expand All @@ -3505,14 +3529,8 @@ int DynamicsCollisionConstraint::Check(const std::vector<dReal>& q0, const std::
}

if( numPostNeighSteps > 1 ) {
bHasRampDeviatedFromInterpolation = true;
// should never happen, but just in case _neighstatefn is some non-linear constraint projection
if( _listCheckBodies.size() > 0 ) {
RAVELOG_WARN_FORMAT("env=%d, have to divide the arc in %d steps even after original interpolation is done, interval=%d", _listCheckBodies.front()->GetEnv()->GetId()%numPostNeighSteps%interval);
}
else {
RAVELOG_WARN_FORMAT("have to divide the arc in %d steps even after original interpolation is done, interval=%d", numPostNeighSteps%interval);
}
bHasRampDeviatedFromInterpolation = true; // set here again just in case
RAVELOG_WARN_FORMAT("env=%s, have to divide the arc in %d steps even after original interpolation is done, interval=%d", _listCheckBodies.front()->GetEnv()->GetNameId()%numPostNeighSteps%interval);

// this case should be rare, so can create a vector here. don't look at constraints since we would never converge...
// note that circular constraints would break here
Expand All @@ -3528,7 +3546,7 @@ int DynamicsCollisionConstraint::Check(const std::vector<dReal>& q0, const std::
_vprevtempconfig = _vtempconfig;
_vprevtempvelconfig = _vtempvelconfig;
// do only numPostNeighSteps-1 since the last step should be checked by _vtempconfig
for(int ipoststep = 0; ipoststep+1 < numPostNeighSteps; ++ipoststep) {
for(int ipoststep = 0; ipoststep < numPostNeighSteps; ++ipoststep) {
for( int idof = 0; idof < (int)_vtempconfig.size(); ++idof) {
_vprevtempconfig[idof] += vpostdq[idof];
}
Expand All @@ -3538,24 +3556,21 @@ int DynamicsCollisionConstraint::Check(const std::vector<dReal>& q0, const std::
}
}

int nstateret = _SetAndCheckState(params, _vprevtempconfig, _vprevtempvelconfig, _vtempaccelconfig, maskoptions, filterreturn);
// if( !!params->_getstatefn ) {
// params->_getstatefn(_vprevtempconfig); // query again in order to get normalizations/joint limits
// }
// since the timeelapsed is not clear, it is dangerous to write filterreturn->_configurations and filterreturn->_configurationtimes since it could force programing using those times to accelerate too fast. so don't write
// if( !!filterreturn && (options & CFO_FillCheckedConfiguration) ) {
// filterreturn->_configurations.insert(filterreturn->_configurations.end(), _vtempconfig.begin(), _vtempconfig.end());
// filterreturn->_configurationtimes.push_back(timestep);
// }
if( nstateret != 0 ) {
int npostneighret = _SetAndCheckState(params, _vprevtempconfig, _vprevtempvelconfig, _vtempaccelconfig, maskoptions, filterreturn);
if( npostneighret != 0 ) {
if( !!filterreturn ) {
filterreturn->_returncode = nstateret;
filterreturn->_returncode = npostneighret;
filterreturn->_invalidvalues = _vprevtempconfig;
if( validVelocities ) {
filterreturn->_invalidvelocities = _vprevtempvelconfig;
}
filterreturn->_fTimeWhenInvalid = 1.0;
}
return nstateret;
return npostneighret;
}
}
}
}
} // end for ipoststep
} // end numPostNeighSteps > 1
} // end dist > 1e-7
}

if( !!filterreturn ) {
Expand Down
Loading