foodforthought-cli 0.2.1__py3-none-any.whl → 0.2.4__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.
- ate/__init__.py +1 -1
- ate/bridge_server.py +622 -0
- ate/cli.py +2625 -242
- ate/compatibility.py +580 -0
- ate/generators/__init__.py +19 -0
- ate/generators/docker_generator.py +461 -0
- ate/generators/hardware_config.py +469 -0
- ate/generators/ros2_generator.py +617 -0
- ate/generators/skill_generator.py +783 -0
- ate/marketplace.py +524 -0
- ate/mcp_server.py +1341 -107
- ate/primitives.py +1016 -0
- ate/robot_setup.py +2222 -0
- ate/skill_schema.py +537 -0
- ate/telemetry/__init__.py +33 -0
- ate/telemetry/cli.py +455 -0
- ate/telemetry/collector.py +444 -0
- ate/telemetry/context.py +318 -0
- ate/telemetry/fleet_agent.py +419 -0
- ate/telemetry/formats/__init__.py +18 -0
- ate/telemetry/formats/hdf5_serializer.py +503 -0
- ate/telemetry/formats/mcap_serializer.py +457 -0
- ate/telemetry/types.py +334 -0
- foodforthought_cli-0.2.4.dist-info/METADATA +300 -0
- foodforthought_cli-0.2.4.dist-info/RECORD +44 -0
- foodforthought_cli-0.2.4.dist-info/top_level.txt +6 -0
- mechdog_labeled/__init__.py +3 -0
- mechdog_labeled/primitives.py +113 -0
- mechdog_labeled/servo_map.py +209 -0
- mechdog_output/__init__.py +3 -0
- mechdog_output/primitives.py +59 -0
- mechdog_output/servo_map.py +203 -0
- test_autodetect/__init__.py +3 -0
- test_autodetect/primitives.py +113 -0
- test_autodetect/servo_map.py +209 -0
- test_full_auto/__init__.py +3 -0
- test_full_auto/primitives.py +113 -0
- test_full_auto/servo_map.py +209 -0
- test_smart_detect/__init__.py +3 -0
- test_smart_detect/primitives.py +113 -0
- test_smart_detect/servo_map.py +209 -0
- foodforthought_cli-0.2.1.dist-info/METADATA +0 -151
- foodforthought_cli-0.2.1.dist-info/RECORD +0 -9
- foodforthought_cli-0.2.1.dist-info/top_level.txt +0 -1
- {foodforthought_cli-0.2.1.dist-info → foodforthought_cli-0.2.4.dist-info}/WHEEL +0 -0
- {foodforthought_cli-0.2.1.dist-info → foodforthought_cli-0.2.4.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,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,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)
|