Updates camera and mocks

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.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

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

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)