🐕 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:
+    def __init__(self, kinematic, IMU=None):
-        pass
+        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))
+
+

simulation/GymEnvs/heightfield.py

@@ -13,6 +13,7 @@ https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/
 
 import pybullet as p
 import pybullet_data as pd
+import random
 
 textureId = -1
 
@@ -24,7 +25,6 @@ updateHeightfield = False
 heightfieldSource = useProgrammatic
 numHeightfieldRows = 256
 numHeightfieldColumns = 256
-import random
 random.seed(10)
 
 

simulation/GymEnvs/spot_bezier_env.py

@@ -6,7 +6,7 @@ import time
 import numpy as np
 import pybullet_data
 from gym import spaces
-import spot
+import entities.spot as spot
 from gym.envs.registration import register
 
 from simulation.GymEnvs.spot_gym_env import spotGymEnv

simulation/GymEnvs/spot_gym_env.py

@@ -20,13 +20,12 @@ import pybullet_data
 from gym import spaces
 from gym.utils import seeding
 from pkg_resources import parse_version
-import spot
+import simulation.entities.spot as spot
 import pybullet_utils.bullet_client as bullet_client
 from gym.envs.registration import register
 from simulation.GymEnvs.heightfield import HeightField
 from OpenLoopSM.SpotOL import BezierStepper
 from raspberry_pi.src.Kinematics import LieAlgebra as LA
-from simulation.GymEnvs.spot_env_randomizer import SpotEnvRandomizer
 
 NUM_SUBSTEPS = 5
 NUM_MOTORS = 12

simulation/entities/spot.py

@@ -17,7 +17,7 @@ import math
 import re
 import sys
 import numpy as np
-import motor
+import entities.motor as motor
 import pybullet_data
 
 sys.path.append("../")

simulation/run_simulation.py

@@ -1,111 +1,55 @@
-#!/usr/bin/env python
-
-import numpy as np
 import copy
-import sys
+from os import sys
 
-sys.path.append("../../..")
+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 GymEnvs.spot_bezier_env import spotBezierEnv
 from util.gui import GUI
-from raspberry_pi.src.Kinematics.SpotKinematics import SpotModel
+
-from raspberry_pi.src.GaitGenerator.Bezier import BezierGait
+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
 
 
-class GaitState:
+if __name__ == '__main__':
-    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 Gait:
-    def __init__(
-        self,
-        env: spotBezierEnv,
-        gui: GUI,
-        bodyState: BodyState,
-        gaitState: GaitState,
-        spotModel: SpotModel,
-        bezierGait: BezierGait,
-    ) -> None:
-        self.env = env
-        self.gui = gui
-        self.body_state = bodyState
-        self.gait_state = gaitState
-        self.spot = spotModel
-        self.bezier_gait = bezierGait
-
-        self.state = self.env.reset()
-        self.action = self.env.action_space.sample()
-        self.body_state.worldFeetPositions = copy.deepcopy(self.spot.WorldToFoot)
-
-        self.dt = 0.01
-
-    def step(self):
-        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))
-
-
-if __name__ == "__main__":
-    env = spotBezierEnv(
-        render=True,
-        on_rack=False,
-        height_field=False,
-        draw_foot_path=False,
-    )
-    gui = GUI(env.spot.quadruped)
-    bodyState = BodyState()
-    gaitState = GaitState()
-    spot = SpotModel()
-    bezierGait = BezierGait()
-
-    gait = Gait(env, gui, bodyState, gaitState, spot, bezierGait)
-
     while True:
-        done = gait.step()
+        done = step(state)
         if done:
-            gait.env.close()
+            env.close()
-            break

simulation/run_standalone_sim.py

@@ -1,12 +1,14 @@
-#!/usr/bin/env python
+# SCRIPT FOR SHOWING WALKING GAIT
-
 import numpy as np
 import copy
+import sys
 
-from .GymEnvs.spot_bezier_env import spotBezierEnv
+sys.path.append("../../..")
-from .util.gui import GUI
+
-from .Kinematics.SpotKinematics import SpotModel
+from GymEnvs.spot_bezier_env import spotBezierEnv
-from .GaitGenerator.Bezier import BezierGait
+from util.gui import GUI
+from raspberry_pi.src.Kinematics.SpotKinematics import SpotModel
+from raspberry_pi.src.GaitGenerator.Bezier import BezierGait
 
 
 class GaitState:
@@ -39,7 +41,7 @@ class BodyState:
         self.worldFeetPositions = {}
 
 
-class Simulator:
+class Gait:
     def __init__(
         self,
         env: spotBezierEnv,
@@ -62,7 +64,7 @@ class Simulator:
 
         self.dt = 0.01
 
-    def step(self, model, command):
+    def step(self):
         self.gait_state.update_gait_state(self.dt)
         self.gui.UserInput(self.body_state, self.gait_state)
         self.gait_state.contacts = self.state[-4:]
@@ -92,7 +94,6 @@ if __name__ == "__main__":
         on_rack=False,
         height_field=False,
         draw_foot_path=False,
-        env_randomizer=None,
     )
     gui = GUI(env.spot.quadruped)
     bodyState = BodyState()
@@ -100,7 +101,7 @@ if __name__ == "__main__":
     spot = SpotModel()
     bezierGait = BezierGait()
 
-    gait = Simulator(env, gui, bodyState, gaitState, spot, bezierGait)
+    gait = Gait(env, gui, bodyState, gaitState, spot, bezierGait)
 
     while True:
         done = gait.step()