Merge pull request #1350 from YutaKojio/embedGripperValuesIntoJointVa…

…lues20231030 Draft: ignore NaN in joint values
rdiankov · Feb 21, 2024 · 2595122 · 2595122
2 parents 1062c92 + daf39c5
commit 2595122
Show file tree

Hide file tree

Showing 4 changed files with 45 additions and 24 deletions.
diff --git a/plugins/basecontrollers/idealcontroller.cpp b/plugins/basecontrollers/idealcontroller.cpp
@@ -478,6 +478,9 @@ If SetDesired is called, only joint values will be set at every timestep leaving
     {
         for(size_t i = 0; i < _vlower[0].size(); ++i) {
             if( !_dofcircular[i] ) {
+                if( std::isnan(curvalues.at(i)) ) {
+                    continue;
+                }
                 if( curvalues.at(i) < _vlower[0][i]-g_fEpsilonJointLimit ) {
                     _ReportError(str(boost::format("robot %s dof %d is violating lower limit %e < %e, time=%f")%probot->GetName()%i%_vlower[0][i]%curvalues[i]%_fCommandTime));
                 }
@@ -490,6 +493,9 @@ If SetDesired is called, only joint values will be set at every timestep leaving
             vector<dReal> vdiff = curvalues;
             probot->SubtractDOFValues(vdiff,prevvalues);
             for(size_t i = 0; i < _vupper[1].size(); ++i) {
+                if( std::isnan(vdiff.at(i)) ) {
+                    continue;
+                }
                 dReal maxallowed = timeelapsed * _vupper[1][i]+1e-6;
                 if( RaveFabs(vdiff.at(i)) > maxallowed ) {
                     _ReportError(str(boost::format("robot %s dof %d is violating max velocity displacement %.15e > %.15e, time=%f")%probot->GetName()%i%RaveFabs(vdiff.at(i))%maxallowed%_fCommandTime));

diff --git a/src/libopenrave-core/generictrajectory.cpp b/src/libopenrave-core/generictrajectory.cpp
@@ -1044,6 +1044,9 @@ class GenericTrajectory : public TrajectoryBase
                 _vgroupvalidators[i] = boost::bind(&GenericTrajectory::_ValidateSextic,this,boost::ref(_spec._vgroups[i]),_1,_2);
                 nNeedNeighboringInfo = 3;
             }
+            if( interpolation == "max" ) {
+                _vgroupinterpolators[i] = boost::bind(&GenericTrajectory::_InterpolateMax,this,boost::ref(_spec._vgroups[i]),_1,_2,_3);
+            }
             else if( interpolation == "" ) {
                 // if there is no interpolation, default to "next". deltatime is such a group, but that is overwritten
                 _vgroupinterpolators[i] = boost::bind(&GenericTrajectory::_InterpolateNext,this,boost::ref(_spec._vgroups[i]),_1,_2,_3);
@@ -1655,6 +1658,14 @@ class GenericTrajectory : public TrajectoryBase
         }
     }
 
+    void _InterpolateMax(const ConfigurationSpecification::Group& g, size_t ipoint, dReal deltatime, const std::vector<dReal>::iterator& itdata)
+    {
+        size_t offset = ipoint*_spec.GetDOF()+g.offset;
+        for(int i = 0; i < g.dof; ++i) {
+            *(itdata + g.offset+i) = std::max(_vtrajdata[offset+i], _vtrajdata[_spec.GetDOF()+offset+i]);
+        }
+    }
+
     void _ValidateLinear(const ConfigurationSpecification::Group& g, size_t ipoint, dReal deltatime)
     {
         size_t offset = ipoint*_spec.GetDOF();

diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp
@@ -2126,34 +2126,33 @@ void KinBody::SetDOFValues(const dReal* pJointValues, int dof, uint32_t checklim
     int expecteddof = dofindices.size() > 0 ? (int)dofindices.size() : GetDOF();
     OPENRAVE_ASSERT_OP_FORMAT((int)dof,>=,expecteddof, "env=%s, not enough values %d<%d", GetEnv()->GetNameId()%dof%GetDOF(),ORE_InvalidArguments);
 
-    if(checklimits == CLA_Nothing && dofindices.empty()) {
-        _vTempJoints.assign(pJointValues, pJointValues + dof);
+    GetDOFValues(_vTempJoints);
+    if( dofindices.size() > 0 ) {
+        // user only set a certain number of indices, so have to fill the temporary array with the full set of values first
+        // and then overwrite with the user set values
+        for(size_t i = 0; i < dofindices.size(); ++i) {
+            if( !std::isnan(pJointValues[i]) ) {
+                _vTempJoints.at(dofindices[i]) = pJointValues[i];
+            }
+        }
     }
     else {
-        _vTempJoints.resize(GetDOF());
-        if( dofindices.size() > 0 ) {
-            // user only set a certain number of indices, so have to fill the temporary array with the full set of values first
-            // and then overwrite with the user set values
-            GetDOFValues(_vTempJoints);
-            for(size_t i = 0; i < dofindices.size(); ++i) {
-                _vTempJoints.at(dofindices[i]) = pJointValues[i];
+        for(size_t i = 0; i < _vTempJoints.size(); ++i) {
+            if( !std::isnan(pJointValues[i]) ) {
+                _vTempJoints[i] = pJointValues[i];
             }
-            pJointValues = &_vTempJoints[0];
         }
+    }
+    pJointValues = &_vTempJoints[0];
+
+    if( checklimits != CLA_Nothing ) {
         dReal* ptempjoints = &_vTempJoints[0];
 
         // check the limits
         for(const JointPtr& pjoint : _vecjoints) {
             const Joint& joint = *pjoint;
 
             const dReal* p = pJointValues+joint.GetDOFIndex();
-            if( checklimits == CLA_Nothing ) {
-                // limits should not be checked, so just copy
-                for(int i = 0; i < joint.GetDOF(); ++i) {
-                    *ptempjoints++ = p[i];
-                }
-                continue;
-            }
             if( joint.GetType() == JointSpherical ) {
                 dReal fcurang = fmod(RaveSqrt(p[0]*p[0]+p[1]*p[1]+p[2]*p[2]),2*PI);
                 dReal lowerlimit = joint.GetLowerLimit(0);

diff --git a/src/libopenrave/xmlreaders.cpp b/src/libopenrave/xmlreaders.cpp
@@ -129,16 +129,21 @@ bool TrajectoryReader::endElement(const std::string& name)
         }
     }
     else if( name == "data" ) {
+        std::string tmpDataStr;
         _vdata.resize(_spec.GetDOF()*_datacount);
         for(size_t i = 0; i < _vdata.size(); ++i) {
-            _ss >> _vdata[i];
-        }
-        if( !_ss ) {
-            throw OPENRAVE_EXCEPTION_FORMAT(_("failed reading %d numbers from trajectory <data> element"), _vdata.size(), ORE_Assert);
-        }
-        else {
-            _ptraj->Insert(_ptraj->GetNumWaypoints(),_vdata);
+            if( !(_ss >> _vdata[i]) ) {
+                _ss.clear(); // clear error state
+                _ss >> tmpDataStr;
+                if( tmpDataStr == "nan" ) {
+                    _vdata[i] = std::numeric_limits<dReal>::quiet_NaN();
+                }
+                else {
+                    throw OPENRAVE_EXCEPTION_FORMAT(_("failed reading %d numbers from trajectory <data> element"), _vdata.size(), ORE_Assert);
+                }
+            }
         }
+        _ptraj->Insert(_ptraj->GetNumWaypoints(),_vdata);
     }
     else if( name == "description" ) {
         _ptraj->SetDescription(_ss.str());