diff --git a/rocolib/utils/roco2sim/Node.py b/rocolib/utils/roco2sim/Node.py
new file mode 100644
index 0000000000000000000000000000000000000000..b3ab00588956a234f06350e6c710723614cb6b2c
--- /dev/null
+++ b/rocolib/utils/roco2sim/Node.py
@@ -0,0 +1,147 @@
+import numpy as np
+
+from rocolib.utils.roco2sim.material_data import Material as mat, MaterialConst as mc
+from rocolib.utils.roco2sim.material_data import materialData as md
+from rocolib.utils.roco2sim.format_3d import format_wrl as wrl
+from rocolib.utils.roco2sim.format_3d import wrl2stl
+
+"""BaseNodes (No children allowed)"""
+
+class AppearanceNode:
+ """appearance"""
+
+ def __init__(self, material: mat = md[mc.MAT_DEFAULT]):
+ # if no material specified, use default material
+ self._material = material
+
+ def getMaterial(self):
+ return self._material
+
+ def getRGB(self):
+ return self._material.rgb
+
+ def getRoughness(self):
+ return self._material.roughness
+
+ def getReflection(self):
+ return self._material.reflection
+
+ def getName(self):
+ return self._material.name
+
+ def getDescription(self):
+ return self._material.description
+
+
+class PhysicsNode:
+ def __init__(self, material: mat = md[mc.MAT_DEFAULT], mass=0.3, contact=None):
+ self.material = material
+ self.mass = mass
+ self.contact = contact
+
+ def isMassSpecified(self):
+ return self.mass is not None
+
+
+
+class DeviceNode:
+ def __int__(self, name=None, type=None, options=None):
+ if options is None:
+ options = {}
+ self.options = options
+ self.name = name
+ self.type = type
+
+
+"""Composable Nodes (can have children)"""
+class BaseComposableNode:
+ def __init__(self, name, children=None):
+ self.name = name
+ if children is None:
+ children = []
+ self.children = children
+
+
+class ShapeNode(BaseComposableNode):
+ """Shape node holds faces and appearance information.
+ If solidified, then includes physics"""
+
+ def __init__(self, name, wrl: wrl, trans, appearance: AppearanceNode = None, solidify: PhysicsNode = None,
+ children=None):
+ super().__init__(name, children)
+ self.trans = trans
+ self.wrl = wrl
+ self.solid = False
+
+
+ # if no appearance specified, default
+ if appearance is None:
+ self.appearance = AppearanceNode()
+ else:
+ self.appearance = appearance
+
+ # physics is None, then inherit, if Physics node is given, then use it.
+ # otherwise not physics
+ self.physics = PhysicsNode
+ if solidify is None:
+ self.solidify()
+ elif isinstance(solidify, PhysicsNode):
+ self.solidifyWithNewPhysics(solidify)
+
+ def solidify(self):
+ """Add physics property to this shape based on appearance material"""
+ self.physics = PhysicsNode(self.appearance.getMaterial())
+ self.solid = True
+
+ def solidifyWithNewPhysics(self, physics: PhysicsNode):
+ """Add physics property to this shape by specified physics nodes"""
+ self.physics = physics
+ self.solid = True
+
+ def gasify(self):
+ self.solid = False
+
+ def isSolid(self):
+ return self.solid
+
+ def getXyz(self):
+ return self.trans
+ def getRotationVector(self):
+ R = self.trans[0:3,0:3]
+ 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
+
+ def getWRL(self):
+ return self.wrl
+
+ def getSTL(self):
+ return wrl2stl(self.wrl)
+
+ # def getFaces(self):
+ # return self.faces
+ # def getAppearance(self):
+ # return self.appearance
+
+
+class ModuleNode(BaseComposableNode):
+ def __init__(self, name, shape: ShapeNode, device: DeviceNode = None, children=None):
+ super().__init__(name, children)
+ self.shape = shape
+ self.device = device
+
+ def getDeviceOptions(self, optionName):
+ if optionName in self.device.options.items():
+ return self.device.options[optionName]
+ return ""
+
+ def getPhysicsNode(self):
+ return self.shape.physics
diff --git a/rocolib/utils/roco2sim/__init__.py b/rocolib/utils/roco2sim/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/rocolib/utils/roco2sim/format_3d.py b/rocolib/utils/roco2sim/format_3d.py
new file mode 100644
index 0000000000000000000000000000000000000000..78fa78ca5f5de3bfca6fb959bd8d6846bf1a25fc
--- /dev/null
+++ b/rocolib/utils/roco2sim/format_3d.py
@@ -0,0 +1,19 @@
+from dataclasses import dataclass
+import numpy as np
+
+@dataclass
+class format_wrl:
+ coord: list
+ normal: list
+ index: list
+
+@dataclass
+class format_stl:
+ face: list
+
+def wrl2stl(wrl: format_wrl):
+ faces = []
+ for (index) in wrl.index:
+ faces.append(np.transpose(np.vstack((wrl.coord[int(index[0])],wrl.coord[int(index[1])],wrl.coord[int(index[2])]))))
+
+ return format_stl(faces)
diff --git a/rocolib/utils/roco2sim/material_data.py b/rocolib/utils/roco2sim/material_data.py
new file mode 100644
index 0000000000000000000000000000000000000000..c80db6db9f599a8f9d587d4dc9a91aa893eb2a7a
--- /dev/null
+++ b/rocolib/utils/roco2sim/material_data.py
@@ -0,0 +1,27 @@
+from dataclasses import dataclass, field
+import numpy as np
+
+@dataclass
+class Material:
+ name: str
+ density: float = 1 # kg/m3
+ rgb: np.ndarray = np.array([0, 0, 0])
+ roughness: float = 1.0
+ reflection: float = 0.
+ optional: dict = field(default_factory=dict)
+ description: str = """"""
+
+@dataclass(frozen=True)
+class MaterialConst:
+ MAT_DEFAULT: str = 'defaultMaterial'
+
+
+
+mc = MaterialConst()
+materialData = {}
+
+materialData[mc.MAT_DEFAULT] \
+ = Material('generic_material', description="""This is a default material.""")
+
+materialData['paperThick'] \
+ = Material('Thick Paper', 833.3, description="""Business card paper""")
diff --git a/rocolib/utils/roco2sim/wbo_nodes.py b/rocolib/utils/roco2sim/wbo_nodes.py
new file mode 100644
index 0000000000000000000000000000000000000000..ebc6a02517add235cc39f85601a4ab19847c232f
--- /dev/null
+++ b/rocolib/utils/roco2sim/wbo_nodes.py
@@ -0,0 +1,132 @@
+from typing import List
+from rocolib.utils.roco2sim.Node import *
+
+
+scale = 0.001
+
+class WrlFormatter:
+ def __init__(self):
+ self.COORD_FACET = """{face[0]:g} {face[1]:g} {face[2]:g}, """
+ 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.astype(float)+0))
+ 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 wboShape(sNode: ShapeNode):
+ appearance = wboAppearance(sNode.appearance)
+ name = wboName(sNode.name)
+ geometry = wboGeometryIndexedFaceSet(sNode)
+ node = """DEF %(name)s Shape {%(appearance)s %(geometry)s}""" % locals()
+ return node
+
+
+def wboGeometryIndexedFaceSet(sNode: ShapeNode):
+ wrl = sNode.wrl
+ if not wrl.coord:
+ return ""
+
+ wf = WrlFormatter()
+ normal = wf.coordPointsFormatter(wrl.normal)
+ points = wf.coordPointsFormatter(wrl.coord)
+ coordIndex = wf.coordIndexFormatter(wrl.index)
+
+ node = """geometry IndexedFaceSet {coord Coordinate {point [%(points)s]} normal Normal {vector [%(normal)s]} coordIndex [%(coordIndex)s]}""" % locals()
+ return node
+
+
+def wboAppearance(aNode: AppearanceNode):
+ baseColor = str(aNode.getRGB()).strip('[ ]')
+ roughness = str(aNode.getRoughness())
+ metalness = str(aNode.getReflection())
+ name = wboName(aNode.getName())
+ node = """appearance PBRAppearance { baseColor %(baseColor)s roughness %(roughness)s metalness %(metalness)s name "%(name)s"}""" % locals()
+ return node
+
+
+def wboPhysics(pNode: PhysicsNode):
+ if pNode is None:
+ return """ """
+ density = pNode.material.density
+ mass = pNode.mass
+ node = """physics Physics {density %(density)s mass %(mass)s}""" % locals()
+ return node
+
+
+def wboXyz(sNode: ShapeNode):
+ return str(scale * np.array(sNode.getXyz()).astype(int)[0:3, 3]).strip('[ ]')
+
+
+def wboSolid(sNode: ShapeNode):
+ if not sNode.isSolid():
+ return wboShape(sNode)
+ shape = wboShape(sNode)
+ contactMaterial = wboContact(sNode)
+ physics = wboPhysics(sNode.getPhysicsNode())
+ name = wboName(sNode.name)
+ node = """DEF %(name)s Solid {children [%(shape)s] %(contactMaterial)s boundingObject USE %(name)s %(physics)s}""" % locals()
+ return node
+
+def wboContact(sNode: ShapeNode):
+ contactMaterial = sNode.getPhysicsNode().contact
+ if contactMaterial is None:
+ return """contactMaterial "default" """
+ return """contactMaterial "%(contactMaterial)s" """
+
+
+def wboRobot(mNode: ShapeNode):
+ # create local var to fill the nodes
+ name = wboName(mNode.name)
+ mNode.gasify()
+ solid = wboSolid(mNode)
+ mNode.children.append(solid)
+ childrenNode = " ".join(mNode.children)
+ physics = wboPhysics(mNode.getPhysicsNode())
+ contactMaterial = wboContact(mNode)
+ rotation = str(mNode.getRotationVector()).strip('[ ]')
+ # battery = mNode.getDeviceOptions('battery')
+ # cpuConsumption = mNode.getDeviceOptions('cpuConsumption')
+ node = """DEF %(name)s Robot {
+ rotation %(rotation)s
+ children [%(childrenNode)s]
+ name "%(name)s"
+ %(contactMaterial)s
+ boundingObject USE %(name)s
+ %(physics)s
+ controller "%(name)s"
+ supervisor TRUE
+}""" % locals()
+ return node
+
+
+def wboHeader(version="R2021b"):
+ return """#VRML_OBJ R2021b utf8
+"""
+
+
+def wboConverter(sNodes: List[ShapeNode]):
+ robot = sNodes[0]
+ for sNode in sNodes[1:len(sNodes)]:
+ robot.children.append(wboSolid(sNode))
+ node = wboHeader() + wboRobot(robot)
+ return node
+
+
+def wboName(name):
+ return name.replace('.', '_')