diff options
author | damic014 <damic014@umn.edu> | 2019-11-26 01:46:09 -0600 |
---|---|---|
committer | damic014 <damic014@umn.edu> | 2019-11-26 16:07:49 -0600 |
commit | 534813ea96a06b103792a682f075535193163854 (patch) | |
tree | 449331ffb8cb5700d34bf465f01b2dda1c0a3b5f /System/system_swingup_test_2.py | |
parent | Merge remote-tracking branch 'origin/library_interrupt_flags' (diff) | |
download | ee4511w-534813ea96a06b103792a682f075535193163854.tar ee4511w-534813ea96a06b103792a682f075535193163854.tar.gz ee4511w-534813ea96a06b103792a682f075535193163854.tar.bz2 ee4511w-534813ea96a06b103792a682f075535193163854.tar.lz ee4511w-534813ea96a06b103792a682f075535193163854.tar.xz ee4511w-534813ea96a06b103792a682f075535193163854.tar.zst ee4511w-534813ea96a06b103792a682f075535193163854.zip |
Add encoder measurements via thread. This should help with linear encoder accuracy and SW interrupt accuracy. Coupled with changes from the interrupt enhancement branch, the limit behavior should be much improved.
Added a parameter to allow user to set their own SW limit-reached routine (default behavior is still the same). This should also help prevent over-excursion in the swingup test.
Slightly modified swingup test to use new SW limit-reached routine. This should help prevent the system from over-excursing when the soft limits of the program are reached. Theoretically the program shouldn't be able to hit the HW limits anymore.
Diffstat (limited to 'System/system_swingup_test_2.py')
-rw-r--r-- | System/system_swingup_test_2.py | 21 |
1 files changed, 17 insertions, 4 deletions
diff --git a/System/system_swingup_test_2.py b/System/system_swingup_test_2.py index 81d5419..e240b01 100644 --- a/System/system_swingup_test_2.py +++ b/System/system_swingup_test_2.py @@ -53,7 +53,7 @@ class SwingUpEnv(): }
def __init__(self):
- self.sys = System(angular_units='Radians')
+ self.sys = System(angular_units='Radians', positive_limit=10., negative_limit=-10., sw_limit_routine=self.x_threshold_routine)
self.force_mag = 10.
self.last_time = time.time() # time for seconds between updates
@@ -73,6 +73,7 @@ class SwingUpEnv(): self.state = None
self.steps_beyond_done = None
+ self.done = False
def seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
@@ -83,7 +84,9 @@ class SwingUpEnv(): state = self.state
x, x_dot, theta, theta_dot = state
force = self.force_mag * action[0]
- self.sys.adjust(force)
+ # Do not adjust the motor further if the x_threshold has been triggered by the SW limit
+ if self.done == False:
+ self.sys.adjust(force)
costheta = math.cos(theta)
sintheta = math.sin(theta)
@@ -105,10 +108,15 @@ class SwingUpEnv(): self.state = (new_x, x_dot, new_theta, theta_dot)
self.sys.add_results(new_theta, new_x, force)
- done = x < -self.x_threshold \
+ done = theta_dot < -self.theta_dot_threshold \
+ or theta_dot > self.theta_dot_threshold \
+ or self.done == True
+
+ '''done = x < -self.x_threshold \
or x > self.x_threshold \
or theta_dot < -self.theta_dot_threshold \
- or theta_dot > self.theta_dot_threshold
+ or theta_dot > self.theta_dot_threshold \
+ or self.done == True'''
done = bool(done)
if not done:
@@ -125,6 +133,10 @@ class SwingUpEnv(): return np.array(self.state), reward, done, {'max_up_time' : self.max_up_time}
+ def x_threshold_routine(self):
+ self.done = True
+ self.sys.adjust(0)
+
def reset(self, home = True):
if home == True:
self.sys.return_home()
@@ -138,6 +150,7 @@ class SwingUpEnv(): self.max_up_time = 0
self.up = False
self.steps_beyond_done = None
+ self.done = False
return np.array(self.state)
def end(self):
|