91 lines
3.9 KiB
Python
91 lines
3.9 KiB
Python
"""G1 29-DoF motor → name / mesh mapping for the 3D temperature dashboard.
|
|
|
|
Ported verbatim from Marcus/Features/TempMonitor/config_g1.py so the copied
|
|
three.js front-end (static/temp3d/index.html) binds temperature colours to the
|
|
correct STL meshes. `build_payload()` turns the arm controller's raw lowstate
|
|
snapshot into the exact 'motor_update' payload shape that front-end expects.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
from typing import Any, Optional
|
|
|
|
# Motor ID → human name (29 motors = 29 DOF)
|
|
MOTOR_NAMES: dict[int, str] = {
|
|
0: "Left Hip Pitch", 1: "Left Hip Roll", 2: "Left Hip Yaw", 3: "Left Knee",
|
|
4: "Left Ankle Pitch", 5: "Left Ankle Roll",
|
|
6: "Right Hip Pitch", 7: "Right Hip Roll", 8: "Right Hip Yaw", 9: "Right Knee",
|
|
10: "Right Ankle Pitch", 11: "Right Ankle Roll",
|
|
12: "Waist Yaw", 13: "Waist Roll", 14: "Waist Pitch",
|
|
15: "Left Shoulder Pitch", 16: "Left Shoulder Roll", 17: "Left Shoulder Yaw",
|
|
18: "Left Elbow", 19: "Left Wrist Roll", 20: "Left Wrist Pitch", 21: "Left Wrist Yaw",
|
|
22: "Right Shoulder Pitch", 23: "Right Shoulder Roll", 24: "Right Shoulder Yaw",
|
|
25: "Right Elbow", 26: "Right Wrist Roll", 27: "Right Wrist Pitch", 28: "Right Wrist Yaw",
|
|
}
|
|
|
|
# Motor ID → URDF link / STL mesh name
|
|
MOTOR_TO_MESH: dict[int, str] = {
|
|
0: "left_hip_pitch_link", 1: "left_hip_roll_link", 2: "left_hip_yaw_link",
|
|
3: "left_knee_link", 4: "left_ankle_pitch_link", 5: "left_ankle_roll_link",
|
|
6: "right_hip_pitch_link", 7: "right_hip_roll_link", 8: "right_hip_yaw_link",
|
|
9: "right_knee_link", 10: "right_ankle_pitch_link", 11: "right_ankle_roll_link",
|
|
12: "waist_yaw_link", 13: "waist_roll_link", 14: "torso_link",
|
|
15: "left_shoulder_pitch_link", 16: "left_shoulder_roll_link", 17: "left_shoulder_yaw_link",
|
|
18: "left_elbow_link", 19: "left_wrist_roll_link", 20: "left_wrist_pitch_link",
|
|
21: "left_wrist_yaw_link",
|
|
22: "right_shoulder_pitch_link", 23: "right_shoulder_roll_link", 24: "right_shoulder_yaw_link",
|
|
25: "right_elbow_link", 26: "right_wrist_roll_link", 27: "right_wrist_pitch_link",
|
|
28: "right_wrist_yaw_link",
|
|
}
|
|
|
|
# Temperature thresholds (°C) — the three.js gradient maps MIN→MAX (blue→red).
|
|
TEMP_MIN = 30
|
|
TEMP_MAX = 120
|
|
TEMP_WARM_THRESHOLD = 45
|
|
TEMP_HOT_THRESHOLD = 60
|
|
|
|
|
|
def _coerce(v: Optional[int]) -> float:
|
|
"""Temperatures default to 0 when the firmware didn't report one, so the
|
|
front-end's Math.max / .toFixed never sees null/NaN."""
|
|
return float(v) if v is not None else 0.0
|
|
|
|
|
|
def build_payload(temps: list[dict[str, Any]],
|
|
positions: list[float],
|
|
timestamp: float) -> dict[str, Any]:
|
|
"""Build the Marcus-compatible 'motor_update' payload.
|
|
|
|
`temps` — arm.get_motor_temps(): [{motor_id, surface, winding}]
|
|
`positions` — arm.get_current_q(): joint angles indexed by motor id
|
|
"""
|
|
temperatures: list[dict[str, Any]] = []
|
|
for t in temps or []:
|
|
i = t.get("motor_id")
|
|
surface = t.get("surface")
|
|
winding = t.get("winding")
|
|
if surface is not None and winding is not None:
|
|
avg = (_coerce(surface) + _coerce(winding)) / 2.0
|
|
else:
|
|
avg = _coerce(surface if surface is not None else winding)
|
|
entry: dict[str, Any] = {
|
|
"motor_id": i,
|
|
"motor_name": MOTOR_NAMES.get(i, f"Motor {i}"),
|
|
"mesh_name": MOTOR_TO_MESH.get(i, ""),
|
|
"surface": _coerce(surface),
|
|
"winding": _coerce(winding),
|
|
"temp1": _coerce(surface),
|
|
"temp2": _coerce(winding),
|
|
"avg": avg,
|
|
}
|
|
if positions and isinstance(i, int) and i < len(positions):
|
|
entry["position"] = float(positions[i])
|
|
temperatures.append(entry)
|
|
|
|
pos_list: list[dict[str, Any]] = [
|
|
{"motor_id": i, "position": float(q), "link_name": MOTOR_TO_MESH.get(i)}
|
|
for i, q in enumerate(positions or [])
|
|
]
|
|
return {"temperatures": temperatures, "positions": pos_list,
|
|
"timestamp": timestamp}
|