From 7208cc7b1cab32403b6f943690fb66ad9b957017 Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Tue, 5 Mar 2024 11:35:10 +0100 Subject: [PATCH] =?UTF-8?q?=F0=9F=8E=8D=20Adds=20virtual=20sensors=20and?= =?UTF-8?q?=20camera=20mjpeg=20stream?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- raspberry_pi/simulate.py | 38 +++++++++++++++++ raspberry_pi/src/Camera/CameraBase.py | 11 +++++ raspberry_pi/src/Camera/WebCamera.py | 18 ++++++++ raspberry_pi/src/Camera/__init__.py | 0 raspberry_pi/src/{ => IMU}/IMU.py | 4 +- raspberry_pi/src/IMU/IMUBase.py | 6 +++ raspberry_pi/src/IMU/__init__.py | 0 raspberry_pi/src/camera_server.py | 51 +++++++++++++++++++++++ simulation/GymEnvs/spot_bezier_env.py | 2 +- simulation/GymEnvs/spot_gym_env.py | 9 ++-- simulation/entities/spot.py | 6 +-- simulation/run_simulation.py | 60 +++++++-------------------- simulation/sensors/IMU.py | 0 simulation/sensors/camera.py | 57 +++++++++++++++++++++++++ simulation/simulator.py | 44 ++++++++++++++++++++ 15 files changed, 252 insertions(+), 54 deletions(-) create mode 100644 raspberry_pi/simulate.py create mode 100644 raspberry_pi/src/Camera/CameraBase.py create mode 100644 raspberry_pi/src/Camera/WebCamera.py create mode 100644 raspberry_pi/src/Camera/__init__.py rename raspberry_pi/src/{ => IMU}/IMU.py (73%) create mode 100644 raspberry_pi/src/IMU/IMUBase.py create mode 100644 raspberry_pi/src/IMU/__init__.py create mode 100644 raspberry_pi/src/camera_server.py create mode 100644 simulation/sensors/IMU.py create mode 100644 simulation/sensors/camera.py create mode 100644 simulation/simulator.py diff --git a/raspberry_pi/simulate.py b/raspberry_pi/simulate.py new file mode 100644 index 0000000..5189cba --- /dev/null +++ b/raspberry_pi/simulate.py @@ -0,0 +1,38 @@ +from os import sys + +from src.spot import Spot +from src.IMU.IMU import IMU +from src.Kinematics.SpotKinematics import SpotModel +from src.camera_server import StreamingServerThread +from src.Camera.WebCamera import WebCamera + +sys.path.append('../') + +from simulation.simulator import Simulator +from simulation.GymEnvs.spot_bezier_env import spotBezierEnv + +from simulation.sensors.camera import PyBulletCamera + +camera = PyBulletCamera() + +env = spotBezierEnv( + render=True, + on_rack=False, + height_field=False, + draw_foot_path=False, + urdf_root="..\simulation\pybullet_data" +) +kinematics = SpotModel() +imu = IMU() +camera = PyBulletCamera() # WebCamera() +spot = Spot(kinematics, imu) + +server = StreamingServerThread(camera) +sim = Simulator(env, spot) + +server.start() + +while True: + camera.update_view_matrix(sim.env.spot.quadruped) + camera.get_image() + sim.step(0.01) \ No newline at end of file diff --git a/raspberry_pi/src/Camera/CameraBase.py b/raspberry_pi/src/Camera/CameraBase.py new file mode 100644 index 0000000..55ad85a --- /dev/null +++ b/raspberry_pi/src/Camera/CameraBase.py @@ -0,0 +1,11 @@ +from abc import abstractmethod +from typing import Any + + +class CameraBase(): + def __init__(self) -> None: + pass + + @abstractmethod + def get_image(self) -> Any: + raise NotImplementedError \ No newline at end of file diff --git a/raspberry_pi/src/Camera/WebCamera.py b/raspberry_pi/src/Camera/WebCamera.py new file mode 100644 index 0000000..33be632 --- /dev/null +++ b/raspberry_pi/src/Camera/WebCamera.py @@ -0,0 +1,18 @@ +import cv2 +from .CameraBase import CameraBase + + +class WebCamera(CameraBase): + def __init__(self, camera_id=0): + self.cap = cv2.VideoCapture(camera_id) + # super().__init__() + + def get_image(self): + ret, frame = self.cap.read() + if ret: + return frame + else: + return None # Return None if the frame could not be captured + + def __del__(self): + self.cap.release() \ No newline at end of file diff --git a/raspberry_pi/src/Camera/__init__.py b/raspberry_pi/src/Camera/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/raspberry_pi/src/IMU.py b/raspberry_pi/src/IMU/IMU.py similarity index 73% rename from raspberry_pi/src/IMU.py rename to raspberry_pi/src/IMU/IMU.py index 67a478a..4d70e0f 100644 --- a/raspberry_pi/src/IMU.py +++ b/raspberry_pi/src/IMU/IMU.py @@ -1,7 +1,7 @@ import numpy as np +from .IMUBase import IMUBase - -class IMU: +class IMU(IMUBase): def __init__(self) -> None: pass diff --git a/raspberry_pi/src/IMU/IMUBase.py b/raspberry_pi/src/IMU/IMUBase.py new file mode 100644 index 0000000..8c57840 --- /dev/null +++ b/raspberry_pi/src/IMU/IMUBase.py @@ -0,0 +1,6 @@ +class IMUBase(): + def __init__(self) -> None: + pass + + def read_orientation(self): + raise NotImplementedError() \ No newline at end of file diff --git a/raspberry_pi/src/IMU/__init__.py b/raspberry_pi/src/IMU/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/raspberry_pi/src/camera_server.py b/raspberry_pi/src/camera_server.py new file mode 100644 index 0000000..1a1c402 --- /dev/null +++ b/raspberry_pi/src/camera_server.py @@ -0,0 +1,51 @@ +from http.server import BaseHTTPRequestHandler, HTTPServer +from threading import Thread +import cv2 +import time + + +class StreamingHandler(BaseHTTPRequestHandler): + def do_GET(self): + if self.path == '/stream.mjpg': + self.send_response(200) + self.send_header('Content-type', 'multipart/x-mixed-replace; boundary=frame') + self.end_headers() + try: + while True: + frame = self.server.camera.get_image() + _, jpeg = cv2.imencode('.jpg', frame) + self.wfile.write(b'--frame\r\n') + self.send_header('Content-Type', 'image/jpeg') + self.send_header('Content-Length', len(jpeg)) + self.end_headers() + self.wfile.write(jpeg.tobytes()) + self.wfile.write(b'\r\n') + time.sleep(0.5) + except Exception as e: + print(f"Stream stopped: {e}") + else: + self.send_error(404) + self.end_headers() + +class StreamingServer(HTTPServer): + def __init__(self, server_address, camera): + super().__init__(server_address, StreamingHandler) + self.camera = camera + +class StreamingServerThread: + def __init__(self, camera, port=8080): + self.camera = camera + self.port = port + self.server_thread = None + + def start(self): + def run_server(): + address = ('', self.port) + server = StreamingServer(address, self.camera) + print(f"Starting server at http://localhost:{self.port}/stream.mjpg") + server.serve_forever() + + self.server_thread = Thread(target=run_server) + self.server_thread.daemon = True + self.server_thread.start() + diff --git a/simulation/GymEnvs/spot_bezier_env.py b/simulation/GymEnvs/spot_bezier_env.py index 92e2287..51c1804 100644 --- a/simulation/GymEnvs/spot_bezier_env.py +++ b/simulation/GymEnvs/spot_bezier_env.py @@ -6,9 +6,9 @@ import time import numpy as np import pybullet_data from gym import spaces -import entities.spot as spot from gym.envs.registration import register +from simulation.entities import spot from simulation.GymEnvs.spot_gym_env import spotGymEnv SENSOR_NOISE_STDDEV = spot.SENSOR_NOISE_STDDEV diff --git a/simulation/GymEnvs/spot_gym_env.py b/simulation/GymEnvs/spot_gym_env.py index 26ab9d0..f6ba12c 100644 --- a/simulation/GymEnvs/spot_gym_env.py +++ b/simulation/GymEnvs/spot_gym_env.py @@ -19,13 +19,14 @@ import pybullet import pybullet_data from gym import spaces from gym.utils import seeding -from pkg_resources import parse_version -import simulation.entities.spot as spot -import pybullet_utils.bullet_client as bullet_client from gym.envs.registration import register +from pkg_resources import parse_version +import pybullet_utils.bullet_client as bullet_client + +import simulation.entities.spot as spot from simulation.GymEnvs.heightfield import HeightField -from OpenLoopSM.SpotOL import BezierStepper from raspberry_pi.src.Kinematics import LieAlgebra as LA +from simulation.OpenLoopSM.SpotOL import BezierStepper NUM_SUBSTEPS = 5 NUM_MOTORS = 12 diff --git a/simulation/entities/spot.py b/simulation/entities/spot.py index c703496..b04b25b 100644 --- a/simulation/entities/spot.py +++ b/simulation/entities/spot.py @@ -17,9 +17,9 @@ import math import re import sys import numpy as np -import entities.motor as motor import pybullet_data +from simulation.entities import motor sys.path.append("../") from raspberry_pi.src.Kinematics.SpotKinematics import SpotModel @@ -397,14 +397,14 @@ class Spot(object): print(pybullet_data.getDataPath()) if self._self_collision_enabled: self.quadruped = self._pybullet_client.loadURDF( - pybullet_data.getDataPath() + "/spot.urdf", + self._urdf_root + "/spot.urdf", init_position, useFixedBase=self._on_rack, flags=self._pybullet_client.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT, ) else: self.quadruped = self._pybullet_client.loadURDF( - pybullet_data.getDataPath() + "/spot.urdf", + self._urdf_root + "/spot.urdf", init_position, INIT_ORIENTATION, useFixedBase=self._on_rack, diff --git a/simulation/run_simulation.py b/simulation/run_simulation.py index 9288c4a..6136d2a 100644 --- a/simulation/run_simulation.py +++ b/simulation/run_simulation.py @@ -1,55 +1,27 @@ -import copy from os import sys sys.path.append('../') from raspberry_pi.src.spot import Spot from raspberry_pi.src.Kinematics.SpotKinematics import SpotModel -from raspberry_pi.src.IMU import IMU +from raspberry_pi.src.IMU.IMU import IMU -from GymEnvs.spot_bezier_env import spotBezierEnv -from util.gui import GUI +# from GymEnvs.spot_bezier_env import spotBezierEnv +from simulator import Simulator -kinematics = SpotModel() -imu = IMU() -env = spotBezierEnv( - render=True, - on_rack=False, - height_field=False, - draw_foot_path=False, -) - -state = env.reset() -dt = 0.01 -gui = GUI(env.spot.quadruped) -action = env.action_space.sample() - -spot = Spot(kinematics, imu) - -def step(state): - # handle_inputs() - gui.UserInput(spot.body_state, spot.gait_state) - spot.gait_state.contacts = state[-4:] - spot.body_state.worldFeetPositions = copy.deepcopy(spot.kinematic.WorldToFoot) - - spot.gait_state.update_gait_state(dt) - - # Generate next feet positions - spot.gait_controller.generate_trajectory(spot.body_state, spot.gait_state, dt) - - # Calculate next joint angles - joint_angles = spot.joint_angles() - - # Update environment - env.pass_joint_angles(joint_angles.reshape(-1)) - state, _, done, _ = env.step(action) - if done: - print("DONE") - return True if __name__ == '__main__': - while True: - done = step(state) - if done: - env.close() \ No newline at end of file + kinematics = SpotModel() + imu = IMU() + # env = spotBezierEnv( + # render=True, + # on_rack=False, + # height_field=False, + # draw_foot_path=False, + # ) + + spot = Spot(kinematics, imu) + + simulation = Simulator(spot=spot) + simulation.run(0.01) \ No newline at end of file diff --git a/simulation/sensors/IMU.py b/simulation/sensors/IMU.py new file mode 100644 index 0000000..e69de29 diff --git a/simulation/sensors/camera.py b/simulation/sensors/camera.py new file mode 100644 index 0000000..17e0d09 --- /dev/null +++ b/simulation/sensors/camera.py @@ -0,0 +1,57 @@ +import cv2 +import numpy as np +import pybullet as pb +import sys + +sys.path.append("../../../") + +from raspberry_pi.src.Camera.CameraBase import CameraBase + +viewMatrix = pb.computeViewMatrix( + cameraEyePosition=[0, 0, 1], + cameraTargetPosition=[0, 0, 0], + cameraUpVector=[0, 1, 0]) +projectionMatrix = pb.computeProjectionMatrixFOV( + fov=160, aspect=1.0, nearVal=0.1, farVal=150.0) + +class PyBulletCamera(CameraBase): + def __init__(self, view_matrix=viewMatrix, projection_matrix=projectionMatrix, width=640, height=480): + """ + Initializes the camera with PyBullet view and projection matrices. + :param view_matrix: The view matrix for the camera in PyBullet. + :param projection_matrix: The projection matrix for the camera. + :param width: Width of the camera image. + :param height: Height of the camera image. + """ + self.view_matrix = view_matrix + self.projection_matrix = projection_matrix + self.width = width + self.height = height + + def get_image(self): + """ + Captures an image from the PyBullet simulation. + :return: An image frame captured from PyBullet. + """ + # Capture an image from the simulation + _, _, px, _, _ = pb.getCameraImage( + width=self.width, height=self.height, + viewMatrix=self.view_matrix, + projectionMatrix=self.projection_matrix) + + # Convert PyBullet's RGBA image to an OpenCV BGR image + rgb_image = np.array(px, dtype=np.uint8) + rgb_image = np.reshape(rgb_image, (self.height, self.width, -1)) + bgr_image = cv2.cvtColor(rgb_image, cv2.COLOR_RGBA2BGR) + + return bgr_image + + def update_view_matrix(self, model_id): + position, orientation = pb.getBasePositionAndOrientation(model_id) + position = [position[0], position[1]-0.1, position[2]] + orientation = [orientation[0], orientation[1], orientation[2]] + + # Calculate the camera's view matrix based on the new position and orientation + camera_eye_pos = position # This should be adjusted based on your specific relative position requirements + camera_target_pos = [position[0] + orientation[0], position[1] + orientation[1], position[2] + orientation[2]] + self.view_matrix = pb.computeViewMatrix(camera_eye_pos, camera_target_pos, [0, 1, 0]) diff --git a/simulation/simulator.py b/simulation/simulator.py new file mode 100644 index 0000000..00d1842 --- /dev/null +++ b/simulation/simulator.py @@ -0,0 +1,44 @@ +import copy +from os import sys + +sys.path.append('../../') + +from .util.gui import GUI + + +class Simulator(): + def __init__(self, env, spot) -> None: + self.env = env + self.state = env.reset() + self.gui = GUI(env.spot.quadruped) + self.action = env.action_space.sample() + + self.spot = spot + + def step(self, dt): + # handle_inputs() + self.gui.UserInput(self.spot.body_state, self.spot.gait_state) + self.spot.gait_state.contacts = self.state[-4:] + self.spot.body_state.worldFeetPositions = copy.deepcopy(self.spot.kinematic.WorldToFoot) + + self.spot.gait_state.update_gait_state(dt) + + # Generate next feet positions + self.spot.gait_controller.generate_trajectory(self.spot.body_state, self.spot.gait_state, dt) + + # Calculate next joint angles + joint_angles = self.spot.joint_angles() + + # Update environment + self.env.pass_joint_angles(joint_angles.reshape(-1)) + self.state, _, done, _ = self.env.step(self.action) + if done: + print("DONE") + return True + + def run(self, dt): + while True: + done = self.step(dt) + if done: + self.env.close() +