🐕 Adds way to simulate the model

raspberry_pi/main.py

@@ -1,3 +1,5 @@
+# SCRIPT FOR RUNNING THE RPI ROBOT
+
 from .src.spot import Spot
 # Server
 

raspberry_pi/src/IMU.py

@@ -0,0 +1,9 @@
+import numpy as np
+
+
+class IMU:
+    def __init__(self) -> None:
+        pass
+
+    def read_orientation(self):
+        return np.array([1, 0, 0, 0])

raspberry_pi/src/State.py

@@ -0,0 +1,58 @@
+import numpy as np
+from enum import Enum
+
+
+# https://github.com/stanfordroboticsclub/StanfordQuadruped/blob/master/src/State.py
+class State:
+    def __init__(self):
+        self.horizontal_velocity = np.array([0.0, 0.0])
+        self.yaw_rate = 0.0
+        self.height = -0.16
+        self.pitch = 0.0
+        self.roll = 0.0
+        self.activation = 0
+        self.behavior_state = BehaviorState.REST
+
+        self.ticks = 0
+        self.foot_locations = np.zeros((3, 4))
+        self.joint_angles = np.zeros((3, 4))
+
+        self.behavior_state = BehaviorState.REST
+
+
+class GaitState:
+    def __init__(self) -> None:
+        self.step_length = 0.0
+        self.yaw_rate = 0
+        self.lateral_fraction = 0
+        self.step_velocity = 0.001
+        self.swing_period = 0.2
+        self.clearance_height = 0.045
+        self.penetration_depth = 0.003
+        self.contacts = [False] * 4
+
+        self.target_step_length = 0
+        self.target_yaw_rate = 0
+        self.target_lateral_fraction = 0
+
+    def update_gait_state(self, dt):
+        self.step_length = self.step_length * (1 - dt) + self.target_step_length * dt
+        self.lateral_fraction = (
+            self.lateral_fraction * (1 - dt) + self.target_lateral_fraction * dt
+        )
+        self.yaw_rate = self.yaw_rate * (1 - dt) + self.target_yaw_rate * dt
+
+
+class BodyState:
+    def __init__(self) -> None:
+        self.position = np.array([0, 0, 0])
+        self.rotation = np.array([0, 0, 0])
+        self.worldFeetPositions = {}
+
+
+class BehaviorState(Enum):
+    DEACTIVATED = -1
+    REST = 0
+    TROT = 1
+    HOP = 2
+    FINISHHOP = 3

raspberry_pi/src/spot.py

@@ -1,5 +1,51 @@
 
+import numpy as np
+import copy
+import sys
+
+from .GaitGenerator.Bezier import BezierGait
+from .State import BodyState, GaitState
+
+sys.path.append("../../..")
 
 class Spot:
-    def __init__(self) -> None:
-        pass
+    def __init__(self, kinematic, IMU=None):
+        self.body_state = BodyState()
+        self.gait_state = GaitState()
+        self.kinematic = kinematic
+        self.gait_controller = BezierGait()
+        self.IMU = IMU
+
+        self.body_state.worldFeetPositions = copy.deepcopy(self.kinematic.WorldToFoot)
+
+    def joint_angles(self):
+        return self.kinematic.IK(
+            self.body_state.rotation,
+            self.body_state.position,
+            self.body_state.worldFeetPositions,
+        )
+
+    def run(self, state, command):
+        self.gait_state.update_gait_state(self.dt)
+        self.gui.UserInput(self.body_state, self.gait_state)
+        self.gait_state.contacts = self.state[-4:]
+        self.body_state.worldFeetPositions = copy.deepcopy(self.spot.WorldToFoot)
+
+        self.bezier_gait.generate_trajectory(self.body_state, self.gait_state, self.dt)
+
+        self.update_environment()
+
+        self.state, _, done, _ = self.env.step(self.action)
+        if done:
+            print("DONE")
+            return True
+
+    def update_environment(self):
+        joint_angles = self.spot.IK(
+            self.body_state.rotation,
+            self.body_state.position,
+            self.body_state.worldFeetPositions,
+        )
+        self.env.pass_joint_angles(joint_angles.reshape(-1))
+
+