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

Support "Assemble them all" trajectory generation from CAD

Browse source
This commit is contained in:
IDONTSUDO 2023-06-18 15:33:16 +00:00 • committed by Igor Brylyov
parent 47773be8d4
commit a38c3bec5a
42 changed files with 537 additions and 119 deletions
Download patch file Download diff file Expand all files Collapse all files

35
asp/helper/fs.py Normal file
View file

@ -0,0 +1,35 @@
import os
import json
import typing
class FS:
def readJSON(path: str):
return json.loads((open(path)).read())
def writeFile(data, filePath, fileName):
file_to_open = filePath + fileName
f = open(file_to_open, 'w', )
f.write(data)
def readFile(path:str):
return open(path).read()
def readFilesTypeFolder(pathFolder: str, fileType = '.json'):
filesJson = list(
filter(lambda x: x[-fileType.__len__():] == fileType, os.listdir(pathFolder)))
return filesJson
def listGetFirstValue(iterable, default=False, pred=None):
return next(filter(pred, iterable), default)
def filterModels(filterModels, filterModelsDescription: list[str]):
models = []
for el in filterModelsDescription:
models.append(listGetFirstValue(
filterModels, None, lambda x: x.name == el))
return models

56
asp/main.py Normal file
View file

@ -0,0 +1,56 @@
import argparse
import shutil
from helper.fs import FS
from src.usecases.stability_check_usecase import StabilityCheckUseCase
from src.usecases.urdf_sub_assembly_usecase import UrdfSubAssemblyUseCase
from src.usecases.sdf_generate_world_usecase import SdfGenerateWorldUseCase
from src.model.sdf_geometry import GeometryModel
from src.usecases.sdf_sub_assembly_usecase import SdfSubAssemblyUseCase
import os
# python3 main.py --generationFolder /Users/idontsudo/robo/Cube3/ --outPath /Users/idontsudo/robo/ --world true --format 'urdf' --stabilityCheck 'true'
# python3 main.py --generationFolder /Users/idontsudo/robo/Cube3/ --outPath /Users/idontsudo/robo/ --world true --format 'sdf'
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument('--generationFolder', help='FreeCad generation folder')
parser.add_argument('--outPath', help='save SDF path')
parser.add_argument('--world', help='adding sdf world')
parser.add_argument('--format', help='urdf,sdf,mujoco')
parser.add_argument('--stabilityCheck',
help='do i need to check the stability?')
args = parser.parse_args()
if args.generationFolder == None or args.outPath == None:
parser.print_help()
outPath = args.outPath
geometryFiles = FS.readFilesTypeFolder(args.generationFolder + '/assets/')
assemblyStructure = FS.readJSON(
args.generationFolder + '/step-structure.json')
geometryModels: list[GeometryModel] = []
for el in geometryFiles:
geometryModels.append(GeometryModel.from_dict(
FS.readJSON(args.generationFolder + '/assets/' + el)))
if os.path.exists(outPath + 'sdf-generation/'):
shutil.rmtree(path=outPath + 'sdf-generation/')
if (args.format == 'sdf'):
SdfSubAssemblyUseCase().call(
geometryModels=geometryModels, assembly=assemblyStructure,
world=args.world,
generationFolder=args.generationFolder,
outPath=args.outPath
)
if (args.format == 'urdf' and args.stabilityCheck != None):
UrdfSubAssemblyUseCase().call(
geometryModels=geometryModels, assembly=assemblyStructure,
world=args.world,
generationFolder=args.generationFolder,
outPath=args.outPath
)
StabilityCheckUseCase().call(
args.outPath
)

18
asp/mocks/Cube1.json Normal file
View file

@ -0,0 +1,18 @@
{
"name": "Cube1",
"ixx": "16.66666666666667",
"ixy": "0.0",
"ixz": "0.0",
"iyy": "16.66666666666667",
"izz": "16.66666666666667",
"massSDF": "0.9999999999999998",
"posX": "0.0",
"posY": "-0.015",
"posZ": "0.0",
"eulerX": "0.0",
"eulerY": "0.0",
"eulerZ": "0.0",
"iyz": "0.0",
"stl": "/meshes/Cube1.stl",
"link": "1554"
}

18
asp/mocks/Cube2.json Normal file
View file

@ -0,0 +1,18 @@
{
"name": "Cube2",
"ixx": "16.66666666666667",
"ixy": "0.0",
"ixz": "-3.637978807091713e-15",
"iyy": "16.66666666666667",
"izz": "16.66666666666667",
"massSDF": "0.9999999999999998",
"posX": "0.0",
"posY": "-0.009",
"posZ": "0.01",
"eulerX": "0.0",
"eulerY": "0.0",
"eulerZ": "0.0",
"iyz": "-3.637978807091713e-15",
"stl": "/meshes/Cube2.stl",
"link": "8838"
}

4
asp/mocks/sdf/include.sdf Normal file
View file

@ -0,0 +1,4 @@
<include>
<name>{name}</name>
<uri>{uri}</uri>
</include>

5
asp/mocks/sdf/include_pose.sdf Normal file
View file

@ -0,0 +1,5 @@
<include>
<name>{name}</name>
<uri>{uri}</uri>
<pose>{posX} {posY} {posZ} {eulerX} {eulerY} {eulerZ}</pose>
</include>

7
asp/mocks/sdf/joint_fixed.sdf Normal file
View file

@ -0,0 +1,7 @@
<joint name="{name}" type="fixed">
<parent>base_link</parent>
<child>{child}::{child}</child>
<pose>{posX} {posY} {posZ} {eulerX} {eulerY} {eulerZ}</pose>
</joint>

36
asp/mocks/sdf/link.sdf Normal file
View file

@ -0,0 +1,36 @@
<link name="{name}">
<pose>{posX} {posY} {posZ} {eulerX} {eulerY} {eulerZ}</pose>
<inertial>
<pose>{posX} {posY} {posZ} {eulerX} {eulerY} {eulerZ}</pose>
<inertia>
<ixx>{ixx}</ixx>
<ixy>{ixy}</ixy>
<ixz>{ixz}</ixz>
<iyy>{iyy}</iyy>
<iyz>{iyz}</iyz>
<izz>{izz}</izz>
</inertia>
<mass>{massSDF}</mass>
</inertial>
<collision name="collision">
<geometry>
<mesh>
<uri>model:/{stl}</uri>
</mesh>
</geometry>
</collision>
<visual name="visual">
<geometry>
<mesh>
<uri>model:/{stl}</uri>
</mesh>
</geometry>
<surface>
<friction>
<ode>
<mu>{friction}</mu>
</ode>
</friction>
</surface>
</visual>
</link>

5
asp/mocks/sdf/model.config Normal file
View file

@ -0,0 +1,5 @@
<?xml version="1.0" ?>
<model>
<sdf version="1.5">model.sdf</sdf>
</model>

27
asp/mocks/sdf/model.sdf Normal file
View file

@ -0,0 +1,27 @@
<?xml version='1.0'?>
<sdf version="1.4">
<model name="{name}">
<link name="{name}">
<gravity>0</gravity>
<collision name="collision">
<mesh>
<uri>model:/{stl}</uri>
</mesh>
</collision>
<visual name="visual">
<geometry>
<mesh>
<uri>model:/{stl}</uri>
</mesh>
</geometry>
<surface>
<friction>
<ode>
<mu>{friction}</mu>
</ode>
</friction>
</surface>
</visual>
</link>
</model>
</sdf>

71
asp/mocks/sdf/world.sdf Normal file
View file

@ -0,0 +1,71 @@
<sdf version='1.7'>
<world name='empty'>
<physics name='1ms' type='ignored'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<plugin name='ignition::gazebo::systems::Physics' filename='ignition-gazebo-physics-system'/>
<plugin name='ignition::gazebo::systems::UserCommands' filename='ignition-gazebo-user-commands-system'/>
<plugin name='ignition::gazebo::systems::SceneBroadcaster' filename='ignition-gazebo-scene-broadcaster-system'/>
<plugin name='ignition::gazebo::systems::Contact' filename='ignition-gazebo-contact-system'/>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>true</shadows>
</scene>
<model name='ground_plane'>
<static>true</static>
<link name='link'>
<collision name='collision'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<surface>
<friction>
<ode/>
</friction>
<bounce/>
<contact/>
</surface>
</collision>
<visual name='visual'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
<pose>0 0 0 0 -0 0</pose>
<inertial>
<pose>0 0 0 0 -0 0</pose>
<mass>1</mass>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
</inertial>
<enable_wind>false</enable_wind>
</link>
<pose>0 0 0 0 -0 0</pose>
<self_collide>false</self_collide>
</model>
</world>
</sdf>

30
asp/mocks/urdf/model.urdf Normal file
View file

@ -0,0 +1,30 @@
<?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"/>
</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}"/>
</inertial>
<visual>
<geometry>
<mesh filename="{stl}" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1. 1. 1. 1."/>
</material>
</visual>
<collision>
<geometry>
<mesh filename="{stl}" scale="1 1 1"/>
</geometry>
</collision>
</link>
</robot>

5
asp/requirements.txt Normal file
View file

@ -0,0 +1,5 @@
argparse
matplotlib
pybullet
argparse
xmlformatter

16
asp/src/model/asm.py Normal file
View file

@ -0,0 +1,16 @@
from distutils.dir_util import copy_tree
from src.model.enum import Enum
class Assembly:
def generateSubAssembly(self, assembly: list[str]):
asm = {}
inc = 0
for el in assembly:
asm[str("asm" + str(inc))] = {
"part": el,
"assembly": assembly[0:inc],
}
inc += 1
return asm
def copy(self,generationFolder,format,outPath ):
copy_tree(generationFolder + format, outPath + Enum.folderPath)

2
asp/src/model/enum.py Normal file
View file

@ -0,0 +1,2 @@
class Enum:
folderPath = 'sdf-generation/';

165
asp/src/model/sdf_geometry.py Normal file
View file

@ -0,0 +1,165 @@
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 GeometryModel:
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 GeometryModel(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['GeometryModel']):
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
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)

16
asp/src/model/sdf_join.py Normal file
View file

@ -0,0 +1,16 @@
from helper.fs import FS
import os
class SdfJoin:
def __init__(self, name, parent, modelAt, child) -> None:
self.name = name
self.parent = parent
self.child = child
self.modelAt = modelAt
pass
def toSDF(self):
return (FS.readFile(os.path.dirname(os.path.realpath(__file__)) + '/../../mocks/sdf/joint_fixed.sdf')).replace('{name}', self.name,).replace('{parent}', self.parent).replace('{child}', self.child).replace('{posX}', self.modelAt.posX).replace('{posY}', self.modelAt.posY).replace('{posZ}', self.modelAt.posZ).replace('{eulerX}', self.modelAt.eulerX).replace('{eulerY}', self.modelAt.eulerY).replace('{eulerZ}', self.modelAt.eulerZ).replace('{ixx}', self.modelAt.ixx).replace('{ixy}', self.modelAt.ixy).replace('{ixz}', self.modelAt.ixz).replace('{iyy}', self.modelAt.iyy).replace('{iyz}', self.modelAt.iyz).replace('{izz}', self.modelAt.izz)

15
asp/src/usecases/formatter_usecase.py Normal file
View file

@ -0,0 +1,15 @@
from src.model.enum import Enum
import xmlformatter
from helper.fs import FS
class FormatterUseCase:
def call(outPath: str, format: str):
formatter = xmlformatter.Formatter(
indent="1", indent_char="\t", encoding_output="ISO-8859-1", preserve=["literal"])
files = FS.readFilesTypeFolder(
outPath + Enum.folderPath, fileType=format)
for el in files:
FS.writeFile(data=str(formatter.format_file(outPath + Enum.folderPath + el),
'utf-8'), filePath=outPath + Enum.folderPath, fileName=el)

12
asp/src/usecases/generate_world.py Normal file
View file

@ -0,0 +1,12 @@
import os
from helper.fs import FS
class SdfGenerateWorldUseCase:
def call(assembly:str) -> str:
world = FS.readFile(os.path.dirname(os.path.realpath(__file__))
+ '/../../mocks/sdf/world.sdf')
beginWorld = world[0:world.find('</world') - 1]
endWorld = world[world.find('</world') - 1: world.__len__()]
return beginWorld + assembly + endWorld

12
asp/src/usecases/sdf_generate_world_usecase.py Normal file
View file

@ -0,0 +1,12 @@
import os
from helper.fs import FS
class SdfGenerateWorldUseCase:
def call(assembly:str) -> str:
world = FS.readFile(os.path.dirname(os.path.realpath(__file__))
+ '/../../mocks/sdf/world.sdf')
beginWorld = world[0:world.find('</world') - 1]
endWorld = world[world.find('</world') - 1: world.__len__()]
return beginWorld + assembly + endWorld

57
asp/src/usecases/sdf_sub_assembly_usecase.py Normal file
View file

@ -0,0 +1,57 @@
import os
from typing import Optional
from helper.fs import FS
from helper.fs import filterModels, listGetFirstValue
from src.model.asm import Assembly
from src.model.enum import Enum
from src.usecases.formatter_usecase import FormatterUseCase
from src.usecases.sdf_generate_world_usecase import SdfGenerateWorldUseCase
from src.model.sdf_geometry import GeometryModel
from distutils.dir_util import copy_tree
SDF_FILE_FORMAT = '.sdf'
CONFIG_PATH = os.path.dirname(os.path.realpath(
__file__)) + '/../../mocks/sdf/model.config'
class SdfSubAssemblyUseCase(Assembly):
def call(self, geometryModels: list[GeometryModel], assembly: list[str], outPath: str, generationFolder: str, world: bool):
asm = {}
generateSubAssemblyModels = self.generateSubAssembly(assembly)
inc = 0
for key, value in generateSubAssemblyModels.items():
inc += 1
if value['assembly'].__len__() != 0:
model: Optional[GeometryModel] = listGetFirstValue(
geometryModels, None, lambda x: x.name == value['assembly'][0])
if model != None:
asm[key] = {"assembly": model.generateSDFatJoinFixed(filterModels(geometryModels, value['assembly'])), "part": (
listGetFirstValue(geometryModels, None, lambda x: x.name == value['part'])).includeLink()}
self.copy(generationFolder=
generationFolder, format='/sdf', outPath=outPath)
dirPath = outPath + Enum.folderPath
for el in geometryModels:
path = dirPath + el.name + '/'
os.makedirs(path)
FS.writeFile(data=el.toSDF(), filePath=path,
fileName='/model' + SDF_FILE_FORMAT)
FS.writeFile(data=FS.readFile(CONFIG_PATH),
filePath=path, fileName='/model' + '.config')
for key, v in asm.items():
FS.writeFile(data=v['assembly'], filePath=dirPath,
fileName='/' + key + SDF_FILE_FORMAT)
else:
for key, v in asm.items():
FS.writeFile(data=SdfGenerateWorldUseCase.call(v['assembly']), filePath=dirPath,
fileName='/' + key + SDF_FILE_FORMAT)
FormatterUseCase.call(outPath=outPath, format=SDF_FILE_FORMAT)

36
asp/src/usecases/stability_check_usecase.py Normal file
View file

@ -0,0 +1,36 @@
import numpy as np
import pybullet as p
import time
import pybullet_data
from helper.fs import FS
from src.usecases.urdf_sub_assembly_usecase import URDF_GENERATOR_FILE
import json
from src.model.enum import Enum
class StabilityCheckUseCase:
def call(self, outPath: str):
dirPath = outPath + Enum.folderPath
DURATION = 10000
asm = json.loads(FS.readFile(dirPath + URDF_GENERATOR_FILE))
inc = 0
for el in asm['asm2']:
FS.writeFile(data=el, filePath=dirPath,
fileName=str(inc) + '.urdf')
inc += 1
assemblyURDFS = list(
map(lambda el: dirPath+el, FS.readFilesTypeFolder(dirPath, '.urdf')))
physicsClient = p.connect(p.GUI)
p.setGravity(0, 0, -10)
for el in assemblyURDFS:
p.loadURDF(el)
for i in range(DURATION):
p.stepSimulation()
time.sleep(1./240.)
p.disconnect()

42
asp/src/usecases/urdf_sub_assembly_usecase.py Normal file
View file

@ -0,0 +1,42 @@
from typing import Optional
from helper.fs import FS
from src.model.enum import Enum
from src.model.asm import Assembly
from src.model.sdf_geometry import GeometryModel
from helper.fs import filterModels, listGetFirstValue
import json
def toUrdf(el: GeometryModel):
return el.toUrdf()
URDF_FILE_FORMAT = '.urdf'
URDF_GENERATOR_FILE = 'urdf-generation' + '.json'
class UrdfSubAssemblyUseCase(Assembly):
def call(self, geometryModels: list[GeometryModel], assembly: list[str], outPath: str, generationFolder: str, world: bool):
dirPath = outPath + Enum.folderPath
asm = {}
generateSubAssemblyModels = self.generateSubAssembly(assembly)
inc = 0
for key, value in generateSubAssemblyModels.items():
inc += 1
if value['assembly'].__len__() != 0:
model: Optional[GeometryModel] = listGetFirstValue(
geometryModels, None, lambda x: x.name == value['assembly'][0])
if model != None:
urdfs = list(map(toUrdf, filterModels(
geometryModels, value['assembly'])))
urdfs.append(listGetFirstValue(
geometryModels, None, lambda x: x.name == value['part']) .toUrdf())
asm[key] = urdfs
self.copy(generationFolder=generationFolder,
format='/sdf', outPath=outPath)
FS.writeFile(data=json.dumps(asm),
fileName=URDF_GENERATOR_FILE, filePath=dirPath)
# for el in asm.keys():