dodox/fancy_gym
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

wip

Browse Source
This commit is contained in:
Maximilian Huettenrauch
2021-04-21 10:45:34 +02:00
parent f3d75b9a60
commit e482fc09f0
20 changed files with 629 additions and 261 deletions
Download Patch File Download Diff File Expand all files Collapse all files
alr_envs/classic_control/__init__.py
+1 -1
View File
@@ -1,3 +1,3 @@
from alr_envs.classic_control.simple_reacher import SimpleReacherEnv
from alr_envs.classic_control.viapoint_reacher import ViaPointReacher
from alr_envs.classic_control.hole_reacher import HoleReacher
from alr_envs.classic_control.hole_reacher import HoleReacher
alr_envs/classic_control/hole_reacher.py
+2 -43
View File
@@ -2,40 +2,7 @@ import gym
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import patches
from alr_envs import DmpWrapper
from alr_envs.utils.wrapper.detpmp_wrapper import DetPMPWrapper
def ccw(A, B, C):
return (C[1] - A[1]) * (B[0] - A[0]) - (B[1] - A[1]) * (C[0] - A[0]) > 1e-12
# Return true if line segments AB and CD intersect
def intersect(A, B, C, D):
return ccw(A, C, D) != ccw(B, C, D) and ccw(A, B, C) != ccw(A, B, D)
def holereacher_dmp(**kwargs):
_env = gym.make("alr_envs:HoleReacher-v0")
# _env = HoleReacher(**kwargs)
return DmpWrapper(_env, num_dof=5, num_basis=5, duration=2, dt=_env.dt, learn_goal=True, alpha_phase=3.5,
start_pos=_env.start_pos, policy_type="velocity", weights_scale=100, goal_scale=0.1)
def holereacher_fix_goal_dmp(**kwargs):
_env = gym.make("alr_envs:HoleReacher-v0")
# _env = HoleReacher(**kwargs)
return DmpWrapper(_env, num_dof=5, num_basis=5, duration=2, dt=_env.dt, learn_goal=False, alpha_phase=3.5,
start_pos=_env.start_pos, policy_type="velocity", weights_scale=50, goal_scale=1,
final_pos=np.array([2.02669572, -1.25966385, -1.51618198, -0.80946476, 0.02012344]))
def holereacher_detpmp(**kwargs):
_env = gym.make("alr_envs:HoleReacher-v0")
# _env = HoleReacher(**kwargs)
return DetPMPWrapper(_env, num_dof=5, num_basis=5, width=0.005, policy_type="velocity", start_pos=_env.start_pos,
duration=2, post_traj_time=0, dt=_env.dt, weights_scale=0.25, zero_start=True, zero_goal=False)
from alr_envs.classic_control.utils import check_self_collision
class HoleReacher(gym.Env):
@@ -166,7 +133,7 @@ class HoleReacher(gym.Env):
wall_collision = False
if not self.allow_self_collision:
self_collision = self.check_self_collision(line_points_in_taskspace)
self_collision = check_self_collision(line_points_in_taskspace)
if np.any(np.abs(self._joint_angles) > np.pi) and not self.allow_self_collision:
self_collision = True
@@ -209,14 +176,6 @@ class HoleReacher(gym.Env):
return np.squeeze(endeffector + self._joints[0, :])
def check_self_collision(self, line_points):
for i, line1 in enumerate(line_points):
for line2 in line_points[i + 2:, :, :]:
# if line1 != line2:
if intersect(line1[0], line1[-1], line2[0], line2[-1]):
return True
return False
def check_wall_collision(self, line_points):
# all points that are before the hole in x
alr_envs/classic_control/utils.py
+12 -150
View File
@@ -1,155 +1,17 @@
from alr_envs.classic_control.hole_reacher import HoleReacher
from alr_envs.classic_control.viapoint_reacher import ViaPointReacher
from alr_envs.utils.wrapper.dmp_wrapper import DmpWrapper
from alr_envs.utils.wrapper.detpmp_wrapper import DetPMPWrapper
import numpy as np
def ccw(A, B, C):
return (C[1] - A[1]) * (B[0] - A[0]) - (B[1] - A[1]) * (C[0] - A[0]) > 1e-12
def make_viapointreacher_env(rank, seed=0):
"""
Utility function for multiprocessed env.
:param env_id: (str) the environment ID
:param num_env: (int) the number of environments you wish to have in subprocesses
:param seed: (int) the initial seed for RNG
:param rank: (int) index of the subprocess
:returns a function that generates an environment
"""
def _init():
_env = ViaPointReacher(n_links=5,
allow_self_collision=False,
collision_penalty=1000)
_env = DmpWrapper(_env,
num_dof=5,
num_basis=5,
duration=2,
alpha_phase=2.5,
dt=_env.dt,
start_pos=_env.start_pos,
learn_goal=False,
policy_type="velocity",
weights_scale=50)
_env.seed(seed + rank)
return _env
return _init
# Return true if line segments AB and CD intersect
def intersect(A, B, C, D):
return ccw(A, C, D) != ccw(B, C, D) and ccw(A, B, C) != ccw(A, B, D)
def make_holereacher_env(rank, seed=0):
"""
Utility function for multiprocessed env.
def check_self_collision(line_points):
for i, line1 in enumerate(line_points):
for line2 in line_points[i + 2:, :, :]:
# if line1 != line2:
if intersect(line1[0], line1[-1], line2[0], line2[-1]):
return True
return False
:param env_id: (str) the environment ID
:param num_env: (int) the number of environments you wish to have in subprocesses
:param seed: (int) the initial seed for RNG
:param rank: (int) index of the subprocess
:returns a function that generates an environment
"""
def _init():
_env = HoleReacher(n_links=5,
allow_self_collision=False,
allow_wall_collision=False,
hole_width=0.25,
hole_depth=1,
hole_x=2,
collision_penalty=100)
_env = DmpWrapper(_env,
num_dof=5,
num_basis=5,
duration=2,
dt=_env.dt,
learn_goal=True,
alpha_phase=2,
start_pos=_env.start_pos,
policy_type="velocity",
weights_scale=50,
goal_scale=0.1
)
_env.seed(seed + rank)
return _env
return _init
def make_holereacher_fix_goal_env(rank, seed=0):
"""
Utility function for multiprocessed env.
:param env_id: (str) the environment ID
:param num_env: (int) the number of environments you wish to have in subprocesses
:param seed: (int) the initial seed for RNG
:param rank: (int) index of the subprocess
:returns a function that generates an environment
"""
def _init():
_env = HoleReacher(n_links=5,
allow_self_collision=False,
allow_wall_collision=False,
hole_width=0.15,
hole_depth=1,
hole_x=1,
collision_penalty=100)
_env = DmpWrapper(_env,
num_dof=5,
num_basis=5,
duration=2,
dt=_env.dt,
learn_goal=False,
final_pos=np.array([2.02669572, -1.25966385, -1.51618198, -0.80946476, 0.02012344]),
alpha_phase=2,
start_pos=_env.start_pos,
policy_type="velocity",
weights_scale=50,
goal_scale=1
)
_env.seed(seed + rank)
return _env
return _init
def make_holereacher_env_pmp(rank, seed=0):
"""
Utility function for multiprocessed env.
:param env_id: (str) the environment ID
:param num_env: (int) the number of environments you wish to have in subprocesses
:param seed: (int) the initial seed for RNG
:param rank: (int) index of the subprocess
:returns a function that generates an environment
"""
def _init():
_env = HoleReacher(n_links=5,
allow_self_collision=False,
allow_wall_collision=False,
hole_width=0.15,
hole_depth=1,
hole_x=1,
collision_penalty=1000)
_env = DetPMPWrapper(_env,
num_dof=5,
num_basis=5,
width=0.02,
policy_type="velocity",
start_pos=_env.start_pos,
duration=2,
post_traj_time=0,
dt=_env.dt,
weights_scale=0.2,
zero_start=True,
zero_goal=False
)
_env.seed(seed + rank)
return _env
return _init
alr_envs/classic_control/viapoint_reacher.py
+1 -9
View File
@@ -2,15 +2,7 @@ import gym
import matplotlib.pyplot as plt
import numpy as np
from alr_envs import DmpWrapper
from alr_envs.utils.utils import check_self_collision
def viapoint_dmp(**kwargs):
_env = gym.make("alr_envs:ViaPointReacher-v0")
# _env = ViaPointReacher(**kwargs)
return DmpWrapper(_env, num_dof=5, num_basis=5, duration=2, alpha_phase=2.5, dt=_env.dt,
start_pos=_env.start_pos, learn_goal=False, policy_type="velocity", weights_scale=50)
from alr_envs.classic_control.utils import check_self_collision
class ViaPointReacher(gym.Env):