🐕 Adds way to simulate the model

2024-03-04 23:41:48 +01:00
parent 5449658df7
commit f28d5e345b
11 changed files with 178 additions and 119 deletions
@@ -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)


@@ -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
@@ -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
@@ -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("../")
@@ -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:
-    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)
-
+if __name__ == '__main__':
    while True:
-        done = gait.step()
+        done = step(state)
        if done:
-            gait.env.close()
-            break
+            env.close()
@@ -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
-from .util.gui import GUI
-from .Kinematics.SpotKinematics import SpotModel
-from .GaitGenerator.Bezier import BezierGait
+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.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()