diff --git a/rocolib/api/composables/GraphComposable.py b/rocolib/api/composables/GraphComposable.py
index b788889f39e7091aa5728e04b5fdc25d4ce6545d..e7d79e401da6d62bcf76f8f3a97b709d29b72836 100644
--- a/rocolib/api/composables/GraphComposable.py
+++ b/rocolib/api/composables/GraphComposable.py
@@ -92,23 +92,17 @@ 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", "3D", self.toSTL, "model.stl", **kwargs)
if kw("display3D", False) or kw("png"):
from rocolib.utils.display import display3D
if basename:
if kw("stl"):
- try:
- stl_handle = open(basename+"model.stl", 'rb')
- except:
- log.info("No Stl found")
- return
+ stl_handle = open(basename+"model.stl", 'rb')
else:
log.info("STL wasn't selected, making now...")
- handle("png", "stl", self.to3D, None, format="stl")
+ handle("png", "3D", self.toSTL, None, **kwargs)
stl_handle = bufs['png']
stl_handle.seek(0)
if kw("png"):
diff --git a/rocolib/api/composables/graph/Graph.py b/rocolib/api/composables/graph/Graph.py
index e3c391abad6169aa91553a5257615b4e72822576..46dafa9dee591a9f348754930893ae94b37e2b53 100644
--- a/rocolib/api/composables/graph/Graph.py
+++ b/rocolib/api/composables/graph/Graph.py
@@ -2,9 +2,7 @@ import logging
from rocolib.api.composables.graph.HyperEdge import HyperEdge
from rocolib.utils.utils import prefix as prefixString
-from rocolib.utils.roco2sim.Node import ShapeNode
import rocolib.utils.numsym as np
-from rocolib.utils.roco2sim.format_3d import format_wrl
log = logging.getLogger(__name__)
@@ -25,78 +23,54 @@ def inflate(face, thickness=.1, edges=False):
return faces
+def STLWrite(faces, fp, **kwargs):
+ scale = .001 # roco units : mm ; STL units m
-def _triangulate(faces, **kwargs):
- """Create triangulated faces in 3D space with thickness"""
- scale = .001 # roco units : mm ; STL units m
+ from .stlwriter import ASCII_STL_Writer as STL_Writer
import triangle
- import numpy as np
-
- triangles_coord_index = []
- triangles_facet_normal = []
- facets_index = np.empty([1, 3])
- B_add = 0
- k = 0 # index initiated
+ shells = []
+ triangles = []
for i, f in enumerate(faces):
+ r = f[0]
+ A = f[1]
- r = f[0] # coord transformation matrix
- A = f[1] # vertices info
-
- facets_coord_index = []
- facets_normal = []
facets = []
B = triangle.triangulate(A, opts='p')
-
if not 'triangles' in B:
log.warning("No triangles in " + f[2])
continue
thickness = "thickness" in kwargs and kwargs["thickness"]
if thickness:
- for t in [np.transpose(np.array([list(B['vertices'][x]) + [0, 1] for x in (face[0], face[1], face[2])])) for
- face in B['triangles']]:
- for x in inflate(t, thickness=thickness):
- facets_coord_index.extend(np.transpose(np.dot(r, x) * scale))
- facets_normal.extend(np.transpose(np.dot(r, x) * scale))
- index_current = [[k, k + 1, k + 2]]
- k = k + 3
- facets_index = np.concatenate((facets_index, index_current), 0)
- for t in [np.transpose(np.array([list(A['vertices'][x]) + [0, 1] for x in (edge[0], edge[1])])) for edge in
- A['segments']]:
- for x in inflate(t, thickness=thickness, edges=True):
- facets_coord_index.extend(np.transpose(np.dot(r, x) * scale))
- facets_normal.extend(np.transpose(np.dot(r, x) * scale))
- index_current = [[k, k + 1, k + 2]]
- k = k + 3
- facets_index = np.concatenate((facets_index, index_current), 0)
+ for t in [np.transpose(np.array([list(B['vertices'][x]) + [0,1] for x in (face[0], face[1], face[2])])) for face in B['triangles']]:
+ facets.extend([np.dot(r, x) * scale for x in inflate(t, thickness=thickness)])
+ for t in [np.transpose(np.array([list(A['vertices'][x]) + [0,1] for x in (edge[0], edge[1])])) for edge in A['segments']]:
+ facets.extend([np.dot(r, x) * scale for x in inflate(t, thickness=thickness, edges=True)])
else:
- for t in [np.transpose(np.array([list(B['vertices'][x]) + [0, 1] for x in (face[0], face[1], face[2])])) for
- face in B['triangles']]:
+ for t in [np.transpose(np.array([list(B['vertices'][x]) + [0,1] for x in (face[0], face[1], face[2])])) for face in B['triangles']]:
facets.append(np.dot(r, t) * scale)
- facets_coord_index.extend(np.transpose(np.dot(r, t)) * scale)
- facets_normal.extend(np.transpose(np.dot(r, t)) * scale)
- #B_triangle = B['triangles'] + B_add
- #B_add = np.amax(B_triangle) + 1
- index_current = [[k, k + 1, k + 2]]
- k = k + 3
- facets_index = np.concatenate((facets_index, index_current), 0)
-
-
- triangles_coord_index.extend(facets_coord_index)
- triangles_facet_normal.extend(facets_normal)
-
- facets_index_return = np.delete(facets_index, 0, 0)
-
- return format_wrl(triangles_coord_index, triangles_facet_normal, facets_index_return)
+ triangles.extend(facets)
+ '''
+ # Output each face to its own STL file
+ if filename:
+ with open(filename.replace(".stl", "_%02d.stl" % i), 'wb') as fp:
+ writer = STL_Writer(fp)
+ writer.add_faces(facets)
+ writer.close()
+ '''
+
+ faces = triangles
+ writer = STL_Writer(fp)
+ writer.add_faces(faces)
+ writer.close()
class Graph():
def __init__(self):
self.faces = []
self.facelists = []
self.edges = []
- self.shapeNodes=None
def addFace(self, f, prefix=None, root=False, faceEdges=None, faceAngles=None, faceFlips=None):
if prefix:
@@ -329,42 +303,15 @@ class Graph():
e.pts2D = None
e.pts3D = None
- def toShapeNodes(self, **kwargs):
- """Create a shape node list"""
+ def toSTL(self, fp, **kwargs):
self.place()
- shapeNodes = []
- for i, facesInFacelists in enumerate(self.facelists):
- faces = []
- for f in facesInFacelists:
- if f.area > 0:
- faces.append([f.transform3D, f.getTriangleDict(), f.name])
- else:
- log.info(f"Omitting face {f.name} with area {f.area} from wbo")
- # Crate 1 shape node per set of faces.
- wrl = _triangulate(faces, **kwargs)
- shapeNodes.append(ShapeNode(f.name, wrl, f.transform3D))
-
- self.shapeNodes = shapeNodes
-
- def to3D(self, fp, **kwargs):
- """stl or webots expects fp instance from handle. if None, not creating it"""
- if self.shapeNodes is None:
- self.toShapeNodes(**kwargs)
-
- if kwargs.get('format') == 'stl':
- f = []
- for sn in self.shapeNodes:
- f.extend(sn.getSTL().face)
- from .stlwriter import ASCII_STL_Writer as STL_Writer
- writer = STL_Writer(fp)
- writer.add_faces(f)
- writer.close()
-
- if kwargs.get('format') == 'webots':
- from rocolib.utils.roco2sim.wbo_nodes import wboConverter
- fp.write(wboConverter(self.shapeNodes))
- fp.close()
-
+ stlFaces = []
+ for face in self.faces:
+ if face.area > 0:
+ stlFaces.append([face.transform3D, face.getTriangleDict(), face.name])
+ else:
+ log.info(f"Omitting face {face.name} with area {face.area} from STL")
+ return STLWrite(stlFaces, fp, **kwargs)
'''
@staticmethod
diff --git a/rocolib/utils/roco2sim/Node.py b/rocolib/utils/roco2sim/Node.py
deleted file mode 100644
index e905bde778aa66bb771aedc07af370c7e7114e7b..0000000000000000000000000000000000000000
--- a/rocolib/utils/roco2sim/Node.py
+++ /dev/null
@@ -1,141 +0,0 @@
-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]
- 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))
-
- 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/format_3d.py b/rocolib/utils/roco2sim/format_3d.py
deleted file mode 100644
index 78fa78ca5f5de3bfca6fb959bd8d6846bf1a25fc..0000000000000000000000000000000000000000
--- a/rocolib/utils/roco2sim/format_3d.py
+++ /dev/null
@@ -1,19 +0,0 @@
-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
deleted file mode 100644
index c80db6db9f599a8f9d587d4dc9a91aa893eb2a7a..0000000000000000000000000000000000000000
--- a/rocolib/utils/roco2sim/material_data.py
+++ /dev/null
@@ -1,27 +0,0 @@
-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
deleted file mode 100644
index ebc6a02517add235cc39f85601a4ab19847c232f..0000000000000000000000000000000000000000
--- a/rocolib/utils/roco2sim/wbo_nodes.py
+++ /dev/null
@@ -1,132 +0,0 @@
-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('.', '_')