From 502e7a8d76124d08527abb45418f852a0a7178f0 Mon Sep 17 00:00:00 2001 From: IDONTSUDO Date: Wed, 12 Jul 2023 17:40:24 +0300 Subject: [PATCH] adding center mass URDF --- asp/mocks/urdf/model.urdf | 31 ++++++++++++------- asp/src/model/sdf_geometry.py | 23 +++++++++++--- cad_generation/model/sdf_geometry_model.py | 21 ++++++++++--- .../usecases/get_sdf_geometry_usecase.py | 9 ++++-- 4 files changed, 60 insertions(+), 24 deletions(-) diff --git a/asp/mocks/urdf/model.urdf b/asp/mocks/urdf/model.urdf index 8c210af..56e19d8 100644 --- a/asp/mocks/urdf/model.urdf +++ b/asp/mocks/urdf/model.urdf @@ -1,30 +1,37 @@ - + - - - - - + + + + + - - - + + + - + - + - + + + + 0.2 + 0.1 + 1 0 0 + + \ No newline at end of file diff --git a/asp/src/model/sdf_geometry.py b/asp/src/model/sdf_geometry.py index 055b859..08b7e44 100644 --- a/asp/src/model/sdf_geometry.py +++ b/asp/src/model/sdf_geometry.py @@ -31,7 +31,7 @@ def to_class(c, x): class GeometryModel: - def __init__(self, name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, link, friction): + def __init__(self, name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, link, friction, centerMassX, centerMassY, centerMassZ): self.name = name self.ixx = ixx self.ixy = ixy @@ -49,6 +49,9 @@ class GeometryModel: self.stl = stl self.link = link self.friction = friction + self.centerMassX = centerMassX + self.centerMassY = centerMassY + self.centerMassZ = centerMassZ @staticmethod def from_dict(obj): @@ -70,8 +73,10 @@ class GeometryModel: 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 GeometryModel(name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, link, friction) + centerMassX = from_union([from_str, from_none], obj.get("centerMassX")) + centerMassY = from_union([from_str, from_none], obj.get("centerMassY")) + centerMassZ = from_union([from_str, from_none], obj.get("centerMassZ")) + return GeometryModel(name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, link, friction, centerMassX, centerMassY, centerMassZ) def to_dict(self): result = {} @@ -109,6 +114,15 @@ class GeometryModel: 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) + if self.centerMassX is not None: + result['centerMassX'] = from_union( + [from_str, from_none], self.centerMassX) + if self.centerMassY is not None: + result['centerMassY'] = from_union( + [from_str, from_none], self.centerMassY) + if self.centerMassZ is not None: + result['centerMassZ'] = from_union( + [from_str, from_none], self.centerMassZ) return result def toJSON(self) -> str: @@ -161,5 +175,4 @@ class GeometryModel: def toUrdf(self): return FS.readFile(os.path.dirname(os.path.realpath(__file__)) - + '/../../mocks/urdf/model.urdf').replace('{name}', self.name).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).replace('{stl}', '/' + self.stl).replace('{massSDF}', self.massSDF) - + + '/../../mocks/urdf/model.urdf').replace('{name}', self.name).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).replace('{stl}', '/' + self.stl).replace('{massSDF}', self.massSDF).replace('{centerMassX}', self.centerMassX).replace('{centerMassY}', self.centerMassY).replace('{centerMassZ}', self.centerMassZ) diff --git a/cad_generation/model/sdf_geometry_model.py b/cad_generation/model/sdf_geometry_model.py index dca815d..21c2a4e 100644 --- a/cad_generation/model/sdf_geometry_model.py +++ b/cad_generation/model/sdf_geometry_model.py @@ -26,7 +26,7 @@ def to_class(c, x): class SdfGeometryModel: - def __init__(self, name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, friction): + def __init__(self, name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, friction, centerMassX, centerMassY, centerMassZ,): self.name = name self.ixx = ixx self.ixy = ixy @@ -43,6 +43,9 @@ class SdfGeometryModel: self.iyz = iyz self.stl = stl self.friction = friction + self.centerMassX = centerMassX + self.centerMassY = centerMassY + self.centerMassZ = centerMassZ @staticmethod def from_dict(obj): @@ -61,9 +64,12 @@ class SdfGeometryModel: 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") ) + stl = from_union([from_str, from_none], obj.get("stl")) 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,friction) + centerMassX = from_union([from_str, from_none], obj.get("centerMassX")) + centerMassY = from_union([from_str, from_none], obj.get("centerMassY")) + centerMassZ = from_union([from_str, from_none], obj.get("centerMassZ")) + return SdfGeometryModel(name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, friction, centerMassX, centerMassY, centerMassZ) def to_dict(self): result = {} @@ -99,9 +105,14 @@ class SdfGeometryModel: result["stl"] = from_union([from_str, from_none], self.stl) if self.friction is not None: result["friction"] = from_union([from_str, from_none], self.eulerZ) + + if self.centerMassX is not None: + result['centerMassX'] = from_union([from_str, from_none], self.centerMassX) + if self.centerMassY is not None: + result['centerMassY'] = from_union([from_str, from_none], self.centerMassY) + if self.centerMassZ is not None: + result['centerMassZ'] = from_union([from_str, from_none], self.centerMassZ) return result def toJSON(self) -> str: return str(self.to_dict()).replace('\'', '"') - - \ No newline at end of file diff --git a/cad_generation/usecases/get_sdf_geometry_usecase.py b/cad_generation/usecases/get_sdf_geometry_usecase.py index 45869c5..4a4ccb4 100644 --- a/cad_generation/usecases/get_sdf_geometry_usecase.py +++ b/cad_generation/usecases/get_sdf_geometry_usecase.py @@ -36,7 +36,9 @@ class SdfGeometryUseCase: eulerX = str(pos.Rotation.toEuler()[0]) eulerY = str(pos.Rotation.toEuler()[1]) eulerZ = str(pos.Rotation.toEuler()[2]) - + centerMassX = str(el.Shape.CenterOfMass[0]) + centerMassY = str(el.Shape.CenterOfMass[1]) + centerMassZ = str(el.Shape.CenterOfMass[2]) geometry.append( SdfGeometryModel( stl=stlPaths.get(el.Label), @@ -55,10 +57,13 @@ class SdfGeometryUseCase: eulerY=eulerY, eulerZ=eulerZ, friction=materialSolid.get(el.Label) or '', + centerMassX=centerMassX, + centerMassY=centerMassY, + centerMassZ=centerMassZ ) ) except Exception as e: - print(200) + print(e) return geometry