adding center mass URDF
This commit is contained in:
IDONTSUDO
parent
70e268c455
commit
502e7a8d76
4 changed files with 60 additions and 24 deletions
|
|
@ -1,30 +1,37 @@
|
|||
<?xml version="1.0" ?>
|
||||
<?xml version="1.0"?>
|
||||
<robot name="{name}">
|
||||
<link name="baseLink">
|
||||
<contact>
|
||||
<friction_anchor/>
|
||||
<lateral_friction value="0.3"/>
|
||||
<rolling_friction value="0.0"/>
|
||||
<contact_cfm value="0.0"/>
|
||||
<contact_erp value="1.0"/>
|
||||
<friction_anchor />
|
||||
<lateral_friction value="0.3" />
|
||||
<rolling_friction value="0.0" />
|
||||
<contact_cfm value="0.0" />
|
||||
<contact_erp value="1.0" />
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="{massSDF}"/>
|
||||
<inertia ixx="{ixx}" ixy="{ixy}" ixz="{ixz}" iyy="{iyy}" iyz="{iyz}" izz="{izz}"/>
|
||||
<origin xyz="{centerMassX} {centerMassY} {centerMassZ}" />
|
||||
<mass value="{massSDF}" />
|
||||
<inertia ixx="{ixx}" ixy="{ixy}" ixz="{ixz}" iyy="{iyy}" iyz="{iyz}" izz="{izz}" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="{stl}" scale="1 1 1"/>
|
||||
<mesh filename="{stl}" scale="1 1 1" />
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1. 1. 1. 1."/>
|
||||
<color rgba="1. 1. 1. 1." />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="{stl}" scale="1 1 1"/>
|
||||
<mesh filename="{stl}" scale="1 1 1" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>0.2</mu>
|
||||
<mu2>0.1</mu2>
|
||||
<fdir1>1 0 0</fdir1>
|
||||
</ode>
|
||||
</friction>
|
||||
</link>
|
||||
</robot>
|
||||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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('\'', '"')
|
||||
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue