1. はじめに
ロボット以外のものもROSやROS2で動かせないかと考えていましたところ、Automating the Tower Crane: Integrating the Development and Simulation of Path Planning and Trajectory Tracking of Tower Crane in ROS Framework"という論文でROSでタワークレーンを動かそうとしている事例がありました。
この論文では以下のgithub👇にデモの動画もあります。
これを再現したいと思ったのですがOpenRoboticsの tower_crane/model.sdf (リンク)は linkが1つ＋jointなし＋static=true なので、制御も物理回転もできない “置物” です。そこで、このモデルを回転できるようBlender APIを使って構造を変更し、実際に回転させてみたのがこの記事の内容になります。
2. 実行環境
- CPU: CORE i7 7th Gen
- メモリ: 32GB
- GPU: GeForce RTX 2070
- OS: Ubuntu22.04(WSL2ではなくPCに直接インストール)
3. 準備
3.1 Blenderのバージョンの確認
Blender 5.0以降は Collada(.dae) Import/Exportが削除されています。
この記事では Blender 4.x（4.5 LTSなど） を使いました
なお、以下のようにblenderをインストールしました・
# 1. Blender 4.5 LTS のバイナリをダウンロード (Linux 64-bit)
wget [https://download.blender.org/release/Blender4.5/blender-4.5.0-linux-x64.tar.xz](https://download.blender.org/release/Blender4.5/blender-4.5.0-linux-x64.tar.xz)
# 2. 展開して /opt ディレクトリへ配置
tar -xf blender-4.5.0-linux-x64.tar.xz
sudo mv blender-4.5.0-linux-x64 /opt/blender4.5
# 3. ターミナルから "blender" で呼び出せるようにパスを通す
sudo ln -sf /opt/blender4.5/blender /usr/local/bin/blender
確認：
blender -b --python-expr "import bpy; print('collada_import' in dir(bpy.ops.wm), 'collada_export' in dir(bpy.ops.wm))"
True True ならOK。
3.2. 素材（tower_crane.dae）の準備
mkdir -p ~/tower_crane_ws && cd ~/tower_crane_ws
git clone https://github.com/osrf/gazebo_models.git
cp gazebo_models/tower_crane/meshes/tower_crane.dae .
4. 解析：DAEを“分割できるか”確認し、分割の基準を作る
ここは Blender不要・Python標準だけでやります。
4.1. 解析スクリプト配置
analyze_dae_stdlib.py を ~/tower_crane_ws/ に置く
（あなたが既に作って動かしたものをそのまま使う）
analyze_dae_stdlib.py
#!/usr/bin/env python3
"""
analyze_dae_stdlib.py
- Pure stdlib Collada(.dae) analyzer for:
* global bbox (min/max/center/height)
* connected components estimate by vertex connectivity (triangles/polylist)
* heuristic z_cut and pivot for yaw joint
* components.csv (bbox per component)
Outputs:
out/analysis.json
out/components.csv
"""
import argparse
import csv
import json
import math
import os
import sys
import xml.etree.ElementTree as ET
from collections import defaultdict
# ---------- Union-Find ----------
class UnionFind:
__slots__ = ("p", "r")
def __init__(self, n: int):
self.p = list(range(n))
self.r = [0]*n
def find(self, a: int) -> int:
p = self.p
while p[a] != a:
p[a] = p[p[a]]
a = p[a]
return a
def union(self, a: int, b: int) -> None:
pa = self.find(a); pb = self.find(b)
if pa == pb:
return
ra = self.r[pa]; rb = self.r[pb]
if ra < rb:
self.p[pa] = pb
elif ra > rb:
self.p[pb] = pa
else:
self.p[pb] = pa
self.r[pa] += 1
# ---------- helpers ----------
def _namespace(tag: str) -> str:
# "{ns}COLLADA" -> "ns"
if tag.startswith("{") and "}" in tag:
return tag[1:tag.index("}")]
return ""
def q(ns: str, name: str) -> str:
return f"{{{ns}}}{name}" if ns else name
def parse_floats(text: str):
return [float(x) for x in text.strip().split() if x]
def parse_ints(text: str):
return [int(x) for x in text.strip().split() if x]
def percentile_sorted(vals_sorted, q01: float):
if not vals_sorted:
return None
n = len(vals_sorted)
idx = int(q01 * (n - 1))
return vals_sorted[idx]
def clamp(x, lo, hi):
return lo if x < lo else hi if x > hi else x
# ---------- Collada parsing ----------
def collect_positions_and_primitives(root: ET.Element):
ns = _namespace(root.tag)
geoms = root.findall(f".//{q(ns,'library_geometries')}/{q(ns,'geometry')}")
if not geoms:
raise RuntimeError("No <geometry> found in DAE")
global_positions = [] # flat list [x,y,z,x,y,z,...]
primitives = [] # list of (base_offset, local_vertex_count, triangles_vertex_indices_global)
# Map to parse each geometry mesh
for geom in geoms:
mesh = geom.find(q(ns, "mesh"))
if mesh is None:
continue
# sources: id -> float_array (for POSITION)
source_floats = {}
for src in mesh.findall(q(ns, "source")):
sid = src.get("id")
fa = src.find(q(ns, "float_array"))
if sid and fa is not None and fa.text:
source_floats[sid] = parse_floats(fa.text)
# vertices: vertices_id -> positions_source_id
vertices = {}
for vtx in mesh.findall(q(ns, "vertices")):
vid = vtx.get("id")
inp = vtx.find(q(ns, "input"))
if vid and inp is not None:
sem = inp.get("semantic")
src = inp.get("source") # "#positions"
if sem == "POSITION" and src and src.startswith("#"):
vertices[vid] = src[1:]
# Determine which positions array is used:
# Many files have only one vertices element. We'll use the first one.
if not vertices:
# fallback: try to find any source named "*positions*"
pos_sid = None
for sid in source_floats.keys():
if "positions" in sid.lower():
pos_sid = sid
break
if pos_sid is None:
continue
else:
# pick first
first_vertices_id = next(iter(vertices.keys()))
pos_sid = vertices[first_vertices_id]
if pos_sid not in source_floats:
continue
pos = source_floats[pos_sid] # flat
if len(pos) % 3 != 0:
continue
local_n = len(pos) // 3
base_offset = len(global_positions) // 3
global_positions.extend(pos)
# Helper to extract VERTEX offset in a primitive
def parse_primitive(elem, kind: str):
# inputs
inputs = elem.findall(q(ns, "input"))
max_off = -1
vtx_off = None
for inp in inputs:
off = int(inp.get("offset", "0"))
sem = inp.get("semantic")
src = inp.get("source", "")
max_off = max(max_off, off)
if sem == "VERTEX":
vtx_off = off
if vtx_off is None:
return []
stride = max_off + 1
p_elem = elem.find(q(ns, "p"))
if p_elem is None or not p_elem.text:
return []
p = parse_ints(p_elem.text)
tris = []
if kind == "triangles":
# count attribute is number of triangles
# each triangle has 3 vertices -> 3*stride ints
tri_count = int(elem.get("count", "0"))
needed = tri_count * 3 * stride
if len(p) < needed:
# still try to parse by length
tri_count = len(p) // (3 * stride)
for t in range(tri_count):
base = t * 3 * stride
v0 = p[base + 0*stride + vtx_off]
v1 = p[base + 1*stride + vtx_off]
v2 = p[base + 2*stride + vtx_off]
tris.append((base_offset + v0, base_offset + v1, base_offset + v2))
return tris
if kind == "polylist":
vcount_elem = elem.find(q(ns, "vcount"))
if vcount_elem is None or not vcount_elem.text:
return []
vcount = parse_ints(vcount_elem.text)
idx = 0
for nverts in vcount:
# polygon has nverts corners -> nverts*stride ints
corners = []
for i in range(nverts):
vi = p[idx + i*stride + vtx_off]
corners.append(base_offset + vi)
idx += nverts * stride
# triangulate fan (0, j, j+1)
if len(corners) >= 3:
v0 = corners[0]
for j in range(1, len(corners)-1):
tris.append((v0, corners[j], corners[j+1]))
return tris
return []
# parse triangles and polylist
tris_all = []
for tri in mesh.findall(q(ns, "triangles")):
tris_all.extend(parse_primitive(tri, "triangles"))
for pl in mesh.findall(q(ns, "polylist")):
tris_all.extend(parse_primitive(pl, "polylist"))
# store primitive set if any indices parsed
if tris_all:
primitives.append((base_offset, local_n, tris_all))
if not global_positions:
raise RuntimeError("No POSITION float_array found/parsed")
return global_positions, primitives
def compute_bbox(global_positions):
# global_positions flat
it = iter(global_positions)
x0 = next(it); y0 = next(it); z0 = next(it)
mnx = mxx = x0
mny = mxy = y0
mnz = mxz = z0
for x, y, z in zip(it, it, it):
if x < mnx: mnx = x
if x > mxx: mxx = x
if y < mny: mny = y
if y > mxy: mxy = y
if z < mnz: mnz = z
if z > mxz: mxz = z
return (mnx, mny, mnz), (mxx, mxy, mxz)
def estimate_z_cut_and_pivot(mn, mx, global_positions, far_ratio=0.35, margin_ratio=0.05):
mnx, mny, mnz = mn
mxx, mxy, mxz = mx
cx = (mnx + mxx) / 2.0
cy = (mny + mxy) / 2.0
cz = (mnz + mxz) / 2.0
H = max(1e-9, (mxz - mnz))
LX = max(1e-9, (mxx - mnx))
LY = max(1e-9, (mxy - mny))
major = "Y" if LY >= LX else "X"
span_major = LY if major == "Y" else LX
c_major = cy if major == "Y" else cx
far_th = far_ratio * span_major
z_far = []
z_all = []
# iterate vertices
pos = global_positions
for i in range(0, len(pos), 3):
x = pos[i]; y = pos[i+1]; z = pos[i+2]
z_all.append(z)
m = y if major == "Y" else x
if abs(m - c_major) > far_th:
z_far.append(z)
z_all.sort()
z_far.sort()
if z_far:
z_jib_bottom = percentile_sorted(z_far, 0.05)
else:
z_jib_bottom = percentile_sorted(z_all, 0.75)
z_cut = z_jib_bottom - margin_ratio * H
z_cut = clamp(z_cut, mnz + 0.20*H, mnz + 0.95*H)
pivot = [cx, cy, z_cut]
return {
"bbox_min": [mnx, mny, mnz],
"bbox_max": [mxx, mxy, mxz],
"center": [cx, cy, cz],
"height": H,
"span_x": LX,
"span_y": LY,
"major_axis": major,
"far_threshold": far_th,
"z_jib_bottom_guess": z_jib_bottom,
"z_cut": z_cut,
"pivot": pivot,
"axis": [0.0, 0.0, 1.0],
}
def main():
ap = argparse.ArgumentParser()
ap.add_argument("--input", required=True, help="tower_crane.dae path")
ap.add_argument("--outdir", required=True, help="output directory")
ap.add_argument("--far_ratio", type=float, default=0.35)
ap.add_argument("--margin_ratio", type=float, default=0.05)
ap.add_argument("--core_ratio", type=float, default=0.08) # mast detection in Blender stage
ap.add_argument("--mast_dz_ratio", type=float, default=0.45) # mast detection in Blender stage
args = ap.parse_args()
inp = os.path.abspath(os.path.expanduser(args.input))
outdir = os.path.abspath(os.path.expanduser(args.outdir))
os.makedirs(outdir, exist_ok=True)
root = ET.parse(inp).getroot()
global_positions, primitives = collect_positions_and_primitives(root)
mn, mx = compute_bbox(global_positions)
est = estimate_z_cut_and_pivot(mn, mx, global_positions, args.far_ratio, args.margin_ratio)
# connected components by vertex connectivity (if primitives available)
n_verts = len(global_positions) // 3
tri_count = 0
cc_count = None
component_stats = [] # will be filled if we can compute cc
if primitives:
uf = UnionFind(n_verts)
for _, _, tris in primitives:
for a, b, c in tris:
uf.union(a, b)
uf.union(a, c)
tri_count += 1
# aggregate bbox per component
comp_min = {}
comp_max = {}
comp_n = defaultdict(int)
pos = global_positions
for vi in range(n_verts):
r = uf.find(vi)
x = pos[3*vi]; y = pos[3*vi+1]; z = pos[3*vi+2]
comp_n[r] += 1
if r not in comp_min:
comp_min[r] = [x, y, z]
comp_max[r] = [x, y, z]
else:
mnv = comp_min[r]; mxv = comp_max[r]
if x < mnv[0]: mnv[0] = x
if y < mnv[1]: mnv[1] = y
if z < mnv[2]: mnv[2] = z
if x > mxv[0]: mxv[0] = x
if y > mxv[1]: mxv[1] = y
if z > mxv[2]: mxv[2] = z
roots = list(comp_n.keys())
cc_count = len(roots)
# build component rows
for rid in roots:
mnv = comp_min[rid]; mxv = comp_max[rid]
dx = mxv[0]-mnv[0]; dy = mxv[1]-mnv[1]; dz = mxv[2]-mnv[2]
cx = (mnv[0]+mxv[0])/2; cy = (mnv[1]+mxv[1])/2; cz = (mnv[2]+mxv[2])/2
component_stats.append({
"root": rid,
"n_verts": comp_n[rid],
"min_x": mnv[0], "min_y": mnv[1], "min_z": mnv[2],
"max_x": mxv[0], "max_y": mxv[1], "max_z": mxv[2],
"cx": cx, "cy": cy, "cz": cz,
"dx": dx, "dy": dy, "dz": dz,
"bbox_vol": dx*dy*dz
})
component_stats.sort(key=lambda r: r["bbox_vol"], reverse=True)
# write components.csv
csv_path = os.path.join(outdir, "components.csv")
with open(csv_path, "w", newline="", encoding="utf-8") as f:
w = csv.DictWriter(f, fieldnames=list(component_stats[0].keys()))
w.writeheader()
w.writerows(component_stats)
# analysis.json
est.update({
"input": inp,
"n_vertices": n_verts,
"n_triangles_parsed": tri_count,
"connected_components": cc_count,
"suggested_method": "loose_grouping" if (cc_count is not None and cc_count > 1) else "bisect_cut",
"core_ratio": args.core_ratio,
"mast_dz_ratio": args.mast_dz_ratio,
"note": "connected_components is estimated via vertex connectivity from triangles/polylist. If None, indices were not parsed.",
})
with open(os.path.join(outdir, "analysis.json"), "w", encoding="utf-8") as f:
json.dump(est, f, indent=2)
print("=== analysis summary ===")
print("input:", inp)
print("n_vertices:", n_verts)
print("n_triangles_parsed:", tri_count)
print("connected_components:", cc_count)
print("suggested_method:", est["suggested_method"])
print("z_cut:", est["z_cut"])
print("pivot:", est["pivot"])
print("outdir:", outdir)
if cc_count is None:
print("WARNING")
if __name__ == "__main__":
main()
4.2. 実行
mkdir -p out
python3 analyze_dae_stdlib.py --input tower_crane.dae --outdir out
出力：
out/analysis.jsonout/components.csv
ここで見るべきポイント
-
connected_components（連結成分数）が 1より大きいなら
→ 「一体メッシュに見えても実は多数部品の集合」
→ BlenderのSeparate by Loose Partsで分割できる可能性が高い - 解析では
z_cut（上下分割の高さ）とpivot（回転中心）も推定している
ただしpivotは最初 bbox中心なので、ジブの偏りでズレることが多い
5. pivot補正：回転中心（XY）をマスト中心に寄せる
5.1. pivot補正スクリプト作成
fix_pivot_xy.py
import json, csv, math, statistics, argparse
ap = argparse.ArgumentParser()
ap.add_argument("--analysis", required=True)
ap.add_argument("--components", required=True)
ap.add_argument("--out", required=True)
args = ap.parse_args()
a = json.load(open(args.analysis, "r", encoding="utf-8"))
H = float(a["height"])
z_cut = float(a["z_cut"])
rows = []
with open(args.components, "r", encoding="utf-8") as f:
rd = csv.DictReader(f)
for r in rd:
cx = float(r["cx"]); cy = float(r["cy"]); cz = float(r["cz"])
# マスト周りは「下部〜z_cut少し下」かつ「中心に近い」部品が多いので、それを使う
if 0.10*H < cz < (z_cut - 0.05*H):
rad = math.hypot(cx, cy)
rows.append((rad, cx, cy))
rows.sort(key=lambda t: t[0])
N = min(200, len(rows))
sel = rows[:N]
px = statistics.median([t[1] for t in sel])
py = statistics.median([t[2] for t in sel])
a["pivot"][0] = px
a["pivot"][1] = py
json.dump(a, open(args.out, "w", encoding="utf-8"), indent=2)
print("pivot_fixed =", a["pivot"])
5.2. 実行
python3 fix_pivot_xy.py --analysis out/analysis.json --components out/components.csv --out out/analysis_fixed.json
出力：
out/analysis_fixed.json
6. 生成：Blenderヘッドレスで base.dae / upper.dae を作る
ここから Blender API（bpy） を使います（GUI不要）。
6.1. 生成スクリプト（blender_generate_base_upper.py）を配置
~/tower_crane_ws/blender_generate_base_upper.py に置く
blender_generate_base_upper.py
import bpy, sys, os, json, argparse, statistics, math
from mathutils import Vector
def parse_args():
argv = sys.argv
argv = argv[argv.index("--")+1:] if "--" in argv else []
p = argparse.ArgumentParser()
p.add_argument("--input", required=True)
p.add_argument("--analysis", required=False, default=None)
p.add_argument("--outdir", required=True)
p.add_argument("--k_pivot", type=int, default=200) # pivot推定に使う近傍部品数
p.add_argument("--pivot_iters", type=int, default=3) # pivot推定の反復回数
return p.parse_args(argv)
def clear_scene():
bpy.ops.object.select_all(action="SELECT")
bpy.ops.object.delete(use_global=False, confirm=False)
def select(objs, active=None):
bpy.ops.object.select_all(action="DESELECT")
for o in objs:
o.select_set(True)
bpy.context.view_layer.objects.active = active or (objs[0] if objs else None)
def world_bbox(obj):
corners = [obj.matrix_world @ Vector(c) for c in obj.bound_box]
mn = Vector((min(v.x for v in corners), min(v.y for v in corners), min(v.z for v in corners)))
mx = Vector((max(v.x for v in corners), max(v.y for v in corners), max(v.z for v in corners)))
return mn, mx
def collada_export_selected(filepath):
try:
bpy.ops.wm.collada_export(filepath=filepath, selected=True, include_children=False)
except TypeError:
bpy.ops.wm.collada_export(filepath=filepath, selected=True)
def export_group(objs, filepath):
if not objs:
return False
select(objs, active=objs[0])
collada_export_selected(filepath)
return True
def join_meshes(meshes):
if len(meshes) <= 1:
return meshes[0]
select(meshes, active=meshes[0])
bpy.ops.object.join()
return bpy.context.view_layer.objects.active
def separate_loose_parts(obj):
select([obj], active=obj)
bpy.ops.object.mode_set(mode="EDIT")
bpy.ops.mesh.select_all(action="SELECT")
bpy.ops.mesh.separate(type="LOOSE")
bpy.ops.object.mode_set(mode="OBJECT")
return [o for o in bpy.context.scene.objects if o.type == "MESH"]
def percentile(vals, q):
if not vals:
return None
vals = sorted(vals)
idx = int(q * (len(vals)-1))
return vals[idx]
def bisect_keep(obj, plane_co_world, keep="below"):
select([obj], active=obj)
bpy.ops.object.mode_set(mode="EDIT")
bpy.ops.mesh.select_all(action="SELECT")
inv = obj.matrix_world.inverted()
plane_co = inv @ plane_co_world
plane_no = Vector((0,0,1))
if keep == "below":
bpy.ops.mesh.bisect(plane_co=plane_co, plane_no=plane_no,
clear_outer=True, clear_inner=False, use_fill=True)
else:
bpy.ops.mesh.bisect(plane_co=plane_co, plane_no=plane_no,
clear_outer=False, clear_inner=True, use_fill=True)
bpy.ops.object.mode_set(mode="OBJECT")
def main():
args = parse_args()
inp = os.path.abspath(os.path.expanduser(args.input))
out = os.path.abspath(os.path.expanduser(args.outdir))
os.makedirs(out, exist_ok=True)
# ===== 解析値（比率）だけ使う：座標そのものはBlender側で再推定する =====
far_ratio = 0.35
margin_ratio = 0.05
core_ratio = 0.08
mast_dz_ratio = 0.45
if args.analysis:
a = json.load(open(os.path.expanduser(args.analysis), "r", encoding="utf-8"))
# far_ratio = far_threshold / span_major
span_major = a["span_y"] if a.get("major_axis", "Y") == "Y" else a["span_x"]
if span_major and a.get("far_threshold"):
far_ratio = float(a["far_threshold"]) / float(span_major)
# margin_ratio = (z_jib_bottom_guess - z_cut) / height
if a.get("z_jib_bottom_guess") is not None and a.get("z_cut") is not None and a.get("height"):
margin_ratio = (float(a["z_jib_bottom_guess"]) - float(a["z_cut"])) / float(a["height"])
core_ratio = float(a.get("core_ratio", core_ratio))
mast_dz_ratio = float(a.get("mast_dz_ratio", mast_dz_ratio))
clear_scene()
# import_units=False で単位変換を抑制（4.5ではOK）
try:
bpy.ops.wm.collada_import(filepath=inp, import_units=False)
except TypeError:
bpy.ops.wm.collada_import(filepath=inp)
meshes = [o for o in bpy.context.scene.objects if o.type == "MESH"]
if not meshes:
raise RuntimeError("No MESH objects imported.")
# いったん結合して全体bboxを取りやすくする
obj_all = join_meshes(meshes)
mn, mx = world_bbox(obj_all)
span = mx - mn
H = max(1e-9, float(span.z))
LX = max(1e-9, float(span.x))
LY = max(1e-9, float(span.y))
center = (mn + mx) * 0.5
major = "Y" if LY >= LX else "X"
span_major = LY if major == "Y" else LX
far_th = far_ratio * span_major
# Loose parts 分割
parts = separate_loose_parts(obj_all)
# ===== pivot推定（XY）：中心近傍の密なクラスタ（=マスト周り）へ反復収束 =====
gx, gy = float(center.x), float(center.y)
z_low = float(mn.z + 0.05 * H)
z_high = float(mn.z + 0.85 * H)
for _ in range(max(1, args.pivot_iters)):
cand = []
for p in parts:
pmn, pmx = world_bbox(p)
c = (pmn + pmx) * 0.5
if z_low < c.z < z_high:
r = math.hypot(float(c.x) - gx, float(c.y) - gy)
cand.append((r, float(c.x), float(c.y)))
cand.sort(key=lambda t: t[0])
sel = cand[:min(args.k_pivot, len(cand))]
if not sel:
break
gx = statistics.median([t[1] for t in sel])
gy = statistics.median([t[2] for t in sel])
px, py = gx, gy
pivot_major = py if major == "Y" else px
# ===== z_cut推定：遠方（ジブ側）頂点の低位分位→少し下げる =====
z_far = []
z_all = []
for p in parts:
for v in p.data.vertices:
w = p.matrix_world @ v.co
m = w.y if major == "Y" else w.x
z_all.append(float(w.z))
if abs(float(m) - pivot_major) > far_th:
z_far.append(float(w.z))
z_jib_bottom = percentile(z_far, 0.05) if z_far else percentile(z_all, 0.75)
z_cut = float(z_jib_bottom - margin_ratio * H)
z_cut = max(float(mn.z + 0.20*H), min(float(mn.z + 0.95*H), z_cut))
# 分類閾値（Blender座標で）
core_x = core_ratio * LX
core_y = core_ratio * LY
mast_dz = mast_dz_ratio * H
base, upper = [], []
for p in parts:
pmn, pmx = world_bbox(p)
dims = pmx - pmn
c = (pmn + pmx) * 0.5
near_center = (abs(float(c.x) - px) < core_x) and (abs(float(c.y) - py) < core_y)
is_mast = near_center and (float(dims.z) > mast_dz)
m = float(c.y) if major == "Y" else float(c.x)
is_far = abs(m - pivot_major) > far_th
is_high = float(c.z) > z_cut # ★max_zではなくcentroid_zを使う（mastがupperに吸われにくい）
if is_far:
upper.append(p)
elif is_high and (not is_mast):
upper.append(p)
else:
base.append(p)
base_path = os.path.join(out, "base.dae")
upper_path = os.path.join(out, "upper.dae")
ok_base = export_group(base, base_path)
ok_upper = export_group(upper, upper_path)
# upperが空などで失敗したら bisect で強制生成（保険）
if (not ok_base) or (not ok_upper):
# partsを全部結合してから切る
meshes2 = [o for o in bpy.context.scene.objects if o.type == "MESH"]
obj2 = join_meshes(meshes2)
base_obj = obj2.copy(); base_obj.data = obj2.data.copy()
upper_obj = obj2.copy(); upper_obj.data = obj2.data.copy()
bpy.context.collection.objects.link(base_obj)
bpy.context.collection.objects.link(upper_obj)
plane = Vector((px, py, z_cut))
bisect_keep(base_obj, plane, keep="below")
bisect_keep(upper_obj, plane, keep="above")
export_group([base_obj], base_path)
export_group([upper_obj], upper_path)
with open(os.path.join(out, "pivot.json"), "w", encoding="utf-8") as f:
json.dump({
"pivot": [px, py, z_cut],
"axis": [0,0,1],
"major_axis": major,
"far_ratio": far_ratio,
"far_threshold": far_th,
"margin_ratio": margin_ratio,
"z_jib_bottom": z_jib_bottom,
}, f, indent=2)
print("=== SUMMARY ===")
print("bbox_min:", [float(mn.x), float(mn.y), float(mn.z)])
print("bbox_max:", [float(mx.x), float(mx.y), float(mx.z)])
print("major_axis:", major)
print("pivot_xy:", px, py)
print("z_cut:", z_cut)
print("far_th:", far_th)
print("base_parts:", len(base), "upper_parts:", len(upper))
print("WROTE:", base_path)
print("WROTE:", upper_path)
print("WROTE:", os.path.join(out, "pivot.json"))
if __name__ == "__main__":
main()
このプログラムでやること
-
DAEを読み込み
-
いったん結合（join）
-
Separate by Loose Partsで部品分割 -
解析結果（
analysis_fixed.json）を使って-
z_cutより上端が高い → upper - major軸方向に遠い（ジブ側）→ upper
- 中心付近で背が高い（マスト）→ base固定
というルールで分類
-
-
base.dae/upper.daeとして書き出し
6.2. 実行
blender -b --factory-startup -P blender_generate_base_upper.py -- \
--input tower_crane.dae \
--analysis out/analysis_fixed.json \
--outdir out
出力：
out/base.daeout/upper.daeout/pivot.json
7. SDFで結合：2リンク＋1回転ジョイントモデルを作る
Gazeboのモデルディレクトリへ配置します
7.1. モデル配置
mkdir -p ~/.gazebo/models/tower_crane_rot/meshes
cp out/base.dae ~/.gazebo/models/tower_crane_rot/meshes/
cp out/upper.dae ~/.gazebo/models/tower_crane_rot/meshes/
7.2. model.config
保存先：~/.gazebo/models/tower_crane_rot/model.config
<?xml version="1.0"?>
<model>
<name>tower_crane_rot</name>
<version>1.0</version>
<sdf version="1.6">model.sdf</sdf>
<author><name>you</name></author>
<description>tower crane split into base+upper with yaw joint</description>
</model>
7.3. model.sdf（pivot.jsonのpivotの内容を反映）
out/pivot.json が例えばこうなら：
{"pivot":[0.0, -0.000005, 1133.3438], "axis":[0,0,1], ...}
SDFのここを以下のようにします：
<pose>0.0 -0.000005 1133.3438 0 0 0</pose>
実際に出力されたout/pivot.jsonは以下です
pivot.json
{
"pivot": [
0.0,
-0.49999988079071045,
26.855416679382323
],
"axis": [
0,
0,
1
],
"major_axis": "Y",
"far_ratio": 0.3499999999999999,
"far_threshold": 13.679608154296872,
"margin_ratio": 0.04999999999999999,
"z_jib_bottom": 28.606826782226562
}
このpivot.jsonを参考にmodel.sdfを作成しますが、この記事では以下の工夫をしました。
-
pivot の Y軸の位置が少しずれていたため、Y座標を 0 に修正し、SDF は以下のように設定しました。
-
また、upper.dae と base.dae の結合部分の形状に少し不自然な点があったため、Blender で upper.dae の根元を少し短く修正しました。そのため、結合位置を下げるために
<pose>0 0 -1.1 0 0 0</pose>と設定しています。
保存先：~/.gazebo/models/tower_crane_rot/model.sdf
<?xml version="1.0" ?>
<sdf version="1.6">
<model name="tower_crane_rot">
<static>false</static>
<pose>0 0 0 0 0 0</pose>
<!-- ===== Fixed part (base) ===== -->
<link name="base_link">
<pose>0 0 0 0 0 0</pose>
<gravity>false</gravity>
<visual name="base_vis">
<geometry>
<mesh>
<uri>model://tower_crane_rot/meshes/base.dae</uri>
</mesh>
</geometry>
</visual>
</link>
<!-- ===== Rotating part (upper) ===== -->
<link name="upper_link">
<pose>0 0 -1.1 0 0 0</pose>
<gravity>false</gravity>
<visual name="upper_vis">
<geometry>
<mesh>
<uri>model://tower_crane_rot/meshes/upper.dae</uri>
</mesh>
</geometry>
</visual>
</link>
<!-- A) 対策：base_link を world に固定 -->
<joint name="base_fixed" type="fixed">
<parent>world</parent>
<child>base_link</child>
</joint>
<!-- ===== Yaw joint (upper rotates around Z) ===== -->
<joint name="yaw_joint" type="revolute">
<parent>base_link</parent>
<child>upper_link</child>
<!-- pivot.json の pivot を反映 -->
<pose>0.0 0.0 26.855416679382323 0 0 0</pose>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-3.14159</lower>
<upper> 3.14159</upper>
<effort>1000</effort>
<velocity>1.0</velocity>
</limit>
<dynamics>
<damping>0.2</damping>
</dynamics>
</axis>
</joint>
</model>
</sdf>
8. Gazeboで確認：上物だけ旋回するか確認
8.1. テストワールド
~/tower_crane_ws/tower_crane_rot.world
<?xml version="1.0" ?>
<sdf version="1.6">
<world name="default">
<include><uri>model://ground_plane</uri></include>
<include><uri>model://sun</uri></include>
<include>
<uri>model://tower_crane_rot</uri>
<pose>0 0 0 0 0 0</pose>
</include>
</world>
</sdf>
8.2. 起動
gazebo --verbose ~/tower_crane_ws/tower_crane_rot.world
8.3. 別ターミナルでジョイント角を与える
gz joint -m tower_crane_rot -j yaw_joint --pos-t 1.57 --pos-p 50 --pos-i 0 --pos-d 5
以下が回転に成功した時の動画です。
9. まとめ
今回は、タワークレーンのモデルを分割することで、回転可能なモデルを作成することができました。今後は、「Automating the Tower Crane: Integrating the Development and Simulation of Path Planning and Trajectory Tracking of Tower Crane in ROS Framework」 の論文を参考にしながら、ROS や ROS2 を用いてタワークレーンの自動搬送シミュレーションの再現に取り組んでみたいと考えています。
なお、現時点ではトロリの移動など、まだ追加していない要素もあるため、今後それらの機能も順次実装していきたいと考えています。