Skip to content

Commit

Permalink
Adds fixes for the remaining case
Browse files Browse the repository at this point in the history 
- If the given velocities are valid (which they might in case IT_AllLinear), then compute the correct velocities to use in _SetAndCheckState.
- Adds missing checking of the the last computed configuration _vtempconfig.
  • Loading branch information
@Puttichai
Puttichai committed Mar 29, 2024
1 parent 2aecde0 commit ceb1092
Showing 1 changed file with 52 additions and 35 deletions.
87 changes: 52 additions & 35 deletions src/libopenrave/planningutils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3273,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 @@ -3362,8 +3356,12 @@ 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 ) {
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 @@ -3481,11 +3479,39 @@ int DynamicsCollisionConstraint::Check(const std::vector<dReal>& q0, const std::
}
} // end collision checking
} // end if maxnumsteps > 1
} // end check neighstatus
if( validVelocities ) {
for( size_t idof = 0; idof < dQ.size(); ++idof ) {
_vtempvelconfig.at(idof) = dq0.at(idof) + _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 @@ -3501,14 +3527,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 @@ -3524,7 +3544,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 @@ -3534,24 +3554,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

0 comments on commit ceb1092

Please sign in to comment.