Merge branch 'main' into 'main'

Pytest Fail fix

See merge request !10

rocolib/api/composables/GraphComposable.py

@@ -92,8 +92,8 @@ class GraphComposable(Composable, BaseGraph):
     handle("silhouette", "Silhouette cut-and-fold", d.toDXF, "silhouette.dxf", mode="silhouette")
     handle("autofolding", "autofolding", d.toDXF, "autofold-default.dxf", mode="autofold")
     handle("autofolding", "  -- (graph)", d.toDXF, "autofold-graph.dxf")
-    handle("3D", "stl", self.to3D, "model.stl", format="stl", **kwargs)
-    handle("3D", "webots", self.to3D, "model.wbo", format="webots", **kwargs)
+    handle("stl", "stl", self.to3D, "model.stl", format="stl", **kwargs)
+    handle("webots", "webots", self.to3D, "model.wbo", format="webots", **kwargs)
 
 
     if kw("display3D", False) or kw("png"):

rocolib/api/composables/graph/Graph.py

@@ -363,7 +363,6 @@ class Graph():
     if kwargs.get('format') == 'webots':
         from rocolib.utils.roco2sim.wbo_nodes import wboConverter
         fp.write(wboConverter(self.shapeNodes))
-        fp.close()
 
 
   '''

rocolib/utils/roco2sim/Node.py

@@ -108,8 +108,14 @@ class ShapeNode(BaseComposableNode):
         return self.trans
     def getRotationVector(self):
         R = self.trans[0:3,0:3]
-        v = np.array([R[2,1]-R[1,2],R[0,2]-R[2,0],R[1,0]-R[0,1]])
-        return np.append(v/np.linalg.norm(v), np.arccos((np.trace(R)-1)/2))
+        if np.array_equal(R, np.identity(3)):
+            rVector = np.array([0,0,0,0])
+        else:
+            v = np.array([R[2,1]-R[1,2],R[0,2]-R[2,0],R[1,0]-R[0,1]])
+            rVector = np.append(v/np.linalg.norm(v), np.arccos((np.trace(R)-1)/2))
+
+        return rVector
+
 
     def getPhysicsNode(self):
         return self.physics