From ceb1092bcd1b3da3ab7416e559c284445ed0d211 Mon Sep 17 00:00:00 2001 From: Puttichai Date: Fri, 29 Mar 2024 19:23:37 +0900 Subject: [PATCH] Adds fixes for the remaining case - 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. --- src/libopenrave/planningutils.cpp | 87 ++++++++++++++++++------------- 1 file changed, 52 insertions(+), 35 deletions(-) diff --git a/src/libopenrave/planningutils.cpp b/src/libopenrave/planningutils.cpp index ca026fac00..5f434fb1b6 100644 --- a/src/libopenrave/planningutils.cpp +++ b/src/libopenrave/planningutils.cpp @@ -3273,13 +3273,7 @@ int DynamicsCollisionConstraint::Check(const std::vector& 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()); @@ -3362,8 +3356,12 @@ int DynamicsCollisionConstraint::Check(const std::vector& 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()); @@ -3481,11 +3479,39 @@ int DynamicsCollisionConstraint::Check(const std::vector& 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; @@ -3501,14 +3527,8 @@ int DynamicsCollisionConstraint::Check(const std::vector& 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 @@ -3524,7 +3544,7 @@ int DynamicsCollisionConstraint::Check(const std::vector& 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]; } @@ -3534,24 +3554,21 @@ int DynamicsCollisionConstraint::Check(const std::vector& 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 ) {