Merge pull request #1486 from rdiankov/checkValidityOfMesh

Check validity of mesh indices
rdiankov · Jan 31, 2025 · c467dc7 · c467dc7
2 parents deffcfc + 3b6804a
commit c467dc7
Show file tree

Hide file tree

Showing 3 changed files with 45 additions and 1 deletion.
diff --git a/python/databases/convexdecomposition.py b/python/databases/convexdecomposition.py
@@ -260,7 +260,6 @@ def setrobot(self):
                         link.GetGeometries()[ig].SetCollisionMesh(self.GenerateTrimeshFromHulls(hulls))
 
     def getfilename(self,read=False):
-        filename = 'convexdecomposition_%.3f.pp'%self._padding
         return RaveFindDatabaseFile(os.path.join('robot.'+self.robot.GetKinematicsGeometryHash(), 'convexdecomposition_%.3f.pp'%self._padding),read)
 
     def autogenerate(self,options=None):
@@ -571,6 +570,11 @@ def GetGeometryInfosFromLink(self,ilink,preservetransform=False,color=None):
                     ginfo._meshcollision.indices[ioffset:(ioffset+len(hull[1])),:] = hull[1]+voffset
                     voffset += len(hull[0])
                     ioffset += len(hull[1])
+                # check
+                nMaxMeshIndex = len(ginfo._meshcollision.vertices) - 1
+                for meshIndex in ginfo._meshcollision.indices:
+                    assert numpy.all(meshIndex >= 0) and numpy.all(meshIndex <= nMaxMeshIndex), 'env=%s, geometry(name=\"%s\";id=\"%s\";type=%d) has incorrect mesh indices %r in convexhull, which is out of range of vertices which size is %d.' % (self.robot.GetEnv().GetNameId(), geometries[ig].GetName(), geometries[ig].GetId(), geometries[ig].GetType(), meshIndex, len(ginfo._meshcollision.vertices))
+                # append
                 geometryinfos.append(ginfo)
             return geometryinfos
 

diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp
@@ -100,6 +100,22 @@ class CallFunctionAtDestructor
     boost::function<void()> _fn;
 };
 
+/// \brief check validity of mesh collision indices. if invalid, throw.
+/// \param[in] vertices, indices : coming from TriMesh
+/// \param[in] name, id, type : coming from GeometryInfo. used for exception message.
+/// \param[in] context : used for exception message.
+static void _CheckValidityOfMeshCollisionIndices(const std::vector<Vector>& vertices,
+                                                 const std::vector<int32_t>& indices,
+                                                 const std::string& name, const std::string& id, const GeometryType type, const char* context)
+{
+    for(const int32_t meshIndex : indices) {
+        if( (int)vertices.size() <= meshIndex || meshIndex < 0 ) {
+            throw OPENRAVE_EXCEPTION_FORMAT(_("geometry(name=\"%s\";id=\"%s\";type=%d) has incorrect mesh indices %d in \"%s\", which is out of range of vertices which size is %d."),
+                                            name%id%(int)type%context%meshIndex%vertices.size(), ORE_InvalidArguments);
+        }
+    }
+}
+
 typedef boost::shared_ptr<ChangeCallbackData> ChangeCallbackDataPtr;
 
 bool KinBody::KinBodyInfo::operator==(const KinBodyInfo& other) const {
@@ -714,6 +730,12 @@ void KinBody::SetLinkGeometriesFromGroup(const std::string& geomname, const bool
 void KinBody::SetLinkGroupGeometries(const std::string& geomname, const std::vector< std::vector<KinBody::GeometryInfoPtr> >& linkgeometries)
 {
     OPENRAVE_ASSERT_OP( linkgeometries.size(), ==, _veclinks.size() );
+    for(const std::vector<KinBody::GeometryInfoPtr>& vGeometries : linkgeometries) {
+        for(const KinBody::GeometryInfoPtr& pGeometry : vGeometries) {
+            const KinBody::GeometryInfo& geometry = *pGeometry;
+            _CheckValidityOfMeshCollisionIndices(geometry._meshcollision.vertices, geometry._meshcollision.indices, geometry._name, geometry._id, geometry._type, __FUNCTION__);
+        }
+    }
     FOREACH(itlink, _veclinks) {
         Link& link = **itlink;
         std::map< std::string, std::vector<KinBody::GeometryInfoPtr> >::iterator it = link._info._mapExtraGeometries.insert(make_pair(geomname,std::vector<KinBody::GeometryInfoPtr>())).first;

diff --git a/src/libopenrave/kinbodygeometry.cpp b/src/libopenrave/kinbodygeometry.cpp
@@ -270,6 +270,22 @@ static void AppendCylinderTriangulation(const Vector& pos, const dReal radius, c
     return AppendConicalFrustumTriangulation(pos, radius, radius, halfHeight, numverts, tri);
 }
 
+/// \brief check validity of mesh collision indices. if invalid, throw.
+/// \param[in] vertices, indices : coming from TriMesh
+/// \param[in] name, id, type : coming from GeometryInfo. used for exception message.
+/// \param[in] context : used for exception message.
+static void _CheckValidityOfMeshCollisionIndices(const std::vector<Vector>& vertices,
+                                                 const std::vector<int32_t>& indices,
+                                                 const std::string& name, const std::string& id, const GeometryType type, const char* context)
+{
+    for(const int32_t meshIndex : indices) {
+        if( (int)vertices.size() <= meshIndex || meshIndex < 0 ) {
+            throw OPENRAVE_EXCEPTION_FORMAT(_("geometry(name=\"%s\";id=\"%s\";type=%d) has incorrect mesh indices %d in \"%s\", which is out of range of vertices which size is %d."),
+                                            name%id%(int)type%context%meshIndex%vertices.size(), ORE_InvalidArguments);
+        }
+    }
+}
+
 void KinBody::GeometryInfo::GenerateCalibrationBoardDotMesh(TriMesh& tri, float fTessellation) const
 {
     // reset dots mesh
@@ -1645,6 +1661,8 @@ void KinBody::GeometryInfo::DeserializeJSON(const rapidjson::Value &value, const
     orjson::LoadJsonValueByKey(value, "diffuseColor", _vDiffuseColor);
     orjson::LoadJsonValueByKey(value, "ambientColor", _vAmbientColor);
     orjson::LoadJsonValueByKey(value, "modifiable", _bModifiable);
+
+    _CheckValidityOfMeshCollisionIndices( _meshcollision.vertices,  _meshcollision.indices, _name, _id, _type, __FUNCTION__);
 }
 
 inline void _UpdateExtrema(const Vector& v, Vector& vmin, Vector& vmax)