diff --git a/Makefile b/Makefile
index 35662dd..531cc36 100644
--- a/Makefile
+++ b/Makefile
@@ -1,9 +1,9 @@
test:
- env PYTHONPATH=./scripts pytest --flake8 # --cov=algorithms
+ env PYTHONPATH=./scripts pytest --flake8 --ignore=./scripts/envs # --cov=algorithms
format:
isort -y
- python3.6 -m black -t py27 .
+ python3.6 -m black -t py27 . --fast
dev:
pip install -r scripts/requirements-dev.txt
diff --git a/README.md b/README.md
index 35e1a52..5874090 100644
--- a/README.md
+++ b/README.md
@@ -23,19 +23,19 @@ The [scripts](/scripts) folder contains implementations of a curated list of RL
- Twin Delayed Deep Deterministic Policy Gradient (TD3)
- TD3 (Fujimoto et al., 2018) is an extension of DDPG (Lillicrap et al., 2015), a deterministic policy gradient algorithm that uses deep neural networks for function approximation. Inspired by Deep Q-Networks (Mnih et al., 2015), DDPG uses experience replay and target network to improve stability. TD3 further improves DDPG by adding clipped double Q-learning (Van Hasselt, 2010) to mitigate overestimation bias (Thrun & Schwartz, 1993) and delaying policy updates to address variance.
- - [Example Script on LunarLander](/scripts/examples/lunarlander_continuous_v2/td3.py)
+ - [Example Script on LunarLander](/scripts/config/agent/lunarlander_continuous_v2/td3.py)
- [ArXiv Preprint](https://arxiv.org/abs/1802.09477)
- (Twin) Soft Actor Critic (SAC)
- SAC (Haarnoja et al., 2018a) incorporates maximum entropy reinforcment learning, where the agent's goal is to maximize expected reward and entropy concurrently. Combined with TD3, SAC achieves state of the art performance in various continuous control tasks. SAC has been extended to allow automatically tuning of the temperature parameter (Haarnoja et al., 2018b), which determines the importance of entropy against the expected reward.
- - [Example Script on LunarLander](/scripts/examples/lunarlander_continuous_v2/sac.py)
+ - [Example Script on LunarLander](/scripts/config/agent/lunarlander_continuous_v2/sac.py)
- [ArXiv Preprint](https://arxiv.org/abs/1801.01290) (Original SAC)
- [ArXiv Preprint](https://arxiv.org/abs/1812.05905) (SAC with autotuned temperature)
- TD3 from Demonstrations, SAC from Demonstrations (TD3fD, SACfD)
- DDPGfD (Vecerik et al., 2017) is an imitation learning algorithm that infuses demonstration data into experience replay. DDPGfD also improved DDPG by (1) using prioritized experience replay (Schaul et al., 2015), (2) adding n-step returns, (3) learning multiple times per environment step, and (4) adding L2 regularizers to actor and critic losses. We incorporated these improvements to TD3 and SAC and found that it dramatically improves their performance.
- - [Example Script of TD3fD on LunarLander](/scripts/examples/lunarlander_continuous_v2/td3fd.py)
- - [Example Script of SACfD on LunarLander](/scripts/examples/lunarlander_continuous_v2/sacfd.py)
+ - [Example Script of TD3fD on LunarLander](/scripts/config/agent/lunarlander_continuous_v2/td3fd.py)
+ - [Example Script of SACfD on LunarLander](/scripts/config/agent/lunarlander_continuous_v2/sacfd.py)
- [ArXiv Preprint](https://arxiv.org/abs/1707.08817)
## Installation
diff --git a/launch/open_manipulator_env.launch b/launch/open_manipulator_env.launch
index 0cbda14..566b8df 100755
--- a/launch/open_manipulator_env.launch
+++ b/launch/open_manipulator_env.launch
@@ -47,6 +47,4 @@
-
-
diff --git a/scripts/examples/__init__.py b/scripts/config/__init__.py
similarity index 100%
rename from scripts/examples/__init__.py
rename to scripts/config/__init__.py
diff --git a/scripts/examples/lunarlander_continuous_v2/__init__.py b/scripts/config/agent/__init__.py
similarity index 100%
rename from scripts/examples/lunarlander_continuous_v2/__init__.py
rename to scripts/config/agent/__init__.py
diff --git a/scripts/examples/open_manipulator_reacher_v0/__init__.py b/scripts/config/agent/lunarlander_continuous_v2/__init__.py
old mode 100755
new mode 100644
similarity index 100%
rename from scripts/examples/open_manipulator_reacher_v0/__init__.py
rename to scripts/config/agent/lunarlander_continuous_v2/__init__.py
diff --git a/scripts/examples/lunarlander_continuous_v2/sac.py b/scripts/config/agent/lunarlander_continuous_v2/sac.py
similarity index 83%
rename from scripts/examples/lunarlander_continuous_v2/sac.py
rename to scripts/config/agent/lunarlander_continuous_v2/sac.py
index bd937d0..eaeb02e 100644
--- a/scripts/examples/lunarlander_continuous_v2/sac.py
+++ b/scripts/config/agent/lunarlander_continuous_v2/sac.py
@@ -33,22 +33,28 @@ hyper_params = {
"AUTO_ENTROPY_TUNING": True,
"WEIGHT_DECAY": 0.0,
"INITIAL_RANDOM_ACTION": 5000,
+ "NETWORK": {
+ "ACTOR_HIDDEN_SIZES": [256, 256],
+ "VF_HIDDEN_SIZES": [256, 256],
+ "QF_HIDDEN_SIZES": [256, 256],
+ },
}
-def run(env, args, state_dim, action_dim):
+def get(env, args):
"""Run training or test.
Args:
env (gym.Env): openAI Gym environment with continuous action space
args (argparse.Namespace): arguments including training settings
- state_dim (int): dimension of states
- action_dim (int): dimension of actions
"""
- hidden_sizes_actor = [256, 256]
- hidden_sizes_vf = [256, 256]
- hidden_sizes_qf = [256, 256]
+ state_dim = env.observation_space.shape[0]
+ action_dim = env.action_space.shape[0]
+
+ hidden_sizes_actor = hyper_params["NETWORK"]["ACTOR_HIDDEN_SIZES"]
+ hidden_sizes_vf = hyper_params["NETWORK"]["VF_HIDDEN_SIZES"]
+ hidden_sizes_qf = hyper_params["NETWORK"]["QF_HIDDEN_SIZES"]
# target entropy
target_entropy = -np.prod((action_dim,)).item() # heuristic
@@ -102,10 +108,4 @@ def run(env, args, state_dim, action_dim):
optims = (actor_optim, vf_optim, qf_1_optim, qf_2_optim)
# create an agent
- agent = Agent(env, args, hyper_params, models, optims, target_entropy)
-
- # run
- if args.test:
- agent.test()
- else:
- agent.train()
+ return Agent(env, args, hyper_params, models, optims, target_entropy)
diff --git a/scripts/examples/lunarlander_continuous_v2/sacfd.py b/scripts/config/agent/lunarlander_continuous_v2/sacfd.py
similarity index 84%
rename from scripts/examples/lunarlander_continuous_v2/sacfd.py
rename to scripts/config/agent/lunarlander_continuous_v2/sacfd.py
index d224699..7e0ccc4 100644
--- a/scripts/examples/lunarlander_continuous_v2/sacfd.py
+++ b/scripts/config/agent/lunarlander_continuous_v2/sacfd.py
@@ -42,22 +42,28 @@ hyper_params = {
"PER_EPS": 1e-6,
"PER_EPS_DEMO": 1.0,
"INITIAL_RANDOM_ACTION": int(5e3),
+ "NETWORK": {
+ "ACTOR_HIDDEN_SIZES": [256, 256],
+ "VF_HIDDEN_SIZES": [256, 256],
+ "QF_HIDDEN_SIZES": [256, 256],
+ },
}
-def run(env, args, state_dim, action_dim):
+def get(env, args):
"""Run training or test.
Args:
env (gym.Env): openAI Gym environment with continuous action space
args (argparse.Namespace): arguments including training settings
- state_dim (int): dimension of states
- action_dim (int): dimension of actions
"""
- hidden_sizes_actor = [256, 256]
- hidden_sizes_vf = [256, 256]
- hidden_sizes_qf = [256, 256]
+ state_dim = env.observation_space.shape[0]
+ action_dim = env.action_space.shape[0]
+
+ hidden_sizes_actor = hyper_params["NETWORK"]["ACTOR_HIDDEN_SIZES"]
+ hidden_sizes_vf = hyper_params["NETWORK"]["VF_HIDDEN_SIZES"]
+ hidden_sizes_qf = hyper_params["NETWORK"]["QF_HIDDEN_SIZES"]
# target entropy
target_entropy = -np.prod((action_dim,)).item() # heuristic
@@ -109,10 +115,4 @@ def run(env, args, state_dim, action_dim):
optims = (actor_optim, vf_optim, qf_1_optim, qf_2_optim)
# create an agent
- agent = Agent(env, args, hyper_params, models, optims, target_entropy)
-
- # run
- if args.test:
- agent.test()
- else:
- agent.train()
+ return Agent(env, args, hyper_params, models, optims, target_entropy)
diff --git a/scripts/examples/lunarlander_continuous_v2/td3.py b/scripts/config/agent/lunarlander_continuous_v2/td3.py
similarity index 88%
rename from scripts/examples/lunarlander_continuous_v2/td3.py
rename to scripts/config/agent/lunarlander_continuous_v2/td3.py
index 32ff29e..a5d3b1a 100644
--- a/scripts/examples/lunarlander_continuous_v2/td3.py
+++ b/scripts/config/agent/lunarlander_continuous_v2/td3.py
@@ -28,21 +28,23 @@ hyper_params = {
"TARGET_POLICY_NOISE_CLIP": 0.5,
"POLICY_UPDATE_FREQ": 2,
"INITIAL_RANDOM_ACTIONS": 1e4,
+ "NETWORK": {"ACTOR_HIDDEN_SIZES": [400, 300], "CRITIC_HIDDEN_SIZES": [400, 300]},
}
-def run(env, args, state_dim, action_dim):
+def get(env, args):
"""Run training or test.
Args:
env (gym.Env): openAI Gym environment with continuous action space
args (argparse.Namespace): arguments including training settings
- state_dim (int): dimension of states
- action_dim (int): dimension of actions
"""
- hidden_sizes_actor = [400, 300]
- hidden_sizes_critic = [400, 300]
+ state_dim = env.observation_space.shape[0]
+ action_dim = env.action_space.shape[0]
+
+ hidden_sizes_actor = hyper_params["NETWORK"]["ACTOR_HIDDEN_SIZES"]
+ hidden_sizes_critic = hyper_params["NETWORK"]["CRITIC_HIDDEN_SIZES"]
# create actor
actor = MLP(
@@ -123,10 +125,4 @@ def run(env, args, state_dim, action_dim):
noises = (exploration_noise, target_policy_noise)
# create an agent
- agent = Agent(env, args, hyper_params, models, optims, noises)
-
- # run
- if args.test:
- agent.test()
- else:
- agent.train()
+ return Agent(env, args, hyper_params, models, optims, noises)
diff --git a/scripts/examples/lunarlander_continuous_v2/td3fd.py b/scripts/config/agent/lunarlander_continuous_v2/td3fd.py
similarity index 89%
rename from scripts/examples/lunarlander_continuous_v2/td3fd.py
rename to scripts/config/agent/lunarlander_continuous_v2/td3fd.py
index 7f13074..5fd2056 100644
--- a/scripts/examples/lunarlander_continuous_v2/td3fd.py
+++ b/scripts/config/agent/lunarlander_continuous_v2/td3fd.py
@@ -40,21 +40,23 @@ hyper_params = {
"PER_BETA": 1.0,
"PER_EPS": 1e-6,
"PER_EPS_DEMO": 1.0,
+ "NETWORK": {"ACTOR_HIDDEN_SIZES": [400, 300], "CRITIC_HIDDEN_SIZES": [400, 300]},
}
-def run(env, args, state_dim, action_dim):
+def get(env, args):
"""Run training or test.
Args:
env (gym.Env): openAI Gym environment with continuous action space
args (argparse.Namespace): arguments including training settings
- state_dim (int): dimension of states
- action_dim (int): dimension of actions
"""
- hidden_sizes_actor = [400, 300]
- hidden_sizes_critic = [400, 300]
+ state_dim = env.observation_space.shape[0]
+ action_dim = env.action_space.shape[0]
+
+ hidden_sizes_actor = hyper_params["NETWORK"]["ACTOR_HIDDEN_SIZES"]
+ hidden_sizes_critic = hyper_params["NETWORK"]["CRITIC_HIDDEN_SIZES"]
# create actor
actor = MLP(
@@ -135,10 +137,4 @@ def run(env, args, state_dim, action_dim):
noises = (exploration_noise, target_policy_noise)
# create an agent
- agent = Agent(env, args, hyper_params, models, optims, noises)
-
- # run
- if args.test:
- agent.test()
- else:
- agent.train()
+ return Agent(env, args, hyper_params, models, optims, noises)
diff --git a/scripts/examples/reacher-v1/__init__.py b/scripts/config/agent/open_manipulator_reacher_v0/__init__.py
old mode 100644
new mode 100755
similarity index 100%
rename from scripts/examples/reacher-v1/__init__.py
rename to scripts/config/agent/open_manipulator_reacher_v0/__init__.py
diff --git a/scripts/examples/reacher-v1/td3.py b/scripts/config/agent/open_manipulator_reacher_v0/td3.py
old mode 100644
new mode 100755
similarity index 88%
rename from scripts/examples/reacher-v1/td3.py
rename to scripts/config/agent/open_manipulator_reacher_v0/td3.py
index d309bfd..665b1ed
--- a/scripts/examples/reacher-v1/td3.py
+++ b/scripts/config/agent/open_manipulator_reacher_v0/td3.py
@@ -28,21 +28,23 @@ hyper_params = {
"TARGET_POLICY_NOISE_CLIP": 0.5,
"POLICY_UPDATE_FREQ": 2,
"INITIAL_RANDOM_ACTIONS": 1e4,
+ "NETWORK": {"ACTOR_HIDDEN_SIZES": [400, 300], "CRITIC_HIDDEN_SIZES": [400, 300]},
}
-def run(env, args, state_dim, action_dim):
+def get(env, args):
"""Run training or test.
Args:
env (gym.Env): openAI Gym environment with continuous action space
args (argparse.Namespace): arguments including training settings
- state_dim (int): dimension of states
- action_dim (int): dimension of actions
"""
- hidden_sizes_actor = [400, 300]
- hidden_sizes_critic = [400, 300]
+ state_dim = env.observation_space.shape[0]
+ action_dim = env.action_space.shape[0]
+
+ hidden_sizes_actor = hyper_params["NETWORK"]["ACTOR_HIDDEN_SIZES"]
+ hidden_sizes_critic = hyper_params["NETWORK"]["CRITIC_HIDDEN_SIZES"]
# create actor
actor = MLP(
@@ -123,10 +125,4 @@ def run(env, args, state_dim, action_dim):
noises = (exploration_noise, target_policy_noise)
# create an agent
- agent = Agent(env, args, hyper_params, models, optims, noises)
-
- # run
- if args.test:
- agent.test()
- else:
- agent.train()
+ return Agent(env, args, hyper_params, models, optims, noises)
diff --git a/scripts/config/agent/reacher-v1/__init__.py b/scripts/config/agent/reacher-v1/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/scripts/examples/reacher-v1/sac.py b/scripts/config/agent/reacher-v1/sac.py
similarity index 83%
rename from scripts/examples/reacher-v1/sac.py
rename to scripts/config/agent/reacher-v1/sac.py
index c1f0b42..42b4159 100644
--- a/scripts/examples/reacher-v1/sac.py
+++ b/scripts/config/agent/reacher-v1/sac.py
@@ -34,22 +34,28 @@ hyper_params = {
"DELAYED_UPDATE": 2,
"WEIGHT_DECAY": 0.0,
"INITIAL_RANDOM_ACTION": int(1e4),
+ "NETWORK": {
+ "ACTOR_HIDDEN_SIZES": [256, 256],
+ "VF_HIDDEN_SIZES": [256, 256],
+ "QF_HIDDEN_SIZES": [256, 256],
+ },
}
-def run(env, args, state_dim, action_dim):
+def get(env, args):
"""Run training or test.
Args:
env (gym.Env): openAI Gym environment with continuous action space
args (argparse.Namespace): arguments including training settings
- state_dim (int): dimension of states
- action_dim (int): dimension of actions
"""
- hidden_sizes_actor = [256, 256]
- hidden_sizes_vf = [256, 256]
- hidden_sizes_qf = [256, 256]
+ state_dim = env.observation_space.shape[0]
+ action_dim = env.action_space.shape[0]
+
+ hidden_sizes_actor = hyper_params["NETWORK"]["ACTOR_HIDDEN_SIZES"]
+ hidden_sizes_vf = hyper_params["NETWORK"]["VF_HIDDEN_SIZES"]
+ hidden_sizes_qf = hyper_params["NETWORK"]["QF_HIDDEN_SIZES"]
# target entropy
target_entropy = -np.prod((action_dim,)).item() # heuristic
@@ -103,10 +109,4 @@ def run(env, args, state_dim, action_dim):
optims = (actor_optim, vf_optim, qf_1_optim, qf_2_optim)
# create an agent
- agent = Agent(env, args, hyper_params, models, optims, target_entropy)
-
- # run
- if args.test:
- agent.test()
- else:
- agent.train()
+ return Agent(env, args, hyper_params, models, optims, target_entropy)
diff --git a/scripts/examples/reacher-v1/sacfd.py b/scripts/config/agent/reacher-v1/sacfd.py
similarity index 84%
rename from scripts/examples/reacher-v1/sacfd.py
rename to scripts/config/agent/reacher-v1/sacfd.py
index 963b9c1..77a0530 100644
--- a/scripts/examples/reacher-v1/sacfd.py
+++ b/scripts/config/agent/reacher-v1/sacfd.py
@@ -42,22 +42,28 @@ hyper_params = {
"PER_EPS": 1e-6,
"PER_EPS_DEMO": 1.0,
"INITIAL_RANDOM_ACTION": int(1e4),
+ "NETWORK": {
+ "ACTOR_HIDDEN_SIZES": [256, 256],
+ "VF_HIDDEN_SIZES": [256, 256],
+ "QF_HIDDEN_SIZES": [256, 256],
+ },
}
-def run(env, args, state_dim, action_dim):
+def get(env, args):
"""Run training or test.
Args:
env (gym.Env): openAI Gym environment with continuous action space
args (argparse.Namespace): arguments including training settings
- state_dim (int): dimension of states
- action_dim (int): dimension of actions
"""
- hidden_sizes_actor = [256, 256]
- hidden_sizes_vf = [256, 256]
- hidden_sizes_qf = [256, 256]
+ state_dim = env.observation_space.shape[0]
+ action_dim = env.action_space.shape[0]
+
+ hidden_sizes_actor = hyper_params["NETWORK"]["ACTOR_HIDDEN_SIZES"]
+ hidden_sizes_vf = hyper_params["NETWORK"]["VF_HIDDEN_SIZES"]
+ hidden_sizes_qf = hyper_params["NETWORK"]["QF_HIDDEN_SIZES"]
# target entropy
target_entropy = -np.prod((action_dim,)).item() # heuristic
@@ -109,10 +115,4 @@ def run(env, args, state_dim, action_dim):
optims = (actor_optim, vf_optim, qf_1_optim, qf_2_optim)
# create an agent
- agent = Agent(env, args, hyper_params, models, optims, target_entropy)
-
- # run
- if args.test:
- agent.test()
- else:
- agent.train()
+ return Agent(env, args, hyper_params, models, optims, target_entropy)
diff --git a/scripts/examples/open_manipulator_reacher_v0/td3.py b/scripts/config/agent/reacher-v1/td3.py
old mode 100755
new mode 100644
similarity index 88%
rename from scripts/examples/open_manipulator_reacher_v0/td3.py
rename to scripts/config/agent/reacher-v1/td3.py
index 0d3810f..665b1ed
--- a/scripts/examples/open_manipulator_reacher_v0/td3.py
+++ b/scripts/config/agent/reacher-v1/td3.py
@@ -7,6 +7,7 @@
import torch
import torch.optim as optim
+
from algorithms.common.networks.mlp import MLP
from algorithms.common.noise import GaussianNoise
from algorithms.td3.agent import Agent
@@ -27,21 +28,23 @@ hyper_params = {
"TARGET_POLICY_NOISE_CLIP": 0.5,
"POLICY_UPDATE_FREQ": 2,
"INITIAL_RANDOM_ACTIONS": 1e4,
+ "NETWORK": {"ACTOR_HIDDEN_SIZES": [400, 300], "CRITIC_HIDDEN_SIZES": [400, 300]},
}
-def run(env, args, state_dim, action_dim):
+def get(env, args):
"""Run training or test.
Args:
env (gym.Env): openAI Gym environment with continuous action space
args (argparse.Namespace): arguments including training settings
- state_dim (int): dimension of states
- action_dim (int): dimension of actions
"""
- hidden_sizes_actor = [400, 300]
- hidden_sizes_critic = [400, 300]
+ state_dim = env.observation_space.shape[0]
+ action_dim = env.action_space.shape[0]
+
+ hidden_sizes_actor = hyper_params["NETWORK"]["ACTOR_HIDDEN_SIZES"]
+ hidden_sizes_critic = hyper_params["NETWORK"]["CRITIC_HIDDEN_SIZES"]
# create actor
actor = MLP(
@@ -122,10 +125,4 @@ def run(env, args, state_dim, action_dim):
noises = (exploration_noise, target_policy_noise)
# create an agent
- agent = Agent(env, args, hyper_params, models, optims, noises)
-
- # run
- if args.test:
- agent.test()
- else:
- agent.train()
+ return Agent(env, args, hyper_params, models, optims, noises)
diff --git a/scripts/examples/reacher-v1/td3fd.py b/scripts/config/agent/reacher-v1/td3fd.py
similarity index 89%
rename from scripts/examples/reacher-v1/td3fd.py
rename to scripts/config/agent/reacher-v1/td3fd.py
index 16176df..9c27f91 100644
--- a/scripts/examples/reacher-v1/td3fd.py
+++ b/scripts/config/agent/reacher-v1/td3fd.py
@@ -40,21 +40,23 @@ hyper_params = {
"PER_BETA": 1.0,
"PER_EPS": 1e-6,
"PER_EPS_DEMO": 1.0,
+ "NETWORK": {"ACTOR_HIDDEN_SIZES": [400, 300], "CRITIC_HIDDEN_SIZES": [400, 300]},
}
-def run(env, args, state_dim, action_dim):
+def get(env, args):
"""Run training or test.
Args:
env (gym.Env): openAI Gym environment with continuous action space
args (argparse.Namespace): arguments including training settings
- state_dim (int): dimension of states
- action_dim (int): dimension of actions
"""
- hidden_sizes_actor = [400, 300]
- hidden_sizes_critic = [400, 300]
+ state_dim = env.observation_space.shape[0]
+ action_dim = env.action_space.shape[0]
+
+ hidden_sizes_actor = hyper_params["NETWORK"]["ACTOR_HIDDEN_SIZES"]
+ hidden_sizes_critic = hyper_params["NETWORK"]["CRITIC_HIDDEN_SIZES"]
# create actor
actor = MLP(
@@ -135,10 +137,4 @@ def run(env, args, state_dim, action_dim):
noises = (exploration_noise, target_policy_noise)
# create an agent
- agent = Agent(env, args, hyper_params, models, optims, noises)
-
- # run
- if args.test:
- agent.test()
- else:
- agent.train()
+ return Agent(env, args, hyper_params, models, optims, noises)
diff --git a/scripts/config/environment/__init__.py b/scripts/config/environment/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/scripts/config/environment/open_manipulator.py b/scripts/config/environment/open_manipulator.py
new file mode 100755
index 0000000..8e76904
--- /dev/null
+++ b/scripts/config/environment/open_manipulator.py
@@ -0,0 +1,52 @@
+from math import pi
+
+from geometry_msgs.msg import Quaternion
+
+config = {
+ "TERM_COUNT": 10,
+ "SUCCESS_COUNT": 10,
+ "OVERHEAD_ORIENTATION": Quaternion(
+ x=-0.00142460053167, y=0.999994209902, z=-0.00177030764765, w=0.00253311793936
+ ),
+ # box boundary
+ "POLAR_RADIAN_BOUNDARY": (0.134, 0.32),
+ "POLAR_THETA_BOUNDARY": (-pi * 0.7 / 4, pi * 0.7 / 4),
+ "Z_BOUNDARY": (0.05, 0.28),
+ "JOINT_LIMITS": {
+ "HIGH": {
+ "J1": pi * 0.9,
+ "J2": pi * 0.5,
+ "J3": pi * 0.44,
+ "J4": pi * 0.65,
+ "GRIP": -0.001,
+ },
+ "LOW": {
+ "J1": -pi * 0.9,
+ "J2": -pi * 0.57,
+ "J3": -pi * 0.3,
+ "J4": -pi * 0.57,
+ "GRIP": 0.019,
+ },
+ },
+ # Global variables
+ "ACTION_DIM": 5, # Cartesian
+ "OBSERVATION_DIM": (25,),
+ # terminal condition
+ "INNER_RADIAN": 0.134,
+ "OUTER_RADIAN": 0.3,
+ "LOWER_RADIAN": 0.384,
+ "INNER_Z": 0.321,
+ "OUTER_Z": 0.250,
+ "LOWER_Z": 0.116,
+ "ENV_MODE": "sim",
+ "TRAIN_MODE": True,
+ "MAX_EPISODE_STEPS": 100,
+ "DISTANCE_THRESHOLD": 0.1,
+ "REWARD_RESCALE_RATIO": 1.0,
+ "REWARD_FUNC": "l2",
+ "CONTROL_MODE": "position",
+}
+
+
+def get():
+ return config
diff --git a/scripts/envs/__init__.py b/scripts/envs/__init__.py
new file mode 100644
index 0000000..e9c3fb4
--- /dev/null
+++ b/scripts/envs/__init__.py
@@ -0,0 +1,3 @@
+from .open_manipulator import OpenManipulatorReacherEnv
+
+__all__ = ["OpenManipulatorReacherEnv"]
diff --git a/scripts/envs/open_manipulator/__init__.py b/scripts/envs/open_manipulator/__init__.py
new file mode 100644
index 0000000..40f86fd
--- /dev/null
+++ b/scripts/envs/open_manipulator/__init__.py
@@ -0,0 +1,3 @@
+from .open_manipulator_reacher_env import OpenManipulatorReacherEnv
+
+__all__ = ["OpenManipulatorReacherEnv"]
diff --git a/scripts/envs/open_manipulator/open_manipulator.py b/scripts/envs/open_manipulator/open_manipulator.py
deleted file mode 100755
index 1841f3a..0000000
--- a/scripts/envs/open_manipulator/open_manipulator.py
+++ /dev/null
@@ -1,523 +0,0 @@
-#! /usr/bin/env python
-
-import gym
-import copy
-import math
-import os
-import sys
-import time
-from random import *
-from string import Template
-from math import pi, cos, sin, radians
-import cv2
-import numpy as np
-import rospkg
-
-import rospy
-from control_msgs.msg import JointTrajectoryControllerState
-from cv_bridge import CvBridge, CvBridgeError
-from gazebo_msgs.msg import ContactsState
-from gazebo_msgs.srv import DeleteModel, GetModelState, SetModelState, SpawnModel
-# reads open_manipulator's state
-from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion
-from open_manipulator_msgs.msg import *
-from sensor_msgs.msg import Image, JointState
-from std_msgs.msg import *
-from tf import TransformListener
-from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
-
-base_dir = os.path.dirname(os.path.realpath(__file__))
-
-overhead_orientation = Quaternion(
- x=-0.00142460053167, y=0.999994209902, z=-0.00177030764765, w=0.00253311793936
-)
-
-# safe joint limits
-joint_limits = {'hi':{'j1':pi*0.9, 'j2':pi*0.5, 'j3':pi*0.44, 'j4': pi*0.65, 'grip':-.001 },
- 'lo':{'j1':-pi*0.9, 'j2':-pi*0.57, 'j3':-pi*0.3, 'j4':-pi*0.57, 'grip': .019 }}
-
-#
-
-
-cartesian_limits = {}
-# safe cartesian limits
-# robot @ home-pose : (0.134, 0.0, 0.241)
-
-# episode termination condition
-X_MIN = 0.1
-X_MAX = 0.5
-Y_MIN = -0.3
-Y_MAX = 0.3
-Z_MIN = 0.0
-Z_MAX = 0.6
-TERM_COUNT = 10
-SUC_COUNT = 10
-
-
-# Global variables
-# -------------------------
-ACTION_DIM = 3 # Cartesian
-OBS_DIM = (100, 100, 3) # POMDP
-STATE_DIM = 24 # MDP
-
-
-class OpenManipulatorEnv:
- def __init__(
- self,
- max_steps=700,
- isdagger=False,
- isPOMDP=False,
- isreal=False,
- train_indicator=0,
- ):
- """An implementation of OpenAI-Gym style robot reacher environment
- TODO: add method that receives target object's pose as state
- """
- rospy.init_node("OpenManipulatorEnv")
- self.train_indicator = train_indicator # 0: Train 1:Test
- self.isdagger = isdagger
- self.isPOMDP = isPOMDP
- self.isreal = isreal
- self.currentDist = 1
- self.previousDist = 1
- self.reached = False
- self.tf = TransformListener()
- self.bridge = CvBridge()
- self.joint_speeds = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
- self.joint_positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
- self.joint_velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
- self.joint_efforts = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
- self.right_endpoint_position = [0, 0, 0]
- self.max_steps = max_steps
- self.done = False
- self.reward = 0
- self.reward_rescale = 1.0
- self.isDemo = False
- self.reward_type = "sparse"
- self.termination_count = 0
- self.success_count = 0
- self._control_mode = 'position' # TODO: add 'velocity', 'effort' control methods
-
- self.pub_gripper_position = rospy.Publisher(
- "/open_manipulator/gripper_position/command", Float64, queue_size=1
- )
- self.pub_gripper_sub_position = rospy.Publisher(
- "/open_manipulator/gripper_sub_position/command", Float64, queue_size=1
- )
- self.pub_joint1_position = rospy.Publisher(
- "/open_manipulator/joint1_position/command", Float64, queue_size=1
- )
- self.pub_joint2_position = rospy.Publisher(
- "/open_manipulator/joint2_position/command", Float64, queue_size=1
- )
- self.pub_joint3_position = rospy.Publisher(
- "/open_manipulator/joint3_position/command", Float64, queue_size=1
- )
- self.pub_joint4_position = rospy.Publisher(
- "/open_manipulator/joint4_position/command", Float64, queue_size=1
- )
-
- # TODO: manage this attribute when it's real test environment
- self.joints_position_cmd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
- self.kinematics_cmd = [0.0, 0.0, 0.0]
- self.sub_joint_state = rospy.Subscriber(
- "/open_manipulator/joint_states", JointState, self.joint_state_callback
- )
- # joint position/velocity/effort
- self.sub_kinematics_pose = rospy.Subscriber(
- "/open_manipulator/gripper/kinematics_pose",
- KinematicsPose,
- self.kinematics_pose_callback,
- )
- self.sub_robot_state = rospy.Subscriber(
- "/open_manipulator/states", OpenManipulatorState, self.robot_state_callback
- )
- # cs position / orientation
- # variables for subscribe the joint states
- self.joint_names = [
- "gripper",
- "gripper_sub",
- "joint1",
- "joint2",
- "joint3",
- "joint4",
- ] # name: [gripper, gripper_sub, joint1, joint2, joint3, joint4]
- self.joint_positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
- self.joint_velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
- self.joint_efforts = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
-
- # ee pose of robot -> used to compute reward.
- self.gripper_position = [0.0, 0.0, 0.0] # [x, y, z] cartesian position
- self.gripper_orientiation = [
- 0.0,
- 0.0,
- 0.0,
- ] # [x, y, z, w] quaternion orientation
- self.distance_threshold = 0.1
-
- # used for per-step elapsed time measurement
- self.tic = 0.0
- self.toc = 0.0
- self.elapsed = 0.0
- # self.initial_state = self.get_joints_states().copy()
- self._action_scale = 1.0
-
- # open manipulator statets
- self.moving_state = ""
- self.actuator_state = ""
- self.init_robot_pose()
- # TODO: replace following attributes with those inherited from gym.env
- self.reward_range = None
- self.metadata = None
- self._max_episode_steps = 50
-
- rospy.on_shutdown(self._delete_target_block)
-
- def get_observation(self):
- """
- Get robot observation.
-
- :return: robot observation
- """
- # cartesian space : TODO: consider if more info (e.g. lin/ang velocitiy in CS) is necessary.
- gripper_pos = np.array(self.gripper_position)
- gripper_ori = np.array(self.gripper_orientiation)
-
- # joint space
- robot_joint_angles = np.array(self.joint_positions)
- robot_joint_velocities = np.array(self.joint_velocities)
- robot_joint_efforts = np.array(self.joint_efforts)
-
- obs = np.concatenate(
- (gripper_pos, gripper_ori, robot_joint_angles,
- robot_joint_velocities, robot_joint_efforts))
- return obs
-
- @property
- def observation_space(self):
- """ return the open manipulator's state space for this specific environment.
- """
- return gym.spaces.Box(
- -np.inf,
- np.inf,
- shape=self.get_observation().shape,
- dtype=np.float32)
-
- @property
- def action_space(self):
- """ return the open manipulator's action space for this specific environment.
- TODO: expand to various action space types.
- """
- if self._control_mode == 'position':
- lower_bounds = np.array(
- [joint_limits['lo']['j1'], joint_limits['lo']['j2'], joint_limits['lo']['j3'], joint_limits['lo']['j4'], joint_limits['lo']['grip']])
- upper_bounds = np.array(
- [joint_limits['hi']['j1'], joint_limits['hi']['j2'], joint_limits['hi']['j3'], joint_limits['hi']['j4'], joint_limits['hi']['grip']])
- elif self._control_mode == 'velocity':
- raise NotImplementedError('Control mode %s is not implemented yet.' % self._control_mode)
-
- elif self._control_mode == 'effort':
- raise NotImplementedError('Control mode %s is not implemented yet.' % self._control_mode)
- else:
- raise ValueError('Control mode %s is not known!' % self._control_mode)
- return gym.spaces.Box(
- lower_bounds,
- upper_bounds,
- dtype=np.float32)
-
- def seed(seed):
- """ apply random seed to the environment.
- TODO: implement this method.
- """
- return True
-
- def render(self, mode):
- pass
-
- def robot_state_callback(self, msg):
- self.moving_state = msg.open_manipulator_moving_state # "MOVING" / "STOPPED"
- self.actuator_state = (
- msg.open_manipulator_actuator_state
- ) # "ACTUATOR_ENABLE" / "ACTUATOR_DISABLE"
-
- def joint_state_callback(self, msg):
- """Callback function of joint states subscriber.
- Argument: msg
- """
- self.joints_states = msg
- self.joint_names = self.joints_states.name
- self.joint_positions = self.joints_states.position
- self.joint_velocities = self.joints_states.velocity
- self.joint_efforts = self.joints_states.effort
- # penalize jerky motion in reward for shaped reward setting.
- self.squared_sum_vel = np.linalg.norm(np.array(self.joint_velocities))
-
- def kinematics_pose_callback(self, msg):
- """Callback function of gripper kinematic pose subscriber.
- Argument: msg
- """
- self.kinematics_pose = msg
- _gripper_position = self.kinematics_pose.pose.position
- self.gripper_position = [
- _gripper_position.x,
- _gripper_position.y,
- _gripper_position.z,
- ]
- _gripper_orientiation = self.kinematics_pose.pose.orientation
- self.gripper_orientiation = [
- _gripper_orientiation.x,
- _gripper_orientiation.y,
- _gripper_orientiation.z,
- _gripper_orientiation.w,
- ]
-
- # get and set function
- # ----------------------------
-
- def get_joints_states(self):
- """Returns current joints states of robot including position, velocity, effort
- Returns: Float64[] self.joints_position, self.joints_velocity, self.joint_effort
- """
- return self.joint_positions, self.joint_velocities, self.joint_efforts
-
- def get_gripper_pose(self):
- """Returns gripper end effector position
- Returns: Pose().position, Pose().orientation
- """
- return self.gripper_position, self.gripper_orientiation
-
- def get_gripper_position(self):
- """Returns gripper end effector position
- Returns: Pose().position
- """
- return self.gripper_position
-
- def set_joints_position(self, joints_angles):
- """Move joints using joint position command publishers.
- Argument: joints_position_cmd
- self.joints_position_cmd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
- """
- # rospy.loginfo(Set joint position)
- self.pub_gripper_position.publish(joints_angles[0])
- self.pub_joint1_position.publish(joints_angles[1])
- self.pub_joint2_position.publish(joints_angles[2])
- self.pub_joint3_position.publish(joints_angles[3])
- self.pub_joint4_position.publish(joints_angles[4])
-
- def step(self, action=np.array([1, 1, 1, 1, 1, 1]), step=0):
- """Function executed each time step.
- Here we get the action execute it in a time step and retrieve the
- observations generated by that action.
- :param action:
- :return: obs, reward, done
- """
- self.prev_tic = self.tic
- self.tic = time.time()
- self.elapsed = time.time() - self.prev_tic
- self.done = False
- if step == self.max_steps:
- self.done = True
- act = action.flatten().tolist()
- self.set_joints_position(act)
- curDist = self._get_dist()
- if not self.isreal:
- self.reward = self._compute_reward()
- if self._check_for_termination():
- print("======================================================")
- print("Terminates current Episode : OUT OF BOUNDARY")
- print("======================================================")
- elif self._check_for_success():
- print("======================================================")
- print("Succeeded current Episode")
- print("======================================================")
- _joint_pos, _joint_vels, _joint_effos = self.get_joints_states()
- # obj_pos = self._get_target_obj_obs() # TODO: implement this function call.
-
- if np.mod(step, 10) == 0:
- if not self.isreal:
- print("DISTANCE : ", curDist)
- print("PER STEP ELAPSED : ", self.elapsed)
- print("SPARSE REWARD : ", self.reward_rescale * self.reward)
- print("Current EE pos: ", self.gripper_position)
- print("Actions: ", act)
-
-
- obs = self.get_observation()
- info = ''
-
- return obs, self.reward_rescale * self.reward, self.done, info
-
- def reset(self):
- # Attempt to reset the simulator. Since we randomize initial conditions, it
- # is possible to get into a state with numerical issues (e.g. due to penetration or
- # Gimbel lock) or we may not achieve an initial condition (e.g. an object is within the hand).
- # In this case, we just keep randomizing until we eventually achieve a valid initial
- # configuration.
- did_reset_sim = False
- self._reset_gazebo_world()
- _joint_pos, _joint_vels, _joint_effos = self.get_joints_states()
- obs = self.get_observation()
-
- return obs
-
- def _check_robot_moving(self):
- """Check if robot has reached its initial pose.
- """
- while not rospy.is_shutdown():
- if self.moving_state == "STOPPED":
- break
- return True
-
- def _reset_gazebo_world(self):
- """
- Method that randomly initialize the state of robot agent and surrounding envs (including target obj.)
- """
- self._delete_target_block()
-
- self.pub_gripper_position.publish(np.random.uniform(0.0, 0.1))
- self.pub_joint1_position.publish(np.random.uniform(-0.1, 0.1))
- self.pub_joint2_position.publish(np.random.uniform(-0.1, 0.1))
- self.pub_joint3_position.publish(np.random.uniform(-0.1, 0.1))
- self.pub_joint4_position.publish(np.random.uniform(-0.1, 0.1))
- self._load_target_block()
-
- def init_robot_pose(self):
- self.pub_gripper_position.publish(np.random.uniform(0.0, 0.1))
- self.pub_joint1_position.publish(np.random.uniform(-0.1, 0.1))
- self.pub_joint2_position.publish(np.random.uniform(-0.1, 0.1))
- self.pub_joint3_position.publish(np.random.uniform(-0.1, 0.1))
- self.pub_joint4_position.publish(np.random.uniform(-0.1, 0.1))
- self._load_target_block()
-
- def _delete_target_block(self):
- # This will be called on ROS Exit, deleting Gazebo models
- # Do not wait for the Gazebo Delete Model service, since
- # Gazebo should already be running. If the service is not
- # available since Gazebo has been killed, it is fine to error out
- try:
- delete_model = rospy.ServiceProxy("/gazebo/delete_model", DeleteModel)
- resp_delete = delete_model("block")
- except rospy.ServiceException as e:
- rospy.loginfo("Delete Model service call failed: {0}".format(e))
-
- def _load_target_block(self,
- block_pose=Pose(position=Point(x=0.6725, y=0.1265, z=0.7825)),
- block_reference_frame="world",
- ):
- # Get Models' Path
- model_path = rospkg.RosPack().get_path("kair_algorithms") + "/urdf/"
-
- # Load Block URDF
- block_xml = ""
-
- with open(model_path + "block/model.urdf", "r") as block_file:
- block_xml = block_file.read().replace("\n", "")
-
- # Spawn Block URDF
- rospy.wait_for_service("/gazebo/spawn_urdf_model")
- try:
- spawn_urdf = rospy.ServiceProxy("/gazebo/spawn_urdf_model", SpawnModel)
- resp_urdf = spawn_urdf(
- "block", block_xml, "/", block_pose, block_reference_frame
- )
- except rospy.ServiceException as e:
- rospy.logerr("Spawn URDF service call failed: {0}".format(e))
-
- def _geom_interpolation(self, in_rad, out_rad, in_z, out_z, query):
- """interpolates along the outer shell of work space, based on z-position.
- must feed the corresponding radius from inner radius.
- """
- slope = (out_z - in_z)/(out_rad - in_rad)
- intercept = in_z
- return slope*(query - in_rad) + intercept
-
-
- def _check_for_termination(self):
- """
- Check if the agent has reached undesirable state. If so, terminate the episode early.
- based on the polar coordinate
- """
- _ee_pose = self.get_gripper_position()
- # define gemetry
- inner_rad = 0.134
- outer_rad = 0.3
- lower_rad = 0.384
- inner_z = 0.321
- outer_z = 0.250
- lower_z = 0.116
- rob_rad = np.linalg.norm([_ee_pose[0], _ee_pose[1]])
- rob_z = _ee_pose[2]
- if self.joint_positions[0] <= abs(joint_limits['hi']['j1']/2):
- if rob_rad < inner_rad:
- self.termination_count += 1
- rospy.logwarn('OUT OF BOUNDARY : exceeds inner radius limit')
- elif inner_rad <= rob_rad < outer_rad:
- upper_z = self._geom_interpolation(inner_rad, outer_rad, inner_z, outer_z, rob_rad)
- if rob_z > upper_z:
- self.termination_count += 1
- rospy.logwarn('OUT OF BOUNDARY : exceeds upper z limit')
- elif outer_rad <= rob_rad < lower_rad:
- bevel_z = self._geom_interpolation(outer_rad, lower_rad, outer_z, lower_z, rob_rad)
- if rob_z > bevel_z:
- self.termination_count += 1
- rospy.logwarn('OUT OF BOUNDARY : exceeds bevel z limit')
- else:
- self.termination_count += 1
- rospy.logwarn('OUT OF BOUNDARY : exceeds outer radius limit')
- else: # joint_1 limit exceeds
- self.termination_count += 1
- rospy.logwarn('OUT OF BOUNDARY : joint_1_limit exceeds')
-
- if self.termination_count == TERM_COUNT:
- self.done = True
- self.termination_count = 0
- return True
- else:
- return False
-
- def _check_for_success(self):
- """
- Check if the agent has succeeded the episode.
- """
- _dist = self._get_dist()
- if _dist < self.distance_threshold:
- self.success_count += 1
- if self.success_count == SUC_COUNT:
- self.done = True
- self.success_count = 0
- return True
- else:
- return False
-
- def _compute_reward(self):
- """Computes shaped/sparse reward for each episode.
- """
- cur_dist = self._get_dist()
- if self.reward_type == "sparse":
- return (cur_dist <= self.distance_threshold).astype(
- np.float32
- ) # 1 for success else 0
- else:
- return -cur_dist - self.squared_sum_vel # -L2 distance -l2_norm(joint_vels)
-
- def _get_dist(self):
- rospy.wait_for_service("/gazebo/get_model_state")
- try:
- object_state_srv = rospy.ServiceProxy(
- "/gazebo/get_model_state", GetModelState
- )
- object_state = object_state_srv("block", "world")
- self._obj_pose = np.array(
- [
- object_state.pose.position.x,
- object_state.pose.position.y,
- object_state.pose.position.z,
- ]
- )
- except rospy.ServiceException as e:
- rospy.logerr("Spawn URDF service call failed: {0}".format(e))
- _ee_pose = np.array(self.get_gripper_position()) # FK state of robot
- return np.linalg.norm(_ee_pose - self._obj_pose)
-
- def close(self):
- rospy.signal_shutdown("done")
diff --git a/scripts/envs/open_manipulator/open_manipulator_reacher_env.py b/scripts/envs/open_manipulator/open_manipulator_reacher_env.py
new file mode 100755
index 0000000..933ff66
--- /dev/null
+++ b/scripts/envs/open_manipulator/open_manipulator_reacher_env.py
@@ -0,0 +1,134 @@
+#! usr/bin/env python
+
+import gym
+import numpy as np
+from gym.utils import seeding
+
+from ros_interface import (
+ OpenManipulatorRosGazeboInterface,
+ OpenManipulatorRosRealInterface,
+)
+
+
+class OpenManipulatorReacherEnv(gym.Env):
+ # TODO: write docstring
+ """Open Manipulator Reacher environment on gym.
+
+ Attributes:
+ cfg (dict): environment config
+ env_mode (str): select mode (sim, real)
+ _max_episode_steps (int): max steps per episodes
+ reward_rescale_ratio (float):
+ reward_func (str): function name for calculating reward
+
+ """
+ # TODO: cfg or config
+ def __init__(self, cfg):
+ """Initialization.
+
+ Args:
+ cfg (dict): environment config
+
+ """
+ self.cfg = cfg
+ self.env_name = self.cfg["ENV_NAME"]
+ self.env_mode = self.cfg["ENV_MODE"]
+ self._max_episode_steps = self.cfg["MAX_EPISODE_STEPS"]
+ self.reward_rescale_ratio = self.cfg["REWARD_RESCALE_RATIO"]
+ self.reward_func = self.cfg["REWARD_FUNC"]
+
+ assert self.env_mode in ["sim", "real"]
+ if self.env_mode == "sim":
+ self.ros_interface = OpenManipulatorRosGazeboInterface(self.cfg)
+ else:
+ self.ros_interface = OpenManipulatorRosRealInterface()
+
+ self.episode_steps = 0
+ self.done = False
+ self.reward = 0
+
+ self.action_space = self.ros_interface.get_action_space()
+ self.observation_space = self.ros_interface.get_observation_space()
+ self.seed()
+
+ def seed(self, seed=None):
+ """Set random seed."""
+ self.np_random, seed = seeding.np_random(seed)
+ return [seed]
+
+ def step(self, action):
+ """Function executed each time step.
+
+ Here we get the action execute it in a time step and retrieve the
+ observations generated by that action.
+
+ Args:
+ action: action
+ Returns:
+ Tuple of obs, reward_rescale * reward, done
+ """
+ if action is None:
+ action = np.array([1, 1, 1, 1, 1, 1])
+
+ self.done = False
+
+ if self.episode_steps == self._max_episode_steps:
+ self.done = True
+ self.episode_steps = 0
+
+ act = action.flatten().tolist()
+ self.ros_interface.set_joints_position(act)
+
+ if self.env_mode == "sim":
+ self.reward = self.compute_reward()
+ if self.ros_interface.check_for_termination():
+ print ("Terminates current Episode : OUT OF BOUNDARY")
+ elif self.ros_interface.check_for_success():
+ print ("Succeeded current Episode")
+ obs = self.ros_interface.get_observation()
+
+ self.episode_steps += 1
+
+ return obs, self.reward_rescale_ratio * self.reward, self.done, None
+
+ def reset(self):
+ """Attempt to reset the simulator.
+
+ Since we randomize initial conditions, it is possible to get into
+ a state with numerical issues (e.g. due to penetration or
+ Gimbel lock) or we may not achieve an initial condition (e.g. an
+ object is within the hand).
+
+ In this case, we just keep randomizing until we eventually achieve
+ a valid initial
+ configuration.
+
+ Returns:
+ obs (array) : Array of joint position, joint velocity, joint effort
+ """
+ self.ros_interface.reset_gazebo_world()
+ obs = self.ros_interface.get_observation()
+
+ return obs
+
+ def compute_reward(self):
+ """Computes shaped/sparse reward for each episode.
+
+ Returns:
+ reward (Float64) : L2 distance of current distance and squared sum velocity.
+ """
+ cur_dist = self.ros_interface.get_dist()
+ if self.reward_func == "sparse":
+ # 1 for success else 0
+ reward = cur_dist <= self.ros_interface.distance_threshold
+ reward = reward.astype(np.float32)
+ elif self.reward_func == "l2":
+ # - L2 distance
+ reward = -cur_dist
+ else:
+ raise ValueError
+
+ return reward
+
+ def render(self):
+ pass
diff --git a/scripts/envs/open_manipulator/ros_interface.py b/scripts/envs/open_manipulator/ros_interface.py
new file mode 100755
index 0000000..2ca66a7
--- /dev/null
+++ b/scripts/envs/open_manipulator/ros_interface.py
@@ -0,0 +1,458 @@
+# ! usr/bin/env python
+
+from abc import ABCMeta
+from math import cos, sin
+
+import gym
+import numpy as np
+import rospkg # noqa
+
+import rospy # noqa
+from gazebo_msgs.srv import DeleteModel, GetModelState, SpawnModel
+from geometry_msgs.msg import Pose
+from open_manipulator_msgs.msg import KinematicsPose, OpenManipulatorState
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Float64
+
+
+class OpenManipulatorRosBaseInterface(object):
+ """Open Manipulator Interface based on ROS."""
+
+ __metaclass__ = ABCMeta
+
+ def __init__(self, cfg):
+ """Initialization."""
+ self.cfg = cfg
+ self.train_mode = self.cfg["TRAIN_MODE"]
+
+ self.joint_speeds = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+ self.joint_positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+ self.joint_velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+ self.joint_efforts = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+ self.right_endpoint_position = [0, 0, 0]
+
+ self.termination_count = 0
+ self.success_count = 0
+
+ self.init_publish_node()
+ self.init_subscribe_node()
+ self.init_robot_pose()
+
+ rospy.on_shutdown(self.delete_target_block)
+
+ def init_publish_node(self):
+ # TODO: write docstring
+
+ self.pub_gripper_position = rospy.Publisher(
+ "/open_manipulator/gripper_position/command", Float64, queue_size=1
+ )
+ self.pub_gripper_sub_position = rospy.Publisher(
+ "/open_manipulator/gripper_sub_position/command", Float64, queue_size=1
+ )
+ self.pub_joint1_position = rospy.Publisher(
+ "/open_manipulator/joint1_position/command", Float64, queue_size=1
+ )
+ self.pub_joint2_position = rospy.Publisher(
+ "/open_manipulator/joint2_position/command", Float64, queue_size=1
+ )
+ self.pub_joint3_position = rospy.Publisher(
+ "/open_manipulator/joint3_position/command", Float64, queue_size=1
+ )
+ self.pub_joint4_position = rospy.Publisher(
+ "/open_manipulator/joint4_position/command", Float64, queue_size=1
+ )
+
+ self.joints_position_cmd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+ self.kinematics_cmd = [0.0, 0.0, 0.0]
+
+ def init_subscribe_node(self):
+ # TODO: write docstring
+
+ self.sub_joint_state = rospy.Subscriber(
+ "/open_manipulator/joint_states", JointState, self.joint_state_callback
+ )
+ self.sub_kinematics_pose = rospy.Subscriber(
+ "/open_manipulator/gripper/kinematics_pose",
+ KinematicsPose,
+ self.kinematics_pose_callback,
+ )
+ self.sub_robot_state = rospy.Subscriber(
+ "/open_manipulator/states", OpenManipulatorState, self.robot_state_callback
+ )
+
+ self.joint_names = [
+ "gripper",
+ "gripper_sub",
+ "joint1",
+ "joint2",
+ "joint3",
+ "joint4",
+ ]
+ self.joint_positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+ self.joint_velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+ self.joint_efforts = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+
+ self._gripper_position = [0.0, 0.0, 0.0]
+ self._gripper_orientation = [0.0, 0.0, 0.0]
+ self.distance_threshold = self.cfg["DISTANCE_THRESHOLD"]
+
+ self.moving_state = ""
+ self.actuator_state = ""
+
+ def init_robot_pose(self):
+ """Initialize robot gripper and joints position."""
+ self.pub_gripper_position.publish(np.random.uniform(0.0, 0.1))
+ self.pub_joint1_position.publish(np.random.uniform(-0.1, 0.1))
+ self.pub_joint2_position.publish(np.random.uniform(-0.1, 0.1))
+ self.pub_joint3_position.publish(np.random.uniform(-0.1, 0.1))
+ self.pub_joint4_position.publish(np.random.uniform(-0.1, 0.1))
+
+ def joint_state_callback(self, msg):
+ """Callback function of joint states subscriber.
+
+ Args:
+ msg (JointState): Callback message contains joint state.
+ """
+ joints_states = msg
+ self.joint_names = joints_states.name
+ self.joint_positions = joints_states.position
+ self.joint_velocities = joints_states.velocity
+ self.joint_efforts = joints_states.effort
+ # penalize jerky motion in reward for shaped reward setting.
+ self.squared_sum_vel = np.linalg.norm(np.array(self.joint_velocities))
+
+ def kinematics_pose_callback(self, msg):
+ """Callback function of gripper kinematic pose subscriber.
+
+ Args:
+ msg (KinematicsPose): Callback message contains kinematics pose.
+ """
+ self.kinematics_pose = msg
+ _gripper_position = self.kinematics_pose.pose.position
+ self._gripper_position = [
+ _gripper_position.x,
+ _gripper_position.y,
+ _gripper_position.z,
+ ]
+ _gripper_orientation = self.kinematics_pose.pose.orientation
+ self._gripper_orientation = [
+ _gripper_orientation.x,
+ _gripper_orientation.y,
+ _gripper_orientation.z,
+ _gripper_orientation.w,
+ ]
+
+ def robot_state_callback(self, msg):
+ """Callback function of robot state subscriber.
+
+ Args:
+ msg (states): Callback message contains openmanipulator's states.
+ """
+ # "MOVING" / "STOPPED"
+ self.moving_state = msg.open_manipulator_moving_state
+ # "ACTUATOR_ENABLE" / "ACTUATOR_DISABLE"
+ self.actuator_state = msg.open_manipulator_actuator_state
+
+ def check_robot_moving(self):
+ """Check if robot has reached its initial pose.
+
+ Returns:
+ True if not stopped.
+ """
+ while not rospy.is_shutdown():
+ if self.moving_state == "STOPPED":
+ break
+ return True
+
+ @property
+ def joints_states(self):
+ """Returns current joints states of robot including position, velocity, effort.
+
+ Returns:
+ Tuple of JointState
+ """
+ return self.joint_positions, self.joint_velocities, self.joint_efforts
+
+ @property
+ def gripper_position(self):
+ """Returns gripper end effector position.
+
+ Returns:
+ Position
+ """
+ return self._gripper_position
+
+ @property
+ def gripper_orientation(self):
+ """Returns gripper orientation.
+
+ Returns:
+ Orientation
+ """
+ return self._gripper_orientation
+
+ def get_observation(self):
+ """Get robot observation."""
+ gripper_pos = np.array(self._gripper_position)
+ gripper_ori = np.array(self._gripper_orientation)
+
+ # joint space
+ robot_joint_angles = np.array(self.joint_positions)
+ robot_joint_velocities = np.array(self.joint_velocities)
+ robot_joint_efforts = np.array(self.joint_efforts)
+
+ obs = np.concatenate(
+ (
+ gripper_pos,
+ gripper_ori,
+ robot_joint_angles,
+ robot_joint_velocities,
+ robot_joint_efforts,
+ )
+ )
+ return obs
+
+ def get_action_space(self):
+ """Return the open manipulator's action space for this specific environment."""
+ control_mode = self.cfg["CONTROL_MODE"]
+
+ if control_mode == "position":
+ joint_limits = self.cfg["JOINT_LIMITS"]
+
+ lower_bounds = np.array(
+ [
+ joint_limits["LOW"]["J1"],
+ joint_limits["LOW"]["J2"],
+ joint_limits["LOW"]["J3"],
+ joint_limits["LOW"]["J4"],
+ joint_limits["LOW"]["GRIP"],
+ ]
+ )
+ upper_bounds = np.array(
+ [
+ joint_limits["HIGH"]["J1"],
+ joint_limits["HIGH"]["J2"],
+ joint_limits["HIGH"]["J3"],
+ joint_limits["HIGH"]["J4"],
+ joint_limits["HIGH"]["GRIP"],
+ ]
+ )
+ elif control_mode == "velocity":
+ raise NotImplementedError(
+ "Control mode %s is not implemented yet." % control_mode
+ )
+
+ elif control_mode == "effort":
+ raise NotImplementedError(
+ "Control mode %s is not implemented yet." % control_mode
+ )
+ else:
+ raise ValueError("Control mode %s is not known!" % control_mode)
+ print (lower_bounds, upper_bounds, self.cfg["ACTION_DIM"])
+ return gym.spaces.Box(low=lower_bounds, high=upper_bounds, dtype=np.float32)
+
+ def get_observation_space(self):
+ """Return the open manipulator's state space for this specific environment."""
+ return gym.spaces.Box(
+ low=-np.inf,
+ high=np.inf,
+ shape=self.cfg["OBSERVATION_DIM"],
+ dtype=np.float32,
+ )
+
+ def set_joints_position(self, joints_angles):
+ """Move joints using joint position command publishers."""
+ self.pub_gripper_position.publish(joints_angles[0])
+ self.pub_joint1_position.publish(joints_angles[1])
+ self.pub_joint2_position.publish(joints_angles[2])
+ self.pub_joint3_position.publish(joints_angles[3])
+ self.pub_joint4_position.publish(joints_angles[4])
+
+ def _geom_interpolation(self, in_rad, out_rad, in_z, out_z, query):
+ """interpolates along the outer shell of work space, based on z-position.
+
+ must feed the corresponding radius from inner radius.
+ """
+ slope = (out_z - in_z) / (out_rad - in_rad)
+ intercept = in_z
+ return slope * (query - in_rad) + intercept
+
+ def check_for_success(self):
+ """Check if the agent has succeeded the episode.
+
+ Returns:
+ True when count reaches suc_count, else False.
+ """
+ dist = self.get_dist()
+ if dist < self.distance_threshold:
+ self.success_count += 1
+ if self.success_count == self.cfg["SUCCESS_COUNT"]:
+ self.done = True
+ self.success_count = 0
+ return True
+ else:
+ return False
+ else:
+ return False
+
+ def check_for_termination(self):
+ """Check if the agent has reached undesirable state.
+
+ If so, terminate the episode early.
+
+ Returns:
+ True when count reaches term_count, else False.
+ """
+ _ee_pose = self._gripper_position
+
+ inner_rad, outer_rad, lower_rad, inner_z, outer_z, lower_z, term_count = (
+ self.cfg["INNER_RADIAN"],
+ self.cfg["OUTER_RADIAN"],
+ self.cfg["LOWER_RADIAN"],
+ self.cfg["INNER_Z"],
+ self.cfg["OUTER_Z"],
+ self.cfg["LOWER_Z"],
+ self.cfg["TERM_COUNT"],
+ )
+
+ rob_rad = np.linalg.norm([_ee_pose[0], _ee_pose[1]])
+ rob_z = _ee_pose[2]
+ if self.joint_positions[0] <= abs(self.cfg["JOINT_LIMITS"]["HIGH"]["J1"] / 2):
+ if rob_rad < self.cfg["INNER_RADIAN"]:
+ self.termination_count += 1
+ rospy.logwarn("OUT OF BOUNDARY : exceeds inner radius limit")
+ elif self.cfg["INNER_RADIAN"] <= rob_rad < self.cfg["OUTER_RADIAN"]:
+ upper_z = self._geom_interpolation(
+ inner_rad, outer_rad, inner_z, outer_z, rob_rad
+ )
+ if rob_z > upper_z:
+ self.termination_count += 1
+ rospy.logwarn("OUT OF BOUNDARY : exceeds upper z limit")
+ elif outer_rad <= rob_rad < lower_rad:
+ bevel_z = self._geom_interpolation(
+ outer_rad, lower_rad, outer_z, lower_z, rob_rad
+ )
+ if rob_z > bevel_z:
+ self.termination_count += 1
+ rospy.logwarn("OUT OF BOUNDARY : exceeds bevel z limit")
+ else:
+ self.termination_count += 1
+ rospy.logwarn("OUT OF BOUNDARY : exceeds outer radius limit")
+ else:
+ # joint_1 limit exceeds
+ self.termination_count += 1
+ rospy.logwarn("OUT OF BOUNDARY : joint_1_limit exceeds")
+
+ if self.termination_count == term_count:
+ self.done = True
+ self.termination_count = 0
+ return True
+ else:
+ return False
+
+ def close(self):
+ """Close by rospy shutdown."""
+ rospy.signal_shutdown("done")
+
+
+class OpenManipulatorRosGazeboInterface(OpenManipulatorRosBaseInterface):
+ # TODO: write docstring
+ """Open Manipulator Interface based on ROS for Gazebo."""
+
+ def __init__(self, cfg):
+ rospy.init_node("OpenManipulatorRosGazeboInterface")
+ super(OpenManipulatorRosGazeboInterface, self).__init__(cfg)
+
+ def reset_gazebo_world(self, block_pose=None):
+ """Initialize randomly the state of robot agent and surrounding envs (including target obj.)."""
+ if block_pose is not None:
+ assert self.train_mode is True
+
+ self.delete_target_block()
+
+ self.pub_gripper_position.publish(np.random.uniform(0.0, 0.1))
+ self.pub_joint1_position.publish(np.random.uniform(-0.1, 0.1))
+ self.pub_joint2_position.publish(np.random.uniform(-0.1, 0.1))
+ self.pub_joint3_position.publish(np.random.uniform(-0.1, 0.1))
+ self.pub_joint4_position.publish(np.random.uniform(-0.1, 0.1))
+
+ self.set_target_block(block_pose)
+
+ def set_target_block(self, block_pose=None):
+ """Set target block Gazebo model"""
+ # random generated blocks for train
+ if block_pose is None:
+ polar_rad, polar_theta, overhead_orientation = (
+ np.random.uniform(*self.cfg["POLAR_RADIAN_BOUNDARY"]),
+ np.random.uniform(*self.cfg["POLAR_THETA_BOUNDARY"]),
+ self.cfg["OVERHEAD_ORIENTATION"],
+ )
+
+ block_pose = Pose()
+ block_pose.position.x = polar_rad * cos(polar_theta)
+ block_pose.position.y = polar_rad * sin(polar_theta)
+ block_pose.position.z = np.random.uniform(0.05, 0.28)
+ block_pose.orientation = overhead_orientation
+
+ block_reference_frame = "world"
+ model_path = rospkg.RosPack().get_path("kair_algorithms") + "/urdf/"
+
+ with open(model_path + "block/model.urdf", "r") as block_file:
+ block_xml = block_file.read().replace("\n", "")
+
+ rospy.wait_for_service("/gazebo/spawn_urdf_model")
+
+ try:
+ spawn_urdf = rospy.ServiceProxy("/gazebo/spawn_urdf_model", SpawnModel)
+ spawn_urdf("block", block_xml, "/", block_pose, block_reference_frame)
+ except rospy.ServiceException as e:
+ rospy.logerr("Spawn URDF service call failed: {0}".format(e))
+
+ def delete_target_block(self):
+ """This will be called on ROS Exit, deleting Gazebo models.
+
+ Do not wait for the Gazebo Delete Model service, since
+ Gazebo should already be running. If the service is not
+ available since Gazebo has been killed, it is fine to error out
+ """
+ try:
+ delete_model = rospy.ServiceProxy("/gazebo/delete_model", DeleteModel)
+ delete_model("block")
+ except rospy.ServiceException as e:
+ rospy.loginfo("Delete Model service call failed: {0}".format(e))
+
+ def get_dist(self):
+ """Get distance between end effector pose and object pose.
+
+ Returns:
+ L2 norm of end effector pose and object pose.
+ """
+ rospy.wait_for_service("/gazebo/get_model_state")
+
+ try:
+ object_state_srv = rospy.ServiceProxy(
+ "/gazebo/get_model_state", GetModelState
+ )
+ object_state = object_state_srv("block", "world")
+ object_pose = [
+ object_state.pose.position.x,
+ object_state.pose.position.y,
+ object_state.pose.position.z,
+ ]
+ self._obj_pose = np.array(object_pose)
+ except rospy.ServiceException as e:
+ rospy.logerr("Spawn URDF service call failed: {0}".format(e))
+
+ # FK state of robot
+ end_effector_pose = np.array(self._gripper_position)
+
+ return np.linalg.norm(end_effector_pose - self._obj_pose)
+
+
+class OpenManipulatorRosRealInterface(OpenManipulatorRosBaseInterface):
+ # TODO: write docstring
+ """Open Manipulator Interface based on ROS for real environment."""
+
+ def __init__(self, cfg):
+ rospy.init_node("OpenManipulatorRosRealInterface")
+ super(OpenManipulatorRosRealInterface, self).__init__(cfg)
diff --git a/scripts/gazebo_test_open_manipulator.py b/scripts/gazebo_test_open_manipulator.py
index 097ac76..97b3123 100755
--- a/scripts/gazebo_test_open_manipulator.py
+++ b/scripts/gazebo_test_open_manipulator.py
@@ -5,26 +5,26 @@ from math import cos, pi, sin
import numpy as np
import rospy
-from envs.open_manipulator import OpenManipulatorEnv
+from config.environment import open_manipulator
+from envs.open_manipulator import OpenManipulatorReacherEnv
from geometry_msgs.msg import Pose, Quaternion
from open_manipulator_msgs.msg import JointPosition, KinematicsPose
from open_manipulator_msgs.srv import SetJointPosition, SetKinematicsPose
overhead_orientation = Quaternion(
- x=-0.00142460053167,
- y=0.999994209902,
- z=-0.00177030764765,
- w=0.00253311793936)
+ x=-0.00142460053167, y=0.999994209902, z=-0.00177030764765, w=0.00253311793936
+)
+
+cfg = open_manipulator
def test_reset():
- env = OpenManipulatorEnv()
+ env = OpenManipulatorReacherEnv(cfg)
_ = env.reset()
- # assert obs in specific boundary
def test_forward():
- env = OpenManipulatorEnv()
+ env = OpenManipulatorReacherEnv(cfg)
_ = env.reset()
_pose = Pose()
_pose.position.x = 0.4
@@ -40,7 +40,9 @@ def test_forward():
forward_pose.max_velocity_scaling_factor = 0.0
forward_pose.tolerance = 0.0
try:
- task_space_srv = rospy.ServiceProxy('/open_manipulator/goal_task_space_path', SetKinematicsPose)
+ task_space_srv = rospy.ServiceProxy(
+ "/open_manipulator/goal_task_space_path", SetKinematicsPose
+ )
_ = task_space_srv("arm", "gripper", forward_pose, 2.0)
except rospy.ServiceException as e:
rospy.loginfo("Path planning service call failed: {0}".format(e))
@@ -48,12 +50,14 @@ def test_forward():
def test_rotate():
_qpose = JointPosition()
- _qpose.joint_name = ['joint1', 'joint2', 'joint3', 'joint4']
+ _qpose.joint_name = ["joint1", "joint2", "joint3", "joint4"]
_qpose.position = [0.5, 0.0, 0.0, 0.5]
_qpose.max_accelerations_scaling_factor = 0.0
_qpose.max_velocity_scaling_factor = 0.0
try:
- task_space_srv = rospy.ServiceProxy('/open_manipulator/goal_joint_space_path_from_present', SetJointPosition)
+ task_space_srv = rospy.ServiceProxy(
+ "/open_manipulator/goal_joint_space_path_from_present", SetJointPosition
+ )
_ = task_space_srv("arm", _qpose, 2.0)
except rospy.ServiceException, e:
rospy.loginfo("Path planning service call failed: {0}".format(e))
@@ -63,36 +67,33 @@ def test_rotate():
_ = task_space_srv("arm", _qpose, 2.0)
except rospy.ServiceException, e:
rospy.loginfo("Path planning service call failed: {0}".format(e))
- # define actions
- # assert obs in specific boundary
def test_block_loc():
- env = OpenManipulatorEnv()
+ env = OpenManipulatorReacherEnv(cfg)
for iter in range(20):
b_pose = Pose()
- b_pose.position.x = np.random.uniform(0.15, .20)
+ b_pose.position.x = np.random.uniform(0.15, 0.20)
b_pose.position.y = np.random.uniform(-0.2, 0.2)
b_pose.position.z = 0.00
b_pose.orientation = overhead_orientation
- env._load_target_block(block_pose=b_pose)
+ env.ros_interface.set_target_block()
rospy.sleep(2.0)
- env._delete_target_block()
- # block generation code
- # assert block in specific boundary (gripper's movable area)
+ env.ros_interface.delete_target_block()
def test_achieve_goal():
- env = OpenManipulatorEnv()
+ env = OpenManipulatorReacherEnv(cfg)
for iter in range(20):
- b_pose = Pose()
- b_pose.position.x = np.random.uniform(0.25, .6)
- b_pose.position.y = np.random.uniform(-0.4, 0.4)
- b_pose.position.z = 0.00
- b_pose.orientation = overhead_orientation
- env._load_target_block(block_pose=b_pose)
+ block_pose = Pose()
+ block_pose.position.x = np.random.uniform(0.25, 0.6)
+ block_pose.position.y = np.random.uniform(-0.4, 0.4)
+ block_pose.position.z = 0.00
+ block_pose.orientation = overhead_orientation
+ env.ros_interface.set_target_block(block_pose)
+
r_pose = Pose()
- r_pose.position = b_pose.position
+ r_pose.position = block_pose.position
r_pose.position.z = 0.08
forward_pose = KinematicsPose()
forward_pose.pose = r_pose
@@ -100,49 +101,49 @@ def test_achieve_goal():
forward_pose.max_velocity_scaling_factor = 0.0
forward_pose.tolerance = 0.0
try:
- task_space_srv = rospy.ServiceProxy('/open_manipulator/goal_task_space_path', SetKinematicsPose)
+ task_space_srv = rospy.ServiceProxy(
+ "/open_manipulator/goal_task_space_path", SetKinematicsPose
+ )
_ = task_space_srv("arm", "gripper", forward_pose, 2.0)
except rospy.ServiceException, e:
rospy.loginfo("Path planning service call failed: {0}".format(e))
rospy.sleep(5.0)
- env._delete_target_block()
+ env.ros_interface.delete_target_block()
def test_workspace_limit():
- """ TODO: add static block
- """
- env = OpenManipulatorEnv()
+ env = OpenManipulatorReacherEnv(cfg)
for iter in range(100):
_polar_rad = np.random.uniform(0.134, 0.32)
_polar_theta = np.random.uniform(-pi * 0.7 / 4, pi * 0.7 / 4)
- b_pose = Pose()
- b_pose.position.x = _polar_rad * cos(_polar_theta)
- b_pose.position.y = _polar_rad * sin(_polar_theta)
- b_pose.position.z = np.random.uniform(0.05, 0.28)
- b_pose.orientation = overhead_orientation
- env._load_target_block(block_pose=b_pose)
+ block_pose = Pose()
+ block_pose.position.x = _polar_rad * cos(_polar_theta)
+ block_pose.position.y = _polar_rad * sin(_polar_theta)
+ block_pose.position.z = np.random.uniform(0.05, 0.28)
+ block_pose.orientation = overhead_orientation
+ env.ros_interface.set_target_block(block_pose)
+
r_pose = Pose()
- r_pose.position = b_pose.position
+ r_pose.position = block_pose.position
forward_pose = KinematicsPose()
forward_pose.pose = r_pose
forward_pose.max_accelerations_scaling_factor = 0.0
forward_pose.max_velocity_scaling_factor = 0.0
forward_pose.tolerance = 0.0
try:
- task_space_srv = rospy.ServiceProxy('/open_manipulator/goal_task_space_path', SetKinematicsPose)
+ task_space_srv = rospy.ServiceProxy(
+ "/open_manipulator/goal_task_space_path", SetKinematicsPose
+ )
_ = task_space_srv("arm", "gripper", forward_pose, 3.0)
except rospy.ServiceException, e:
rospy.loginfo("Path planning service call failed: {0}".format(e))
rospy.sleep(3.0)
- env._check_for_termination()
- env._delete_target_block()
- # define actions
- # define goal
- # assert gripper reach goal
+ env.ros_interface.check_for_termination()
+ env.ros_interface.delete_target_block()
-if __name__ == '__main__':
+if __name__ == "__main__":
# test_reset()
# test_forward()
# test_rotate()
diff --git a/scripts/run_lunarlander_continuous.py b/scripts/run_lunarlander_continuous.py
index dd76e1b..b3a7e0b 100644
--- a/scripts/run_lunarlander_continuous.py
+++ b/scripts/run_lunarlander_continuous.py
@@ -56,16 +56,19 @@ def main():
"""Main."""
# env initialization
env = gym.make("LunarLanderContinuous-v2")
- state_dim = env.observation_space.shape[0]
- action_dim = env.action_space.shape[0]
-
# set a random seed
common_utils.set_random_seed(args.seed, env)
# run
- module_path = "examples.lunarlander_continuous_v2." + args.algo
- example = importlib.import_module(module_path)
- example.run(env, args, state_dim, action_dim)
+ module_path = "config.agent.lunarlander_continuous_v2." + args.algo
+ agent = importlib.import_module(module_path)
+ agent = agent.get(env, args)
+
+ # run
+ if args.test:
+ agent.test()
+ else:
+ agent.train()
if __name__ == "__main__":
diff --git a/scripts/run_open_manipulator_reacher_v0.py b/scripts/run_open_manipulator_reacher_v0.py
index fe60dc3..d1563ed 100755
--- a/scripts/run_open_manipulator_reacher_v0.py
+++ b/scripts/run_open_manipulator_reacher_v0.py
@@ -11,7 +11,8 @@ import argparse
import importlib
import algorithms.common.helper_functions as common_utils
-from envs.open_manipulator.open_manipulator import OpenManipulatorEnv
+from config.environment.open_manipulator import config as env_cfg
+from envs.open_manipulator.open_manipulator_reacher_env import OpenManipulatorReacherEnv
# configurations
parser = argparse.ArgumentParser(description="Pytorch RL algorithms")
@@ -54,18 +55,21 @@ args = parser.parse_args()
def main():
"""Main."""
# env initialization
- env = OpenManipulatorEnv()
- # env = gym.make("Omreacher-v0")
- # TODO: uncomment here.
- state_dim = env.observation_space.shape[0]
- action_dim = env.action_space.shape[0]
+ env = OpenManipulatorReacherEnv(env_cfg)
+
# set a random seed
- # common_utils.set_random_seed(args.seed, env)
+ common_utils.set_random_seed(args.seed, env)
+
+ # agent initialization
+ module_path = "config.agent.open_manipulator_reacher_v0." + args.algo
+ agent = importlib.import_module(module_path)
+ agent = agent.get(env, args)
# run
- module_path = "examples.open_manipulator_reacher_v0." + args.algo
- example = importlib.import_module(module_path)
- example.run(env, args, state_dim, action_dim)
+ if args.test:
+ agent.test()
+ else:
+ agent.train()
if __name__ == "__main__":
diff --git a/scripts/run_reacher_v1.py b/scripts/run_reacher_v1.py
index caa8641..fcbfe56 100644
--- a/scripts/run_reacher_v1.py
+++ b/scripts/run_reacher_v1.py
@@ -54,16 +54,20 @@ def main():
"""Main."""
# env initialization
env = gym.make("Reacher-v1")
- state_dim = env.observation_space.shape[0]
- action_dim = env.action_space.shape[0]
# set a random seed
common_utils.set_random_seed(args.seed, env)
+ # agent initialization
+ module_path = "config.agent.reacher-v1." + args.algo
+ agent = importlib.import_module(module_path)
+ agent = agent.get(env, args)
+
# run
- module_path = "examples.reacher-v1." + args.algo
- example = importlib.import_module(module_path)
- example.run(env, args, state_dim, action_dim)
+ if args.test:
+ agent.test()
+ else:
+ agent.train()
if __name__ == "__main__":