diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index abac9ed0..09b7bd9e 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -21,11 +21,12 @@ jobs: run: | python -m pip install --upgrade pip if [ -f requirements.txt ]; then pip install -r requirements.txt; fi + if [ -f install.sh ]; then chmod +x install.sh && ./install.sh cody; fi - name: Run unittests run: | - PYTHONPATH=src python -m unittest discover -s src/tests -p 'test_*.py' -t src - PYTHONPATH=src python -m unittest discover -s src/modules -p 'test_*.py' -t src + PYTHONPATH=src myenv/bin/python3 -m unittest discover -s src/tests -p 'test_*.py' -t src + PYTHONPATH=src myenv/bin/python3 -m unittest discover -s src/modules -p 'test_*.py' -t src permissions: pull-requests: write diff --git a/environments/cody.yml b/environments/cody.yml index cd2e9b17..a206e317 100644 --- a/environments/cody.yml +++ b/environments/cody.yml @@ -20,47 +20,53 @@ bus_servo: enabled: true config: poses: - - legs_forward: {'leg_r_tilt': 2806, 'leg_r_hip': 1989, 'leg_r_knee': 1349, 'leg_r_ankle': 2754, 'neck_pan': 1698, 'neck_tilt': 118, 'leg_l_tilt': 1619, 'leg_l_hip': 1028, 'leg_l_knee': 2634, 'leg_l_ankle': 1567} - - sit: {'leg_r_tilt': 2806, 'leg_r_hip': 2063, 'leg_r_knee': 1840, 'leg_r_ankle': 3641, 'neck_pan': 1697, 'neck_tilt': 118, 'leg_l_tilt': 1621, 'leg_l_hip': 853, 'leg_l_knee': 2161, 'leg_l_ankle': 614} - - wave_1: {'leg_r_tilt': 2809, 'leg_r_hip': 2063, 'leg_r_knee': 1840, 'leg_r_ankle': 3641, 'neck_pan': 1697, 'neck_tilt': 118, 'leg_l_tilt': 1618, 'leg_l_hip': 855, 'leg_l_knee': 2614, 'leg_l_ankle': 1337} - - wave_2: {'leg_r_tilt': 2807, 'leg_r_hip': 2063, 'leg_r_knee': 1840, 'leg_r_ankle': 3641, 'neck_pan': 1697, 'neck_tilt': 118, 'leg_l_tilt': 1618, 'leg_l_hip': 855, 'leg_l_knee': 2614, 'leg_l_ankle': 874} - - stand_low: {'leg_r_tilt': 2790, 'leg_r_hip': 422, 'leg_r_knee': 717, 'leg_r_ankle': 3174, 'neck_pan': 1697, 'neck_tilt': 119, 'leg_l_tilt': 1578, 'leg_l_hip': 2668, 'leg_l_knee': 3342, 'leg_l_ankle': 1107} - - stand_high: {'leg_r_tilt': 2790, 'leg_r_hip': 692, 'leg_r_knee': 1322, 'leg_r_ankle': 2861, 'neck_pan': 1700, 'neck_tilt': 123, 'leg_l_tilt': 1578, 'leg_l_hip': 2510, 'leg_l_knee': 2712, 'leg_l_ankle': 1560} - - sit_edge: {'leg_r_tilt': 2790, 'leg_r_hip': 1918, 'leg_r_knee': 2768, 'leg_r_ankle': 2854, 'neck_pan': 1704, 'neck_tilt': 124, 'leg_l_tilt': 1577, 'leg_l_hip': 1183, 'leg_l_knee': 1332, 'leg_l_ankle': 1562} - - sit_edge_swing_l: {'leg_r_tilt': 2790, 'leg_r_hip': 1918, 'leg_r_knee': 2768, 'leg_r_ankle': 2854, 'neck_pan': 1704, 'neck_tilt': 124, 'leg_l_tilt': 1577, 'leg_l_hip': 1185, 'leg_l_knee': 1789, 'leg_l_ankle': 1988} - - sit_edge_swing_r: {'leg_r_tilt': 2790, 'leg_r_hip': 1918, 'leg_r_knee': 2222, 'leg_r_ankle': 2496, 'neck_pan': 1704, 'neck_tilt': 124, 'leg_l_tilt': 1577, 'leg_l_hip': 1182, 'leg_l_knee': 1345, 'leg_l_ankle': 1511} + - storage: {'leg_r_tilt': 2784, 'leg_r_hip': 1006, 'leg_r_knee': 772, 'leg_r_ankle': 2991, 'neck_pan': 1708, 'neck_tilt': 121, 'leg_l_tilt': 1601, 'leg_l_hip': 3108, 'leg_l_knee': 3258, 'leg_l_ankle': 1286} + - legs_forward: {'leg_r_tilt': 2784, 'leg_r_hip': 1005, 'leg_r_knee': 1998, 'leg_r_ankle': 2351, 'neck_pan': 1710, 'neck_tilt': 121, 'leg_l_tilt': 1606, 'leg_l_hip': 3099, 'leg_l_knee': 2107, 'leg_l_ankle': 1865} + - sit: {'leg_r_tilt': 2784, 'leg_r_hip': 866, 'leg_r_knee': 2042, 'leg_r_ankle': 3396, 'neck_pan': 1711, 'neck_tilt': 121, 'leg_l_tilt': 1597, 'leg_l_hip': 3229, 'leg_l_knee': 2095, 'leg_l_ankle': 698} + - wave_1: {'leg_r_tilt': 2785, 'leg_r_hip': 857, 'leg_r_knee': 2037, 'leg_r_ankle': 3396, 'neck_pan': 1710, 'neck_tilt': 121, 'leg_l_tilt': 1599, 'leg_l_hip': 3131, 'leg_l_knee': 2626, 'leg_l_ankle': 1371} + - wave_2: {'leg_r_tilt': 2785, 'leg_r_hip': 857, 'leg_r_knee': 2037, 'leg_r_ankle': 3396, 'neck_pan': 1710, 'neck_tilt': 121, 'leg_l_tilt': 1599, 'leg_l_hip': 3131, 'leg_l_knee': 2626, 'leg_l_ankle': 701} + # - stand_straight: {'leg_r_tilt': 2735, 'leg_r_hip': 1968, 'leg_r_knee': 2029, 'leg_r_ankle': 2353, 'neck_pan': 1711, 'neck_tilt': 121, 'leg_l_tilt': 1596, 'leg_l_hip': 2087, 'leg_l_knee': 2075, 'leg_l_ankle': 1799} + - stand_low: {'leg_r_tilt': 2698, 'leg_r_hip': 3051, 'leg_r_knee': 596, 'leg_r_ankle': 2751, 'neck_pan': 1708, 'neck_tilt': 120, 'leg_l_tilt': 1626, 'leg_l_hip': 1149, 'leg_l_knee': 3481, 'leg_l_ankle': 1306} + - stand_high: {'leg_r_tilt': 2687, 'leg_r_hip': 2547, 'leg_r_knee': 1080, 'leg_r_ankle': 2753, 'neck_pan': 1711, 'neck_tilt': 116, 'leg_l_tilt': 1595, 'leg_l_hip': 1536, 'leg_l_knee': 2976, 'leg_l_ankle': 1522} + - stand_dip_l: {'leg_r_tilt': 2693, 'leg_r_hip': 2539, 'leg_r_knee': 1081, 'leg_r_ankle': 2742, 'neck_pan': 1713, 'neck_tilt': 119, 'leg_l_tilt': 1623, 'leg_l_hip': 1386, 'leg_l_knee': 3139, 'leg_l_ankle': 1522} + - stand_dip_r: {'leg_r_tilt': 2685, 'leg_r_hip': 2703, 'leg_r_knee': 971, 'leg_r_ankle': 2743, 'neck_pan': 1709, 'neck_tilt': 119, 'leg_l_tilt': 1657, 'leg_l_hip': 1463, 'leg_l_knee': 2992, 'leg_l_ankle': 1511} + - sit_edge: {'leg_r_tilt': 2784, 'leg_r_hip': 985, 'leg_r_knee': 2897, 'leg_r_ankle': 2816, 'neck_pan': 1711, 'neck_tilt': 121, 'leg_l_tilt': 1606, 'leg_l_hip': 3072, 'leg_l_knee': 1431, 'leg_l_ankle': 1161} + - sit_edge_swing_l: {'leg_r_tilt': 2784, 'leg_r_hip': 985, 'leg_r_knee': 2897, 'leg_r_ankle': 2816, 'neck_pan': 1709, 'neck_tilt': 121, 'leg_l_tilt': 1606, 'leg_l_hip': 3071, 'leg_l_knee': 1928, 'leg_l_ankle': 1154} + - sit_edge_swing_r: {'leg_r_tilt': 2783, 'leg_r_hip': 986, 'leg_r_knee': 2028, 'leg_r_ankle': 2633, 'neck_pan': 1709, 'neck_tilt': 121, 'leg_l_tilt': 1606, 'leg_l_hip': 3072, 'leg_l_knee': 1326, 'leg_l_ankle': 1161} + - legs_straight_out: {'leg_r_tilt': 3696, 'leg_r_hip': 2066, 'leg_r_knee': 1904, 'leg_r_ankle': 2777, 'neck_pan': 1713, 'neck_tilt': 116, 'leg_l_tilt': 599, 'leg_l_hip': 2103, 'leg_l_knee': 2285, 'leg_l_ankle': 1140} + - crab: {'leg_r_tilt': 3695, 'leg_r_hip': 1046, 'leg_r_knee': 1451, 'leg_r_ankle': 2745, 'neck_pan': 1712, 'neck_tilt': 116, 'leg_l_tilt': 599, 'leg_l_hip': 2930, 'leg_l_knee': 2855, 'leg_l_ankle': 1230} + # - sit_safe: + # - sit_safe_toe_up_l + # - sit_safe_toe_up_r + # - sit_safe_toe_down_l + # - sit_safe_toe_down_r instances: - name: "leg_r_tilt" model: 'ST3215' id: 1 - range: [2511, 3944] # 1433 total range (4095 = 360 degrees) - range_degrees: 125.9 - start: 2810 - # calibrate_on_boot: true + range: [2352, 3749] + # start: 2791 - name: "leg_r_hip" model: 'ST3215' id: 3 - range: [0, 2063] - range_degrees: 181.3 + range: [615, 3423] + # center_on_boot: true # calibrate_on_boot: true - # start: 1688 + # start: 977 - name: "leg_r_knee" model: 'ST3215' id: 2 - range: [717, 3297] - range_degrees: 226.2 - # start: 1345 + range: [626, 3384] + # start: 1999 - name: "leg_r_ankle" model: 'ST3215' id: 6 - range: [2459, 3872] - range_degrees: 123.7 - # start: 2762 + range: [1123, 3638] + # start: 2351 - name: "neck_pan" model: 'ST3215' id: 12 - range: [0, 3412] # 300 degrees = 3412, 360 degrees = 4095 - range_degrees: 299.7 + range: [1200, 2200] # Reduced range to prevent damaging wires start: 1706 # calibrate_on_boot: true # demonstrate_on_boot: true @@ -69,7 +75,6 @@ bus_servo: # baudrate: 115200 # id: 4 # range: [0, 1023] - # range_degrees: 300 # start: 511 # calibrate_on_boot: true # demonstrate_on_boot: true @@ -78,10 +83,9 @@ bus_servo: model: 'SC09' baudrate: 115200 id: 11 - range: [30, 220] - range_degrees: 55.6 + range: [30, 200] start: 120 - speed: 60 # max=0, 1 - 3000 tested + speed: 200 # max=0, 1 - 3000 tested # acceleration: 0 # max (no significant difference between this and 1 - 3000) # demonstrate_on_boot: true # - name: "neck_tilt2" @@ -97,27 +101,24 @@ bus_servo: model: 'ST3215' id: 7 range: [8, 1796] # 1788 total range (4095 = 360 degrees) - range_degrees: 156.9 - start: 1617 - # calibrate_on_boot: true + # start: 1607 - name: "leg_l_hip" model: 'ST3215' id: 8 - range: [853, 3098] - range_degrees: 197.3 - # start: 2451 + range: [615, 3423] + # center_on_boot: true + # calibrate_on_boot: true + # start: 977 - name: "leg_l_knee" model: 'ST3215' id: 9 - range: [1332, 3404] - range_degrees: 181.1 - # start: 2643 + range: [684, 3471] + # start: 2106 - name: "leg_l_ankle" model: 'ST3215' id: 10 - range: [614, 2270] - range_degrees: 145.2 - # start: 1566 + range: [520, 3059] + # start: 1856 # demonstrate_on_boot: true chatgpt: @@ -351,6 +352,36 @@ personality: config: mode: 6 # Config.MODE_LIVE sleep_timeout: 60 # Time in seconds before entering sleep mode + balance_enabled: true + track_people: true + track_people_servos: true + animate_pose_enabled: true + check_being_carried_enabled: false + knee_pose_thresholds: + sitting_edge: 1700 # 1431 + sitting: 2000 # 2095 + standing: 2900 # 2976 + poses: + - storage: {'leg_r_tilt': 2784, 'leg_r_hip': 1006, 'leg_r_knee': 772, 'leg_r_ankle': 2991, 'neck_pan': 1708, 'neck_tilt': 121, 'leg_l_tilt': 1601, 'leg_l_hip': 3108, 'leg_l_knee': 3258, 'leg_l_ankle': 1286} + - legs_forward: {'leg_r_tilt': 2784, 'leg_r_hip': 1005, 'leg_r_knee': 1998, 'leg_r_ankle': 2351, 'leg_l_tilt': 1606, 'leg_l_hip': 3099, 'leg_l_knee': 2107, 'leg_l_ankle': 1865} + - sit: {'leg_r_tilt': 2784, 'leg_r_hip': 866, 'leg_r_knee': 2042, 'leg_r_ankle': 3396, 'leg_l_tilt': 1597, 'leg_l_hip': 3229, 'leg_l_knee': 2095, 'leg_l_ankle': 698} + - wave_1: {'leg_r_tilt': 2785, 'leg_r_hip': 857, 'leg_r_knee': 2037, 'leg_r_ankle': 3396, 'leg_l_tilt': 1599, 'leg_l_hip': 3131, 'leg_l_knee': 2626, 'leg_l_ankle': 1371} + - wave_2: {'leg_r_tilt': 2785, 'leg_r_hip': 857, 'leg_r_knee': 2037, 'leg_r_ankle': 3396, 'leg_l_tilt': 1599, 'leg_l_hip': 3131, 'leg_l_knee': 2626, 'leg_l_ankle': 701} + # - stand_straight: {'leg_r_tilt': 2735, 'leg_r_hip': 1968, 'leg_r_knee': 2029, 'leg_r_ankle': 2353, 'leg_l_tilt': 1596, 'leg_l_hip': 2087, 'leg_l_knee': 2075, 'leg_l_ankle': 1799} + - stand_low: {'leg_r_tilt': 2698, 'leg_r_hip': 3051, 'leg_r_knee': 596, 'leg_r_ankle': 2751, 'leg_l_tilt': 1626, 'leg_l_hip': 1149, 'leg_l_knee': 3481, 'leg_l_ankle': 1306} + - stand_high: {'leg_r_tilt': 2687, 'leg_r_hip': 2547, 'leg_r_knee': 1080, 'leg_r_ankle': 2753, 'leg_l_tilt': 1595, 'leg_l_hip': 1536, 'leg_l_knee': 2976, 'leg_l_ankle': 1522} + - stand_dip_l: {'leg_r_tilt': 2693, 'leg_r_hip': 2539, 'leg_r_knee': 1081, 'leg_r_ankle': 2742, 'leg_l_tilt': 1623, 'leg_l_hip': 1386, 'leg_l_knee': 3139, 'leg_l_ankle': 1522} + - stand_dip_r: {'leg_r_tilt': 2685, 'leg_r_hip': 2703, 'leg_r_knee': 971, 'leg_r_ankle': 2743, 'leg_l_tilt': 1657, 'leg_l_hip': 1463, 'leg_l_knee': 2992, 'leg_l_ankle': 1511} + - sit_edge: {'leg_r_tilt': 2784, 'leg_r_hip': 985, 'leg_r_knee': 2897, 'leg_r_ankle': 2816, 'leg_l_tilt': 1606, 'leg_l_hip': 3072, 'leg_l_knee': 1431, 'leg_l_ankle': 1161} + - sit_edge_swing_l: {'leg_r_tilt': 2784, 'leg_r_hip': 985, 'leg_r_knee': 2897, 'leg_r_ankle': 2816, 'leg_l_tilt': 1606, 'leg_l_hip': 3071, 'leg_l_knee': 1928, 'leg_l_ankle': 1154} + - sit_edge_swing_r: {'leg_r_tilt': 2783, 'leg_r_hip': 986, 'leg_r_knee': 2028, 'leg_r_ankle': 2633, 'leg_l_tilt': 1606, 'leg_l_hip': 3072, 'leg_l_knee': 1326, 'leg_l_ankle': 1161} + - legs_straight_out: {'leg_r_tilt': 3696, 'leg_r_hip': 2066, 'leg_r_knee': 1904, 'leg_r_ankle': 2777, 'leg_l_tilt': 599, 'leg_l_hip': 2103, 'leg_l_knee': 2285, 'leg_l_ankle': 1140} + - crab: {'leg_r_tilt': 3695, 'leg_r_hip': 1046, 'leg_r_knee': 1451, 'leg_r_ankle': 2745, 'leg_l_tilt': 599, 'leg_l_hip': 2930, 'leg_l_knee': 2855, 'leg_l_ankle': 1230} + # - sit_safe: + # - sit_safe_toe_up_l + # - sit_safe_toe_up_r + # - sit_safe_toe_down_l + # - sit_safe_toe_down_r pitemperature: enabled: true diff --git a/src/main.py b/src/main.py index 5c82ebaa..55a6e462 100644 --- a/src/main.py +++ b/src/main.py @@ -35,6 +35,7 @@ def main(): # Inject messaging service and all module-to-module dependencies declared in the # environment YAML file (inject: / on_inject: blocks). loader.inject_dependencies(module_instances) + loader.modules_loaded(module_instances) # Call on_load() for all modules after injection # Use the new SystemLoop class to run the main loop system_loop = SystemLoop(module_instances['MessagingService'].messaging_service, module_instances) diff --git a/src/module_loader.py b/src/module_loader.py index 1da2afc0..ac472cb3 100755 --- a/src/module_loader.py +++ b/src/module_loader.py @@ -204,6 +204,13 @@ def inject_dependencies(self, instances): else: print(f"[ModuleLoader] on_inject: {target_key}.{method_name} not found or not callable") + def modules_loaded(self, instances): + """Call on_load() for all modules after dependencies have been injected.""" + for name, module in instances.items(): + if hasattr(module, 'on_load') and callable(module.on_load): + print(f"[ModuleLoader] Calling on_load for {name}") + module.on_load() + def _resolve_inject_source(self, source_spec, instances): """ Resolve an inject source specification to a value. diff --git a/src/modules/actuators/bus_servo/bus_servo.py b/src/modules/actuators/bus_servo/bus_servo.py index fd53ab8f..f19b161d 100644 --- a/src/modules/actuators/bus_servo/bus_servo.py +++ b/src/modules/actuators/bus_servo/bus_servo.py @@ -48,7 +48,7 @@ def __init__(self, **kwargs): self.model = kwargs.get('model', 'ST') self.index = kwargs.get('id') self.range = kwargs.get('range') - self.range_degrees = kwargs.get('range_degrees', None) # Optional range in degrees for easier control + self.range_degrees = kwargs.get('range_degrees', self.calculate_range_degrees(self.range[0], self.range[1])) self.start = kwargs.get('start') # Default start position self.poses = kwargs.get('poses') # Dictionary of poses self.baudrate = kwargs.get('baudrate', 1000000) @@ -57,8 +57,8 @@ def __init__(self, **kwargs): self.demonstrate_on_boot = kwargs.get('demonstrate_on_boot', False) # Move to min and max to demonstrate range self.center_on_boot = kwargs.get('center_on_boot', False) # Move to center of range on boot self.pos = None - self.speed = kwargs.get('speed', 300) # 3073 - self.acceleration = kwargs.get('acceleration', 50) + self.speed = kwargs.get('speed', 0) # 3073 + self.acceleration = kwargs.get('acceleration', 0) self._move_queue = collections.deque() # After loading YAML: poses_list = kwargs.get('poses', []) @@ -108,7 +108,7 @@ def setup_messaging(self): self.subscribe('servo:' + self.identifier + ':mv', self.move_relative) self.subscribe('servo:' + self.identifier + ':queue', self.move) self.subscribe('system/exit', self.exit) - self.subscribe('servo/pose', self.move_to_pose) + # self.subscribe('servo/pose', self.move_to_pose) # Disabled as this was causing issues with servos. Personality should call move directly. if self.calibrate_on_boot: self.calibrate_dynamic() # Log will show current position repeatedly to help with manual configuration @@ -135,6 +135,10 @@ def setup_messaging(self): def move_to_pose(self, pose_name): # print(self.poses) + # if pose in list of poses + if pose_name not in self.poses: + self.log(f"Pose '{pose_name}' not found for servo {self.identifier}", level='warning') + return pose_value = self.poses.get(pose_name) # print(f"{self.identifier} - Pose '{pose_name}' value: {pose_value}") my_pose_value = pose_value.get(self.identifier) @@ -168,13 +172,15 @@ def _sc_write(self, type, value, verbose=False): # self.log(f"[SCServo][{self.identifier}] Error context: load={load} (comm_result={load_comm_result}, error={load_error}), position={pos} (comm_result={pos_comm_result}, error={pos_error}), speed={speed} (comm_result={speed_comm_result}, error={speed_error})") return comm_result == COMM_SUCCESS and error == 0 - def move_degrees(self, degrees): + def move_degrees(self, degrees, speed=None): if self.range_degrees is None: self.log(f"Servo {self.identifier} does not have range_degrees set, cannot move by degrees", level='error') return + # self.log(f"[move_degrees] Moving servo {self.identifier} by {degrees} degrees") # Convert degrees to position value based on range, adjusting RELATIVE to current position self.pos = self.get_position() # Update current position before calculating new position if self.range is not None and self.pos is not None: + # self.log(f"Current position: {self.pos}, Range: {self.range}, Range degrees: {self.range_degrees}") # Calculate how many position units correspond to the degree change units_per_degree = (self.range[1] - self.range[0]) / self.range_degrees position_delta = degrees * units_per_degree @@ -187,7 +193,9 @@ def move_degrees(self, degrees): if self.range_degrees > 0: pc_move = round((degrees / self.range_degrees) * 100) self.log(f"Moving servo {self.identifier} by {degrees} degrees (position {self.pos} -> {new_position} | {pc_move}% of range)") - self.move(new_position) + self.move(new_position, speed) + else: + self.log(f"Invalid range_degrees for servo {self.identifier}, cannot move by degrees {self.range_degrees}", level='error') def move(self, position, speed=None, acceleration=None, delay=0, **kwargs): """ @@ -277,8 +285,13 @@ def move_relative(self, delta): def is_moving(self): if self.get_moving() == 1: return True - elif abs(self.pos - self.get_position()) > 15: - print(f"Warning: Servo {self.identifier} is not reporting as moving but position {self.get_position()} does not match target position {self.pos}") + current_position = self.get_position() + if self.pos is not None and current_position is not None and abs(self.pos - current_position) > 15: + self.log( + f"Servo {self.identifier} is not reporting as moving but position {current_position} does not match target position {self.pos}", + level='warning' + ) + return False return False def get_position(self): @@ -416,7 +429,7 @@ def calibrate_dynamic(self): max_pos = pos # Print on the same line, pad with spaces to clear previous content # (4095 = 360 degrees, so 1264 = 111 degrees) - range_degrees = (360/4095)*(max_pos - min_pos) if min_pos is not None and max_pos is not None else 'N/A' + range_degrees = self.calculate_range_degrees(max_pos, min_pos) print(f"\rCurrent position: {pos}, Min: {min_pos}, Max: {max_pos} Range(deg): {range_degrees}", end='', flush=True) time.sleep(0.05) if sys.stdin in select.select([sys.stdin], [], [], 0)[0]: @@ -436,6 +449,10 @@ def calibrate_dynamic(self): self.start = (min_pos + max_pos) // 2 self.log(f"Start position {self.start} out of new range, setting to midpoint {self.start}") + def calculate_range_degrees(self, max_pos, min_pos): + max_val = SC_MAX if self.model.startswith('SC') else ST_MAX + return (360/max_val)*abs(max_pos - min_pos) if min_pos is not None and max_pos is not None else 0 + def calibrate_to_center(self): """ Move the servo to the center of its range. @@ -453,7 +470,4 @@ def calibrate_to_center(self): self.packetHandler.write1ByteTxRx(self.portHandler, self.index, ADDR_SCS_GOAL_ACC, self.acceleration) self.packetHandler.write2ByteTxRx(self.portHandler, self.index, ADDR_SCS_GOAL_SPEED, self.speed) self.packetHandler.write2ByteTxRx(self.portHandler, self.index, ADDR_SCS_GOAL_POSITION, self.pos) - self.log(f"Moved servo {self.identifier} to position {self.pos}") - - - + self.log(f"Moved servo {self.identifier} to position {self.pos}") \ No newline at end of file diff --git a/src/modules/base_module.py b/src/modules/base_module.py index 2a70004f..f37072e2 100644 --- a/src/modules/base_module.py +++ b/src/modules/base_module.py @@ -15,6 +15,10 @@ def messaging_service(self, service): def setup_messaging(self): """Override this method in child classes to subscribe to topics.""" pass # No default implementation, subclasses should define their own subscriptions + + def on_load(self): + """Called when the module is loaded. Override in subclasses for initialization.""" + pass def publish(self, topic, *args, **kwargs): if self.messaging_service is None: diff --git a/src/modules/personality/personality.py b/src/modules/personality/personality.py index 3ca15cdb..108de46b 100644 --- a/src/modules/personality/personality.py +++ b/src/modules/personality/personality.py @@ -34,13 +34,28 @@ def __init__(self, **kwargs): self.imu = {} # Set in main.py self.vision = None # Set in main.py self.euler = None - self.balance_enabled = kwargs.get('balance_enabled', True) + self.balance_enabled = kwargs.get('balance_enabled', False) self.chicken_head_enabled = kwargs.get('chicken_head_enabled', False) - self.track_people = kwargs.get('track_people', True) # Uses vision data to track detected people with the eyes, and optionally neck servos. + self.track_people = kwargs.get('track_people', False) # Uses vision data to track detected people with the eyes, and optionally neck servos. self.track_people_servos = kwargs.get('track_people_servos', False) # Moves neck servos to track detected people. - self.one_leg_balance_enabled = kwargs.get('one_leg_balance_enabled', True) + self.one_leg_balance_enabled = kwargs.get('one_leg_balance_enabled', False) + self.animate_pose_enabled = kwargs.get('animate_pose_enabled', False) # Cycles through predefined poses randomly every 10 seconds in loop_10() self.servos = {} # Set in main.py + + self.pose_list = kwargs.get('poses', []) # List of predefined poses with servo positions + # After loading YAML: + poses_list = kwargs.get('poses', []) + # Convert to dict: + self.pose_list = {list(pose.keys())[0]: list(pose.values())[0] for pose in poses_list} + self.pose = None + self.knee_pose_thresholds = kwargs.get('knee_pose_thresholds') + self._pose_queue = [] # To queue animations + self._animating_pose = False # Flag to indicate if currently animating a pose + + self.check_being_carried_enabled = kwargs.get('check_being_carried_enabled', False) # If true, monitor body IMU for signs of being carried and disable leg servos if detected + self._carried = False # To track if being carried based on IMU data + self._last_imu_acceleration = None # To track last IMU acceleration for carried detection # Define possible actions self.actions = [ @@ -54,6 +69,7 @@ def setup_messaging(self): """Subscribe to necessary topics.""" self.subscribe('system/loop/1', self.loop_second) self.subscribe('system/loop/10', self.loop_10) + self.subscribe('system/loop/60', self.loop_60) if self.track_people: self.subscribe('vision/detections', self.handle_vision_detections) # Enable after testing north facing self.subscribe('gpio/motion', self.update_motion_time) @@ -71,6 +87,9 @@ def setup_messaging(self): # self.subscribe('imu/imu_body/data', self.handle_imu_data) # self.publish('gpio/laser', state=True) # Turn on laser if no one has been detected + def on_load(self): + self.animate_pose() + def test_servos(self): self.log("Testing neck tilt servos") @@ -162,17 +181,22 @@ def one_leg_balance(self): def balance(self): """Use head and body IMU data to maintain balance by adjusting leg servos.""" if not self.balance_enabled or 'body' not in self.imu: + # self.log("Balance check skipped: IMU data not available or balance disabled") return + # self.log("Balance check") euler = self.imu['body'].get_euler() + # self.log(f"Current body Euler angles: {euler}") # if euler has changed significantly since last update, adjust servos if self.euler is None or any(abs(e - self.euler[i]) > 1 for i, e in enumerate(euler)): self.euler = euler pitch = euler[1] + # self.log(f"Adjusting balance with pitch: {pitch}") if abs(pitch) < 2: return # No need to adjust for small angles # print(f"Angle to move: {pitch}") - self.servos['leg_l_hip'].move_degrees(-pitch) - self.servos['leg_r_hip'].move_degrees(pitch) + self.log(f"Moving leg servos to adjust balance: leg_l_hip {-pitch} degrees, leg_r_hip {pitch} degrees") + self.servos['leg_l_hip'].move_degrees(pitch, 200) + self.servos['leg_r_hip'].move_degrees(-pitch, 200) def handle_user_message(self, user_id=None, message=None): print(f"Received message from user {user_id}: {message}") @@ -213,69 +237,158 @@ def cycle_display(self): self.publish('display/body/text', text=f"{self.current_hz}Hz", font_size=20) elif self.display_state == 5: self.publish('display/body/text', text=f"{self.pose}", font_size=20) - - def loop_10(self): - # loop 3 times: - # for i in range(3): - # self.servos['leg_l_ankle'].move(1000, speed=0) - # self.servos['leg_l_ankle'].move(1500, speed=0) - # self.servos['leg_l_ankle'].move_degrees(50) - # self.scan_vision() - # self.output_current_pose() - # self.publish('servo/pose', pose_name='wave_1') # For testing pose movement - # time.sleep(1) + + def animate_pose(self): + if not self.animate_pose_enabled: + return # current_pose = self.estimate_current_pose() - # if current_pose not in self.servos['leg_r_tilt'].poses: - # return - # if current_pose == 'sit': - # self.publish('servo/pose', pose_name='wave_1') # For testing pose movement - # self.publish('servo/pose', pose_name='wave_2') # For testing pose movement - # self.publish('servo/pose', pose_name='wave_1') # For testing pose movement - # self.publish('servo/pose', pose_name='wave_2') # For testing pose movement - # self.publish('servo/pose', pose_name='sit') # For testing pose movement - # self.pose = 'sit' - # elif current_pose == 'sit_edge': - # self.pose = 'sit_edge_swing_l' - # elif current_pose == 'sit_edge_swing_l': - # self.pose = 'sit_edge_swing_r' - # elif current_pose == 'sit_edge_swing_r': - # self.pose = 'sit_edge' - # elif current_pose == 'legs_forward': - # self.pose = 'sit' - # self.publish('servo/pose', pose_name=self.pose) # For testing pose movement - pass + current_pose = self.estimate_pose_from_knee() + if current_pose is None: + self.pose = 'No Pose' + self.log("Current pose could not be estimated. Cannot animate pose.", level='warning') + return + + if current_pose == 'sitting': + self.balance_enabled = False + self.animate_wave('sit') + elif current_pose == 'sitting_edge': + self.balance_enabled = False + self.animate_swing_legs('sit_edge') + elif current_pose == 'standing': + self.animate_stand_high() + self.animate_pose_enabled = False # Don't move servos again, just stand. + self.balance_enabled = True + + def check_being_carried(self): + """ Detect if being carried by monitoring body IMU for prolonged movement (3 seconds) without corresponding leg movement. If detected, disable leg servos and set self._carried to true. """ + # _last_imu_acceleration to store last acceleration value from body IMU. If acceleration above threshold and no leg movement for 3 seconds, consider being carried. + if self.check_being_carried_enabled == False or 'body' not in self.imu: + return + acceleration = self.imu['body'].get_linear_acceleration() + # if any values in tuple above threshold, consider as significant movement + if isinstance(acceleration, (tuple, list)): + if any(abs(a) > 0.5 for a in acceleration): + acceleration = max(abs(a) for a in acceleration) # Use max acceleration value for threshold check + else: + acceleration = 0 + if acceleration > 0: + if self._last_imu_acceleration is None: + self._last_imu_acceleration = time.time() + if time.time() - self._last_imu_acceleration > 3 and not self._carried: + self._carried = True + for servo in self.servos.values(): + servo.detach() # Disable torque to allow free movement when being carried + self.publish('log', message="[Personality] Detected being carried. Disabling leg servos.") + else: + self._last_imu_acceleration = None + if self._carried: + self._carried = False + self.publish('log', message="[Personality] No longer being carried. Re-enabling leg servos.") + + + def animate_pose_queue(self): + """Non-blocking: queue pose for animation. Actual processing is in loop().""" + if self._pose_queue and not any(servo.is_moving() for servo in self.servos.values()): + pose_name = self._pose_queue.pop(0) + self.manually_trigger_pose(pose_name) + + def animate_stand_high(self): + self.log("Animating stand_high pose") + self.manually_trigger_pose('stand_high') + + def animate_wave(self, return_to_pose): + self.log(f"Animating wave from pose: {return_to_pose}") + queue = [] + for _ in range(3): + queue.append('wave_1') + queue.append('wave_2') + queue.append(return_to_pose) + self._pose_queue.extend(queue) + + def animate_swing_legs(self, return_to_pose): + self.log(f"Animating leg swing from pose: {return_to_pose}") + queue = [] + for _ in range(4): + queue.append('sit_edge_swing_l') + queue.append('sit_edge_swing_r') + queue.append(return_to_pose) + self._pose_queue.extend(queue) + + def manually_trigger_pose(self, pose_name): + self.log(f"Manually triggering pose: {pose_name}") + pose = self.pose_list.get(pose_name) + if pose is None: + self.log(f"Pose '{pose_name}' not found in pose list", level='warning') + return + for servo_name, position in pose.items(): + if servo_name not in self.servos: + self.log(f"Servo '{servo_name}' not found for pose '{pose_name}'", level='warning') + continue + try: + self.servos[servo_name].move(position) + except Exception as e: + self.log(f"Error moving servo '{servo_name}' to position {position} for pose '{pose_name}': {e}", level='error') def loop(self): pass def loop_second(self): now = time.time() - self.cycle_display() - self.balance() - self.chicken_head() - self.one_leg_balance() - self.estimate_current_pose() + self.cycle_display() # Update display every second + self.balance() # Adjust balance every second based on IMU data + self.chicken_head() # Move head to match head IMU orientation every second + self.one_leg_balance() # Adjust legs for one legged balance based on body roll every second + self.animate_pose_queue() # Process pose animation queue every second. This allows it to run in the background without blocking other actions. + self.check_being_carried() # Check if being carried every second based on IMU data - # Handle ongoing object reaction - if self.object_reaction_end_time and now >= self.object_reaction_end_time: - self.publish('led', identifiers=[ - 'right', 'top_right', 'top_left', 'left', - 'bottom_left', 'bottom_right' - ], color="off") - self.object_reaction_end_time = None + # self.estimate_current_pose() + + # # Handle ongoing object reaction + # if self.object_reaction_end_time and now >= self.object_reaction_end_time: + # self.publish('led', identifiers=[ + # 'right', 'top_right', 'top_left', 'left', + # 'bottom_left', 'bottom_right' + # ], color="off") + # self.object_reaction_end_time = None - # self.update_eye() - self.random_neopixel_status() + # # self.update_eye() + # self.random_neopixel_status() # Check if it's time for the next action - if now >= self.next_action_time: - action = choice(self.actions) - action() - self.next_action_time = self.calculate_next_action_time() + # if now >= self.next_action_time: + # action = choice(self.actions) + # action() + # self.next_action_time = self.calculate_next_action_time() + + # # If serial has been idle for more than 10 seconds, call random_animation() + # if self.last_serial_time and (now - self.last_serial_time > 10): + # self.random_animation() + + def loop_10(self): + # self.scan_vision() + # self.output_current_pose() + pass + + def loop_60(self): + self.animate_pose() - # If serial has been idle for more than 10 seconds, call random_animation() - if self.last_serial_time and (now - self.last_serial_time > 10): - self.random_animation() + def estimate_pose_from_knee(self): + """ Simplify by just checking leg_l_knee position against self.knee_pose_thresholds to determine if sitting, standing, or sitting on an edge. """ + knee_pose = None + if self.knee_pose_thresholds and self.servos['leg_l_knee']: + + try: + knee_pos = self.servos['leg_l_knee'].get_position() + if knee_pos <= self.knee_pose_thresholds['sitting_edge']: + knee_pose = 'sitting_edge' + elif knee_pos > self.knee_pose_thresholds['sitting_edge'] and knee_pos < self.knee_pose_thresholds['standing']: + knee_pose = 'sitting' + else: + knee_pose = 'standing' + self.log(f"Estimated pose: {knee_pose} based on knee position: {knee_pos}") + except Exception as e: + self.log(f"Error estimating pose from knee position: {e}", level='warning') + return knee_pose def estimate_current_pose(self): """ Identify current pose by matching servo positions to known poses in config. Do not require exact match of values, but 'close enough' check""" @@ -292,7 +405,9 @@ def estimate_current_pose(self): # print(f"Current positions: {current_pose}") try: for pose_name, pose_values in self.servos['leg_r_tilt'].poses.items(): - if all(abs(current_pose.get(servo_name, 0) - pose_value) < 100 for servo_name, pose_value in pose_values.items()): + #ignore pan and tilt servos + servos_to_check = {k: v for k, v in pose_values.items() if k not in ['neck_pan', 'neck_tilt']} + if all(abs(current_pose.get(servo_name, 0) - pose_value) < 100 for servo_name, pose_value in servos_to_check.items()): # print(f"Current pose is approximately: {pose_name}") self.pose = pose_name return pose_name @@ -448,7 +563,7 @@ def track_match(self, match): if not self.track_people_servos: return - track_threshold = [15, 15] # Minimum angle change to trigger servo movement + track_threshold = [15, 10] # Minimum angle change to trigger servo movement # Move pan servo based on center_pos_x position relative to camera_size pan_angle = int(((center_pos_x - (camera_size[0] / 2)) / (camera_size[0] / 2)) * 40) # Scale to -40 to 40 degrees diff --git a/src/modules/personality/tests/test_personality.py b/src/modules/personality/tests/test_personality.py index 4d0acd61..1759e1c9 100644 --- a/src/modules/personality/tests/test_personality.py +++ b/src/modules/personality/tests/test_personality.py @@ -9,7 +9,7 @@ def test_init_defaults(self): self.assertEqual(p.eye, 'blue') self.assertEqual(p.min_interval, 20) self.assertEqual(p.max_interval, 60) - self.assertEqual(p.balance_enabled, True) + self.assertEqual(p.balance_enabled, False) self.assertIsNotNone(p.next_action_time) def test_init_custom(self): diff --git a/src/modules/sensor/imu/bno055/bno055.py b/src/modules/sensor/imu/bno055/bno055.py index 6cdc1509..7ec3ab05 100644 --- a/src/modules/sensor/imu/bno055/bno055.py +++ b/src/modules/sensor/imu/bno055/bno055.py @@ -34,6 +34,12 @@ def __init__(self, **kwargs): def get_euler(self): return self.sensor.euler + def get_acceleration(self): + return self.sensor.acceleration + + def get_linear_acceleration(self): + return self.sensor.linear_acceleration + def _get_data(self): data = { 'temperature': self.sensor.temperature,