foodforthought-cli 0.2.1__py3-none-any.whl → 0.2.3__py3-none-any.whl

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
Files changed (46) hide show
  1. ate/__init__.py +1 -1
  2. ate/bridge_server.py +622 -0
  3. ate/cli.py +2625 -242
  4. ate/compatibility.py +580 -0
  5. ate/generators/__init__.py +19 -0
  6. ate/generators/docker_generator.py +461 -0
  7. ate/generators/hardware_config.py +469 -0
  8. ate/generators/ros2_generator.py +617 -0
  9. ate/generators/skill_generator.py +783 -0
  10. ate/marketplace.py +524 -0
  11. ate/mcp_server.py +1341 -107
  12. ate/primitives.py +1016 -0
  13. ate/robot_setup.py +2222 -0
  14. ate/skill_schema.py +537 -0
  15. ate/telemetry/__init__.py +33 -0
  16. ate/telemetry/cli.py +455 -0
  17. ate/telemetry/collector.py +444 -0
  18. ate/telemetry/context.py +318 -0
  19. ate/telemetry/fleet_agent.py +419 -0
  20. ate/telemetry/formats/__init__.py +18 -0
  21. ate/telemetry/formats/hdf5_serializer.py +503 -0
  22. ate/telemetry/formats/mcap_serializer.py +457 -0
  23. ate/telemetry/types.py +334 -0
  24. foodforthought_cli-0.2.3.dist-info/METADATA +300 -0
  25. foodforthought_cli-0.2.3.dist-info/RECORD +44 -0
  26. foodforthought_cli-0.2.3.dist-info/top_level.txt +6 -0
  27. mechdog_labeled/__init__.py +3 -0
  28. mechdog_labeled/primitives.py +113 -0
  29. mechdog_labeled/servo_map.py +209 -0
  30. mechdog_output/__init__.py +3 -0
  31. mechdog_output/primitives.py +59 -0
  32. mechdog_output/servo_map.py +203 -0
  33. test_autodetect/__init__.py +3 -0
  34. test_autodetect/primitives.py +113 -0
  35. test_autodetect/servo_map.py +209 -0
  36. test_full_auto/__init__.py +3 -0
  37. test_full_auto/primitives.py +113 -0
  38. test_full_auto/servo_map.py +209 -0
  39. test_smart_detect/__init__.py +3 -0
  40. test_smart_detect/primitives.py +113 -0
  41. test_smart_detect/servo_map.py +209 -0
  42. foodforthought_cli-0.2.1.dist-info/METADATA +0 -151
  43. foodforthought_cli-0.2.1.dist-info/RECORD +0 -9
  44. foodforthought_cli-0.2.1.dist-info/top_level.txt +0 -1
  45. {foodforthought_cli-0.2.1.dist-info → foodforthought_cli-0.2.3.dist-info}/WHEEL +0 -0
  46. {foodforthought_cli-0.2.1.dist-info → foodforthought_cli-0.2.3.dist-info}/entry_points.txt +0 -0
@@ -0,0 +1,113 @@
1
+ """
2
+ Primitive skills for unnamed_robot
3
+ Generated by ate robot-setup wizard
4
+ """
5
+
6
+ import time
7
+ from typing import List, Dict, Optional
8
+ from dataclasses import dataclass
9
+
10
+ from .servo_map import ServoID, SERVO_LIMITS, ALL_SERVOS
11
+
12
+
13
+ @dataclass
14
+ class ServoCommand:
15
+ """Command for a single servo."""
16
+ servo_id: int
17
+ position: int
18
+ time_ms: int = 500
19
+
20
+
21
+ class PrimitiveSkills:
22
+ """Low-level primitive skills for the robot."""
23
+
24
+ def __init__(self, robot_controller):
25
+ """
26
+ Args:
27
+ robot_controller: Controller with move_servo(), read_position() methods
28
+ """
29
+ self.controller = robot_controller
30
+
31
+ def move_servo(self, servo_id: ServoID, position: int, time_ms: int = 500):
32
+ """Move a single servo to position."""
33
+ limits = SERVO_LIMITS.get(servo_id, {"min": 0, "max": 1000})
34
+ position = max(limits["min"], min(limits["max"], position))
35
+ self.controller.move_servo(servo_id, position, time_ms)
36
+
37
+ def move_servos(self, commands: List[ServoCommand]):
38
+ """Move multiple servos simultaneously."""
39
+ for cmd in commands:
40
+ limits = SERVO_LIMITS.get(cmd.servo_id, {"min": 0, "max": 1000})
41
+ pos = max(limits["min"], min(limits["max"], cmd.position))
42
+ self.controller.move_servo(cmd.servo_id, pos, cmd.time_ms)
43
+
44
+ def home(self):
45
+ """Move all servos to center/home position."""
46
+ commands = [
47
+ ServoCommand(servo_id, SERVO_LIMITS[servo_id]["center"])
48
+ for servo_id in ALL_SERVOS
49
+ ]
50
+ self.move_servos(commands)
51
+
52
+ def read_positions(self) -> Dict[ServoID, int]:
53
+ """Read all servo positions."""
54
+ positions = {}
55
+ for servo_id in ALL_SERVOS:
56
+ pos = self.controller.read_position(servo_id)
57
+ if pos is not None:
58
+ positions[servo_id] = pos
59
+ return positions
60
+
61
+ def move_front_right_leg(self, positions: Dict[ServoID, int], time_ms: int = 500):
62
+ """Move front_right_leg servos."""
63
+ commands = [
64
+ ServoCommand(servo_id, pos, time_ms)
65
+ for servo_id, pos in positions.items()
66
+ if servo_id in FRONT_RIGHT_LEG
67
+ ]
68
+ self.move_servos(commands)
69
+
70
+ def move_front_left_leg(self, positions: Dict[ServoID, int], time_ms: int = 500):
71
+ """Move front_left_leg servos."""
72
+ commands = [
73
+ ServoCommand(servo_id, pos, time_ms)
74
+ for servo_id, pos in positions.items()
75
+ if servo_id in FRONT_LEFT_LEG
76
+ ]
77
+ self.move_servos(commands)
78
+
79
+ def move_back_right_leg(self, positions: Dict[ServoID, int], time_ms: int = 500):
80
+ """Move back_right_leg servos."""
81
+ commands = [
82
+ ServoCommand(servo_id, pos, time_ms)
83
+ for servo_id, pos in positions.items()
84
+ if servo_id in BACK_RIGHT_LEG
85
+ ]
86
+ self.move_servos(commands)
87
+
88
+ def move_back_left_leg(self, positions: Dict[ServoID, int], time_ms: int = 500):
89
+ """Move back_left_leg servos."""
90
+ commands = [
91
+ ServoCommand(servo_id, pos, time_ms)
92
+ for servo_id, pos in positions.items()
93
+ if servo_id in BACK_LEFT_LEG
94
+ ]
95
+ self.move_servos(commands)
96
+
97
+ def move_arm(self, positions: Dict[ServoID, int], time_ms: int = 500):
98
+ """Move arm servos."""
99
+ commands = [
100
+ ServoCommand(servo_id, pos, time_ms)
101
+ for servo_id, pos in positions.items()
102
+ if servo_id in ARM
103
+ ]
104
+ self.move_servos(commands)
105
+
106
+ def move_other(self, positions: Dict[ServoID, int], time_ms: int = 500):
107
+ """Move other servos."""
108
+ commands = [
109
+ ServoCommand(servo_id, pos, time_ms)
110
+ for servo_id, pos in positions.items()
111
+ if servo_id in OTHER
112
+ ]
113
+ self.move_servos(commands)
@@ -0,0 +1,209 @@
1
+ """
2
+ Servo map for unnamed_robot
3
+ Generated by ate robot-setup wizard
4
+ Robot type: quadruped_with_arm
5
+ """
6
+
7
+ from enum import IntEnum
8
+
9
+
10
+ class ServoID(IntEnum):
11
+ """Servo IDs mapped to descriptive names."""
12
+ FRONT_RIGHT_HIP = 0
13
+ FRONT_RIGHT_UPPER = 1
14
+ FRONT_RIGHT_LOWER = 2
15
+ FRONT_LEFT_HIP = 3
16
+ FRONT_LEFT_UPPER = 4
17
+ FRONT_LEFT_LOWER = 5
18
+ BACK_RIGHT_HIP = 6
19
+ BACK_RIGHT_UPPER = 7
20
+ BACK_RIGHT_LOWER = 8
21
+ BACK_LEFT_HIP = 9
22
+ BACK_LEFT_UPPER = 10
23
+ BACK_LEFT_LOWER = 11
24
+ ARM_SHOULDER = 12
25
+ ARM_ELBOW = 13
26
+ ARM_GRIPPER = 15
27
+ SERVO_16 = 16
28
+ SERVO_17 = 17
29
+ SERVO_18 = 18
30
+ SERVO_19 = 19
31
+ SERVO_20 = 20
32
+ SERVO_21 = 21
33
+ SERVO_22 = 22
34
+ SERVO_23 = 23
35
+ SERVO_24 = 24
36
+ SERVO_25 = 25
37
+ SERVO_26 = 26
38
+ SERVO_28 = 28
39
+ SERVO_29 = 29
40
+ SERVO_30 = 30
41
+ SERVO_31 = 31
42
+
43
+
44
+ # Servo groups
45
+ FRONT_RIGHT_LEG = [ServoID.FRONT_RIGHT_HIP, ServoID.FRONT_RIGHT_UPPER, ServoID.FRONT_RIGHT_LOWER]
46
+ FRONT_LEFT_LEG = [ServoID.FRONT_LEFT_HIP, ServoID.FRONT_LEFT_UPPER, ServoID.FRONT_LEFT_LOWER]
47
+ BACK_RIGHT_LEG = [ServoID.BACK_RIGHT_HIP, ServoID.BACK_RIGHT_UPPER, ServoID.BACK_RIGHT_LOWER]
48
+ BACK_LEFT_LEG = [ServoID.BACK_LEFT_HIP, ServoID.BACK_LEFT_UPPER, ServoID.BACK_LEFT_LOWER]
49
+ ARM = [ServoID.ARM_SHOULDER, ServoID.ARM_ELBOW, ServoID.ARM_GRIPPER]
50
+ OTHER = [ServoID.SERVO_16, ServoID.SERVO_17, ServoID.SERVO_18, ServoID.SERVO_19, ServoID.SERVO_20, ServoID.SERVO_21, ServoID.SERVO_22, ServoID.SERVO_23, ServoID.SERVO_24, ServoID.SERVO_25, ServoID.SERVO_26, ServoID.SERVO_28, ServoID.SERVO_29, ServoID.SERVO_30, ServoID.SERVO_31]
51
+
52
+
53
+ # All servos
54
+ ALL_SERVOS = list(ServoID)
55
+
56
+
57
+ # Servo characteristics
58
+ SERVO_LIMITS = {
59
+ ServoID.FRONT_RIGHT_HIP: {
60
+ "min": 0,
61
+ "max": 1000,
62
+ "center": 500,
63
+ },
64
+ ServoID.FRONT_RIGHT_UPPER: {
65
+ "min": 0,
66
+ "max": 1000,
67
+ "center": 500,
68
+ },
69
+ ServoID.FRONT_RIGHT_LOWER: {
70
+ "min": 0,
71
+ "max": 1000,
72
+ "center": 500,
73
+ },
74
+ ServoID.FRONT_LEFT_HIP: {
75
+ "min": 0,
76
+ "max": 1000,
77
+ "center": 500,
78
+ },
79
+ ServoID.FRONT_LEFT_UPPER: {
80
+ "min": 0,
81
+ "max": 1000,
82
+ "center": 500,
83
+ },
84
+ ServoID.FRONT_LEFT_LOWER: {
85
+ "min": 0,
86
+ "max": 1000,
87
+ "center": 500,
88
+ },
89
+ ServoID.BACK_RIGHT_HIP: {
90
+ "min": 0,
91
+ "max": 1000,
92
+ "center": 500,
93
+ },
94
+ ServoID.BACK_RIGHT_UPPER: {
95
+ "min": 0,
96
+ "max": 1000,
97
+ "center": 500,
98
+ },
99
+ ServoID.BACK_RIGHT_LOWER: {
100
+ "min": 0,
101
+ "max": 1000,
102
+ "center": 500,
103
+ },
104
+ ServoID.BACK_LEFT_HIP: {
105
+ "min": 0,
106
+ "max": 1000,
107
+ "center": 500,
108
+ },
109
+ ServoID.BACK_LEFT_UPPER: {
110
+ "min": 0,
111
+ "max": 1000,
112
+ "center": 500,
113
+ },
114
+ ServoID.BACK_LEFT_LOWER: {
115
+ "min": 0,
116
+ "max": 1000,
117
+ "center": 500,
118
+ },
119
+ ServoID.ARM_SHOULDER: {
120
+ "min": 0,
121
+ "max": 1000,
122
+ "center": 500,
123
+ },
124
+ ServoID.ARM_ELBOW: {
125
+ "min": 0,
126
+ "max": 1000,
127
+ "center": 500,
128
+ },
129
+ ServoID.ARM_GRIPPER: {
130
+ "min": 0,
131
+ "max": 1000,
132
+ "center": 500,
133
+ },
134
+ ServoID.SERVO_16: {
135
+ "min": 0,
136
+ "max": 1000,
137
+ "center": 500,
138
+ },
139
+ ServoID.SERVO_17: {
140
+ "min": 0,
141
+ "max": 1000,
142
+ "center": 500,
143
+ },
144
+ ServoID.SERVO_18: {
145
+ "min": 0,
146
+ "max": 1000,
147
+ "center": 500,
148
+ },
149
+ ServoID.SERVO_19: {
150
+ "min": 0,
151
+ "max": 1000,
152
+ "center": 500,
153
+ },
154
+ ServoID.SERVO_20: {
155
+ "min": 0,
156
+ "max": 1000,
157
+ "center": 500,
158
+ },
159
+ ServoID.SERVO_21: {
160
+ "min": 0,
161
+ "max": 1000,
162
+ "center": 500,
163
+ },
164
+ ServoID.SERVO_22: {
165
+ "min": 0,
166
+ "max": 1000,
167
+ "center": 500,
168
+ },
169
+ ServoID.SERVO_23: {
170
+ "min": 0,
171
+ "max": 1000,
172
+ "center": 500,
173
+ },
174
+ ServoID.SERVO_24: {
175
+ "min": 0,
176
+ "max": 1000,
177
+ "center": 500,
178
+ },
179
+ ServoID.SERVO_25: {
180
+ "min": 0,
181
+ "max": 1000,
182
+ "center": 500,
183
+ },
184
+ ServoID.SERVO_26: {
185
+ "min": 0,
186
+ "max": 1000,
187
+ "center": 500,
188
+ },
189
+ ServoID.SERVO_28: {
190
+ "min": 0,
191
+ "max": 1000,
192
+ "center": 500,
193
+ },
194
+ ServoID.SERVO_29: {
195
+ "min": 0,
196
+ "max": 1000,
197
+ "center": 500,
198
+ },
199
+ ServoID.SERVO_30: {
200
+ "min": 0,
201
+ "max": 1000,
202
+ "center": 500,
203
+ },
204
+ ServoID.SERVO_31: {
205
+ "min": 0,
206
+ "max": 1000,
207
+ "center": 500,
208
+ },
209
+ }
@@ -0,0 +1,3 @@
1
+ """Generated robot package for unnamed_robot."""
2
+ from .servo_map import ServoID, SERVO_LIMITS, ALL_SERVOS
3
+ from .primitives import PrimitiveSkills, ServoCommand
@@ -0,0 +1,59 @@
1
+ """
2
+ Primitive skills for unnamed_robot
3
+ Generated by ate robot-setup wizard
4
+ """
5
+
6
+ import time
7
+ from typing import List, Dict, Optional
8
+ from dataclasses import dataclass
9
+
10
+ from .servo_map import ServoID, SERVO_LIMITS, ALL_SERVOS
11
+
12
+
13
+ @dataclass
14
+ class ServoCommand:
15
+ """Command for a single servo."""
16
+ servo_id: int
17
+ position: int
18
+ time_ms: int = 500
19
+
20
+
21
+ class PrimitiveSkills:
22
+ """Low-level primitive skills for the robot."""
23
+
24
+ def __init__(self, robot_controller):
25
+ """
26
+ Args:
27
+ robot_controller: Controller with move_servo(), read_position() methods
28
+ """
29
+ self.controller = robot_controller
30
+
31
+ def move_servo(self, servo_id: ServoID, position: int, time_ms: int = 500):
32
+ """Move a single servo to position."""
33
+ limits = SERVO_LIMITS.get(servo_id, {"min": 0, "max": 1000})
34
+ position = max(limits["min"], min(limits["max"], position))
35
+ self.controller.move_servo(servo_id, position, time_ms)
36
+
37
+ def move_servos(self, commands: List[ServoCommand]):
38
+ """Move multiple servos simultaneously."""
39
+ for cmd in commands:
40
+ limits = SERVO_LIMITS.get(cmd.servo_id, {"min": 0, "max": 1000})
41
+ pos = max(limits["min"], min(limits["max"], cmd.position))
42
+ self.controller.move_servo(cmd.servo_id, pos, cmd.time_ms)
43
+
44
+ def home(self):
45
+ """Move all servos to center/home position."""
46
+ commands = [
47
+ ServoCommand(servo_id, SERVO_LIMITS[servo_id]["center"])
48
+ for servo_id in ALL_SERVOS
49
+ ]
50
+ self.move_servos(commands)
51
+
52
+ def read_positions(self) -> Dict[ServoID, int]:
53
+ """Read all servo positions."""
54
+ positions = {}
55
+ for servo_id in ALL_SERVOS:
56
+ pos = self.controller.read_position(servo_id)
57
+ if pos is not None:
58
+ positions[servo_id] = pos
59
+ return positions
@@ -0,0 +1,203 @@
1
+ """
2
+ Servo map for unnamed_robot
3
+ Generated by ate robot-setup wizard
4
+ Robot type:
5
+ """
6
+
7
+ from enum import IntEnum
8
+
9
+
10
+ class ServoID(IntEnum):
11
+ """Servo IDs mapped to descriptive names."""
12
+ SERVO_0 = 0
13
+ SERVO_1 = 1
14
+ SERVO_2 = 2
15
+ SERVO_3 = 3
16
+ SERVO_4 = 4
17
+ SERVO_5 = 5
18
+ SERVO_6 = 6
19
+ SERVO_7 = 7
20
+ SERVO_8 = 8
21
+ SERVO_9 = 9
22
+ SERVO_10 = 10
23
+ SERVO_11 = 11
24
+ SERVO_12 = 12
25
+ SERVO_13 = 13
26
+ SERVO_15 = 15
27
+ SERVO_16 = 16
28
+ SERVO_17 = 17
29
+ SERVO_18 = 18
30
+ SERVO_19 = 19
31
+ SERVO_20 = 20
32
+ SERVO_21 = 21
33
+ SERVO_22 = 22
34
+ SERVO_23 = 23
35
+ SERVO_24 = 24
36
+ SERVO_25 = 25
37
+ SERVO_26 = 26
38
+ SERVO_28 = 28
39
+ SERVO_29 = 29
40
+ SERVO_30 = 30
41
+ SERVO_31 = 31
42
+
43
+
44
+ # Servo groups
45
+
46
+
47
+ # All servos
48
+ ALL_SERVOS = list(ServoID)
49
+
50
+
51
+ # Servo characteristics
52
+ SERVO_LIMITS = {
53
+ ServoID.SERVO_0: {
54
+ "min": 0,
55
+ "max": 1000,
56
+ "center": 500,
57
+ },
58
+ ServoID.SERVO_1: {
59
+ "min": 0,
60
+ "max": 1000,
61
+ "center": 500,
62
+ },
63
+ ServoID.SERVO_2: {
64
+ "min": 0,
65
+ "max": 1000,
66
+ "center": 500,
67
+ },
68
+ ServoID.SERVO_3: {
69
+ "min": 0,
70
+ "max": 1000,
71
+ "center": 500,
72
+ },
73
+ ServoID.SERVO_4: {
74
+ "min": 0,
75
+ "max": 1000,
76
+ "center": 500,
77
+ },
78
+ ServoID.SERVO_5: {
79
+ "min": 0,
80
+ "max": 1000,
81
+ "center": 500,
82
+ },
83
+ ServoID.SERVO_6: {
84
+ "min": 0,
85
+ "max": 1000,
86
+ "center": 500,
87
+ },
88
+ ServoID.SERVO_7: {
89
+ "min": 0,
90
+ "max": 1000,
91
+ "center": 500,
92
+ },
93
+ ServoID.SERVO_8: {
94
+ "min": 0,
95
+ "max": 1000,
96
+ "center": 500,
97
+ },
98
+ ServoID.SERVO_9: {
99
+ "min": 0,
100
+ "max": 1000,
101
+ "center": 500,
102
+ },
103
+ ServoID.SERVO_10: {
104
+ "min": 0,
105
+ "max": 1000,
106
+ "center": 500,
107
+ },
108
+ ServoID.SERVO_11: {
109
+ "min": 0,
110
+ "max": 1000,
111
+ "center": 500,
112
+ },
113
+ ServoID.SERVO_12: {
114
+ "min": 0,
115
+ "max": 1000,
116
+ "center": 500,
117
+ },
118
+ ServoID.SERVO_13: {
119
+ "min": 0,
120
+ "max": 1000,
121
+ "center": 500,
122
+ },
123
+ ServoID.SERVO_15: {
124
+ "min": 0,
125
+ "max": 1000,
126
+ "center": 500,
127
+ },
128
+ ServoID.SERVO_16: {
129
+ "min": 0,
130
+ "max": 1000,
131
+ "center": 500,
132
+ },
133
+ ServoID.SERVO_17: {
134
+ "min": 0,
135
+ "max": 1000,
136
+ "center": 500,
137
+ },
138
+ ServoID.SERVO_18: {
139
+ "min": 0,
140
+ "max": 1000,
141
+ "center": 500,
142
+ },
143
+ ServoID.SERVO_19: {
144
+ "min": 0,
145
+ "max": 1000,
146
+ "center": 500,
147
+ },
148
+ ServoID.SERVO_20: {
149
+ "min": 0,
150
+ "max": 1000,
151
+ "center": 500,
152
+ },
153
+ ServoID.SERVO_21: {
154
+ "min": 0,
155
+ "max": 1000,
156
+ "center": 500,
157
+ },
158
+ ServoID.SERVO_22: {
159
+ "min": 0,
160
+ "max": 1000,
161
+ "center": 500,
162
+ },
163
+ ServoID.SERVO_23: {
164
+ "min": 0,
165
+ "max": 1000,
166
+ "center": 500,
167
+ },
168
+ ServoID.SERVO_24: {
169
+ "min": 0,
170
+ "max": 1000,
171
+ "center": 500,
172
+ },
173
+ ServoID.SERVO_25: {
174
+ "min": 0,
175
+ "max": 1000,
176
+ "center": 500,
177
+ },
178
+ ServoID.SERVO_26: {
179
+ "min": 0,
180
+ "max": 1000,
181
+ "center": 500,
182
+ },
183
+ ServoID.SERVO_28: {
184
+ "min": 0,
185
+ "max": 1000,
186
+ "center": 500,
187
+ },
188
+ ServoID.SERVO_29: {
189
+ "min": 0,
190
+ "max": 1000,
191
+ "center": 500,
192
+ },
193
+ ServoID.SERVO_30: {
194
+ "min": 0,
195
+ "max": 1000,
196
+ "center": 500,
197
+ },
198
+ ServoID.SERVO_31: {
199
+ "min": 0,
200
+ "max": 1000,
201
+ "center": 500,
202
+ },
203
+ }
@@ -0,0 +1,3 @@
1
+ """Generated robot package for unnamed_robot."""
2
+ from .servo_map import ServoID, SERVO_LIMITS, ALL_SERVOS
3
+ from .primitives import PrimitiveSkills, ServoCommand
@@ -0,0 +1,113 @@
1
+ """
2
+ Primitive skills for unnamed_robot
3
+ Generated by ate robot-setup wizard
4
+ """
5
+
6
+ import time
7
+ from typing import List, Dict, Optional
8
+ from dataclasses import dataclass
9
+
10
+ from .servo_map import ServoID, SERVO_LIMITS, ALL_SERVOS
11
+
12
+
13
+ @dataclass
14
+ class ServoCommand:
15
+ """Command for a single servo."""
16
+ servo_id: int
17
+ position: int
18
+ time_ms: int = 500
19
+
20
+
21
+ class PrimitiveSkills:
22
+ """Low-level primitive skills for the robot."""
23
+
24
+ def __init__(self, robot_controller):
25
+ """
26
+ Args:
27
+ robot_controller: Controller with move_servo(), read_position() methods
28
+ """
29
+ self.controller = robot_controller
30
+
31
+ def move_servo(self, servo_id: ServoID, position: int, time_ms: int = 500):
32
+ """Move a single servo to position."""
33
+ limits = SERVO_LIMITS.get(servo_id, {"min": 0, "max": 1000})
34
+ position = max(limits["min"], min(limits["max"], position))
35
+ self.controller.move_servo(servo_id, position, time_ms)
36
+
37
+ def move_servos(self, commands: List[ServoCommand]):
38
+ """Move multiple servos simultaneously."""
39
+ for cmd in commands:
40
+ limits = SERVO_LIMITS.get(cmd.servo_id, {"min": 0, "max": 1000})
41
+ pos = max(limits["min"], min(limits["max"], cmd.position))
42
+ self.controller.move_servo(cmd.servo_id, pos, cmd.time_ms)
43
+
44
+ def home(self):
45
+ """Move all servos to center/home position."""
46
+ commands = [
47
+ ServoCommand(servo_id, SERVO_LIMITS[servo_id]["center"])
48
+ for servo_id in ALL_SERVOS
49
+ ]
50
+ self.move_servos(commands)
51
+
52
+ def read_positions(self) -> Dict[ServoID, int]:
53
+ """Read all servo positions."""
54
+ positions = {}
55
+ for servo_id in ALL_SERVOS:
56
+ pos = self.controller.read_position(servo_id)
57
+ if pos is not None:
58
+ positions[servo_id] = pos
59
+ return positions
60
+
61
+ def move_right_arm(self, positions: Dict[ServoID, int], time_ms: int = 500):
62
+ """Move right_arm servos."""
63
+ commands = [
64
+ ServoCommand(servo_id, pos, time_ms)
65
+ for servo_id, pos in positions.items()
66
+ if servo_id in RIGHT_ARM
67
+ ]
68
+ self.move_servos(commands)
69
+
70
+ def move_left_arm(self, positions: Dict[ServoID, int], time_ms: int = 500):
71
+ """Move left_arm servos."""
72
+ commands = [
73
+ ServoCommand(servo_id, pos, time_ms)
74
+ for servo_id, pos in positions.items()
75
+ if servo_id in LEFT_ARM
76
+ ]
77
+ self.move_servos(commands)
78
+
79
+ def move_right_leg(self, positions: Dict[ServoID, int], time_ms: int = 500):
80
+ """Move right_leg servos."""
81
+ commands = [
82
+ ServoCommand(servo_id, pos, time_ms)
83
+ for servo_id, pos in positions.items()
84
+ if servo_id in RIGHT_LEG
85
+ ]
86
+ self.move_servos(commands)
87
+
88
+ def move_left_leg(self, positions: Dict[ServoID, int], time_ms: int = 500):
89
+ """Move left_leg servos."""
90
+ commands = [
91
+ ServoCommand(servo_id, pos, time_ms)
92
+ for servo_id, pos in positions.items()
93
+ if servo_id in LEFT_LEG
94
+ ]
95
+ self.move_servos(commands)
96
+
97
+ def move_head(self, positions: Dict[ServoID, int], time_ms: int = 500):
98
+ """Move head servos."""
99
+ commands = [
100
+ ServoCommand(servo_id, pos, time_ms)
101
+ for servo_id, pos in positions.items()
102
+ if servo_id in HEAD
103
+ ]
104
+ self.move_servos(commands)
105
+
106
+ def move_torso(self, positions: Dict[ServoID, int], time_ms: int = 500):
107
+ """Move torso servos."""
108
+ commands = [
109
+ ServoCommand(servo_id, pos, time_ms)
110
+ for servo_id, pos in positions.items()
111
+ if servo_id in TORSO
112
+ ]
113
+ self.move_servos(commands)