diff --git a/.gitignore b/.gitignore index 1dce6d6..8928756 100644 --- a/.gitignore +++ b/.gitignore @@ -159,7 +159,10 @@ cython_debug/ # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore # and can be added to the global gitignore or merged into this file. For a more nuclear # option (not recommended) you can uncomment the following to ignore the entire idea folder. -#.idea/ +.idea/ -output/* \ No newline at end of file +output/* +*.bkp + +docs/_site/ \ No newline at end of file diff --git a/docs/_config.yaml b/docs/_config.yaml index 3dc5370..f2f4fa2 100644 --- a/docs/_config.yaml +++ b/docs/_config.yaml @@ -2,8 +2,9 @@ title: Scenery Builder description: Model-to-model transformation of floorplan models to execution artefacts (including ROS simulation) theme: just-the-docs -url: https://secorolab.github.io/scenery_builder - +baseurl: "/scenery_builder" # the subpath of your site, e.g. /blog +url: https://secorolab.github.io +repository: secorolab/scenery_builder # for github-metadata permalink: pretty defaults: diff --git a/pyproject.toml b/pyproject.toml index 0235aaf..3f5ce88 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -14,7 +14,9 @@ dependencies = [ "rdflib@git+https://github.com/secorolab/rdflib@fix/multi-type-scoped-context", "click>=8.1.7", "pyyaml", - "floorplan_dsl@git+https://github.com/secorolab/FloorPlan-DSL" + "floorplan_dsl@git+https://github.com/secorolab/FloorPlan-DSL", + "transforms3d", + "ifcld@git+https://github.com/secorolab/ifcld" ] requires-python = ">= 3.11" readme = "README.md" diff --git a/src/fpm/__init__.py b/src/fpm/__init__.py index e69de29..fa6b001 100644 --- a/src/fpm/__init__.py +++ b/src/fpm/__init__.py @@ -0,0 +1,41 @@ +import logging + + +class AnsiColorFormatter(logging.Formatter): + def format(self, record: logging.LogRecord): + no_style = "\033[0m" + bold = "\033[91m" + grey = "\033[90m" + yellow = "\033[93m" + red = "\033[31m" + red_light = "\033[91m" + blue = "\033[34m" + start_style = { + "DEBUG": grey, + "INFO": no_style, + "WARNING": yellow, + "ERROR": red_light, + "CRITICAL": red + bold, + }.get(record.levelname, no_style) + end_style = no_style + return f"{start_style}{super().format(record)}{end_style}" + + +logger = logging.getLogger("floorplan") +logger.setLevel(logging.INFO) +fh = logging.FileHandler("floorplan-cli.log") +fh.setLevel(logging.DEBUG) +ch = logging.StreamHandler() +ch.setLevel(logging.INFO) +file_formatter = logging.Formatter( + "{asctime} | {levelname:<8s} | {name:<30s} | {message}", + style="{", +) +console_formatter = AnsiColorFormatter( + "{asctime} | {levelname:<8s} | {name:<30s} | {message}", + style="{", +) +# ch.setFormatter(console_formatter) +fh.setFormatter(file_formatter) +logger.addHandler(ch) +logger.addHandler(fh) diff --git a/src/fpm/cli.py b/src/fpm/cli.py index 6ef68b8..4b8a571 100644 --- a/src/fpm/cli.py +++ b/src/fpm/cli.py @@ -1,6 +1,8 @@ import os import click +import logging +from fpm.generators.dot import visualize_frame_tree from fpm.graph import build_graph_from_directory, get_floorplan_model_name from fpm.generators.gazebo import gazebo_world, door_object_models from fpm.generators.tasks import get_disinfection_tasks @@ -8,14 +10,25 @@ from fpm.generators.mesh import get_3d_mesh from fpm.generators.polyline import get_polyline_floorplan from fpm.generators.door_keyframes import get_keyframes +from fpm.generators.tts import ( + gen_tts_wall_description, + gen_tts_task_description, + gen_ros_frames, +) +from fpm.generators.scenery import generate_fpm_rep_from_rdf from textx import generator_for_language_target, metamodel_for_language +logger = logging.getLogger("floorplan.cli") +logger.setLevel(logging.DEBUG) + def configure(ctx, param, filename): if not filename: return import tomllib + logger.debug("Using config file: %s", filename) + ctx.default_map = dict() with open(filename, "rb") as f: data = tomllib.load(f) @@ -117,14 +130,15 @@ def transform(ctx, model_path, output_path, **kwargs): This requires that the [FloorPlan DSL](https://github.com/secorolab/FloorPlan-DSL) is installed. """ - print(model_path, output_path) + logger.debug("transform command arguments: %s", kwargs) + logger.debug("%s %s", model_path, output_path) generator = generator_for_language_target("fpm", "json-ld") mm = metamodel_for_language("fpm") model = mm.model_from_file(model_path) try: generator(mm, model, output_path, overwrite=True) except Exception as e: - print(f"Error transforming model: {e}") + logger.error(f"Error transforming model: {e}") @floorplan.command(short_help="Generate FPM variations from a variation model") @@ -181,10 +195,10 @@ def variation(ctx, model_path, variations, seed, output_path, **kwargs): See https://github.com/secorolab/FloorPlan-DSL/blob/devel/docs/tutorials/variation.md for more information on creating variation models. """ - print(f"Generating {variations} variation(s) from {model_path}") - print(f"Output path: {output_path}") + logger.info(f"Generating {variations} variation(s) from {model_path}") + logger.info(f"Output path: {output_path}") if seed is not None: - print(f"Using seed: {seed}") + logger.info(f"Using seed: {seed}") generator = generator_for_language_target("fpm-variation", "fpm") mm = metamodel_for_language("fpm-variation") @@ -200,7 +214,31 @@ def variation(ctx, model_path, variations, seed, output_path, **kwargs): seed=seed, ) except Exception as e: - print(f"Error generating variations: {e}") + logger.error(f"Error generating variations: {e}") + + +@floorplan.command( + short_help="Generate FPM JSON-LD models from an IFCLD model", +) +@click.pass_context +@click.option( + "-m", + "--model", + "model_path", + type=click.Path(exists=True, resolve_path=True, file_okay=True, dir_okay=False), + required=True, + help="Path to the fpm model to transform into JSON-LD", +) +@click.option( + "-o", + "--output-path", + type=click.Path(exists=True, resolve_path=True), + default=os.path.join("."), + help="Output path for generated artefacts", +) +def ifc(ctx, model_path, output_path, **kwargs): + + generate_fpm_rep_from_rdf(model_path, output_path) @floorplan.group( @@ -245,9 +283,13 @@ def variation(ctx, model_path, variations, seed, output_path, **kwargs): def generate(ctx, inputs, **kwargs): """Generate execution artefacts from JSON-LD models""" - print(kwargs) + logger.debug("generate command arguments: inputs: %s, kwargs: %s", inputs, kwargs) + _load_graph_to_ctx(ctx, inputs) + - g = build_graph_from_directory(inputs) +def _load_graph_to_ctx(ctx, input_paths): + logger.debug("Loading graph from paths: %s", input_paths) + g = build_graph_from_directory(input_paths) try: model_name = get_floorplan_model_name(g) except ValueError as e: @@ -260,6 +302,18 @@ def generate(ctx, inputs, **kwargs): @generate.command(short_help="Generate a 3D-mesh of the floorplan") @click.pass_context +@click.option( + "--include-doors", + is_flag=True, + help="Flag to indicate that the mesh should include the door meshes", +) +@click.option( + "--format", + type=click.Choice(["stl", "gltf"], case_sensitive=False), + default="stl", + show_default=True, + help="Output format of the 3D mesh", +) def mesh(ctx, **kwargs): """Generate a 3D-mesh in STL or gltF 2.0 format""" get_3d_mesh(**ctx.obj, **ctx.parent.params, **kwargs) @@ -412,8 +466,25 @@ def gazebo(ctx, **kwargs): show_default=True, help="Value for cells to be considered free in the occupancy map", ) +@click.option( + "--source", + type=click.Choice(["fpm", "bim"], case_sensitive=False), + default="fpm", + show_default=True, + help="ROS version for launch files", +) +@click.option( + "--visualize-frames", + type=click.Choice( + ["wall", "door", "entryway", "space", "opening"], case_sensitive=False + ), + help="Which element frames to visualize", + multiple=True, +) def occ_grid(ctx, **kwargs): """Generate the occupancy grid map of the floorplan""" + logger.info("Generating occupancy grid...") + logger.debug("Arguments: %s", kwargs) get_occ_grid(**ctx.obj, **ctx.parent.params, **kwargs) @@ -469,6 +540,91 @@ def door_keyframes(ctx, **kwargs): get_keyframes(**ctx.obj, **ctx.parent.params, **kwargs) +@generate.command() +@click.pass_context +@click.option( + "--robot-translation-x", + "x", + type=click.FLOAT, + default=-1.0, + show_default=True, + help="Translation of the robot in x wrt to a task element for a navigation goal", +) +@click.option( + "--robot-translation-z", + "z", + type=click.FLOAT, + default=1.7, + show_default=True, + help="Translation of the robot in z wrt to a task element for a navigation goal", +) +@click.option( + "--ros-frames", + is_flag=True, + help="Generate a ROS launch file with frame transformations for task elements", +) +@click.option( + "--visualize", + is_flag=True, + help="Generate images visualizing the environment and tasks", +) +def tts(ctx, **kwargs): + """Generate the artefacts for the TTS simulator""" + logger.info("Generating artefacts for the TTS simulator...") + logger.debug("Arguments: %s", kwargs) + gen_tts_wall_description(**ctx.obj, **ctx.parent.params, **kwargs) + outlets, ducts = gen_tts_task_description(**ctx.obj, **ctx.parent.params, **kwargs) + if kwargs.get("ros_frames"): + gen_ros_frames(**ctx.obj, **ctx.parent.params, **kwargs) + if kwargs.get("visualize"): + logger.info("Visualizing milling task for the outlets on occupancy grid") + get_occ_grid( + **ctx.obj, **ctx.parent.params, **kwargs, outlets=outlets, source="bim" + ) + get_occ_grid( + **ctx.obj, **ctx.parent.params, **kwargs, ducts=ducts, source="bim" + ) + logger.info("Visualizing frames on occupancy grid") + get_occ_grid( + **ctx.obj, + **ctx.parent.params, + **kwargs, + visualize_frames=["outlet", "duct", "wall"], + source="bim", + ) + + +@floorplan.command(short_help="Visualize aspects of a floorplan model") +@click.pass_context +@click.option( + "-i", + "--inputs", + "--input-path", + type=click.Path(exists=True, resolve_path=True), + required=True, + multiple=True, + help="Path with JSON-LD models to be used as inputs", +) +@click.option( + "-o", + "--output-path", + type=click.Path(exists=True, resolve_path=True), + default=os.path.join("."), + help="Output path for generated artefacts", +) +def visualize(ctx, inputs, output_path, **kwargs): + floorplan_elements = ["Space", "Opening", "Wall", "Door", "Entryway", "DoorPanel"] + print(ctx.obj, ctx.parent.params) + _load_graph_to_ctx(ctx, inputs) + visualize_frame_tree( + output_path=output_path, + floorplan_elements=floorplan_elements, + **ctx.obj, + # **ctx.parent.params, + **kwargs, + ) + + if __name__ == "__main__": import sys diff --git a/src/fpm/generators/dot.py b/src/fpm/generators/dot.py new file mode 100644 index 0000000..0b3f233 --- /dev/null +++ b/src/fpm/generators/dot.py @@ -0,0 +1,26 @@ +import subprocess + +from fpm.graph import get_floorplan_elements, get_frame_tree +from fpm.utils import load_template, save_file + + +def visualize_frame_tree(g, floorplan_elements, **kwargs): + import os + + output_path = kwargs.get("output_path") + print(kwargs) + element_poses = get_floorplan_elements(g, floorplan_elements) + frames = get_frame_tree(g, element_poses) + + template = load_template("frame-tree.dot.jinja") + + file_name = "frame-tree" + frames_gv = "{:s}.gv".format(file_name) + frames_pdf = "{:s}.pdf".format(file_name) + contents = template.render(frames=frames) + save_file(output_path, frames_gv, contents) + + dot_file_path = os.path.join(output_path, frames_gv) + pdf_file_path = os.path.join(output_path, frames_pdf) + cmd = ["dot", "-Tpdf", dot_file_path, "-o", pdf_file_path] + subprocess.Popen(cmd).communicate() diff --git a/src/fpm/generators/mesh.py b/src/fpm/generators/mesh.py index 29cba9b..6fcd08a 100644 --- a/src/fpm/generators/mesh.py +++ b/src/fpm/generators/mesh.py @@ -1,3 +1,4 @@ +import logging import bpy from fpm.transformations.blender import ( @@ -9,9 +10,13 @@ from fpm.graph import get_floorplan_model_name, get_3d_structure from fpm.utils import save_file, get_output_path +logger = logging.getLogger("floorplan.generators.mesh") +logger.setLevel(logging.DEBUG) -def generate_3d_mesh(g, output_path, **custom_args): + +def generate_3d_mesh(g, output_path, include_doors=False, **custom_args): file_format = custom_args.get("format", "stl") + logger.info("Generating 3D mesh in %s format", file_format) model_name = get_floorplan_model_name(g) @@ -19,20 +24,33 @@ def generate_3d_mesh(g, output_path, **custom_args): # clear the blender scene clear_scene() - print("Getting 3D structures") + logger.debug("Getting 3D structure for walls") elements = get_3d_structure(g, "Wall") create_element_mesh(building, elements) + logger.debug("Getting 3D structure for columns") columns = get_3d_structure(g, "Column") create_element_mesh(building, columns) + logger.debug("Getting 3D structure for dividers") dividers = get_3d_structure(g, "Divider") create_element_mesh(building, dividers) + if include_doors: + logger.debug("Getting 3D structure for doors") + doors = get_3d_structure(g, "Door") + create_element_mesh(building, doors) + + logger.debug("Getting 3D structure for door linings") + door_linings = get_3d_structure(g, "DoorLining") + create_element_mesh(building, door_linings) + + logger.debug("Getting 3D structure for entryways") entryways = get_3d_structure(g, "Entryway") create_element_mesh(building, entryways) subtract_opening(entryways) + logger.debug("Getting 3D structure for windows") windows = get_3d_structure(g, "Window") create_element_mesh(building, windows) subtract_opening(windows) diff --git a/src/fpm/generators/occ_grid.py b/src/fpm/generators/occ_grid.py index 398444d..e339681 100644 --- a/src/fpm/generators/occ_grid.py +++ b/src/fpm/generators/occ_grid.py @@ -1,5 +1,7 @@ import os +import logging +import matplotlib.pyplot as plt import numpy as np from PIL import Image, ImageDraw, ImageOps @@ -9,22 +11,28 @@ get_floorplan_model_name, get_element_points, get_opening_points, - get_waypoint_coord, + get_waypoint_coord_wrt_world, get_waypoint_coord_list, + get_frame_transform, ) from fpm.utils import load_template, save_file, get_output_path -from fpm.constants import FPMODEL +from fpm.visualization.plot import plot_2d_frame, plot_2d_robot + +logger = logging.getLogger("floorplan.generators.occ_grid") +logger.setLevel(logging.DEBUG) def generate_occ_grid(g, output_path, **custom_args): + plt.clf() map_name = get_floorplan_model_name(g) + logger.info("Map name: {}".format(map_name)) - resolution = custom_args.get("map_resolution", 0.05) + resolution = custom_args.get("resolution", 0.05) - unknown = custom_args.get("map_unknown_value", 200) - occupied = custom_args.get("map_occupied_value", 0) - free = custom_args.get("map_free_value", 255) - border = custom_args.get("map_border", 50) + unknown = custom_args.get("unknown_value", 200) + occupied = custom_args.get("occupied_value", 0) + free = custom_args.get("free_value", 255) + border = custom_args.get("border", 50) if "{{model_name}}" in output_path: output_path = output_path.replace("{{model_name}}", map_name) @@ -34,9 +42,12 @@ def generate_occ_grid(g, output_path, **custom_args): points = [] directions = [] + logger.debug("Getting coordinates map") coords_m = get_coordinates_map(g) + logger.debug("Getting space points") space_points = get_space_points(g) for s in space_points: + logger.debug("Getting waypoint coords") w_coords = get_waypoint_coord_list(g, s.get("points"), coords_m) w_coords = np.array(w_coords) @@ -78,25 +89,65 @@ def generate_occ_grid(g, output_path, **custom_args): draw = ImageDraw.Draw(im) # Draw free space from floorplan spaces (rooms) + logger.debug("Drawing free space") draw_floorplan_element(points, draw, free, west=west, south=south, **custom_args) # Draw obstacles (walls and columns) + logger.debug("Drawing walls") draw_floorplan_obstacle( g, "Wall", draw, west, south, occupied, coords_m, **custom_args ) + logger.debug("Drawing columns") draw_floorplan_obstacle( g, "Column", draw, west, south, occupied, coords_m, **custom_args ) + logger.debug("Drawing dividers") draw_floorplan_obstacle( g, "Divider", draw, west, south, occupied, coords_m, **custom_args ) # Clear out wall openings; mark them as free space + logger.debug("Drawing entryways") draw_floorplan_opening( g, "Entryway", draw, west, south, free, coords_m, **custom_args ) # draw_floorplan_opening(g, "Window", draw, west, south, resolution, border, free, coords_m) + if custom_args.get("outlets"): + logger.debug("Drawing outlet task elements") + draw_tasks( + g, + im, + center, + tasks="outlets", + map_name=map_name, + output_path=output_path, + **custom_args, + ) + if custom_args.get("ducts"): + logger.debug("Drawing duct task elements") + draw_tasks( + g, + im, + center, + tasks="ducts", + map_name=map_name, + output_path=output_path, + **custom_args, + ) + + for frame_type in custom_args.get("visualize_frames", []): + plt.clf() + logger.debug("Drawing frames for %s", frame_type) + draw_frames( + g, + im, + center, + map_name=map_name, + output_path=output_path, + frame_type=frame_type, + **custom_args, + ) im = ImageOps.flip(im) for ext in ["pgm", "jpg"]: @@ -108,7 +159,7 @@ def draw_floorplan_obstacle(g, element, draw, west, south, fill, coords_map, **k column_points = get_element_points(g, element) c_points = list() - laser_height = kwargs.get("map_laser_height", 0.7) + laser_height = kwargs.get("laser_height", 0.7) for s in column_points: height = s.get("height") if laser_height > height: @@ -134,29 +185,43 @@ def draw_floorplan_obstacle(g, element, draw, west, south, fill, coords_map, **k def draw_floorplan_opening(g, element, draw, west, south, fill, coords_map, **kwargs): opening_points = get_opening_points(g, element) resolution = kwargs.get("resolution", 0.05) - laser_height = kwargs.get("map_laser_height", 0.7) + laser_height = kwargs.get("laser_height", 0.7) + + source = kwargs.get("source", "fpm") all_points = list() for opening in opening_points: opening_height_max = 0.0 opening_height_min = float("inf") for face in opening: - z_vals = [p.get("z") for p in face] + points = [get_waypoint_coord_wrt_world(g, p, coords_map) for p in face] + if source == "fpm": + z_vals = [p.get("z") for p in face] + else: + z_vals = [z for x, y, z in points] if not np.all(np.array(z_vals) == z_vals[0]): # Only process faces that are parallel to the floor (where the z is the same) continue - f_coords = list() - for p in face: - if p["y"] == 0.0: - p["y"] = p["y"] - resolution - else: - p["y"] = p["y"] + resolution - x, y, z = get_waypoint_coord(g, p, coords_map) - f_coords.append([x, y, 0, 1]) - if z > opening_height_max: - opening_height_max = z - elif z < opening_height_min: - opening_height_min = z + + # TODO how to make the 3d object slightly larger than the wall when we can't rely on a convention for the axis direction + # TODO Check if there is an alternative for floorplan models that do follow a convention + if source == "fpm": + f_coords, opening_height_max, opening_height_min = ( + get_fpm_opening_points( + face, + opening_height_max, + opening_height_min, + resolution, + g, + coords_map, + ) + ) + else: + f_coords, opening_height_max, opening_height_min = ( + get_bim_opening_points( + points, opening_height_max, opening_height_min + ) + ) if opening_height_min <= laser_height <= opening_height_max: f_coords = np.array(f_coords) all_points.append(f_coords) @@ -164,11 +229,44 @@ def draw_floorplan_opening(g, element, draw, west, south, fill, coords_map, **kw draw_floorplan_element(all_points, draw, fill, west=west, south=south, **kwargs) +def get_bim_opening_points(points, opening_height_max, opening_height_min): + # BIM: No assumptions for frames + coords = list() + for x, y, z in points: + coords.append([x, y, 0, 1]) + if z > opening_height_max: + opening_height_max = z + elif z < opening_height_min: + opening_height_min = z + + return coords, opening_height_max, opening_height_min + + +def get_fpm_opening_points( + face, opening_height_max, opening_height_min, resolution, graph, coords_map +): + # FPM: Assumptions for direction of XYZ vectors + coords = list() + for p in face: + if p["y"] == 0.0: + p["y"] = p["y"] - resolution + else: + p["y"] = p["y"] + resolution + x, y, z = get_waypoint_coord_wrt_world(graph, p, coords_map) + coords.append([x, y, 0, 1]) + if z > opening_height_max: + opening_height_max = z + elif z < opening_height_min: + opening_height_min = z + + return coords, opening_height_max, opening_height_min + + def draw_floorplan_element(points, draw, fill, **kwargs): west = kwargs.get("west") south = kwargs.get("south") - resolution = kwargs.get("map_resolution", 0.05) - border = kwargs.get("map_border", 50) + resolution = kwargs.get("resolution", 0.05) + border = kwargs.get("border", 50) for shape in points: element_shape = get_2d_shape(west, south, resolution, border, shape=shape) @@ -210,3 +308,83 @@ def save_map_metadata(output_path, map_name, center, **custom_args): def get_occ_grid(g, base_path, **kwargs): output_path = get_output_path(base_path, "maps") generate_occ_grid(g, output_path, **kwargs) + + +def draw_tasks(g, im, center, tasks, **kwargs): + resolution = kwargs.get("resolution", 0.05) + w, h = im.size + border = kwargs.get("border", 50) + orig_x, orig_y, _ = center + + logger.info("Drawing tasks: %s", tasks) + + imax = plt.imshow( + im, + cmap="gray", + interpolation="none", + origin="upper", + extent=( + orig_x, + (w * resolution) - abs(orig_x), + (h * resolution) - abs(orig_y), + orig_y, + ), + ) + fig = imax.get_figure() + ax = fig.get_axes().pop() + for task in kwargs.get(tasks, []): + name = task["name"] + nav_pose = task.get("nav_pose") + plot_2d_robot(ax, nav_pose, 1.4, 1.9) + nav_pose[3, 3] = 0.25 + plot_2d_frame(ax, nav_pose, name) + + milling_task = np.array(task.get("milling_vector")) + + ax.scatter( + milling_task[0, 0], + milling_task[0, 1], + c="red", + marker=".", + s=15, + ) + ax.plot(milling_task[:, 0], milling_task[:, 1], color="yellow") + map_name = kwargs.get("map_name") + output_path = kwargs.get("output_path") + name_image = "tasks-{}-{}.{}".format(tasks, map_name, "jpg") + ax.yaxis.set_inverted(False) + ax.set_aspect("equal", adjustable="box") + + fig.savefig(os.path.join(output_path, name_image), dpi=300) + + +def draw_frames(g, im, center, map_name, output_path, frame_type, **kwargs): + + resolution = kwargs.get("resolution", 0.05) + w, h = im.size + orig_x, orig_y, _ = center + + imax = plt.imshow( + im, + cmap="gray", + interpolation="none", + origin="upper", + extent=( + orig_x, + (w * resolution) - abs(orig_x), + (h * resolution) - abs(orig_y), + orig_y, + ), + ) + fig = imax.get_figure() + ax = fig.get_axes().pop() + name_image = "{}-frames-{}.{}".format(frame_type, map_name, "jpg") + matrices = get_frame_transform(g, frame_type) + for m in matrices: + m[3, 3] = 0.25 + plot_2d_frame(ax, m) + + ax.yaxis.set_inverted(False) + ax.set_aspect("equal", adjustable="box") + plt.tight_layout() + fig.savefig(os.path.join(output_path, name_image), dpi=300) diff --git a/src/fpm/generators/polyline.py b/src/fpm/generators/polyline.py index e17a6e3..253e387 100644 --- a/src/fpm/generators/polyline.py +++ b/src/fpm/generators/polyline.py @@ -1,11 +1,16 @@ +import logging + from fpm.graph import get_floorplan_model_name, get_internal_walls from fpm.utils import save_file, load_template, get_output_path +logger = logging.getLogger("floorplan.generators.polyline") +logger.setLevel(logging.DEBUG) + def generate_polyline_representation( g, output_path, template_name="polyline.poly.jinja", template_path=None, **kwargs ): - print("Generating polyline representation...") + logger.info("Generating polyline representation...") model_name = get_floorplan_model_name(g) file_name = kwargs.get("file_name", "{}.poly".format(model_name)) wall_planes_by_space = get_internal_walls(g) diff --git a/src/fpm/generators/ros.py b/src/fpm/generators/ros.py index cb20525..e4a4b5d 100644 --- a/src/fpm/generators/ros.py +++ b/src/fpm/generators/ros.py @@ -1,5 +1,10 @@ +import logging + from fpm.utils import load_template, save_file, get_output_path +logger = logging.getLogger("floorplan.generators.ros") +logger.setLevel(logging.DEBUG) + def generate_launch_file(model_name, output_path, file_name, **custom_args): template_name = custom_args.get("template_name", "world.launch.jinja") @@ -14,6 +19,7 @@ def generate_launch_file(model_name, output_path, file_name, **custom_args): def gazebo_world_launch(model_name, base_path, **kwargs): + logger.info("Generating gazebo world launch file") template_path = kwargs.get("template_path") output_path = get_output_path(base_path, "ros/launch") diff --git a/src/fpm/generators/scenery.py b/src/fpm/generators/scenery.py new file mode 100644 index 0000000..f895362 --- /dev/null +++ b/src/fpm/generators/scenery.py @@ -0,0 +1,1005 @@ +import json +import os +import logging + +import rdflib +from rdflib import Graph, RDF +from rdflib.plugins.sparql import prepareQuery + +from fpm.graph import get_list_values, get_list_from_ptr +from fpm.utils import load_template, save_file +from ifcld.interpreters.namespaces import IFC_CONCEPTS +from ifcld.query import units_query, convert_units_query, project_units_query + +logger = logging.getLogger("floorplan.generators.scenery") +logger.setLevel(logging.DEBUG) + + +def add_polyhedron_faces(floorplan): + from itertools import pairwise + + for f in floorplan: + if "Polyhedron" not in f.get("@type", list()): + continue + points = f["points"] + bottom = points[:4] + top = points[4:] + + if f["faces"]: + continue + f["faces"].append(list(bottom)) + f["faces"].append(list(top)) + + bottom.append(bottom[0]) + top.append(top[0]) + + for b, t in zip(pairwise(bottom), pairwise(top)): + p1, p2 = b + p4, p3 = t + f["faces"].append([p1, p2, p3, p4]) + + +def generate_fpm_rep_from_rdf(model_path, output_path): + logger.debug("Output path: %s", output_path) + model_name = os.path.basename(model_path).lower().replace(".ifc.json", "") + + # RDF graph + g = Graph() + g.parse(model_path, format="json-ld") + g.bind( + "ifc-model", + "https://secorolab.github.io/models/{}/ifc/data#".format(model_name), + ) + g.bind( + "ifc", + "https://secorolab.github.io/metamodels/ifc/", + ) + + # Get the FPM context template for this model + fp_ctx_template = load_template("ifc/fpm-context.json.jinja") + fpm_ctx = json.loads(fp_ctx_template.render(model_id=model_name)) + + # Add FloorPlan node + logger.debug("Adding floorplan node") + floorplan = [ + {"@id": model_name, "@type": "FloorPlan"}, + { + "@id": "world-frame", + "@type": "Frame", + "origin": "world-origin", + }, + {"@id": "world-origin", "@type": ["3D", "Euclidean", "Point"]}, + ] + save_file( + output_path, + "{}.floorplan.fpm.json".format(model_name), + {"@graph": floorplan, "@context": fpm_ctx}, + ) + + length_unit = str(g.namespace_manager.curie(query_ifc_units(g)[0])).split(":")[-1] + + logger.info("Transforming IFC local placements...") + placements = query_ifc_local_placements(g, length_unit) + save_file( + output_path, + "{}.placement.fpm.json".format(model_name), + {"@graph": placements, "@context": fpm_ctx}, + ) + + logger.info("Transforming IFC walls...") + walls = query_ifc_walls(g, length_unit) + save_file( + output_path, + "{}.walls.fpm.json".format(model_name), + {"@graph": walls, "@context": fpm_ctx}, + ) + + logger.info("Transforming IFC doors...") + doors = query_ifc_doors(g, length_unit) + save_file( + output_path, + "{}.doors.fpm.json".format(model_name), + {"@graph": doors, "@context": fpm_ctx}, + ) + + logger.info("Transforming IFC spaces...") + spaces = query_ifc_spaces(g, model_name, length_unit) + save_file( + output_path, + "{}.spaces.fpm.json".format(model_name), + {"@graph": spaces, "@context": fpm_ctx}, + ) + + logger.info("Transforming task elements...") + task_elements = query_ifc_task_elements(g, length_unit) + save_file( + output_path, + "{}.task.fpm.json".format(model_name), + {"@graph": task_elements, "@context": fpm_ctx}, + ) + + stats(g) + + # doc = list() + # for l in [placements, walls, doors, spaces]: + # doc.extend(l) + # + # # compacted = jsonld.compact(doc, fpm_ctx.get("@context")) + # full_doc = dict(**fpm_ctx) + # full_doc["@graph"] = doc + # + # save_file(output_path, "{}.compacted.fpm.json".format(model_name), full_doc) + + +def get_entity_id(g, e, entity_type="placement"): + return g.namespace_manager.curie(e).replace("ifc-model:", "{}-".format(entity_type)) + + +def render_ifc_template(template_path, **kwargs): + template = load_template(template_path) + content = template.render(**kwargs) + if kwargs.get("debug"): + print("\n**\n", content) + return json.loads(content) + + +def query_ifc_local_placements(g: Graph, length_unit): + placements = list() + world_frame = list() + + for p in g.subjects(RDF.type, IFC_CONCEPTS["IFCLOCALPLACEMENT"]): + entity = get_entity_id(g, p) + e = render_ifc_template( + "ifc/placement/object-placement.json.jinja", + placement_id=entity, + ) + placements.extend(e) + + prt = g.value(p, IFC_CONCEPTS["placementrelto"]) + if prt is not None: + prt_entity = get_entity_id(g, prt) + pj = render_ifc_template( + "ifc/placement/placement-rel-to.json.jinja", + placement_id=entity, + ref_placement_id=prt_entity, + ) + else: + # TODO Adding world_frame to all local placements, + # but this needs to be reviewed as it depends on the geometric context + pj = render_ifc_template( + "ifc/placement/placement-rel-to.json.jinja", + placement_id=entity, + world_frame=True, + ) + world_frame.append(pj) + placements.extend(pj) + + rp = g.value(p, IFC_CONCEPTS["relativeplacement"]) + ap = transform_axis_placement_3d(g, rp, entity, length_unit) + placements.extend(ap) + + return placements + + +def transform_axis_placement_3d(g: Graph, rel_placement, entity, length_unit): + placement = list() + vars = dict( + placement_id=entity, + g=g, + relative_placement=rel_placement, + IFC_CONCEPTS=IFC_CONCEPTS, + rdflib=rdflib, + list=list, + length_unit=length_unit, + ) + + axis = g.value(rel_placement, IFC_CONCEPTS["axis"]) + if axis is not None: + a = render_ifc_template("ifc/placement/rel-placement-axis.json.jinja", **vars) + placement.extend(a) + + refdir = g.value(rel_placement, IFC_CONCEPTS["refdirection"]) + if refdir is not None: + r = render_ifc_template( + "ifc/placement/rel-placement-refdirection.json.jinja", **vars + ) + placement.extend(r) + + c = render_ifc_template("ifc/placement/rel-placement-coords.json.jinja", **vars) + placement.extend(c) + return placement + + +def query_placement_rel_to(g: Graph, placement): + parent = g.value(placement, IFC_CONCEPTS["placementrelto"]) + + while parent is not None: + new_parent = g.value(parent, IFC_CONCEPTS["placementrelto"]) + if new_parent is None: + return parent + else: + parent = new_parent + + +def query_ifc_walls(g: Graph, length_unit): + wall_concepts = [ + "IFCWALLSTANDARDCASE", + "IFCWALL", + "IFCWALLELEMENTEDCASE", + ] + + rep_query = """ + SELECT DISTINCT ?wall ?placement + WHERE { + ?wall rdf:type ifc:IFCWALL . + ?wall ifc:objectplacement ?placement . + } + """ + + qres = g.query(rep_query) + logger.info("Total walls: %d", len(qres)) + wall_json = list() + for r in qres: + wall_id = get_entity_id(g, r["wall"], "wall") + placement_id = get_entity_id(g, r["placement"], "placement") + logger.debug("%s: %s", wall_id, placement_id) + parent = query_placement_rel_to(g, r["placement"]) + logger.debug( + "Highest level parent frame: %s", get_entity_id(g, parent, "placement") + ) + + w = render_ifc_template("ifc/walls/wall-entity.json.jinja", wall_id=wall_id) + wall_json.append(w) + + # TODO this is hardcoded for now (list of length 1 for this type of wall) + representation = query_product_shape_representations(g, r["wall"])[0] + for s in g.objects(representation, IFC_CONCEPTS["items"]): + depth, position, _, _, g_contents = transform_extruded_area_solid( + g, wall_id, s, length_unit + ) + wall_json.extend(g_contents) + + w_rep = render_ifc_template( + "ifc/walls/wall-representation.json.jinja", + wall_id=wall_id, + depth=depth, + length_unit=length_unit, + ) + wall_json.append(w_rep) + + # Wall placement + # TODO for now it assumes position is not None + # TODO this also implies that position is 0, 0? + ap = transform_axis_placement_3d(g, position, wall_id, length_unit) + wall_json.extend(ap) + op = render_ifc_template( + "ifc/placement/object-placement.json.jinja", + placement_id=wall_id, + ) + wall_json.extend(op) + rp = render_ifc_template( + "ifc/placement/placement-rel-to.json.jinja", + placement_id=wall_id, + ref_placement_id=placement_id, + ) + wall_json.extend(rp) + + return wall_json + + +def transform_extruded_area_solid( + g: Graph, element_id, representation, length_unit, parent_id=None +): + logger.debug("Transforming extruded area solid %s", element_id) + graph_contents = list() + if parent_id is None: + parent_id = element_id + depth, position, swept_area, ext_dir = query_extruded_area_solid(g, representation) + + swept_area_type = g.value(swept_area, RDF["type"]) + if swept_area_type == IFC_CONCEPTS["IFCARBITRARYCLOSEDPROFILEDEF"]: + coords = query_arbitrary_closed_profile(g, swept_area) + + w_polygon = render_ifc_template( + "ifc/walls/wall-polygon.json.jinja", + parent_id=parent_id, + element_id=element_id, + coords=coords, + length_unit=length_unit, + ) + graph_contents.extend(w_polygon) + w_polyhedron = render_ifc_template( + "ifc/walls/wall-polyhedron.json.jinja", + parent_id=parent_id, + element_id=element_id, + coords=coords, + depth=depth, + extruded_dir=ext_dir, + length_unit=length_unit, + ) + graph_contents.extend(w_polyhedron) + add_polyhedron_faces(graph_contents) + return depth, position, coords, ext_dir, graph_contents + elif swept_area_type == IFC_CONCEPTS["IFCCIRCLEPROFILEDEF"]: + center_pos = g.value(swept_area, IFC_CONCEPTS["position"]) + radius = g.value(swept_area, IFC_CONCEPTS["radius"]) + circle_id = get_entity_id(g, swept_area, "circle-profile") + # center = transform_axis_placement_3d(g, position, parent_id) + poly = render_ifc_template( + "ifc/base/circle-profile.json.jinja", + parent_id=parent_id, + element_id=element_id, + point_id=circle_id, + radius=radius, + depth=depth, + extruded_dir=ext_dir, + relative_placement=center_pos, + rdflib=rdflib, + list=list, + IFC_CONCEPTS=IFC_CONCEPTS, + g=g, + length_unit=length_unit, + ) + graph_contents.extend(poly) + return depth, position, None, ext_dir, graph_contents + elif swept_area_type == IFC_CONCEPTS["IFCRECTANGLEPROFILEDEF"]: + xdim = g.value(swept_area, IFC_CONCEPTS["xdim"]) + ydim = g.value(swept_area, IFC_CONCEPTS["ydim"]) + x = xdim.toPython() / 2 + y = ydim.toPython() / 2 + z = depth.toPython() * ext_dir[-1].toPython() + coords = [ + [-x, -y], + [-x, y], + [x, y], + [x, -y], + [-x, -y, z], + [-x, y, z], + [x, y, z], + [x, -y, z], + ] + poly = render_ifc_template( + "ifc/base/rectangle-profile.json.jinja", + parent_id=parent_id, + element_id=element_id, + depth=depth, + extruded_dir=ext_dir, + coords=coords, + length_unit=length_unit, + ) + add_polyhedron_faces(poly) + graph_contents.extend(poly) + return depth, position, coords, ext_dir, graph_contents + else: + logger.warning("Support for {} not implemented yet".format(swept_area_type)) + + +def query_extruded_area_solid(g: Graph, representation): + rep_query = """ + SELECT ?ext_dir ?depth ?swept_area ?position + WHERE { + ?representation ifc:depth ?depth . + ?representation ifc:sweptarea ?swept_area . + ?representation ifc:position ?position . + ?representation ifc:extrudeddirection ?ext_dir . + } + """ + q = prepareQuery(rep_query, initNs={"ifc": IFC_CONCEPTS, "rdf": RDF}) + + qres = g.query(q, initBindings={"representation": representation}) + assert len(qres) == 1 + res = list(qres)[0] + ext_dir = get_list_values(g, res["ext_dir"], IFC_CONCEPTS["directionratios"]) + return res["depth"], res["position"], res["swept_area"], ext_dir + + +def query_arbitrary_closed_profile(g: Graph, profile): + rep_query = """ + SELECT ?points + WHERE { + ?swept_area ifc:outercurve/ifc:points ?points . + } + """ + q = prepareQuery(rep_query, initNs={"ifc": IFC_CONCEPTS, "rdf": RDF}) + + qres = g.query(q, initBindings={"swept_area": profile}) + assert len(qres) == 1 + res = list(qres)[0] + + coords = list() + for ptr in get_list_values(g, res["points"], IFC_CONCEPTS["coordlist"]): + c = get_list_from_ptr(g, ptr) + c = [coord.toPython() for coord in c] + coords.append(c) + + return coords + + +def query_product_shape_representations(g: Graph, product): + # TODO it would be better to pass target_view, context identifier and type + # as arguments, but I couldn't find a way to pass them to the query + rep_query = """ + SELECT ?representation + WHERE { + ?object ifc:representation/ifc:representations/rdf:rest*/rdf:first ?representation . + ?representation ifc:contextofitems ?context . + ?context ifc:targetview "MODEL_VIEW" . + ?context ifc:contextidentifier "Body" . + ?context ifc:contexttype "Model" . + } + """ + q = prepareQuery(rep_query, initNs={"ifc": IFC_CONCEPTS, "rdf": RDF}) + + qres = g.query(q, initBindings={"object": product}) + representations = list() + for row in qres: + representations.append(row["representation"]) + + return representations + + +def query_mapped_item(g: Graph, product): + rep_query = """ + SELECT ?representation ?origin ?target + WHERE { + ?object ifc:mappingsource ?source . + ?source ifc:mappingorigin ?origin . + ?source ifc:mappedrepresentation ?representation . + ?object ifc:mappingtarget ?target . + } + """ + + q = prepareQuery(rep_query, initNs={"ifc": IFC_CONCEPTS, "rdf": RDF}) + + qres = g.query(q, initBindings={"object": product}) + assert len(qres) == 1 + # for row in qres: + res = list(qres)[0] + return res["representation"], res["origin"], res["target"] + + +def transform_cartesian_transformation_operator( + g: Graph, target, target_id, length_unit +): + vars = dict( + placement_id=target_id, + g=g, + target=target, + IFC_CONCEPTS=IFC_CONCEPTS, + rdflib=rdflib, + list=list, + length_unit=length_unit, + ) + cto = render_ifc_template( + "ifc/base/cartesian-transformation-operator.json.jinja", **vars + ) + return cto + + +def transform_mapped_item( + g: Graph, origin, origin_id, target, object_placement_id, length_unit +): + graph_contents = list() + + target_id = get_entity_id(g, target, "mapping-target") + + # Transformation of mapping origin (T) to mapping target (ref) + axis_placement = transform_axis_placement_3d(g, origin, origin_id, length_unit) + graph_contents.extend(axis_placement) + e = render_ifc_template( + "ifc/placement/object-placement.json.jinja", + placement_id=origin_id, + ) + graph_contents.extend(e) + pj = render_ifc_template( + "ifc/placement/placement-rel-to.json.jinja", + placement_id=origin_id, + ref_placement_id=target_id, + ) + graph_contents.extend(pj) + + # mapping target (T) to object placement (ref) + cto = transform_cartesian_transformation_operator(g, target, target_id, length_unit) + graph_contents.extend(cto) + e = render_ifc_template( + "ifc/placement/object-placement.json.jinja", + placement_id=target_id, + ) + graph_contents.extend(e) + pj = render_ifc_template( + "ifc/placement/placement-rel-to.json.jinja", + placement_id=target_id, + ref_placement_id=object_placement_id, + ) + graph_contents.extend(pj) + + return graph_contents + + +def transform_mapped_extruded_area_solid( + g: Graph, rep, parent_id, origin_id, length_unit +): + graph_contents = list() + solid_id = parent_id + "-" + get_entity_id(g, rep, "extruded-area-solid") + # Individual points are specified wrt to extruded area solid frame/origin (ref) + _, position, _, _, poly = transform_extruded_area_solid( + g, solid_id, rep, length_unit, parent_id=parent_id + ) + graph_contents.extend(poly) + + # Transformation of extruded area solid (mapped repr, T) to mapping origin (ref) + axis_placement = transform_axis_placement_3d(g, position, solid_id, length_unit) + graph_contents.extend(axis_placement) + e = render_ifc_template( + "ifc/placement/object-placement.json.jinja", + placement_id=solid_id, + ) + graph_contents.extend(e) + pj = render_ifc_template( + "ifc/placement/placement-rel-to.json.jinja", + placement_id=solid_id, + ref_placement_id=origin_id, + ) + graph_contents.extend(pj) + + return graph_contents + + +def query_ifc_doors(g: Graph, length_unit): + door_wall_query = """ + SELECT DISTINCT ?wall ?opening ?door ?voids ?fills + WHERE { + ?wall rdf:type ifc:IFCWALL . + ?opening rdf:type ifc:IFCOPENINGELEMENT . + ?door rdf:type ifc:IFCDOOR . + + ?voids rdf:type ifc:IFCRELVOIDSELEMENT . + ?voids ifc:relatingbuildingelement ?wall . + ?voids ifc:relatedopeningelement ?opening . + + ?fills rdf:type ifc:IFCRELFILLSELEMENT . + ?fills ifc:relatedbuildingelement ?door . + ?fills ifc:relatingopeningelement ?opening . + } + """ + + qres = g.query(door_wall_query) + logger.info("Total Door-Wall relations: %d", len(qres)) + graph_contents = list() + for r in qres: + wall_id = get_entity_id(g, r["wall"], "wall") + opening_id = get_entity_id(g, r["opening"], "opening") + door_id = get_entity_id(g, r["door"], "door") + logger.debug("%s <-- voids -- %s <-- fills -- %s", wall_id, opening_id, door_id) + + d = render_ifc_template( + "ifc/doors/door-entity.json.jinja", + door_id=door_id, + ) + graph_contents.append(d) + + f = render_ifc_template( + "ifc/doors/filling-rel.json.jinja", + door_id=door_id, + opening_id=opening_id, + ) + + o = render_ifc_template( + "ifc/openings/entryway-entity.json.jinja", + opening_id=opening_id, + ) + graph_contents.append(o) + + v = render_ifc_template( + "ifc/openings/voiding-rel.json.jinja", + opening_id=opening_id, + wall_id=wall_id, + ) + graph_contents.append(v) + + logger.info("Processing doorway %s", opening_id) + opening_reps = query_product_shape_representations(g, r["opening"]) + + opening_placement = g.value(r["opening"], IFC_CONCEPTS["objectplacement"]) + opening_placement_id = get_entity_id(g, opening_placement, "placement") + parent = query_placement_rel_to(g, opening_placement) + + # TODO The current test file only has a single door, test with a case with multiple mapped items + + for op_shape in g.objects(opening_reps, IFC_CONCEPTS["items"]): + rep, origin, target = query_mapped_item(g, op_shape) + origin_id = opening_id + "-" + get_entity_id(g, origin, "mapping-origin") + target_id = get_entity_id(g, target, "mapping-target") + + graph_contents.extend( + transform_mapped_item( + g, origin, origin_id, target, opening_placement_id, length_unit + ) + ) + + # Get the IfcRepresentationItems + for o in g.objects(rep, IFC_CONCEPTS["items"]): + graph_contents.extend( + transform_mapped_extruded_area_solid( + g, o, opening_id, origin_id, length_unit + ) + ) + + logger.info("Processing door %s", door_id) + door_reps = query_product_shape_representations(g, r["door"]) + door_placement = g.value(r["door"], IFC_CONCEPTS["objectplacement"]) + door_placement_id = get_entity_id(g, door_placement, "placement") + parent = query_placement_rel_to(g, door_placement) + + for d_shape in g.objects(door_reps, IFC_CONCEPTS["items"]): + rep, origin, target = query_mapped_item(g, d_shape) + origin_id = door_id + "-" + get_entity_id(g, origin, "mapping-origin") + target_id = get_entity_id(g, target, "mapping-target") + + graph_contents.extend( + transform_mapped_item( + g, origin, origin_id, target, door_placement_id, length_unit + ) + ) + + handle = 1 + lining = 1 + panel = 1 + for i in g.objects(rep, IFC_CONCEPTS["items"]): + shape_aspect = str(get_shape_aspect(g, i)).lower() + if shape_aspect == "lining": + parent_id = f"{door_id}-{shape_aspect}-{lining}" + dl = render_ifc_template( + "ifc/doors/door-lining.json.jinja", + element_id=parent_id, + door_id=door_id, + ) + graph_contents.extend(dl) + lining = lining + 1 + elif shape_aspect == "handle": + parent_id = f"{door_id}-{shape_aspect}-{handle}" + dh = render_ifc_template( + "ifc/doors/door-handle.json.jinja", + door_id=door_id, + element_id=parent_id, + ) + graph_contents.extend(dh) + handle = handle + 1 + elif shape_aspect == "panel": + parent_id = f"{door_id}-{shape_aspect}-{panel}" + dp = render_ifc_template( + "ifc/doors/door-panel.json.jinja", + door_id=door_id, + element_id=parent_id, + ) + graph_contents.extend(dp) + panel = panel + 1 + else: + raise ValueError("Unknown shape aspect: %s" % shape_aspect) + + if g.value(i, RDF["type"]) == IFC_CONCEPTS["IFCPOLYGONALFACESET"]: + shape = transform_polygonal_face_set( + g, i, parent_id, length_unit, placement_id=origin_id + ) + graph_contents.extend(shape) + elif g.value(i, RDF["type"]) == IFC_CONCEPTS["IFCEXTRUDEDAREASOLID"]: + shape = transform_mapped_extruded_area_solid( + g, i, parent_id, origin_id, length_unit + ) + graph_contents.extend(shape) + else: + logger.warning("Shape is %s", g.value(i, RDF["type"])) + + return graph_contents + + +def get_shape_aspect(g: Graph, rep): + query = """ + SELECT DISTINCT ?shape_rep ?shape_aspect ?shape_aspect_name + WHERE { + ?shape_aspect rdf:type ifc:IFCSHAPEASPECT . + ?shape_aspect ifc:shaperepresentations/rdf:rest*/rdf:first ?shape_rep . + ?shape_rep ifc:items ?item . + ?shape_aspect ifc:name ?shape_aspect_name . + } + """ + qres = g.query(query, initBindings={"item": rep}) + assert len(qres) == 1 + + return list(qres)[0]["shape_aspect_name"] + + +def transform_polygonal_face_set(g, element, parent_id, length_unit, placement_id=None): + solid_id = get_entity_id(g, element, "polygonal-face-set") + if placement_id is None: + placement_id = solid_id + logger.debug("Transforming %s", solid_id) + # TODO the frame version doesn't seem to include a polygon concept... + # Not including it for now, but review if it's needed + coord_list = rdflib.collection.Collection( + g, + g[element : IFC_CONCEPTS["coordinates"] / IFC_CONCEPTS["coordlist"]].__next__(), + ) + faces = rdflib.collection.Collection( + g, + g[element : IFC_CONCEPTS["faces"]].__next__(), + ) + poly = render_ifc_template( + "ifc/base/polygonal-face-set.json.jinja", + element_id=solid_id, + # TODO Add a separate frame for the object? Is it needed? + # The IFCPOLYGONALFACESET concept doesn't have an origin. + # If adding a frame, it would be hardcoded to no translation and no rotation + placement_id=placement_id, # Transformation of the individual points to mapping origin (ref) + parent_id=parent_id, + coords=coord_list, + faces=faces, + g=g, + rdflib=rdflib, + list=list, + IFC_CONCEPTS=IFC_CONCEPTS, + length_unit=length_unit, + ) + return poly + + +def query_ifc_spaces(g: Graph, model_name, length_unit): + graph_contents = [] + spaces = [] + for s in g.subjects(RDF.type, IFC_CONCEPTS["IFCSPACE"]): + space_id = get_entity_id(g, s, "space") + spaces.append(space_id) + + space_placement = g.value(s, IFC_CONCEPTS["objectplacement"]) + space_placement_id = get_entity_id(g, space_placement, "placement") + logger.info("Processing %s", space_id) + parent = query_placement_rel_to(g, space_placement) + space_json = render_ifc_template( + "ifc/spaces/space-entity.json.jinja", + space_id=space_id, + space_ref_frame=space_placement_id, + model_name=model_name, + length_unit=length_unit, + ) + graph_contents.extend(space_json) + + space_reps = query_product_shape_representations(g, s) + for space_shape in g.objects(space_reps, IFC_CONCEPTS["items"]): + solid_id = get_entity_id(g, space_shape, "polygonal-face-set") + # TODO Check if the space frame is needed. + # The points could be defined wrt to the space_placement_id directly + poly = transform_polygonal_face_set( + g, space_shape, space_id, length_unit, placement_id=space_id + ) + graph_contents.extend(poly) + # TODO We create a polygon of the space by identifying the face that has an equal and the minimum z value + # This has assumptions about frames and coords that may not generalize. + # This is currently needed for the scenery_builder queries + points = get_space_polygon_points(g, space_shape) + polygon = render_ifc_template( + "ifc/spaces/space-polygon.json.jinja", + parent_id=space_id, + element_id=solid_id, + points=points, + ) + graph_contents.extend(polygon) + + # TODO This is needed because "spaces" in the metamodel has a @list container. It should probably be a set instead + graph_contents.append({"@id": model_name, "spaces": spaces}) + return graph_contents + + +def get_space_polygon_points(g: Graph, space_shape): + import numpy as np + + all_points = [] + coord_list = rdflib.collection.Collection( + g, + g[ + space_shape : IFC_CONCEPTS["coordinates"] / IFC_CONCEPTS["coordlist"] + ].__next__(), + ) + for p in coord_list: + coords = [c.toPython() for c in rdflib.collection.Collection(g, p)] + all_points.append(coords) + + all_points = np.array(all_points) + min_height = np.min(all_points[:, 2]) + + faces = rdflib.collection.Collection( + g, + g[space_shape : IFC_CONCEPTS["faces"]].__next__(), + ) + for f in faces: + coord_idx = rdflib.collection.Collection( + g, g[f : IFC_CONCEPTS["coordindex"]].__next__() + ) + face_height = {all_points[idx.toPython() - 1][-1] for idx in coord_idx} + if len(face_height) == 1 and list(face_height)[0] == min_height: + return coord_idx + + +def query_ifc_units(g: Graph): + print("\nUnits...\n-----------------") + + print("@id\tname\tunittype\tprefix\tdimensions") + qres = g.query(units_query) + for row in qres: + print( + "{}\t{}\t{}\t{}\t{}".format( + get_entity_id(g, row["unit"], "unit"), + row["name"], + row["unittype"], + row["prefix"], + row["dimensions"], + ) + ) + + qres = g.query(convert_units_query) + print("\n@id\tname\tunittype\tprefix\tdimensions\tconv. factor") + for row in qres: + print( + "{}\t{}\t{}\t{}\t{}\t{}".format( + get_entity_id(g, row["unit"], "unit"), + row["name"], + row["unittype"], + row["prefix"], + row["dimensions"], + row["factor"], + ) + ) + + qres = g.query(project_units_query) + assert len(qres) == 1 + return list(qres)[0] + + +def query_ifc_task_elements(g, length_unit, elements=["IFCOUTLET", "IFCDUCTSEGMENT"]): + logger.info("Querying for: {}".format(elements)) + task_query = """ + SELECT DISTINCT ?wall ?opening ?object ?voids ?fills + WHERE { + ?wall rdf:type ifc:IFCWALL . + ?opening rdf:type ifc:IFCOPENINGELEMENT . + ?object rdf:type ?object_type . + + ?voids rdf:type ifc:IFCRELVOIDSELEMENT . + ?voids ifc:relatingbuildingelement ?wall . + ?voids ifc:relatedopeningelement ?opening . + + ?fills rdf:type ifc:IFCRELFILLSELEMENT . + ?fills ifc:relatedbuildingelement ?object . + ?fills ifc:relatingopeningelement ?opening . + } + """ + q = prepareQuery(task_query, initNs={"ifc": IFC_CONCEPTS, "rdf": RDF}) + + graph_contents = list() + + for concept in elements: + logger.info("Processing {}s".format(concept)) + qres = g.query(q, initBindings={"object_type": IFC_CONCEPTS[concept]}) + logger.info("Total elements: %d", len(qres)) + for row in qres: + wall_id = get_entity_id(g, row["wall"], "wall") + opening_id = get_entity_id(g, row["opening"], "opening") + object_id = get_entity_id( + g, row["object"], concept.replace("IFC", "").lower() + ) + logger.info("Processing {}".format(object_id)) + logger.debug( + "%s <-- voids -- %s <-- fills -- %s", wall_id, opening_id, object_id + ) + + space_placement, plane_pos, plane_shape = query_space_boundary_rel( + g, row["object"] + ) + plane_id = object_id + "-milling" + plane_pos = transform_axis_placement_3d(g, plane_pos, plane_id, length_unit) + graph_contents.extend(plane_pos) + space_placement_id = get_entity_id(g, space_placement, "placement") + plane_placement = render_ifc_template( + "ifc/placement/object-placement.json.jinja", + placement_id=plane_id, + ) + graph_contents.extend(plane_placement) + plane_rel_to = render_ifc_template( + "ifc/placement/placement-rel-to.json.jinja", + placement_id=plane_id, + ref_placement_id=space_placement_id, + ) + graph_contents.extend(plane_rel_to) + plane_normal = render_ifc_template( + "ifc/task-elements/milling-task.json.jinja", + placement_id=plane_id, + element_id=plane_id, + opening_id=opening_id, + wall_id=wall_id, + length_unit=length_unit, + ) + graph_contents.extend(plane_normal) + + opening_placement = g.value(row["opening"], IFC_CONCEPTS["objectplacement"]) + opening_placement_id = get_entity_id(g, opening_placement, "placement") + + opening_reps = query_product_shape_representations(g, row["opening"]) + for rep in g.objects(opening_reps, IFC_CONCEPTS["items"]): + logger.debug("Shape representation: %s", g.value(rep, RDF["type"])) + if g.value(rep, RDF["type"]) == IFC_CONCEPTS["IFCEXTRUDEDAREASOLID"]: + meas = transform_mapped_extruded_area_solid( + g, rep, opening_id, opening_placement_id, length_unit + ) + graph_contents.extend(meas) + elif g.value(rep, RDF["type"]) == IFC_CONCEPTS["IFCPOLYGONALFACESET"]: + poly = transform_polygonal_face_set( + g, + rep, + opening_id, + length_unit, + placement_id=opening_placement_id, + ) + graph_contents.extend(poly) + else: + logger.warning("Not a supported shape representation") + + return graph_contents + + +def query_space_boundary_rel(g: Graph, obj): + space_boundary_query = """ + SELECT DISTINCT ?boundary ?space_placement ?position ?shape + WHERE { + ?boundary rdf:type ifc:IFCRELSPACEBOUNDARY . + ?boundary ifc:relatedbuildingelement ?object . + ?boundary ifc:relatingspace/ifc:objectplacement ?space_placement . + ?boundary ifc:connectiongeometry/ifc:surfaceonrelatingelement ?plane . + ?plane ifc:basissurface/ifc:position ?position . + ?plane ifc:outerboundary ?shape . + } + """ + q = prepareQuery(space_boundary_query, initNs={"ifc": IFC_CONCEPTS, "rdf": RDF}) + qres = g.query(q, initBindings={"object": obj}) + assert len(qres) == 1 + res = list(qres)[0] + return res["space_placement"], res["position"], res["shape"] + + +def stats(g): + voiding_query = """ + SELECT DISTINCT ?wall ?opening + WHERE { + ?wall rdf:type ifc:IFCWALL . + ?opening rdf:type ifc:IFCOPENINGELEMENT . + ?r rdf:type ifc:IFCRELVOIDSELEMENT . + ?r ifc:relatingbuildingelement ?wall . + ?r ifc:relatedopeningelement ?opening . + } + """ + + print() + qres = g.query(voiding_query) + print("Total openings voiding walls:", len(qres)) + # for r in qres: + # print( + # get_entity_id(g, r["opening"], "opening"), + # " --> voids --> ", + # get_entity_id(g, r["wall"], "wall"), + # ) + # + filling_query = """ + SELECT DISTINCT ?object ?opening + WHERE { + ?opening rdf:type ifc:IFCOPENINGELEMENT . + ?r rdf:type ifc:IFCRELFILLSELEMENT . + ?r ifc:relatedbuildingelement ?object . + ?r ifc:relatingopeningelement ?opening . + } + """ + qres = g.query(filling_query) + print("Total objects filling openings:", len(qres)) + # for r in qres: + # print( + # get_entity_id(g, r["object"], "object"), + # " --> fills --> ", + # get_entity_id(g, r["opening"], "opening"), + # ) diff --git a/src/fpm/generators/tts.py b/src/fpm/generators/tts.py new file mode 100644 index 0000000..2e2de79 --- /dev/null +++ b/src/fpm/generators/tts.py @@ -0,0 +1,333 @@ +import logging +import numpy as np +import rdflib +from rdflib import Graph, RDF +from transforms3d.quaternions import mat2quat + +from fpm.constants import FP, POLY, GEO, COORD, GEOM +from fpm.graph import ( + get_3d_structure, + get_coordinates_map, + prefixed, + get_unit_multiplier, + get_list_values, + get_point_position, + get_waypoint_coord_wrt_world, + get_list_from_ptr, + get_pose_transform_wrt_world, + get_coordinates, + _coord_to_np_matrix, + get_frame_tree, + get_floorplan_elements, +) +from fpm.utils import render_model_template, get_output_path, save_file +from ifcld.interpreters.namespaces import IFC_CONCEPTS + +logger = logging.getLogger("floorplan.generators.tts") +logger.setLevel(logging.DEBUG) + + +def get_dim_and_center(element): + points = np.array(element.get("vertices")) + min_values = np.min(points, axis=0) + max_values = np.max(points, axis=0) + dims = np.abs(max_values - min_values) + center = min_values + dims / 2 + return {"id": element.get("name"), "center": center, "dimensions": dims} + + +def gen_tts_wall_description(g, base_path, **kwargs): + logger.info("Generating wall description for simulator...") + template_path = kwargs.get("template_path") + output_path = get_output_path(base_path, "tts") + + entryways_elements = get_3d_structure(g, "Entryway") + window_elements = get_3d_structure(g, "Window") + openings = dict() + for entryway in entryways_elements: + voids = entryway.get("voids") + e = get_dim_and_center(entryway) + for w in voids: + openings.setdefault(w, list()).append(e) + + for window in window_elements: + voids = window.get("voids") + e = get_dim_and_center(window) + openings.setdefault(voids, list()).append(e) + + wall_elements = get_3d_structure(g, "Wall") + model = list() + for wall in wall_elements: + w = get_dim_and_center(wall) + w["cutouts"] = openings.get(wall.get("name")) + model.append(w) + + render_model_template( + model, + output_path, + "HDT-wall-description.json", + "tts/walls.json.jinja", + template_path, + ) + + +def get_outlet_milling_task(g: Graph, element_type="Opening", **kwargs): + logger.info("Getting 3D structure of all {}s...".format(element_type)) + elements = list() + coords_m = get_coordinates_map(g) + for e, _, _ in g.triples((None, RDF.type, FP[element_type])): + name = prefixed(g, e).split(":")[-1] + wall = (g.value(e, FP["voids"] / RDF["first"]),) + assert len(wall) == 1 + wall = wall[0] + wall_id = prefixed(g, wall).split(":")[-1] + + poly = g.value(e, FP["3d-shape"]) + if g.value(poly, RDF.type) != POLY["Cylinder"]: + continue + + logger.debug("%s: %s", name, prefixed(g, g.value(poly, RDF.type))) + + base = g.value(poly, POLY["base"]) + unit_multiplier = get_unit_multiplier(g, poly) + height = g.value(poly, POLY["height"]).toPython() * unit_multiplier + axis = get_list_values(g, poly, POLY["axis"]) + + # TODO unit conversion for non-zero values for the center + center = g.value(base, POLY["center"]) + radius = g.value(base, POLY["radius"]).toPython() * unit_multiplier + logger.debug("radius: %s, depth: %s", radius, height) + + # Find center of base circle, use axis and height to project 2nd face and get its center + p = get_point_position(g, center) + pp = dict(**p) + pp["z"] = axis[-1].toPython() * height + positions = list() + for coord in [p, pp]: + x, y, z = get_waypoint_coord_wrt_world(g, coord, coords_m) + positions.append((x, y, z)) + logger.debug("Milling vector: %s", positions) + + plane = get_milling_plane(g, e) + start = g.value(plane, GEO["start"]) + start_pose_ref = get_start_pose_coords(g, start) + m_start_wrt_world = get_pose_transform_wrt_world(g, start_pose_ref) + nav_pose = translate_nav_pose(g, start_pose_ref, **kwargs) + + logger.debug("Position: [%s, %s, %s]", round(x, 2), round(y, 2), round(z, 2)) + element = { + "name": name, + "depth": height, + "milling_vector": positions, + "nav_pose": nav_pose, + "radius": radius, + "origin": m_start_wrt_world[:3, 3], + "voids": wall_id, + } + elements.append(element) + + return elements + + +def translate_nav_pose(g: Graph, pose_ref, x=0.0, y=0.0, z=0.0, **_): + # Navigation actions require a robot transformation wrt to the outlet/duct + # Rotation: X axis of the robot must be parallel to the wall. + # This (currently) matches the space boundary frame in the IFC model + translation = np.array( + [ + [1.0, 0.0, 0.0, x], + [0.0, 0.0, 1.0, y], + [0.0, 1.0, 0.0, z], + [0.0, 0.0, 0.0, 1.0], + ] + ) + t_wrt_world = get_pose_transform_wrt_world(g, pose_ref) + nav_pose = np.dot(t_wrt_world, translation) + return nav_pose + + +def get_milling_plane(g: Graph, opening): + plane = get_list_values(g, opening, GEO["vectors"]) + assert len(plane) == 1 + plane = plane[0] + + return plane + + +def get_start_pose_coords(g: Graph, point): + frame = g.value(predicate=GEO["origin"], object=point) + start_pose = g.value(predicate=GEOM["of"], object=frame) + start_pose_ref = g.value(predicate=COORD["of-pose"], object=start_pose) + return start_pose_ref + + +def get_duct_milling_task(g: Graph, element_type="Opening", **kwargs): + logger.info("Getting 3D structure of all {}s...".format(element_type)) + elements = list() + for e, _, _ in g.triples((None, RDF.type, FP[element_type])): + name = prefixed(g, e).split(":")[-1] + + wall = (g.value(e, FP["voids"] / RDF["first"]),) + assert len(wall) == 1 + wall = wall[0] + wall_id = prefixed(g, wall).split(":")[-1] + + poly = g.value(e, FP["3d-shape"]) + if g.value(poly, RDF.type) == POLY["Cylinder"]: + continue + + logger.debug("%s: %s", name, prefixed(g, g.value(poly, RDF.type))) + + plane = get_milling_plane(g, e) + start = g.value(plane, GEO["start"]) + end = g.value(plane, GEO["end"]) + start_pose_ref = get_start_pose_coords(g, start) + + m_start_wrt_world = get_pose_transform_wrt_world(g, start_pose_ref) + + end_position = get_point_position(g, end) + coords_m = get_coordinates_map(g) + end_position_coord = get_waypoint_coord_wrt_world(g, end_position, coords_m) + end_position_coord = np.array(end_position_coord) + + # Find both faces that are parallel to ground + faces = rdflib.collection.Collection( + g, + g[poly : POLY["faces"]].__next__(), + ) + all_faces = list() + for f in faces: + face = rdflib.collection.Collection(g, f) + face_coords = [] + for point in face: + p = get_point_position(g, point) + + x, y, z = get_waypoint_coord_wrt_world(g, p, coords_m) + face_coords.append((x, y, z)) + face_coords = np.array(face_coords) + if np.allclose(face_coords[:, 2], face_coords[0, 2]): + all_faces.append(face_coords) + + # Milling action: Vector of milling is their direction (e.g., bottom-top) + assert len(all_faces) == 2 + milling_vector = list() + for f in all_faces: + center_xy = np.mean(f, axis=0) + milling_vector.append(center_xy) + + # Sort bottom to top + milling_vector.sort(key=lambda x: x[2]) + depth = abs(milling_vector[0][2] - milling_vector[1][2]) + logger.info("Depth: %s", depth) + + # Calculate width + base = all_faces[0] + v_thickness = m_start_wrt_world[:3, 3] - end_position_coord + v1 = abs(base[0, :] - base[1, :]) + v2 = abs(base[0, :] - base[-1, :]) + if np.dot(v1, v_thickness) == 0.0: + width = v1.sum() + thickness = v2.sum() + else: + width = v2.sum() + thickness = v1.sum() + logger.info("width: %s, thickness: %s", width, thickness) + + nav_pose = translate_nav_pose(g, start_pose_ref, **kwargs) + + element = { + "name": name, + "width": width, + "thickness": thickness, + "depth": depth, + "milling_vector": milling_vector, + "origin": m_start_wrt_world[:3, 3], + "nav_pose": nav_pose, + "voids": wall_id, + } + elements.append(element) + + return elements + + +def convert_to_nav2_goal_format(goals: list) -> list: + nav2_goals = list() + for g in goals: + m = g["nav_pose"] + t = { + "name": g["name"], + "voids": g["voids"], + "nav2_goal": { + "p": list(m[:3, 3]), + "q": list(mat2quat(m[:3, :3])), + }, + "milling_vector": [list(p) for p in g["milling_vector"]], + "depth": g["depth"], + "position": list(g["origin"]), + } + if g.get("radius"): + t["radius"] = g["radius"] + t["type"] = "outlet" + else: + t["width"] = g["width"] + t["thickness"] = g["thickness"] + t["type"] = "duct" + nav2_goals.append(t) + return nav2_goals + + +def gen_tts_task_description(g, base_path, **kwargs): + logger.info("Generating task description...") + template_path = kwargs.get("template_path") + output_path = get_output_path(base_path, "tts") + + tasks = list() + + logger.info("Generating task description for outlets") + outlets = get_outlet_milling_task(g, "Opening", **kwargs) + tasks.extend(convert_to_nav2_goal_format(outlets)) + + logger.info("Generating task description for ducts") + ducts = get_duct_milling_task(g, "Opening", **kwargs) + tasks.extend(convert_to_nav2_goal_format(ducts)) + + save_file(output_path, "tasks-gui.json", tasks) + save_file( + output_path, + "HDT-task-description.json", + [ + { + "entity_name": "KukaPlatform1", + "tasks": tasks, + }, + { + "entity_name": "Human1", + "tasks": list(), + }, + ], + ) + + render_model_template( + tasks, + output_path, + "nav-goals.yaml", + "tts/nav-goals.yaml.jinja", + template_path, + ) + return outlets, ducts + + +def gen_ros_frames(g, base_path, **kwargs): + logger.info("Generating ROS frames launch file...") + template_path = kwargs.get("template_path") + output_path = get_output_path(base_path, "tts") + floorplan_elements = ["Space", "Opening", "Wall", "Door", "Entryway"] + element_poses = get_floorplan_elements(g, floorplan_elements) + frames = get_frame_tree(g, element_poses) + render_model_template( + frames, + output_path, + "frames-ros2.launch", + "tts/frames-ros2.launch.jinja", + template_path, + ) diff --git a/src/fpm/graph.py b/src/fpm/graph.py index 8eeb0be..938ae58 100644 --- a/src/fpm/graph.py +++ b/src/fpm/graph.py @@ -1,10 +1,13 @@ import os import glob +import logging import numpy as np import rdflib -from rdflib import RDF +from rdflib import RDF, Graph, Literal +from rdflib.tools.rdf2dot import rdf2dot +from transforms3d.quaternions import mat2quat from fpm import traversal from fpm.constants import ( @@ -15,30 +18,28 @@ QUDT_VOCAB, FP, POLY, - FPMODEL, ) from fpm.utils import build_transformation_matrix +logger = logging.getLogger("floorplan.graph") +logger.setLevel(logging.DEBUG) -def build_graph_from_directory(inputs: tuple, debug=False): + +def build_graph_from_directory(inputs: tuple, draw_dot=False): # Build the graph by reading all composable models in the input folder g = rdflib.Graph() for input_folder in inputs: input_models = glob.glob(os.path.join(input_folder, "*.json")) - print("Found {} models in {}".format(len(input_models), input_folder)) - print("Adding to the graph...") + logger.info("Found {} models in {}".format(len(input_models), input_folder)) for file_path in input_models: - print("\t{}".format(file_path)) + logger.info("Adding {}".format(file_path)) g.parse(file_path, format="json-ld") + logger.debug("\t...done!") - if debug: - from rdflib.tools.rdf2dot import rdf2dot - + if draw_dot: with open("floorplan.dot", "w+") as dotfile: rdf2dot(g, dotfile) - g.serialize("floorplan.json", format="json-ld", auto_compact=True) - return g @@ -47,7 +48,15 @@ def prefixed(g, node): return node.n3(g.namespace_manager) -def get_list_from_ptr(g, ptr): +def get_list_values(g: Graph, subject, predicate): + ptr = g.value(subject, predicate) + if ptr == RDF.nil: + return [] + values = get_list_from_ptr(g, ptr) + return values + + +def get_list_from_ptr(g: Graph, ptr): result_list = [] while True: result_list.append(g.value(ptr, RDF.first)) @@ -58,20 +67,37 @@ def get_list_from_ptr(g, ptr): return result_list -def get_point_position(g, point): +def get_point_position(g: Graph, point): position = g.value(predicate=GEOM["of"], object=point) coordinates = g.value(predicate=COORD["of-position"], object=position) - x = get_coord_value(g.value(coordinates, COORD["x"], default=0.0)) - y = get_coord_value(g.value(coordinates, COORD["y"], default=0.0)) - z = get_coord_value(g.value(coordinates, COORD["z"], default=0.0)) + if g.value(coordinates, COORD["coordinates"]): + coord_values = get_list_values(g, coordinates, COORD["coordinates"]) + if len(coord_values) == 2: + z = 0.0 + x, y = coord_values + else: + x, y, z = coord_values + else: + x = get_coord_value(g, coordinates, "x", default=0.0) + y = get_coord_value(g, coordinates, "y", default=0.0) + z = get_coord_value(g, coordinates, "z", default=0.0) asb = g.value(coordinates, COORD["as-seen-by"]) name = prefixed(g, coordinates) - return {"name": name, "x": x, "y": y, "z": z, "as-seen-by": prefixed(g, asb)} + # Convert units if not in M + unit_multiplier = get_unit_multiplier(g, coordinates) + + return { + "name": name, + "x": float(x) * unit_multiplier, + "y": float(y) * unit_multiplier, + "z": float(z) * unit_multiplier, + "as-seen-by": prefixed(g, asb), + } -def get_floorplan_model_name(g): +def get_floorplan_model_name(g: Graph): floorplan = g.value(predicate=RDF.type, object=FP["FloorPlan"]) if floorplan is None: raise ValueError("No FloorPlan found.") @@ -80,18 +106,16 @@ def get_floorplan_model_name(g): return model_name -def traverse_to_world_origin(g, frame): +def traverse_to_world_origin(g: Graph, frame): # Go through the geometric relation predicates pred_filter = traversal.filter_by_predicates([GEOM["with-respect-to"], GEOM["of"]]) # Algorithm to traverse the graph open_set = traversal.BreadthFirst # Set beginning and end point - if "fpm:" in frame: - root = g.namespace_manager.expand_curie(frame) - else: - root = g.namespace_manager.expand_curie("fpm:{}".format(frame)) - goal = g.namespace_manager.expand_curie("fpm:world-frame") + pref, f = frame.split(":") + root = g.namespace_manager.expand_curie(frame) + goal = g.namespace_manager.expand_curie("{}:world-frame".format(pref)) # Set map of visited nodes for path building parent_map = {} @@ -114,7 +138,10 @@ def traverse_to_world_origin(g, frame): return path -def get_transformation_matrix_wrt_frame(g, root, target): +def get_transformation_matrix_wrt_frame(g: Graph, root, target): + # TODO Refactor this since it's duplicated with utils.py + # Only used for the object instances (doors) + # Configure the traversal algorithm f = [GEOM["with-respect-to"], GEOM["of"]] pred_filter = traversal.filter_by_predicates(f) @@ -146,16 +173,16 @@ def get_transformation_matrix_wrt_frame(g, root, target): # Get the coordinates for the pose current_frame_coordinates = g.value(predicate=COORD["of-pose"], object=pose) + coordinates = get_coordinates(g, current_frame_coordinates) # Get x and y values - x = g.value(current_frame_coordinates, COORD["x"]).toPython() - y = g.value(current_frame_coordinates, COORD["y"]).toPython() + x = coordinates.get("x") + y = coordinates.get("y") # If the pose is defined with a VectorXYZ, read the z value, otherwise the value is 0 - z_value = g.value(current_frame_coordinates, COORD["z"]) - z = 0 if z_value is None else z_value.toPython() + z = coordinates.get("z") # Read the theta value, if the values is in degrees, transform to radians - t = g.value(current_frame_coordinates, COORD["alpha"]).toPython() + t = coordinates.get("alpha") if QUDT_VOCAB["DEG"] in g.objects(current_frame_coordinates, QUDT["unit"]): t = np.deg2rad(t) @@ -164,6 +191,7 @@ def get_transformation_matrix_wrt_frame(g, root, target): # If the pose is defined by two wall frames, then invert the transformation matrix # This is necessary as the FloorPlan DSL calculate certain transformations using frames from walls + # TODO this works for models generated from the DSL but makes assumptions about IDs, needs a generalized solution if prefixed(g, pose).count("wall") > 1: new_T = np.linalg.pinv(new_T) @@ -173,16 +201,16 @@ def get_transformation_matrix_wrt_frame(g, root, target): return T -def get_space_points(g): +def get_space_points(g: Graph): floorplan = g.value(predicate=RDF.type, object=FP["FloorPlan"]) # Get the list of spaces - print("Querying all spaces...") - space_ptr = g.value(floorplan, FP["spaces"]) - spaces = get_list_from_ptr(g, space_ptr) + logger.info("Querying all spaces...") + spaces = get_list_values(g, floorplan, FP["spaces"]) + logger.info("Found %d spaces", len(spaces)) # for each space, find the polygon - print("Get all points of a space...") + logger.info("Get all points of a space...") space_points = [] for space in spaces: space_points_json = get_point_positions_in_space(g, space) @@ -191,24 +219,43 @@ def get_space_points(g): return space_points -def get_element_points(g, element_type="Wall"): - print("Querying all {}s...".format(element_type)) +def get_unit_multiplier(g: Graph, element_id): + # Convert units if not in M + pose_units = list(g.objects(element_id, QUDT["unit"])) + for unit in pose_units: + if unit in [QUDT_VOCAB["MilliM"], QUDT_VOCAB["M"]]: + break + if unit == QUDT_VOCAB["M"]: + m = 1 + elif unit == QUDT_VOCAB["MilliM"]: + m = 0.001 + else: + raise ValueError("Unknown unit", unit) + return m + + +def get_element_points(g: Graph, element_type="Wall"): + logger.info("Querying all {}s...".format(element_type)) element_points = list() for element, _, _ in g.triples((None, RDF.type, FP[element_type])): + unit_multiplier = get_unit_multiplier(g, element) element_points_json = get_point_positions_in_space(g, element) - element_points_json["height"] = g.value(element, FP["height"]).toPython() + + height = g.value(element, FP["height"]).toPython() * unit_multiplier + element_points_json["height"] = height + element_points.append(element_points_json) return element_points -def get_opening_points(g, element="Entryway"): - print("Querying all {}s...".format(element)) +def get_opening_points(g: Graph, element="Entryway"): + logger.info("Querying all {}s...".format(element)) opening_points = list() for opening, _, _ in g.triples((None, RDF.type, FP[element])): poly = g.value(opening, FP["3d-shape"]) - faces_ptr = g.value(poly, POLY["faces"]) - faces_nodes = get_list_from_ptr(g, faces_ptr) + assert poly is not None + faces_nodes = get_list_values(g, poly, POLY["faces"]) face_positions = list() for f in faces_nodes: face_vertices = get_list_from_ptr(g, f) @@ -223,16 +270,20 @@ def get_opening_points(g, element="Entryway"): return opening_points -def get_3d_structure(g, element="Wall", threshold=0.05): - print("Getting 3D structure of all {}s...".format(element)) +def get_3d_structure(g: Graph, element="Wall", threshold=0.05): + """ + Return the 3D structure of non-cylindrical elements + """ + logger.info("Getting 3D structure of all {}s...".format(element)) elements = list() coords_m = get_coordinates_map(g) for e, _, _ in g.triples((None, RDF.type, FP[element])): name = prefixed(g, e).split(":")[-1] poly = g.value(e, FP["3d-shape"]) - vertices_ptr = g.value(poly, POLY["points"]) - vertices = get_list_from_ptr(g, vertices_ptr) + if g.value(poly, RDF.type) != POLY["Polyhedron"]: + continue + vertices = get_list_values(g, poly, POLY["points"]) positions = list() for point in vertices: p = get_point_position(g, point) @@ -242,11 +293,10 @@ def get_3d_structure(g, element="Wall", threshold=0.05): else: p["y"] = p["y"] + threshold - x, y, z = get_waypoint_coord(g, p, coords_m) + x, y, z = get_waypoint_coord_wrt_world(g, p, coords_m) positions.append((x, y, z)) - faces_ptr = g.value(poly, POLY["faces"]) - faces_nodes = get_list_from_ptr(g, faces_ptr) + faces_nodes = get_list_values(g, poly, POLY["faces"]) faces = list() for f in faces_nodes: face_vertices = get_list_from_ptr(g, f) @@ -255,28 +305,25 @@ def get_3d_structure(g, element="Wall", threshold=0.05): d = {"name": name, "vertices": positions, "faces": faces} if element in ["Entryway", "Window"]: - voids_ptr = g.value(e, FP["voids"]) - voids = get_list_from_ptr(g, voids_ptr) + voids = get_list_values(g, e, FP["voids"]) d["voids"] = [prefixed(g, v).split(":")[-1] for v in voids] elements.append(d) return elements -def get_internal_walls(g): +def get_internal_walls(g: Graph): coords_m = get_coordinates_map(g) - print("Getting internal walls...") + logger.info("Getting internal walls...") wall_planes_by_space = dict() for s, r, w in g.triples((None, FP["walls"], None)): - wall_ptr = g.value(s, FP["walls"]) - wall_nodes = get_list_from_ptr(g, wall_ptr) + wall_nodes = get_list_values(g, s, FP["walls"]) wall_planes = dict() for w_ in wall_nodes: wall_name = prefixed(g, w_).split(":")[-1] poly = g.value(w_, FP["3d-shape"]) - faces_ptr = g.value(poly, POLY["faces"]) - faces_nodes = get_list_from_ptr(g, faces_ptr) + faces_nodes = get_list_values(g, poly, POLY["faces"]) inner_wall = list() for f in faces_nodes: face_vertices = get_list_from_ptr(g, f) @@ -284,7 +331,7 @@ def get_internal_walls(g): for point in face_vertices: p = get_point_position(g, point) if p["y"] == 0.0: - x, y, z = get_waypoint_coord(g, p, coords_m) + x, y, z = get_waypoint_coord_wrt_world(g, p, coords_m) positions.append((x, y, z)) # Only one face (the inner face) will have 4 points aligned with the wall frame @@ -299,14 +346,14 @@ def get_internal_walls(g): return wall_planes_by_space -def get_point_positions_in_space(g, space): +def get_point_positions_in_space(g: Graph, space): + logger.debug("Querying points from polygon attribute for %s", prefixed(g, space)) polygon = g.value(space, FP["shape"]) - point_ptr = g.value(polygon, POLY["points"]) - - point_nodes = get_list_from_ptr(g, point_ptr) + point_nodes = get_list_values(g, polygon, POLY["points"]) positions = [] + logger.debug("Querying point positions") for point in point_nodes: position = get_point_position(g, point) positions.append(position) @@ -314,29 +361,59 @@ def get_point_positions_in_space(g, space): return {"name": prefixed(g, space), "points": positions} -def get_coordinates_map(g): +def get_coordinates_map(g: Graph): coordinates_map = {} for coord, _, _ in g.triples((None, RDF.type, COORD["PoseCoordinate"])): - coordinates_map[prefixed(g, g.value(coord, COORD["of-pose"]))] = { - "x": get_coord_value(g.value(coord, COORD["x"], default=0.0)), - "y": get_coord_value(g.value(coord, COORD["y"], default=0.0)), - "z": get_coord_value(g.value(coord, COORD["z"], default=0.0)), - "alpha": get_coord_value(g.value(coord, COORD["alpha"], default=0.0)), - "beta": get_coord_value(g.value(coord, COORD["beta"], default=0.0)), - } + pose = prefixed(g, g.value(coord, COORD["of-pose"])) + coordinates_map[pose] = get_coordinates(g, coord) return coordinates_map -def get_coord_value(v): +def get_coordinates(g: Graph, coord): + """ + coord: @id of the PoseCoordinate element + """ + unit_multiplier = get_unit_multiplier(g, coord) + + if g.value(coord, COORD["coordinates"]): + coordinates = dict() + coord_values = get_list_values(g, coord, COORD["coordinates"]) + for k, v in zip(["x", "y", "z"], coord_values): + coordinates[k] = v.toPython() * unit_multiplier + cosx = get_list_values(g, coord, COORD["direction-cosine-x"]) + cosx = [v.toPython() for v in cosx if isinstance(v, Literal)] + coordinates["direction-cosine-x"] = cosx + + cosz = get_list_values(g, coord, COORD["direction-cosine-z"]) + cosz = [v.toPython() for v in cosz if isinstance(v, Literal)] + coordinates["direction-cosine-z"] = cosz + + coordinates["direction-cosine-y"] = np.cross( + coordinates["direction-cosine-z"], coordinates["direction-cosine-x"] + ).tolist() + else: + logger.debug("Coordinates specified as individual values") + coordinates = { + "x": get_coord_value(g, coord, "x", default=0.0) * unit_multiplier, + "y": get_coord_value(g, coord, "y", default=0.0) * unit_multiplier, + "z": get_coord_value(g, coord, "z", default=0.0) * unit_multiplier, + "alpha": get_coord_value(g, coord, "alpha", default=0.0), + "beta": get_coord_value(g, coord, "beta", default=0.0), + } + return coordinates + + +def get_coord_value(g: Graph, coord, key, default=0.0): + v = g.value(coord, COORD[key], default=default) if v is not None and isinstance(v, float): return v else: return v.toPython() -def get_path_positions(g, path): +def get_path_positions(g: Graph, path): positions = list() for p in path: @@ -347,7 +424,7 @@ def get_path_positions(g, path): return positions -def get_waypoint_coord(g, point, coordinates_map): +def get_waypoint_coord_wrt_world(g: Graph, point, coordinates_map): """Gets the coordinates of a point wrt world frame""" frame = point["as-seen-by"] @@ -362,9 +439,7 @@ def get_waypoint_coord(g, point, coordinates_map): for pose, next_pose in zip(path_positions[:-1], path_positions[1:]): coordinates = coordinates_map[pose] - T = build_transformation_matrix( - coordinates["x"], coordinates["y"], coordinates["z"], coordinates["alpha"] - ).astype(float) + T = build_transformation_matrix(**coordinates).astype(float) if not next_pose == 0: if next_pose.count("wall") > 1 and ( "entryway" not in pose and "window" not in pose @@ -380,10 +455,104 @@ def get_waypoint_coord(g, point, coordinates_map): return x, y, z -def get_waypoint_coord_list(g, points, coordinates_map): +def get_waypoint_coord_list(g: Graph, points, coordinates_map): w_coords = list() for p in points: - x, y, _ = get_waypoint_coord(g, p, coordinates_map) + x, y, _ = get_waypoint_coord_wrt_world(g, p, coordinates_map) w_coords.append([x, y, 0, 1]) return w_coords + + +def _coord_to_np_matrix(coord, scale=1.0): + t = np.zeros((4, 4)) + t[0, 3] = coord.get("x") + t[1, 3] = coord.get("y") + t[2, 3] = coord.get("z") + t[3, 3] = scale + t[:3, 0] = coord.get("direction-cosine-x") + t[:3, 1] = coord.get("direction-cosine-y") + t[:3, 2] = coord.get("direction-cosine-z") + + return t + + +def get_pose_transform_wrt_world(g: Graph, pose_ref): + coord = get_coordinates(g, pose_ref) + m = _coord_to_np_matrix(coord) + pose = g.value(pose_ref, COORD["of-pose"]) + frame = g.value(pose, GEOM["with-respect-to"]) + transformation_path = traverse_to_world_origin(g, prefixed(g, frame)) + + for t in transformation_path[::-1]: + if g.value(t, RDF["type"]) == GEO["Frame"]: + continue + new_pose_ref = g.value(predicate=COORD["of-pose"], object=t) + new_m = get_coordinates(g, new_pose_ref) + new_m = _coord_to_np_matrix(new_m) + m = np.dot(new_m, m) + + return m + + +def get_frame_transform(g: Graph, filter_str: str): + matrices = list() + for s, p, o in g.triples((None, RDF["type"], COORD["PoseReference"])): + if filter_str not in str(s): + continue + m = get_pose_transform_wrt_world(g, s) + matrices.append(m) + + return matrices + + +def get_frame_tree(g: Graph, poses=None): + frames = dict() + if poses is None: + pose_list = g.subjects(RDF["type"], COORD["PoseReference"]) + else: + pose_list = poses + + for pose_ref in pose_list: + coord = get_coordinates(g, pose_ref) + pose = g.value(pose_ref, COORD["of-pose"]) + frame = g.value(pose, GEOM["with-respect-to"]) + transformation_path = traverse_to_world_origin(g, prefixed(g, frame)) + frame = g.value(pose, GEOM["of"]) + for t in transformation_path[::-1]: + if g.value(t, RDF["type"]) == GEO["Frame"]: + m = _coord_to_np_matrix(coord) + d = { + "parent_frame_id": prefixed(g, t).split(":")[-1], + "frame_id": prefixed(g, frame).split(":")[-1], + "p": list(m[:3, 3]), + "q": list(mat2quat(m[:3, :3])), + } + # TODO This is not optimal, rewrite to avoid loops for transforms we already know + frames[frame] = d + frame = t + else: + new_pose_ref = g.value(predicate=COORD["of-pose"], object=t) + coord = get_coordinates(g, new_pose_ref) + return frames + + +def get_floorplan_elements(g: Graph, floorplan_elements: list): + poses = list() + for element in floorplan_elements: + for s, p, o in g.triples((None, RDF["type"], FP[element])): + shape3d = g.value(s, FP["3d-shape"]) + shape_type = g.value(shape3d, RDF["type"]) + if shape_type == POLY["Polyhedron"]: + point = g.value(shape3d, POLY["points"] / RDF["first"]) + elif shape_type == POLY["Cylinder"]: + point = g.value(shape3d, POLY["base"] / POLY["center"]) + + position_ref = g.value( + predicate=COORD["of-position"] / GEOM["of"], object=point + ) + asb = g.value(position_ref, COORD["as-seen-by"]) + assert g.value(asb, RDF["type"]) == GEO["Frame"] + pose_ref = g.value(predicate=COORD["of-pose"] / GEOM["of"], object=asb) + poses.append(pose_ref) + return poses diff --git a/src/fpm/templates/frame-tree.dot.jinja b/src/fpm/templates/frame-tree.dot.jinja new file mode 100644 index 0000000..d8e5472 --- /dev/null +++ b/src/fpm/templates/frame-tree.dot.jinja @@ -0,0 +1,5 @@ +digraph G { +{% for k, v in frames.items() -%} + {{ v.parent_frame_id | replace("-", "_") }} -> {{ v.frame_id | replace("-", "_") }} [weight=8]; +{% endfor %} +}; \ No newline at end of file diff --git a/src/fpm/templates/ifc/base/cartesian-transformation-operator.json.jinja b/src/fpm/templates/ifc/base/cartesian-transformation-operator.json.jinja new file mode 100644 index 0000000..872e628 --- /dev/null +++ b/src/fpm/templates/ifc/base/cartesian-transformation-operator.json.jinja @@ -0,0 +1,16 @@ +[ + { + "@id": "pose-coord-{{ placement_id }}", + "@type": ["PoseReference", "PoseCoordinate", "DirectionCosineXYZ", "VectorXYZ"], + "unit": ["UNITLESS", "{{ length_unit }}"], + {% set ax1 = list(rdflib.collection.Collection(g, g[target : IFC_CONCEPTS["axis1"] / IFC_CONCEPTS["directionratios"]].__next__())) -%} + "direction-cosine-x": [{{ ax1[0] }}, {{ ax1[1] }}, {{ ax1[2] }}], + {% set ax2 = list(rdflib.collection.Collection(g, g[target : IFC_CONCEPTS["axis2"] / IFC_CONCEPTS["directionratios"]].__next__())) -%} + "direction-cosine-y": [{{ ax2[0] }}, {{ ax2[1] }}, {{ ax2[2] }}], + {% set ax3 = list(rdflib.collection.Collection(g, g[target : IFC_CONCEPTS["axis3"] / IFC_CONCEPTS["directionratios"]].__next__())) -%} + "direction-cosine-z": [{{ ax3[0] }}, {{ ax3[1] }}, {{ ax3[2] }}], + {% set coords = list(rdflib.collection.Collection(g, g[target : IFC_CONCEPTS["localorigin"] / IFC_CONCEPTS["coordinates"]].__next__())) -%} + "coordinates": [{{ coords[0] }}, {{ coords[1] }}, {{ coords[2] }}] +{# "scale": {{ g.value(target, IFC_CONCEPTS["scale"]) }}#} + } +] \ No newline at end of file diff --git a/src/fpm/templates/ifc/base/circle-profile.json.jinja b/src/fpm/templates/ifc/base/circle-profile.json.jinja new file mode 100644 index 0000000..0f8a973 --- /dev/null +++ b/src/fpm/templates/ifc/base/circle-profile.json.jinja @@ -0,0 +1,47 @@ +[ + { + "@id": "{{ parent_id }}", + "shape": "{{ element_id }}-polygon", + "3d-shape": "{{ element_id }}-cylinder" + }, + { + "@id": "{{ element_id }}-polygon", + "@type": "Circle", + "center": "{{ point_id }}-center", + "quantity-kind": ["Radius"], + "unit": "{{ length_unit }}", + "radius": {{ radius }} + }, + { + "@id": "{{ element_id }}-cylinder", + "@type": "Cylinder", + "base": "{{ element_id }}-polygon", +{# {% set axis = shape["items"]["extrudeddirection"]["directionratios"]["@list"] %}#} +{# "axis": [{% for i in axis %}{{ i["@value"] }}{% if not loop.last %},{% endif %}{% endfor %}],#} + "axis": [{{ extruded_dir[0] }}, {{ extruded_dir[1] }}, {{ extruded_dir[2] }}], + "height": {{ depth }}, + "unit": "{{ length_unit }}" + }, + { + "@id": "{{ point_id }}-center", + "@type": [ "3D", "Euclidean", "Point" ] + }, + { + "@id": "position-{{ point_id }}-center", + "@type": "Position", + "of": "{{ point_id }}-center", + "with-respect-to": "{{ element_id }}-origin", + "quantity-kind": "Length" + }, + { + "@id": "position-coord-{{ point_id }}-center-wrt-{{ element_id }}-origin", + "@type": [ "PositionReference", "PositionCoordinate", "VectorXYZ" ], + "of-position": "position-{{ point_id }}-center", + "as-seen-by": "{{ element_id }}-frame", + "unit": "{{ length_unit }}", +{# "coordinates": {{ coordinates }}#} + {% set coords = list(rdflib.collection.Collection(g, g[relative_placement : IFC_CONCEPTS["location"] / IFC_CONCEPTS["coordinates"]].__next__())) -%} + "coordinates": [{{ coords[0] }}, {{ coords[1] }} ] + } + +] \ No newline at end of file diff --git a/src/fpm/templates/ifc/base/polygonal-face-set.json.jinja b/src/fpm/templates/ifc/base/polygonal-face-set.json.jinja new file mode 100644 index 0000000..3b69bad --- /dev/null +++ b/src/fpm/templates/ifc/base/polygonal-face-set.json.jinja @@ -0,0 +1,54 @@ +[ + { + "@id": "{{ parent_id }}", +{# "shape": "{{ element_id }}-polygon",#} + "3d-shape": "{{ element_id }}-polyhedron" + }, +{# {#} +{# "@id": "{{ element_id }}-polygon",#} +{# "@type": "Polygon",#} +{# "points": [#} +{# ]#} +{# },#} + { + "@id": "{{ element_id }}-polyhedron", + "@type": "Polyhedron", + "points": [ + {% for p in coords -%} + "{{ element_id }}-point-{{ loop.index }}"{% if not loop.last %},{% endif %} + {% endfor %} + ], + "faces": [ + {% for f in faces %} + [ + {% for idx in rdflib.collection.Collection(g, g[f: IFC_CONCEPTS["coordindex"]].__next__()) %} + "{{ element_id }}-point-{{ idx }}"{% if not loop.last %},{% endif %} + {% endfor %} + ]{% if not loop.last %},{% endif %} + {% endfor %} + ] + }, + {%- for p in coords -%} + {% set point_id = element_id ~ "-point-" ~ loop.index %} + { + "@id": "{{ point_id }}", + "@type": [ "3D", "Euclidean", "Point" ] + }, + { + "@id": "position-{{ point_id }}", + "@type": "Position", + "of": "{{ point_id }}", + "with-respect-to": "{{ placement_id }}-origin", + "quantity-kind": "Length" + }, + { + "@id": "position-coord-{{ point_id }}-wrt-{{ element_id }}-origin", + "@type": [ "PositionReference", "PositionCoordinate", "VectorXYZ" ], + "of-position": "position-{{ point_id }}", + "as-seen-by": "{{ placement_id }}-frame", + "unit": "{{ length_unit }}", + {% set point_coords = list(rdflib.collection.Collection(g, p)) -%} + "coordinates": [{{ point_coords[0] }}, {{ point_coords[1] }}, {{ point_coords[2] }} ] + }{% if not loop.last %},{% endif %} + {% endfor %} +] diff --git a/src/fpm/templates/ifc/base/rectangle-profile.json.jinja b/src/fpm/templates/ifc/base/rectangle-profile.json.jinja new file mode 100644 index 0000000..d9c322d --- /dev/null +++ b/src/fpm/templates/ifc/base/rectangle-profile.json.jinja @@ -0,0 +1,49 @@ +[ + { + "@id": "{{ parent_id }}", + "shape": "{{ element_id }}-polygon", + "3d-shape": "{{ element_id }}-polyhedron" + }, +{ + "@id": "{{ element_id }}-polygon", + "@type": "Polygon", + "points": [ + {% for i in range(1,5) -%} + "{{ element_id }}-point-{{ i }}"{% if not loop.last %},{% endif %} + {%- endfor %} + ] +}, +{ +"@id": "{{ element_id }}-polyhedron", +"@type": "Polyhedron", +"points": [ +{% for i in range(1,9) -%} +"{{ element_id }}-point-{{ i }}"{% if not loop.last %},{% endif %} +{%- endfor %} +], +"faces": [] +}, + {% for p in coords %} + {% set point_id = element_id ~ "-point-" ~ loop.index %} + { + "@id": "{{ point_id }}", + "@type": [ "3D", "Euclidean", "Point" ] + }, + { + "@id": "position-{{ point_id }}", + "@type": "Position", + "of": "{{ point_id }}", + "with-respect-to": "{{ element_id }}-origin", + "quantity-kind": "Length" + }, + { + "@id": "position-coord-{{ point_id }}-wrt-{{ element_id }}-origin", + "@type": [ "PositionReference", "PositionCoordinate", "VectorXYZ" ], + "of-position": "position-{{ point_id }}", + "as-seen-by": "{{ element_id }}-frame", + "unit": "{{ length_unit }}", + "coordinates": {{ p }} + + }{% if not loop.last %},{% endif %} + {% endfor %} +] diff --git a/src/fpm/templates/ifc/doors/door-entity.json.jinja b/src/fpm/templates/ifc/doors/door-entity.json.jinja new file mode 100644 index 0000000..29a5062 --- /dev/null +++ b/src/fpm/templates/ifc/doors/door-entity.json.jinja @@ -0,0 +1,4 @@ +{ + "@id": "{{ door_id }}", + "@type": "Door" +} diff --git a/src/fpm/templates/ifc/doors/door-handle.json.jinja b/src/fpm/templates/ifc/doors/door-handle.json.jinja new file mode 100644 index 0000000..7eb182a --- /dev/null +++ b/src/fpm/templates/ifc/doors/door-handle.json.jinja @@ -0,0 +1,10 @@ +[ + { + "@id": "{{ element_id }}", + "@type": "Handle" + }, + { + "@id": "{{ door_id }}", + "handles": ["{{ element_id }}"] + } +] diff --git a/src/fpm/templates/ifc/doors/door-lining.json.jinja b/src/fpm/templates/ifc/doors/door-lining.json.jinja new file mode 100644 index 0000000..cb8819b --- /dev/null +++ b/src/fpm/templates/ifc/doors/door-lining.json.jinja @@ -0,0 +1,10 @@ +[ + { + "@id": "{{ element_id }}", + "@type": "DoorLining" + }, + { + "@id": "{{ door_id }}", + "lining": ["{{ element_id }}"] + } +] diff --git a/src/fpm/templates/ifc/doors/door-panel.json.jinja b/src/fpm/templates/ifc/doors/door-panel.json.jinja new file mode 100644 index 0000000..1b45921 --- /dev/null +++ b/src/fpm/templates/ifc/doors/door-panel.json.jinja @@ -0,0 +1,10 @@ +[ + { + "@id": "{{ element_id }}", + "@type": "DoorPanel" + }, + { + "@id": "{{ door_id }}", + "panels": ["{{ element_id }}"] + } +] diff --git a/src/fpm/templates/ifc/doors/filling-rel.json.jinja b/src/fpm/templates/ifc/doors/filling-rel.json.jinja new file mode 100644 index 0000000..f845417 --- /dev/null +++ b/src/fpm/templates/ifc/doors/filling-rel.json.jinja @@ -0,0 +1,4 @@ +{ + "@id": "{{ door_id }}", + "fills": "{{ opening_id }}" +} diff --git a/src/fpm/templates/ifc/fpm-context.json.jinja b/src/fpm/templates/ifc/fpm-context.json.jinja new file mode 100644 index 0000000..a5f8208 --- /dev/null +++ b/src/fpm/templates/ifc/fpm-context.json.jinja @@ -0,0 +1,14 @@ +[ + { + "@base": "https://secorolab.github.io/models/{{ model_id }}/floorplan/", + "model": "https://secorolab.github.io/models/{{ model_id }}/", + "fpm-model": "https://secorolab.github.io/models/{{ model_id }}/floorplan/" + }, + "http://comp-rob2b.github.io/metamodels/qudt.json", + "https://comp-rob2b.github.io/metamodels/geometry/coordinates.json", + "https://secorolab.github.io/metamodels/geometry/coordinates.json", + "https://comp-rob2b.github.io/metamodels/geometry/spatial-relations.json", + "https://secorolab.github.io/metamodels/geometry/polytope.json", + "https://comp-rob2b.github.io/metamodels/geometry/structural-entities.json", + "https://secorolab.github.io/metamodels/floorplan/floorplan.json" + ] \ No newline at end of file diff --git a/src/fpm/templates/ifc/openings/entryway-entity.json.jinja b/src/fpm/templates/ifc/openings/entryway-entity.json.jinja new file mode 100644 index 0000000..132e2c5 --- /dev/null +++ b/src/fpm/templates/ifc/openings/entryway-entity.json.jinja @@ -0,0 +1,7 @@ +{ + "@id": "{{ opening_id }}", + "@type": "Entryway" +{# "shape": "{{ opening_id }}-polygon",#} +{# "3d-shape": "{{ opening_id }}-polyhedron"#} + {# "thickness": 0.4,#} +} diff --git a/src/fpm/templates/ifc/openings/opening-entity.json.jinja b/src/fpm/templates/ifc/openings/opening-entity.json.jinja new file mode 100644 index 0000000..7571c8d --- /dev/null +++ b/src/fpm/templates/ifc/openings/opening-entity.json.jinja @@ -0,0 +1,4 @@ +{ + "@id": "{{ opening_id }}", + "@type": "Opening" +} diff --git a/src/fpm/templates/ifc/openings/voiding-rel.json.jinja b/src/fpm/templates/ifc/openings/voiding-rel.json.jinja new file mode 100644 index 0000000..37ecb95 --- /dev/null +++ b/src/fpm/templates/ifc/openings/voiding-rel.json.jinja @@ -0,0 +1,6 @@ +{ + "@id": "{{ opening_id }}", + "voids": [ + "{{ wall_id }}" + ] +} diff --git a/src/fpm/templates/ifc/placement/object-placement.json.jinja b/src/fpm/templates/ifc/placement/object-placement.json.jinja new file mode 100644 index 0000000..9b4a386 --- /dev/null +++ b/src/fpm/templates/ifc/placement/object-placement.json.jinja @@ -0,0 +1,26 @@ +{% set placement_frame = placement_id + "-frame" %} +{% set placement_origin = placement_id + "-origin" %} +{% set pose_id = "pose-" + placement_id %} +{% set coord_id = "pose-coord-" + placement_id %} +[ + { + "@id": "{{ coord_id }}", + "@type": ["PoseReference", "PoseCoordinate", "DirectionCosineXYZ", "VectorXYZ"], + "of-pose": "{{ pose_id }}" + }, + { + "@id": "{{ pose_id }}", + "@type": "Pose", + "of": "{{ placement_frame }}", + "quantity-kind": [ "Angle", "Length" ] + }, + { + "@id": "{{ placement_frame }}", + "@type": "Frame", + "origin": "{{ placement_origin }}" + }, + { + "@id": "{{ placement_origin }}", + "@type": [ "3D", "Euclidean", "Point" ] + } +] diff --git a/src/fpm/templates/ifc/placement/placement-rel-to.json.jinja b/src/fpm/templates/ifc/placement/placement-rel-to.json.jinja new file mode 100644 index 0000000..c7fd17a --- /dev/null +++ b/src/fpm/templates/ifc/placement/placement-rel-to.json.jinja @@ -0,0 +1,20 @@ +[ + { + "@id": "pose-coord-{{ placement_id }}", + "@type": "PoseCoordinate", + {% if world_frame %} + "as-seen-by": "world-frame" + {% else %} + "as-seen-by": "{{ ref_placement_id }}-frame" + {% endif %} + }, + { + "@id": "pose-{{ placement_id }}", + "@type": "Pose", + {% if world_frame %} + "with-respect-to": "world-frame" + {% else %} + "with-respect-to": "{{ ref_placement_id }}-frame" + {% endif %} + } +] diff --git a/src/fpm/templates/ifc/placement/rel-placement-axis.json.jinja b/src/fpm/templates/ifc/placement/rel-placement-axis.json.jinja new file mode 100644 index 0000000..390cb4c --- /dev/null +++ b/src/fpm/templates/ifc/placement/rel-placement-axis.json.jinja @@ -0,0 +1,9 @@ +[ + { + "@id": "pose-coord-{{ placement_id }}", + "@type": "DirectionCosineXYZ", + "unit": "UNITLESS", + {% set axis = list(rdflib.collection.Collection(g, g[relative_placement : IFC_CONCEPTS["axis"] / IFC_CONCEPTS["directionratios"]].__next__())) -%} + "direction-cosine-z": [{{ axis[0] }}, {{ axis[1] }}, {{ axis[2] }}] + } +] \ No newline at end of file diff --git a/src/fpm/templates/ifc/placement/rel-placement-coords.json.jinja b/src/fpm/templates/ifc/placement/rel-placement-coords.json.jinja new file mode 100644 index 0000000..6180e63 --- /dev/null +++ b/src/fpm/templates/ifc/placement/rel-placement-coords.json.jinja @@ -0,0 +1,9 @@ +[ + { + "@id": "pose-coord-{{ placement_id }}", + "@type": "PoseCoordinate", + "unit": "{{ length_unit }}", + {% set coords = list(rdflib.collection.Collection(g, g[relative_placement : IFC_CONCEPTS["location"] / IFC_CONCEPTS["coordinates"]].__next__())) -%} + "coordinates": [{{ coords[0] }}, {{ coords[1] }}, {{ coords[2] }}] + } +] diff --git a/src/fpm/templates/ifc/placement/rel-placement-refdirection.json.jinja b/src/fpm/templates/ifc/placement/rel-placement-refdirection.json.jinja new file mode 100644 index 0000000..77df893 --- /dev/null +++ b/src/fpm/templates/ifc/placement/rel-placement-refdirection.json.jinja @@ -0,0 +1,9 @@ +[ + { + "@id": "pose-coord-{{ placement_id }}", + "@type": "DirectionCosineXYZ", + "unit": "UNITLESS", + {% set refdirection = list(rdflib.collection.Collection(g, g[relative_placement : IFC_CONCEPTS["refdirection"] / IFC_CONCEPTS["directionratios"]].__next__())) -%} + "direction-cosine-x": [{{ refdirection[0] }}, {{ refdirection[1] }}, {{ refdirection[2] }}] + } +] diff --git a/src/fpm/templates/ifc/spaces/space-entity.json.jinja b/src/fpm/templates/ifc/spaces/space-entity.json.jinja new file mode 100644 index 0000000..bf7d584 --- /dev/null +++ b/src/fpm/templates/ifc/spaces/space-entity.json.jinja @@ -0,0 +1,43 @@ +[ +{# {#} +{# "@id": "{{ model_name }}",#} +{# "spaces": [#} +{# "{{ space_id }}"#} +{# ]#} +{# },#} + { + "@id": "{{ space_id }}", + "@type": "Space" +{# "walls": [],#} +{# "shape": "{{ space_id }}-polygon",#} +{# "3d-shape": "{{ space_id }}-polyhedron"#} + }, + { + "@id": "{{ space_id }}-origin", + "@type": [ "3D", "Euclidean", "Point" ] + }, + { + "@id": "{{ space_id }}-frame", + "@type": "Frame", + "origin": "{{ space_id }}-origin" + }, + { + "@id": "pose-{{ space_id }}", + "@type": "Pose", + "of": "{{ space_id }}-frame", + "with-respect-to": "{{ space_ref_frame }}-frame", + "quantity-kind": [ "Angle", "Length" ] + }, + {# Hardcoded frame of space wrt to the element placement, but this may not be true in other cases #} + { + "@id": "pose-coord-{{ space_id }}", + "@type": [ "PoseReference", "PoseCoordinate", "DirectionCosineXYZ", "VectorXYZ" ], + "of-pose": "pose-{{ space_id }}", + "as-seen-by": "{{ space_ref_frame }}-frame", + "unit": [ "UNITLESS", "{{ length_unit }}" ], + "direction-cosine-x": [1.0, 0.0, 0.0], + "direction-cosine-y": [0.0, 1.0, 0.0], + "direction-cosine-z": [0.0, 0.0, 1.0], + "coordinates": [0.0, 0.0, 0.0] + } +] diff --git a/src/fpm/templates/ifc/spaces/space-polygon.json.jinja b/src/fpm/templates/ifc/spaces/space-polygon.json.jinja new file mode 100644 index 0000000..54d3cd7 --- /dev/null +++ b/src/fpm/templates/ifc/spaces/space-polygon.json.jinja @@ -0,0 +1,15 @@ +[ + { + "@id": "{{ parent_id }}", + "shape": "{{ element_id }}-polygon" + }, + { + "@id": "{{ element_id }}-polygon", + "@type": "Polygon", + "points": [ + {% for idx in points %} + "{{ element_id }}-point-{{ idx }}"{% if not loop.last %},{% endif %} + {% endfor %} + ] + } +] diff --git a/src/fpm/templates/ifc/task-elements/milling-task.json.jinja b/src/fpm/templates/ifc/task-elements/milling-task.json.jinja new file mode 100644 index 0000000..82b448f --- /dev/null +++ b/src/fpm/templates/ifc/task-elements/milling-task.json.jinja @@ -0,0 +1,38 @@ +[ + {% set point_id = element_id ~ "-end" %} + { + "@id": "{{ point_id }}", + "@type": [ "3D", "Euclidean", "Point" ] + }, + { + "@id": "position-{{ point_id }}", + "@type": "Position", + "of": "{{ point_id }}", + "with-respect-to": "{{ placement_id }}-origin", + "quantity-kind": "Length" + }, + { + "@id": "position-coord-{{ point_id }}-wrt-{{ element_id }}-origin", + "@type": [ "PositionReference", "PositionCoordinate", "VectorXYZ" ], + "of-position": "position-{{ point_id }}", + "as-seen-by": "{{ placement_id }}-frame", + "unit": "{{ length_unit }}", + "coordinates": [0.0, 0.0, 1000.0] + }, + { + "@id": "{{ element_id }}-normal-to-boundary", + "@type": "Vector", + "start": "{{ placement_id }}-origin", + "end": "{{ point_id }}" + }, + { + "@id": "{{ opening_id }}", + "@type": "Opening", + "vectors": [ + "{{ element_id }}-normal-to-boundary" + ], + "voids": [ + "{{ wall_id }}" + ] + } +] \ No newline at end of file diff --git a/src/fpm/templates/ifc/walls/wall-entity.json.jinja b/src/fpm/templates/ifc/walls/wall-entity.json.jinja new file mode 100644 index 0000000..09f17af --- /dev/null +++ b/src/fpm/templates/ifc/walls/wall-entity.json.jinja @@ -0,0 +1,6 @@ +{ + "@id": "{{ wall_id }}", + "@type": "Wall" +{# "shape": "{{ wall_id }}-polygon",#} +{# "3d-shape": "{{ wall_id }}-polyhedron"#} +} diff --git a/src/fpm/templates/ifc/walls/wall-polygon.json.jinja b/src/fpm/templates/ifc/walls/wall-polygon.json.jinja new file mode 100644 index 0000000..d4e8325 --- /dev/null +++ b/src/fpm/templates/ifc/walls/wall-polygon.json.jinja @@ -0,0 +1,38 @@ +[ + { + "@id": "{{ parent_id }}", + "shape": "{{ element_id }}-polygon" + }, +{ + "@id": "{{ element_id }}-polygon", + "@type": "Polygon", + "points": [ + {%- for p in coords[:4] -%} + {% set point_id = element_id ~ "-point-" ~ loop.index %} + "{{ point_id }}"{% if not loop.last %},{% endif %} + {%- endfor %} + ] +}, +{%- for p in coords[:4] -%} +{% set point_id = element_id ~ "-point-" ~ loop.index %} +{ +"@id": "{{ point_id }}", +"@type": [ "3D", "Euclidean", "Point" ] +}, +{ +"@id": "position-{{ point_id }}", +"@type": "Position", +"of": "{{ point_id }}", +"with-respect-to": "{{ element_id }}-origin", +"quantity-kind": "Length" +}, +{ +"@id": "position-coord-{{ point_id }}-wrt-{{ element_id }}-origin", +"@type": [ "PositionReference", "PositionCoordinate", "VectorXYZ" ], +"of-position": "position-{{ point_id }}", +"as-seen-by": "{{ element_id }}-frame", +"unit": "{{ length_unit }}", +"coordinates": {{ p }} +}{% if not loop.last %},{% endif %} +{% endfor %} +] diff --git a/src/fpm/templates/ifc/walls/wall-polyhedron.json.jinja b/src/fpm/templates/ifc/walls/wall-polyhedron.json.jinja new file mode 100644 index 0000000..cbb4aa3 --- /dev/null +++ b/src/fpm/templates/ifc/walls/wall-polyhedron.json.jinja @@ -0,0 +1,43 @@ +[ + { + "@id": "{{ parent_id }}", + "3d-shape": "{{ element_id }}-polyhedron" + }, +{ + "@id": "{{ element_id }}-polyhedron", + "@type": "Polyhedron", + "points": [ + {% for p in coords[:4] -%} + "{{ element_id }}-point-{{ loop.index }}", + {% endfor %} + {%- for p in coords[:4] -%} + "{{ element_id }}-point-{{ loop.index + 4 }}"{% if not loop.last %},{% endif %} + {% endfor -%} + ], + "faces": [ + ] +}, +{%- for p in coords[:4] -%} +{% set point_id = element_id ~ "-point-" ~ (loop.index + 4) %} +{ +"@id": "{{ point_id }}", +"@type": [ "3D", "Euclidean", "Point" ] +}, +{ +"@id": "position-{{ point_id }}", +"@type": "Position", +"of": "{{ point_id }}", +"with-respect-to": "{{ element_id }}-origin", +"quantity-kind": "Length" +}, +{ +"@id": "position-coord-{{ point_id }}-wrt-{{ element_id }}-origin", +"@type": [ "PositionReference", "PositionCoordinate", "VectorXYZ" ], +"of-position": "position-{{ point_id }}", +"as-seen-by": "{{ element_id }}-frame", +"unit": "{{ length_unit }}", +{% set z = depth.toPython() * extruded_dir[-1].toPython() %} +"coordinates": [{{ p[0] }}, {{ p[1] }}, {{ z }} ] +}{% if not loop.last %},{% endif %} +{% endfor %} +] diff --git a/src/fpm/templates/ifc/walls/wall-representation.json.jinja b/src/fpm/templates/ifc/walls/wall-representation.json.jinja new file mode 100644 index 0000000..71be33e --- /dev/null +++ b/src/fpm/templates/ifc/walls/wall-representation.json.jinja @@ -0,0 +1,6 @@ +{ + "@id": "{{ wall_id }}", + {# "thickness": "",#} + "height": {{ depth }}, + "unit": "{{ length_unit }}" +} diff --git a/src/fpm/templates/tts/frames-ros2.launch.jinja b/src/fpm/templates/tts/frames-ros2.launch.jinja new file mode 100644 index 0000000..5fe826a --- /dev/null +++ b/src/fpm/templates/tts/frames-ros2.launch.jinja @@ -0,0 +1,16 @@ + + + + + + + {% for k, frame in model.items() %} + + {% endfor %} + \ No newline at end of file diff --git a/src/fpm/templates/tts/nav-goals.yaml.jinja b/src/fpm/templates/tts/nav-goals.yaml.jinja new file mode 100644 index 0000000..971b785 --- /dev/null +++ b/src/fpm/templates/tts/nav-goals.yaml.jinja @@ -0,0 +1,7 @@ +{% for task in model %} +{% set goal = task.nav2_goal %} +- id: {{ task.name }} + position: {{ goal.p }} + orientation: {{ goal.q }} + frame: world-frame +{% endfor %} diff --git a/src/fpm/templates/tts/walls.json.jinja b/src/fpm/templates/tts/walls.json.jinja new file mode 100644 index 0000000..4a3397a --- /dev/null +++ b/src/fpm/templates/tts/walls.json.jinja @@ -0,0 +1,34 @@ +[ + {% for wall in model %} + { + "wallID":"{{ wall.id }}", + "center": { + "x": {{ wall.center[0] }}, + "y": {{ wall.center[1] }}, + "z": {{ wall.center[2] }} + }, + "dimensions":{ + "x": {{ wall.dimensions[0] }}, + "y": {{ wall.dimensions[1] }}, + "z": {{ wall.dimensions[2] }} + }, + "angleZ": 0, + "cutouts":[ + {% if wall.cutouts %} + {% for opening in wall.cutouts %} + { + "center":{ + "x": {{ opening.center[0] }}, + "z": {{ opening.center[2] }} + }, + "dimensions":{ + "x": {{ opening.dimensions[0] }}, + "z": {{ opening.dimensions[1] }} + } + }{% if not loop.last %},{% endif %} + {% endfor %} + {% endif %} + ] + }{% if not loop.last %},{% endif %} + {% endfor %} +] \ No newline at end of file diff --git a/src/fpm/transformations/tasks.py b/src/fpm/transformations/tasks.py index 7cbbaff..dcf7816 100644 --- a/src/fpm/transformations/tasks.py +++ b/src/fpm/transformations/tasks.py @@ -5,13 +5,17 @@ import numpy as np import matplotlib.pyplot as plt from matplotlib.patches import Polygon as Pol +import logging from fpm.graph import ( get_space_points, get_coordinates_map, - get_waypoint_coord, + get_waypoint_coord_wrt_world, ) +logger = logging.getLogger("floorplan.transformations.tasks") +logger.setLevel(logging.DEBUG) + def inset_shape(points, width=0.3): lines = [] @@ -108,7 +112,7 @@ def transform_insets(g, inset_model_framed, coordinates_map): id_sem = point["name"].split("-") name = "{}-point-{}".format(id_sem[5], id_sem[3]) - x, y, _ = get_waypoint_coord(g, point, coordinates_map) + x, y, _ = get_waypoint_coord_wrt_world(g, point, coordinates_map) inset_points.append({"id": name, "x": x, "y": y, "z": 0, "yaw": 0}) @@ -134,14 +138,14 @@ def get_all_disinfection_tasks(g, inset_width): space_points = get_space_points(g) - print("Creating the insets...") + logger.debug("Creating the insets...") inset_model_framed = create_inset_json_ld(space_points, inset_width) - print("Calculating transformation path...") + logger.debug("Calculating transformation path...") # This just gets the coordinates of all poses in the graph, it doesn't calculate anything coordinates_map = get_coordinates_map(g) - print("Transforming the insets") + logger.debug("Transforming the insets") insets = transform_insets(g, inset_model_framed, coordinates_map) return [dict(id=inset["name"], task=[inset]) for inset in insets] diff --git a/src/fpm/utils.py b/src/fpm/utils.py index f0803d1..19050f8 100644 --- a/src/fpm/utils.py +++ b/src/fpm/utils.py @@ -1,5 +1,6 @@ import os import tomllib +import logging import yaml import json @@ -9,6 +10,9 @@ from jinja2 import Environment, FileSystemLoader, PackageLoader +logger = logging.getLogger("floorplan.utils") +logger.setLevel(logging.DEBUG) + def load_config_file(file_path): with open(file_path, "rb") as f: @@ -16,6 +20,20 @@ def load_config_file(file_path): return data +def render_model_template( + model, output_folder, file_name, template_name, template_path=None +): + template = load_template(template_name, template_path) + + output = template.render(model=model, trim_blocks=True, lstrip_blocks=True) + if file_name.endswith(".json"): + output = json.loads(output) + elif file_name.endswith(".yaml"): + output = yaml.safe_load(output) + + save_file(output_folder, file_name, output) + + def load_template(template_name, template_folder=None): if template_folder is None: loader = PackageLoader("fpm") @@ -39,7 +57,7 @@ def save_file(output_path, file_name, contents): with open(output_file, "w") as f: json.dump(contents, f, indent=4) elif ext in [".pgm", ".jpg"]: - contents.save(output_file, quality=95) + contents.save(output_file, quality=100) elif ext in [".stl"]: # Use different STL export method depending on Blender version if bpy.app.version >= (4, 1, 0): @@ -48,26 +66,32 @@ def save_file(output_path, file_name, contents): bpy.ops.export_mesh.stl(filepath=output_file) elif ext in [".dae"]: bpy.ops.wm.collada_export(filepath=output_file) + elif ext == ".gltf": + bpy.ops.export_scene.gltf(filepath=output_file) else: with open(output_file, "w") as f: f.write(contents) - print("Generated {path}".format(path=output_file)) - + logger.info("Generated {path}".format(path=output_file)) -def build_transformation_matrix(x, y, z, alpha): - c = np.cos - s = np.sin +def build_transformation_matrix(x, y, z, alpha=None, beta=0.0, gamma=0.0, **kwargs): t = np.array([[x], [y], [z], [1]]) # fmt: off - R = np.array([ - [c(alpha), -s(alpha), 0], - [s(alpha), c(alpha), 0], + if alpha is not None: + R = np.array([ + [np.cos(alpha), -np.sin(alpha), 0], + [np.sin(alpha), np.cos(alpha), 0], [0, 0, 1], [0, 0, 0]] - ) + ) + else: + cosx = kwargs.get("direction-cosine-x", [1.0, 0.0, 0.0]) + cosz = kwargs.get("direction-cosine-z", [0.0, 0.0, 1.0]) + cosy = kwargs.get("direction-cosine-y", np.cross(cosz, cosx)) + R = np.vstack((np.array([cosx, cosy, cosz]).T, [0.0] *3)) + # fmt: on return np.hstack((R, t)) diff --git a/src/fpm/visualization/__init__.py b/src/fpm/visualization/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/fpm/visualization/plot.py b/src/fpm/visualization/plot.py new file mode 100644 index 0000000..c9aae77 --- /dev/null +++ b/src/fpm/visualization/plot.py @@ -0,0 +1,160 @@ +import matplotlib.pyplot as plt +from matplotlib.axes import Axes +import numpy as np + +# colors = ("#FF6666", "#005533", "#1199EE") # Colorblind-safe RGB +colors = ("#D55E00", "#009E73", "#56B4E9") # Using a different colorblind-safe palette + + +def plot_3d_frame(ax, matrix, name=None): + """Plot a frame in 3D + Adapted from: https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.html + """ + loc = np.array([matrix[:3, 3], matrix[:3, 3]]) + for i, (axis, c) in enumerate(zip((ax.xaxis, ax.yaxis, ax.zaxis), colors)): + axlabel = axis.axis_name + axis.set_label_text(axlabel) + axis.label.set_color(c) + axis.line.set_color(c) + axis.set_tick_params(colors=c) + line = np.zeros((2, 3)) + line[1, i] = matrix[3, 3] + line_rot = np.zeros((2, 3)) + line_rot[0] = np.dot(matrix[:3, :3], line[0, :]) + line_rot[1] = np.dot(matrix[:3, :3], line[1, :]) + line_plot = line_rot + loc + ax.plot(line_plot[:, 0], line_plot[:, 1], line_plot[:, 2], c) + text_loc = line[1] * 1.2 + text_loc_rot = np.dot(matrix[:3, :3], text_loc) + text_plot = text_loc_rot + loc[0] + ax.text(*text_plot, axlabel.upper(), color=c, va="center", ha="center") + ax.text( + *matrix[:3, 3], + name, + color="k", + va="center", + ha="center", + # fontsize=5 * matrix[3, 3] * 2, + ) + + +def plot_2d_robot(ax, matrix, width, length): + x_vector = get_vector_x_axis(matrix) + x_vector = x_vector / np.linalg.norm(x_vector) + if np.dot(x_vector, np.array([1.0, 0.0, 0.0])) == 0.0: + angle = 0.0 + else: + angle = 90.0 + + x = matrix[0, 3] - (width / 2) + y = matrix[1, 3] - (length / 2) + ax.add_patch( + plt.Rectangle( + (x, y), + width, + length, + rotation_point="center", + angle=angle, + alpha=0.25, + fc="grey", + edgecolor="black", + ) + ) + + +def get_vector_x_axis(matrix): + loc = np.array([matrix[:3, 3], matrix[:3, 3]]) + line = np.zeros((2, 3)) + line[1, 0] = matrix[3, 3] + line_rot = np.zeros((2, 3)) + line_rot[0] = np.dot(matrix[:3, :3], line[0, :]) + line_rot[1] = np.dot(matrix[:3, :3], line[1, :]) + line_plot = line_rot + loc + return line_plot[1, :] - line_plot[0, :] + + +def plot_2d_frame(ax: Axes, matrix, name=None, label_axis=False): + loc = np.array([matrix[:3, 3], matrix[:3, 3]]) + for i, (axis_label, c) in enumerate(zip(("x", "y", "z"), colors)): + line = np.zeros((2, 3)) + line[1, i] = matrix[3, 3] + line_rot = np.zeros((2, 3)) + line_rot[0] = np.dot(matrix[:3, :3], line[0, :]) + line_rot[1] = np.dot(matrix[:3, :3], line[1, :]) + line_plot = line_rot + loc + ax.plot(line_plot[:, 0], line_plot[:, 1], c) + text_loc = line[1] * 1.5 + text_loc_rot = np.dot(matrix[:3, :3], text_loc) + text_plot = text_loc_rot + loc[0] + if label_axis: + ax.text( + text_plot[0], + text_plot[1], + axis_label, + color=c, + va="center", + ha="center", + fontsize="xx-small", + ) + ax.text( + matrix[0, 3], + matrix[1, 3], + name, + color="k", + va="bottom", + ha="center", + fontsize="xx-small", + # fontsize=5 * matrix[3, 3] * 2, + ) + + +if __name__ == "__main__": + + m1 = np.array( + [ + [1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1], + ] + ) + m2 = np.array( + [ + [0.0, 1.0, 0.0, 3.0], + [1.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.0], + [0.0, 0.0, 0.0, 1.0], + ] + ) + m3 = np.array( + [ + [1.0, 0.0, 0.0, 6.0], + [0.0, 0.0, 1.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 1.0], + ] + ) + m4 = np.array( + [ + [0.0, 0.0, 1.0, 9.0], + [0.0, 1.0, 0.0, 0.0], + [1.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 1.0], + ] + ) + ax = plt.figure().add_subplot(projection="3d", proj_type="ortho") + plot_3d_frame(ax, m1, "placement-1234") + plot_3d_frame(ax, m2, "placement-5678") + + ax.set_aspect("equal", adjustable="box") + plt.tight_layout() + plt.show() + + ax = plt.figure().add_subplot() + plot_2d_frame(ax, m1, "placement-1234") + plot_2d_frame(ax, m2, "placement-5678") + plot_2d_frame(ax, m3, "placement-9") + plot_2d_frame(ax, m4, "placement-10") + ax.set_aspect("equal", adjustable="box") + plt.tight_layout() + plt.show()