robossembler/framework
7
0
Fork 0
Code Issues 21 Pull requests Projects Releases Packages Wiki Activity Actions

adding center mass URDF

Browse source
This commit is contained in:
IDONTSUDO 2023-07-12 17:40:24 +03:00
parent 70e268c455
commit 502e7a8d76
4 changed files with 60 additions and 24 deletions
Download patch file Download diff file Expand all files Collapse all files

31
asp/mocks/urdf/model.urdf
View file

@ -1,30 +1,37 @@
<?xml version="1.0" ?> <?xml version="1.0"?>
<robot name="{name}"> <robot name="{name}">
<link name="baseLink"> <link name="baseLink">
<contact> <contact>
<friction_anchor/> <friction_anchor />
<lateral_friction value="0.3"/> <lateral_friction value="0.3" />
<rolling_friction value="0.0"/> <rolling_friction value="0.0" />
<contact_cfm value="0.0"/> <contact_cfm value="0.0" />
<contact_erp value="1.0"/> <contact_erp value="1.0" />
</contact> </contact>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin xyz="{centerMassX} {centerMassY} {centerMassZ}" />
<mass value="{massSDF}"/> <mass value="{massSDF}" />
<inertia ixx="{ixx}" ixy="{ixy}" ixz="{ixz}" iyy="{iyy}" iyz="{iyz}" izz="{izz}"/> <inertia ixx="{ixx}" ixy="{ixy}" ixz="{ixz}" iyy="{iyy}" iyz="{iyz}" izz="{izz}" />
</inertial> </inertial>
<visual> <visual>
<geometry> <geometry>
<mesh filename="{stl}" scale="1 1 1"/> <mesh filename="{stl}" scale="1 1 1" />
</geometry> </geometry>
<material name="white"> <material name="white">
<color rgba="1. 1. 1. 1."/> <color rgba="1. 1. 1. 1." />
</material> </material>
</visual> </visual>
<collision> <collision>
<geometry> <geometry>
<mesh filename="{stl}" scale="1 1 1"/> <mesh filename="{stl}" scale="1 1 1" />
</geometry> </geometry>
</collision> </collision>
<friction>
<ode>
<mu>0.2</mu>
<mu2>0.1</mu2>
<fdir1>1 0 0</fdir1>
</ode>
</friction>
</link> </link>
</robot> </robot>

23
asp/src/model/sdf_geometry.py
View file

@ -31,7 +31,7 @@ def to_class(c, x):
class GeometryModel: 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.name = name
self.ixx = ixx self.ixx = ixx
self.ixy = ixy self.ixy = ixy
@ -49,6 +49,9 @@ class GeometryModel:
self.stl = stl self.stl = stl
self.link = link self.link = link
self.friction = friction self.friction = friction
self.centerMassX = centerMassX
self.centerMassY = centerMassY
self.centerMassZ = centerMassZ
@staticmethod @staticmethod
def from_dict(obj): def from_dict(obj):
@ -70,8 +73,10 @@ class GeometryModel:
stl = from_union([from_str, from_none], obj.get("stl")) stl = from_union([from_str, from_none], obj.get("stl"))
link = from_union([from_str, from_none], obj.get('link')) link = from_union([from_str, from_none], obj.get('link'))
friction = from_union([from_str, from_none], obj.get("friction")) friction = from_union([from_str, from_none], obj.get("friction"))
centerMassX = from_union([from_str, from_none], obj.get("centerMassX"))
return GeometryModel(name, ixx, ixy, ixz, iyy, izz, massSDF, posX, posY, posZ, eulerX, eulerY, eulerZ, iyz, stl, link, friction) 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): def to_dict(self):
result = {} result = {}
@ -109,6 +114,15 @@ class GeometryModel:
result['link'] = from_union([from_str, from_none], self.link) result['link'] = from_union([from_str, from_none], self.link)
if self.friction is not None: if self.friction is not None:
result["friction"] = from_union([from_str, from_none], self.eulerZ) 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 return result
def toJSON(self) -> str: def toJSON(self) -> str:
@ -161,5 +175,4 @@ class GeometryModel:
def toUrdf(self): def toUrdf(self):
return FS.readFile(os.path.dirname(os.path.realpath(__file__)) 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)

21
cad_generation/model/sdf_geometry_model.py
View file

@ -26,7 +26,7 @@ def to_class(c, x):
class SdfGeometryModel: 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.name = name
self.ixx = ixx self.ixx = ixx
self.ixy = ixy self.ixy = ixy
@ -43,6 +43,9 @@ class SdfGeometryModel:
self.iyz = iyz self.iyz = iyz
self.stl = stl self.stl = stl
self.friction = friction self.friction = friction
self.centerMassX = centerMassX
self.centerMassY = centerMassY
self.centerMassZ = centerMassZ
@staticmethod @staticmethod
def from_dict(obj): def from_dict(obj):
@ -61,9 +64,12 @@ class SdfGeometryModel:
eulerY = from_union([from_str, from_none], obj.get("eulerY")) eulerY = from_union([from_str, from_none], obj.get("eulerY"))
eulerZ = from_union([from_str, from_none], obj.get("eulerZ")) eulerZ = from_union([from_str, from_none], obj.get("eulerZ"))
iyz = from_union([from_str, from_none], obj.get("iyz")) 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")) 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): def to_dict(self):
result = {} result = {}
@ -99,9 +105,14 @@ class SdfGeometryModel:
result["stl"] = from_union([from_str, from_none], self.stl) result["stl"] = from_union([from_str, from_none], self.stl)
if self.friction is not None: if self.friction is not None:
result["friction"] = from_union([from_str, from_none], self.eulerZ) 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 return result
def toJSON(self) -> str: def toJSON(self) -> str:
return str(self.to_dict()).replace('\'', '"') return str(self.to_dict()).replace('\'', '"')

9
cad_generation/usecases/get_sdf_geometry_usecase.py
View file

@ -36,7 +36,9 @@ class SdfGeometryUseCase:
eulerX = str(pos.Rotation.toEuler()[0]) eulerX = str(pos.Rotation.toEuler()[0])
eulerY = str(pos.Rotation.toEuler()[1]) eulerY = str(pos.Rotation.toEuler()[1])
eulerZ = str(pos.Rotation.toEuler()[2]) 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( geometry.append(
SdfGeometryModel( SdfGeometryModel(
stl=stlPaths.get(el.Label), stl=stlPaths.get(el.Label),
@ -55,10 +57,13 @@ class SdfGeometryUseCase:
eulerY=eulerY, eulerY=eulerY,
eulerZ=eulerZ, eulerZ=eulerZ,
friction=materialSolid.get(el.Label) or '', friction=materialSolid.get(el.Label) or '',
centerMassX=centerMassX,
centerMassY=centerMassY,
centerMassZ=centerMassZ
) )
) )
except Exception as e: except Exception as e:
print(200) print(e)
return geometry return geometry