reorganisation folder

setup/generateSets/render_robot.py

@@ -0,0 +1,805 @@
+import bpy
+import math
+import mathutils
+import json
+from pathlib import Path
+from typing import Any, Dict, Iterable, List, Optional, Tuple
+from mathutils import Matrix
+
+# ============================================================
+# PATHS
+# ============================================================
+
+ROBOT_JSON_FILE = r"C:\Users\kech\SynologyDrive\2026-AppServer-AppRobot\appRobotRendering\data\robot\robot.json"
+OUTPUT_FILE = r"C:\Users\kech\SynologyDrive\2026-AppServer-AppRobot\appRobotRendering\data\simulation\debug\render.png"
+
+# ============================================================
+# DEFAULT MATERIALS
+# ============================================================
+
+DEFAULT_MATERIALS = {
+    "wood": {"baseColor": (0.72, 0.52, 0.33, 1.0), "roughness": 0.85, "metallic": 0.0},
+    "plaWhite": {"baseColor": (0.95, 0.95, 0.95, 1.0), "roughness": 0.45, "metallic": 0.0},
+    "steel": {"baseColor": (0.72, 0.72, 0.75, 1.0), "roughness": 0.25, "metallic": 1.0},
+    "powderCoatBlue": {"baseColor": (0.15, 0.25, 0.70, 1.0), "roughness": 0.55, "metallic": 0.0},
+    "defaultPlastic": {"baseColor": (0.95, 0.95, 0.95, 1.0), "roughness": 0.40, "metallic": 0.0},
+    "skeletonRed": {"baseColor": (0.85, 0.20, 0.20, 1.0), "roughness": 0.35, "metallic": 0.0},
+    "markerBlack": {"baseColor": (0.04, 0.04, 0.04, 1.0), "roughness": 0.80, "metallic": 0.0},
+}
+
+STATE_KEYS = ["x", "y", "z", "a", "b", "c", "e"]
+
+marker_export = []
+
+# ============================================================
+# JSON LOADING
+# ============================================================
+
+with open(ROBOT_JSON_FILE, "r", encoding="utf-8") as f:
+    robot: Dict[str, Any] = json.load(f)
+
+rendering_info = robot.get("renderingInfo", {})
+metric = rendering_info.get("metric", "mm")
+scale_factor = 0.001 if metric == "mm" else 1.0
+
+RENDER_WIDTH = int(rendering_info.get("width", 1200))
+RENDER_HEIGHT = int(rendering_info.get("height", 800))
+
+def as_bool(value: Any, default: bool = False) -> bool:
+    if value is None:
+        return default
+    if isinstance(value, bool):
+        return value
+    if isinstance(value, str):
+        return value.strip().lower() in ("1", "true", "yes", "on")
+    return bool(value)
+
+show_skeleton = as_bool(rendering_info.get("showSkeleton", False))
+show_markers = as_bool(rendering_info.get("showMarkers", False))
+
+state: Dict[str, float] = {k: 0.0 for k in STATE_KEYS}
+for source_name in ("defaultPosition", "recognized", "movements"):
+    source = robot.get(source_name, {}) or {}
+    for k in STATE_KEYS:
+        v = source.get(k, None)
+        if v is not None:
+            try:
+                state[k] = float(v)
+            except Exception:
+                pass
+
+links_def = robot.get("links", {})
+if not isinstance(links_def, dict):
+    raise ValueError("robot.json must contain a top-level 'links' object")
+
+# ============================================================
+# HELPERS
+# ============================================================
+
+def mm_to_m(value: float) -> float:
+    return value * scale_factor
+
+def resolve_scalar(value: Any, state_map: Dict[str, float]) -> float:
+    if value is None:
+        return 0.0
+    if isinstance(value, (int, float)):
+        return float(value)
+    if isinstance(value, str):
+        key = value.strip().lower()
+        if key in state_map:
+            return float(state_map[key])
+        try:
+            return float(key)
+        except ValueError:
+            return 0.0
+    return 0.0
+
+def resolve_vector(value: Any, state_map: Dict[str, float], default_len: int = 3) -> Tuple[float, ...]:
+    if value is None:
+        return tuple(0.0 for _ in range(default_len))
+    if isinstance(value, (int, float, str)):
+        return (resolve_scalar(value, state_map),)
+    if isinstance(value, (list, tuple)):
+        resolved = [resolve_scalar(v, state_map) for v in value]
+        if len(resolved) < default_len:
+            resolved.extend([0.0] * (default_len - len(resolved)))
+        return tuple(resolved[:default_len])
+    return tuple(0.0 for _ in range(default_len))
+
+def resolve_vec3_m(value: Any, state_map: Dict[str, float]) -> Tuple[float, float, float]:
+    vec = list(resolve_vector(value, state_map, default_len=3))
+    while len(vec) < 3:
+        vec.append(0.0)
+    x, y, z = vec[:3]
+    return mm_to_m(x), mm_to_m(y), mm_to_m(z)
+
+def normalize_axis(axis: Iterable[Any]) -> mathutils.Vector:
+    ax = mathutils.Vector((float(axis[0]), float(axis[1]), float(axis[2])))
+    return ax.normalized() if ax.length > 0 else mathutils.Vector((1.0, 0.0, 0.0))
+
+def euler_deg_xyz(values: Any) -> Tuple[float, float, float]:
+    vec = list(resolve_vector(values, state, default_len=3))
+    while len(vec) < 3:
+        vec.append(0.0)
+    return math.radians(vec[0]), math.radians(vec[1]), math.radians(vec[2])
+
+def create_or_get_material(name: str, fallback: str = "defaultPlastic") -> bpy.types.Material:
+    info = rendering_info.get("materials", {}) or {}
+    spec = None
+
+    if isinstance(info, dict):
+        spec = info.get(name)
+
+    if isinstance(spec, dict):
+        base = DEFAULT_MATERIALS.get(name, DEFAULT_MATERIALS[fallback]).copy()
+        if "baseColor" in spec:
+            color = tuple(spec["baseColor"])
+            base["baseColor"] = (*color[:3], 1.0) if len(color) == 3 else tuple(color[:4])
+        if "roughness" in spec:
+            base["roughness"] = float(spec["roughness"])
+        if "metallic" in spec:
+            base["metallic"] = float(spec["metallic"])
+        spec = base
+    else:
+        spec = DEFAULT_MATERIALS.get(name, DEFAULT_MATERIALS[fallback])
+
+    if name in bpy.data.materials:
+        mat = bpy.data.materials[name]
+    else:
+        mat = bpy.data.materials.new(name=name)
+
+    mat.use_nodes = True
+    bsdf = mat.node_tree.nodes.get("Principled BSDF")
+    if bsdf is not None:
+        bsdf.inputs["Base Color"].default_value = spec["baseColor"]
+        bsdf.inputs["Roughness"].default_value = spec["roughness"]
+        bsdf.inputs["Metallic"].default_value = spec["metallic"]
+    return mat
+
+def import_stl(filepath: str) -> List[bpy.types.Object]:
+    path = Path(filepath).resolve()
+    if not path.exists():
+        raise FileNotFoundError(f"STL file not found:\n{path}")
+
+    before = set(bpy.data.objects)
+    bpy.ops.wm.stl_import(filepath=str(path))
+    after = [obj for obj in bpy.data.objects if obj not in before]
+    return after
+
+def create_empty(name: str) -> bpy.types.Object:
+    empty = bpy.data.objects.new(name, None)
+    bpy.context.collection.objects.link(empty)
+    return empty
+
+def safe_parent(child: bpy.types.Object, parent: Optional[bpy.types.Object], keep_world: bool = False):
+    if parent is None:
+        return
+    world_matrix = child.matrix_world.copy()
+    child.parent = parent
+    if keep_world:
+        child.matrix_parent_inverse = parent.matrix_world.inverted()
+        child.matrix_world = world_matrix
+    else:
+        child.matrix_parent_inverse = Matrix.Identity(4)
+
+def create_material_segment(name: str, color: Tuple[float, float, float], roughness: float = 0.35) -> bpy.types.Material:
+    if name in bpy.data.materials:
+        mat = bpy.data.materials[name]
+    else:
+        mat = bpy.data.materials.new(name=name)
+    mat.use_nodes = True
+    bsdf = mat.node_tree.nodes.get("Principled BSDF")
+    if bsdf is not None:
+        bsdf.inputs["Base Color"].default_value = (color[0], color[1], color[2], 1.0)
+        bsdf.inputs["Roughness"].default_value = roughness
+        bsdf.inputs["Metallic"].default_value = 0.0
+    return mat
+
+def create_cylinder_between(
+    name: str,
+    p1_local: Tuple[float, float, float],
+    p2_local: Tuple[float, float, float],
+    radius_m: float,
+    parent: bpy.types.Object,
+    material: bpy.types.Material
+) -> bpy.types.Object:
+    v1 = mathutils.Vector(p1_local)
+    v2 = mathutils.Vector(p2_local)
+    delta = v2 - v1
+    length = delta.length
+    if length <= 1e-9:
+        length = 1e-6
+        delta = mathutils.Vector((0.0, 0.0, 1e-6))
+
+    bpy.ops.mesh.primitive_cylinder_add(radius=radius_m, depth=length)
+    obj = bpy.context.active_object
+    obj.name = name
+    safe_parent(obj, parent, keep_world=False)
+    obj.location = (v1 + v2) * 0.5
+    obj.rotation_mode = "QUATERNION"
+    obj.rotation_quaternion = mathutils.Vector((0, 0, 1)).rotation_difference(delta.normalized())
+    if len(obj.data.materials) == 0:
+        obj.data.materials.append(material)
+    else:
+        obj.data.materials[0] = material
+    return obj
+
+def derive_default_skeleton_from_size(size_mm: List[float]) -> Dict[str, Any]:
+    sx, sy, sz = (float(size_mm[0]), float(size_mm[1]), float(size_mm[2]))
+    ax = max((abs(sx), 0), (abs(sy), 1), (abs(sz), 2), key=lambda x: x[0])[1]
+
+    if ax == 0:
+        return {"from": [0, sy * 0.5, sz * 0.5], "to": [sx, sy * 0.5, sz * 0.5]}
+    if ax == 1:
+        return {"from": [sx * 0.5, 0, sz * 0.5], "to": [sx * 0.5, sy, sz * 0.5]}
+    return {"from": [sx * 0.5, sy * 0.5, 0], "to": [sx * 0.5, sy * 0.5, sz]}
+
+def resolve_stl_path(stl_file: str) -> Path:
+    base_dir = Path(ROBOT_JSON_FILE).parent
+    candidates = [
+        base_dir / stl_file,
+        base_dir / "surfaces" / stl_file,
+        Path(stl_file),
+    ]
+    for c in candidates:
+        p = c.resolve()
+        if p.exists():
+            return p
+    raise FileNotFoundError(
+        "STL file not found in any expected location:\n" +
+        "\n".join(str(c.resolve()) for c in candidates)
+    )
+
+# ============================================================
+# SCENE RESET
+# ============================================================
+
+bpy.ops.object.select_all(action="SELECT")
+bpy.ops.object.delete(use_global=False)
+
+scene = bpy.context.scene
+scene.unit_settings.system = "METRIC"
+scene.unit_settings.length_unit = "MILLIMETERS"
+scene.unit_settings.scale_length = scale_factor
+
+# ============================================================
+# WORLD / RENDER SETTINGS
+# ============================================================
+
+world = scene.world or bpy.data.worlds.new("World")
+scene.world = world
+world.use_nodes = True
+bg = world.node_tree.nodes["Background"]
+bg.inputs[0].default_value = tuple(rendering_info.get("backgroundColor", [0.70, 0.85, 1.0])) + (1.0,)
+bg.inputs[1].default_value = float(rendering_info.get("backgroundStrength", 0.20))
+
+scene.render.engine = "CYCLES"
+scene.view_settings.exposure = float(rendering_info.get("exposure", -1.5))
+scene.cycles.samples = 16
+scene.cycles.preview_samples = 32
+scene.render.resolution_x = RENDER_WIDTH
+scene.render.resolution_y = RENDER_HEIGHT
+scene.render.resolution_percentage = 100
+scene.render.image_settings.file_format = "PNG"
+scene.render.filepath = OUTPUT_FILE
+scene.render.film_transparent = False
+
+# ============================================================
+# FLOOR
+# ============================================================
+
+bpy.ops.mesh.primitive_plane_add(size=2.0, location=(0, 0, mm_to_m(-28.0)))
+floor = bpy.context.active_object
+checker_mat = bpy.data.materials.new(name="Checkerboard")
+checker_mat.use_nodes = True
+nodes = checker_mat.node_tree.nodes
+links = checker_mat.node_tree.links
+nodes.clear()
+
+output_node = nodes.new(type="ShaderNodeOutputMaterial")
+bsdf_node = nodes.new(type="ShaderNodeBsdfPrincipled")
+checker_node = nodes.new(type="ShaderNodeTexChecker")
+mapping_node = nodes.new(type="ShaderNodeMapping")
+texcoord_node = nodes.new(type="ShaderNodeTexCoord")
+
+checker_node.inputs["Color1"].default_value = (0.82, 0.82, 0.82, 1.0)
+checker_node.inputs["Color2"].default_value = (0.18, 0.18, 0.18, 1.0)
+mapping_node.inputs["Scale"].default_value = (20.0, 20.0, 20.0)
+
+links.new(texcoord_node.outputs["UV"], mapping_node.inputs["Vector"])
+links.new(mapping_node.outputs["Vector"], checker_node.inputs["Vector"])
+links.new(checker_node.outputs["Color"], bsdf_node.inputs["Base Color"])
+links.new(bsdf_node.outputs["BSDF"], output_node.inputs["Surface"])
+floor.data.materials.append(checker_mat)
+
+# ============================================================
+# CAMERA
+# ============================================================
+
+cam_data = bpy.data.cameras.new("Camera")
+cam_obj = bpy.data.objects.new("Camera", cam_data)
+bpy.context.collection.objects.link(cam_obj)
+
+cam_pos = resolve_vec3_m(rendering_info.get("cameraPosition", [-400, -700, 300]), state)
+cam_target = resolve_vec3_m(rendering_info.get("cameraTarget", [0, 0, 0]), state)
+cam_obj.location = cam_pos
+cam_data.lens = 50
+cam_vec = mathutils.Vector(cam_target) - mathutils.Vector(cam_pos)
+if cam_vec.length == 0:
+    cam_vec = mathutils.Vector((1, 0, 0))
+cam_obj.rotation_euler = cam_vec.to_track_quat("-Z", "Y").to_euler()
+scene.camera = cam_obj
+
+# ============================================================
+# EXPORT CAMERA CALIBRATION (.npz)
+# ============================================================
+
+import numpy as np
+
+CALIBRATION_OUTPUT = str(
+    Path(OUTPUT_FILE).with_suffix(".npz")
+)
+
+render = scene.render
+cam = cam_obj.data
+
+width_px = render.resolution_x
+height_px = render.resolution_y
+
+scale = render.resolution_percentage / 100.0
+
+width_px *= scale
+height_px *= scale
+
+sensor_width_mm = cam.sensor_width
+sensor_height_mm = cam.sensor_height
+
+focal_mm = cam.lens
+
+# focal length in pixels
+fx = (width_px * focal_mm) / sensor_width_mm
+fy = (height_px * focal_mm) / sensor_height_mm
+
+cx = width_px / 2.0
+cy = height_px / 2.0
+
+camera_matrix = np.array([
+    [fx, 0,  cx],
+    [0,  fy, cy],
+    [0,  0,   1]
+], dtype=np.float32)
+
+# ideal synthetic camera
+dist_coeffs = np.zeros((5, 1), dtype=np.float32)
+
+np.savez(
+    CALIBRATION_OUTPUT,
+
+    # common names
+    camera_matrix=camera_matrix,
+    dist_coeffs=dist_coeffs,
+
+    # compatibility aliases
+    K=camera_matrix,
+    mtx=camera_matrix,
+    dist=dist_coeffs
+)
+
+print("Saved camera calibration:", CALIBRATION_OUTPUT)
+print(camera_matrix)
+
+# ============================================================
+# LIGHTS
+# ============================================================
+
+sun_data = bpy.data.lights.new(name="Sun", type="SUN")
+sun_obj = bpy.data.objects.new(name="Sun", object_data=sun_data)
+bpy.context.collection.objects.link(sun_obj)
+sun_pos = resolve_vec3_m(rendering_info.get("lightPosition", [-500, -500, 500]), state)
+light_target = resolve_vec3_m(rendering_info.get("lightTarget", [0, 0, 0]), state)
+sun_obj.location = sun_pos
+light_vec = mathutils.Vector(light_target) - mathutils.Vector(sun_pos)
+if light_vec.length == 0:
+    light_vec = mathutils.Vector((1, 0, -1))
+sun_obj.rotation_euler = light_vec.to_track_quat("-Z", "Y").to_euler()
+sun_data.energy = float(rendering_info.get("sunEnergy", 0.35))
+
+area_data = bpy.data.lights.new(name="AreaLight", type="AREA")
+area_obj = bpy.data.objects.new(name="AreaLight", object_data=area_data)
+bpy.context.collection.objects.link(area_obj)
+area_obj.location = (mm_to_m(-800), mm_to_m(-1200), mm_to_m(1500))
+area_obj.rotation_euler = (math.radians(60), 0.0, math.radians(-20))
+area_data.energy = float(rendering_info.get("areaEnergy", 120))
+area_data.size = 2.0
+
+# ============================================================
+# ROBOT HIERARCHY
+# ============================================================
+
+link_frames: Dict[str, bpy.types.Object] = {}
+for link_name in links_def.keys():
+    link_frames[link_name] = create_empty(f"{link_name}_frame")
+
+for link_name, link_info in links_def.items():
+    parent_name = link_info.get("parent")
+    parent_frame = link_frames.get(parent_name) if parent_name else None
+    size_mm = link_info.get("size", [100, 100, 100])
+
+    # mount: static position/rotation in parent coordinates
+    mount = create_empty(f"{link_name}_mount")
+    safe_parent(mount, parent_frame, keep_world=False)
+    mount.location = resolve_vec3_m(link_info.get("mountPosition", [0, 0, 0]), state)
+    mount.rotation_euler = euler_deg_xyz(link_info.get("mountRotation", [0, 0, 0]))
+
+    # joint: sits inside the mount, defines pivot/orientation
+    joint_info = link_info.get("jointToParent", {}) or {}
+    joint = create_empty(f"{link_name}_joint")
+    safe_parent(joint, mount, keep_world=False)
+    joint.location = resolve_vec3_m(joint_info.get("origin", [0, 0, 0]), state)
+    joint.rotation_euler = euler_deg_xyz(joint_info.get("rotation", [0, 0, 0]))
+
+    # motion: only this node gets the commanded position/angle
+    motion = create_empty(f"{link_name}_motion")
+    safe_parent(motion, joint, keep_world=False)
+
+    joint_type = str(joint_info.get("type", "fixed")).lower()
+    control_var = str(joint_info.get("variable", joint_info.get("control", ""))).lower()
+    axis = joint_info.get("axis", [1, 0, 0])
+
+    if joint_type == "linear":
+        value_mm = state.get(control_var, 0.0) if control_var else 0.0
+        motion.location = normalize_axis(axis) * mm_to_m(value_mm)
+    elif joint_type == "revolute":
+        value_deg = state.get(control_var, 0.0) if control_var else 0.0
+        motion.rotation_mode = "QUATERNION"
+        motion.rotation_quaternion = mathutils.Quaternion(normalize_axis(axis), math.radians(value_deg))
+
+    # link frame: everything belonging to this link follows motion
+    link_frame = link_frames[link_name]
+    safe_parent(link_frame, motion, keep_world=False)
+
+    # --------------------------------------------------------
+    # VISUAL MESHES
+    # --------------------------------------------------------
+
+    visual_root = create_empty(f"{link_name}_visual")
+    safe_parent(visual_root, link_frame, keep_world=False)
+
+    model_list = link_info.get("model", [])
+    if not isinstance(model_list, list):
+        model_list = []
+
+    for idx, model_def in enumerate(model_list):
+        stl_file = model_def.get("stlFile")
+        if not stl_file:
+            continue
+
+        stl_path = resolve_stl_path(stl_file)
+        imported = import_stl(str(stl_path))
+
+        model_node = create_empty(f"{link_name}_model_{idx}")
+        safe_parent(model_node, visual_root, keep_world=False)
+        model_node.location = resolve_vec3_m(model_def.get("originOfModel", [0, 0, 0]), state)
+        model_node.rotation_euler = euler_deg_xyz(model_def.get("rotationOfModelDegree", [0, 0, 0]))
+
+        material_name = model_def.get("material", "defaultPlastic")
+        material = create_or_get_material(material_name)
+
+        for obj in imported:
+            if obj.type != "MESH":
+                continue
+            safe_parent(obj, model_node, keep_world=True)
+            obj.scale = (scale_factor, scale_factor, scale_factor)
+            if len(obj.data.materials) == 0:
+                obj.data.materials.append(material)
+            else:
+                obj.data.materials[0] = material
+
+    # --------------------------------------------------------
+    # SKELETON DEBUG
+    # --------------------------------------------------------
+
+    if show_skeleton:
+        skeleton_spec = link_info.get("skeleton")
+        if not isinstance(skeleton_spec, dict):
+            skeleton_spec = derive_default_skeleton_from_size(size_mm)
+
+        p1_mm = skeleton_spec.get("from", [0, 0, 0])
+        p2_mm = skeleton_spec.get("to", [0, 0, 0])
+        p1 = resolve_vec3_m(p1_mm, state)
+        p2 = resolve_vec3_m(p2_mm, state)
+
+        sk_radius_mm = float(
+            skeleton_spec.get(
+                "radius",
+                rendering_info.get("skeletonDefaults", {}).get("radius", 4)
+            )
+        )
+        sk_color = skeleton_spec.get(
+            "color",
+            rendering_info.get("skeletonDefaults", {}).get("color", [0.85, 0.20, 0.20])
+        )
+        sk_mat = create_material_segment(f"{link_name}_skeletonMat", tuple(sk_color[:3]))
+
+        create_cylinder_between(
+            f"{link_name}_skeleton",
+            p1,
+            p2,
+            mm_to_m(sk_radius_mm),
+            link_frame,
+            sk_mat
+        )
+
+    # --------------------------------------------------------
+    # MARKERS
+    # --------------------------------------------------------
+
+    import cv2
+    import numpy as np
+    import tempfile
+
+    def create_aruco_material(marker_id: int, marker_name: str):
+        dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_250)
+
+        img = np.zeros((256, 256), dtype=np.uint8)
+        cv2.aruco.generateImageMarker(dictionary, marker_id, 256, img, 1)
+
+        tmpfile = Path(tempfile.gettempdir()) / f"aruco_{marker_id}.png"
+        cv2.imwrite(str(tmpfile), img)
+
+        image = bpy.data.images.load(str(tmpfile))
+
+        mat = bpy.data.materials.new(name=f"{marker_name}_mat")
+        mat.use_nodes = True
+
+        nodes = mat.node_tree.nodes
+        links = mat.node_tree.links
+        nodes.clear()
+
+        out = nodes.new(type="ShaderNodeOutputMaterial")
+        bsdf = nodes.new(type="ShaderNodeBsdfPrincipled")
+        tex = nodes.new(type="ShaderNodeTexImage")
+
+        tex.image = image
+
+        links.new(tex.outputs["Color"], bsdf.inputs["Base Color"])
+        links.new(bsdf.outputs["BSDF"], out.inputs["Surface"])
+
+        return mat
+
+
+    def normal_to_quaternion(normal_vec):
+        normal = mathutils.Vector(normal_vec).normalized()
+
+        default_normal = mathutils.Vector((0, 0, 1))
+
+        return default_normal.rotation_difference(normal)
+
+
+    if show_markers:
+
+        marker_defaults = rendering_info.get("markerDefaults", {}) or {}
+
+        for m in link_info.get("markers", []):
+
+            if not isinstance(m, dict):
+                continue
+
+            marker_id = int(m.get("id", 0))
+
+            marker_name = m.get(
+                "name",
+                f"{link_name}_marker_{marker_id}"
+            )
+
+            marker_size_mm = float(
+                m.get(
+                    "size",
+                    marker_defaults.get("size", 25)
+                )
+            )
+
+            marker_pos = resolve_vec3_m(
+                m.get("position", [0, 0, 0]),
+                state
+            )
+
+            normal = m.get("normal", [0, 0, 1])
+            marker_spin_deg = float(m.get("spin", 0.0))
+
+            bpy.ops.mesh.primitive_plane_add(
+                size=mm_to_m(marker_size_mm)
+            )
+
+            marker_obj = bpy.context.active_object
+            marker_obj.name = marker_name
+
+            safe_parent(marker_obj, link_frame, keep_world=False)
+
+            marker_obj.rotation_mode = "QUATERNION"
+            base_quat = normal_to_quaternion(normal)
+
+            spin_quat = mathutils.Quaternion(
+                mathutils.Vector(normal).normalized(),
+                math.radians(marker_spin_deg)
+            )
+            marker_obj.rotation_quaternion = (
+                base_quat @ spin_quat
+            )
+
+
+            # Marker-Normale im lokalen Link-Raum (aus Marker-Rotation)
+            normal_local = (
+                marker_obj.rotation_quaternion
+                @ mathutils.Vector((0, 0, 1))
+            )
+            normal_local.normalize()
+
+            # minimal vorziehen gegen Z-Fighting (lokaler Versatz)
+            marker_obj.location = (
+                mathutils.Vector(marker_pos)
+                + normal_local * mm_to_m(0.5)
+            )
+
+            marker_mat = create_aruco_material(
+                marker_id,
+                marker_name
+            )
+
+            if len(marker_obj.data.materials) == 0:
+                marker_obj.data.materials.append(marker_mat)
+            else:
+                marker_obj.data.materials[0] = marker_mat    
+
+            # --------------------------------------------------------
+            # --------------------------------------------------------
+            # BACKING PLATE (white PLA behind marker)
+            # --------------------------------------------------------
+
+            plate_side_mm = 28.0
+            plate_thickness_mm = 1.0
+            gap_mm = 0.2   # kleiner Abstand gegen Z-Fighting
+
+            bpy.ops.mesh.primitive_cube_add(size=1.0)
+            plate_obj = bpy.context.active_object
+            plate_obj.name = marker_name + "_plate"
+
+            safe_parent(plate_obj, link_frame, keep_world=False)
+
+            # gleiche Orientierung wie der Marker
+            plate_obj.rotation_mode = "QUATERNION"
+            plate_obj.rotation_quaternion = marker_obj.rotation_quaternion.copy()
+
+            # Normale des Markers im lokalen Link-Raum (aus Marker-Rotation)
+            normal_local = marker_obj.rotation_quaternion @ mathutils.Vector((0, 0, 1))
+            normal_local.normalize()
+
+            # Platte liegt "hinter" dem Marker (lokaler Versatz)
+            plate_obj.location = (
+                marker_obj.location
+                - normal_local * mm_to_m((plate_thickness_mm * 0.5) + gap_mm)
+            )
+
+            # exakte Abmessungen: 26 x 26 x 1 mm
+            plate_obj.dimensions = (
+                mm_to_m(plate_side_mm),
+                mm_to_m(plate_side_mm),
+                mm_to_m(plate_thickness_mm)
+            )
+
+            pla_mat = create_or_get_material("plaWhite")
+            if len(plate_obj.data.materials) == 0:
+                plate_obj.data.materials.append(pla_mat)
+            else:
+                plate_obj.data.materials[0] = pla_mat
+            
+            # Weltmatrix holen (inkl. ALLER Transformationen!)
+            mw = marker_obj.matrix_world
+
+            world_pos = mw.translation
+            world_rot = mw.to_quaternion()
+
+            # Optional: Normal in Weltkoordinaten
+            world_normal = (world_rot @ mathutils.Vector((0, 0, 1))).normalized()
+
+            marker_export.append({
+                "name": marker_name,
+                "id": marker_id,
+                "link": link_name,
+
+                "position_m": [world_pos.x, world_pos.y, world_pos.z],
+
+                # oft nützlich für Computer Vision:
+                "position_mm": [
+                    world_pos.x / scale_factor,
+                    world_pos.y / scale_factor,
+                    world_pos.z / scale_factor
+                ],
+
+                "rotation_quaternion": [
+                    world_rot.w,
+                    world_rot.x,
+                    world_rot.y,
+                    world_rot.z
+                ],
+
+                "normal": [
+                    world_normal.x,
+                    world_normal.y,
+                    world_normal.z
+                ]
+            })
+
+            
+
+# ============================================================
+# DEBUG WORLD AXES
+# ============================================================
+
+def create_axis_arrow(
+    name,
+    direction,
+    color,
+    length_mm=200,
+    radius_mm=2,
+    cone_radius_mm=5,
+    cone_length_mm=20
+):
+    length = mm_to_m(length_mm)
+    radius = mm_to_m(radius_mm)
+    cone_radius = mm_to_m(cone_radius_mm)
+    cone_length = mm_to_m(cone_length_mm)
+
+    dir_vec = mathutils.Vector(direction).normalized()
+
+    bpy.ops.mesh.primitive_cylinder_add(
+        radius=radius,
+        depth=length - cone_length
+    )
+
+    cyl = bpy.context.active_object
+    cyl.name = f"{name}_shaft"
+    cyl.rotation_mode = 'QUATERNION'
+    cyl.rotation_quaternion = mathutils.Vector((0, 0, 1)).rotation_difference(dir_vec)
+    cyl.location = dir_vec * ((length - cone_length) * 0.5)
+
+    bpy.ops.mesh.primitive_cone_add(
+        radius1=cone_radius,
+        depth=cone_length
+    )
+
+    cone = bpy.context.active_object
+    cone.name = f"{name}_tip"
+    cone.rotation_mode = 'QUATERNION'
+    cone.rotation_quaternion = mathutils.Vector((0, 0, 1)).rotation_difference(dir_vec)
+    cone.location = dir_vec * (length - cone_length * 0.5)
+
+    mat = bpy.data.materials.new(name=f"{name}_material")
+    mat.use_nodes = True
+    bsdf = mat.node_tree.nodes["Principled BSDF"]
+    bsdf.inputs["Base Color"].default_value = (color[0], color[1], color[2], 1.0)
+    bsdf.inputs["Roughness"].default_value = 0.3
+    bsdf.inputs["Metallic"].default_value = 0.0
+
+    cyl.data.materials.append(mat)
+    cone.data.materials.append(mat)
+
+create_axis_arrow("AxisX", (1, 0, 0), (1, 0, 0))
+create_axis_arrow("AxisY", (0, 1, 0), (0, 1, 0))
+create_axis_arrow("AxisZ", (0, 0, 1), (0, 0, 1))
+
+# ============================================================
+# RENDER
+# ============================================================
+
+bpy.ops.render.render(write_still=True)
+print("Finished rendering:", OUTPUT_FILE)
+
+
+
+MARKER_OUTPUT = str(Path(OUTPUT_FILE).with_name("markers.json"))
+
+with open(MARKER_OUTPUT, "w", encoding="utf-8") as f:
+    json.dump(marker_export, f, indent=2)
+
+print("Saved marker positions:", MARKER_OUTPUT)

setup/generateSets/runLoop.py

@@ -0,0 +1,104 @@
+import json
+import os
+import subprocess
+import shutil
+import argparse
+
+def update_robot_json(robot_json_file, camera_position, default_position):
+    """Aktualisiert die cameraPosition und defaultPosition in der robot.json-Datei."""
+    try:
+        with open(robot_json_file, 'r') as f:
+            data = json.load(f)
+
+        data['renderingInfo']['cameraPosition'] = camera_position
+        data['renderingInfo']['defaultPosition'] = default_position
+
+        with open(robot_json_file, 'w') as f:
+            json.dump(data, f, indent=2)
+    except FileNotFoundError:
+        print(f"Fehler: Datei {robot_json_file} nicht gefunden.")
+        return False
+    except json.JSONDecodeError:
+        print(f"Fehler: JSON-Datei {robot_json_file} ist ungültig.")
+        return False
+    return True
+
+def run_blender(blender_executable, script_path, log_level):
+    """Führt Blender mit dem angegebenen Skript aus."""
+    try:
+        command = [
+            blender_executable,
+            "-b",
+            "--python",
+            script_path,
+            "--log-level",
+            str(log_level)
+        ]
+        subprocess.run(command, check=True, capture_output=True, text=True)
+        return True
+    except subprocess.CalledProcessError as e:
+        print(f"Blender-Skript fehlgeschlagen:\n{e.stderr}")
+        return False
+
+def copy_and_rename_file(source_file, destination_dir, new_filename):
+    """Kopiert die erstellte Bilddatei in den Zielordner und benennt sie um."""
+    destination_path = os.path.join(destination_dir, new_filename)
+    try:
+        shutil.copy2(source_file, destination_path)  # copy2 behält Metadaten
+        return True
+    except FileNotFoundError:
+        print(f"Fehler: Quelldatei {source_file} nicht gefunden.")
+        return False
+    except Exception as e:
+        print(f"Fehler beim Kopieren/Umbenennen der Datei: {e}")
+        return False
+
+def main():
+    parser = argparse.ArgumentParser(description="Automatisiert die Roboter-Rendering-Pipeline.")
+    parser.add_argument("robot_json", help="Pfad zur robot.json-Datei.")
+    parser.add_argument("blender_executable", help="Pfad zur Blender-Executable.")
+    parser.add_argument("render_script", help="Pfad zum render_robot.py-Skript.")
+    parser.add_argument("output_dir", help="Zielordner für die gerenderten Bilder.")
+    parser.add_argument("--log_level", type=int, default=2, help="Log-Level für Blender (Standard: 2).")
+    args = parser.parse_args()
+
+    # Kamerapositions-Dictionary
+    camera_positions = {
+        "a": [-300, -800, 500],
+        "b": [300, -700, 500],
+        "c": [600, -500, 600],
+        "d": [-10, -800, 500],
+        "e": [-500, 300, 1200],
+        "f": [1200, 200, 300]
+    }
+
+    # Robot-Pose-Dictionary
+    robot_poses = {
+        "3": {"x": 10, "y": 4, "z": 20, "a": 10, "b": 2, "c": 9, "e": 1},
+        "4": {"x": 40, "y": 48, "z": -30, "a": 30, "b": 23, "c": 9, "e": 8},
+        "5": {"x": 80, "y": 93, "z": -120, "a": 120, "b": 23, "c": 9, "e": 3},
+        "6": {"x": 80, "y": 20, "z": 80, "a": -120, "b": 23, "c": 9, "e": 3}
+    }
+
+    for set_name, camera_position in camera_positions.items():
+        for pose_name, default_position in robot_poses.items():
+
+            # 1. JSON aktualisieren
+            if not update_robot_json(args.robot_json, camera_position, default_position):
+                continue  # Gehe zum nächsten Schleifendurchlauf
+
+            # 2. Blender-Skript ausführen
+            if not run_blender(args.blender_executable, args.render_script, args.log_level):
+                continue # Gehe zum nächsten Schleifendurchlauf
+
+            # 3. Datei kopieren und umbenennen
+            render_script_dir = os.path.dirname(args.render_script)
+            render_file = os.path.join(render_script_dir, "render.png")
+            new_filename = f"render_set{set_name}_{pose_name}.png"
+            if not copy_and_rename_file(render_file, args.output_dir, new_filename):
+                continue # Gehe zum nächsten Schleifendurchlauf
+
+            print(f"Rendering für Set {set_name}, Pose {pose_name} erfolgreich abgeschlossen.")
+
+if __name__ == "__main__":
+    main()

setup/precheck/check_constraints.py

@@ -0,0 +1,123 @@
+import json
+import math
+
+# Load robot.json
+with open(r'../../data/robot/robot.json') as f:
+    robot = json.load(f)
+
+# Extract markers by link
+markers_by_link = {}
+for link_name, link_data in robot['links'].items():
+    markers = link_data.get('markers', [])
+    if markers:
+        markers_by_link[link_name] = markers
+
+print("=" * 70)
+print("MARKER DISTRIBUTION BY LINK")
+print("=" * 70)
+for link, markers in markers_by_link.items():
+    print(f"\n{link}: {len(markers)} markers")
+    for m in markers:
+        pos = m['position']
+        print(f"  ID {m['id']:3d}: pos={pos}")
+
+# Check rigid body constraints: distances within each link
+print("\n" + "=" * 70)
+print("RIGID BODY CONSTRAINT CHECK (Arm1, Ellbow, Arm2)")
+print("=" * 70)
+
+for link_name in ['Arm1', 'Ellbow', 'Arm2']:
+    if link_name not in markers_by_link:
+        continue
+    
+    markers = markers_by_link[link_name]
+    positions = [(m['position'][0], m['position'][1], m['position'][2]) for m in markers]
+    
+    print(f"\n{link_name}:")
+    if len(positions) > 1:
+        for i in range(len(positions)):
+            for j in range(i+1, len(positions)):
+                dx = positions[i][0] - positions[j][0]
+                dy = positions[i][1] - positions[j][1]
+                dz = positions[i][2] - positions[j][2]
+                dist = math.sqrt(dx*dx + dy*dy + dz*dz)
+                print(f"  Marker {markers[i]['id']:3d} <-> {markers[j]['id']:3d}: {dist:7.2f} mm")
+    else:
+        print(f"  Only {len(positions)} marker(s)")
+
+# Check X-distances between links
+print("\n" + "=" * 70)
+print("X-POSITION CONSTRAINT CHECK (Link relationships)")
+print("=" * 70)
+
+# Get marker positions per link
+link_x_positions = {}
+for link_name in ['Board', 'Base', 'Arm1', 'Ellbow', 'Arm2']:
+    if link_name in markers_by_link:
+        x_vals = [m['position'][0] for m in markers_by_link[link_name]]
+        link_x_positions[link_name] = {
+            'min_x': min(x_vals),
+            'max_x': max(x_vals),
+            'count': len(x_vals)
+        }
+
+print("\nX-position ranges by link (local coords):")
+for link, data in link_x_positions.items():
+    print(f"  {link:10s}: x in [{data['min_x']:7.2f}, {data['max_x']:7.2f}] mm  ({data['count']} markers)")
+
+# Check X-distance between Arm1 and Ellbow
+print("\nKey constraint analysis:")
+arm1_x = [m['position'][0] for m in markers_by_link['Arm1']]
+ellbow_x = [m['position'][0] for m in markers_by_link['Ellbow']]
+print(f"  Arm1 local X: {arm1_x}")
+print(f"  Ellbow local X: {ellbow_x}")
+print(f"  => BOTH rotate around X-axis, so X-spread within each link is FIXED")
+print(f"  => X-distance between links is FIXED in world (different local X values)")
+
+# Check Arm2 structure
+print("\n" + "=" * 70)
+print("ARM2 KINEMATIC CHECK (sin(a) dependency)")
+print("=" * 70)
+
+arm2_markers = markers_by_link['Arm2']
+print(f"\nArm2 has {len(arm2_markers)} markers")
+print("Arm2 marker positions (local coords, before 'a' rotation around Y):")
+for m in arm2_markers:
+    pos = m['position']
+    print(f"  ID {m['id']:3d}: x={pos[0]:8.2f}, y={pos[1]:8.2f}, z={pos[2]:8.2f}")
+
+print("\nWhen Arm2 rotates around Y-axis (variable 'a'):")
+print("  Rotation matrix Ry(a) acts on [x_local, y_local, z_local]:")
+print("  X_world = 90 + x_local * cos(a) - z_local * sin(a)")
+print("  Y_world = y_local (unchanged)")
+print("  Z_world = x_local * sin(a) + z_local * cos(a)")
+print("\n=> X_world DEPENDS on sin(a) and z_local values!")
+
+# Verify: X differences between Arm2 markers
+arm2_x_vals = [m['position'][0] for m in arm2_markers]
+arm2_z_vals = [m['position'][2] for m in arm2_markers]
+print(f"\nArm2 local X values: {sorted(set(arm2_x_vals))}")
+print(f"Arm2 local Z values: {sorted(set(arm2_z_vals))}")
+if len(set(arm2_z_vals)) > 1:
+    print(f"=> Multiple Z values present: X_world will differ by sin(a) contributions")
+
+print("\n" + "=" * 70)
+print("SUMMARY: WHICH CONSTRAINTS MAKE SENSE?")
+print("=" * 70)
+print("""
+1. RIGID BODY DISTANCES within Arm1/Ellbow/Arm2:
+   ✓ VALID - Fixed distances between markers on same rigid link
+   ✓ HELPS with: Preventing deformation, reducing degrees of freedom
+   
+2. X-DISTANCES between links (Arm1, Ellbow, Base):
+   ✓ VALID - Both rotate around X-axis, so relative X is fixed
+   ✓ HELPS with: Preventing sliding along links
+   
+3. X-POSITION of Arm2 dependent on sin(a):
+   ⚠ PARTIALLY VALID - Different markers have different dependencies
+   ✓ HELPS with: Constraining the a variable from Z-spread observation
+   
+IMPLEMENTATION RECOMMENDATION:
+- Start with constraints 1 & 2 (rigid body + inter-link X)
+- Use constraint 3 as a sanity check on 'a' estimation
+""")

setup/precheck/check_robot_marker_uniqueness.py

@@ -0,0 +1,105 @@
+#!/usr/bin/env python3
+"""
+check_robot_marker_uniqueness.py
+
+Standalone checker for duplicate / invalid marker IDs in robot.json.
+
+Use this before running bundle adjustment:
+    python check_robot_marker_uniqueness.py -robot robot.json
+
+Exit codes:
+    0 = no duplicates found
+    1 = duplicates or structural problems found
+"""
+
+from __future__ import annotations
+
+import argparse
+import json
+import os
+import sys
+from collections import defaultdict
+from typing import Any, Dict, List, Tuple
+
+
+def resolve_path(path: str) -> str:
+    path = os.path.expanduser(path)
+    if os.path.isabs(path):
+        return path
+    return os.path.abspath(path)
+
+
+def load_json(path: str) -> Dict[str, Any]:
+    with open(resolve_path(path), "r", encoding="utf-8") as f:
+        return json.load(f)
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser(description="Check marker ID uniqueness in robot.json")
+    parser.add_argument("-robot", "--robot", default="../../data/robot/robot.json", help="Path to robot.json")
+    parser.add_argument(
+        "--strict",
+        action="store_true",
+        help="Return exit code 1 if any duplicate or invalid marker is found",
+    )
+    args = parser.parse_args()
+
+    robot = load_json(args.robot)
+    links = robot.get("links", {}) or {}
+
+    id_locations: Dict[int, List[Tuple[str, int, str]]] = defaultdict(list)
+    invalid_entries: List[str] = []
+    total_markers = 0
+
+    for link_name, link_data in links.items():
+        markers = link_data.get("markers", []) or []
+        for idx, marker in enumerate(markers):
+            total_markers += 1
+            marker_id = marker.get("id", None)
+            if marker_id is None:
+                invalid_entries.append(f"link='{link_name}' marker_index={idx}: missing id")
+                continue
+            try:
+                marker_id_int = int(marker_id)
+            except Exception:
+                invalid_entries.append(f"link='{link_name}' marker_index={idx}: non-integer id={marker_id!r}")
+                continue
+
+            name = str(marker.get("name", f"marker_{marker_id_int}"))
+            id_locations[marker_id_int].append((link_name, idx, name))
+
+            pos = marker.get("position", None)
+            if pos is None or not isinstance(pos, list) or len(pos) != 3:
+                invalid_entries.append(
+                    f"link='{link_name}' marker_id={marker_id_int}: invalid/missing 3D position"
+                )
+
+    duplicates = {mid: locs for mid, locs in id_locations.items() if len(locs) > 1}
+
+    print(f"[INFO] Total marker entries: {total_markers}")
+    print(f"[INFO] Unique marker IDs   : {len(id_locations)}")
+    print(f"[INFO] Duplicate IDs       : {len(duplicates)}")
+    print(f"[INFO] Invalid entries     : {len(invalid_entries)}")
+
+    if duplicates:
+        print("\n[DUPLICATES]")
+        for mid in sorted(duplicates.keys()):
+            print(f"  marker_id={mid}")
+            for link_name, idx, name in duplicates[mid]:
+                print(f"    - link='{link_name}' marker_index={idx} name='{name}'")
+
+    if invalid_entries:
+        print("\n[INVALID ENTRIES]")
+        for item in invalid_entries:
+            print(f"  - {item}")
+
+    if duplicates or invalid_entries:
+        print("\n[WARN] robot.json is not clean for deterministic constraint generation.")
+        if args.strict:
+            sys.exit(1)
+    else:
+        print("\n[OK] No duplicate marker IDs found.")
+
+
+if __name__ == "__main__":
+    main()

setup/precheck/json_to_urdf.py

@@ -0,0 +1,246 @@
+# json_to_urdf.py
+import json
+import math
+import xml.etree.ElementTree as ET
+from pathlib import Path
+from xml.dom import minidom
+
+# ============================================================
+# CONFIG
+# ============================================================
+
+ROBOT_JSON = "../../data/robot/robot.json"
+OUTPUT_URDF = "robot.urdf"
+
+# ============================================================
+# HELPERS
+# ============================================================
+
+
+def mm_to_m(v):
+    return float(v) / 1000.0
+
+
+def vec_mm_to_m(v):
+    return [mm_to_m(x) for x in v]
+
+
+def deg_to_rad(v):
+    return math.radians(float(v))
+
+
+def vec_deg_to_rad(v):
+    return [deg_to_rad(x) for x in v]
+
+
+def xyz_str(v):
+    return f"{v[0]:.6f} {v[1]:.6f} {v[2]:.6f}"
+
+
+# ============================================================
+# LOAD JSON
+# ============================================================
+
+with open(ROBOT_JSON, "r", encoding="utf-8") as f:
+    robot_json = json.load(f)
+
+links = robot_json.get("links", {})
+
+# ============================================================
+# ROOT
+# ============================================================
+
+robot_name = Path(ROBOT_JSON).stem
+robot = ET.Element("robot")
+robot.set("name", robot_name)
+
+# ============================================================
+# CREATE LINKS
+# ============================================================
+
+for link_name, link_info in links.items():
+
+    urdf_link = ET.SubElement(robot, "link")
+    urdf_link.set("name", link_name)
+
+    # --------------------------------------------------------
+    # VISUALS
+    # --------------------------------------------------------
+
+    models = link_info.get("model", [])
+
+    for model in models:
+
+        stl_file = model.get("stlFile")
+        if not stl_file:
+            continue
+
+        visual = ET.SubElement(urdf_link, "visual")
+
+        origin = ET.SubElement(visual, "origin")
+
+        xyz = vec_mm_to_m(model.get("originOfModel", [0, 0, 0]))
+        rpy = vec_deg_to_rad(model.get("rotationOfModelDegree", [0, 0, 0]))
+
+        origin.set("xyz", xyz_str(xyz))
+        origin.set("rpy", xyz_str(rpy))
+
+        geometry = ET.SubElement(visual, "geometry")
+
+        mesh = ET.SubElement(geometry, "mesh")
+        mesh.set("filename", stl_file)
+        mesh.set("scale", "0.001 0.001 0.001")
+
+    # --------------------------------------------------------
+    # SIMPLE COLLISION FROM SKELETON
+    # --------------------------------------------------------
+
+    skeleton = link_info.get("skeleton")
+
+    if isinstance(skeleton, dict):
+
+        p1 = skeleton.get("from", [0, 0, 0])
+        p2 = skeleton.get("to", [0, 0, 0])
+
+        dx = p2[0] - p1[0]
+        dy = p2[1] - p1[1]
+        dz = p2[2] - p1[2]
+
+        length_mm = math.sqrt(dx * dx + dy * dy + dz * dz)
+        length_m = mm_to_m(length_mm)
+
+        radius_mm = skeleton.get("radius", 4)
+        radius_m = mm_to_m(radius_mm)
+
+        cx = (p1[0] + p2[0]) * 0.5
+        cy = (p1[1] + p2[1]) * 0.5
+        cz = (p1[2] + p2[2]) * 0.5
+
+        collision = ET.SubElement(urdf_link, "collision")
+
+        collision_origin = ET.SubElement(collision, "origin")
+        collision_origin.set(
+            "xyz",
+            xyz_str(vec_mm_to_m([cx, cy, cz]))
+        )
+
+        # ----------------------------------------------------
+        # CYLINDER ORIENTATION
+        # URDF cylinder points along Z
+        # ----------------------------------------------------
+
+        roll = 0.0
+        pitch = 0.0
+        yaw = 0.0
+
+        if abs(dx) >= abs(dy) and abs(dx) >= abs(dz):
+            pitch = math.radians(90)
+            yaw = 0.0
+
+        elif abs(dy) >= abs(dx) and abs(dy) >= abs(dz):
+            pitch = math.radians(90)
+            yaw = math.radians(90)
+
+        collision_origin.set(
+            "rpy",
+            xyz_str([roll, pitch, yaw])
+        )
+
+        geometry = ET.SubElement(collision, "geometry")
+
+        cylinder = ET.SubElement(geometry, "cylinder")
+        cylinder.set("radius", f"{radius_m:.6f}")
+        cylinder.set("length", f"{length_m:.6f}")
+
+# ============================================================
+# CREATE JOINTS
+# ============================================================
+
+for link_name, link_info in links.items():
+
+    parent_name = link_info.get("parent")
+
+    if parent_name is None:
+        continue
+
+    joint_info = link_info.get("jointToParent", {})
+
+    joint_name = joint_info.get("name", f"joint_{parent_name}_{link_name}")
+    joint_type = joint_info.get("type", "fixed")
+
+    if joint_type == "linear":
+        urdf_joint_type = "prismatic"
+    elif joint_type == "revolute":
+        urdf_joint_type = "revolute"
+    else:
+        urdf_joint_type = "fixed"
+
+    joint = ET.SubElement(robot, "joint")
+    joint.set("name", joint_name)
+    joint.set("type", urdf_joint_type)
+
+    # --------------------------------------------------------
+    # PARENT / CHILD
+    # --------------------------------------------------------
+
+    parent = ET.SubElement(joint, "parent")
+    parent.set("link", parent_name)
+
+    child = ET.SubElement(joint, "child")
+    child.set("link", link_name)
+
+    # --------------------------------------------------------
+    # ORIGIN
+    # --------------------------------------------------------
+
+    origin_xyz = joint_info.get("origin", [0, 0, 0])
+    origin_rpy = joint_info.get("rotation", [0, 0, 0])
+
+    origin = ET.SubElement(joint, "origin")
+    origin.set(
+        "xyz",
+        xyz_str(vec_mm_to_m(origin_xyz))
+    )
+    origin.set(
+        "rpy",
+        xyz_str(vec_deg_to_rad(origin_rpy))
+    )
+
+    # --------------------------------------------------------
+    # AXIS
+    # --------------------------------------------------------
+
+    axis = joint_info.get("axis", [1, 0, 0])
+
+    axis_tag = ET.SubElement(joint, "axis")
+    axis_tag.set("xyz", xyz_str(axis))
+
+    # --------------------------------------------------------
+    # DEFAULT LIMITS
+    # --------------------------------------------------------
+
+    if urdf_joint_type in ["revolute", "prismatic"]:
+
+        limit = ET.SubElement(joint, "limit")
+
+        if urdf_joint_type == "revolute":
+            limit.set("lower", f"{-math.pi:.6f}")
+            limit.set("upper", f"{math.pi:.6f}")
+        else:
+            limit.set("lower", "-1.0")
+            limit.set("upper", "1.0")
+
+        limit.set("effort", "100")
+        limit.set("velocity", "10")
+
+# ============================================================
+# PRETTY XML
+# ============================================================
+
+xml_string = ET.tostring(robot, encoding="utf-8")
+pretty = minidom.parseString(xml_string).toprettyxml(indent="  ")
+
+with open(OUTPUT_URDF, "w", encoding="utf-8") as f:
+    f.write(pretty)
+
+print("URDF written:", OUTPUT_URDF)

setup/precheck/robot.urdf

@@ -0,0 +1,179 @@
+<?xml version="1.0" ?>
+<robot name="robot">
+  <link name="Board">
+    <visual>
+      <origin xyz="0.000000 0.000000 0.000000" rpy="0.000000 0.000000 -1.570796"/>
+      <geometry>
+        <mesh filename="surfaces/Board.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <visual>
+      <origin xyz="0.000000 0.000000 0.000000" rpy="0.000000 0.000000 -1.570796"/>
+      <geometry>
+        <mesh filename="surfaces/BoardRail.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.500000 0.000000 0.016000" rpy="0.000000 1.570796 0.000000"/>
+      <geometry>
+        <cylinder radius="0.004000" length="1.000000"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="Base">
+    <visual>
+      <origin xyz="-0.030000 0.000000 -0.035000" rpy="0.000000 0.000000 0.000000"/>
+      <geometry>
+        <mesh filename="surfaces/Base.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.055000 0.108000 0.045000" rpy="0.000000 1.570796 0.000000"/>
+      <geometry>
+        <cylinder radius="0.004000" length="0.110000"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="Arm1">
+    <visual>
+      <origin xyz="-0.029000 0.025000 0.028500" rpy="3.141593 0.000000 -1.570796"/>
+      <geometry>
+        <mesh filename="surfaces/Holm.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.000000 -0.125000 0.000000" rpy="0.000000 1.570796 1.570796"/>
+      <geometry>
+        <cylinder radius="0.004000" length="0.250000"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="Ellbow">
+    <visual>
+      <origin xyz="0.090000 0.000000 0.000000" rpy="0.000000 -1.570796 -1.570796"/>
+      <geometry>
+        <mesh filename="surfaces/Ellebogen.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.045000 0.000000 0.000000" rpy="0.000000 1.570796 0.000000"/>
+      <geometry>
+        <cylinder radius="0.004000" length="0.090000"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="Arm2">
+    <visual>
+      <origin xyz="0.000000 -0.250000 0.000000" rpy="3.141593 0.000000 -1.570796"/>
+      <geometry>
+        <mesh filename="surfaces/Unterarm.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.000000 -0.125000 0.000000" rpy="0.000000 1.570796 1.570796"/>
+      <geometry>
+        <cylinder radius="0.004000" length="0.250000"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="Hand">
+    <collision>
+      <origin xyz="0.000000 -0.017500 0.000000" rpy="0.000000 1.570796 1.570796"/>
+      <geometry>
+        <cylinder radius="0.004000" length="0.035000"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="Palm">
+    <collision>
+      <origin xyz="0.000000 -0.035000 0.000000" rpy="0.000000 1.570796 0.000000"/>
+      <geometry>
+        <cylinder radius="0.007000" length="0.100000"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="FingerA">
+    <visual>
+      <origin xyz="0.024000 0.000000 -0.009100" rpy="1.570796 -1.570796 0.000000"/>
+      <geometry>
+        <mesh filename="surfaces/Finger.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.000000 -0.030000 0.000000" rpy="0.000000 1.570796 1.570796"/>
+      <geometry>
+        <cylinder radius="0.004000" length="0.060000"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="FingerB">
+    <visual>
+      <origin xyz="-0.024000 0.000000 0.009100" rpy="1.570796 1.570796 0.000000"/>
+      <geometry>
+        <mesh filename="surfaces/Finger.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.000000 -0.030000 0.000000" rpy="0.000000 1.570796 1.570796"/>
+      <geometry>
+        <cylinder radius="0.004000" length="0.060000"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="Slider" type="prismatic">
+    <parent link="Board"/>
+    <child link="Base"/>
+    <origin xyz="0.000000 0.000000 0.016000" rpy="0.000000 0.000000 0.000000"/>
+    <axis xyz="1.000000 0.000000 0.000000"/>
+    <limit lower="-1.0" upper="1.0" effort="100" velocity="10"/>
+  </joint>
+  <joint name="Joint1" type="revolute">
+    <parent link="Base"/>
+    <child link="Arm1"/>
+    <origin xyz="0.110000 0.108000 0.045000" rpy="0.000000 0.000000 0.000000"/>
+    <axis xyz="-1.000000 0.000000 0.000000"/>
+    <limit lower="-3.141593" upper="3.141593" effort="100" velocity="10"/>
+  </joint>
+  <joint name="Joint2" type="revolute">
+    <parent link="Arm1"/>
+    <child link="Ellbow"/>
+    <origin xyz="0.000000 -0.250000 0.000000" rpy="0.000000 0.000000 0.000000"/>
+    <axis xyz="-1.000000 0.000000 0.000000"/>
+    <limit lower="-3.141593" upper="3.141593" effort="100" velocity="10"/>
+  </joint>
+  <joint name="Joint3" type="revolute">
+    <parent link="Ellbow"/>
+    <child link="Arm2"/>
+    <origin xyz="0.090000 0.000000 0.000000" rpy="0.000000 0.000000 0.000000"/>
+    <axis xyz="0.000000 -1.000000 0.000000"/>
+    <limit lower="-3.141593" upper="3.141593" effort="100" velocity="10"/>
+  </joint>
+  <joint name="Joint4" type="revolute">
+    <parent link="Arm2"/>
+    <child link="Hand"/>
+    <origin xyz="0.000000 -0.250000 0.000000" rpy="0.000000 0.000000 0.000000"/>
+    <axis xyz="1.000000 0.000000 0.000000"/>
+    <limit lower="-3.141593" upper="3.141593" effort="100" velocity="10"/>
+  </joint>
+  <joint name="Joint3" type="revolute">
+    <parent link="Hand"/>
+    <child link="Palm"/>
+    <origin xyz="0.000000 0.000000 0.000000" rpy="0.000000 0.000000 0.000000"/>
+    <axis xyz="0.000000 -1.000000 0.000000"/>
+    <limit lower="-3.141593" upper="3.141593" effort="100" velocity="10"/>
+  </joint>
+  <joint name="Slider" type="prismatic">
+    <parent link="Palm"/>
+    <child link="FingerA"/>
+    <origin xyz="0.004000 -0.035000 0.000000" rpy="0.000000 0.000000 0.000000"/>
+    <axis xyz="1.000000 0.000000 0.000000"/>
+    <limit lower="-1.0" upper="1.0" effort="100" velocity="10"/>
+  </joint>
+  <joint name="Slider" type="prismatic">
+    <parent link="Palm"/>
+    <child link="FingerB"/>
+    <origin xyz="-0.004000 -0.035000 0.000000" rpy="0.000000 0.000000 0.000000"/>
+    <axis xyz="-1.000000 0.000000 0.000000"/>
+    <limit lower="-1.0" upper="1.0" effort="100" velocity="10"/>
+  </joint>
+</robot>