diff --git a/ci_group/revolve2/ci_group/morphological_measures.py b/ci_group/revolve2/ci_group/morphological_measures.py index f08131e00..b46e3fa3c 100644 --- a/ci_group/revolve2/ci_group/morphological_measures.py +++ b/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), diff --git a/examples/physical_robot_remote/main.py b/examples/physical_robot_remote/main.py index db84fa81d..e4c9e54f0 100644 --- a/examples/physical_robot_remote/main.py +++ b/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, ) diff --git a/examples/run_physical_robot_manually/README.md b/examples/run_physical_robot_manually/README.md deleted file mode 100644 index eaf40939c..000000000 --- a/examples/run_physical_robot_manually/README.md +++ /dev/null @@ -1,21 +0,0 @@ -In this example, you will manually create and run a physical robot using SSH. You are most likely looking for the `physical_robot_remote` example instead, which applies most of the steps explained here automatically. - -You learn: -- How to create a configuration describing how a physical modular robot should be controlled. -- How to upload this configuration onto the robot. -- How to run the physical robot using this configuration. - -Requirements: -- A physical modular robot. Not whether you are using V1 or V2 hardware; If you do not know, it is probably V2. TODO in the next version of Revolve2 a guide will be added on how to build a physical robot. For now, ask the CI Group lab. -- The modular robot body and brain defined in a Revolve2 ModularRobot object. It is recommended that you test it in simulation first. The code in this example shows you how to create a ModularRobot as well. -- Understanding of simulation and other examples related to simulation will greatly help with your understanding of this example. - -Steps to take: -- Create a config pickle file as shown in `make_config_file.py`. -- Obtain the IP address of the core. If you are on the same network, we recommend using the `nmap` tool, if available on your system: `sudo nmap -sP `. For CI Group people: the core should be connected to ThymioNet. -- Copy the config file onto the core using `scp @:~/config.pickle`. The default user and password are `pi` and `raspberry`. -- SSH to the core using `ssh @`. -- Run the robot using the following command: `run_brain --config ~/config.pickle --hardware `, where `hardware type` is either `v1` or `v2`. -- To get all possible arguments for the previous command, see `run_brain --help` - -Congratulations! The robot should be moving now. diff --git a/examples/run_physical_robot_manually/make_config_file.py b/examples/run_physical_robot_manually/make_config_file.py deleted file mode 100644 index 5e1a6bb39..000000000 --- a/examples/run_physical_robot_manually/make_config_file.py +++ /dev/null @@ -1,95 +0,0 @@ -"""An example on how to make a config file for running a physical modular robot.""" - -import pickle - -from revolve2.experimentation.rng import make_rng_time_seed -from revolve2.modular_robot import ModularRobot -from revolve2.modular_robot.body import RightAngles -from revolve2.modular_robot.body.base import ActiveHinge -from revolve2.modular_robot.body.v1 import ActiveHingeV1, BodyV1, BrickV1 -from revolve2.modular_robot.brain.cpg import BrainCpgNetworkNeighborRandom -from revolve2.modular_robot_physical import Config, UUIDKey - - -def make_body() -> ( - tuple[BodyV1, tuple[ActiveHinge, ActiveHinge, ActiveHinge, ActiveHinge]] -): - """ - Create a body for the robot. - - :returns: The created body and a tuple of all ActiveHinge objects for mapping later on. - """ - # A modular robot body follows a 'tree' structure. - # The 'Body' class automatically creates a center 'core'. - # From here, other modular can be attached. - # Modules can be attached in a rotated fashion. - # This can be any angle, although the original design takes into account only multiples of 90 degrees. - body = BodyV1() - body.core_v1.left = ActiveHingeV1(RightAngles.DEG_0) - body.core_v1.left.attachment = ActiveHingeV1(RightAngles.DEG_0) - body.core_v1.left.attachment.attachment = BrickV1(RightAngles.DEG_0) - body.core_v1.right = ActiveHingeV1(RightAngles.DEG_0) - body.core_v1.right.attachment = ActiveHingeV1(RightAngles.DEG_0) - body.core_v1.right.attachment.attachment = BrickV1(RightAngles.DEG_0) - - """Here we collect all ActiveHinges, to map them later onto the physical robot.""" - active_hinges = ( - body.core_v1.left, - body.core_v1.left.attachment, - body.core_v1.right, - body.core_v1.right.attachment, - ) - return body, active_hinges - - -def main() -> None: - """Create a Config for the physical robot.""" - rng = make_rng_time_seed() - # Create a modular robot, similar to what was done in the simulate_single_robot example. Of course, you can replace this with your own robot, such as one you have optimized using an evolutionary algorithm. - body, hinges = make_body() - brain = BrainCpgNetworkNeighborRandom(body=body, rng=rng) - robot = ModularRobot(body, brain) - - """ - Some important notes to understand: - - Hinge mappings are specific to each robot, so they have to be created new for each type of body. - - The pin`s id`s can be found on th physical robots HAT. - - The order of the pin`s is crucial for a correct translation into the physical robot. - - Each ActiveHinge needs one corresponding pin to be able to move. - - If the mapping is faulty check the simulators behavior versus the physical behavior and adjust the mapping iteratively. - - For a concrete implementation look at the following example of mapping the robots`s hinges: - """ - hinge_1, hinge_2, hinge_3, hinge_4 = hinges - hinge_mapping = { - UUIDKey(hinge_1): 6, - UUIDKey(hinge_2): 12, - UUIDKey(hinge_3): 13, - UUIDKey(hinge_4): 16, - } - - """ - A configuration consists of the follow parameters: - - modular_robot: The ModularRobot object, exactly as you would use it in simulation. - - hinge_mapping: This maps active hinges to GPIO pins on the physical modular robot core. TODO in the next version we will add a guide to the documentation on how to create a modular robot body. For now, ask the CI Group lab. - - run_duration: How long to run the robot for in seconds. - - control_frequency: Frequency at which to call the brain control functions in seconds. If you also ran the robot in simulation, this must match your setting there. - - initial_hinge_positions: Initial positions for the active hinges. In Revolve2 the simulator defaults to 0.0. - - inverse_servos: Sometimes servos on the physical robot are mounted backwards by accident. Here you inverse specific servos in software. Example: {13: True} would inverse the servo connected to GPIO pin 13. - """ - config = Config( - modular_robot=robot, - hinge_mapping=hinge_mapping, - run_duration=30, - control_frequency=20, - initial_hinge_positions={UUIDKey(active_hinge): 0.0 for active_hinge in hinges}, - inverse_servos={}, - ) - - # Serialize the configuration object and save it to a file. This file will later be read by the modular robot core. - with open("config.pickle", "wb") as f: - pickle.dump(config, f) - - -if __name__ == "__main__": - main() diff --git a/examples/run_physical_robot_manually/requirements.txt b/examples/run_physical_robot_manually/requirements.txt deleted file mode 100644 index e69de29bb..000000000 diff --git a/modular_robot_physical/pyproject.toml b/modular_robot_physical/pyproject.toml index 7ba7b340d..974b33f37 100644 --- a/modular_robot_physical/pyproject.toml +++ b/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 } diff --git a/modular_robot_physical/revolve2/modular_robot_physical/physical_interfaces/_physical_interface.py b/modular_robot_physical/revolve2/modular_robot_physical/physical_interfaces/_physical_interface.py index b1ca9e774..ab76f7682 100644 --- a/modular_robot_physical/revolve2/modular_robot_physical/physical_interfaces/_physical_interface.py +++ b/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. + """ diff --git a/modular_robot_physical/revolve2/modular_robot_physical/physical_interfaces/v1/_v1_physical_interface.py b/modular_robot_physical/revolve2/modular_robot_physical/physical_interfaces/v1/_v1_physical_interface.py index 06d7dde75..d483d9b9a 100644 --- a/modular_robot_physical/revolve2/modular_robot_physical/physical_interfaces/v1/_v1_physical_interface.py +++ b/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.") diff --git a/modular_robot_physical/revolve2/modular_robot_physical/physical_interfaces/v2/_v2_physical_interface.py b/modular_robot_physical/revolve2/modular_robot_physical/physical_interfaces/v2/_v2_physical_interface.py index 4776dcc4d..1ced1b57e 100644 --- a/modular_robot_physical/revolve2/modular_robot_physical/physical_interfaces/v2/_v2_physical_interface.py +++ b/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] diff --git a/modular_robot_physical/revolve2/modular_robot_physical/remote/_active_hinge_sensor_state_impl.py b/modular_robot_physical/revolve2/modular_robot_physical/remote/_active_hinge_sensor_state_impl.py new file mode 100644 index 000000000..7f60d9529 --- /dev/null +++ b/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 diff --git a/modular_robot_physical/revolve2/modular_robot_physical/remote/_modular_robot_sensor_state_impl.py b/modular_robot_physical/revolve2/modular_robot_physical/remote/_modular_robot_sensor_state_impl_v1.py similarity index 66% rename from modular_robot_physical/revolve2/modular_robot_physical/remote/_modular_robot_sensor_state_impl.py rename to modular_robot_physical/revolve2/modular_robot_physical/remote/_modular_robot_sensor_state_impl_v1.py index 2f5dd7750..e073cbdcf 100644 --- a/modular_robot_physical/revolve2/modular_robot_physical/remote/_modular_robot_sensor_state_impl.py +++ b/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.") diff --git a/modular_robot_physical/revolve2/modular_robot_physical/remote/_modular_robot_sensor_state_impl_v2.py b/modular_robot_physical/revolve2/modular_robot_physical/remote/_modular_robot_sensor_state_impl_v2.py new file mode 100644 index 000000000..c90ddd6e6 --- /dev/null +++ b/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)]] + ) diff --git a/modular_robot_physical/revolve2/modular_robot_physical/remote/_remote.py b/modular_robot_physical/revolve2/modular_robot_physical/remote/_remote.py index e6524bac4..41bd22248 100644 --- a/modular_robot_physical/revolve2/modular_robot_physical/remote/_remote.py +++ b/modular_robot_physical/revolve2/modular_robot_physical/remote/_remote.py @@ -5,14 +5,17 @@ import capnp from revolve2.modular_robot.body.base import ActiveHinge +from revolve2.modular_robot.sensor_state import ModularRobotSensorState from .._config import Config +from .._hardware_type import HardwareType from .._protocol_version import PROTOCOL_VERSION from .._standard_port import STANDARD_PORT from .._uuid_key import UUIDKey from ..robot_daemon_api import robot_daemon_protocol_capnp from ._modular_robot_control_interface_impl import ModularRobotControlInterfaceImpl -from ._modular_robot_sensor_state_impl import ModularRobotSensorStateImpl +from ._modular_robot_sensor_state_impl_v1 import ModularRobotSensorStateImplV1 +from ._modular_robot_sensor_state_impl_v2 import ModularRobotSensorStateImplV2 def _active_hinge_targets_to_pin_controls( @@ -41,6 +44,13 @@ async def _run_remote_impl( port: int, debug: bool, ) -> None: + active_hinge_sensor_to_pin = { + UUIDKey(key.value.sensor): pin + for key, pin in config.hinge_mapping.items() + if key.value.sensor + if not None + } + # Make controller controller = config.modular_robot.brain.make_instance() @@ -57,11 +67,20 @@ async def _run_remote_impl( # Setup the robot and check protocol version setup_response: robot_daemon_protocol_capnp.SetupResponse = ( await service.setup( - robot_daemon_protocol_capnp.Setupargs(version=PROTOCOL_VERSION) + robot_daemon_protocol_capnp.SetupArgs( + version=PROTOCOL_VERSION, activePins=[x for x in range(32)] + ) ) ).response if not setup_response.versionOk: raise RuntimeError("Protocol version does not match for robot.") + match setup_response.hardwareType: + case "v1": + hardware_type = HardwareType.v1 + case "v2": + hardware_type = HardwareType.v2 + case _: + raise NotImplementedError() # Set hinges to initial positions. pin_controls = _active_hinge_targets_to_pin_controls( @@ -71,9 +90,20 @@ async def _run_remote_impl( for active_hinge in config.hinge_mapping ], ) - await service.control( - robot_daemon_protocol_capnp.ControlCommands(pins=pin_controls) - ) + match hardware_type: + case HardwareType.v1: + await service.control( + robot_daemon_protocol_capnp.ControlArgs(setPins=pin_controls) + ) + case HardwareType.v2: + sensor_readings: robot_daemon_protocol_capnp.SensorReadings = ( + await service.controlAndReadSensors( + robot_daemon_protocol_capnp.ControlAndReadSensorsArgs( + setPins=pin_controls, readPins=[] + ) + ) + ).response + print(f"Battery level is at {sensor_readings.battery*100.0}%.") # Fire prepared callback on_prepared() @@ -84,11 +114,35 @@ async def _run_remote_impl( start_time = time.time() last_update_time = start_time + battery_print_timer = 0.0 + + sensor_state: ModularRobotSensorState + + # Get initial sensor state + match hardware_type: + case HardwareType.v1: + sensor_state = ModularRobotSensorStateImplV1() + case HardwareType.v2: + pins = [pin for pin in active_hinge_sensor_to_pin.values()] + sensor_readings = ( + await service.readSensors( + robot_daemon_protocol_capnp.ReadSensorsArgs(readPins=pins) + ) + ).response + sensor_state = ModularRobotSensorStateImplV2( + hinge_sensor_mapping=active_hinge_sensor_to_pin, + positions={ + pin: position for pin, position in zip(pins, sensor_readings.pins) + }, + ) + case _: + raise NotImplementedError("Hardware type not supported.") + while (current_time := time.time()) - start_time < config.run_duration: # Sleep until next control update next_update_at = last_update_time + control_period if current_time < next_update_at: - time.sleep(next_update_at - current_time) + await asyncio.sleep(next_update_at - current_time) last_update_time = next_update_at elapsed_time = control_period else: @@ -100,20 +154,47 @@ async def _run_remote_impl( # Get targets from brain control_interface = ModularRobotControlInterfaceImpl() - sensor_state = ModularRobotSensorStateImpl() controller.control( elapsed_time, sensor_state=sensor_state, control_interface=control_interface, ) + # Increase the timer for the battery value message + battery_print_timer += elapsed_time + # Reading sensors will come in a later update. pin_controls = _active_hinge_targets_to_pin_controls( config, control_interface._set_active_hinges ) - await service.control( - robot_daemon_protocol_capnp.ControlCommands(pins=pin_controls) - ) + match hardware_type: + case HardwareType.v1: + await service.control( + robot_daemon_protocol_capnp.ControlArgs(setPins=pin_controls) + ) + sensor_state = ModularRobotSensorStateImplV1() + case HardwareType.v2: + pins = [pin for pin in active_hinge_sensor_to_pin.values()] + sensor_readings = ( + await service.controlAndReadSensors( + robot_daemon_protocol_capnp.ControlAndReadSensorsArgs( + setPins=pin_controls, readPins=pins + ) + ) + ).response + sensor_state = ModularRobotSensorStateImplV2( + hinge_sensor_mapping=active_hinge_sensor_to_pin, + positions={ + pin: position + for pin, position in zip(pins, sensor_readings.pins) + }, + ) + + if battery_print_timer > 5.0: + print(f"Battery level is at {sensor_readings.battery*100.0}%.") + battery_print_timer = 0.0 + case _: + raise NotImplementedError("Hardware type not supported.") def run_remote( diff --git a/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon/_robo_server_impl.py b/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon/_robo_server_impl.py index e4ecd255e..59c5ae1e7 100644 --- a/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon/_robo_server_impl.py +++ b/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon/_robo_server_impl.py @@ -1,7 +1,8 @@ import threading import time -from typing import Any +from typing import Any, Sequence +from .._hardware_type import HardwareType from .._protocol_version import PROTOCOL_VERSION from ..physical_interfaces import PhysicalInterface from ..robot_daemon_api import robot_daemon_protocol_capnp @@ -13,24 +14,40 @@ class RoboServerImpl(robot_daemon_protocol_capnp.RoboServer.Server): # type: ig _CAREFUL_STEP = 0.1 _debug: bool + _hardware_type: HardwareType _physical_interface: PhysicalInterface + _active_pins: list[int] | None + _enabled: bool _update_loop_thread: threading.Thread + _lock: threading.Lock + _targets: dict[int, float] # pin -> target _current_targets: dict[int, float] # pin -> target - _targets_lock: threading.Lock - def __init__(self, debug: bool, physical_interface: PhysicalInterface) -> None: + _measured_hinge_positions: dict[int, float] # pin -> position + _battery: float + + def __init__( + self, + debug: bool, + hardware_type: HardwareType, + physical_interface: PhysicalInterface, + ) -> None: """ Initialize this object. :param debug: Enable debug messages. + :param hardware_type: The type of hardware this runs on. :param physical_interface: Interface to the actual robot. """ self._debug = debug + self._hardware_type = hardware_type self._physical_interface = physical_interface + self._active_pins = None + if self._debug: print("client connected") @@ -38,13 +55,14 @@ def __init__(self, debug: bool, physical_interface: PhysicalInterface) -> None: self._targets = {} self._current_targets = {} - self._targets_lock = threading.Lock() + self._lock = threading.Lock() - self._enabled = True - self._update_loop_thread = threading.Thread(target=self._update_loop) - self._update_loop_thread.start() + self._measured_hinge_positions = {} + self._battery = 0.0 def _update_loop(self) -> None: + assert self._active_pins is not None + while self._enabled: desired_pins = [] desired_targets = [] @@ -52,7 +70,7 @@ def _update_loop(self) -> None: # copy out of the set. # that is fast, setting the actual targets is slow. # lock for as short as possible. - with self._targets_lock: + with self._lock: for pin, target in self._targets.items(): desired_pins.append(pin) desired_targets.append(target) @@ -81,10 +99,24 @@ def _update_loop(self) -> None: self._physical_interface.set_servo_targets(pins, targets) + # Read measured hinge positions + if self._hardware_type is not HardwareType.v1: + hinge_positions = self._physical_interface.get_multiple_servo_positions( + self._active_pins + ) + + with self._lock: + for pin, position in zip(self._active_pins, hinge_positions): + self._measured_hinge_positions[pin] = position + + battery = self._physical_interface.get_battery_level() + with self._lock: + self._battery = battery + time.sleep(1 / 60) def _queue_servo_targets(self, pins: list[int], targets: list[float]) -> None: - with self._targets_lock: + with self._lock: for pin, target in zip(pins, targets): self._targets[pin] = target @@ -102,42 +134,57 @@ def cleanup(self) -> None: async def setup( self, - setupargs: robot_daemon_protocol_capnp.Setupargs, + args: robot_daemon_protocol_capnp.SetupArgs, _context: Any, ) -> robot_daemon_protocol_capnp.SetupResponse: """ Handle a setup command. - :param setupargs: Arguments to the setup process. + :param args: Arguments to the setup process. :returns: Whether the setup was successful. """ if self._debug: print("setup") + with self._lock: + self._active_pins = [pin for pin in args.activePins] + + self._enabled = True + self._update_loop_thread = threading.Thread(target=self._update_loop) + self._update_loop_thread.start() + + hardware_type_str: robot_daemon_protocol_capnp.HardwareType + match self._hardware_type: + case HardwareType.v1: + hardware_type_str = "v1" + case HardwareType.v2: + hardware_type_str = "v2" return robot_daemon_protocol_capnp.SetupResponse( - versionOk=(setupargs.version == PROTOCOL_VERSION) + versionOk=(args.version == PROTOCOL_VERSION), + hardwareType=hardware_type_str, ) async def control( self, - commands: robot_daemon_protocol_capnp.ControlCommandsReader, + args: robot_daemon_protocol_capnp.ControlArgsReader, _context: Any, ) -> None: """ Handle control commands. - :param commands: The commands to perform. + :param args: Args to the function. """ if self._debug: print("control") self._queue_servo_targets( - [pin_control.pin for pin_control in commands.pins], - [pin_control.target for pin_control in commands.pins], + [pin_control.pin for pin_control in args.setPins], + [pin_control.target for pin_control in args.setPins], ) async def readSensors( self, + args: robot_daemon_protocol_capnp.ReadSensorsArgsReader, _context: Any, ) -> robot_daemon_protocol_capnp.SensorReadings: """ @@ -145,16 +192,17 @@ async def readSensors( Stub that currently does reads nothing. + :param args: Args to the function. :returns: The readings. """ if self._debug: print("read_sensors") - return robot_daemon_protocol_capnp.SensorReadings() + return self._get_sensor_readings(args.readPins) - def controlAndReadSensors( + async def controlAndReadSensors( self, - commands: robot_daemon_protocol_capnp.ControlCommandsReader, + args: robot_daemon_protocol_capnp.ControlAndReadSensorsArgsReader, _context: Any, ) -> robot_daemon_protocol_capnp.SensorReadings: """ @@ -162,15 +210,39 @@ def controlAndReadSensors( Currently reads nothing. - :param commands: The commands to perform. + :param args: Args to the function. :returns: The readings. """ if self._debug: print("control_and_read_sensors") self._queue_servo_targets( - [pin_control.pin for pin_control in commands.pins], - [pin_control.target for pin_control in commands.pins], + [pin_control.pin for pin_control in args.setPins], + [pin_control.target for pin_control in args.setPins], ) - return robot_daemon_protocol_capnp.SensorReadings() + return self._get_sensor_readings(args.readPins) + + def _get_sensor_readings( + self, pins: Sequence[int] + ) -> robot_daemon_protocol_capnp.SensorReadings: + if self._active_pins is None: + raise RuntimeError("setup must be called first.") + + pins_readings: list[float] = [] + with self._lock: + for pin in pins: + if pin not in self._active_pins: + raise ValueError( + "Position requested for pin that was not flagged as active during setup." + ) + value = self._measured_hinge_positions.get(pin) + if value is None: + value = 0.0 + pins_readings.append(value) + + battery = self._physical_interface.get_battery_level() + + return robot_daemon_protocol_capnp.SensorReadings( + pins=pins_readings, battery=battery + ) diff --git a/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon/_robot_daemon.py b/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon/_robot_daemon.py index cfd6f23a4..961e3f3f3 100644 --- a/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon/_robot_daemon.py +++ b/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon/_robot_daemon.py @@ -16,18 +16,21 @@ class _Program: _dry: bool _has_client: bool + _hardware_type: HardwareType _physical_interface: PhysicalInterface - def __init__(self, debug: bool, dry: bool) -> None: + def __init__(self, debug: bool, dry: bool, hardware_type: HardwareType) -> None: """ Initialize this object. :param debug: Enable debug messages. :param dry: Run in dry mode, not writing/reading hardware. + :param hardware_type: The type of hardware this runs on. """ self._debug = debug self._dry = dry self._has_client = False + self._hardware_type = hardware_type async def _new_connection(self, stream: Any) -> None: """ @@ -45,21 +48,19 @@ async def _new_connection(self, stream: Any) -> None: self._has_client = True impl = RoboServerImpl( - debug=self._debug, physical_interface=self._physical_interface + debug=self._debug, + hardware_type=self._hardware_type, + physical_interface=self._physical_interface, ) await capnp.TwoPartyServer(stream, bootstrap=impl).on_disconnect() impl.cleanup() self._has_client = False - async def run(self, hardware_type: HardwareType) -> None: - """ - Run the program. - - :param hardware_type: The type of hardware this runs on. - """ + async def run(self) -> None: + """Run the program.""" self._physical_interface = get_interface( - hardware_type=hardware_type, debug=self._debug, dry=self._dry + hardware_type=self._hardware_type, debug=self._debug, dry=self._dry ) server = await capnp.AsyncIoStream.create_server( @@ -78,5 +79,5 @@ def run_robot_daemon(debug: bool, dry: bool, hardware_type: HardwareType) -> Non :param hardware_type: The type of hardware this runs on. """ asyncio.run( - capnp.run(_Program(debug=debug, dry=dry).run(hardware_type=hardware_type)) + capnp.run(_Program(debug=debug, dry=dry, hardware_type=hardware_type).run()) ) diff --git a/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol.capnp b/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol.capnp index 8e0d748b5..1577824b6 100644 --- a/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol.capnp +++ b/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol.capnp @@ -1,11 +1,18 @@ @0xd4d97e2cea03a846; # unique file ID, generated by `capnp id` -struct Setupargs { +struct SetupArgs { version @0 :Text; + activePins @1 :List(Int32); +} + +enum HardwareType { + v1 @0; + v2 @1; } struct SetupResponse { versionOk @0 :Bool; + hardwareType @1 :HardwareType; } struct PinControl { @@ -13,17 +20,27 @@ struct PinControl { target @1 :Float32; } -struct ControlCommands { - pins @0 :List(PinControl); +struct ControlArgs { + setPins @0 :List(PinControl); } -struct SensorReadings { +struct ReadSensorsArgs { + readPins @0 :List(Int32); +} +struct ControlAndReadSensorsArgs { + setPins @0 :List(PinControl); + readPins @1 :List(Int32); +} + +struct SensorReadings { + pins @0 :List(Float32); + battery @1 :Float32; } interface RoboServer { - setup @0 (setupargs :Setupargs) -> (response :SetupResponse); - control @1 (commands :ControlCommands); - readSensors @2 () -> (response :SensorReadings); - controlAndReadSensors @3 (commands :ControlCommands) -> (response :SensorReadings); + setup @0 (args :SetupArgs) -> (response :SetupResponse); + control @1 (args :ControlArgs); + readSensors @2 (args :ReadSensorsArgs) -> (response :SensorReadings); + controlAndReadSensors @3 (args :ControlAndReadSensorsArgs) -> (response :SensorReadings); } diff --git a/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol_capnp.py b/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol_capnp.py index 89b37cbbb..49245fd20 100644 --- a/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol_capnp.py +++ b/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol_capnp.py @@ -10,18 +10,24 @@ capnp.remove_import_hook() here = os.path.dirname(os.path.abspath(__file__)) module_file = os.path.abspath(os.path.join(here, "robot_daemon_protocol.capnp")) -Setupargs = capnp.load(module_file).Setupargs -SetupargsBuilder = Setupargs -SetupargsReader = Setupargs +SetupArgs = capnp.load(module_file).SetupArgs +SetupArgsBuilder = SetupArgs +SetupaArgsReader = SetupArgs SetupResponse = capnp.load(module_file).SetupResponse SetupResponseBuilder = SetupResponse SetupResponseReader = SetupResponse PinControl = capnp.load(module_file).PinControl PinControlBuilder = PinControl PinControlReader = PinControl -ControlCommands = capnp.load(module_file).ControlCommands -ControlCommandsBuilder = ControlCommands -ControlCommandsReader = ControlCommands +ControlArgs = capnp.load(module_file).ControlArgs +ControlArgsBuilder = ControlArgs +ControlArgsReader = ControlArgs +ReadSensorsArgs = capnp.load(module_file).ReadSensorsArgs +ReadSensorsArgsBuilder = ReadSensorsArgs +ReadSensorsArgsReader = ReadSensorsArgs +ControlAndReadSensorsArgs = capnp.load(module_file).ControlAndReadSensorsArgs +ControlAndReadSensorsArgsBuilder = ControlAndReadSensorsArgs +ControlAndReadSensorsArgsReader = ControlAndReadSensorsArgs SensorReadings = capnp.load(module_file).SensorReadings SensorReadingsBuilder = SensorReadings SensorReadingsReader = SensorReadings diff --git a/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol_capnp.pyi b/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol_capnp.pyi index 97e9797ba..9a2caf59b 100644 --- a/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol_capnp.pyi +++ b/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol_capnp.pyi @@ -13,47 +13,55 @@ from __future__ import annotations from contextlib import contextmanager from io import BufferedWriter -from typing import Any, Iterator, Sequence +from typing import Any, Iterator, Literal, Sequence -class Setupargs: +class SetupArgs: version: str - def __init__(self, version: str) -> None: ... + activePins: Sequence[int] + def __init__( + self, + version: str, + activePins: Sequence[int], + ) -> None: ... @staticmethod @contextmanager def from_bytes( data: bytes, traversal_limit_in_words: int | None = ..., nesting_limit: int | None = ..., - ) -> Iterator[SetupargsReader]: ... + ) -> Iterator[SetupArgsReader]: ... @staticmethod def from_bytes_packed( data: bytes, traversal_limit_in_words: int | None = ..., nesting_limit: int | None = ..., - ) -> SetupargsReader: ... + ) -> SetupArgsReader: ... @staticmethod - def new_message() -> SetupargsBuilder: ... + def new_message() -> SetupArgsBuilder: ... def to_dict(self) -> dict[Any, Any]: ... -class SetupargsReader(Setupargs): - def as_builder(self) -> SetupargsBuilder: ... +class SetupArgsReader(SetupArgs): + def as_builder(self) -> SetupArgsBuilder: ... -class SetupargsBuilder(Setupargs): +class SetupArgsBuilder(SetupArgs): @staticmethod - def from_dict(dictionary: dict[Any, Any]) -> SetupargsBuilder: ... - def copy(self) -> SetupargsBuilder: ... + def from_dict(dictionary: dict[Any, Any]) -> SetupArgsBuilder: ... + def copy(self) -> SetupArgsBuilder: ... def to_bytes(self) -> bytes: ... def to_bytes_packed(self) -> bytes: ... def to_segments(self) -> list[bytes]: ... - def as_reader(self) -> SetupargsReader: ... + def as_reader(self) -> SetupArgsReader: ... @staticmethod def write(file: BufferedWriter) -> None: ... @staticmethod def write_packed(file: BufferedWriter) -> None: ... +HardwareType = Literal["v1", "v2"] + class SetupResponse: versionOk: bool - def __init__(self, versionOk: bool) -> None: ... + hardwareType: HardwareType + def __init__(self, versionOk: bool, hardwareType: HardwareType) -> None: ... @staticmethod @contextmanager def from_bytes( @@ -124,11 +132,89 @@ class PinControlBuilder(PinControl): @staticmethod def write_packed(file: BufferedWriter) -> None: ... -class ControlCommands: - pins: Sequence[PinControl | PinControlBuilder | PinControlReader] +class ControlArgs: + setPins: Sequence[PinControl | PinControlBuilder | PinControlReader] + def __init__( + self, setPins: Sequence[PinControl | PinControlBuilder | PinControlReader] + ) -> None: ... + @staticmethod + @contextmanager + def from_bytes( + data: bytes, + traversal_limit_in_words: int | None = ..., + nesting_limit: int | None = ..., + ) -> Iterator[ControlArgsReader]: ... + @staticmethod + def from_bytes_packed( + data: bytes, + traversal_limit_in_words: int | None = ..., + nesting_limit: int | None = ..., + ) -> ControlArgsReader: ... + @staticmethod + def new_message() -> ControlArgsBuilder: ... + def to_dict(self) -> dict[Any, Any]: ... + +class ControlArgsReader(ControlArgs): + setPins: Sequence[PinControlReader] + def as_builder(self) -> ControlArgsBuilder: ... + +class ControlArgsBuilder(ControlArgs): + setPins: Sequence[PinControl | PinControlBuilder | PinControlReader] + @staticmethod + def from_dict(dictionary: dict[Any, Any]) -> ControlArgsBuilder: ... + def copy(self) -> ControlArgsBuilder: ... + def to_bytes(self) -> bytes: ... + def to_bytes_packed(self) -> bytes: ... + def to_segments(self) -> list[bytes]: ... + def as_reader(self) -> ControlArgsReader: ... + @staticmethod + def write(file: BufferedWriter) -> None: ... + @staticmethod + def write_packed(file: BufferedWriter) -> None: ... + +class ReadSensorsArgs: + readPins: Sequence[int] + def __init__(self, readPins: Sequence[int]) -> None: ... + @staticmethod + @contextmanager + def from_bytes( + data: bytes, + traversal_limit_in_words: int | None = ..., + nesting_limit: int | None = ..., + ) -> Iterator[ReadSensorsArgsReader]: ... + @staticmethod + def from_bytes_packed( + data: bytes, + traversal_limit_in_words: int | None = ..., + nesting_limit: int | None = ..., + ) -> ReadSensorsArgsReader: ... + @staticmethod + def new_message() -> ReadSensorsArgsBuilder: ... + def to_dict(self) -> dict[Any, Any]: ... + +class ReadSensorsArgsReader(ReadSensorsArgs): + def as_builder(self) -> ReadSensorsArgsBuilder: ... + +class ReadSensorsArgsBuilder(ReadSensorsArgs): + @staticmethod + def from_dict(dictionary: dict[Any, Any]) -> ReadSensorsArgsBuilder: ... + def copy(self) -> ReadSensorsArgsBuilder: ... + def to_bytes(self) -> bytes: ... + def to_bytes_packed(self) -> bytes: ... + def to_segments(self) -> list[bytes]: ... + def as_reader(self) -> ReadSensorsArgsReader: ... + @staticmethod + def write(file: BufferedWriter) -> None: ... + @staticmethod + def write_packed(file: BufferedWriter) -> None: ... + +class ControlAndReadSensorsArgs: + setPins: Sequence[PinControl | PinControlBuilder | PinControlReader] + readPins: Sequence[int] def __init__( self, - pins: Sequence[PinControl | PinControlBuilder | PinControlReader], + setPins: Sequence[PinControl | PinControlBuilder | PinControlReader], + readPins: Sequence[int], ) -> None: ... @staticmethod @contextmanager @@ -136,36 +222,39 @@ class ControlCommands: data: bytes, traversal_limit_in_words: int | None = ..., nesting_limit: int | None = ..., - ) -> Iterator[ControlCommandsReader]: ... + ) -> Iterator[ControlAndReadSensorsArgsReader]: ... @staticmethod def from_bytes_packed( data: bytes, traversal_limit_in_words: int | None = ..., nesting_limit: int | None = ..., - ) -> ControlCommandsReader: ... + ) -> ControlAndReadSensorsArgsReader: ... @staticmethod - def new_message() -> ControlCommandsBuilder: ... + def new_message() -> ControlAndReadSensorsArgsBuilder: ... def to_dict(self) -> dict[Any, Any]: ... -class ControlCommandsReader(ControlCommands): - pins: Sequence[PinControlReader] - def as_builder(self) -> ControlCommandsBuilder: ... +class ControlAndReadSensorsArgsReader(ControlAndReadSensorsArgs): + setPins: Sequence[PinControlReader] + def as_builder(self) -> ControlAndReadSensorsArgsBuilder: ... -class ControlCommandsBuilder(ControlCommands): - pins: Sequence[PinControl | PinControlBuilder | PinControlReader] +class ControlAndReadSensorsArgsBuilder(ControlAndReadSensorsArgs): + setPins: Sequence[PinControl | PinControlBuilder | PinControlReader] @staticmethod - def from_dict(dictionary: dict[Any, Any]) -> ControlCommandsBuilder: ... - def copy(self) -> ControlCommandsBuilder: ... + def from_dict(dictionary: dict[Any, Any]) -> ControlAndReadSensorsArgsBuilder: ... + def copy(self) -> ControlAndReadSensorsArgsBuilder: ... def to_bytes(self) -> bytes: ... def to_bytes_packed(self) -> bytes: ... def to_segments(self) -> list[bytes]: ... - def as_reader(self) -> ControlCommandsReader: ... + def as_reader(self) -> ControlAndReadSensorsArgsReader: ... @staticmethod def write(file: BufferedWriter) -> None: ... @staticmethod def write_packed(file: BufferedWriter) -> None: ... class SensorReadings: + pins: Sequence[float] + battery: float + def __init__(self, pins: Sequence[float], battery: float) -> None: ... @staticmethod @contextmanager def from_bytes( diff --git a/project.yml b/project.yml index dd3f79925..305fd7805 100644 --- a/project.yml +++ b/project.yml @@ -22,7 +22,6 @@ examples: - robot_bodybrain_ea_database - robot_brain_cmaes - robot_brain_cmaes_database - - run_physical_robot_manually - simple_ea_xor - simple_ea_xor_database - simulate_single_robot