diff --git a/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv.egg-info/PKG-INFO b/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv.egg-info/PKG-INFO new file mode 100644 index 0000000..ec40b95 --- /dev/null +++ b/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv.egg-info/PKG-INFO @@ -0,0 +1,10 @@ +Metadata-Version: 1.0 +Name: gym-GyroscopeEnv +Version: 0.0.1 +Summary: UNKNOWN +Home-page: UNKNOWN +Author: UNKNOWN +Author-email: UNKNOWN +License: UNKNOWN +Description: UNKNOWN +Platform: UNKNOWN diff --git a/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv.egg-info/SOURCES.txt b/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv.egg-info/SOURCES.txt new file mode 100644 index 0000000..7738a4e --- /dev/null +++ b/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv.egg-info/SOURCES.txt @@ -0,0 +1,6 @@ +setup.py +gym_GyroscopeEnv.egg-info/PKG-INFO +gym_GyroscopeEnv.egg-info/SOURCES.txt +gym_GyroscopeEnv.egg-info/dependency_links.txt +gym_GyroscopeEnv.egg-info/requires.txt +gym_GyroscopeEnv.egg-info/top_level.txt \ No newline at end of file diff --git a/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv.egg-info/dependency_links.txt b/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv.egg-info/dependency_links.txt new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv.egg-info/dependency_links.txt @@ -0,0 +1 @@ + diff --git a/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv.egg-info/requires.txt b/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv.egg-info/requires.txt new file mode 100644 index 0000000..7dcf672 --- /dev/null +++ b/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv.egg-info/requires.txt @@ -0,0 +1,3 @@ +gym +numpy +scipy diff --git a/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv.egg-info/top_level.txt b/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv.egg-info/top_level.txt new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv.egg-info/top_level.txt @@ -0,0 +1 @@ + diff --git a/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv/__init__.py b/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv/__init__.py new file mode 100644 index 0000000..8da0dc3 --- /dev/null +++ b/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv/__init__.py @@ -0,0 +1,6 @@ +from gym.envs.registration import register + +register( + id='gyroscopeenv-v0', + entry_point='gym_GyroscopeEnv.envs:GyroscopeEnv', +) diff --git a/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv/__pycache__/__init__.cpython-36.pyc b/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv/__pycache__/__init__.cpython-36.pyc new file mode 100644 index 0000000..5887210 Binary files /dev/null and b/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv/__pycache__/__init__.cpython-36.pyc differ diff --git a/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv/envs/__init__.py b/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv/envs/__init__.py new file mode 100644 index 0000000..3125897 --- /dev/null +++ b/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv/envs/__init__.py @@ -0,0 +1 @@ +from gym_GyroscopeEnv.envs.gyroscope_env import GyroscopeEnv diff --git a/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv/envs/__pycache__/__init__.cpython-36.pyc b/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv/envs/__pycache__/__init__.cpython-36.pyc new file mode 100644 index 0000000..45cb08f Binary files /dev/null and b/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv/envs/__pycache__/__init__.cpython-36.pyc differ diff --git a/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv/envs/__pycache__/gyroscope_env.cpython-36.pyc b/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv/envs/__pycache__/gyroscope_env.cpython-36.pyc new file mode 100644 index 0000000..94ace5a Binary files /dev/null and b/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv/envs/__pycache__/gyroscope_env.cpython-36.pyc differ diff --git a/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv/envs/gyroscope_env.py b/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv/envs/gyroscope_env.py new file mode 100644 index 0000000..2b498ec --- /dev/null +++ b/code/environment/gym-GyroscopeEnv/gym_GyroscopeEnv/envs/gyroscope_env.py @@ -0,0 +1,176 @@ +import gym +from gym import error, spaces, utils +from gym.utils import seeding +from os import path +from scipy.integrate import solve_ivp +import random +import numpy as np + +class GyroscopeEnv(gym.Env): + + + """ + GyroscopeEnv is a double gimbal control moment gyroscope (DGCMG) with 2 input voltage u1 and u2 + on the two gimbals, and disk speed assumed constant (parameter w). Simulation is based on the + Quanser 3-DOF gyroscope setup. + + + **STATE:** + The state consists of the angle and angular speed of the outer red gimbal (theta = x1, thetadot = x2), + the angle and angular speed of the inner blue gimbal (phi = x3, phidot = x4), the difference to the reference + for tracking on theta and phi (tracking error theta = diff_x1, tracking error phi = diff_x3), and the + disk speed (disk speed = w): + + state = [x1, x2, x3, x4, diff_x1, diff_x3, w] + + **ACTIONS:** + The actions are the input voltage to create the red and blue gimbal torque (red voltage = u1, blue voltage = u2), + and are continuous in a range of -10 and 10V: + + action = [u1,u2] + + """ + + + metadata = { + 'render.modes' : ['human', 'rgb_array'], + 'video.frames_per_second' : 30 + } + + def __init__(self): + + # Inertias in Kg*m2 + self.Jbx1 = 0.0019 + self.Jbx2 = 0.0008 + self.Jbx3 = 0.0012 + self.Jrx1 = 0.0179 + self.Jdx1 = 0.0028 + self.Jdx3 = 0.0056 + + # Combined inertias + self.J1 = self.Jbx1 - self.Jbx3 + self.Jdx1 - self.Jdx3 + self.J2 = self.Jbx1 + self.Jdx1 + self.Jrx1 + self.J3 = self.Jbx2 + self.Jdx1 + + # Motor constants + self.Kamp = 0.5 # A/V + self.Ktorque = 0.0704 # Nm/A + self.eff = 0.86 + self.nRed = 1.5 + self.nBlue = 1 + self.KtotRed = self.Kamp*self.Ktorque*self.eff*self.nRed + self.KtotBlue = self.Kamp*self.Ktorque*self.eff*self.nBlue + + # Time step in s + self.dt = 0.05 + + # Error + self.int_diff_x1 = 0 + self.int_diff_x3 = 0 + + # Action space + self.maxVoltage = 10 # V + self.highAct = np.array([self.maxVoltage,self.maxVoltage]) + self.action_space = spaces.Box(low = -self.highAct, high = self.highAct, dtype=np.float32) + + # Observation space (here it is equal to state space) + self.maxSpeed = 100 * 2 * np.pi / 60 + self.maxAngle = np.pi + self.maxdiskSpeed = 300 * 2 * np.pi / 60 + self.highObs = np.array([self.maxAngle,self.maxSpeed,self.maxAngle,self.maxSpeed,self.maxAngle,self.maxAngle,self.maxdiskSpeed]) + self.observation_space = spaces.Box(low = -self.highObs, high = self.highObs, dtype=np.float32) + + # Seed for random number generation + self.seed() + + self.viewer = None + + def seed(self, seed=None): + self.np_random, seed = seeding.np_random(seed) + return [seed] + + + + def step(self,u): + x1, x2, x3, x4, x1_ref, x3_ref, w= self.state + u1,u2 = u + + # Angle error + diff_x1 = angle_normalize(x1 - x1_ref) + diff_x3 = angle_normalize(x3 - x3_ref) + + # Integral of error + self.int_diff_x1 = self.int_diff_x1 + diff_x1 + self.int_diff_x3 = self.int_diff_x3 + diff_x3 + + # Reward 1: differentiable reward (LQR obj function) + reward = -((3*diff_x1)**2 + (3*diff_x3)**2 + (.2*x2)**2 + (.2*x4)**2 + (.1*u1)**2 + (.1*u2)**2)\ + #-(0.01*abs(self.int_diff_x1) + 0.01*abs(self.int_diff_x3)) + + """# Count time spent in goal: + if abs(diff_x1)<0.05 and abs(diff_x3)<0.05: + self.countGoal +=1 + else: + self.countGoal = 0 + + # Reward 2: sparse reward for staying in goal range for a long time + if self.countGoal >= (self.timeGoal)/self.dt: #max expected reward over length becomes 0 + (totaltime-goaltime) + reward += 1""" + + + results = solve_ivp(fun = dxdt, t_span = (0, self.dt), y0 = [x1,x2,x3,x4], method='RK45', args=(u1,u2,self)) + + x1 = angle_normalize(results.y[0][-1]) + x2 = np.clip(results.y[1][-1],-self.maxSpeed,self.maxSpeed) + x3 = angle_normalize(results.y[2][-1]) + x4 = np.clip(results.y[3][-1],-self.maxSpeed,self.maxSpeed) + + self.state = np.asarray([x1,x2,x3,x4,x1_ref, x3_ref,w]) + + return (self.state, reward, False, {}) + + def reset(self, state = None): + + + # Generate random state (for training) or use given state (for simulation) + if state is None: + self.state = self.np_random.uniform(low=-self.highObs, high=self.highObs) + else: + self.state = state + + + return self.state + + + def render(self, mode='human'): + return None + + def close(self): + if self.viewer: + self.viewer.close() + self.viewer = None + +def dxdt(t, x, u1, u2, gyro): + + # Rewrite constants shorter + J1 = gyro.J1 + J2 = gyro.J2 + J3 = gyro.J3 + Jdx3 = gyro.Jdx3 + KtotRed = gyro.KtotRed + KtotBlue = gyro.KtotBlue + w = x[-1] + + # Convert input voltage to input torque + u1,u2 = KtotRed*u1, KtotBlue*u2 + + # Equations of motion + dx_dt = [0, 0, 0, 0] + dx_dt[0] = x[1] + dx_dt[1] = (u1+J1*np.sin(2*x[2])*x[1]*x[3]-Jdx3*np.cos(x[2])*x[3]*w)/(J2 + J1*np.power(np.sin(x[2]),2)) + dx_dt[2] = x[3] + dx_dt[3] = (u2 - J1*np.cos(x[2])*np.sin(x[2])*np.power(x[1],2)+Jdx3*np.cos(x[2])*x[1]*w)/J3 + return dx_dt + +def angle_normalize(x): + return (((x+np.pi) % (2*np.pi)) - np.pi) # To keep the angles between -pi and pi diff --git a/code/environment/gym-GyroscopeEnv/setup.py b/code/environment/gym-GyroscopeEnv/setup.py new file mode 100644 index 0000000..5540914 --- /dev/null +++ b/code/environment/gym-GyroscopeEnv/setup.py @@ -0,0 +1,6 @@ +from setuptools import setup + +setup(name='gym_GyroscopeEnv', + version='0.0.1', + install_requires=['gym','numpy','scipy'] # And any other dependencies foo needs +)