Updates camera and mocks

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__)
+            spot.run()
-            time.sleep(1)
     except KeyboardInterrupt:
         spot.stop()
 

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.Kinematics.SpotKinematics import SpotModel
+from src.camera.WebCamera import WebCamera
-from src.camera_server import StreamingServerThread
+from src.imu.IMU import IMU
-from src.Camera.WebCamera import WebCamera
+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(
+    kinematics = SpotModel()
-    render=True,
+    camera = WebCamera()
-    on_rack=False,
+    camera = PyBulletCamera(env.spot.quadruped)
-    height_field=False,
+    imu = IMU()
-    draw_foot_path=False,
+    hardware_interface = PyBulletHardwareInterface(env)
-    urdf_root="..\simulation\pybullet_data"
+    # shared_controller_state = SharedState()
-)
+    controller_interface = WebsocketController(shared_state)
-kinematics = SpotModel()
-imu = IMU()
-camera = PyBulletCamera() # WebCamera()
-spot = Spot(kinematics, imu)
 
-server = StreamingServerThread(camera)
+    spot = Spot(
-sim = Simulator(env, 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)

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

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
+            self._last_frame = frame.copy()
-        else:
-            return None  # Return None if the frame could not be captured
 
     def __del__(self):
         self.cap.release()

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()

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):

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,

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

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]

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.kinematics.SpotKinematics import SpotModel
-from raspberry_pi.src.IMU.IMU import IMU
+from raspberry_pi.src.imu.IMU import IMU
 
 # from GymEnvs.spot_bezier_env import spotBezierEnv
 from simulator import Simulator

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
 
 

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 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
+    def update_view_matrix(self):
-        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])
+        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])

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)