diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md new file mode 100644 index 000000000..d5ebe7ca2 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -0,0 +1,31 @@ +--- +name: Bug report +about: Create a report to help us improve Revolve2 +title: "[BUG]" +labels: bug +assignees: '' + +--- + +**Describe the bug** +A clear and concise description of what the bug is. + +**To Reproduce** +Steps to reproduce the behavior: +1. Go to '...' +2. Click on '....' +3. Scroll down to '....' +4. See error + +**Expected behavior** +A clear and concise description of what you expected to happen. + +**Screenshots** +If applicable, add screenshots to help explain your problem. + +**Platform:** + - OS: [e.g. Windows, Linux, MacOS] + - Python Version + +**Additional context** +Add any other context about the problem here. diff --git a/.github/ISSUE_TEMPLATE/feature_request.md b/.github/ISSUE_TEMPLATE/feature_request.md new file mode 100644 index 000000000..feae27f5a --- /dev/null +++ b/.github/ISSUE_TEMPLATE/feature_request.md @@ -0,0 +1,21 @@ +--- +name: Feature request +about: Suggest an idea for Revolve2 +title: "[FEATURE]" +labels: enhancement +assignees: '' + +--- + +**Is your feature request related to a problem? Please describe.** +A clear and concise description of what the problem is. + +**Describe the solution you'd like** +A clear and concise description of what you want to happen, and in which package this solution should be. + +**Describe alternatives you've considered** +A clear and concise description of any alternative solutions or features you've considered. +Also mention your current workarounds if applicable. + +**Additional context** +Add any other context or screenshots about the feature request here. diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 000000000..b3a4f9991 --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,6 @@ + + +### Contributing guide + +If you intend to contribute you may find this guide helpful: [Revolve2 Contributing Guide](https://ci-group.github.io/revolve2/contributing_guide/index.html).
+Contributions are highly appreciated. \ No newline at end of file diff --git a/ci_group/pyproject.toml b/ci_group/pyproject.toml index 0b52b9755..b96bc821e 100644 --- a/ci_group/pyproject.toml +++ b/ci_group/pyproject.toml @@ -9,12 +9,12 @@ build-backend = "poetry.core.masonry.api" [tool.poetry] name = "revolve2-ci-group" -version = "1.0.1" +version = "1.0.2" description = "Revolve2: Computational Intelligence Group experimentation tools and standards." readme = "../README.md" authors = [ "Aart Stuurman ", - "Oliver Weissl ", + "Oliver Weissl ", ] repository = "https://github.com/ci-group/revolve2" classifiers = [ @@ -38,13 +38,15 @@ script = "revolve2/ci_group/morphological_novelty_metric/_build_cmodule.py" [tool.poetry.dependencies] python = "^3.10,<3.12" -revolve2-modular-robot-simulation = "1.0.1" +revolve2-modular-robot-simulation = "1.0.2" noise = "^1.2.2" multineat = "^0.12" sqlalchemy = "^2.0.0" numpy = "^1.21.2" Cython = "^3.0.4" setuptools = "^68.2.2" +opencv-contrib-python = "^4.9.0.80" +opencv-python = "^4.9.0.80" [tool.poetry.extras] dev = [] diff --git a/ci_group/revolve2/ci_group/ci_lab_utilities/__init__.py b/ci_group/revolve2/ci_group/ci_lab_utilities/__init__.py new file mode 100644 index 000000000..000d15f45 --- /dev/null +++ b/ci_group/revolve2/ci_group/ci_lab_utilities/__init__.py @@ -0,0 +1,5 @@ +"""Utility functions for the CI-group lab.""" +from ._calibrate_camera import calibrate_camera +from ._ip_camera import IPCamera + +__all__ = ["IPCamera", "calibrate_camera"] diff --git a/ci_group/revolve2/ci_group/ci_lab_utilities/_calibrate_camera.py b/ci_group/revolve2/ci_group/ci_lab_utilities/_calibrate_camera.py new file mode 100644 index 000000000..191534762 --- /dev/null +++ b/ci_group/revolve2/ci_group/ci_lab_utilities/_calibrate_camera.py @@ -0,0 +1,83 @@ +import cv2 +import numpy as np +from numpy.typing import NDArray + + +def calibrate_camera( + calibration_images_paths: list[str], checkerboard_size: tuple[int, int] = (9, 9) +) -> tuple[tuple[int, ...], NDArray[np.float_], NDArray[np.float_]]: + """ + Calibrate cameras for distortion and fisheye effects. + + In order to use this function effectively please use at least 5 valid calibration images, with differently places checkerboards. + The checkerboard has to be fully visible with no occlusion, but it does mot have to lie flat on the ground. + + :param calibration_images_paths: The calibration images. + :param checkerboard_size: The checkerboard size. Note if you have a 10 x 10 checkerboard the size should be (9, 9). + :return: The dimension of the calibration images, the camera matrix and the distortion coefficient. + """ + subpix_criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.1) + calibration_flags = ( + cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC + + cv2.fisheye.CALIB_CHECK_COND + + cv2.fisheye.CALIB_FIX_SKEW + ) + + object_point = np.zeros( + (1, checkerboard_size[0] * checkerboard_size[1], 3), np.float32 + ) + object_point[0, :, :2] = np.mgrid[ + 0 : checkerboard_size[0], 0 : checkerboard_size[1] + ].T.reshape(-1, 2) + + _image_shape = None + + object_points = [] # 3d point in real world space + image_points = [] # 2d points in image plane + + for image_path in calibration_images_paths: + image = cv2.imread(image_path) + if _image_shape is None: + _image_shape = image.shape[:2] + else: + assert ( + _image_shape == image.shape[:2] + ), "All images must share the same size." + + # Detect checkerboard + returned, corners = cv2.findChessboardCorners( + image, + checkerboard_size, + None, + flags=cv2.CALIB_CB_ADAPTIVE_THRESH + + cv2.CALIB_CB_FAST_CHECK + + cv2.CALIB_CB_NORMALIZE_IMAGE, + ) + if returned: + object_points.append(object_point) + cv2.cornerSubPix(image, corners, (3, 3), (-1, -1), subpix_criteria) + image_points.append(corners) + + camera_matrix = np.zeros((3, 3)) + distortion_coefficients = np.zeros((4, 1)) + rotation_vectors = [ + np.zeros((1, 1, 3), dtype=np.float64) for i in range(len(object_points)) + ] + translation_vectors = [ + np.zeros((1, 1, 3), dtype=np.float64) for i in range(len(object_points)) + ] + + # cv2 operations on numpy objects are in-place -> therefore we do not need to extract the return output. + _ = cv2.fisheye.calibrate( + object_points, + image_points, + image_size=image.shape[::-1], + K=camera_matrix, + D=distortion_coefficients, + rvecs=rotation_vectors, + tvecs=translation_vectors, + flags=calibration_flags, + criteria=(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6), + ) + print(f"Found {len(object_points)} valid images for calibration") + return image.shape[:2][::-1], camera_matrix, distortion_coefficients diff --git a/ci_group/revolve2/ci_group/ci_lab_utilities/_ip_camera.py b/ci_group/revolve2/ci_group/ci_lab_utilities/_ip_camera.py new file mode 100644 index 000000000..b13576571 --- /dev/null +++ b/ci_group/revolve2/ci_group/ci_lab_utilities/_ip_camera.py @@ -0,0 +1,189 @@ +import queue +import threading +import time + +import cv2 +import numpy as np +from numpy.typing import NDArray + + +class IPCamera: + """ + A general class to steam and record from IP cameras via opencv. + + How to use: + >>> address = "rtsp://:@:/..." + >>> camera = IPCamera(camera_location=address, recording_path="") + >>> camera.start(display=True, record=True) + + If you are experiencing XDG_SESSION_TYPE error messages because of wayland use the following code before you start the recording: + >>> import os + >>> os.environ["XDG_SESSION_TYPE"] = "xcb" + >>> os.environ["OPENCV_FFMPEG_CAPTURE_OPTIONS"] = "rtsp_transport;udp" + """ + + _d_q: queue.Queue[cv2.typing.MatLike] # Display queue + _r_q: queue.Queue[cv2.typing.MatLike] # Record queue + + """Threads for camera functionality.""" + _recieve_thread: threading.Thread + _display_thread: threading.Thread + _record_thread: threading.Thread + + _camera_location: str # Location (address) of the camera + _is_running: bool # Allows to break threads + _image_dimensions: tuple[int, int] + _fps: int + + """Maps for undistorting the camera.""" + _map1: cv2.typing.MatLike + _map2: cv2.typing.MatLike + + def __init__( + self, + camera_location: str, + recording_path: str | None = None, + image_dimensions: tuple[int, int] = (1920, 1080), + distortion_coefficients: NDArray[np.float_] = np.array( + [ + [-0.2976428547328032], + [3.2508343621538445], + [-17.38410840159056], + [30.01965021834286], + ] + ), + camera_matrix: NDArray[np.float_] = np.array( + [ + [1490.4374643604199, 0.0, 990.6557248821284], + [0.0, 1490.6535480621505, 544.6243597123726], + [0.0, 0.0, 1.0], + ] + ), + fps: int = 30, + ) -> None: + """ + Initialize the ip camera. + + :param camera_location: The location of the camera. + :param recording_path: The path to store the recording. + :param image_dimensions: The dimensions of the image produced by the camera and used for calibration. + :param distortion_coefficients: The distortion coefficients for the camera. + :param camera_matrix: The camera matrix for calibration. + :param fps: The FPS of the camera. + """ + self._camera_location = camera_location + self._recording_path = recording_path or f"{time.time()}_output.mp4" + + self._d_q = queue.Queue() + self._r_q = queue.Queue() + + self._image_dimensions = image_dimensions + self._fps = fps + + self._map1, self._map2 = cv2.fisheye.initUndistortRectifyMap( + camera_matrix, + distortion_coefficients, + np.eye(3), + camera_matrix, + self._image_dimensions, + cv2.CV_16SC2, + ) + + def _receive(self) -> None: + """Recieve data from the camera.""" + capture = cv2.VideoCapture(self._camera_location, cv2.CAP_FFMPEG) + capture.set(cv2.CAP_PROP_FRAME_WIDTH, self._image_dimensions[0]) + capture.set(cv2.CAP_PROP_FRAME_HEIGHT, self._image_dimensions[1]) + ret, frame = capture.read() + + frame = self._unfish(frame) + self._d_q.put(frame) + self._r_q.put(frame) + while ret and self._is_running: + ret, frame = capture.read() + + frame = self._unfish(frame) + self._d_q.put(frame) + self._r_q.put(frame) + else: + capture.release() + + def _display(self) -> None: + """Display the data from the camera.""" + while self._is_running: + if not self._d_q.empty(): + frame = self._d_q.get() + cv2.imshow("Camera View", frame) + key = cv2.waitKey(1) + if key == 27: # Exit the viewer using ESC-button + self._is_running = False + else: + cv2.destroyAllWindows() + + def _record(self) -> None: + """Record the data from the camera.""" + out = cv2.VideoWriter( + self._recording_path, + cv2.VideoWriter.fourcc(*"mp4v"), + self._fps, + self._image_dimensions, + ) + print("Recording in progress.") + while self._is_running: + if not self._r_q.empty(): + out.write(self._r_q.get()) + else: + print(f"Saving video to: {self._recording_path}") + out.release() + + def _dump_record(self) -> None: + """Dump record queue if not used.""" + while self._is_running: + if not self._r_q.empty(): + self._r_q.get() + + def _dump_display(self) -> None: + """Dump display queue if not used.""" + while self._is_running: + if not self._d_q.empty(): + self._d_q.get() + + def _unfish(self, image: cv2.typing.MatLike) -> cv2.typing.MatLike: + """ + Remove fisheye effect from the camera. + + :param image: The image + :return: The undistorted image. + """ + undistorted = cv2.remap( + image, + self._map1, + self._map2, + interpolation=cv2.INTER_LINEAR, + borderMode=cv2.BORDER_CONSTANT, + ) + return undistorted + + def start(self, record: bool = False, display: bool = True) -> None: + """ + Start the camera. + + :param record: Whether to record. + :param display: Whether to display the video stream. + """ + assert ( + record or display + ), "The camera is neither recording or displaying, are you sure you are using it?" + self._recieve_thread = threading.Thread(target=self._receive) + self._display_thread = threading.Thread( + target=self._display if display else self._dump_display + ) + self._record_thread = threading.Thread( + target=self._record if record else self._dump_record + ) + + self._is_running = True + + self._recieve_thread.start() + self._record_thread.start() + self._display_thread.start() diff --git a/ci_group/revolve2/ci_group/genotypes/cppnwin/modular_robot/v2/_body_develop.py b/ci_group/revolve2/ci_group/genotypes/cppnwin/modular_robot/v2/_body_develop.py index c9520cdf9..57f26f3d0 100644 --- a/ci_group/revolve2/ci_group/genotypes/cppnwin/modular_robot/v2/_body_develop.py +++ b/ci_group/revolve2/ci_group/genotypes/cppnwin/modular_robot/v2/_body_develop.py @@ -31,7 +31,7 @@ def develop( :param genotype: The genotype to create the body from. :returns: The create body. """ - max_parts = 10 + max_parts = 20 body_net = multineat.NeuralNetwork() genotype.BuildPhenotype(body_net) @@ -127,9 +127,8 @@ def __add_child( new_pos = np.array(np.round(position + attachment_point.offset), dtype=np.int64) child_type, child_rotation = __evaluate_cppn(body_net, new_pos, chain_length) angle = child_rotation * (np.pi / 2.0) - child = child_type(angle) if child_type is None or not module.module_reference.can_set_child( - child, attachment_index + child := child_type(angle), attachment_index ): return None diff --git a/ci_group/revolve2/ci_group/morphological_measures.py b/ci_group/revolve2/ci_group/morphological_measures.py index b46e3fa3c..21bbf7d42 100644 --- a/ci_group/revolve2/ci_group/morphological_measures.py +++ b/ci_group/revolve2/ci_group/morphological_measures.py @@ -19,7 +19,7 @@ class MorphologicalMeasures(Generic[TModule]): Only works for robot with only right angle module rotations (90 degrees). Some measures only work for 2d robots, which is noted in their docstring. - Some measures are based on the following paper: + The measures are based on the following paper: Miras, K., Haasdijk, E., Glette, K., Eiben, A.E. (2018). Search Space Analysis of Evolvable Robot Morphologies. In: Sim, K., Kaufmann, P. (eds) Applications of Evolutionary Computation. diff --git a/contributors.txt b/contributors.txt deleted file mode 100644 index 50b7f3f81..000000000 --- a/contributors.txt +++ /dev/null @@ -1 +0,0 @@ -Computation Intelligence Group, Vrije Universiteit \ No newline at end of file diff --git a/docs/source/contributing_guide/index.rst b/docs/source/contributing_guide/index.rst index 99ef8d759..8146cd27b 100644 --- a/docs/source/contributing_guide/index.rst +++ b/docs/source/contributing_guide/index.rst @@ -13,8 +13,8 @@ The first time you make a contribution to a Revolve2 package your PR should add Developer installation ---------------------- The normal installation guide applies. You should definitely use :ref:`editable mode`. -The ``dev_install.sh`` script installs all Revolve2 packages in editable mode, as well as required packages for developer tools. -If you want to uninstall all Revolve2 packages, you can use ``./uninstall_revolve2star.sh``, which uninstall all packages like ``revolve2*``. +Using the ``requirements_dev.txt`` allows you to quickly install all packages in editable mode, by executing: ``pip install -r requirements_dev.txt``. +If you want to uninstall all Revolve2 packages, you can use ``./uninstall.sh``, which uninstall all packages like ``revolve2*``. ---------------------- Continuous integration diff --git a/examples/brain_with_feedback/main.py b/examples/brain_with_feedback/main.py index 85d3a528d..6786c699a 100644 --- a/examples/brain_with_feedback/main.py +++ b/examples/brain_with_feedback/main.py @@ -2,11 +2,12 @@ import logging -from revolve2.ci_group import modular_robots_v1, terrains +from revolve2.ci_group import modular_robots_v2, terrains from revolve2.ci_group.simulation_parameters import make_standard_batch_parameters from revolve2.experimentation.logging import setup_logging from revolve2.modular_robot import ModularRobot, ModularRobotControlInterface -from revolve2.modular_robot.body.base import ActiveHinge, ActiveHingeSensor +from revolve2.modular_robot.body.base import ActiveHinge +from revolve2.modular_robot.body.sensors import ActiveHingeSensor, IMUSensor from revolve2.modular_robot.brain import Brain, BrainInstance from revolve2.modular_robot.sensor_state import ModularRobotSensorState from revolve2.modular_robot_simulation import ModularRobotScene, simulate_scenes @@ -17,17 +18,17 @@ class ANNBrainInstance(BrainInstance): """ANN brain instance.""" active_hinges: list[ActiveHinge] + imu_sensor: IMUSensor - def __init__( - self, - active_hinges: list[ActiveHinge], - ) -> None: + def __init__(self, active_hinges: list[ActiveHinge], imu_sensor: IMUSensor) -> None: """ Initialize the Object. :param active_hinges: The active hinges to control. + :param imu_sensor: The IMU sensor. """ self.active_hinges = active_hinges + self.imu_sensor = imu_sensor def control( self, @@ -57,7 +58,13 @@ def control( sensor_state.get_active_hinge_sensor_state(sensor).position for sensor in sensors ] - logging.info(current_positions) + logging.info(f"current positions: {current_positions}") + + # Get the imu state + imu_state = sensor_state.get_imu_sensor_state(self.imu_sensor) + logging.info(f"orientation: {imu_state.orientation}") + logging.info(f"angular rate: {imu_state.angular_rate}") + logging.info(f"specific force: {imu_state.specific_force}") # Here you can implement your controller. # The current controller does nothing except for always settings the joint positions to 0.5. @@ -70,17 +77,17 @@ class ANNBrain(Brain): """The ANN brain.""" active_hinges: list[ActiveHinge] + imu_sensor: IMUSensor - def __init__( - self, - active_hinges: list[ActiveHinge], - ) -> None: + def __init__(self, active_hinges: list[ActiveHinge], imu_sensor: IMUSensor) -> None: """ Initialize the Object. :param active_hinges: The active hinges to control. + :param imu_sensor: The IMU sensor. """ self.active_hinges = active_hinges + self.imu_sensor = imu_sensor def make_instance(self) -> BrainInstance: """ @@ -89,7 +96,7 @@ def make_instance(self) -> BrainInstance: :returns: The created instance. """ return ANNBrainInstance( - active_hinges=self.active_hinges, + active_hinges=self.active_hinges, imu_sensor=self.imu_sensor ) @@ -99,18 +106,18 @@ def main() -> None: setup_logging() # Create a body for the robot. - body = modular_robots_v1.gecko_v1() + body = modular_robots_v2.gecko_v2() # Add sensors to each active hinge that measure the current angle of the hinge. active_hinges = body.find_modules_of_type(ActiveHinge) for active_hinge in active_hinges: active_hinge.sensor = ActiveHingeSensor() + body.core.imu_sensor = IMUSensor() + # Create a brain for the robot. active_hinges = body.find_modules_of_type(ActiveHinge) - brain = ANNBrain( - active_hinges=active_hinges, - ) + brain = ANNBrain(active_hinges=active_hinges, imu_sensor=body.core.imu_sensor) # Combine the body and brain into a modular robot. robot = ModularRobot(body, brain) diff --git a/examples/compare_simulated_and_physical_robot/main_physical.py b/examples/compare_simulated_and_physical_robot/main_physical.py index 15da6a66d..2885fa63a 100644 --- a/examples/compare_simulated_and_physical_robot/main_physical.py +++ b/examples/compare_simulated_and_physical_robot/main_physical.py @@ -1,13 +1,14 @@ """Manually control a physical robot to test if it works as expected.""" from revolve2.modular_robot.body import RightAngles -from revolve2.modular_robot.body.base import ActiveHinge, ActiveHingeSensor -from revolve2.modular_robot.body.v1 import ActiveHingeV1, BodyV1, BrickV1 +from revolve2.modular_robot.body.base import ActiveHinge +from revolve2.modular_robot.body.sensors import ActiveHingeSensor +from revolve2.modular_robot.body.v2 import ActiveHingeV2, BodyV2, BrickV2 from revolve2.modular_robot_physical import UUIDKey from revolve2.modular_robot_physical.remote import test_physical_robot def make_body() -> ( - tuple[BodyV1, tuple[ActiveHinge, ActiveHinge, ActiveHinge, ActiveHinge]] + tuple[BodyV2, tuple[ActiveHinge, ActiveHinge, ActiveHinge, ActiveHinge]] ): """ Create a body for the robot. @@ -19,24 +20,25 @@ def make_body() -> ( # 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.sensor = ActiveHingeSensor() - body.core_v1.left.attachment = ActiveHingeV1(RightAngles.DEG_0) - body.core_v1.left.attachment.sensor = ActiveHingeSensor() - body.core_v1.left.attachment.attachment = BrickV1(RightAngles.DEG_0) - body.core_v1.right = ActiveHingeV1(RightAngles.DEG_0) - body.core_v1.right.sensor = ActiveHingeSensor() - body.core_v1.right.attachment = ActiveHingeV1(RightAngles.DEG_0) - body.core_v1.right.attachment.sensor = ActiveHingeSensor() - body.core_v1.right.attachment.attachment = BrickV1(RightAngles.DEG_0) + body = BodyV2() + body.core_v2.left_face.bottom = ActiveHingeV2(RightAngles.DEG_0) + body.core_v2.left_face.bottom.sensor = ActiveHingeSensor() + body.core_v2.left_face.bottom.attachment = ActiveHingeV2(RightAngles.DEG_0) + body.core_v2.left_face.bottom.attachment.sensor = ActiveHingeSensor() + body.core_v2.left_face.bottom.attachment.attachment = BrickV2(RightAngles.DEG_0) + + body.core_v2.right_face.bottom = ActiveHingeV2(RightAngles.DEG_0) + body.core_v2.right_face.bottom.sensor = ActiveHingeSensor() + body.core_v2.right_face.bottom.attachment = ActiveHingeV2(RightAngles.DEG_0) + body.core_v2.right_face.bottom.attachment.sensor = ActiveHingeSensor() + body.core_v2.right_face.bottom.attachment.attachment = BrickV2(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, + body.core_v2.left_face.bottom, + body.core_v2.left_face.bottom.attachment, + body.core_v2.right_face.bottom, + body.core_v2.right_face.bottom.attachment, ) return body, active_hinges diff --git a/examples/custom_brain/main.py b/examples/custom_brain/main.py index 05340f4d8..996b1e78c 100644 --- a/examples/custom_brain/main.py +++ b/examples/custom_brain/main.py @@ -5,7 +5,7 @@ from revolve2.experimentation.logging import setup_logging from revolve2.modular_robot import ModularRobot, ModularRobotControlInterface from revolve2.modular_robot.body import RightAngles -from revolve2.modular_robot.body.v1 import ActiveHingeV1, BodyV1, BrickV1 +from revolve2.modular_robot.body.v2 import ActiveHingeV2, BodyV2, BrickV2 from revolve2.modular_robot.brain import Brain, BrainInstance from revolve2.modular_robot.sensor_state import ModularRobotSensorState from revolve2.modular_robot_simulation import ModularRobotScene, simulate_scenes @@ -13,24 +13,24 @@ def make_body() -> ( - tuple[BodyV1, ActiveHingeV1, ActiveHingeV1, ActiveHingeV1, ActiveHingeV1] + tuple[BodyV2, ActiveHingeV2, ActiveHingeV2, ActiveHingeV2, ActiveHingeV2] ): """ Create a body for the robot. :returns: The created body and references to each hinge: first_left_active_hinge, second_left_active_hinge, first_right_active_hinge, second_right_active_hinge. """ - body = BodyV1() - first_left_active_hinge = ActiveHingeV1(RightAngles.DEG_0) - second_left_active_hinge = ActiveHingeV1(RightAngles.DEG_0) - first_right_active_hinge = ActiveHingeV1(RightAngles.DEG_0) - second_right_active_hinge = ActiveHingeV1(RightAngles.DEG_0) - body.core_v1.left = first_left_active_hinge + body = BodyV2() + first_left_active_hinge = ActiveHingeV2(RightAngles.DEG_0) + second_left_active_hinge = ActiveHingeV2(RightAngles.DEG_0) + first_right_active_hinge = ActiveHingeV2(RightAngles.DEG_0) + second_right_active_hinge = ActiveHingeV2(RightAngles.DEG_0) + body.core_v2.left_face.bottom = first_left_active_hinge first_left_active_hinge.attachment = second_left_active_hinge - second_left_active_hinge.attachment = BrickV1(RightAngles.DEG_0) - body.core_v1.right = first_right_active_hinge + second_left_active_hinge.attachment = BrickV2(RightAngles.DEG_0) + body.core_v2.right_face.bottom = first_right_active_hinge first_right_active_hinge.attachment = second_right_active_hinge - second_right_active_hinge.attachment = BrickV1(RightAngles.DEG_0) + second_right_active_hinge.attachment = BrickV2(RightAngles.DEG_0) return ( body, first_left_active_hinge, @@ -47,17 +47,17 @@ class CustomBrainInstance(BrainInstance): Created by the `CustomBrain` class. """ - first_left_active_hinge: ActiveHingeV1 - second_left_active_hinge: ActiveHingeV1 - first_right_active_hinge: ActiveHingeV1 - second_right_active_hinge: ActiveHingeV1 + first_left_active_hinge: ActiveHingeV2 + second_left_active_hinge: ActiveHingeV2 + first_right_active_hinge: ActiveHingeV2 + second_right_active_hinge: ActiveHingeV2 def __init__( self, - first_left_active_hinge: ActiveHingeV1, - second_left_active_hinge: ActiveHingeV1, - first_right_active_hinge: ActiveHingeV1, - second_right_active_hinge: ActiveHingeV1, + first_left_active_hinge: ActiveHingeV2, + second_left_active_hinge: ActiveHingeV2, + first_right_active_hinge: ActiveHingeV2, + second_right_active_hinge: ActiveHingeV2, ) -> None: """ Initialize the Object. @@ -99,17 +99,17 @@ class CustomBrain(Brain): A brain has a function `make_instance`, which creates the actual object that controls a robot. """ - first_left_active_hinge: ActiveHingeV1 - second_left_active_hinge: ActiveHingeV1 - first_right_active_hinge: ActiveHingeV1 - second_right_active_hinge: ActiveHingeV1 + first_left_active_hinge: ActiveHingeV2 + second_left_active_hinge: ActiveHingeV2 + first_right_active_hinge: ActiveHingeV2 + second_right_active_hinge: ActiveHingeV2 def __init__( self, - first_left_active_hinge: ActiveHingeV1, - second_left_active_hinge: ActiveHingeV1, - first_right_active_hinge: ActiveHingeV1, - second_right_active_hinge: ActiveHingeV1, + first_left_active_hinge: ActiveHingeV2, + second_left_active_hinge: ActiveHingeV2, + first_right_active_hinge: ActiveHingeV2, + second_right_active_hinge: ActiveHingeV2, ) -> None: """ Initialize the Object. diff --git a/examples/custom_terrain/main.py b/examples/custom_terrain/main.py index 9c6078662..dbc85b3a6 100644 --- a/examples/custom_terrain/main.py +++ b/examples/custom_terrain/main.py @@ -4,7 +4,7 @@ from pyrr import Quaternion, Vector3 -from revolve2.ci_group.modular_robots_v1 import gecko_v1 +from revolve2.ci_group.modular_robots_v2 import gecko_v2 from revolve2.ci_group.simulation_parameters import make_standard_batch_parameters from revolve2.experimentation.logging import setup_logging from revolve2.experimentation.rng import make_rng_time_seed @@ -90,7 +90,7 @@ def main() -> None: rng = make_rng_time_seed() # Create a robot - body = gecko_v1() + body = gecko_v2() brain = BrainCpgNetworkNeighborRandom(body=body, rng=rng) robot = ModularRobot(body, brain) diff --git a/examples/evaluate_single_robot/main.py b/examples/evaluate_single_robot/main.py index fcaf33913..bd2073f62 100644 --- a/examples/evaluate_single_robot/main.py +++ b/examples/evaluate_single_robot/main.py @@ -2,7 +2,7 @@ import logging -from revolve2.ci_group import fitness_functions, modular_robots_v1, terrains +from revolve2.ci_group import fitness_functions, modular_robots_v2, terrains from revolve2.ci_group.simulation_parameters import make_standard_batch_parameters from revolve2.experimentation.logging import setup_logging from revolve2.experimentation.rng import make_rng_time_seed @@ -21,7 +21,7 @@ def main() -> None: rng = make_rng_time_seed() # Create the robot. - body = modular_robots_v1.gecko_v1() + body = modular_robots_v2.gecko_v2() brain = BrainCpgNetworkNeighborRandom(body=body, rng=rng) robot = ModularRobot(body, brain) diff --git a/examples/physical_robot_remote/main.py b/examples/physical_robot_remote/main.py index e4c9e54f0..9e7657adc 100644 --- a/examples/physical_robot_remote/main.py +++ b/examples/physical_robot_remote/main.py @@ -4,14 +4,14 @@ 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.body.v2 import ActiveHingeV2, BodyV2, BrickV2 from revolve2.modular_robot.brain.cpg import BrainCpgNetworkNeighborRandom from revolve2.modular_robot_physical import Config, UUIDKey from revolve2.modular_robot_physical.remote import run_remote def make_body() -> ( - tuple[BodyV1, tuple[ActiveHinge, ActiveHinge, ActiveHinge, ActiveHinge]] + tuple[BodyV2, tuple[ActiveHinge, ActiveHinge, ActiveHinge, ActiveHinge]] ): """ Create a body for the robot. @@ -23,20 +23,20 @@ def make_body() -> ( # 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) + body = BodyV2() + body.core_v2.left_face.bottom = ActiveHingeV2(RightAngles.DEG_0) + body.core_v2.left_face.bottom.attachment = ActiveHingeV2(RightAngles.DEG_0) + body.core_v2.left_face.bottom.attachment.attachment = BrickV2(RightAngles.DEG_0) + body.core_v2.right_face.bottom = ActiveHingeV2(RightAngles.DEG_0) + body.core_v2.right_face.bottom.attachment = ActiveHingeV2(RightAngles.DEG_0) + body.core_v2.right_face.bottom.attachment.attachment = BrickV2(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, + body.core_v2.left_face.bottom, + body.core_v2.left_face.bottom.attachment, + body.core_v2.right_face.bottom, + body.core_v2.right_face.bottom.attachment, ) return body, active_hinges diff --git a/examples/robot_bodybrain_ea/genotype.py b/examples/robot_bodybrain_ea/genotype.py index d58522247..3665c01b2 100644 --- a/examples/robot_bodybrain_ea/genotype.py +++ b/examples/robot_bodybrain_ea/genotype.py @@ -8,12 +8,12 @@ import numpy as np from revolve2.ci_group.genotypes.cppnwin.modular_robot import BrainGenotypeCpg -from revolve2.ci_group.genotypes.cppnwin.modular_robot.v1 import BodyGenotypeV1 +from revolve2.ci_group.genotypes.cppnwin.modular_robot.v2 import BodyGenotypeV2 from revolve2.modular_robot import ModularRobot @dataclass -class Genotype(BodyGenotypeV1, BrainGenotypeCpg): +class Genotype(BodyGenotypeV2, BrainGenotypeCpg): """A genotype for a body and brain using CPPN.""" @classmethod diff --git a/examples/robot_bodybrain_ea_database/genotype.py b/examples/robot_bodybrain_ea_database/genotype.py index 9fcbd798d..406efd64b 100644 --- a/examples/robot_bodybrain_ea_database/genotype.py +++ b/examples/robot_bodybrain_ea_database/genotype.py @@ -7,12 +7,12 @@ from base import Base from revolve2.ci_group.genotypes.cppnwin.modular_robot import BrainGenotypeCpgOrm -from revolve2.ci_group.genotypes.cppnwin.modular_robot.v1 import BodyGenotypeOrmV1 +from revolve2.ci_group.genotypes.cppnwin.modular_robot.v2 import BodyGenotypeOrmV2 from revolve2.experimentation.database import HasId from revolve2.modular_robot import ModularRobot -class Genotype(Base, HasId, BodyGenotypeOrmV1, BrainGenotypeCpgOrm): +class Genotype(Base, HasId, BodyGenotypeOrmV2, BrainGenotypeCpgOrm): """SQLAlchemy model for a genotype for a modular robot body and brain.""" __tablename__ = "genotype" diff --git a/examples/robot_brain_cmaes_database/config.py b/examples/robot_brain_cmaes_database/config.py index 34e52d113..5e1374124 100644 --- a/examples/robot_brain_cmaes_database/config.py +++ b/examples/robot_brain_cmaes_database/config.py @@ -1,10 +1,10 @@ """Configuration parameters for this example.""" -from revolve2.ci_group.modular_robots_v1 import gecko_v1 +from revolve2.ci_group.modular_robots_v2 import gecko_v2 DATABASE_FILE = "database.sqlite" NUM_REPETITIONS = 5 NUM_SIMULATORS = 8 INITIAL_STD = 0.5 NUM_GENERATIONS = 100 -BODY = gecko_v1() +BODY = gecko_v2() diff --git a/examples/simulate_single_robot/main.py b/examples/simulate_single_robot/main.py index e240b0de6..175db6523 100644 --- a/examples/simulate_single_robot/main.py +++ b/examples/simulate_single_robot/main.py @@ -5,13 +5,13 @@ 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.v1 import ActiveHingeV1, BodyV1, BrickV1 +from revolve2.modular_robot.body.v2 import ActiveHingeV2, BodyV2, BrickV2 from revolve2.modular_robot.brain.cpg import BrainCpgNetworkNeighborRandom from revolve2.modular_robot_simulation import ModularRobotScene, simulate_scenes from revolve2.simulators.mujoco_simulator import LocalSimulator -def make_body() -> BodyV1: +def make_body() -> BodyV2: """ Create a body for the robot. @@ -22,13 +22,13 @@ def make_body() -> BodyV1: # 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) + body = BodyV2() + body.core_v2.left_face.bottom = ActiveHingeV2(RightAngles.DEG_0) + body.core_v2.left_face.bottom.attachment = ActiveHingeV2(RightAngles.DEG_0) + body.core_v2.left_face.bottom.attachment.attachment = BrickV2(RightAngles.DEG_0) + body.core_v2.right_face.bottom = ActiveHingeV2(RightAngles.DEG_0) + body.core_v2.right_face.bottom.attachment = ActiveHingeV2(RightAngles.DEG_0) + body.core_v2.right_face.bottom.attachment.attachment = BrickV2(RightAngles.DEG_0) return body diff --git a/experimentation/pyproject.toml b/experimentation/pyproject.toml index acaa9c589..a105d5fe7 100644 --- a/experimentation/pyproject.toml +++ b/experimentation/pyproject.toml @@ -4,10 +4,13 @@ build-backend = "poetry.core.masonry.api" [tool.poetry] name = "revolve2-experimentation" -version = "1.0.1" +version = "1.0.2" description = "Revolve2: Tools for experimentation." readme = "../README.md" -authors = ["Aart Stuurman "] +authors = [ + "Aart Stuurman ", + "Oliver Weissl ", +] repository = "https://github.com/ci-group/revolve2" classifiers = [ "Development Status :: 4 - Beta", diff --git a/modular_robot/pyproject.toml b/modular_robot/pyproject.toml index ab0fbeeb9..bbdc41a36 100644 --- a/modular_robot/pyproject.toml +++ b/modular_robot/pyproject.toml @@ -4,10 +4,13 @@ build-backend = "poetry.core.masonry.api" [tool.poetry] name = "revolve2-modular-robot" -version = "1.0.1" +version = "1.0.2" description = "Revolve2: Everything for defining modular robots." readme = "../README.md" -authors = ["Aart Stuurman "] +authors = [ + "Aart Stuurman ", + "Oliver Weissl ", +] repository = "https://github.com/ci-group/revolve2" classifiers = [ "Development Status :: 4 - Beta", diff --git a/modular_robot/revolve2/modular_robot/body/base/__init__.py b/modular_robot/revolve2/modular_robot/body/base/__init__.py index d28362d52..83fa114a2 100644 --- a/modular_robot/revolve2/modular_robot/body/base/__init__.py +++ b/modular_robot/revolve2/modular_robot/body/base/__init__.py @@ -1,6 +1,5 @@ """Base Modules for Robots.""" from ._active_hinge import ActiveHinge -from ._active_hinge_sensor import ActiveHingeSensor from ._attachment_face import AttachmentFace from ._body import Body from ._brick import Brick @@ -8,7 +7,6 @@ __all__ = [ "ActiveHinge", - "ActiveHingeSensor", "AttachmentFace", "Body", "Brick", diff --git a/modular_robot/revolve2/modular_robot/body/base/_active_hinge.py b/modular_robot/revolve2/modular_robot/body/base/_active_hinge.py index 023e03345..8b5b2c900 100644 --- a/modular_robot/revolve2/modular_robot/body/base/_active_hinge.py +++ b/modular_robot/revolve2/modular_robot/body/base/_active_hinge.py @@ -4,7 +4,7 @@ from .._color import Color from .._module import Module from .._right_angles import RightAngles -from ._active_hinge_sensor import ActiveHingeSensor +from ..sensors import ActiveHingeSensor class ActiveHinge(Module): diff --git a/modular_robot/revolve2/modular_robot/body/base/_core.py b/modular_robot/revolve2/modular_robot/body/base/_core.py index dc2435762..cc697e155 100644 --- a/modular_robot/revolve2/modular_robot/body/base/_core.py +++ b/modular_robot/revolve2/modular_robot/body/base/_core.py @@ -6,6 +6,7 @@ from .._color import Color from .._module import Module from .._right_angles import RightAngles +from ..sensors import IMUSensor class Core(Module): @@ -23,6 +24,8 @@ class Core(Module): See `_get_new_module_id` on what this is used for. """ + imu_sensor: IMUSensor | None + def __init__( self, rotation: float | RightAngles, diff --git a/modular_robot/revolve2/modular_robot/body/sensors/__init__.py b/modular_robot/revolve2/modular_robot/body/sensors/__init__.py new file mode 100644 index 000000000..538ce19bd --- /dev/null +++ b/modular_robot/revolve2/modular_robot/body/sensors/__init__.py @@ -0,0 +1,8 @@ +"""Base Sensors for Modular Robots.""" + +from ._active_hinge_sensor import ActiveHingeSensor +from ._camera_sensor import CameraSensor +from ._imu_sensor import IMUSensor +from ._sensor import Sensor + +__all__ = ["ActiveHingeSensor", "CameraSensor", "IMUSensor", "Sensor"] diff --git a/modular_robot/revolve2/modular_robot/body/sensors/_active_hinge_sensor.py b/modular_robot/revolve2/modular_robot/body/sensors/_active_hinge_sensor.py new file mode 100644 index 000000000..c23b53123 --- /dev/null +++ b/modular_robot/revolve2/modular_robot/body/sensors/_active_hinge_sensor.py @@ -0,0 +1,11 @@ +import uuid +from dataclasses import dataclass, field + +from ._sensor import Sensor + + +@dataclass +class ActiveHingeSensor(Sensor): + """A sensors for an active hinge that measures its angle.""" + + _uuid: uuid.UUID = field(init=False, default_factory=uuid.uuid1) diff --git a/modular_robot/revolve2/modular_robot/body/sensors/_camera_sensor.py b/modular_robot/revolve2/modular_robot/body/sensors/_camera_sensor.py new file mode 100644 index 000000000..b9050c422 --- /dev/null +++ b/modular_robot/revolve2/modular_robot/body/sensors/_camera_sensor.py @@ -0,0 +1,19 @@ +import uuid +from dataclasses import dataclass, field + +from pyrr import Vector3 + +from revolve2.simulation.scene.vector2 import Vector2 + +from ._sensor import Sensor + + +@dataclass +class CameraSensor(Sensor): + """A camera for the Modular Robot.""" + + position: Vector3 # The position of the camera on the parent module. + camera_size: Vector2 = field( + init=False, default_factory=Vector2([200, 200]) + ) # The size of the image produced by the camera (w x h). + _uuid: uuid.UUID = field(init=False, default_factory=uuid.uuid1) diff --git a/modular_robot/revolve2/modular_robot/body/sensors/_imu_sensor.py b/modular_robot/revolve2/modular_robot/body/sensors/_imu_sensor.py new file mode 100644 index 000000000..e0e1459d3 --- /dev/null +++ b/modular_robot/revolve2/modular_robot/body/sensors/_imu_sensor.py @@ -0,0 +1,15 @@ +import uuid +from dataclasses import dataclass, field + +from ._sensor import Sensor + + +@dataclass +class IMUSensor(Sensor): + """ + An inertial measurement unit. + + Reports specific force(closely related to acceleration), angular rate(closely related to angular velocity), and orientation. + """ + + _uuid: uuid.UUID = field(init=False, default_factory=uuid.uuid1) diff --git a/modular_robot/revolve2/modular_robot/body/base/_active_hinge_sensor.py b/modular_robot/revolve2/modular_robot/body/sensors/_sensor.py similarity index 62% rename from modular_robot/revolve2/modular_robot/body/base/_active_hinge_sensor.py rename to modular_robot/revolve2/modular_robot/body/sensors/_sensor.py index 5be899781..448cdbff8 100644 --- a/modular_robot/revolve2/modular_robot/body/base/_active_hinge_sensor.py +++ b/modular_robot/revolve2/modular_robot/body/sensors/_sensor.py @@ -1,18 +1,19 @@ import uuid +from abc import ABC from dataclasses import dataclass, field @dataclass -class ActiveHingeSensor: - """A sensor for an active hinge that measures its angle.""" +class Sensor(ABC): + """An abstract Sensor Class.""" _uuid: uuid.UUID = field(init=False, default_factory=uuid.uuid1) @property def uuid(self) -> uuid.UUID: """ - Get the uuid. + Get the uuid of the sensor. - :returns: The uuid. + :return: The uuid. """ return self._uuid diff --git a/modular_robot/revolve2/modular_robot/body/v2/_attachment_face_core_v2.py b/modular_robot/revolve2/modular_robot/body/v2/_attachment_face_core_v2.py index faa22928d..7685a9c94 100644 --- a/modular_robot/revolve2/modular_robot/body/v2/_attachment_face_core_v2.py +++ b/modular_robot/revolve2/modular_robot/body/v2/_attachment_face_core_v2.py @@ -61,7 +61,13 @@ def __init__( h_o = (i % 3 - 1) * horizontal_offset v_o = -(i // 3 - 1) * vertical_offset - offset = rot * Vector3([0.0, h_o, v_o]) + h_o = h_o if int(rot.angle / np.pi) % 2 == 0 else -h_o + offset = ( + Vector3([0.0, h_o, v_o]) + if np.isclose(rot.angle % np.pi, 0) + else Vector3([h_o, 0.0, v_o]) + ) + offset = rot * offset attachment_points[i] = AttachmentPoint( orientation=rot, offset=self._child_offset + offset diff --git a/modular_robot/revolve2/modular_robot/sensor_state/__init__.py b/modular_robot/revolve2/modular_robot/sensor_state/__init__.py index 4b4f56f1a..5194cad02 100644 --- a/modular_robot/revolve2/modular_robot/sensor_state/__init__.py +++ b/modular_robot/revolve2/modular_robot/sensor_state/__init__.py @@ -1,6 +1,13 @@ """Sensor states from modular robots.""" from ._active_hinge_sensor_state import ActiveHingeSensorState +from ._camera_sensor_state import CameraSensorState +from ._imu_sensor_state import IMUSensorState from ._modular_robot_sensor_state import ModularRobotSensorState -__all__ = ["ActiveHingeSensorState", "ModularRobotSensorState"] +__all__ = [ + "ActiveHingeSensorState", + "CameraSensorState", + "IMUSensorState", + "ModularRobotSensorState", +] diff --git a/modular_robot/revolve2/modular_robot/sensor_state/_camera_sensor_state.py b/modular_robot/revolve2/modular_robot/sensor_state/_camera_sensor_state.py new file mode 100644 index 000000000..62aeeb8a7 --- /dev/null +++ b/modular_robot/revolve2/modular_robot/sensor_state/_camera_sensor_state.py @@ -0,0 +1,18 @@ +from abc import ABC, abstractmethod + +import numpy as np +from numpy.typing import NDArray + + +class CameraSensorState(ABC): + """The state of a camera sensor.""" + + @property + @abstractmethod + def image(self) -> NDArray[np.float_]: + """ + Get the current image. + + :returns: The image. + """ + pass diff --git a/modular_robot/revolve2/modular_robot/sensor_state/_imu_sensor_state.py b/modular_robot/revolve2/modular_robot/sensor_state/_imu_sensor_state.py new file mode 100644 index 000000000..426122d7d --- /dev/null +++ b/modular_robot/revolve2/modular_robot/sensor_state/_imu_sensor_state.py @@ -0,0 +1,34 @@ +from abc import ABC, abstractmethod + +from pyrr import Vector3 + + +class IMUSensorState(ABC): + """The state of an IMU sensor.""" + + @property + @abstractmethod + def specific_force(self) -> Vector3: + """ + Get the measured specific force. + + :returns: The measured specific force. + """ + + @property + @abstractmethod + def angular_rate(self) -> Vector3: + """ + Get the measured angular rate. + + :returns: The measured angular rate. + """ + + @property + @abstractmethod + def orientation(self) -> Vector3: + """ + Get the measured orientation. + + :returns: The measured orientation. + """ diff --git a/modular_robot/revolve2/modular_robot/sensor_state/_modular_robot_sensor_state.py b/modular_robot/revolve2/modular_robot/sensor_state/_modular_robot_sensor_state.py index a93e3ec42..08a715f09 100644 --- a/modular_robot/revolve2/modular_robot/sensor_state/_modular_robot_sensor_state.py +++ b/modular_robot/revolve2/modular_robot/sensor_state/_modular_robot_sensor_state.py @@ -1,7 +1,9 @@ from abc import ABC, abstractmethod -from ..body.base._active_hinge_sensor import ActiveHingeSensor +from ..body.sensors import ActiveHingeSensor, CameraSensor, IMUSensor from ._active_hinge_sensor_state import ActiveHingeSensorState +from ._camera_sensor_state import CameraSensorState +from ._imu_sensor_state import IMUSensorState class ModularRobotSensorState(ABC): @@ -17,3 +19,21 @@ def get_active_hinge_sensor_state( :param sensor: The sensor. :returns: The state. """ + + @abstractmethod + def get_imu_sensor_state(self, sensor: IMUSensor) -> IMUSensorState: + """ + Get the state of the provided IMU sensor. + + :param sensor: The sensor. + :returns: The state. + """ + + @abstractmethod + def get_camera_sensor_state(self, sensor: CameraSensor) -> CameraSensorState: + """ + Get the state of the provided camera sensor. + + :param sensor: The sensor. + :returns: The state. + """ diff --git a/modular_robot_physical/pyproject.toml b/modular_robot_physical/pyproject.toml index 14f097054..c3f806e02 100644 --- a/modular_robot_physical/pyproject.toml +++ b/modular_robot_physical/pyproject.toml @@ -4,12 +4,12 @@ build-backend = "poetry.core.masonry.api" [tool.poetry] name = "revolve2-modular-robot-physical" -version = "1.0.1" +version = "1.0.2" description = "Revolve2: Everything for physical modular robot control. This package is intended to be installed on the modular robot hardware." readme = "../README.md" authors = [ "Aart Stuurman ", - "Oliver Weissl ", + "Oliver Weissl ", ] repository = "https://github.com/ci-group/revolve2" classifiers = [ @@ -27,8 +27,9 @@ packages = [{ include = "revolve2" }] [tool.poetry.dependencies] python = "^3.10,<3.12" -revolve2-modular-robot = "1.0.1" +revolve2-modular-robot = "1.0.2" revolve2-robohat = { version = "0.5.0", optional = true } +pyrr = "^0.10.3" 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 ab76f7682..0be69f254 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,6 +1,8 @@ from abc import ABC, abstractmethod from typing import Sequence +from pyrr import Vector3 + class PhysicalInterface(ABC): """Abstract implementation for interfacing with hardware.""" @@ -46,3 +48,30 @@ def get_multiple_servo_positions(self, pins: Sequence[int]) -> list[float]: :returns: The current positions. :raises NotImplementedError: If getting the servo position is not supported on this hardware. """ + + @abstractmethod + def get_imu_angular_rate(self) -> Vector3: + """ + Get the angular rate from the IMU. + + :returns: The angular rate. + :raises NotImplementedError: If the IMU is not supported on this hardware. + """ + + @abstractmethod + def get_imu_orientation(self) -> Vector3: + """ + Get the orientation from the IMU. + + :returns: The orientation. + :raises NotImplementedError: If the IMU is not supported on this hardware. + """ + + @abstractmethod + def get_imu_specific_force(self) -> Vector3: + """ + Get the specific force from the IMU. + + :returns: The specific force. + :raises NotImplementedError: If the IMU 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 d483d9b9a..c061d2d36 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 @@ -3,6 +3,7 @@ from typing import Sequence import pigpio +from pyrr import Vector3 from .._physical_interface import PhysicalInterface @@ -101,3 +102,27 @@ def get_multiple_servo_positions(self, pins: Sequence[int]) -> list[float]: :raises NotImplementedError: If getting the servo position is not supported on this hardware. """ raise NotImplementedError("Getting servo position not supported on v1 harware.") + + def get_imu_angular_rate(self) -> Vector3: + """ + Get the angular rate from the IMU. + + :raises NotImplementedError: Always. + """ + raise NotImplementedError() + + def get_imu_orientation(self) -> Vector3: + """ + Get the orientation from the IMU. + + :raises NotImplementedError: Always. + """ + raise NotImplementedError() + + def get_imu_specific_force(self) -> Vector3: + """ + Get the specific force from the IMU. + + :raises NotImplementedError: Always. + """ + raise NotImplementedError() 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 1ced1b57e..4353d4167 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,6 +1,7 @@ import math from typing import Sequence +from pyrr import Vector3 from robohatlib.hal.assemblyboard.PwmPlug import PwmPlug from robohatlib.hal.assemblyboard.servo.ServoData import ServoData from robohatlib.hal.assemblyboard.ServoAssemblyConfig import ServoAssemblyConfig @@ -136,3 +137,39 @@ def get_multiple_servo_positions(self, pins: Sequence[int]) -> list[float]: """ angles = self._robohat.get_servo_multiple_angles() return [(angles[pin] - 90) / 360.0 * math.pi * 2.0 for pin in pins] + + def get_imu_angular_rate(self) -> Vector3: + """ + Get the angular rate from the IMU. + + :returns: The angular rate. + :raises RuntimeError: When imu could not be read. + """ + gyro = self._robohat.get_imu_gyro() + if gyro is None: + raise RuntimeError("Could not get IMU gyro reading!") + return Vector3(gyro) + + def get_imu_orientation(self) -> Vector3: + """ + Get the orientation from the IMU. + + :returns: The orientation. + :raises RuntimeError: When imu could not be read. + """ + orientation = self._robohat.get_imu_magnetic_fields() + if orientation is None: + raise RuntimeError("Could not get IMU magnetic fields reading!") + return Vector3(orientation) + + def get_imu_specific_force(self) -> Vector3: + """ + Get the specific force from the IMU. + + :returns: The specific force. + :raises RuntimeError: When imu could not be read. + """ + accel = self._robohat.get_imu_acceleration() + if accel is None: + raise RuntimeError("Could not get IMU acceleration reading!") + return Vector3(accel) diff --git a/modular_robot_physical/revolve2/modular_robot_physical/remote/_imu_sensor_state_impl.py b/modular_robot_physical/revolve2/modular_robot_physical/remote/_imu_sensor_state_impl.py new file mode 100644 index 000000000..0ce09193c --- /dev/null +++ b/modular_robot_physical/revolve2/modular_robot_physical/remote/_imu_sensor_state_impl.py @@ -0,0 +1,52 @@ +from pyrr import Vector3 + +from revolve2.modular_robot.sensor_state import IMUSensorState + + +class IMUSensorStateImpl(IMUSensorState): + """The state of an IMU sensor.""" + + _specific_force: Vector3 + _angular_rate: Vector3 + _orientation: Vector3 + + def __init__( + self, specific_force: Vector3, angular_rate: Vector3, orientation: Vector3 + ) -> None: + """ + Initialize this object. + + :param specific_force: Speicfic force. + :param angular_rate: Angular rate. + :param orientation: Orientation. + """ + self._specific_force = specific_force + self._angular_rate = angular_rate + self._orientation = orientation + + @property + def specific_force(self) -> Vector3: + """ + Get the measured specific force. + + :returns: The measured specific force. + """ + return self._specific_force + + @property + def angular_rate(self) -> Vector3: + """ + Get the measured angular rate. + + :returns: The measured angular rate. + """ + return self._angular_rate + + @property + def orientation(self) -> Vector3: + """ + Get the measured orientation. + + :returns: The measured orientation. + """ + return self._orientation diff --git a/modular_robot_physical/revolve2/modular_robot_physical/remote/_modular_robot_sensor_state_impl_v1.py b/modular_robot_physical/revolve2/modular_robot_physical/remote/_modular_robot_sensor_state_impl_v1.py index e073cbdcf..dbbbc9cd5 100644 --- a/modular_robot_physical/revolve2/modular_robot_physical/remote/_modular_robot_sensor_state_impl_v1.py +++ b/modular_robot_physical/revolve2/modular_robot_physical/remote/_modular_robot_sensor_state_impl_v1.py @@ -1,6 +1,12 @@ -from revolve2.modular_robot.body.base import ActiveHingeSensor +from revolve2.modular_robot.body.sensors import ( + ActiveHingeSensor, + CameraSensor, + IMUSensor, +) from revolve2.modular_robot.sensor_state import ( ActiveHingeSensorState, + CameraSensorState, + IMUSensorState, ModularRobotSensorState, ) @@ -18,3 +24,21 @@ def get_active_hinge_sensor_state( :raises NotImplementedError: Always. """ raise NotImplementedError("V1 hardware does not support sensor reading.") + + def get_imu_sensor_state(self, sensor: IMUSensor) -> IMUSensorState: + """ + Get the state of the provided IMU sensor. + + :param sensor: The sensor. + :raises NotImplementedError: Always. + """ + raise NotImplementedError() + + def get_camera_sensor_state(self, sensor: CameraSensor) -> CameraSensorState: + """ + Get the state of the provided camera sensor. + + :param sensor: The sensor. + :raises NotImplementedError: Always. + """ + raise NotImplementedError() 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 index c90ddd6e6..732b7f608 100644 --- 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 @@ -1,32 +1,44 @@ -from revolve2.modular_robot.body.base import ActiveHingeSensor +from revolve2.modular_robot.body.sensors import ( + ActiveHingeSensor, + CameraSensor, + IMUSensor, +) from revolve2.modular_robot.sensor_state import ( ActiveHingeSensorState, + CameraSensorState, + IMUSensorState, ModularRobotSensorState, ) from .._uuid_key import UUIDKey from ._active_hinge_sensor_state_impl import ActiveHingeSensorStateImpl +from ._imu_sensor_state_impl import IMUSensorStateImpl class ModularRobotSensorStateImplV2(ModularRobotSensorState): """Implementation of ModularRobotSensorState for v2 robots.""" _hinge_sensor_mapping: dict[UUIDKey[ActiveHingeSensor], int] - _positions: dict[int, float] + _hinge_positions: dict[int, float] + + _imu_sensor_states: dict[UUIDKey[IMUSensor], IMUSensorStateImpl] def __init__( self, hinge_sensor_mapping: dict[UUIDKey[ActiveHingeSensor], int], - positions: dict[int, float], + hinge_positions: dict[int, float], + imu_sensor_states: dict[UUIDKey[IMUSensor], IMUSensorStateImpl], ) -> 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. + :param hinge_positions: Position of hinges accessed by pin id. + :param imu_sensor_states: State of the IMU sensors. """ self._hinge_sensor_mapping = hinge_sensor_mapping - self._positions = positions + self._hinge_positions = hinge_positions + self._imu_sensor_states = imu_sensor_states def get_active_hinge_sensor_state( self, sensor: ActiveHingeSensor @@ -38,5 +50,29 @@ def get_active_hinge_sensor_state( :returns: The Sensor State. """ return ActiveHingeSensorStateImpl( - self._positions[self._hinge_sensor_mapping[UUIDKey(sensor)]] + self._hinge_positions[self._hinge_sensor_mapping[UUIDKey(sensor)]] ) + + def get_imu_sensor_state(self, sensor: IMUSensor) -> IMUSensorState: + """ + Get the state of the provided IMU sensor. + + :param sensor: The sensor. + :raises ValueError: If IMU sensors is not part of the robot. + :returns: The state. + """ + state = self._imu_sensor_states.get(UUIDKey(sensor)) + if state is None: + raise ValueError( + "State for IMU sensor not found. Does it exist in the robot definition?" + ) + return state + + def get_camera_sensor_state(self, sensor: CameraSensor) -> CameraSensorState: + """ + Get the state of the provided camera sensor. + + :param sensor: The sensor. + :raises NotImplementedError: Always. + """ + raise NotImplementedError() 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 0f4e13ed9..341e6ebf0 100644 --- a/modular_robot_physical/revolve2/modular_robot_physical/remote/_remote.py +++ b/modular_robot_physical/revolve2/modular_robot_physical/remote/_remote.py @@ -3,6 +3,7 @@ from typing import Callable import capnp +from pyrr import Vector3 from revolve2.modular_robot.body.base import ActiveHinge from revolve2.modular_robot.sensor_state import ModularRobotSensorState @@ -13,6 +14,7 @@ from .._standard_port import STANDARD_PORT from .._uuid_key import UUIDKey from ..robot_daemon_api import robot_daemon_protocol_capnp +from ._imu_sensor_state_impl import IMUSensorStateImpl from ._modular_robot_control_interface_impl import ModularRobotControlInterfaceImpl from ._modular_robot_sensor_state_impl_v1 import ModularRobotSensorStateImplV1 from ._modular_robot_sensor_state_impl_v2 import ModularRobotSensorStateImplV2 @@ -153,12 +155,25 @@ async def _run_remote_impl( robot_daemon_protocol_capnp.ReadSensorsArgs(readPins=pins) ) ).response + if config.modular_robot.body.core.imu_sensor is None: + imu_sensor_states = {} + else: + imu_sensor_states = { + UUIDKey( + config.modular_robot.body.core.imu_sensor + ): IMUSensorStateImpl( + _capnp_to_vector3(sensor_readings.imuSpecificForce), + _capnp_to_vector3(sensor_readings.imuAngularRate), + _capnp_to_vector3(sensor_readings.imuOrientation), + ) + } sensor_state = ModularRobotSensorStateImplV2( hinge_sensor_mapping=active_hinge_sensor_to_pin, - positions={ + hinge_positions={ pin: position for pin, position in zip(pins, sensor_readings.pins) }, + imu_sensor_states=imu_sensor_states, ) case _: raise NotImplementedError("Hardware type not supported.") @@ -207,12 +222,25 @@ async def _run_remote_impl( ) ) ).response + if config.modular_robot.body.core.imu_sensor is None: + imu_sensor_states = {} + else: + imu_sensor_states = { + UUIDKey( + config.modular_robot.body.core.imu_sensor + ): IMUSensorStateImpl( + _capnp_to_vector3(sensor_readings.imuSpecificForce), + _capnp_to_vector3(sensor_readings.imuAngularRate), + _capnp_to_vector3(sensor_readings.imuOrientation), + ) + } sensor_state = ModularRobotSensorStateImplV2( hinge_sensor_mapping=active_hinge_sensor_to_pin, - positions={ + hinge_positions={ pin: position for pin, position in zip(pins, sensor_readings.pins) }, + imu_sensor_states=imu_sensor_states, ) if battery_print_timer > 5.0: @@ -222,6 +250,10 @@ async def _run_remote_impl( raise NotImplementedError("Hardware type not supported.") +def _capnp_to_vector3(vector: robot_daemon_protocol_capnp.Vector3) -> Vector3: + return Vector3([vector.x, vector.y, vector.z]) + + def run_remote( config: Config, hostname: str, 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 c428c9238..0d49e97a0 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 @@ -2,6 +2,8 @@ import time from typing import Any, Sequence +from pyrr import Vector3 + from .._hardware_type import HardwareType from .._protocol_version import PROTOCOL_VERSION from ..physical_interfaces import PhysicalInterface @@ -248,6 +250,20 @@ def _get_sensor_readings( battery = self._physical_interface.get_battery_level() + imu_orientation = self._physical_interface.get_imu_orientation() + imu_specific_force = self._physical_interface.get_imu_specific_force() + imu_angular_rate = self._physical_interface.get_imu_angular_rate() + return robot_daemon_protocol_capnp.SensorReadings( - pins=pins_readings, battery=battery + pins=pins_readings, + battery=battery, + imuOrientation=self._vector3_to_capnp(imu_orientation), + imuSpecificForce=self._vector3_to_capnp(imu_specific_force), + imuAngularRate=self._vector3_to_capnp(imu_angular_rate), + ) + + @staticmethod + def _vector3_to_capnp(vector: Vector3) -> Vector3: + return robot_daemon_protocol_capnp.Vector3( + x=float(vector.x), y=float(vector.y), z=float(vector.z) ) 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 1577824b6..e2d9fa269 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 @@ -33,9 +33,18 @@ struct ControlAndReadSensorsArgs { readPins @1 :List(Int32); } +struct Vector3 { + x @0 :Float32; + y @1 :Float32; + z @2 :Float32; +} + struct SensorReadings { pins @0 :List(Float32); battery @1 :Float32; + imuOrientation @2 :Vector3; + imuSpecificForce @3 :Vector3; + imuAngularRate @4 :Vector3; } interface RoboServer { 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 49245fd20..38c8d2b99 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 @@ -12,7 +12,7 @@ module_file = os.path.abspath(os.path.join(here, "robot_daemon_protocol.capnp")) SetupArgs = capnp.load(module_file).SetupArgs SetupArgsBuilder = SetupArgs -SetupaArgsReader = SetupArgs +SetupArgsReader = SetupArgs SetupResponse = capnp.load(module_file).SetupResponse SetupResponseBuilder = SetupResponse SetupResponseReader = SetupResponse @@ -28,6 +28,9 @@ ControlAndReadSensorsArgs = capnp.load(module_file).ControlAndReadSensorsArgs ControlAndReadSensorsArgsBuilder = ControlAndReadSensorsArgs ControlAndReadSensorsArgsReader = ControlAndReadSensorsArgs +Vector3 = capnp.load(module_file).Vector3 +Vector3Builder = Vector3 +Vector3Reader = Vector3 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 9a2caf59b..a5532de14 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 @@ -251,10 +251,58 @@ class ControlAndReadSensorsArgsBuilder(ControlAndReadSensorsArgs): @staticmethod def write_packed(file: BufferedWriter) -> None: ... +class Vector3: + x: float + y: float + z: float + def __init__(self, x: float, y: float, z: float) -> None: ... + @staticmethod + @contextmanager + def from_bytes( + data: bytes, + traversal_limit_in_words: int | None = ..., + nesting_limit: int | None = ..., + ) -> Iterator[Vector3Reader]: ... + @staticmethod + def from_bytes_packed( + data: bytes, + traversal_limit_in_words: int | None = ..., + nesting_limit: int | None = ..., + ) -> Vector3Reader: ... + @staticmethod + def new_message() -> Vector3Builder: ... + def to_dict(self) -> dict[Any, Any]: ... + +class Vector3Reader(Vector3): + def as_builder(self) -> Vector3Builder: ... + +class Vector3Builder(Vector3): + @staticmethod + def from_dict(dictionary: dict[Any, Any]) -> Vector3Builder: ... + def copy(self) -> Vector3Builder: ... + def to_bytes(self) -> bytes: ... + def to_bytes_packed(self) -> bytes: ... + def to_segments(self) -> list[bytes]: ... + def as_reader(self) -> Vector3Reader: ... + @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: ... + imuOrientation: Vector3 | Vector3Builder | Vector3Reader + imuSpecificForce: Vector3 | Vector3Builder | Vector3Reader + imuAngularRate: Vector3 | Vector3Builder | Vector3Reader + def __init__( + self, + pins: Sequence[float], + battery: float, + imuOrientation: Vector3 | Vector3Builder | Vector3Reader, + imuSpecificForce: Vector3 | Vector3Builder | Vector3Reader, + imuAngularRate: Vector3 | Vector3Builder | Vector3Reader, + ) -> None: ... @staticmethod @contextmanager def from_bytes( diff --git a/modular_robot_simulation/pyproject.toml b/modular_robot_simulation/pyproject.toml index 21cfe417e..5dc4eda74 100644 --- a/modular_robot_simulation/pyproject.toml +++ b/modular_robot_simulation/pyproject.toml @@ -4,10 +4,13 @@ build-backend = "poetry.core.masonry.api" [tool.poetry] name = "revolve2-modular-robot-simulation" -version = "1.0.1" +version = "1.0.2" description = "Revolve2: Functionality to define scenes with modular robots in a terrain and simulate them." readme = "../README.md" -authors = ["Aart Stuurman "] +authors = [ + "Aart Stuurman ", + "Oliver Weissl " +] repository = "https://github.com/ci-group/revolve2" classifiers = [ "Development Status :: 4 - Beta", @@ -24,8 +27,8 @@ packages = [{ include = "revolve2" }] [tool.poetry.dependencies] python = "^3.10,<3.12" -revolve2-modular-robot = "1.0.1" -revolve2-simulation = "1.0.1" +revolve2-modular-robot = "1.0.2" +revolve2-simulation = "1.0.2" [tool.poetry.extras] dev = [] diff --git a/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_body_to_multi_body_system_converter.py b/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_body_to_multi_body_system_converter.py index f263742bb..1d9c83a5b 100644 --- a/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_body_to_multi_body_system_converter.py +++ b/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_body_to_multi_body_system_converter.py @@ -5,6 +5,7 @@ from revolve2.modular_robot.body.base import Body from revolve2.simulation.scene import MultiBodySystem, Pose, RigidBody +from revolve2.simulation.scene.sensors import IMUSensor from ._body_to_multi_body_system_mapping import BodyToMultiBodySystemMapping from ._get_builder import get_builder @@ -29,14 +30,21 @@ def convert_robot_body( :returns: The created multi-body system, and a mapping from body to multi-body system. """ multi_body_system = MultiBodySystem(pose=pose, is_static=False) - mapping = BodyToMultiBodySystemMapping() + + core_imu = IMUSensor(pose=Pose(position=Vector3([0.0, 0.0, 0.0]))) rigid_body = RigidBody( initial_pose=Pose(), static_friction=self._STATIC_FRICTION, dynamic_friction=self._DYNAMIC_FRICTION, geometries=[], + imu_sensors=[core_imu], + ) + + mapping = BodyToMultiBodySystemMapping( + core_imu=core_imu, multi_body_system=multi_body_system ) + multi_body_system.add_rigid_body(rigid_body) unbuilt = UnbuiltChild( diff --git a/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_body_to_multi_body_system_mapping.py b/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_body_to_multi_body_system_mapping.py index 42930c4d2..058d81acc 100644 --- a/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_body_to_multi_body_system_mapping.py +++ b/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_body_to_multi_body_system_mapping.py @@ -1,13 +1,17 @@ from dataclasses import dataclass, field -from revolve2.modular_robot.body.base import ActiveHinge, ActiveHingeSensor -from revolve2.simulation.scene import JointHinge, UUIDKey +from revolve2.modular_robot.body.base import ActiveHinge +from revolve2.modular_robot.body.sensors import ActiveHingeSensor +from revolve2.simulation.scene import JointHinge, MultiBodySystem, UUIDKey +from revolve2.simulation.scene.sensors import IMUSensor @dataclass(eq=False) class BodyToMultiBodySystemMapping: """Mappings from bodies to multi-body system objects.""" + core_imu: IMUSensor + multi_body_system: MultiBodySystem active_hinge_to_joint_hinge: dict[UUIDKey[ActiveHinge], JointHinge] = field( init=False, default_factory=dict ) diff --git a/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_builders/__init__.py b/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_builders/__init__.py new file mode 100644 index 000000000..43162bc99 --- /dev/null +++ b/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_builders/__init__.py @@ -0,0 +1,15 @@ +"""Builders for specific modules or the modular robots.""" + +from ._active_hinge_builder import ActiveHingeBuilder +from ._attachment_face_builder import AttachmentFaceBuilder +from ._brick_builder import BrickBuilder +from ._builder import Builder +from ._core_builder import CoreBuilder + +__all__ = [ + "ActiveHingeBuilder", + "AttachmentFaceBuilder", + "BrickBuilder", + "Builder", + "CoreBuilder", +] diff --git a/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_active_hinge_builder.py b/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_builders/_active_hinge_builder.py similarity index 96% rename from modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_active_hinge_builder.py rename to modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_builders/_active_hinge_builder.py index 567fb9012..367f38210 100644 --- a/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_active_hinge_builder.py +++ b/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_builders/_active_hinge_builder.py @@ -12,10 +12,10 @@ from revolve2.simulation.scene.geometry import GeometryBox from revolve2.simulation.scene.geometry.textures import Texture -from ._body_to_multi_body_system_mapping import BodyToMultiBodySystemMapping +from .._body_to_multi_body_system_mapping import BodyToMultiBodySystemMapping +from .._convert_color import convert_color +from .._unbuilt_child import UnbuiltChild from ._builder import Builder -from ._convert_color import convert_color -from ._unbuilt_child import UnbuiltChild class ActiveHingeBuilder(Builder): @@ -103,6 +103,7 @@ def build( static_friction=self._module.static_friction, dynamic_friction=self._module.dynamic_friction, geometries=[], + imu_sensors=[], ) multi_body_system.add_rigid_body(next_rigid_body) diff --git a/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_attachment_face_builder.py b/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_builders/_attachment_face_builder.py similarity index 93% rename from modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_attachment_face_builder.py rename to modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_builders/_attachment_face_builder.py index 89e94b05f..b805325aa 100644 --- a/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_attachment_face_builder.py +++ b/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_builders/_attachment_face_builder.py @@ -1,9 +1,9 @@ from revolve2.modular_robot.body.base import AttachmentFace from revolve2.simulation.scene import MultiBodySystem, Pose, RigidBody -from ._body_to_multi_body_system_mapping import BodyToMultiBodySystemMapping +from .._body_to_multi_body_system_mapping import BodyToMultiBodySystemMapping +from .._unbuilt_child import UnbuiltChild from ._builder import Builder -from ._unbuilt_child import UnbuiltChild class AttachmentFaceBuilder(Builder): diff --git a/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_brick_builder.py b/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_builders/_brick_builder.py similarity index 94% rename from modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_brick_builder.py rename to modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_builders/_brick_builder.py index f3df09bf4..2f004fe74 100644 --- a/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_brick_builder.py +++ b/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_builders/_brick_builder.py @@ -5,10 +5,10 @@ from revolve2.simulation.scene.geometry import GeometryBox from revolve2.simulation.scene.geometry.textures import Texture -from ._body_to_multi_body_system_mapping import BodyToMultiBodySystemMapping +from .._body_to_multi_body_system_mapping import BodyToMultiBodySystemMapping +from .._convert_color import convert_color +from .._unbuilt_child import UnbuiltChild from ._builder import Builder -from ._convert_color import convert_color -from ._unbuilt_child import UnbuiltChild class BrickBuilder(Builder): diff --git a/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_builder.py b/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_builders/_builder.py similarity index 83% rename from modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_builder.py rename to modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_builders/_builder.py index 4f2fe99c2..0ca518505 100644 --- a/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_builder.py +++ b/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_builders/_builder.py @@ -2,8 +2,8 @@ from revolve2.simulation.scene import MultiBodySystem -from ._body_to_multi_body_system_mapping import BodyToMultiBodySystemMapping -from ._unbuilt_child import UnbuiltChild +from .._body_to_multi_body_system_mapping import BodyToMultiBodySystemMapping +from .._unbuilt_child import UnbuiltChild class Builder(ABC): diff --git a/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_core_builder.py b/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_builders/_core_builder.py similarity index 93% rename from modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_core_builder.py rename to modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_builders/_core_builder.py index f5eb5b146..2f1eb250a 100644 --- a/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_core_builder.py +++ b/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_builders/_core_builder.py @@ -3,10 +3,10 @@ from revolve2.simulation.scene.geometry import GeometryBox from revolve2.simulation.scene.geometry.textures import Texture -from ._body_to_multi_body_system_mapping import BodyToMultiBodySystemMapping +from .._body_to_multi_body_system_mapping import BodyToMultiBodySystemMapping +from .._convert_color import convert_color +from .._unbuilt_child import UnbuiltChild from ._builder import Builder -from ._convert_color import convert_color -from ._unbuilt_child import UnbuiltChild class CoreBuilder(Builder): diff --git a/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_get_builder.py b/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_get_builder.py index 6d902632c..15fe6d0b7 100644 --- a/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_get_builder.py +++ b/modular_robot_simulation/revolve2/modular_robot_simulation/_build_multi_body_systems/_get_builder.py @@ -1,10 +1,12 @@ from revolve2.modular_robot.body.base import ActiveHinge, AttachmentFace, Brick, Core -from ._active_hinge_builder import ActiveHingeBuilder -from ._attachment_face_builder import AttachmentFaceBuilder -from ._brick_builder import BrickBuilder -from ._builder import Builder -from ._core_builder import CoreBuilder +from ._builders import ( + ActiveHingeBuilder, + AttachmentFaceBuilder, + BrickBuilder, + Builder, + CoreBuilder, +) from ._unbuilt_child import UnbuiltChild diff --git a/modular_robot_simulation/revolve2/modular_robot_simulation/_convert_terrain.py b/modular_robot_simulation/revolve2/modular_robot_simulation/_convert_terrain.py index 9d2dff2e7..78b491c83 100644 --- a/modular_robot_simulation/revolve2/modular_robot_simulation/_convert_terrain.py +++ b/modular_robot_simulation/revolve2/modular_robot_simulation/_convert_terrain.py @@ -16,6 +16,7 @@ def convert_terrain(terrain: Terrain) -> MultiBodySystem: static_friction=1.0, dynamic_friction=1.0, geometries=[], + imu_sensors=[], ) # We use these friction values but in the future they should be set through the terrain description. multi_body_system.add_rigid_body(rigid_body) diff --git a/modular_robot_simulation/revolve2/modular_robot_simulation/_modular_robot_simulation_handler.py b/modular_robot_simulation/revolve2/modular_robot_simulation/_modular_robot_simulation_handler.py index 1e3bf9136..f2d6fc1e1 100644 --- a/modular_robot_simulation/revolve2/modular_robot_simulation/_modular_robot_simulation_handler.py +++ b/modular_robot_simulation/revolve2/modular_robot_simulation/_modular_robot_simulation_handler.py @@ -7,7 +7,7 @@ from ._build_multi_body_systems import BodyToMultiBodySystemMapping from ._modular_robot_control_interface_impl import ModularRobotControlInterfaceImpl -from ._modular_robot_sensor_state_impl import ModularRobotSensorStateImpl +from ._sensor_state_impl import ModularRobotSensorStateImpl class ModularRobotSimulationHandler(SimulationHandler): diff --git a/modular_robot_simulation/revolve2/modular_robot_simulation/_sensor_state_impl/__init__.py b/modular_robot_simulation/revolve2/modular_robot_simulation/_sensor_state_impl/__init__.py new file mode 100644 index 000000000..0ce76dad9 --- /dev/null +++ b/modular_robot_simulation/revolve2/modular_robot_simulation/_sensor_state_impl/__init__.py @@ -0,0 +1,10 @@ +"""Sensor state implementations for the simulations.""" +from ._active_hinge_sensor_state_impl import ActiveHingeSensorStateImpl +from ._imu_sensor_state_impl import IMUSensorStateImpl +from ._modular_robot_sensor_state_impl import ModularRobotSensorStateImpl + +__all__ = [ + "ActiveHingeSensorStateImpl", + "IMUSensorStateImpl", + "ModularRobotSensorStateImpl", +] diff --git a/modular_robot_simulation/revolve2/modular_robot_simulation/_active_hinge_sensor_state_impl.py b/modular_robot_simulation/revolve2/modular_robot_simulation/_sensor_state_impl/_active_hinge_sensor_state_impl.py similarity index 100% rename from modular_robot_simulation/revolve2/modular_robot_simulation/_active_hinge_sensor_state_impl.py rename to modular_robot_simulation/revolve2/modular_robot_simulation/_sensor_state_impl/_active_hinge_sensor_state_impl.py diff --git a/modular_robot_simulation/revolve2/modular_robot_simulation/_sensor_state_impl/_imu_sensor_state_impl.py b/modular_robot_simulation/revolve2/modular_robot_simulation/_sensor_state_impl/_imu_sensor_state_impl.py new file mode 100644 index 000000000..874228ee4 --- /dev/null +++ b/modular_robot_simulation/revolve2/modular_robot_simulation/_sensor_state_impl/_imu_sensor_state_impl.py @@ -0,0 +1,59 @@ +from pyrr import Quaternion, Vector3 + +from revolve2.modular_robot.sensor_state import IMUSensorState +from revolve2.simulation.scene import MultiBodySystem, SimulationState +from revolve2.simulation.scene.sensors import IMUSensor + + +class IMUSensorStateImpl(IMUSensorState): + """Implements the IMU sensor state.""" + + _simulation_state: SimulationState + _multi_body_system: MultiBodySystem + _core_imu: IMUSensor + + def __init__( + self, + simulation_state: SimulationState, + multi_body_system: MultiBodySystem, + core_imu: IMUSensor, + ) -> None: + """ + Initialize this object. + + :param simulation_state: The state of the simulation. + :param multi_body_system: The multi body system this imu is attached to. + :param core_imu: The imu attached to the core. + """ + self._simulation_state = simulation_state + self._multi_body_system = multi_body_system + self._core_imu = core_imu + + @property + def specific_force(self) -> Vector3: + """ + Get the measured specific force. + + :returns: The measured specific force. + """ + return self._simulation_state.get_imu_specific_force(self._core_imu) + + @property + def angular_rate(self) -> Vector3: + """ + Get the measured angular rate. + + :returns: The measured angular rate. + """ + return self._simulation_state.get_imu_angular_rate(self._core_imu) + + @property + def orientation(self) -> Quaternion: + """ + Get the measured orientation. + + :returns: The measured position. + """ + return self._simulation_state.get_multi_body_system_pose( + self._multi_body_system + ).orientation diff --git a/modular_robot_simulation/revolve2/modular_robot_simulation/_modular_robot_sensor_state_impl.py b/modular_robot_simulation/revolve2/modular_robot_simulation/_sensor_state_impl/_modular_robot_sensor_state_impl.py similarity index 59% rename from modular_robot_simulation/revolve2/modular_robot_simulation/_modular_robot_sensor_state_impl.py rename to modular_robot_simulation/revolve2/modular_robot_simulation/_sensor_state_impl/_modular_robot_sensor_state_impl.py index 75c408309..6b4b6b558 100644 --- a/modular_robot_simulation/revolve2/modular_robot_simulation/_modular_robot_sensor_state_impl.py +++ b/modular_robot_simulation/revolve2/modular_robot_simulation/_sensor_state_impl/_modular_robot_sensor_state_impl.py @@ -1,12 +1,19 @@ -from revolve2.modular_robot.body.base import ActiveHingeSensor +from revolve2.modular_robot.body.sensors import ( + ActiveHingeSensor, + CameraSensor, + IMUSensor, +) from revolve2.modular_robot.sensor_state import ( ActiveHingeSensorState, + CameraSensorState, + IMUSensorState, ModularRobotSensorState, ) from revolve2.simulation.scene import SimulationState, UUIDKey +from .._build_multi_body_systems import BodyToMultiBodySystemMapping from ._active_hinge_sensor_state_impl import ActiveHingeSensorStateImpl -from ._build_multi_body_systems import BodyToMultiBodySystemMapping +from ._imu_sensor_state_impl import IMUSensorStateImpl class ModularRobotSensorStateImpl(ModularRobotSensorState): @@ -46,3 +53,25 @@ def get_active_hinge_sensor_state( return ActiveHingeSensorStateImpl( simulation_state=self._simulation_state, hinge_joint=maybe_joint ) + + def get_imu_sensor_state(self, sensor: IMUSensor) -> IMUSensorState: + """ + Get the state of the provided IMU sensor. + + :param sensor: The sensor. + :returns: The state. + """ + return IMUSensorStateImpl( + simulation_state=self._simulation_state, + multi_body_system=self._body_to_multi_body_system_mapping.multi_body_system, + core_imu=self._body_to_multi_body_system_mapping.core_imu, + ) + + def get_camera_sensor_state(self, sensor: CameraSensor) -> CameraSensorState: + """ + Get the state of the camera sensor. + + :param sensor: The sensor. + :raises NotImplementedError: It is not implemented. + """ + raise NotImplementedError("Camera not yet defined for simulation") diff --git a/simulation/pyproject.toml b/simulation/pyproject.toml index e6c415abf..49230b967 100644 --- a/simulation/pyproject.toml +++ b/simulation/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "poetry.core.masonry.api" [tool.poetry] name = "revolve2-simulation" -version = "1.0.1" +version = "1.0.2" description = "Revolve2: Physics simulation abstraction layer." readme = "../README.md" authors = [ diff --git a/simulation/revolve2/simulation/scene/_camera.py b/simulation/revolve2/simulation/scene/_camera.py deleted file mode 100644 index 9ead8fcf8..000000000 --- a/simulation/revolve2/simulation/scene/_camera.py +++ /dev/null @@ -1,19 +0,0 @@ -from dataclasses import dataclass - -from ._pose import Pose -from ._rigid_body import RigidBody - - -@dataclass -class Camera: - """Camera sensor.""" - - parent: RigidBody - """ - Parent rigid body. - - The camera will be rigidly attached to its parent, relatively using the camera's pose. - """ - - pose: Pose - """Pose of the geometry, relative to its parent rigid body.""" diff --git a/simulation/revolve2/simulation/scene/_rigid_body.py b/simulation/revolve2/simulation/scene/_rigid_body.py index 0cc5b112b..28ddec4dc 100644 --- a/simulation/revolve2/simulation/scene/_rigid_body.py +++ b/simulation/revolve2/simulation/scene/_rigid_body.py @@ -5,6 +5,7 @@ from ._pose import Pose from .geometry import Geometry, GeometryBox +from .sensors import IMUSensor @dataclass(kw_only=True) @@ -38,6 +39,9 @@ def uuid(self) -> uuid.UUID: geometries: list[Geometry] """Geometries describing the shape of the body.""" + imu_sensors: list[IMUSensor] + """The IMU sensors attached to this rigid body.""" + def mass(self) -> float: """Get mass of the rigid body. diff --git a/simulation/revolve2/simulation/scene/_simulation_state.py b/simulation/revolve2/simulation/scene/_simulation_state.py index 0ebbd4340..4920627c8 100644 --- a/simulation/revolve2/simulation/scene/_simulation_state.py +++ b/simulation/revolve2/simulation/scene/_simulation_state.py @@ -1,9 +1,12 @@ from abc import ABC, abstractmethod +from pyrr import Vector3 + from ._joint_hinge import JointHinge from ._multi_body_system import MultiBodySystem from ._pose import Pose from ._rigid_body import RigidBody +from .sensors import IMUSensor class SimulationState(ABC): @@ -44,3 +47,21 @@ def get_hinge_joint_position(self, joint: JointHinge) -> float: :param joint: The joint to get the rotational position for. :returns: The rotational position. """ + + @abstractmethod + def get_imu_specific_force(self, imu_sensor: IMUSensor) -> Vector3: + """ + Get the specific force measured an IMU. + + :param imu_sensor: The IMU. + :returns: The specific force. + """ + + @abstractmethod + def get_imu_angular_rate(self, imu_sensor: IMUSensor) -> Vector3: + """ + Get the angular rate measured by am IMU. + + :param imu_sensor: The IMU. + :returns: The angular rate. + """ diff --git a/simulation/revolve2/simulation/scene/conversion/_multi_body_system_to_urdf.py b/simulation/revolve2/simulation/scene/conversion/_multi_body_system_to_urdf.py index acab8546e..50fe2f575 100644 --- a/simulation/revolve2/simulation/scene/conversion/_multi_body_system_to_urdf.py +++ b/simulation/revolve2/simulation/scene/conversion/_multi_body_system_to_urdf.py @@ -21,6 +21,7 @@ def multi_body_system_to_urdf( list[GeometryHeightmap], list[tuple[JointHinge, str]], list[tuple[Geometry, str]], + list[tuple[RigidBody, str]], ]: """ Convert a multi-body system to URDF. @@ -33,7 +34,7 @@ def multi_body_system_to_urdf( :param multi_body_system: The multi-body system to convert. :param name: The name to using in the URDF. It will be a prefix for every name in the model. - :returns: A urdf string, plane geometries, heightmap geometries, joints and their names in the urdf, geometries and their names in the urdf + :returns: A urdf string, plane geometries, heightmap geometries, joints and their names in the urdf, geometries and their names in the urdf, rigid bodies and their names in the urdf. :raises ValueError: In case the graph is cyclic. # noqa: DAR402 ValueError @@ -47,6 +48,7 @@ class _URDFConverter: visited_rigid_bodies: set[uuid.UUID] # their indices joints_and_names: list[tuple[JointHinge, str]] geometries_and_names: list[tuple[Geometry, str]] + rigid_bodies_and_names: list[tuple[RigidBody, str]] planes: list[GeometryPlane] heightmaps: list[GeometryHeightmap] @@ -58,6 +60,7 @@ def build( list[GeometryHeightmap], list[tuple[JointHinge, str]], list[tuple[Geometry, str]], + list[tuple[RigidBody, str]], ]: assert multi_body_system.has_root() @@ -65,6 +68,7 @@ def build( self.visited_rigid_bodies = set() self.joints_and_names = [] self.geometries_and_names = [] + self.rigid_bodies_and_names = [] self.planes = [] self.heightmaps = [] @@ -73,7 +77,7 @@ def build( for element in self._make_links_xml_elements( multi_body_system.root, multi_body_system.root.initial_pose, - f"{name}_root", + f"{name}", parent_rigid_body=None, ): urdf.append(element) @@ -86,6 +90,7 @@ def build( self.heightmaps, self.joints_and_names, self.geometries_and_names, + self.rigid_bodies_and_names, ) def _make_links_xml_elements( @@ -103,6 +108,7 @@ def _make_links_xml_elements( link = xml.Element("link", {"name": rigid_body_name}) elements.append(link) + self.rigid_bodies_and_names.append((rigid_body, rigid_body_name)) com_xyz = link_pose.orientation.inverse * ( rigid_body.initial_pose.position diff --git a/simulation/revolve2/simulation/scene/sensors/__init__.py b/simulation/revolve2/simulation/scene/sensors/__init__.py new file mode 100644 index 000000000..6162ad197 --- /dev/null +++ b/simulation/revolve2/simulation/scene/sensors/__init__.py @@ -0,0 +1,6 @@ +"""Sensor classes for the simulators.""" +from ._camera_sensor import CameraSensor +from ._imu_sensor import IMUSensor +from ._sensor import Sensor + +__all__ = ["CameraSensor", "IMUSensor", "Sensor"] diff --git a/simulation/revolve2/simulation/scene/sensors/_camera_sensor.py b/simulation/revolve2/simulation/scene/sensors/_camera_sensor.py new file mode 100644 index 000000000..7325a4e35 --- /dev/null +++ b/simulation/revolve2/simulation/scene/sensors/_camera_sensor.py @@ -0,0 +1,12 @@ +from dataclasses import dataclass + +from .._pose import Pose +from ._sensor import Sensor + + +@dataclass +class CameraSensor(Sensor): + """Camera sensor.""" + + pose: Pose + """Pose of the geometry, relative to its parent rigid body.""" diff --git a/simulation/revolve2/simulation/scene/sensors/_imu_sensor.py b/simulation/revolve2/simulation/scene/sensors/_imu_sensor.py new file mode 100644 index 000000000..1beaeacc1 --- /dev/null +++ b/simulation/revolve2/simulation/scene/sensors/_imu_sensor.py @@ -0,0 +1,15 @@ +from dataclasses import dataclass + +from .._pose import Pose +from ._sensor import Sensor + + +@dataclass +class IMUSensor(Sensor): + """ + An inertial measurement unit. + + Reports specific force(closely related to acceleration), angular rate(closely related to angularvelocity), and orientation. + """ + + pose: Pose diff --git a/simulation/revolve2/simulation/scene/sensors/_sensor.py b/simulation/revolve2/simulation/scene/sensors/_sensor.py new file mode 100644 index 000000000..2580aeb1a --- /dev/null +++ b/simulation/revolve2/simulation/scene/sensors/_sensor.py @@ -0,0 +1,23 @@ +import uuid +from abc import ABC +from dataclasses import dataclass, field + + +@dataclass +class Sensor(ABC): + """ + An inertial measurement unit. + + Reports specific force(closely related to acceleration), angular rate(closely related to angularvelocity), and orientation. + """ + + _uuid: uuid.UUID = field(init=False, default_factory=uuid.uuid1) + + @property + def uuid(self) -> uuid.UUID: + """ + Get the uuid. + + :returns: The uuid. + """ + return self._uuid diff --git a/simulators/mujoco_simulator/pyproject.toml b/simulators/mujoco_simulator/pyproject.toml index f90399286..426e099d4 100644 --- a/simulators/mujoco_simulator/pyproject.toml +++ b/simulators/mujoco_simulator/pyproject.toml @@ -4,10 +4,13 @@ build-backend = "poetry.core.masonry.api" [tool.poetry] name = "revolve2-mujoco-simulator" -version = "1.0.1" +version = "1.0.2" description = "Revolve2: MuJoCo simulator." readme = "../../README.md" -authors = ["Aart Stuurman "] +authors = [ + "Aart Stuurman ", + "Oliver Weissl " +] repository = "https://github.com/ci-group/revolve2" classifiers = [ "Development Status :: 4 - Beta", @@ -24,7 +27,7 @@ packages = [{ include = "revolve2" }] [tool.poetry.dependencies] python = "^3.10,<3.12" -revolve2-simulation = "1.0.1" +revolve2-simulation = "1.0.2" mujoco-python-viewer = "^0.1.3" mujoco = "^2.2.0" dm-control = "^1.0.3" diff --git a/simulators/mujoco_simulator/revolve2/simulators/mujoco_simulator/_abstraction_to_mujoco_mapping.py b/simulators/mujoco_simulator/revolve2/simulators/mujoco_simulator/_abstraction_to_mujoco_mapping.py index 39717b946..42e56b4de 100644 --- a/simulators/mujoco_simulator/revolve2/simulators/mujoco_simulator/_abstraction_to_mujoco_mapping.py +++ b/simulators/mujoco_simulator/revolve2/simulators/mujoco_simulator/_abstraction_to_mujoco_mapping.py @@ -1,6 +1,7 @@ from dataclasses import dataclass, field from revolve2.simulation.scene import JointHinge, MultiBodySystem, UUIDKey +from revolve2.simulation.scene.sensors import IMUSensor @dataclass @@ -19,6 +20,14 @@ class MultiBodySystemMujoco: id: int +@dataclass +class IMUSensorMujoco: + """Information about a MuJoCo IMU sensor.""" + + gyro_id: int + accelerometer_id: int + + @dataclass(eq=False) class AbstractionToMujocoMapping: """Data to interpret a MuJoCo model using the simulation abstraction.""" @@ -30,3 +39,7 @@ class AbstractionToMujocoMapping: multi_body_system: dict[UUIDKey[MultiBodySystem], MultiBodySystemMujoco] = field( init=False, default_factory=dict ) + + imu_sensor: dict[UUIDKey[IMUSensor], IMUSensorMujoco] = field( + init=False, default_factory=dict + ) diff --git a/simulators/mujoco_simulator/revolve2/simulators/mujoco_simulator/_custom_mujoco_viewer.py b/simulators/mujoco_simulator/revolve2/simulators/mujoco_simulator/_custom_mujoco_viewer.py index f438bb73a..b732f11ac 100644 --- a/simulators/mujoco_simulator/revolve2/simulators/mujoco_simulator/_custom_mujoco_viewer.py +++ b/simulators/mujoco_simulator/revolve2/simulators/mujoco_simulator/_custom_mujoco_viewer.py @@ -56,11 +56,6 @@ def __init__( :param render_every_frame: If every frame is rendered or not. :param mode: The mode of the viewer (classic, manual). """ - self._viewer_mode = mode - self._position = 0 - self._paused = start_paused - self._render_every_frame = render_every_frame - self._mujoco_version = tuple(map(int, mujoco.__version__.split("."))) super().__init__( model, data, @@ -70,6 +65,11 @@ def __init__( height=None, hide_menus=False, ) + self._viewer_mode = mode + self._position = 0 + self._paused = start_paused + self._mujoco_version = tuple(map(int, mujoco.__version__.split("."))) + self._render_every_frame = render_every_frame def _add_overlay(self, gridpos: int, text1: str, text2: str) -> None: """ diff --git a/simulators/mujoco_simulator/revolve2/simulators/mujoco_simulator/_scene_to_model.py b/simulators/mujoco_simulator/revolve2/simulators/mujoco_simulator/_scene_to_model.py index b968b7c69..20925bb2e 100644 --- a/simulators/mujoco_simulator/revolve2/simulators/mujoco_simulator/_scene_to_model.py +++ b/simulators/mujoco_simulator/revolve2/simulators/mujoco_simulator/_scene_to_model.py @@ -28,6 +28,7 @@ from ._abstraction_to_mujoco_mapping import ( AbstractionToMujocoMapping, + IMUSensorMujoco, JointHingeMujoco, MultiBodySystemMujoco, ) @@ -74,21 +75,22 @@ def scene_to_model( scene.multi_body_systems ) ] - all_joints_and_names = [v for _, _, _, v, _ in conversions] + all_joints_and_names = [c[3] for c in conversions] + all_rigid_bodies_and_names = [c[5] for c in conversions] heightmaps: list[GeometryHeightmap] = [] - for multi_body_system, ( - urdf, - plane_geometries, - heightmap_geometries, - joints_and_names, - geoms_and_names, - ) in zip( - scene.multi_body_systems, - conversions, - strict=True, - ): + for mbs_i, ( + multi_body_system, + ( + urdf, + plane_geometries, + heightmap_geometries, + joints_and_names, + geoms_and_names, + rigid_bodies_and_names, + ), + ) in enumerate(zip(scene.multi_body_systems, conversions, strict=True)): multi_body_system_model = mujoco.MjModel.from_xml_string(urdf) # MuJoCo can only save to a file, not directly to string, @@ -165,6 +167,43 @@ def scene_to_model( name=f"actuator_velocity_{name}", ) + # Add IMUs + for rigid_body, name in rigid_bodies_and_names: + if len(rigid_body.imu_sensors) > 0: + if name == f"mbs{mbs_i}": + rigid_body_mjcf = multi_body_system_mjcf.worldbody + else: + rigid_body_mjcf = multi_body_system_mjcf.find( + namespace="body", identifier=name + ) + for imu_i, imu in enumerate(rigid_body.imu_sensors): + site_name = f"{name}_site_imu_{imu_i}" + rigid_body_mjcf.add( + "site", + name=site_name, + pos=[ + imu.pose.position.x, + imu.pose.position.y, + imu.pose.position.z, + ], + quat=[ + imu.pose.orientation.x, + imu.pose.orientation.y, + imu.pose.orientation.z, + imu.pose.orientation.w, + ], + ) + gyro_name = f"imu_gyro_{name}_{imu_i}" + multi_body_system_mjcf.sensor.add( + "gyro", name=gyro_name, site=site_name + ) + accelerometer_name = f"imu_accelerometer_{name}_{imu_i}" + multi_body_system_mjcf.sensor.add( + "accelerometer", + name=accelerometer_name, + site=site_name, + ) + # Add plane geometries i_plane = 0 for plane in plane_geometries: @@ -290,6 +329,19 @@ def scene_to_model( id=model.body(f"mbs{mbs_i}/").id ) + # Create IMU map + for mbs_i, rigid_bodies_and_names in enumerate(all_rigid_bodies_and_names): + for rigid_body, name in rigid_bodies_and_names: + for imu_i, imu in enumerate(rigid_body.imu_sensors): + gyro_name = f"imu_gyro_{name}_{imu_i}" + accelerometer_name = f"imu_accelerometer_{name}_{imu_i}" + mapping.imu_sensor[UUIDKey(imu)] = IMUSensorMujoco( + gyro_id=model.sensor(f"mbs{mbs_i}/{gyro_name}").id, + accelerometer_id=model.sensor( + f"mbs{mbs_i}/{accelerometer_name}" + ).id, + ) + return (model, mapping) diff --git a/simulators/mujoco_simulator/revolve2/simulators/mujoco_simulator/_simulation_state_impl.py b/simulators/mujoco_simulator/revolve2/simulators/mujoco_simulator/_simulation_state_impl.py index f7f1ddfb8..7726e47f4 100644 --- a/simulators/mujoco_simulator/revolve2/simulators/mujoco_simulator/_simulation_state_impl.py +++ b/simulators/mujoco_simulator/revolve2/simulators/mujoco_simulator/_simulation_state_impl.py @@ -11,6 +11,7 @@ SimulationState, UUIDKey, ) +from revolve2.simulation.scene.sensors import IMUSensor from ._abstraction_to_mujoco_mapping import AbstractionToMujocoMapping @@ -21,6 +22,7 @@ class SimulationStateImpl(SimulationState): _xpos: npt.NDArray[np.float_] _xquat: npt.NDArray[np.float_] _qpos: npt.NDArray[np.float_] + _sensordata: npt.NDArray[np.float_] _abstraction_to_mujoco_mapping: AbstractionToMujocoMapping def __init__( @@ -40,6 +42,7 @@ def __init__( self._xpos = data.xpos.copy() self._xquat = data.xquat.copy() self._qpos = data.qpos.copy() + self._sensordata = data.sensordata.copy() self._abstraction_to_mujoco_mapping = abstraction_to_mujoco_mapping def get_rigid_body_relative_pose(self, rigid_body: RigidBody) -> Pose: @@ -88,3 +91,29 @@ def get_hinge_joint_position(self, joint: JointHinge) -> float: """ joint_mujoco = self._abstraction_to_mujoco_mapping.hinge_joint[UUIDKey(joint)] return float(self._qpos[joint_mujoco.id]) + + def get_imu_specific_force(self, imu_sensor: IMUSensor) -> Vector3: + """ + Get the specific force measured an IMU. + + :param imu_sensor: The IMU. + :returns: The specific force. + """ + accelerometer_id = self._abstraction_to_mujoco_mapping.imu_sensor[ + UUIDKey(imu_sensor) + ].accelerometer_id + specific_force = self._sensordata[accelerometer_id : accelerometer_id + 3] + return Vector3(specific_force) + + def get_imu_angular_rate(self, imu_sensor: IMUSensor) -> Vector3: + """ + Get the angular rate measured by am IMU. + + :param imu_sensor: The IMU. + :returns: The angular rate. + """ + gyro_id = self._abstraction_to_mujoco_mapping.imu_sensor[ + UUIDKey(imu_sensor) + ].gyro_id + angular_rate = self._sensordata[gyro_id : gyro_id + 3] + return Vector3(angular_rate) diff --git a/tests/examples/test_brain_with_feedback.py b/tests/examples/test_brain_with_feedback.py new file mode 100644 index 000000000..ed10f5d73 --- /dev/null +++ b/tests/examples/test_brain_with_feedback.py @@ -0,0 +1,30 @@ +import os +import sys +from unittest.mock import Mock + +from ..conftest import EXAMPLES_DIR +from ._clear_example_modules_from_cache import clear_exp_modules_from_cache +from ._patched_batch_parameters import make_patched_batch_parameters + + +def test_brain_with_feedback(mocker: Mock) -> None: + """ + Test brain_with_feedback example can complete. + :param mocker: The mock object. + """ + exp_dir = os.path.join(EXAMPLES_DIR, "brain_with_feedback") + # Clear any previously imported modules from examples directory from cache + clear_exp_modules_from_cache() + # Add examples directory to path, so we can import them without the examples being packages. + sys.path.insert(0, exp_dir) + # Override import for patching batch parameters. + mocker.patch("main.make_standard_batch_parameters", make_patched_batch_parameters) + + try: + # This type ignore is required since mypy cant resolve this import. + import main # type: ignore + + main.main() + finally: + # Remove the example directory from path, even if the test fails. + sys.path.pop(0)