Desmond-Dong commited on
Commit
12f9fe4
·
1 Parent(s): e298bb3

✨ feat(esphome): add head joints JSON sensor for 3D visualization

Browse files

- add get_head_joints_json() method to ReachyController
- add Phase 13 entity: head_joints (single JSON sensor)
- returns [yaw_body, stewart_1, ..., stewart_6] array in radians
- replaces complex multi-entity approach with single JSON sensor
- improves 3D model pose synchronization in HA dashboard

This allows the Home Assistant 3D card to properly control
all 7 head joints using the URDF joint system, matching
the desktop-app implementation.

reachy_mini_ha_voice/reachy_controller.py CHANGED
@@ -1006,3 +1006,27 @@ class ReachyController:
1006
  except Exception as e:
1007
  logger.debug(f"Error getting AEC converged status: {e}")
1008
  return False
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1006
  except Exception as e:
1007
  logger.debug(f"Error getting AEC converged status: {e}")
1008
  return False
1009
+
1010
+ # ========== Phase 13: Robot Joints ==========
1011
+
1012
+ def get_head_joints_json(self) -> str:
1013
+ """
1014
+ Get head joints as JSON string.
1015
+
1016
+ Returns:
1017
+ JSON string: "[yaw_body, stewart_1, stewart_2, stewart_3, stewart_4, stewart_5, stewart_6]"
1018
+ Values in radians
1019
+ """
1020
+ if not self.is_available:
1021
+ return "[]"
1022
+ try:
1023
+ import json
1024
+ head_joints, _ = self.reachy.get_current_joint_positions()
1025
+ if head_joints and len(head_joints) >= 7:
1026
+ # Convert radians to list
1027
+ joints_list = [float(j) for j in head_joints[:7]]
1028
+ return json.dumps(joints_list)
1029
+ return "[]"
1030
+ except Exception as e:
1031
+ logger.error(f"Error getting head joints JSON: {e}")
1032
+ return "[]"
reachy_mini_ha_voice/satellite.py CHANGED
@@ -126,6 +126,8 @@ class VoiceSatelliteProtocol(APIServer):
126
  "agc_max_gain": 1201,
127
  "noise_suppression": 1202,
128
  "echo_cancellation_converged": 1203,
 
 
129
  }
130
 
131
  def _get_entity_key(self, object_id: str) -> int:
@@ -176,6 +178,7 @@ class VoiceSatelliteProtocol(APIServer):
176
  self._setup_phase10_entities() # Camera
177
  # Phase 11 (LED control) disabled - LEDs are inside the robot and not visible
178
  self._setup_phase12_entities() # Audio processing
 
179
 
180
  # Mark entities as initialized
181
  self.state._entities_initialized = True
@@ -1413,3 +1416,19 @@ class VoiceSatelliteProtocol(APIServer):
1413
  self.state.entities.append(echo_cancellation_converged)
1414
 
1415
  _LOGGER.info("Phase 12 entities registered: agc_enabled, agc_max_gain, noise_suppression, echo_cancellation_converged")
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
126
  "agc_max_gain": 1201,
127
  "noise_suppression": 1202,
128
  "echo_cancellation_converged": 1203,
129
+ # Phase 13: Robot joints (single JSON sensor)
130
+ "head_joints": 1300,
131
  }
132
 
133
  def _get_entity_key(self, object_id: str) -> int:
 
178
  self._setup_phase10_entities() # Camera
179
  # Phase 11 (LED control) disabled - LEDs are inside the robot and not visible
180
  self._setup_phase12_entities() # Audio processing
181
+ self._setup_phase13_entities() # Robot joints
182
 
183
  # Mark entities as initialized
184
  self.state._entities_initialized = True
 
1416
  self.state.entities.append(echo_cancellation_converged)
1417
 
1418
  _LOGGER.info("Phase 12 entities registered: agc_enabled, agc_max_gain, noise_suppression, echo_cancellation_converged")
1419
+
1420
+ def _setup_phase13_entities(self) -> None:
1421
+ """Setup Phase 13 entities: Robot joints as JSON sensor."""
1422
+
1423
+ # Head joints sensor - returns JSON array of [yaw_body, stewart_1, ..., stewart_6]
1424
+ head_joints_sensor = TextSensorEntity(
1425
+ server=self,
1426
+ key=self._get_entity_key("head_joints"),
1427
+ name="Head Joints",
1428
+ object_id="head_joints",
1429
+ icon="mdi:robot",
1430
+ value_getter=self.reachy_controller.get_head_joints_json,
1431
+ )
1432
+ self.state.entities.append(head_joints_sensor)
1433
+
1434
+ _LOGGER.info("Phase 13 entities registered: head_joints")