diff --git a/raspberry_pi/main.py b/raspberry_pi/main.py index 8540a06..055a0a9 100644 --- a/raspberry_pi/main.py +++ b/raspberry_pi/main.py @@ -1,3 +1,5 @@ +# SCRIPT FOR RUNNING THE RPI ROBOT + from .src.spot import Spot # Server diff --git a/raspberry_pi/src/IMU.py b/raspberry_pi/src/IMU.py new file mode 100644 index 0000000..67a478a --- /dev/null +++ b/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]) \ No newline at end of file diff --git a/raspberry_pi/src/State.py b/raspberry_pi/src/State.py new file mode 100644 index 0000000..4666148 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/raspberry_pi/src/spot.py b/raspberry_pi/src/spot.py index 9e74dfc..7654c65 100644 --- a/raspberry_pi/src/spot.py +++ b/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 \ No newline at end of file + 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)) + + diff --git a/simulation/GymEnvs/heightfield.py b/simulation/GymEnvs/heightfield.py index 3186916..200c1ed 100644 --- a/simulation/GymEnvs/heightfield.py +++ b/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) diff --git a/simulation/GymEnvs/spot_bezier_env.py b/simulation/GymEnvs/spot_bezier_env.py index d6dbb43..92e2287 100644 --- a/simulation/GymEnvs/spot_bezier_env.py +++ b/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 diff --git a/simulation/GymEnvs/spot_gym_env.py b/simulation/GymEnvs/spot_gym_env.py index 38a9bf1..26ab9d0 100644 --- a/simulation/GymEnvs/spot_gym_env.py +++ b/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 diff --git a/simulation/motor.py b/simulation/entities/motor.py similarity index 100% rename from simulation/motor.py rename to simulation/entities/motor.py diff --git a/simulation/spot.py b/simulation/entities/spot.py similarity index 99% rename from simulation/spot.py rename to simulation/entities/spot.py index 4bfbdce..c703496 100644 --- a/simulation/spot.py +++ b/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("../") diff --git a/simulation/run_simulation.py b/simulation/run_simulation.py index 06a1a94..9288c4a 100644 --- a/simulation/run_simulation.py +++ b/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: - 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() \ No newline at end of file diff --git a/simulation/simulator.py b/simulation/run_standalone_sim.py similarity index 88% rename from simulation/simulator.py rename to simulation/run_standalone_sim.py index 9de3230..73faf8d 100644 --- a/simulation/simulator.py +++ b/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 -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()