diff --git a/raspberry_pi/run_spot.py b/raspberry_pi/run_spot.py index 0205a1c..23fe280 100644 --- a/raspberry_pi/run_spot.py +++ b/raspberry_pi/run_spot.py @@ -16,16 +16,23 @@ def main(): imu = IMU() hardware_interface = HardwareInterface() # shared_controller_state = SharedState() - controller_interface = WebsocketController() # shared_controller_state) + shared_controller_state = SharedState() + controller_interface = WebsocketController(shared_controller_state) - spot = Spot(kinematics, camera, imu, hardware_interface, controller_interface) + spot = Spot( + kinematics, + camera, + imu, + hardware_interface, + controller_interface, + shared_controller_state + ) spot.start() try: while True: - spot.run(shared_state.get_latest_state().__dict__) - time.sleep(1) + spot.run() except KeyboardInterrupt: spot.stop() diff --git a/raspberry_pi/simulate.py b/raspberry_pi/simulate.py index 5189cba..975ab15 100644 --- a/raspberry_pi/simulate.py +++ b/raspberry_pi/simulate.py @@ -1,38 +1,66 @@ from os import sys +import time 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 +from src.kinematics.SpotKinematics import SpotModel +from src.camera.WebCamera import WebCamera +from src.imu.IMU import IMU +from src.controller_interface.WebsocketController import WebsocketController +from src.controller_interface.SharedControllerState import SharedState, shared_state + +# from src.camera_server import StreamingServerThread sys.path.append('../') from simulation.simulator import Simulator from simulation.GymEnvs.spot_bezier_env import spotBezierEnv - from simulation.sensors.camera import PyBulletCamera +from simulation.sensors.hardware_interface import PyBulletHardwareInterface -camera = PyBulletCamera() +def main(): + env = spotBezierEnv( + render=True, + on_rack=False, + height_field=False, + draw_foot_path=False, + urdf_root="..\simulation\pybullet_data" + ) -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) + kinematics = SpotModel() + camera = WebCamera() + camera = PyBulletCamera(env.spot.quadruped) + imu = IMU() + hardware_interface = PyBulletHardwareInterface(env) + # shared_controller_state = SharedState() + controller_interface = WebsocketController(shared_state) -server = StreamingServerThread(camera) -sim = Simulator(env, spot) + spot = Spot( + kinematics, + camera, + imu, + hardware_interface, + controller_interface, + shared_state + ) -server.start() + simulator = Simulator(env, spot) + + spot.start() + + last_time = time.time() + + try: + while True: + contacts, done = simulator.step() + spot.gait_state.contacts = contacts + spot.run() # time.time() - last_time + last_time = time.time() + if done: + break + except KeyboardInterrupt: + print("Program was interrupted") + spot.stop() + +if __name__ == '__main__': + main() -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 index 55ad85a..2ce837e 100644 --- a/raspberry_pi/src/Camera/CameraBase.py +++ b/raspberry_pi/src/Camera/CameraBase.py @@ -1,5 +1,4 @@ from abc import abstractmethod -from typing import Any class CameraBase(): @@ -7,5 +6,5 @@ class CameraBase(): pass @abstractmethod - def get_image(self) -> Any: + def update(self) -> None: raise NotImplementedError \ No newline at end of file diff --git a/raspberry_pi/src/Camera/WebCamera.py b/raspberry_pi/src/Camera/WebCamera.py index 33be632..4931d19 100644 --- a/raspberry_pi/src/Camera/WebCamera.py +++ b/raspberry_pi/src/Camera/WebCamera.py @@ -5,14 +5,16 @@ from .CameraBase import CameraBase class WebCamera(CameraBase): def __init__(self, camera_id=0): self.cap = cv2.VideoCapture(camera_id) - # super().__init__() + self._last_frame = None + super().__init__() - def get_image(self): + def get_frame(self): + self._last_frame + + def update(self) -> None: ret, frame = self.cap.read() if ret: - return frame - else: - return None # Return None if the frame could not be captured + self._last_frame = frame.copy() def __del__(self): self.cap.release() \ No newline at end of file diff --git a/raspberry_pi/src/camera_server.py b/raspberry_pi/src/camera_server.py index 1a1c402..40d1989 100644 --- a/raspberry_pi/src/camera_server.py +++ b/raspberry_pi/src/camera_server.py @@ -12,7 +12,7 @@ class StreamingHandler(BaseHTTPRequestHandler): self.end_headers() try: while True: - frame = self.server.camera.get_image() + frame = self.server.camera.get_frame() _, jpeg = cv2.imencode('.jpg', frame) self.wfile.write(b'--frame\r\n') self.send_header('Content-Type', 'image/jpeg') @@ -49,3 +49,5 @@ class StreamingServerThread: self.server_thread.daemon = True self.server_thread.start() + def stop(self): + self.server_thread.join() diff --git a/raspberry_pi/src/controller_interface/WebsocketController.py b/raspberry_pi/src/controller_interface/WebsocketController.py index 930bec6..65ee872 100644 --- a/raspberry_pi/src/controller_interface/WebsocketController.py +++ b/raspberry_pi/src/controller_interface/WebsocketController.py @@ -41,7 +41,6 @@ class WebsocketController: clients[client_id] = { "websocket": websocket, } - print("Got a new connection") try: async for message in websocket: if isinstance(message, bytes): diff --git a/raspberry_pi/src/spot.py b/raspberry_pi/src/spot.py index 8252d6b..0c495b8 100644 --- a/raspberry_pi/src/spot.py +++ b/raspberry_pi/src/spot.py @@ -3,18 +3,26 @@ import numpy as np import copy import sys +from .camera.CameraBase import CameraBase +from .camera_server import StreamingServerThread from .GaitGenerator.Bezier import BezierGait from .State import BodyState, GaitState sys.path.append("../../..") +def map_value(x, from_low, from_high, to_low, to_high): + return ((x - from_low) / (from_high - from_low)) * (to_high - to_low) + to_low + class Spot: - def __init__(self, kinematic, camera, imu, hardware_interface, controller_interface): + def __init__(self, kinematic, camera:CameraBase, imu, hardware_interface, controller_interface, shared_controller_state): self.kinematic = kinematic self.camera = camera self.imu = imu self.hardware_interface = hardware_interface self.controller_interface = controller_interface + self.shared_controller_state = shared_controller_state + + self.camera_stream = StreamingServerThread(self.camera) self.body_state = BodyState() self.gait_state = GaitState() @@ -23,10 +31,13 @@ class Spot: self.body_state.worldFeetPositions = copy.deepcopy(self.kinematic.WorldToFoot) def start(self): + self.controller_interface.start() + self.camera_stream.start() def stop(self): self.controller_interface.stop() + self.camera_stream.stop() def joint_angles(self): return self.kinematic.IK( @@ -34,16 +45,22 @@ class Spot: self.body_state.position, self.body_state.worldFeetPositions, ) + + def handle_input(self, controller): + self.gait_state.target_step_length = map_value(max(controller.lx, controller.ly, key=abs), -127, 128, -0.1, 0.1) + self.gait_state.target_lateral_fraction = map_value(controller.lx, -127, 128, -np.pi / 2.0, np.pi / 2.0) + # self.gait_state.target_yaw_rate = map_value(controller.rx, -127, 128, -2.0, 2.0) def run(self, dt=.01): + # self.camera.update() + controller = self.shared_controller_state.get_latest_state() + self.handle_input(controller) self.gait_state.update_gait_state(dt) - # self.gui.UserInput(self.body_state, self.gait_state) + + self.body_state.worldFeetPositions = copy.deepcopy(self.kinematic.WorldToFoot) self.gait_controller.generate_trajectory(self.body_state, self.gait_state, dt) - self.update_environment() - - def update_environment(self): joint_angles = self.kinematic.IK( self.body_state.rotation, self.body_state.position, diff --git a/simulation/GymEnvs/spot_gym_env.py b/simulation/GymEnvs/spot_gym_env.py index f6ba12c..c0dfbd0 100644 --- a/simulation/GymEnvs/spot_gym_env.py +++ b/simulation/GymEnvs/spot_gym_env.py @@ -25,7 +25,7 @@ import pybullet_utils.bullet_client as bullet_client import simulation.entities.spot as spot from simulation.GymEnvs.heightfield import HeightField -from raspberry_pi.src.Kinematics import LieAlgebra as LA +from raspberry_pi.src.kinematics import LieAlgebra as LA from simulation.OpenLoopSM.SpotOL import BezierStepper NUM_SUBSTEPS = 5 diff --git a/simulation/entities/spot.py b/simulation/entities/spot.py index b04b25b..c9830a5 100644 --- a/simulation/entities/spot.py +++ b/simulation/entities/spot.py @@ -22,7 +22,7 @@ import pybullet_data from simulation.entities import motor sys.path.append("../") -from raspberry_pi.src.Kinematics.SpotKinematics import SpotModel +from raspberry_pi.src.kinematics.SpotKinematics import SpotModel INIT_POSITION = [0, 0, 0.25] INIT_RACK_POSITION = [0, 0, 1] diff --git a/simulation/run_simulation.py b/simulation/run_simulation.py index 6136d2a..44d8730 100644 --- a/simulation/run_simulation.py +++ b/simulation/run_simulation.py @@ -3,8 +3,8 @@ 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.IMU import IMU +from raspberry_pi.src.kinematics.SpotKinematics import SpotModel +from raspberry_pi.src.imu.IMU import IMU # from GymEnvs.spot_bezier_env import spotBezierEnv from simulator import Simulator diff --git a/simulation/run_standalone_sim.py b/simulation/run_standalone_sim.py index 73faf8d..478262e 100644 --- a/simulation/run_standalone_sim.py +++ b/simulation/run_standalone_sim.py @@ -7,7 +7,7 @@ sys.path.append("../../..") from GymEnvs.spot_bezier_env import spotBezierEnv from util.gui import GUI -from raspberry_pi.src.Kinematics.SpotKinematics import SpotModel +from raspberry_pi.src.kinematics.SpotKinematics import SpotModel from raspberry_pi.src.GaitGenerator.Bezier import BezierGait diff --git a/simulation/sensors/camera.py b/simulation/sensors/camera.py index 17e0d09..c3ff0b6 100644 --- a/simulation/sensors/camera.py +++ b/simulation/sensors/camera.py @@ -1,3 +1,4 @@ +import math import cv2 import numpy as np import pybullet as pb @@ -5,7 +6,7 @@ import sys sys.path.append("../../../") -from raspberry_pi.src.Camera.CameraBase import CameraBase +from raspberry_pi.src.camera.CameraBase import CameraBase viewMatrix = pb.computeViewMatrix( cameraEyePosition=[0, 0, 1], @@ -14,8 +15,10 @@ viewMatrix = pb.computeViewMatrix( projectionMatrix = pb.computeProjectionMatrixFOV( fov=160, aspect=1.0, nearVal=0.1, farVal=150.0) +distance = 100000 + class PyBulletCamera(CameraBase): - def __init__(self, view_matrix=viewMatrix, projection_matrix=projectionMatrix, width=640, height=480): + def __init__(self, model_id, 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. @@ -23,35 +26,69 @@ class PyBulletCamera(CameraBase): :param width: Width of the camera image. :param height: Height of the camera image. """ + super().__init__() + self.model_id = model_id self.view_matrix = view_matrix self.projection_matrix = projection_matrix self.width = width self.height = height + self._last_frame = None - def get_image(self): + def get_frame(self): + self._last_frame + + def update(self): """ Captures an image from the PyBullet simulation. :return: An image frame captured from PyBullet. """ + + self.update_view_matrix() + self._last_frame = self.get_frame() + + def get_frame(self): # Capture an image from the simulation - _, _, px, _, _ = pb.getCameraImage( + imgs = pb.getCameraImage( width=self.width, height=self.height, viewMatrix=self.view_matrix, - projectionMatrix=self.projection_matrix) + projectionMatrix=self.projection_matrix, + shadow=True, renderer=pb.ER_BULLET_HARDWARE_OPENGL) + _, _, px, _, _ = imgs # 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]] + return cv2.cvtColor(rgb_image, cv2.COLOR_RGBA2BGR) - # 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]) + + def update_view_matrix(self): + + position, orientation = pb.getBasePositionAndOrientation(self.model_id) + yaw = pb.getEulerFromQuaternion(orientation)[-1] + xA, yA, zA = position + zA = zA + 0.3 # make the camera a little higher than the robot + + # compute focusing point of the camera + xB = xA + math.cos(yaw) * distance + yB = yA + math.sin(yaw) * distance + zB = zA + + self.view_matrix = pb.computeViewMatrix( + cameraEyePosition=[xA, yA, zA], + cameraTargetPosition=[xB, yB, zB], + cameraUpVector=[0, 0, 1.0] + ) + + self.projection_matrix = pb.computeProjectionMatrixFOV(fov=90, aspect=1.5, nearVal=0.02, farVal=3.5) + + # imgs = pb.getCameraImage(self.width, self.height, + # view_matrix, + # self.projection_matrix, shadow=True, + # renderer=pb.ER_BULLET_HARDWARE_OPENGL) + # position = [position[0], position[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/sensors/hardware_interface.py b/simulation/sensors/hardware_interface.py new file mode 100644 index 0000000..4b371b8 --- /dev/null +++ b/simulation/sensors/hardware_interface.py @@ -0,0 +1,15 @@ +import sys +import numpy as np + +sys.path.append("../../../") + +from raspberry_pi.src.hardware_interface.HardwareInterfaceBase import HardwareInterfaceBase + +class PyBulletHardwareInterface(HardwareInterfaceBase): + def __init__(self, env): + self.env = env + self.set_actuator_positions(np.array([0]*12)) + super().__init__() + + def set_actuator_positions(self, joint_angles): + return self.env.pass_joint_angles(joint_angles) \ No newline at end of file