including roco2sim

rocolib/utils/roco2sim/Node.py

+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

rocolib/utils/roco2sim/format_3d.py

+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)

rocolib/utils/roco2sim/material_data.py

+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""")

rocolib/utils/roco2sim/wbo_nodes.py

+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('.', '_')