apirrone
commited on
Commit
·
2c56db2
1
Parent(s):
3f413a3
improve head tracking on wireless
Browse files
src/reachy_mini_conversation_app/camera_worker.py
CHANGED
|
@@ -156,6 +156,9 @@ class CameraWorker:
|
|
| 156 |
translation = target_pose[:3, 3]
|
| 157 |
rotation = R.from_matrix(target_pose[:3, :3]).as_euler("xyz", degrees=False)
|
| 158 |
|
|
|
|
|
|
|
|
|
|
| 159 |
# Thread-safe update of face tracking offsets (use pose as-is)
|
| 160 |
with self.face_tracking_lock:
|
| 161 |
self.face_tracking_offsets = [
|
|
@@ -189,7 +192,8 @@ class CameraWorker:
|
|
| 189 |
pose_matrix = np.eye(4, dtype=np.float32)
|
| 190 |
pose_matrix[:3, 3] = current_translation
|
| 191 |
pose_matrix[:3, :3] = R.from_euler(
|
| 192 |
-
"xyz",
|
|
|
|
| 193 |
).as_matrix()
|
| 194 |
self.interpolation_start_pose = pose_matrix
|
| 195 |
|
|
@@ -199,7 +203,9 @@ class CameraWorker:
|
|
| 199 |
|
| 200 |
# Interpolate between current pose and neutral pose
|
| 201 |
interpolated_pose = linear_pose_interpolation(
|
| 202 |
-
self.interpolation_start_pose,
|
|
|
|
|
|
|
| 203 |
)
|
| 204 |
|
| 205 |
# Extract translation and rotation from interpolated pose
|
|
|
|
| 156 |
translation = target_pose[:3, 3]
|
| 157 |
rotation = R.from_matrix(target_pose[:3, :3]).as_euler("xyz", degrees=False)
|
| 158 |
|
| 159 |
+
translation *= 0.5 # Scale down translation effect
|
| 160 |
+
rotation *= 0.5 # Scale down rotation effect
|
| 161 |
+
|
| 162 |
# Thread-safe update of face tracking offsets (use pose as-is)
|
| 163 |
with self.face_tracking_lock:
|
| 164 |
self.face_tracking_offsets = [
|
|
|
|
| 192 |
pose_matrix = np.eye(4, dtype=np.float32)
|
| 193 |
pose_matrix[:3, 3] = current_translation
|
| 194 |
pose_matrix[:3, :3] = R.from_euler(
|
| 195 |
+
"xyz",
|
| 196 |
+
current_rotation_euler,
|
| 197 |
).as_matrix()
|
| 198 |
self.interpolation_start_pose = pose_matrix
|
| 199 |
|
|
|
|
| 203 |
|
| 204 |
# Interpolate between current pose and neutral pose
|
| 205 |
interpolated_pose = linear_pose_interpolation(
|
| 206 |
+
self.interpolation_start_pose,
|
| 207 |
+
neutral_pose,
|
| 208 |
+
t,
|
| 209 |
)
|
| 210 |
|
| 211 |
# Extract translation and rotation from interpolated pose
|