161 lines
No EOL
7.3 KiB
Python
161 lines
No EOL
7.3 KiB
Python
import os
|
|
from helper.fs import FS
|
|
|
|
from src.model.sdf_join import SdfJoin
|
|
import typing
|
|
import uuid
|
|
|
|
|
|
def from_str(x):
|
|
assert isinstance(x, str)
|
|
return x
|
|
|
|
|
|
def from_none(x):
|
|
assert x is None
|
|
return x
|
|
|
|
|
|
def from_union(fs, x):
|
|
for f in fs:
|
|
try:
|
|
return f(x)
|
|
except:
|
|
pass
|
|
assert False
|
|
|
|
|
|
def to_class(c, x):
|
|
assert isinstance(x, c)
|
|
return x.to_dict()
|
|
|
|
|
|
class SdfGeometryModel:
|
|
def __init__(self, name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, link, friction):
|
|
self.name = name
|
|
self.ixx = ixx
|
|
self.ixy = ixy
|
|
self.ixz = ixz
|
|
self.iyy = iyy
|
|
self.izz = izz
|
|
self.massSDF = massSDF
|
|
self.posX = posX
|
|
self.posY = posY
|
|
self.posZ = posZ
|
|
self.eulerX = eulerX
|
|
self.eulerY = eulerY
|
|
self.eulerZ = eulerZ
|
|
self.iyz = iyz
|
|
self.stl = stl
|
|
self.link = link
|
|
self.friction = friction
|
|
|
|
@staticmethod
|
|
def from_dict(obj):
|
|
assert isinstance(obj, dict)
|
|
name = from_union([from_str, from_none], obj.get("name"))
|
|
ixx = from_union([from_str, from_none], obj.get("ixx"))
|
|
ixy = from_union([from_str, from_none], obj.get("ixy"))
|
|
ixz = from_union([from_str, from_none], obj.get("ixz"))
|
|
iyy = from_union([from_str, from_none], obj.get("iyy"))
|
|
izz = from_union([from_str, from_none], obj.get("izz"))
|
|
massSDF = from_union([from_str, from_none], obj.get("massSDF"))
|
|
posX = from_union([from_str, from_none], obj.get("posX"))
|
|
posY = from_union([from_str, from_none], obj.get("posY"))
|
|
posZ = from_union([from_str, from_none], obj.get("posZ"))
|
|
eulerX = from_union([from_str, from_none], obj.get("eulerX"))
|
|
eulerY = from_union([from_str, from_none], obj.get("eulerY"))
|
|
eulerZ = from_union([from_str, from_none], obj.get("eulerZ"))
|
|
iyz = from_union([from_str, from_none], obj.get("iyz"))
|
|
stl = from_union([from_str, from_none], obj.get("stl"))
|
|
link = from_union([from_str, from_none], obj.get('link'))
|
|
friction = from_union([from_str, from_none], obj.get("friction"))
|
|
|
|
return SdfGeometryModel(name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, link, friction)
|
|
|
|
def to_dict(self):
|
|
result = {}
|
|
if self.name is not None:
|
|
result["name"] = from_union([from_str, from_none], self.name)
|
|
if self.ixx is not None:
|
|
result["ixx"] = from_union([from_str, from_none], self.ixx)
|
|
if self.ixy is not None:
|
|
result["ixy"] = from_union([from_str, from_none], self.ixy)
|
|
if self.ixz is not None:
|
|
result["ixz"] = from_union([from_str, from_none], self.ixz)
|
|
if self.iyy is not None:
|
|
result["iyy"] = from_union([from_str, from_none], self.iyy)
|
|
if self.izz is not None:
|
|
result["izz"] = from_union([from_str, from_none], self.izz)
|
|
if self.massSDF is not None:
|
|
result["massSDF"] = from_union([from_str, from_none], self.massSDF)
|
|
if self.posX is not None:
|
|
result["posX"] = from_union([from_str, from_none], self.posX)
|
|
if self.posY is not None:
|
|
result["posY"] = from_union([from_str, from_none], self.posY)
|
|
if self.posZ is not None:
|
|
result["posZ"] = from_union([from_str, from_none], self.posZ)
|
|
if self.eulerX is not None:
|
|
result["eulerX"] = from_union([from_str, from_none], self.eulerX)
|
|
if self.eulerY is not None:
|
|
result["eulerY"] = from_union([from_str, from_none], self.eulerY)
|
|
if self.eulerZ is not None:
|
|
result["eulerZ"] = from_union([from_str, from_none], self.eulerZ)
|
|
if self.iyz is not None:
|
|
result["iyz"] = from_union([from_str, from_none], self.iyz)
|
|
if self.stl is not None:
|
|
result["stl"] = from_union([from_str, from_none], self.stl)
|
|
if self.link is not None:
|
|
result['link'] = from_union([from_str, from_none], self.link)
|
|
if self.friction is not None:
|
|
result["friction"] = from_union([from_str, from_none], self.eulerZ)
|
|
return result
|
|
|
|
def toJSON(self) -> str:
|
|
return str(self.to_dict()).replace('\'', '"')
|
|
|
|
def toSDF(self):
|
|
return FS.readFile(os.path.dirname(os.path.realpath(__file__))
|
|
+ '/../../mocks/sdf/model.sdf').replace('{name}', self.name,).replace('{posX}', self.posX).replace('{posY}', self.posY).replace('{posZ}', self.posZ).replace('{eulerX}', self.eulerX).replace('{eulerY}', self.eulerY).replace('{eulerZ}', self.eulerZ).replace('{ixx}', self.ixx).replace('{ixy}', self.ixy).replace('{ixz}', self.ixz).replace('{iyy}', self.iyy).replace('{iyz}', self.iyz).replace('{izz}', self.izz).replace('{massSDF}', self.massSDF,).replace('{stl}', self.stl).replace('{friction}',self.friction)
|
|
|
|
def toSdfLink(self):
|
|
|
|
return FS.readFile(os.path.dirname(os.path.realpath(__file__))
|
|
+ '/../../mocks/sdf/link.sdf').replace('{name}', self.name,).replace('{posX}', self.posX).replace('{posY}', self.posY).replace('{posZ}', self.posZ).replace('{eulerX}', self.eulerX).replace('{eulerY}', self.eulerY).replace('{eulerZ}', self.eulerZ).replace('{ixx}', self.ixx).replace('{ixy}', self.ixy).replace('{ixz}', self.ixz).replace('{iyy}', self.iyy).replace('{iyz}', self.iyz).replace('{izz}', self.izz).replace('{massSDF}', self.massSDF,).replace('{stl}', self.stl).replace('{friction}',self.friction)
|
|
def includeLink(self, pose = False):
|
|
if(pose == False):
|
|
return FS.readFile(os.path.dirname(os.path.realpath(__file__))
|
|
+ '/../../mocks/sdf/include.sdf').replace('{name}', self.name).replace('{uri}', '/' + self.name)
|
|
return FS.readFile(os.path.dirname(os.path.realpath(__file__))
|
|
+ '/../../mocks/sdf/include_pose.sdf').replace('{name}', self.name).replace('{uri}', '/' + self.name).replace('{posX}', self.posX).replace('{posY}', self.posY).replace('{posZ}', self.posZ).replace('{eulerX}', self.eulerX).replace('{eulerY}', self.eulerY).replace('{eulerZ}', self.eulerZ).replace('{ixx}', self.ixx).replace('{ixy}', self.ixy).replace('{ixz}', self.ixz).replace('{iyy}', self.iyy).replace('{iyz}', self.iyz).replace('{izz}', self.izz)
|
|
|
|
def generateSDFatJoinFixed(self, sdfModels: list['SdfGeometryModel']):
|
|
sdf = '\n<model name="assembly">\n'
|
|
sdf+= ' <link name="base_link">\n'
|
|
sdf += " <pose>0 0 0 0 0 0</pose>\n"
|
|
sdf+= " </link>\n"
|
|
|
|
link = sdf + self.includeLink(pose=True)
|
|
if sdfModels.__len__() == 0:
|
|
return link
|
|
endTagLinkInc = link.__len__()
|
|
beginSDF = link[0: endTagLinkInc]
|
|
|
|
|
|
sdfJoin = beginSDF + '\n'
|
|
|
|
for el in sdfModels:
|
|
if el.name != self.name:
|
|
sdfJoin += el.includeLink(pose=True) + '\n'
|
|
|
|
endSDF = link[endTagLinkInc:link.__len__()]
|
|
|
|
for el in sdfModels:
|
|
if el.name != self.name:
|
|
sdfJoin += SdfJoin(name=str(uuid.uuid4()),
|
|
parent=self.name, child=el.name,modelAt=el).toSDF() + '\n'
|
|
|
|
sdfJoin += endSDF
|
|
sdfJoin += '</model>'
|
|
return sdfJoin
|
|
|