Servo feedback for physical robots (#389)

ci_group/revolve2/ci_group/morphological_measures.py

@@ -251,7 +251,6 @@ def __calculate_xz_symmetry(self) -> float:
     def __calculate_yz_symmetry(self) -> float:
         num_along_plane = 0
         num_symmetrical = 0
-        print(self.symmetry_grid.shape)
         for x, y, z in product(
             range(1, (self.bounding_box_depth - 1) // 2),
             range(self.bounding_box_width),

examples/physical_robot_remote/main.py

@@ -99,9 +99,9 @@ def main() -> None:
     print("Initializing robot..")
     run_remote(
         config=config,
-        hostname="localhost",
+        hostname="localhost",  # "Set the robot IP here.
         debug=True,
-        on_prepared=on_prepared,  # "Set the robot IP here.
+        on_prepared=on_prepared,
     )
 
 

modular_robot_physical/pyproject.toml

@@ -28,7 +28,7 @@ packages = [{ include = "revolve2" }]
 [tool.poetry.dependencies]
 python = "^3.10,<3.12"
 revolve2-modular-robot = "1.0.0rc1"
-revolve2-robohat = { version = "0.4.1", optional = true }
+revolve2-robohat = { version = "0.5.0", optional = true }
 typed-argparse = "^0.3.1"
 pycapnp = { version = "^2.0.0b2" }
 pigpio = { version = "^1.78", optional = true }

modular_robot_physical/revolve2/modular_robot_physical/physical_interfaces/_physical_interface.py

@@ -1,4 +1,5 @@
 from abc import ABC, abstractmethod
+from typing import Sequence
 
 
 class PhysicalInterface(ABC):
@@ -26,3 +27,22 @@ def disable(self) -> None:
 
         This disables all active modules and sensors.
         """
+
+    @abstractmethod
+    def get_battery_level(self) -> float:
+        """
+        Get the battery level.
+
+        :returns: The battery level as a number between 0.0 and 1.0.
+        :raises NotImplementedError: If getting the battery level is not supported on this hardware.
+        """
+
+    @abstractmethod
+    def get_multiple_servo_positions(self, pins: Sequence[int]) -> list[float]:
+        """
+        Get the current position of multiple servos.
+
+        :param pins: The GPIO pin numbers.
+        :returns: The current positions.
+        :raises NotImplementedError: If getting the servo position is not supported on this hardware.
+        """

modular_robot_physical/revolve2/modular_robot_physical/physical_interfaces/v1/_v1_physical_interface.py

@@ -1,5 +1,6 @@
 import math
 import time
+from typing import Sequence
 
 import pigpio
 
@@ -66,7 +67,8 @@ def enable(self) -> None:
             assert self._gpio is not None
             for pin in self._PINS:
                 self._gpio.set_PWM_dutycycle(pin, self._CENTER)
-                print(f"setting {pin}..")
+                if self._debug:
+                    print(f"setting {pin}..")
                 time.sleep(0.1)
 
     def disable(self) -> None:
@@ -82,3 +84,20 @@ def disable(self) -> None:
 
             for pin in self._PINS:
                 self._gpio.set_PWM_dutycycle(pin, 0)
+
+    def get_battery_level(self) -> float:
+        """
+        Get the battery level.
+
+        :raises NotImplementedError: If getting the battery level is not supported on this hardware.
+        """
+        raise NotImplementedError("Getting battery level not supported on v1 harware.")
+
+    def get_multiple_servo_positions(self, pins: Sequence[int]) -> list[float]:
+        """
+        Get the current position of multiple servos.
+
+        :param pins: The GPIO pin numbers.
+        :raises NotImplementedError: If getting the servo position is not supported on this hardware.
+        """
+        raise NotImplementedError("Getting servo position not supported on v1 harware.")

modular_robot_physical/revolve2/modular_robot_physical/physical_interfaces/v2/_v2_physical_interface.py

@@ -1,4 +1,5 @@
 import math
+from typing import Sequence
 
 from robohatlib.hal.assemblyboard.PwmPlug import PwmPlug
 from robohatlib.hal.assemblyboard.servo.ServoData import ServoData
@@ -98,7 +99,7 @@ def set_servo_targets(self, pins: list[int], targets: list[float]) -> None:
             angles = [90.0 + target / (2.0 * math.pi) * 360.0 for target in targets]
             for pin, angle in zip(pins, angles):
                 all_angles[pin] = angle
-            self._robohat.set_servo_multiple_angles(all_angles)
+            self._robohat.update_servo_data_direct(all_angles)
 
     def enable(self) -> None:
         """Start the robot."""
@@ -117,3 +118,21 @@ def disable(self) -> None:
             print("Putting servos to sleep.")
         if not self._dry:
             self._robohat.put_servo_to_sleep()
+
+    def get_battery_level(self) -> float:
+        """
+        Get the battery level.
+
+        :returns: The battery level as a number between 0.0 and 1.0.
+        """
+        return self._robohat.get_battery_percentage_capacity() / 100.0
+
+    def get_multiple_servo_positions(self, pins: Sequence[int]) -> list[float]:
+        """
+        Get the current position of multiple servos.
+
+        :param pins: The GPIO pin numbers.
+        :returns: The current positions.
+        """
+        angles = self._robohat.get_servo_multiple_angles()
+        return [(angles[pin] - 90) / 360.0 * math.pi * 2.0 for pin in pins]

modular_robot_physical/revolve2/modular_robot_physical/remote/_active_hinge_sensor_state_impl.py

@@ -0,0 +1,24 @@
+from revolve2.modular_robot.sensor_state import ActiveHingeSensorState
+
+
+class ActiveHingeSensorStateImpl(ActiveHingeSensorState):
+    """ActiveHingeSensorState implementation for physical robots."""
+
+    _position: float
+
+    def __init__(self, position: float) -> None:
+        """
+        Initialize this object.
+
+        :param position: The position of the active hinge.
+        """
+        self._position = position
+
+    @property
+    def position(self) -> float:
+        """
+        Get the measured position of the active hinge.
+
+        :returns: The measured position.
+        """
+        return self._position

modular_robot_physical/revolve2/modular_robot_physical/remote/_modular_robot_sensor_state_impl.py → modular_robot_physical/revolve2/modular_robot_physical/remote/_modular_robot_sensor_state_impl_v1.py

@@ -5,8 +5,8 @@
 )
 
 
-class ModularRobotSensorStateImpl(ModularRobotSensorState):
-    """Implementation of ModularRobotSensorState."""
+class ModularRobotSensorStateImplV1(ModularRobotSensorState):
+    """Implementation of ModularRobotSensorState for v1 robots."""
 
     def get_active_hinge_sensor_state(
         self, sensor: ActiveHingeSensor
@@ -15,8 +15,6 @@ def get_active_hinge_sensor_state(
         Get sensor states for Hinges.
 
         :param sensor: The sensor to query.
-        :returns: The Sensor State.
         :raises NotImplementedError: Always.
         """
-        raise NotImplementedError()
-        return ActiveHingeSensorState()
+        raise NotImplementedError("V1 hardware does not support sensor reading.")

modular_robot_physical/revolve2/modular_robot_physical/remote/_modular_robot_sensor_state_impl_v2.py

@@ -0,0 +1,42 @@
+from revolve2.modular_robot.body.base import ActiveHingeSensor
+from revolve2.modular_robot.sensor_state import (
+    ActiveHingeSensorState,
+    ModularRobotSensorState,
+)
+
+from .._uuid_key import UUIDKey
+from ._active_hinge_sensor_state_impl import ActiveHingeSensorStateImpl
+
+
+class ModularRobotSensorStateImplV2(ModularRobotSensorState):
+    """Implementation of ModularRobotSensorState for v2 robots."""
+
+    _hinge_sensor_mapping: dict[UUIDKey[ActiveHingeSensor], int]
+    _positions: dict[int, float]
+
+    def __init__(
+        self,
+        hinge_sensor_mapping: dict[UUIDKey[ActiveHingeSensor], int],
+        positions: dict[int, float],
+    ) -> None:
+        """
+        Initialize this object.
+
+        :param hinge_sensor_mapping: Mapping from active hinge sensors to pin ids.
+        :param positions: Position of hinges accessed by pin id.
+        """
+        self._hinge_sensor_mapping = hinge_sensor_mapping
+        self._positions = positions
+
+    def get_active_hinge_sensor_state(
+        self, sensor: ActiveHingeSensor
+    ) -> ActiveHingeSensorState:
+        """
+        Get sensor states for Hinges.
+
+        :param sensor: The sensor to query.
+        :returns: The Sensor State.
+        """
+        return ActiveHingeSensorStateImpl(
+            self._positions[self._hinge_sensor_mapping[UUIDKey(sensor)]]
+        )