ChK/appRobotRender
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

reorganisation folder

Browse Source
This commit is contained in:
chk
2026-05-30 12:38:31 +02:00
parent 423f3402e3
commit 5f1b27f631
26 changed files with 657 additions and 618 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
pipeline/SetGenerator/render.npz
View File

Binary file not shown.

BIN
pipeline/SetGenerator/render.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 1.2 MiB

805
pipeline/SetGenerator/render_robot.py
View File

@@ -1,805 +0,0 @@
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\robot.json"
OUTPUT_FILE = r"C:\Users\kech\SynologyDrive\2026-AppServer-AppRobot\appRobotRendering\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)

518
pipeline/SetGenerator/robot.json
View File

@@ -1,518 +0,0 @@
{
"coordinateSystem": {
"handedness": "right",
"x": "right",
"y": "backward",
"z": "up"
},
"units": {
"length": "mm",
"rotation": "degree"
},
"vision_config": {
"MarkerType": "DICT_4X4_250",
"MarkerSize": 0.025
},
"renderingInfo": {
"width": 1280,
"height": 720,
"cameraPosition__1": [-10, -800, 500],
"cameraPosition__2": [-500, 300, 1200],
"cameraPosition__3": [-200, -900, 200],
"cameraPosition__4": [1200, 200, 300],
"cameraPosition_a":[-300, -800,500],
"cameraPosition":[300, -700,500],
"cameraPosition_c":[600, -500,600],
"cameraTarget": [210, -180, 180],
"cameraUpVector": [0, 0, 1],
"lightPosition": [-500, -500, 500],
"lightTarget": [0, 0, 0],
"lightUpVector": [0, 0, 1],
"metric": "mm",
"showSkeleton": true,
"showMarkers": true,
"backgroundColor": [0.70, 0.85, 1.0],
"backgroundStrength": 0.20,
"sunEnergy": 0.35,
"areaEnergy": 120,
"exposure": -1.5,
"materials": {
"wood": {
"baseColor": [0.72, 0.52, 0.33],
"roughness": 0.85,
"metallic": 0.0
},
"plaWhite": {
"baseColor": [0.95, 0.95, 0.95],
"roughness": 0.45,
"metallic": 0.0
},
"steel": {
"baseColor": [0.72, 0.72, 0.75],
"roughness": 0.25,
"metallic": 1.0
},
"powderCoatBlue": {
"baseColor": [0.15, 0.25, 0.7],
"roughness": 0.55,
"metallic": 0.0
},
"defaultPlastic": {
"baseColor": [0.95, 0.95, 0.95],
"roughness": 0.4,
"metallic": 0.0
},
"skeletonRed": {
"baseColor": [0.85, 0.20, 0.20],
"roughness": 0.35,
"metallic": 0.0
},
"markerBlack": {
"baseColor": [0.04, 0.04, 0.04],
"roughness": 0.80,
"metallic": 0.0
}
},
"skeletonDefaults": {
"radius": 4,
"color": [0.85, 0.20, 0.20]
},
"markerDefaults": {
"size": 25,
"thickness": 1,
"color": [0.04, 0.04, 0.04]
}
},
"defaultPosition__": {
"x": 10,
"y": 4,
"z": 20,
"a": 10,
"b": 2,
"c": 9,
"e": 1
},
"defaultPosition": {
"x": 140,
"y": 46,
"z": -70,
"a": 120,
"b": 22,
"c": 91,
"e": 10
},
"recognized": {
"x": null,
"y": null,
"z": null,
"a": null,
"b": null,
"c": null,
"e": null
},
"constraint_rules": {
"rigid_distance": {
"enabled": true,
"mode": "mst",
"weight": 1.0
},
"joint_axis_projection": {
"enabled": true,
"max_pairs": 2,
"weight": 0.35
},
"chain_axis_projection": {
"enabled": false,
"max_depth": 3,
"max_pairs": 2,
"weight": 0.15
},
"axis_alignment_threshold": 0.95
},
"observation_weighting": {
"enabled": true,
"distance_weight": true,
"marker_size_weight": true,
"view_angle_weight": true
},
"multiview_calculation": {
"combine_mode": "mean",
"size_ref_px": 50.0,
"border_ref_px": 120.0,
"center_ref_norm": 0.01,
"sharpness_ref": 2500.0,
"homography_ref": 0.18,
"size_factor": 0.3,
"aspect_factor": 0.3,
"border_factor": 0.01,
"center_factor": 0.01,
"sharpness_factor": 0.5,
"homography_factor": 0.2,
"normal_visibility_factor": 0.01,
"spin_factor": 0.3,
"weight_floor": 0.3
},
"movements": {
"x": null,
"y": null,
"z": null,
"a": null,
"b": null,
"c": null,
"e": null
},
"state_pose_params":{
"numbers_of_Elements_to_consider_start": 3,
"numbers_of_Elements_to_consider_final": 5,
"solver_in_between_geometrical": false,
"solver_after_geometrical": false,
"geometric_passes_per_stage": 2,
"revolute_search_coarse_deg": 5.0,
"revolute_search_fine_deg": 1.0,
"root_pose_min_markers": 3,
"use_marker_normals_flip_tiebreak": true,
"normal_flip_weight": 0.05
},
"links": {
"Board": {
"parent": null,
"size": [1000, 200, 25],
"mountPosition": [0, 0, 0],
"mountRotation": [0, 0, 0],
"skeleton": {
"from": [0, 0, 16],
"to": [1000, 0, 16],
"radius": 4,
"color": [0.85, 0.20, 0.20]
},
"markers":[
{"id":210,"position":[20, -20, 0.3], "normal":[0,0,1]},
{"id":211,"position":[250, -10, 0.3], "normal":[0,0,1]},
{"id":215,"position":[250, -90, 0.3], "normal":[0,0,1]},
{"id":214,"position":[350, -10, 0.3], "normal":[0,0,1]},
{"id":208,"position":[350, -90, 0.3], "normal":[0,0,1]},
{"id":206,"position":[650, -10, 0.3], "normal":[0,0,1]},
{"id":205,"position":[750, -90, 0.3], "normal":[0,0,1]},
{"id":207,"position":[750, -10, 0.3], "normal":[0,0,1]},
{"id":217,"position":[650, -90, 0.3], "normal":[0,0,1]}
],
"model": [
{
"stlFile": "surfaces/Board.stl",
"originOfModel": [0, 0, 0],
"rotationOfModelDegree": [0, 0, -90],
"material": "wood"
},
{
"stlFile": "surfaces/BoardRail.stl",
"originOfModel": [0, 0, 0],
"rotationOfModelDegree": [0, 0, -90],
"material": "steel"
}
]
},
"Base": {
"parent": "Board",
"size": [150, 200, 150],
"mountPosition": [0, 0, 0],
"mountRotation": [0, 0, 0],
"jointToParent": {
"name": "Slider",
"type": "linear",
"axis": [1, 0, 0],
"origin": [0, 0, 16],
"rotation": [0, 0, 0],
"variable": "x"
},
"skeleton": {
"from": [0, 108, 45],
"to": [110, 108, 45],
"radius": 4,
"color": [0.20, 0.80, 0.20]
},
"markers": [
],
"model": [
{
"stlFile": "surfaces/Base.stl",
"originOfModel": [-30, 0, -35],
"rotationOfModelDegree": [0, 0, 0],
"material": "plaWhite"
}
]
},
"Arm1": {
"parent": "Base",
"size": [70, 250, 70],
"mountPosition": [0, 0, 0],
"mountRotation": [0, 0, 0],
"jointToParent": {
"name": "Joint1",
"type": "revolute",
"axis": [-1, 0, 0],
"origin": [110, 108, 45],
"rotation": [0, 0, 0],
"variable": "y"
},
"skeleton": {
"from": [0, 0, 0],
"to": [0, -250, 0],
"radius": 4,
"color": [0.20, 0.20, 0.90]
},
"markers": [
{
"id": 198,
"name": "aruco_198",
"position": [0, -160, 35],
"normal": [0, 0, 1],
"size": 25,
"spin": 0
},
{
"id": 229,
"name": "aruco_229",
"position": [0, -250, 35],
"normal": [0, 0, 1],
"size": 25,
"spin": 0
},
{
"id": 242,
"name": "aruco_242",
"position": [0, -250, -35],
"normal": [0, 0, -1],
"size": 25,
"spin": 0
},
{
"id": 243,
"name": "aruco_243",
"position": [0, -285, 0],
"normal": [0, -1, 0],
"size": 25,
"spin": 0
}
],
"model": [
{
"stlFile": "surfaces/Holm.stl",
"originOfModel__": [-25,29,-28.5],
"originOfModel": [-29,25,28.5],
"rotationOfModelDegree__": [0, 0, 0],
"rotationOfModelDegree": [180, 0, -90],
"material": "powderCoatBlue"
}
]
},
"Ellbow": {
"parent": "Arm1",
"mountPosition": [0, 0, 0],
"mountRotation": [0, 0, 0],
"jointToParent": {
"name": "Joint2",
"type": "revolute",
"axis": [-1, 0, 0],
"origin": [0, -250, 0],
"rotation": [0, 0, 0],
"variable": "z"
},
"skeleton": {
"from": [0, 0, 0],
"to": [90, 0, 0],
"radius": 4,
"color": [0.90, 0.20, 0.20]
},
"model": [
{
"stlFile": "surfaces/Ellebogen.stl",
"originOfModel": [90,0,0],
"rotationOfModelDegree": [0,-90,-90],
"material": "defaultPlastic"
}
],
"markers": [
{
"id": 244,
"name": "aruco_244",
"position": [125, 0, 0],
"normal": [1, 0, 0],
"size": 25,
"spin": 0
},
{
"id": 245,
"name": "aruco_245",
"position": [90, 0, -35],
"normal": [0, 0, -1],
"size": 25,
"spin": 0
},
{
"id": 246,
"name": "aruco_246",
"position": [90, 0, 35],
"normal": [0, 0, 1],
"size": 25
},
{
"id": 247,
"name": "aruco_247",
"position": [52.5, 0, 35],
"normal": [0, 0, 1],
"size": 25
}
]
},
"Arm2": {
"parent": "Ellbow",
"mountPosition": [0, 0, 0],
"mountRotation": [0, 0, 0],
"jointToParent": {
"name": "Joint3",
"type": "revolute",
"axis": [0, -1, 0],
"origin": [90, 0, 0],
"rotation": [0, 0, 0],
"variable": "a"
},
"skeleton": {
"from": [0, 0, 0],
"to": [0, -250, 0],
"radius": 4,
"color": [0.95, 0.85, 0.20]
},
"model": [
{
"stlFile": "surfaces/Unterarm.stl",
"originOfModel": [0,-250,0],
"rotationOfModelDegree": [180, 0, -90],
"material": "defaultPlastic"
}
],
"markers":[
{"id":120, "position":[24.75, -112, -24.75],"normal":[1,0,-1]},
{"id":122, "name": "aruco_122", "position":[-35,-112,0], "normal":[-1,0,0]},
{"id":218, "name": "aruco_218", "position":[35,-112,0], "normal":[1,0,0]},
{"id":113, "name": "aruco_113", "position":[0, -182, 30],"normal":[0,0,1]},
{"id":101, "name": "aruco_101", "position":[ 24.75, -182, -24.75],"normal":[ 1,0,-1]},
{"id":102, "name": "aruco_102", "position":[-24.75, -182, -24.75],"normal":[-1,0,-1]},
{"id":124, "name": "aruco_124", "position":[-35,-219,0], "normal":[-1,0,0]},
{"id":219, "name": "aruco_219", "position":[35,-219,0], "normal":[1,0,0]}
]
},
"Hand": {
"parent": "Arm2",
"mountPosition": [0, 0, 0],
"mountRotation": [0, 0, 0],
"jointToParent": {
"name": "Joint4",
"type": "revolute",
"axis": [1, 0, 0],
"origin": [0, -250, 0],
"rotation": [0, 0, 0],
"variable": "b"
},
"skeleton": {
"from": [0, 0, 0],
"to": [0, -35, 0],
"radius": 4,
"color": [0.95, 0.55, 0.15]
}
},
"Palm": {
"parent": "Hand",
"mountPosition": [0, 0, 0],
"mountRotation": [0, 0, 0],
"jointToParent": {
"name": "Joint3",
"type": "revolute",
"axis": [0, -1, 0],
"origin": [0, 0, 0],
"rotation": [0, 0, 0],
"variable": "c"
},
"skeleton": {
"from": [-50, -35, 0],
"to": [50, -35, 0],
"radius": 7,
"color": [0.95, 0.20, 0.20]
}
},
"FingerA": {
"parent": "Palm",
"size": [80, 60, 20],
"mountPosition": [0, 0, 0],
"mountRotation": [0, 0, 0],
"jointToParent": {
"name": "Slider",
"type": "linear",
"axis": [1, 0, 0],
"origin": [4, -35,0],
"rotation": [0, 0, 0],
"variable": "e"
},
"skeleton": {
"from": [0, 0,0],
"to": [0, -60, 0],
"radius": 4,
"color": [0.20, 0.80, 0.20]
},
"model": [
{
"stlFile": "surfaces/Finger.stl",
"originOfModel": [24,0,-9.1],
"rotationOfModelDegree": [90, -90,0],
"material": "defaultPlastic"
}
]
},
"FingerB": {
"parent": "Palm",
"size": [80, 60, 20],
"mountPosition": [0, 0, 0],
"mountRotation": [0, 0, 0],
"jointToParent": {
"name": "Slider",
"type": "linear",
"axis": [-1, 0, 0],
"origin": [-4, -35,0],
"rotation": [0, 0, 0],
"variable": "e"
},
"skeleton": {
"from": [0, 0,0],
"to": [0, -60, 0],
"radius": 4,
"color": [0.20, 0.80, 0.20]
},
"model": [
{
"stlFile": "surfaces/Finger.stl",
"originOfModel": [-24,0,9.1],
"rotationOfModelDegree": [90, 90,0],
"material": "defaultPlastic"
}
]
}
}
}

95
pipeline/SetGenerator/robot.urdf
View File

@@ -1,95 +0,0 @@
<?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">
<collision>
<origin xyz="0.035000 0.000000 0.000000" rpy="0.000000 1.570796 0.000000"/>
<geometry>
<cylinder radius="0.004000" length="0.070000"/>
</geometry>
</collision>
</link>
<link name="Arm2">
<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>
<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.070000 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>
</robot>

334
pipeline/SetGenerator/robot_commented.json
View File

@@ -1,334 +0,0 @@
{
"_comment": "Robot definition file for Blender/Robot/URDF-style kinematic rendering",
"coordinateSystem": {
"_comment": "Global coordinate system definition",
"system": "right-handed",
"axes": {
"x": "right",
"y": "backward",
"z": "up"
},
"_important": [
"This coordinate system is intentionally identical to Blender.",
"All positions are expressed in millimeters.",
"All rotations are expressed in degrees.",
"Positive rotations follow the right-hand rule."
]
},
"units": {
"length": "mm",
"rotation": "degree"
},
"renderingInfo": {
"_comment": "Pure rendering settings. Does NOT affect robot kinematics.",
"cameraPosition": [-400, -700, 300],
"cameraTarget": [0, 0, 150],
"_cameraTargetInfo": [
"cameraTarget defines the world-space point the camera looks at.",
"Values may also reference state variables like 'x'."
],
"cameraUpVector": [0, 0, 1],
"lightPosition": [-500, -500, 500],
"lightTarget": [0, 0, 0],
"lightUpVector": [0, 0, 1],
"metric": "mm",
"materials": {
"wood": {
"baseColor": [0.72, 0.52, 0.33],
"roughness": 0.85,
"metallic": 0.0
},
"plaWhite": {
"baseColor": [0.95, 0.95, 0.95],
"roughness": 0.45,
"metallic": 0.0
},
"steel": {
"baseColor": [0.72, 0.72, 0.75],
"roughness": 0.25,
"metallic": 1.0
},
"powderCoatBlue": {
"baseColor": [0.15, 0.25, 0.70],
"roughness": 0.55,
"metallic": 0.0
},
"defaultPlastic": {
"baseColor": [0.95, 0.95, 0.95],
"roughness": 0.40,
"metallic": 0.0
}
}
},
"defaultPosition": {
"_comment": "Robot zero/home position",
"_important": [
"These values define the robot's neutral pose.",
"All joints are evaluated relative to this pose."
],
"x": 0,
"y": 0,
"z": 0,
"a": 0,
"b": 0,
"c": 0,
"e": 0
},
"recognized": {
"_comment": "Pose reconstructed from machine vision / markers",
"x": null,
"y": null,
"z": null,
"a": null,
"b": null,
"c": null,
"e": null
},
"movements": {
"_comment": "Current commanded movement state (e.g. from GCode)",
"x": null,
"y": null,
"z": null,
"a": null,
"b": null,
"c": null,
"e": null
},
"_statePriority": [
"movements overrides recognized",
"recognized overrides defaultPosition",
"defaultPosition overrides zero"
],
"links": {
"Board": {
"_comment": "Static world/base object",
"parent": null,
"_mountPositionMeaning": [
"Position of THIS LINK coordinate system",
"relative to the PARENT LINK coordinate system"
],
"mountPosition": [0, 0, 0],
"_mountRotationMeaning": [
"Mechanical installation rotation",
"Defines how the LINK coordinate system",
"is rotated relative to its parent link",
"This is NOT the joint movement."
],
"mountRotation": [0, 0, 0],
"_modelMeaning": [
"One link may consist of multiple STL surfaces.",
"Each STL is positioned relative to the LINK coordinate system."
],
"model": [
{
"_comment": "Visual geometry only",
"stlFile": "surfaces/Board.stl",
"_originOfModelMeaning": [
"Position of STL inside the LINK coordinate system",
"Purely visual adjustment",
"Used to compensate CAD export offsets"
],
"originOfModel": [0, 0, 0],
"_rotationOfModelMeaning": [
"Rotation of STL inside LINK coordinate system",
"Purely visual adjustment",
"Used to compensate CAD export orientation"
],
"rotationOfModelDegree": [0, 0, 90],
"material": "wood"
}
]
},
"Base": {
"_comment": "Linear axis mounted on Board",
"parent": "Board",
"mountPosition": [0, 0, 25],
"mountRotation": [0, 0, 0],
"_jointToParentMeaning": [
"Defines the kinematic transformation",
"between parent link and this link"
],
"jointToParent": {
"name": "Slider",
"_jointTypeMeaning": [
"fixed = no movement",
"linear = translational movement",
"revolute = rotational movement"
],
"type": "linear",
"_axisMeaning": [
"Joint movement axis in LOCAL joint coordinates",
"[1,0,0] = local X axis",
"[0,1,0] = local Y axis",
"[0,0,1] = local Z axis"
],
"axis": [1, 0, 0],
"_originMeaning": [
"Position of joint coordinate system",
"inside the parent link coordinate system"
],
"origin": [0, 0, 0],
"_rotationMeaning": [
"Orientation of joint coordinate system",
"inside the parent link coordinate system"
],
"rotation": [0, 0, 0],
"_variableMeaning": [
"Maps robot state variable",
"to this joint"
],
"variable": "x"
},
"model": [
{
"stlFile": "surfaces/Base.stl",
"originOfModel": [0, 0, 0],
"rotationOfModelDegree": [0, 0, 0],
"material": "plaWhite"
}
]
},
"Arm1": {
"_comment": "Rotational arm",
"parent": "Base",
"mountPosition": [150, 0, 150],
"mountRotation": [0, 0, 0],
"jointToParent": {
"name": "Joint1",
"type": "revolute",
"_important": [
"Positive rotation follows right-hand rule."
],
"axis": [1, 0, 0],
"origin": [0, 0, 0],
"rotation": [0, 0, 0],
"variable": "a"
},
"model": [
{
"stlFile": "surfaces/Holm.stl",
"originOfModel": [0, 0, 0],
"rotationOfModelDegree": [0, 0, 0],
"material": "powderCoatBlue"
}
]
}
},
"_kinematicStructure": {
"_comment": "Conceptual hierarchy",
"hierarchy": [
"ParentLink",
"-> JointOrigin",
"-> JointMotion",
"-> ChildLink",
"-> VisualMeshes"
],
"_important": [
"All children automatically inherit parent movement.",
"This creates full forward kinematics automatically.",
"No manual propagation of child transformations is required."
]
},
"_futureExtensions": {
"_comment": "Planned future capabilities",
"possibleFeatures": [
"marker definitions",
"inverse kinematics",
"collision geometry",
"joint limits",
"joint damping",
"mass/inertia",
"vision calibration",
"urdf export",
"gcode import",
"trajectory playback"
]
}
}

1
pipeline/SetGenerator/run.bat
View File

@@ -1 +0,0 @@
"C:\Program Files\Blender Foundation\Blender 4.5\blender.exe" -b --python render_robot.py --log-level 2

104
pipeline/SetGenerator/runLoop.py
View File

@@ -1,104 +0,0 @@
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()

123
pipeline/precheck/check_constraints.py
View File

@@ -1,123 +0,0 @@
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
""")

105
pipeline/precheck/check_robot_marker_uniqueness.py
View File

@@ -1,105 +0,0 @@
#!/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()

246
pipeline/precheck/json_to_urdf.py
View File

@@ -1,246 +0,0 @@
# 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)

179
pipeline/precheck/robot.urdf
View File

@@ -1,179 +0,0 @@
<?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>