Add PDDL, 3D-assets & SDF-URDF generator from Blender Scene Config
This commit is contained in:
IDONTSUDO
parent
b77687ea14
commit
e305d486f2
41 changed files with 2793 additions and 664 deletions
|
|
@ -1,12 +1,14 @@
|
|||
import argparse
|
||||
from usecases.stability_check_usecase import StabilityCheckUseCase
|
||||
|
||||
# python3 main.py --aspPath /Users/idontsudo/framework/asp/out
|
||||
# python3 main.py --aspPath /Users/idontsudo/Desktop/asp-example/
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument('--aspPath', help='asp folder generation path')
|
||||
parser.add_argument("--aspPath", help="asp folder generation path")
|
||||
args = parser.parse_args()
|
||||
StabilityCheckUseCase().call(args.aspPath)
|
||||
|
||||
|
||||
main()
|
||||
|
||||
|
|
@ -53,7 +53,7 @@ class Coords:
|
|||
self.z = z
|
||||
|
||||
@staticmethod
|
||||
def from_dict(obj: Any) -> 'Coords':
|
||||
def from_dict(obj: Any) -> "Coords":
|
||||
assert isinstance(obj, dict)
|
||||
x = from_float(obj.get("x"))
|
||||
y = from_float(obj.get("y"))
|
||||
|
|
@ -79,7 +79,7 @@ class SimulatorStabilityResultModel:
|
|||
self.position = position
|
||||
|
||||
@staticmethod
|
||||
def from_dict(obj: Any) -> 'SimulatorStabilityResultModel':
|
||||
def from_dict(obj: Any) -> "SimulatorStabilityResultModel":
|
||||
assert isinstance(obj, dict)
|
||||
id = from_str(obj.get("id"))
|
||||
quaternion = Coords.from_dict(obj.get("quaternion"))
|
||||
|
|
@ -103,90 +103,127 @@ def SimulatorStabilityModeltodict(x: List[SimulatorStabilityResultModel]) -> Any
|
|||
|
||||
|
||||
class StabilityCheckUseCase:
|
||||
def urdfLoader(self, assembly: list[str], outPath: str, urdfGeneration: dict[str:str]):
|
||||
|
||||
def urdfLoader(
|
||||
self, assembly: list[str], outPath: str, urdfGeneration: dict[str:str]
|
||||
):
|
||||
urdfs = []
|
||||
for assemblyCount in range(len(assembly)):
|
||||
|
||||
urdf = urdfGeneration.get(assembly[assemblyCount])
|
||||
file_to_open = outPath + '/sdf-generation/' + \
|
||||
str(assemblyCount) + '.urdf'
|
||||
f = open(file_to_open, 'w', encoding='utf-8',
|
||||
errors='ignore')
|
||||
file_to_open = outPath + "/generation/" + str(assemblyCount) + ".urdf"
|
||||
f = open(file_to_open, "w", encoding="utf-8", errors="ignore")
|
||||
f.write(urdf)
|
||||
f.close()
|
||||
urdfs.append(os.path.abspath(f.name))
|
||||
return urdfs
|
||||
|
||||
def executeSimulation(self, assembly: list[str], outPath: str, urdfGeneration: dict[str:str], duration: int) -> list['SimulatorStabilityResultModel']:
|
||||
|
||||
def executeSimulation(
|
||||
self,
|
||||
assembly: list[str],
|
||||
outPath: str,
|
||||
urdfGeneration: dict[str:str],
|
||||
duration: int,
|
||||
) -> list["SimulatorStabilityResultModel"]:
|
||||
p.connect(p.DIRECT)
|
||||
p.setGravity(0, 0, -10)
|
||||
p.setAdditionalSearchPath(pybullet_data.getDataPath())
|
||||
p.loadURDF("plane.urdf")
|
||||
resultCoords = []
|
||||
|
||||
urdfs = self.urdfLoader(assembly=assembly,
|
||||
urdfGeneration=urdfGeneration, outPath=outPath)
|
||||
urdfs = self.urdfLoader(
|
||||
assembly=assembly, urdfGeneration=urdfGeneration, outPath=outPath
|
||||
)
|
||||
bulletIds = []
|
||||
for el in urdfs:
|
||||
id = p.loadURDF(el)
|
||||
bulletIds.append(id)
|
||||
for i in range(duration):
|
||||
if (i + 200 == duration):
|
||||
if i + 200 == duration:
|
||||
inc = 0
|
||||
for bulletUUID in bulletIds:
|
||||
pos, rot = p.getBasePositionAndOrientation(bulletUUID)
|
||||
resultCoords.append(SimulatorStabilityResultModel(id=assembly[inc], quaternion=Coords(
|
||||
x=rot[0], y=rot[1], z=rot[2]), position=Coords(x=pos[0], y=pos[1], z=pos[2])))
|
||||
resultCoords.append(
|
||||
SimulatorStabilityResultModel(
|
||||
id=assembly[inc],
|
||||
quaternion=Coords(x=rot[0], y=rot[1], z=rot[2]),
|
||||
position=Coords(x=pos[0], y=pos[1], z=pos[2]),
|
||||
)
|
||||
)
|
||||
p.removeBody(bulletUUID)
|
||||
inc += 1
|
||||
|
||||
p.stepSimulation()
|
||||
|
||||
time.sleep(1./240.)
|
||||
time.sleep(1.0 / 240.0)
|
||||
return resultCoords
|
||||
|
||||
def call(self, aspPath: str):
|
||||
try:
|
||||
assemblyFolder = aspPath
|
||||
assemblesStructures = json.loads(
|
||||
(open(assemblyFolder + 'sequences.json')).read()).get('sequences')
|
||||
(open(assemblyFolder + "sequences.json")).read()
|
||||
).get("sequences")
|
||||
|
||||
tasks = len(assemblesStructures) * len(assemblesStructures[0])
|
||||
taskCounter = 0
|
||||
urdfGeneration = json.loads(
|
||||
(open(assemblyFolder + 'sdf-generation/urdf-generation.json')).read())
|
||||
(open(assemblyFolder + "generation/urdf-generation.json")).read()
|
||||
)
|
||||
for activeAssemblyNumber in range(len(assemblesStructures)):
|
||||
pathSaveResultAssemblyFolder = aspPath + 'stability' + \
|
||||
'/' + str(activeAssemblyNumber + 1) + '/'
|
||||
pathSaveResultAssemblyFolder = (
|
||||
aspPath + "stability" + "/" + str(activeAssemblyNumber + 1) + "/"
|
||||
)
|
||||
if not os.path.exists(pathSaveResultAssemblyFolder):
|
||||
os.makedirs(pathSaveResultAssemblyFolder)
|
||||
|
||||
for subAssemblyNumber in range(len(assemblesStructures[activeAssemblyNumber])):
|
||||
for subAssemblyNumber in range(
|
||||
len(assemblesStructures[activeAssemblyNumber])
|
||||
):
|
||||
taskCounter += 1
|
||||
subAssembly = assemblesStructures[activeAssemblyNumber][0:subAssemblyNumber+1]
|
||||
asm = []
|
||||
for el in subAssembly:
|
||||
asm.append(el)
|
||||
subAssembly = assemblesStructures[activeAssemblyNumber][
|
||||
0 : subAssemblyNumber + 1
|
||||
]
|
||||
print(subAssembly)
|
||||
|
||||
resultSimulationStates = self.executeSimulation(
|
||||
assembly=asm, outPath=aspPath, urdfGeneration=urdfGeneration, duration=1000)
|
||||
if subAssembly == [
|
||||
"disk_top",
|
||||
"disk_middel",
|
||||
]:
|
||||
asm = []
|
||||
for el in subAssembly:
|
||||
asm.append(el)
|
||||
|
||||
pathSaveResultSubAssemblyFolder = aspPath + 'stability' + '/' + \
|
||||
str(activeAssemblyNumber + 1) + \
|
||||
'/' + str(subAssemblyNumber) + '/'
|
||||
if not os.path.exists(pathSaveResultSubAssemblyFolder):
|
||||
os.makedirs(pathSaveResultSubAssemblyFolder)
|
||||
results = {}
|
||||
for state in resultSimulationStates:
|
||||
results[state.id] = state.to_dict()
|
||||
f = open(pathSaveResultSubAssemblyFolder + '/' +
|
||||
'motion_result.json', 'w', encoding='utf-8', errors='ignore')
|
||||
f.write(json.dumps(results,
|
||||
ensure_ascii=False, indent=4))
|
||||
f.close()
|
||||
percentageOfCompletion = taskCounter / tasks * 100
|
||||
print('process complete: ' +
|
||||
str(percentageOfCompletion) + '%')
|
||||
resultSimulationStates = self.executeSimulation(
|
||||
assembly=asm,
|
||||
outPath=aspPath,
|
||||
urdfGeneration=urdfGeneration,
|
||||
duration=1000,
|
||||
)
|
||||
|
||||
pathSaveResultSubAssemblyFolder = (
|
||||
aspPath
|
||||
+ "stability"
|
||||
+ "/"
|
||||
+ str(activeAssemblyNumber + 1)
|
||||
+ "/"
|
||||
+ str(subAssemblyNumber)
|
||||
+ "/"
|
||||
)
|
||||
if not os.path.exists(pathSaveResultSubAssemblyFolder):
|
||||
os.makedirs(pathSaveResultSubAssemblyFolder)
|
||||
results = {}
|
||||
for state in resultSimulationStates:
|
||||
results[state.id] = state.to_dict()
|
||||
f = open(
|
||||
pathSaveResultSubAssemblyFolder
|
||||
+ "/"
|
||||
+ "motion_result.json",
|
||||
"w",
|
||||
encoding="utf-8",
|
||||
errors="ignore",
|
||||
)
|
||||
f.write(json.dumps(results, ensure_ascii=False, indent=4))
|
||||
f.close()
|
||||
percentageOfCompletion = taskCounter / tasks * 100
|
||||
print("process complete: " + str(percentageOfCompletion) + "%")
|
||||
except Exception as e:
|
||||
print(e)
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue