diff --git a/.gitignore b/.gitignore index 036004b..4a7c700 100755 --- a/.gitignore +++ b/.gitignore @@ -3,7 +3,12 @@ docker/ earth_moving/clutter/__pycache__/ pyenv/ +.vscode/ +__pycache__/ +outputs/ +earth_moving/clutter/urd5/ # files core.* +*.pyc diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100755 index 5a56a4e..0000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,11 +0,0 @@ -{ - "cSpell.words": [ - "maxpos", - "minpos", - "Njoints", - "Nwaypoints", - "opengl", - "pybullet", - "urdf" - ] -} \ No newline at end of file diff --git a/earth_moving/clutter/scratchpad_ral.ipynb b/earth_moving/clutter/scratchpad_ral.ipynb new file mode 100755 index 0000000..8097d43 --- /dev/null +++ b/earth_moving/clutter/scratchpad_ral.ipynb @@ -0,0 +1,83 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [ + { + "ename": "ImportError", + "evalue": "cannot import name 'BaseSensorBackend' from partially initialized module 'sensor.sensor_backend' (most likely due to a circular import) (/home/fedeoli/Documents/Work/earth_moving/earth_moving/ral/sensor/sensor_backend.py)", + "output_type": "error", + "traceback": [ + "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[0;31mImportError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[0;32mIn[1], line 6\u001b[0m\n\u001b[1;32m 3\u001b[0m PATH \u001b[38;5;241m=\u001b[39m \u001b[38;5;124m'\u001b[39m\u001b[38;5;124m/home/fedeoli/Documents/Work/earth_moving/earth_moving/ral\u001b[39m\u001b[38;5;124m'\u001b[39m\n\u001b[1;32m 4\u001b[0m sys\u001b[38;5;241m.\u001b[39mpath\u001b[38;5;241m.\u001b[39mappend(PATH)\n\u001b[0;32m----> 6\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01msensor\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01msensor_backend\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m BaseSensorBackend\n\u001b[1;32m 7\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01msensor\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01msensor_rgb\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m SensorRGBBackend\n\u001b[1;32m 8\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mbackend\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mpybullet_backend\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m PybulletBackend\n", + "File \u001b[0;32m~/Documents/Work/earth_moving/earth_moving/ral/sensor/sensor_backend.py:2\u001b[0m\n\u001b[1;32m 1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mnumpy\u001b[39;00m \u001b[38;5;28;01mas\u001b[39;00m \u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[0;32m----> 2\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mbackend\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mbase_backend\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m BaseBackend\n\u001b[1;32m 5\u001b[0m \u001b[38;5;28;01mclass\u001b[39;00m \u001b[38;5;21;01mBaseSensorBackend\u001b[39;00m(BaseBackend): \n\u001b[1;32m 7\u001b[0m \u001b[38;5;28;01mdef\u001b[39;00m \u001b[38;5;21m__init__\u001b[39m(\u001b[38;5;28mself\u001b[39m, \u001b[38;5;241m*\u001b[39m\u001b[38;5;241m*\u001b[39mkwargs) \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m>\u001b[39m \u001b[38;5;28;01mNone\u001b[39;00m:\n", + "File \u001b[0;32m~/Documents/Work/earth_moving/earth_moving/ral/backend/base_backend.py:3\u001b[0m\n\u001b[1;32m 1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mnumpy\u001b[39;00m \u001b[38;5;28;01mas\u001b[39;00m \u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[1;32m 2\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mabc\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m ABC, abstractmethod\n\u001b[0;32m----> 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01msensor\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01msensor_backend\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m BaseSensorBackend\n\u001b[1;32m 4\u001b[0m \u001b[38;5;28;01mclass\u001b[39;00m \u001b[38;5;21;01mBaseBackend\u001b[39;00m(ABC):\n\u001b[1;32m 5\u001b[0m \u001b[38;5;28;01mdef\u001b[39;00m \u001b[38;5;21m__init__\u001b[39m(\u001b[38;5;28mself\u001b[39m) \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m>\u001b[39m \u001b[38;5;28;01mNone\u001b[39;00m:\n", + "\u001b[0;31mImportError\u001b[0m: cannot import name 'BaseSensorBackend' from partially initialized module 'sensor.sensor_backend' (most likely due to a circular import) (/home/fedeoli/Documents/Work/earth_moving/earth_moving/ral/sensor/sensor_backend.py)" + ] + } + ], + "source": [ + "import sys\n", + "\n", + "PATH = '/home/fedeoli/Documents/Work/earth_moving/earth_moving/ral'\n", + "sys.path.append(PATH)\n", + "\n", + "from sensor.sensor_backend import BaseSensorBackend\n", + "from sensor.sensor_rgb import SensorRGBBackend\n", + "from backend.pybullet_backend import PybulletBackend" + ] + }, + { + "cell_type": "code", + "execution_count": 16, + "metadata": {}, + "outputs": [ + { + "ename": "AttributeError", + "evalue": "'PybulletBackend' object has no attribute '_timedelta'", + "output_type": "error", + "traceback": [ + "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[0;31mAttributeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[0;32mIn[16], line 1\u001b[0m\n\u001b[0;32m----> 1\u001b[0m backend \u001b[38;5;241m=\u001b[39m \u001b[43mPybulletBackend\u001b[49m\u001b[43m(\u001b[49m\u001b[43mtimedelta\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[38;5;241;43m0.1\u001b[39;49m\u001b[43m)\u001b[49m\n\u001b[1;32m 2\u001b[0m cam \u001b[38;5;241m=\u001b[39m backend\u001b[38;5;241m.\u001b[39mPybulletSensorRGBBackend(\n\u001b[1;32m 3\u001b[0m name\u001b[38;5;241m=\u001b[39m\u001b[38;5;124m'\u001b[39m\u001b[38;5;124mcam\u001b[39m\u001b[38;5;124m'\u001b[39m,\n\u001b[1;32m 4\u001b[0m pose \u001b[38;5;241m=\u001b[39m [\u001b[38;5;241m0\u001b[39m,\u001b[38;5;241m0\u001b[39m,\u001b[38;5;241m0\u001b[39m,\u001b[38;5;241m0\u001b[39m,\u001b[38;5;241m0\u001b[39m,\u001b[38;5;241m0\u001b[39m],\n\u001b[1;32m 5\u001b[0m imgW\u001b[38;5;241m=\u001b[39m\u001b[38;5;124m'\u001b[39m\u001b[38;5;124m120\u001b[39m\u001b[38;5;124m'\u001b[39m,\n\u001b[1;32m 6\u001b[0m imgH\u001b[38;5;241m=\u001b[39m\u001b[38;5;124m'\u001b[39m\u001b[38;5;124m100\u001b[39m\u001b[38;5;124m'\u001b[39m\n\u001b[1;32m 7\u001b[0m )\n", + "File \u001b[0;32m~/Documents/Work/earth_moving/earth_moving/ral/backend/pybullet_backend.py:10\u001b[0m, in \u001b[0;36mPybulletBackend.__init__\u001b[0;34m(self, timedelta)\u001b[0m\n\u001b[1;32m 8\u001b[0m \u001b[38;5;28;01mdef\u001b[39;00m \u001b[38;5;21m__init__\u001b[39m(\u001b[38;5;28mself\u001b[39m, timedelta) \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m>\u001b[39m \u001b[38;5;28;01mNone\u001b[39;00m:\n\u001b[1;32m 9\u001b[0m \u001b[38;5;28msuper\u001b[39m()\u001b[38;5;241m.\u001b[39m\u001b[38;5;21m__init__\u001b[39m()\n\u001b[0;32m---> 10\u001b[0m \u001b[38;5;28;43mself\u001b[39;49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43m_timedelta\u001b[49m\n", + "\u001b[0;31mAttributeError\u001b[0m: 'PybulletBackend' object has no attribute '_timedelta'" + ] + } + ], + "source": [ + "backend = PybulletBackend(timedelta=0.1)\n", + "cam = backend.PybulletSensorRGBBackend(\n", + " name='cam',\n", + " pose = [0,0,0,0,0,0],\n", + " imgW='120',\n", + " imgH='100'\n", + " )" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "earthmov", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.3" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/earth_moving/ral/backend/base_backend.py b/earth_moving/ral/backend/base_backend.py index 3db8784..746340d 100644 --- a/earth_moving/ral/backend/base_backend.py +++ b/earth_moving/ral/backend/base_backend.py @@ -1,24 +1,24 @@ -import numpy as np - - -class BaseSensorBackend: # TODO: move to a separate file? - def __init__(self, **kwargs) -> None: - self._kwargs = kwargs - self._pose = ... - - def get(self) -> np.array: # TODO: what if numpy isn't installed? - raise NotImplementedError() - - def update_pose(self): - raise NotImplementedError() - - -class BaseBackend: - def __init__(self) -> None: +from abc import ABC, abstractmethod +from ral.sensor.sensor_backend import BaseSensorBackend +class BaseBackend(ABC): + def __init__(self,**kwargs) -> None: pass - + + def initiate_backend(self,**kwargs) -> None: + _simulation = kwargs.get('simulation') + self._backend_type = kwargs.get('backend_type') + if self._backend_type == 'pybullet': + from ral.backend.pybullet_backend import PybulletBackend + return PybulletBackend(simulation=_simulation) + def step(self): raise NotImplementedError() def initiate_rgb_sensor(self) -> BaseSensorBackend: raise NotImplementedError() + + def initiate_imu_sensor(self) -> BaseSensorBackend: + raise NotImplementedError() + + def initiate_gyro_sensor(self) -> BaseSensorBackend: + raise NotImplementedError() diff --git a/earth_moving/ral/backend/pybullet_backend.py b/earth_moving/ral/backend/pybullet_backend.py index 8c16fd7..8027844 100644 --- a/earth_moving/ral/backend/pybullet_backend.py +++ b/earth_moving/ral/backend/pybullet_backend.py @@ -1,14 +1,85 @@ import pybullet as p -from earth_moving.ral.backend.base_backend import BaseBackend, BaseSensorBackend +import pybullet_data +import time as t +import os +import matplotlib.pyplot as plt +import numpy as np +from ral.sensor.sensor_backend import BaseSensorBackend +from ral.backend.base_backend import BaseBackend class PybulletBackend(BaseBackend): - def __init__(self, timedelta) -> None: - super().__init__() - self._timedelta + def __init__(self, **kwargs) -> None: + self._kwargs = kwargs + simulation = self._kwargs.get('simulation') + self._timedelta = simulation.get('timedelta') + self._gui = simulation.get('gui') + if self._gui: + self._physicsClient = p.connect(p.GUI) + else: + self._physicsClient = p.connect(p.DIRECT) + self._gravity = simulation.get('gravity') + p.setGravity(self._gravity[0], self._gravity[1], self._gravity[2]) + p.setAdditionalSearchPath(pybullet_data.getDataPath()) # PyBullet_data package (see doc) + self._ID = [] + self._ID.append(p.loadURDF("plane.urdf")) + self._ID.append(p.loadURDF('r2d2.urdf', basePosition=[0.0, 0.0, 1])) + + def step(self): + p.stepSimulation() + t.sleep(self._timedelta) + - def initiate_rgb_sensor(self) -> BaseSensorBackend: # TODO: unlike ROS, this needs to happen for all sensors that we want at the beginning of the run - class PybulletSensorBackend(BaseSensorBackend): + def initiate_rgb_sensor(self,**kwargs) -> BaseSensorBackend: # TODO: unlike ROS, this needs to happen for all sensors that we want at the beginning of the run + + class PybulletSensorRGBBackend(BaseSensorBackend): + def __init__(self,**kwargs) -> None: + super().__init__(**kwargs) + self._imgW = self._sensor.get('imgW') + self._imgH = self._sensor.get('imgH') + + self._camera_pose = self._sensor.get('camera_pose') + self._target_pose = self._sensor.get('target_pose') + self._camera_up_vector = self._sensor.get('camera_up_vector') + self._viewMatrix = p.computeViewMatrix(self._camera_pose, self._target_pose, self._camera_up_vector) + + self._fov = self._sensor.get('fov') + self._aspect = self._imgW / self._imgH + self._near = self._sensor.get('near') + self._far = self._sensor.get('far') + self._projectionMatrix = p.computeProjectionMatrixFOV(self._fov, self._aspect, self._near, self._far) + + def get_data(self) -> np.array: + data = p.getCameraImage(self._imgW,self._imgH,self._viewMatrix,self._projectionMatrix,renderer=p.ER_BULLET_HARDWARE_OPENGL) + return data + + def plot_data(self,**kwargs): + data = kwargs.get('data') + rgb = np.reshape(data[2], (self._imgH, self._imgW, 4)) * 1. / 255. + plt.imshow(rgb) + plt.title('RGB image') + plt.show() + + def save_data(self,**kwargs): + data = kwargs.get('data') + save_path = kwargs.get('path') + rgb = np.reshape(data[2], (self._imgH, self._imgW, 4)) * 1. / 255. + name = kwargs.get('name') + '.png' + if name in os.listdir(save_path): + os.remove(save_path + name) + plt.title('RGB image') + plt.imsave(save_path + name,rgb,format='png') + + sensor_backend = PybulletSensorRGBBackend(**kwargs) + return sensor_backend + + def initiate_imu_sensor(self) -> BaseSensorBackend: + raise NotImplementedError() + + def initiate_gyro_sensor(self) -> BaseSensorBackend: + raise NotImplementedError() + + '''class PybulletSensorRGBBackend(SensorRGBBackend): def __init__(self, **kwargs) -> None: super().__init__(kwargs) self._imgW = self._kwargs.get('imgW') @@ -27,6 +98,6 @@ def get(self): viewMatrix, projectionMatrix = f(linkWorldPosition, linkWorldOrientation) img = p.getCameraImage(self._imgW, self._imgH, viewMatrix, projectionMatrix, renderer=p.ER_BULLET_HARDWARE_OPENGL) # TODO: what if camera is mounted on the robot? - return img - sensor_backend = PybulletSensorBackend() - return sensor_backend \ No newline at end of file + return img''' + + \ No newline at end of file diff --git a/earth_moving/ral/sensor/sensor_backend.py b/earth_moving/ral/sensor/sensor_backend.py new file mode 100644 index 0000000..5ece96e --- /dev/null +++ b/earth_moving/ral/sensor/sensor_backend.py @@ -0,0 +1,14 @@ +import numpy as np +from abc import ABC, abstractmethod + + +class BaseSensorBackend: + + def __init__(self, **kwargs) -> None: + self._kwargs = kwargs + self._name = self._kwargs.get('name') + self._sensor = self._kwargs.get('sensor') + + @abstractmethod # ! Is this correct? I want Pybullet/ROS to handle the actual data fetching + def get_data(self) -> np.array: # TODO: what if numpy isn't installed? + pass \ No newline at end of file diff --git a/earth_moving/runners/my_basic_test_scenario_pybullet_runner.py b/earth_moving/runners/my_basic_test_scenario_pybullet_runner.py index 540711a..7cbdb52 100644 --- a/earth_moving/runners/my_basic_test_scenario_pybullet_runner.py +++ b/earth_moving/runners/my_basic_test_scenario_pybullet_runner.py @@ -1,19 +1,30 @@ -from earth_moving.scenarios.basic_test_scenario import basic_test_scenario +from scenarios.basic_test_scenario import basic_test_scenario + pybullet_running_camera_config = { 'backend_type': 'pybullet', 'sensor': - {'imgW': 180, - 'imgH': 360, - 'camera_pose_method': lambda t: 5*t + 2}, # TODO: still need to think about `t` + { + 'imgW': 400, + 'imgH': 400, + 'camera_pose': [10.0, 10.0, 5.0], + 'camera_up_vector': [0.0, 0.0, 1.0], + 'target_pose': [0.0, 0.0, 0.0], + 'fov': 30, + 'near': 0.0, + 'far': 10, + 'save_path': './outputs/fig/', + 'camera_pose_method': lambda t: 5*t + 2 # TODO: still need to think about `t` + }, 'robot': - {'field1': 'blah', - 'field2': 'blahblah', + { + }, 'simulation': { - 'timedelta': 1.0/30, + 'timedelta': 1.0/240, + 'gravity': (0, 0, -9.81), + 'gui': False } } -} if __name__ == '__main__': diff --git a/earth_moving/scenarios/basic_test_scenario.py b/earth_moving/scenarios/basic_test_scenario.py index f6a442c..7b3cad4 100644 --- a/earth_moving/scenarios/basic_test_scenario.py +++ b/earth_moving/scenarios/basic_test_scenario.py @@ -1,4 +1,17 @@ - +from ral.backend.base_backend import BaseBackend def basic_test_scenario(config): - pass \ No newline at end of file + Basebackend = BaseBackend() + _simulation = config.get('simulation') + _backend = config.get('backend_type') + Backend = Basebackend.initiate_backend(simulation=_simulation, backend_type=_backend) + + _sensor = config.get('sensor') + _path = _sensor.get('save_path') + Camera = Backend.initiate_rgb_sensor(name='cam',sensor=_sensor) + + # while True: + Backend.step() + _data = Camera.get_data() + # Camera.plot_data(data=_data) + Camera.save_data(data=_data,path=_path, name='cam_image') \ No newline at end of file