.wbo output added

.idea/Pycharm.iml

@@ -4,7 +4,7 @@
     <content url="file://$MODULE_DIR$">
       <excludeFolder url="file://$MODULE_DIR$/venv" />
     </content>
-    <orderEntry type="jdk" jdkName="Python 3.8 (Pycharm)" jdkType="Python SDK" />
+    <orderEntry type="jdk" jdkName="Python 3.8 (Roco)" jdkType="Python SDK" />
     <orderEntry type="sourceFolder" forTests="false" />
   </component>
 </module>
\ No newline at end of file

.idea/misc.xml

 <?xml version="1.0" encoding="UTF-8"?>
 <project version="4">
-  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.8 (Pycharm)" project-jdk-type="Python SDK" />
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.8 (Roco)" project-jdk-type="Python SDK" />
 </project>
\ No newline at end of file

TestPaperbot.py

@@ -5,7 +5,7 @@ a = getComponent("Paperbot", length=85, width=65, height=30)
 #d1 = getComponent("Die")
 #d1.makeOutput("output/die50mm", tree=False, display=False)
 
-a.makeOutput("output/Paperbot_Whole", thickness=1)
+a.makeOutput("output/Paperbot_Whole", thickness=1, tree=False, display=False)
 
 
 # =============================================================================

rocolib/api/composables/GraphComposable.py

@@ -148,8 +148,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("stl", "3D", self.toSTL, "model.stl", **kwargs)
-    handle("wbo", "3D", self.toWBO, "model.txt", **kwargs)
+    #handle("stl", "3D", self.toSTL, "model.stl", **kwargs)
+    handle("wbo", "3D", self.toWBO, "model.wbo", **kwargs)
 
 
 

rocolib/api/composables/graph/TestWbo.py

+from .WboExporter import WboNodeGenerator as wng
+import WboExporter as we
+
+results = wng.addShape("test",str("0,0,0").replace(',', ' '), "0 1 0", str("0,0").replace(',', ' '))
+print(results)

rocolib/api/composables/graph/WboExporter.py

+
+
+import numpy as np
+
+class WboRobotGenerator:
+
+    def __init__(self, fp, nameRobot="robot", nameController="controller"):
+        # dictionary to store nodes string
+        # node name: shape node structure string
+        self.robotShapes = {nameRobot: None}
+        self.servoShapes = {}
+        self.tireShapes = {}
+        self.transform3D = {}
+        self.nameRobot = nameRobot
+        self.nameController = nameController
+        self.fp = fp
+
+    def saveShapebyName(self, name, shape):
+        if 'servo' in name:
+            self.servoShapes[name.replace('.','_')] = shape
+        elif 'tire' in name:
+            self.tireShapes[name.replace('.','_')] = shape
+            #otherwise
+        elif self.robotShapes[self.nameRobot] is None:
+            self.robotShapes[self.nameRobot] = shape
+        else:
+            self.robotShapes[name.replace('.','_')] = shape
+
+
+    def exportRobotWbo(self, node):
+        self.fp.write(node)
+
+    def constractRobot(self):
+        robotChildrenNodes = []
+
+        # Create children list of joints with matching servo and tire
+        for servoNameKey, servoShapeStr in self.servoShapes.items():
+            upperTreeName = servoNameKey.split('_drive_servo_')[0]
+            for tireNameKey, tireShapeStr in self.tireShapes.items():
+                if upperTreeName in tireNameKey.split('_tire_'):
+                    robotChildrenNodes.append(self.generateHingeJointNode(servoNameKey, tireNameKey))
+                    break
+
+        for shapeName, shape in self.robotShapes.items():
+            #for The main body no solid added. Only shape in the children
+            if shapeName is self.nameRobot:
+                continue
+            #other parts are added with solid
+            robotChildrenNodes.append(self.generateSolidNode(shapeName, shape))
+
+        #generate robot node and export
+        self.exportRobotWbo(self.generateRobotNode(robotChildrenNodes))
+
+    def generateSolidNode(self, name, shape):
+
+        return """DEF %(name)s_solid Solid {
+                children %(shape)s
+                boundingObject USE %(nameRobot)s
+                physics Physics {
+                }
+                }""" %locals()
+
+    def generateRobotNode(self, children):
+        # create local var to fill the nodes
+        nameRobot = self.nameRobot
+        nameController = self.nameController
+
+        return """DEF %(nameRobot)s Robot {
+            children %(children)s
+            boundingObject USE %(nameRobot)s
+            physics Physics {
+                density -1
+                mass 2
+            }
+            controller "%(nameController)s"
+            supervisor TRUE
+        }"""  %locals()
+
+    def generateHingeJointNode(self, servoName, tireName):
+
+        tireShape = self.tireShapes[tireName]
+        return """
+                DEF %(servoName)s_hinge HingeJoint {
+                  jointParameters HingeJointParameters {
+                    anchor %(trans)s
+                  }
+                  device [
+                    RotationalMotor {
+                      name "%(servoName)s"
+                      controlPID 0.01 0 0
+                      maxVelocity 25
+                    }
+                  ]
+                  endPoint Solid {
+                    translation %(trans)s
+                    rotation %(rotate)s
+                    children [%(tireShape)s]
+                    name "%(tireName)s"
+                    physics Physics {
+                      density -1
+                      mass 3
+                    }
+                  }
+                }
+                """
+
+
+
+
+
+
+class WboShapeGenerator:
+    # """
+    # WboExporter is a class to generate wbo file for webots simulations
+    # """
+
+    def __init__(self):
+        self.COORD_FACET = """{face[0]:.4f} {face[1]:.4f} {face[2]:.4f}, """
+        self.COORD_FACET_INDEX = """{face_pt_num[0]}, {face_pt_num[1]}, {face_pt_num[2]}, -1, """
+        self.FACET_NORMAL = """0 0 0, """
+
+    def _coordPointsFormatter(self, p):
+        self.coordPoints = []
+        for x in p:
+            self.coordPoints.append(self.COORD_FACET.format(face=x))
+        return ''.join(self.coordPoints).strip(', ')
+
+    def _normalFormatter(self,n):
+        self.normal = []
+        for x in n:
+            self.normal.append(self.FACET_NORMAL.format(face=x))
+        return ''.join(self.normal).strip(', ')
+
+    def _coordIndexFormatter(self,c):
+        self.coordIndex = []
+        for x in c:
+
+            self.coordIndex.append(self.COORD_FACET_INDEX.format(face_pt_num = [int(y) for y in x]))
+        return ''.join(self.coordIndex).strip(', ')
+
+    def formShape(self, name, p, n, c):
+        #points = " ".join(map(str, p))
+        #coordIndex = ", ".join(map(str, [int(x) for x in c]))
+        #normal_list = []
+        #for i, v in enumerate(n):
+        #    normal_list.append('0 0 0')
+        #normal = str(normal_list).replace(r"""'""",' ')
+        points = self._coordPointsFormatter(p)
+        normal = self._normalFormatter(n)
+        coordIndex = self._coordIndexFormatter(c)
+        name = name.replace('.','_')
+
+        return ("""DEF %(name)s Shape {
+                appearance PBRAppearance {
+                baseColor 0.0614481 0 1
+                roughness 1
+                metalness 0
+                name "DefaultMaterial"
+                }
+                  geometry IndexedFaceSet {
+                    coord Coordinate {
+                      point [
+                        %(points)s
+                      ]
+                    }
+                    normal Normal {
+                      vector [
+                        %(normal)s
+                        ]
+                    }
+                coordIndex 
+                      [
+                        %(coordIndex)s
+                     ]
+                  }
+                }""" % locals())