"""Scene Builder utilities for creating Blender objects from generic Robot models."""
from __future__ import annotations
import contextlib
import typing
from pathlib import Path
from linkforge.core import (
Box,
Color,
Cylinder,
Geometry,
Joint,
Link,
LinkPhysics,
Mesh,
Robot,
Sphere,
get_logger,
)
from linkforge.core.constants import (
GEOM_BOX,
GEOM_CYLINDER,
GEOM_MESH,
GEOM_SPHERE,
HW_IF_EFFORT,
HW_IF_POSITION,
HW_IF_VELOCITY,
JOINT_CONTINUOUS,
JOINT_FIXED,
JOINT_FLOATING,
JOINT_PLANAR,
JOINT_PRISMATIC,
JOINT_REVOLUTE,
SENSOR_CAMERA,
SENSOR_CONTACT,
SENSOR_DEPTH_CAMERA,
SENSOR_FORCE_TORQUE,
SENSOR_GPS,
SENSOR_GPU_LIDAR,
SENSOR_IMU,
SENSOR_LIDAR,
)
if typing.TYPE_CHECKING:
bpy: typing.Any
Matrix: typing.Any
if not typing.TYPE_CHECKING:
import bpy
from mathutils import Matrix
from ..constants import (
DEFAULT_JOINT_GIZMO_SIZE,
DEFAULT_LINK_GIZMO_SIZE,
PROP_LINK,
PROP_ROBOT,
PROP_SENSOR,
SUFFIX_COLLISION,
SUFFIX_VISUAL,
TAG_COLLISION_GEOM,
TAG_IMPORTED_SOURCE,
TAG_SOURCE_GEOM,
TAG_SOURCE_NAME,
)
from ..preferences import get_addon_prefs
from ..utils.joint_utils import resolve_mimic_joints
from ..utils.property_helpers import get_joint_props, get_link_props
from ..utils.scene_utils import move_to_collection, sync_object_collections
from .context import IBlenderContext
logger = get_logger(__name__)
[docs]
def create_material_from_color(
context: IBlenderContext, color: Color, name: str
) -> bpy.types.Material | None:
"""Create Blender material from Color model.
Args:
context: Blender context adapter
color: Color model
name: Material name
Returns:
Blender Material or None
"""
if name in context.data.materials:
return context.data.materials[name]
mat = context.data.materials.new(name=name)
mat.use_nodes = True
if mat.node_tree:
nodes = mat.node_tree.nodes
links = mat.node_tree.links
nodes.clear()
node_principled = nodes.new(type="ShaderNodeBsdfPrincipled")
node_output = nodes.new(type="ShaderNodeOutputMaterial")
if hasattr(node_principled.inputs[0], "default_value"):
node_principled.inputs[0].default_value = (color.r, color.g, color.b, color.a)
links.new(node_principled.outputs[0], node_output.inputs[0])
return mat
[docs]
def create_primitive_mesh(
context: IBlenderContext, geometry: Box | Cylinder | Sphere, name: str
) -> bpy.types.Object | None:
"""Create a Blender mesh object from primitive geometry.
This function generates native Blender mesh primitives (Cube, Cylinder,
Sphere) based on the Core geometry model and applies the correct
dimensions and format-specific metadata tags.
Args:
context: Blender context adapter
geometry: One of Box, Cylinder, or Sphere models.
name: Name to assign to the created Blender object.
Returns:
The created Blender Object or None if creation failed.
"""
# Deselect all first
context.ops.object.select_all(action="DESELECT")
obj = None
try:
if isinstance(geometry, Box):
context.ops.mesh.primitive_cube_add(location=(0, 0, 0))
obj = context.get_active_object()
if obj:
obj.name = name
obj.dimensions = (geometry.size.x, geometry.size.y, geometry.size.z)
# Force update to ensure dimensions are applied correctly before return
if context.view_layer is not None:
context.view_layer.update()
obj[TAG_SOURCE_GEOM] = GEOM_BOX
elif isinstance(geometry, Cylinder):
context.ops.mesh.primitive_cylinder_add(location=(0, 0, 0))
obj = context.get_active_object()
if obj:
obj.name = name
obj.dimensions = (geometry.radius * 2, geometry.radius * 2, geometry.length)
# Force update to ensure dimensions are applied correctly
if hasattr(context.scene, "update"):
context.scene.update()
obj[TAG_SOURCE_GEOM] = GEOM_CYLINDER
elif isinstance(geometry, Sphere):
context.ops.mesh.primitive_uv_sphere_add(location=(0, 0, 0))
obj = context.get_active_object()
if obj:
obj.name = name
obj.dimensions = (geometry.radius * 2, geometry.radius * 2, geometry.radius * 2)
if hasattr(context.scene, "update"):
context.scene.update()
obj[TAG_SOURCE_GEOM] = GEOM_SPHERE
else:
return None
except (AttributeError, TypeError, ValueError) as e:
logger.error(f"Failed to create primitive geometry: {e}")
return None
except Exception as e:
logger.critical(f"Unexpected error creating geometry: {e}", exc_info=True)
raise
return obj
[docs]
def import_mesh_file(
context: IBlenderContext, mesh_path: Path, name: str
) -> bpy.types.Object | None:
"""Import an external mesh file into the Blender scene.
Supported formats include STL, OBJ, and GLB. This function utilizes
modern Blender WM operators for improved performance and stability.
Args:
context: Blender context adapter
mesh_path: Absolute path to the mesh file.
name: Name to assign to the imported object.
Returns:
The imported Blender Object or None if import failed.
"""
if not mesh_path.exists():
logger.error(f"Mesh file not found: {mesh_path}")
return None
logger.info(f"Importing mesh: {mesh_path} as '{name}'")
# Deselect all
context.ops.object.select_all(action="DESELECT")
# Import based on file extension
ext = mesh_path.suffix.lower()
# Define operator candidates for each file extension
# We use modern WM operators (C++ based) for performance and stability
operators = {
".obj": ["wm.obj_import"],
".stl": ["wm.stl_import"],
".glb": ["wm.gltf_import", "import_scene.gltf"],
".gltf": ["wm.gltf_import", "import_scene.gltf"],
".dae": ["wm.collada_import"],
}
# Compatibility Notice: Blender 5.0.0+ removes native Collada (.dae) support.
# We enforce this check early to guide users toward modern formats (GLB/OBJ).
if ext == ".dae":
if bpy.app.version >= (5, 0, 0):
logger.error(
f"Collada (.dae) support was removed in Blender 5.0. "
f"Please convert '{mesh_path.name}' to .glb or .obj."
)
return None
logger.warning(
f"Collada (.dae) support is deprecated and will be removed in Blender 5.0. "
f"Consider converting '{mesh_path.name}' to a modern format like .glb."
)
if ext not in operators:
logger.warning(f"Unsupported mesh file extension: {ext} for '{mesh_path.name}'")
return None
# Snapshot objects and collections to identify stragglers afterward.
pre_import_objects = set(context.data.objects)
pre_import_collections = set(context.data.collections)
# Dispatcher: Try each operator until one succeeds
success = False
for op_name in operators[ext]:
try:
# Dynamically look up operator
op_parts = op_name.split(".")
op = context.ops
for part in op_parts:
op = getattr(op, part)
# Call the operator
callable_op = typing.cast(typing.Callable[..., typing.Any], op)
callable_op(filepath=str(mesh_path))
success = True
logger.debug(f"Successfully used importer: {op_name}")
break
except (AttributeError, RuntimeError) as e:
logger.debug(f"Importer '{op_name}' failed or not found: {e}")
continue
except Exception as e:
logger.warning(f"Importer '{op_name}' raised unexpected error: {e}")
continue
if not success:
logger.error(f"No functional {ext.upper()} importer found for '{mesh_path.name}'.")
return None
try:
# Consolidate ALL new objects created by the importer.
new_objects = set(context.data.objects) - pre_import_objects
res_obj = normalize_and_consolidate_imported_objects(context, new_objects, name)
if res_obj:
# CLEANUP: Handle unwanted collections created by certain importers (e.g. GLTF).
post_import_collections = set(context.data.collections)
new_collections = post_import_collections - pre_import_collections
if new_collections:
# Ensure result object is in current active collection
current_col = context.scene.collection
if current_col and res_obj.name not in current_col.objects:
current_col.objects.link(res_obj)
# Unlink from new collections and delete them
for new_col in new_collections:
# Unlink object if it's there
if res_obj.name in new_col.objects:
new_col.objects.unlink(res_obj)
# Remove the collection itself
context.data.collections.remove(new_col, do_unlink=True)
logger.debug(f"Cleaned up {len(new_collections)} import collections.")
return res_obj
return None
except (RuntimeError, OSError) as e:
logger.error(f"Failed to process imported mesh '{mesh_path.name}': {e}")
return None
except Exception as e:
logger.critical(f"Unexpected error processing mesh '{mesh_path.name}': {e}", exc_info=True)
raise
[docs]
def normalize_and_consolidate_imported_objects(
context: IBlenderContext, objects: typing.Iterable[bpy.types.Object], name: str
) -> bpy.types.Object | None:
"""Consolidation logic that processes all supplied objects (meshes → join, others → delete)."""
if not objects:
return None
mesh_objs = set()
to_delete = set()
seen = set()
def process_recursive(o: bpy.types.Object) -> None:
if o in seen:
return
seen.add(o)
# Unparent while keeping world transform
world_mat = o.matrix_world.copy()
o.parent = None
o.matrix_world = world_mat
if o.type == "MESH":
mesh_objs.add(o)
else:
to_delete.add(o)
for child in list(o.children):
process_recursive(child)
for obj in objects:
with contextlib.suppress(ReferenceError):
process_recursive(obj)
if not mesh_objs:
for obj in to_delete:
with contextlib.suppress(RuntimeError, ReferenceError):
context.data.objects.remove(obj, do_unlink=True)
return None
# Prepare for join
mesh_list = list(mesh_objs)
context.ops.object.select_all(action="DESELECT")
for obj in mesh_list:
obj.matrix_world = Matrix.Identity(4)
obj.select_set(True)
# In real Blender, we set the active object on the view layer.
vl = context.view_layer
if vl:
vl.objects.active = mesh_list[0]
# Bake axis/scale
context.ops.object.transform_apply(location=False, rotation=True, scale=True)
# Join
final_obj = mesh_list[0]
if len(mesh_list) > 1:
context.ops.object.join()
# Final cleanup
final_obj.name = name
final_obj.rotation_mode = "XYZ"
final_obj.location = (0, 0, 0)
final_obj.rotation_euler = (0, 0, 0)
final_obj.scale = (1, 1, 1)
# Remove all containers/stragglers
for obj in to_delete:
with contextlib.suppress(RuntimeError, ReferenceError):
context.data.objects.remove(obj, do_unlink=True)
vl = context.view_layer
if vl:
vl.objects.active = final_obj
return final_obj
def _get_geometry_type_str(geometry: Geometry | None) -> str:
"""Map core Geometry model to Blender geometry type string."""
if isinstance(geometry, Box):
return GEOM_BOX
if isinstance(geometry, Sphere):
return GEOM_SPHERE
if isinstance(geometry, Cylinder):
return GEOM_CYLINDER
if isinstance(geometry, Mesh):
return GEOM_MESH
return GEOM_MESH
[docs]
def create_link_object(
context: IBlenderContext,
link: Link,
robot: Robot,
source_directory: Path,
collection: bpy.types.Collection | None = None,
) -> bpy.types.Object | None:
"""Create Blender object from Link model with support for multiple visual/collision elements.
Args:
context: Blender context adapter
link: Link model
robot: Robot model (for resolving resources)
source_directory: Directory containing source file (for resolving relative paths)
collection: Blender Collection to add object to
Returns:
Blender Object or None (returns the link Empty object with properties)
"""
link_obj = context.data.objects.new(link.name, None)
link_obj.empty_display_type = "PLAIN_AXES"
link_obj.rotation_mode = "XYZ"
link_obj.location = (0, 0, 0)
prefs = get_addon_prefs()
link_obj.empty_display_size = prefs.link_empty_size if prefs else DEFAULT_LINK_GIZMO_SIZE
if collection:
move_to_collection(link_obj, collection)
if props := get_link_props(link_obj):
props.is_robot_link = True
props.source_name_stored = link.name
props.link_name = link.name
# Create all visual geometries (URDF allows multiple <visual> per link)
for idx, visual in enumerate(link.visuals):
visual_obj = None
# Determine visual object name (single vs multiple)
if len(link.visuals) == 1:
visual_name = f"{link.name}{SUFFIX_VISUAL}"
else:
# Use robot model name attribute if available, otherwise use index
suffix = visual.name if visual.name else str(idx)
visual_name = f"{link.name}{SUFFIX_VISUAL}_{suffix}"
if isinstance(visual.geometry, Mesh):
try:
mesh_path = robot.resolve_resource(
visual.geometry.resource, relative_to=source_directory
)
visual_obj = import_mesh_file(context, mesh_path, visual_name)
except FileNotFoundError as e:
logger.warning(f"Mesh not found for visual '{visual_name}': {e}")
visual_obj = None
if visual_obj and visual.geometry.scale:
scale = visual.geometry.scale
visual_obj.scale = (scale.x, scale.y, scale.z)
else:
visual_obj = create_primitive_mesh(context, visual.geometry, visual_name)
if visual_obj:
visual_obj.parent = link_obj
# Reset matrix_parent_inverse to ensure clean local transform
# Critical: Without this, visual geometry won't be centered at link frame
visual_obj.matrix_parent_inverse.identity()
# Apply visual origin transform (URDF visual offset)
if visual.origin:
origin = visual.origin
visual_obj.rotation_mode = "XYZ"
visual_obj.location = (origin.xyz.x, origin.xyz.y, origin.xyz.z)
visual_obj.rotation_euler = (origin.rpy.x, origin.rpy.y, origin.rpy.z)
else:
visual_obj.rotation_mode = "XYZ"
visual_obj.location = (0, 0, 0)
visual_obj.rotation_euler = (0, 0, 0)
if visual.name:
visual_obj[TAG_SOURCE_NAME] = visual.name
if collection:
move_to_collection(visual_obj, collection)
if visual.material and visual.material.color:
mat = create_material_from_color(
context, visual.material.color, visual.material.name
)
if mat and visual_obj.data and hasattr(visual_obj.data, "materials"):
# Use a localized ignore or cast if hasattr is not enough for MyPy's Mesh union
mesh_data = typing.cast(bpy.types.Mesh, visual_obj.data)
mesh_data.materials.clear()
mesh_data.materials.append(mat)
# Create all collision geometries (URDF allows multiple <collision> per link)
for idx, collision in enumerate(link.collisions):
collision_obj = None
# Determine collision object name (single vs multiple)
if len(link.collisions) == 1:
collision_name = f"{link.name}{SUFFIX_COLLISION}"
else:
# Use robot model name attribute if available, otherwise use index
suffix = collision.name if collision.name else str(idx)
collision_name = f"{link.name}{SUFFIX_COLLISION}_{suffix}"
if isinstance(collision.geometry, Mesh):
try:
mesh_path = robot.resolve_resource(
collision.geometry.resource, relative_to=source_directory
)
collision_obj = import_mesh_file(context, mesh_path, collision_name)
except FileNotFoundError as e:
logger.warning(f"Mesh not found for collision '{collision_name}': {e}")
collision_obj = None
if collision_obj and collision.geometry.scale:
scale = collision.geometry.scale
collision_obj.scale = (scale.x, scale.y, scale.z)
else:
collision_obj = create_primitive_mesh(context, collision.geometry, collision_name)
if collision_obj:
collision_obj.parent = link_obj
# Reset matrix_parent_inverse to ensure clean local transform
# Critical: Without this, collision geometry won't be centered at link frame
collision_obj.matrix_parent_inverse.identity()
# Apply collision origin transform
if collision.origin:
origin = collision.origin
collision_obj.rotation_mode = "XYZ"
collision_obj.location = (origin.xyz.x, origin.xyz.y, origin.xyz.z)
collision_obj.rotation_euler = (origin.rpy.x, origin.rpy.y, origin.rpy.z)
else:
collision_obj.rotation_mode = "XYZ"
collision_obj.location = (0, 0, 0)
collision_obj.rotation_euler = (0, 0, 0)
if collision.name:
collision_obj[TAG_SOURCE_NAME] = collision.name
# Mark as imported to prevent re-simplification on export.
# Without this, collision meshes degrade with each import-export cycle.
collision_obj[TAG_IMPORTED_SOURCE] = True
sync_object_collections(collision_obj, link_obj)
# Clear materials from collision mesh (collision doesn't need materials)
# Materials may come from imported mesh files (OBJ, DAE, etc.)
if collision_obj.data and hasattr(collision_obj.data, "materials"):
mesh_data = typing.cast(bpy.types.Mesh, collision_obj.data)
mesh_data.materials.clear()
collision_obj.display_type = "WIRE"
collision_obj.show_in_front = (
True # X-ray mode for consistency with generated collisions
)
collision_obj.hide_render = True
if isinstance(collision.geometry, Box):
collision_obj[TAG_COLLISION_GEOM] = GEOM_BOX
elif isinstance(collision.geometry, Cylinder):
collision_obj[TAG_COLLISION_GEOM] = GEOM_CYLINDER
elif isinstance(collision.geometry, Sphere):
collision_obj[TAG_COLLISION_GEOM] = GEOM_SPHERE
elif isinstance(collision.geometry, Mesh):
collision_obj[TAG_COLLISION_GEOM] = GEOM_MESH
if link.inertial and (props := get_link_props(link_obj)):
props.mass = link.inertial.mass
if link.inertial.inertia:
inertia = link.inertial.inertia
props.inertia_ixx = inertia.ixx
props.inertia_ixy = inertia.ixy
props.inertia_ixz = inertia.ixz
props.inertia_iyy = inertia.iyy
props.inertia_iyz = inertia.iyz
props.inertia_izz = inertia.izz
props.use_auto_inertia = False # Don't recalculate
# Store inertial origin
if link.inertial.origin:
origin = link.inertial.origin
props.inertia_origin_xyz = (origin.xyz.x, origin.xyz.y, origin.xyz.z)
props.inertia_origin_rpy = (origin.rpy.x, origin.rpy.y, origin.rpy.z)
if props := get_link_props(link_obj):
phys = link.physics
# Map all physics values (so they are ready if user enables the toggle later)
props.mu = phys.mu
props.mu2 = phys.mu2
props.kp = phys.kp
props.kd = phys.kd
props.self_collide = phys.self_collide
props.gravity = phys.gravity
# Only enable the "Advanced Simulation" toggle if physics are non-default
# or if there is an explicit Gazebo extension (plugins, etc.) for this link.
is_physics_modified = phys != LinkPhysics()
has_gazebo_element = any(ge.reference == link.name for ge in robot.gazebo_elements)
props.use_simulation_props = is_physics_modified or has_gazebo_element
# Final check for massless links if no inertial data was provided
if not link.inertial and (props := get_link_props(link_obj)):
# If link has no inertial data (e.g. a dummy root link), set mass to 0
# and disable auto-inertia to avoid falling back to the default 1.0 kg.
props.mass = 0.0
props.use_auto_inertia = False
if props := get_link_props(link_obj):
if link.collisions:
# Set collision quality to 100% for imported meshes (preserve original detail)
# This prevents degradation on re-export and gives user full control
props.collision_quality = 100.0 # 100% preservation for imported collision
# Set collision_type based on imported geometry (for UI display)
# This ensures the UI shows the correct type instead of defaulting to "Auto"
collision_geom_type = _get_geometry_type_str(link.collisions[0].geometry)
if collision_geom_type in (GEOM_BOX, GEOM_CYLINDER, GEOM_SPHERE):
props.collision_type = collision_geom_type
elif collision_geom_type == GEOM_MESH:
# For mesh collisions, default to MESH (Simplified)
# (most imported mesh collisions are simplified meshes)
props.collision_type = GEOM_MESH
# Enable material export if imported URDF has material
if link.visuals and link.visuals[0].material:
props.use_material = True
# Material name will come from Blender material assigned to visual child
return link_obj
[docs]
def create_joint_object(
context: IBlenderContext,
joint: Joint,
link_objects: dict[str, bpy.types.Object],
collection: bpy.types.Collection | None = None,
) -> bpy.types.Object | None:
"""Create Empty object from Joint model.
Args:
context: Blender context adapter
joint: Joint model
link_objects: Dictionary mapping link names to Blender objects
collection: Blender Collection to add object to
Returns:
Blender Empty object or None
"""
prefs = get_addon_prefs()
empty_size = (
getattr(prefs, "joint_empty_size", DEFAULT_JOINT_GIZMO_SIZE)
if prefs
else DEFAULT_JOINT_GIZMO_SIZE
)
empty = context.data.objects.new(joint.name, None)
empty.empty_display_type = "ARROWS"
empty.empty_display_size = empty_size
empty.rotation_mode = "XYZ"
empty.location = (0, 0, 0)
if collection:
if isinstance(collection, bpy.types.Collection):
collection.objects.link(empty)
else:
# It's an object, sync to its collections
sync_object_collections(empty, collection)
elif context.scene and context.scene.collection:
context.scene.collection.objects.link(empty)
if props := get_joint_props(empty):
props.is_robot_joint = True
props.source_name_stored = joint.name
props.joint_name = joint.name
type_map = {
JOINT_REVOLUTE: JOINT_REVOLUTE,
JOINT_CONTINUOUS: JOINT_CONTINUOUS,
JOINT_PRISMATIC: JOINT_PRISMATIC,
JOINT_FIXED: JOINT_FIXED,
JOINT_FLOATING: JOINT_FLOATING,
JOINT_PLANAR: JOINT_PLANAR,
}
props.joint_type = type_map.get(joint.type.value, JOINT_FIXED)
# Set parent and child links (PointerProperty expects Blender objects)
props.parent_link = link_objects.get(joint.parent)
props.child_link = link_objects.get(joint.child)
if joint.axis:
# Check if it's a standard axis (X, Y, or Z)
if joint.axis.x == 1.0 and joint.axis.y == 0.0 and joint.axis.z == 0.0:
props.axis = "X"
elif joint.axis.x == 0.0 and joint.axis.y == 1.0 and joint.axis.z == 0.0:
props.axis = "Y"
elif joint.axis.x == 0.0 and joint.axis.y == 0.0 and joint.axis.z == 1.0:
props.axis = "Z"
else:
# Custom axis
props.axis = "CUSTOM"
props.custom_axis_x = joint.axis.x
props.custom_axis_y = joint.axis.y
props.custom_axis_z = joint.axis.z
if joint.limits:
props.use_limits = True
# Continuous joints may have None for lower/upper (no position limits)
# Blender properties require float values, so use defaults for None
props.limit_lower = joint.limits.lower if joint.limits.lower is not None else 0.0
props.limit_upper = joint.limits.upper if joint.limits.upper is not None else 0.0
props.limit_effort = joint.limits.effort
props.limit_velocity = joint.limits.velocity
if joint.dynamics:
props.use_dynamics = True
props.dynamics_damping = joint.dynamics.damping
props.dynamics_friction = joint.dynamics.friction
if joint.mimic:
props.use_mimic = True
# Mimic joint pointer will be resolved in a second pass in import_robot_to_scene
# because the mimicked joint might not be created yet.
props.mimic_multiplier = joint.mimic.multiplier
props.mimic_offset = joint.mimic.offset
if joint.safety_controller:
props.use_safety_controller = True
props.safety_soft_lower_limit = joint.safety_controller.soft_lower_limit
props.safety_soft_upper_limit = joint.safety_controller.soft_upper_limit
props.safety_k_position = joint.safety_controller.k_position
props.safety_k_velocity = joint.safety_controller.k_velocity
if joint.calibration:
props.use_calibration = True
if joint.calibration.rising is not None:
props.use_calibration_rising = True
props.calibration_rising = joint.calibration.rising
if joint.calibration.falling is not None:
props.use_calibration_falling = True
props.calibration_falling = joint.calibration.falling
parent_obj = link_objects.get(joint.parent)
child_obj = link_objects.get(joint.child)
if parent_obj and child_obj:
# Parent the joint Empty to the parent link
empty.parent = parent_obj
# Reset matrix_parent_inverse to ensure clean local transform
empty.matrix_parent_inverse.identity()
# Apply joint origin transform (offset from parent link frame)
if joint.origin:
origin = joint.origin
empty.rotation_mode = "XYZ"
empty.location = (origin.xyz.x, origin.xyz.y, origin.xyz.z)
empty.rotation_euler = (origin.rpy.x, origin.rpy.y, origin.rpy.z)
# Parent the child link to the joint Empty
child_obj.parent = empty
child_obj.matrix_parent_inverse.identity()
# Force update of parent's world matrix
_ = empty.matrix_world
child_obj.rotation_mode = "XYZ"
child_obj.location = (0, 0, 0)
child_obj.rotation_euler = (0, 0, 0)
else:
logger.error(
f"Failed to parent joint '{joint.name}': "
f"Parent link '{joint.parent}' or child link '{joint.child}' not found in internal map. "
f"Existing links: {list(link_objects.keys())}"
)
if collection:
move_to_collection(empty, collection)
# Joint visibility is controlled by RGB axes (GPU overlay) and empty display size
# Empties are always visible in viewport, hide from render only
empty.hide_render = True
return empty
[docs]
def create_sensor_object(
context: IBlenderContext,
sensor: typing.Any,
link_objects: dict[str, bpy.types.Object],
collection: bpy.types.Collection | None = None,
) -> bpy.types.Object | None:
"""Create Empty object from Sensor model.
Args:
context: Blender context adapter
sensor: Sensor model from core
link_objects: Dictionary mapping link names to Blender objects
collection: Blender Collection to add object to
Returns:
Blender Empty object or None
"""
# Verify attached link exists
if sensor.link_name not in link_objects:
logger.warning(f"Sensor '{sensor.name}' attached to unknown link '{sensor.link_name}'")
return None
empty = context.data.objects.new(sensor.name, None)
empty.empty_display_type = "SPHERE"
prefs = get_addon_prefs()
empty.empty_display_size = prefs.sensor_empty_size if prefs else 0.1
if hasattr(empty, PROP_SENSOR):
props = getattr(empty, PROP_SENSOR)
props.is_robot_sensor = True
props.sensor_name = sensor.name
type_map = {
SENSOR_CAMERA: SENSOR_CAMERA,
SENSOR_DEPTH_CAMERA: SENSOR_DEPTH_CAMERA,
SENSOR_LIDAR: SENSOR_LIDAR,
SENSOR_GPU_LIDAR: SENSOR_GPU_LIDAR,
SENSOR_IMU: SENSOR_IMU,
SENSOR_GPS: SENSOR_GPS,
SENSOR_CONTACT: SENSOR_CONTACT,
SENSOR_FORCE_TORQUE: SENSOR_FORCE_TORQUE,
}
props.sensor_type = type_map.get(sensor.type.value, SENSOR_CAMERA)
# Set attached link (PointerProperty expects Blender object)
props.attached_link = link_objects.get(sensor.link_name)
# Common properties
if sensor.update_rate:
props.update_rate = sensor.update_rate
if sensor.topic:
props.topic_name = sensor.topic
# Camera properties
if sensor.camera_info:
cam = sensor.camera_info
props.camera_horizontal_fov = cam.horizontal_fov
props.camera_width = cam.width
props.camera_height = cam.height
if cam.format:
props.camera_format = cam.format
if cam.near_clip:
props.camera_near_clip = cam.near_clip
if cam.far_clip:
props.camera_far_clip = cam.far_clip
# LIDAR properties
if sensor.lidar_info:
lidar = sensor.lidar_info
props.lidar_horizontal_samples = lidar.horizontal_samples
props.lidar_horizontal_min_angle = lidar.horizontal_min_angle
props.lidar_horizontal_max_angle = lidar.horizontal_max_angle
if lidar.vertical_samples:
props.lidar_vertical_samples = lidar.vertical_samples
props.lidar_range_min = lidar.range_min
props.lidar_range_max = lidar.range_max
# LIDAR noise
if lidar.noise:
props.use_noise = True
props.noise_type = lidar.noise.type
props.noise_mean = lidar.noise.mean
props.noise_stddev = lidar.noise.stddev
# IMU properties
if sensor.imu_info:
imu = sensor.imu_info
# Gravity magnitude is handled by World settings
# IMU noise (from angular velocity or linear acceleration)
if imu.angular_velocity_noise:
props.use_noise = True
props.noise_type = imu.angular_velocity_noise.type
props.noise_mean = imu.angular_velocity_noise.mean
props.noise_stddev = imu.angular_velocity_noise.stddev
# GPS properties (GPS info mainly contains noise)
if sensor.gps_info:
gps = sensor.gps_info
if gps.position_sensing_horizontal_noise:
props.use_noise = True
props.noise_type = gps.position_sensing_horizontal_noise.type
props.noise_mean = gps.position_sensing_horizontal_noise.mean
props.noise_stddev = gps.position_sensing_horizontal_noise.stddev
# Gazebo plugin
if sensor.plugin:
props.use_gazebo_plugin = True
props.plugin_filename = sensor.plugin.filename
# Store raw XML for round-trip fidelity
if sensor.plugin.raw_xml:
props.plugin_raw_xml = sensor.plugin.raw_xml
link_obj = link_objects[sensor.link_name]
empty.parent = link_obj
# Reset matrix_parent_inverse to ensure clean local transform
empty.matrix_parent_inverse.identity()
if sensor.origin:
origin = sensor.origin
empty.rotation_mode = "XYZ"
empty.location = (origin.xyz.x, origin.xyz.y, origin.xyz.z)
empty.rotation_euler = (origin.rpy.x, origin.rpy.y, origin.rpy.z)
if collection:
move_to_collection(empty, collection)
empty.hide_render = True
return empty
[docs]
def setup_scene_for_robot(context: IBlenderContext, robot: Robot) -> None:
"""Initialize scene properties for a robot model.
This populates the Centralized Control Dashboard, Gazebo settings,
and metadata based on the robot model.
Args:
context: Blender context adapter
robot: Robot model to extract settings from
"""
scene = context.scene
if hasattr(scene, PROP_ROBOT):
getattr(scene, PROP_ROBOT).robot_name = robot.name
if robot.ros2_controls:
lp = getattr(scene, PROP_ROBOT)
lp.use_ros2_control = True
control = robot.ros2_controls[0]
lp.ros2_control_name = control.name
lp.ros2_control_type = control.type
lp.hardware_plugin = control.hardware_plugin
lp.ros2_control_parameters.clear()
for key, value in control.parameters.items():
param_item = lp.ros2_control_parameters.add()
param_item.name = key
param_item.value = value
lp.ros2_control_joints.clear()
for rc_joint in control.joints:
item = lp.ros2_control_joints.add()
item.name = rc_joint.name
item.cmd_position = HW_IF_POSITION in rc_joint.command_interfaces
item.cmd_velocity = HW_IF_VELOCITY in rc_joint.command_interfaces
item.cmd_effort = HW_IF_EFFORT in rc_joint.command_interfaces
item.state_position = HW_IF_POSITION in rc_joint.state_interfaces
item.state_velocity = HW_IF_VELOCITY in rc_joint.state_interfaces
item.state_effort = HW_IF_EFFORT in rc_joint.state_interfaces
item.parameters.clear()
for key, value in rc_joint.parameters.items():
param_item = item.parameters.add()
param_item.name = key
param_item.value = value
# Master reset of ROS2 Control / Gazebo state if not present in robot
if not robot.ros2_controls and hasattr(scene, PROP_ROBOT):
lp = getattr(scene, PROP_ROBOT)
lp.use_ros2_control = False
lp.ros2_control_joints.clear()
lp.ros2_control_parameters.clear()
lp.gazebo_plugin_name = "gz_ros2_control::GazeboSimROS2ControlPlugin" # Default
lp.controllers_yaml_path = ""
# Map Gazebo simulation settings if present
if robot.gazebo_elements and hasattr(scene, PROP_ROBOT):
plugin_found = False
for elem in robot.gazebo_elements:
for plugin in elem.plugins:
if "ros2_control" in plugin.name.lower():
lp_tmp = getattr(scene, PROP_ROBOT)
lp_tmp.gazebo_plugin_name = plugin.name
if "parameters" in plugin.parameters:
lp_tmp.controllers_yaml_path = plugin.parameters["parameters"]
plugin_found = True
break
if plugin_found:
break
[docs]
def import_robot_to_scene(
robot: Robot, source_path: Path, context: IBlenderContext | bpy.types.Context
) -> bool:
"""Import Robot model to Blender scene.
Args:
robot: Robot model
source_path: Path to source file
context: Blender context (real or adapter)
"""
from .context import BlenderContext
# Auto-wrap raw context if passed directly
if not isinstance(context, IBlenderContext):
context = BlenderContext(context)
if context.scene:
setup_scene_for_robot(context, robot)
collection = context.data.collections.new(robot.name)
if context.scene:
context.scene.collection.children.link(collection)
source_directory = source_path.parent
sensor_count = len(robot.sensors) if hasattr(robot, "sensors") else 0
link_objects = {}
parts = [f"{len(robot.links)} links", f"{len(robot.joints)} joints"]
if sensor_count > 0:
parts.append(f"{sensor_count} sensors")
logger.info(f"Importing robot '{robot.name}' ({', '.join(parts)})")
for link in robot.links:
obj = create_link_object(context, link, robot, source_directory, collection)
if obj:
link_objects[link.name] = obj
sorted_joints = robot.graph.get_topological_joints()
joint_objects = {}
for joint in sorted_joints:
joint_obj = create_joint_object(context, joint, link_objects, collection)
if joint_obj:
joint_objects[joint.name] = joint_obj
# Second pass: Resolve mimic joint pointers
# Convert tuple to list for MyPy compatibility
all_joints_list = list(robot.joints)
resolve_mimic_joints(all_joints_list, joint_objects)
sensors_created = 0
if hasattr(robot, "sensors") and robot.sensors:
for sensor in robot.sensors:
if create_sensor_object(context, sensor, link_objects, collection):
sensors_created += 1
# Update scene to ensure all transforms are calculated correctly
# This is critical for round-trip: ensures child link locations are properly evaluated
if context.view_layer is not None:
context.view_layer.update()
completion_parts = [
f"{len(link_objects)}/{len(robot.links)} links",
f"{len(joint_objects)}/{len(robot.joints)} joints",
]
if sensor_count > 0:
completion_parts.append(f"{sensors_created}/{sensor_count} sensors")
logger.info(f"Import complete - {', '.join(completion_parts)} created")
# Sync collision visibility with scene property
# This ensures that if 'Show Collisions' is off (default), the newly imported collision meshes are hidden
if (
context.scene
and hasattr(context.scene, PROP_LINK)
and hasattr(getattr(context.scene, PROP_LINK), "update_collision_visibility")
):
# We need to pass the context or property group self. Since update method expects self,
# we can call the function directly or trigger property update.
# Calling the update function bound to the property group instance is safest.
getattr(context.scene, PROP_LINK).update_collision_visibility(context)
return True