import bpy, socket, math, threading, queue, json
from enum import Enum
from bpy.app.handlers import persistent
# -----------------------
# Constants
# -----------------------
ARM_NAME_PREFIX = 'arm.'
LIGHT_NAME_PREFIX = 'light.'
BGR_PREFIX = 'bgr-'
LIGHT_PWM_MAX = 65534
LIGHT_PWM_RED_MAX = 30000
LIGHT_LUM_MAX = 1.0
# -----------------------
# Networking
# -----------------------
ConnState = Enum('ConnState', 'none connecting connected')
import time as _time
class Connection:
def __init__(self, sock=None, state=ConnState.none):
self.socket = sock
self.state = state
self.failures = 0 # consecutive failed connect attempts
self.next_retry_ts = 0.0 # wall-clock earliest retry; backoff target
sockets = {}
quad_last_command_dict = {}
sockets_queue = queue.Queue()
def connect_socket(ip, port=1337):
server_address = (ip, port)
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.settimeout(3.0) # tolerate ESP32 hiccups during accept
print('Connecting to', server_address)
sockets[ip].state = ConnState.connecting
try:
sock.connect(server_address)
print('Connected to', server_address)
sock.sendall(b'speed 200gZ78\n')
sockets[ip].socket = sock
sockets[ip].state = ConnState.connected
sockets[ip].failures = 0
sockets[ip].next_retry_ts = 0.0
return sock
except (socket.timeout, OSError) as e:
# Reset state so future sendString calls re-queue. Apply exponential
# backoff so a dead unit doesn't generate a connect storm.
sockets[ip].failures += 1
backoff = min(30.0, 2.0 ** min(sockets[ip].failures, 5))
sockets[ip].next_retry_ts = _time.monotonic() + backoff
sockets[ip].state = ConnState.none
print(f'[ERROR] connection failed on {ip}: {e} (next retry in {backoff:.0f}s)')
try: sock.close()
except OSError: pass
class SocketThread(threading.Thread):
def __init__(self):
self.shouldExit = False
threading.Thread.__init__(self, daemon=True)
self.start()
def run(self):
while True:
if self.shouldExit: return
ip = sockets_queue.get()
if sockets[ip].state != ConnState.connected:
connect_socket(ip)
sockets_queue.task_done()
# Guard against module reload spawning orphan worker threads.
if 'socket_thread' not in globals():
socket_thread = SocketThread()
def sendString(ip, s):
if not ip: return
conn = sockets.get(ip)
if not conn:
sockets[ip] = Connection(None)
sockets_queue.put(ip)
return
if conn.state == ConnState.connected:
try:
conn.socket.sendall(bytes(s, 'UTF-8'))
print(f"[SEND] {ip} -> {s.strip()}")
except (socket.timeout, BrokenPipeError, ConnectionResetError, OSError) as e:
print(f"[ERROR] Lost connection to {ip}: {e}; re-queueing")
conn.state = ConnState.none
conn.failures = max(conn.failures, 1)
conn.next_retry_ts = _time.monotonic() # one immediate retry
sockets_queue.put(ip)
elif conn.state == ConnState.none:
# Honor backoff timer so dead IPs don't generate a connect storm.
if _time.monotonic() >= conn.next_retry_ts:
sockets_queue.put(ip)
# -----------------------
# Helpers
# -----------------------
def getLamp(arm):
for child in arm.children:
if child.type == 'LIGHT':
return child
return None
def pwmFromLuminosity(lum, pwm_max=LIGHT_PWM_MAX):
return int(round(abs(lum) / LIGHT_LUM_MAX * pwm_max))
def luminosityFromPWM(pwm, pwm_max=LIGHT_PWM_MAX):
return pwm * LIGHT_LUM_MAX / pwm_max
def getAngles(arm):
def dynamixel_from_degrees(angle):
return int(round((angle - 90) * 600.0 / 180 + 512))
rad_to_deg = 180.0 / math.pi
up = arm.pose.bones['base']
fo = arm.pose.bones['forearm']
up_q = up.matrix.to_quaternion()
fo_q = fo.matrix.to_quaternion()
fo_q.rotate(up_q.inverted())
up_a = up.matrix.to_euler().x * rad_to_deg + 180
fo_a = fo_q.to_euler().z * rad_to_deg + 90
return [dynamixel_from_degrees(a) for a in [up_a, fo_a]]
# -----------------------
# Lazy Recompute Cache for Arms Only
# -----------------------
last_arm_state = {}
quad_last_command_dict = {}
# -----------------------
# Playback / keyframe helpers
# -----------------------
last_pwm2_target = {} # ip -> last 12-tuple of ints we sent in a pwm2
def is_playing():
"""True if any window is actively playing the timeline (not scrubbing)."""
wm = bpy.context.window_manager
if not wm: return False
for win in wm.windows:
if win.screen and win.screen.is_animation_playing:
return True
return False
def frames_to_ms(scene, frames):
fps = scene.render.fps / scene.render.fps_base
return max(1, int(round(frames * 1000.0 / fps)))
def get_animated_fcurves(ad):
"""Return the FCurves driving a given AnimData block.
Handles both the legacy (≤ 4.3) Action.fcurves shape and the Blender 4.4+
Slotted Actions shape, where FCurves live on a per-slot channelbag and
must be fetched via bpy_extras.anim_utils.action_get_channelbag_for_slot.
Only the slot bound to *this* AnimData is returned (other slots in the
same Action belong to other datablocks)."""
if not ad or not ad.action:
return []
action = ad.action
# Legacy compat path — pre-4.4 single-fcurves-on-Action.
if hasattr(action, 'fcurves'):
return list(action.fcurves)
# 4.4+: action.slots[*] + channelbag lookup.
slot = getattr(ad, 'action_slot', None)
if slot is None:
return []
try:
from bpy_extras import anim_utils
cb = anim_utils.action_get_channelbag_for_slot(action, slot)
except Exception:
return []
return list(cb.fcurves) if cb else []
def next_keyframe_time(fcurves, current_frame):
"""Smallest keyframe.x strictly greater than current_frame across fcurves,
or None if no keyframe is ahead."""
nxt = None
for fc in fcurves:
for kp in fc.keyframe_points:
x = kp.co.x
if x > current_frame and (nxt is None or x < nxt):
nxt = x
return nxt
def pick_pwm2_args(total_ms):
"""Decompose total fade ms into (numPeriods, periodMs), both 1..1000.
Total fade cap = 1,000,000 ms (~1000 s)."""
total_ms = max(1, min(1_000_000, int(total_ms)))
if total_ms <= 1000:
return (1, total_ms)
period_ms = (total_ms + 999) // 1000 # ceil so numPeriods stays <= 1000
num_periods = max(1, total_ms // period_ms)
return (num_periods, period_ms)
# -----------------------
# Core Handler
# -----------------------
@persistent
def robot_anim_handler(scene):
global last_arm_state, quad_last_command_dict
quad_pwm_dicts = {}
for obj in bpy.data.objects:
if obj.name.startswith(ARM_NAME_PREFIX):
ip = obj.name[len(ARM_NAME_PREFIX):]
angles = getAngles(obj)
lamp = getLamp(obj)
energy = lamp.data.energy if lamp else 0.0
pwm = pwmFromLuminosity(energy)
cur_state = (tuple(angles), pwm)
if last_arm_state.get(obj.name) == cur_state:
print(f"[SKIP] {obj.name} unchanged")
continue
last_arm_state[obj.name] = cur_state
s = "s " + " ".join(f"{i+1}:{a}" for i,a in enumerate(angles))
s += f"\npwm {pwm}\n"
print(f"[SEND] {obj.name} -> {s.strip()}")
sendString(ip, s)
elif obj.name.startswith(LIGHT_NAME_PREFIX):
ip = obj.name[len(LIGHT_NAME_PREFIX):]
pwm = pwmFromLuminosity(obj.data.energy)
s = f"pwm {pwm}\n"
print(f"[SEND] {obj.name} -> {s.strip()}")
sendString(ip, s)
elif obj.name.startswith(BGR_PREFIX):
# bgr-INDEX-IP — maxsplit=2 so hostnames containing '-' survive.
tokens = obj.name.split('-', 2)
if len(tokens) < 3: continue
index = int(tokens[1])
ip = tokens[2]
pwm_r = pwmFromLuminosity(obj.data.color[0], LIGHT_PWM_RED_MAX)
pwm_g = pwmFromLuminosity(obj.data.color[1])
pwm_b = pwmFromLuminosity(obj.data.color[2])
cur_str = f"{pwm_b} {pwm_g} {pwm_r} "
# Keep the obj reference so playback can re-evaluate fcurves at the
# next keyframe (target ≠ current interpolated value).
quad_pwm_dicts.setdefault(ip, {})[index] = (obj, cur_str)
scene = bpy.context.scene
cur_frame = scene.frame_current_final
playing = is_playing()
for ip, slot_dict in quad_pwm_dicts.items():
# Current-frame plain-pwm payload, used for scrub/pause fallback.
cur_vals = []
for i in range(1, 5):
if i in slot_dict:
cur_vals += slot_dict[i][1].split()
else:
cur_vals += ["0", "0", "0"]
if playing:
# Index each slot's color fcurves by array_index (0=R 1=G 2=B).
slot_fcurves = {}
all_fcurves = []
for idx, (obj, _) in slot_dict.items():
ad = obj.data.animation_data if obj.data else None
curves = [fc for fc in get_animated_fcurves(ad)
if fc.data_path == 'color']
all_fcurves += curves
slot_fcurves[idx] = {fc.array_index: fc for fc in curves}
nxt = next_keyframe_time(all_fcurves, cur_frame)
if nxt is not None:
# Build target by evaluating each fcurve AT THE NEXT KEYFRAME.
# This is the value the firmware should fade toward; it's
# constant within a segment so dedup catches the next frame
# and we send exactly one pwm2 per segment.
target_vals = []
for i in range(1, 5):
if i in slot_dict:
obj = slot_dict[i][0]
fcs = slot_fcurves[i]
def channel(ai, max_pwm):
if ai in fcs:
v = fcs[ai].evaluate(nxt)
else:
v = obj.data.color[ai]
return pwmFromLuminosity(v, max_pwm)
# BGR send order, matches firmware expectation.
target_vals += [
channel(2, LIGHT_PWM_MAX),
channel(1, LIGHT_PWM_MAX),
channel(0, LIGHT_PWM_RED_MAX),
]
else:
target_vals += [0, 0, 0]
target_tuple = tuple(target_vals)
if last_pwm2_target.get(ip) == target_tuple:
continue # same segment, already commanded
last_pwm2_target[ip] = target_tuple
fade_ms = frames_to_ms(scene, nxt - cur_frame)
num, per = pick_pwm2_args(fade_ms)
s = f"pwm2 {num} {per} " + " ".join(str(v) for v in target_vals) + "\n"
sendString(ip, s)
quad_last_command_dict.pop(ip, None)
continue
# no keyframes ahead — fall through to plain pwm
# Scrubbing, paused, or no more keyframes: plain pwm (cancels any fade).
s = "pwm " + " ".join(cur_vals) + "\n"
if quad_last_command_dict.get(ip) == s:
continue
quad_last_command_dict[ip] = s
last_pwm2_target.pop(ip, None)
sendString(ip, s)
# -----------------------
# File Save/Load
# -----------------------
def saveToFile(filepath):
arms = {}
for arm in bpy.data.objects:
if not arm.name.startswith(ARM_NAME_PREFIX): continue
ip = arm.name[len(ARM_NAME_PREFIX):]
angles = getAngles(arm)
lamp = getLamp(arm)
energy = pwmFromLuminosity(lamp.data.energy) if lamp else 0
arms[ip] = {"angles": angles, "light": energy}
text = {"LightArms": arms}
with open(filepath, 'w') as f:
json.dump(text, f, indent=2)
print(f"[SAVE] Robot positions saved to {filepath}")
def loadFromFile(filepath):
with open(filepath, 'r') as f:
text = json.load(f)
arms = text['LightArms']
for ip,data in arms.items():
name = ARM_NAME_PREFIX + ip
arm = bpy.data.objects[name]
lamp = getLamp(arm)
if lamp:
lamp.data.energy = luminosityFromPWM(data['light'])
print(f"[LOAD] Robot positions loaded from {filepath}")
# TODO: set angles if needed
# -----------------------
# UI Panel
# -----------------------
class ROBOT_PT_scene(bpy.types.Panel):
bl_label = "Robot Scene"
bl_space_type = "VIEW_3D"
bl_region_type = "UI"
bl_category = "Robot"
def draw(self, context):
self.layout.operator("circus.saver", text='Save Robot Positions')
self.layout.operator("circus.loader", text='Load Robot Positions')
class CircusSaver(bpy.types.Operator):
bl_idname = "circus.saver"
bl_label = "Save the robots"
filepath: bpy.props.StringProperty(subtype="FILE_PATH")
def execute(self, context):
saveToFile(self.filepath)
return {'FINISHED'}
def invoke(self, context, event):
context.window_manager.fileselect_add(self)
return {'RUNNING_MODAL'}
class CircusLoader(bpy.types.Operator):
bl_idname = "circus.loader"
bl_label = "Load the robots"
filepath: bpy.props.StringProperty(subtype="FILE_PATH")
def execute(self, context):
loadFromFile(self.filepath)
return {'FINISHED'}
def invoke(self, context, event):
context.window_manager.fileselect_add(self)
return {'RUNNING_MODAL'}
# -----------------------
# Register Everything
# -----------------------
_CLASSES = (ROBOT_PT_scene, CircusSaver, CircusLoader)
def unregister():
for cls in _CLASSES:
try: bpy.utils.unregister_class(cls)
except Exception: pass
# Strip *all* handlers named robot_anim_handler — catches stale instances
# left by previous script runs (different function-object identity, same
# name). identity-only remove() doesn't see those.
bpy.app.handlers.frame_change_post[:] = [
h for h in bpy.app.handlers.frame_change_post
if getattr(h, '__name__', None) != 'robot_anim_handler'
]
if __name__ == "__main__":
unregister()
for cls in _CLASSES:
bpy.utils.register_class(cls)
if robot_anim_handler not in bpy.app.handlers.frame_change_post:
bpy.app.handlers.frame_change_post.append(robot_anim_handler)
print("[INFO] Robot script initialized")