mirror of
https://github.com/wassname/kair_algorithms_draft.git
synced 2026-09-09 11:25:10 +08:00
Refactor OpenManipulator env class (#46)
* Merge subin branch Squashed commit of the following: commit 98112b8c05f955b1eb49a6b78023cad0979d5f95 Author: Subin Yang <ysb8049@naver.com> Date: Sat Mar 30 18:19:46 2019 +0900 Remove noqa commit f45571a80afd403c8ec56db8a2fb5cbedf288db7 Author: Subin Yang <ysb8049@naver.com> Date: Sat Mar 30 17:50:39 2019 +0900 Resolve flake8 commit 058d85bc4ed09441d27065e6d304bfb946942a98 Author: Subin Yang <ysb8049@naver.com> Date: Sat Mar 30 17:41:35 2019 +0900 Modify structures of ros interface and reacher env commit ae4c859ffa6b008823050f310bdccbee6a1de30a Author: Subin Yang <ysb8049@naver.com> Date: Sat Mar 30 17:28:15 2019 +0900 Resolve flake8 commit 4c74ec6527b52d75882ddbe1b4f518f9c252a25c Author: Subin Yang <ysb8049@naver.com> Date: Sat Mar 30 17:23:30 2019 +0900 Resolve flake8 commit 243b2f3739b4388d814a886d5cf1b85a05bb526a Author: Subin Yang <ysb8049@naver.com> Date: Sat Mar 30 17:18:13 2019 +0900 Add open manipulator environment * Refactor openmanipulator environment class * Refactored env structure * Fix errors * Fix error * Add open_manipulator launch file * Fix errors * fix error * fix error * fix error * fix error * fix error * fix error * fix error * Fix typo * Delete unused script * Change reward * Fix typo, add env name to config * Change demo file compatible to python2 (#40) * Change demo file to python2 compatible * Add object to classes for compatibility with python2 * Refactoring config, envs and ros interface (#48) * Refactoring config architecture * Replace network hyper params on agent config * Modify env class and ros interface class * Modify getter and setter on ros interface * Modify wrong code * Fix typo * Add env config * Final environment class and test scripts before the test (#43) * new user branch * Resolve formatting issues on test scripts * Resolve formatting issues on test scripts * Merge subin branch Squashed commit of the following: commit 98112b8c05f955b1eb49a6b78023cad0979d5f95 Author: Subin Yang <ysb8049@naver.com> Date: Sat Mar 30 18:19:46 2019 +0900 Remove noqa commit f45571a80afd403c8ec56db8a2fb5cbedf288db7 Author: Subin Yang <ysb8049@naver.com> Date: Sat Mar 30 17:50:39 2019 +0900 Resolve flake8 commit 058d85bc4ed09441d27065e6d304bfb946942a98 Author: Subin Yang <ysb8049@naver.com> Date: Sat Mar 30 17:41:35 2019 +0900 Modify structures of ros interface and reacher env commit ae4c859ffa6b008823050f310bdccbee6a1de30a Author: Subin Yang <ysb8049@naver.com> Date: Sat Mar 30 17:28:15 2019 +0900 Resolve flake8 commit 4c74ec6527b52d75882ddbe1b4f518f9c252a25c Author: Subin Yang <ysb8049@naver.com> Date: Sat Mar 30 17:23:30 2019 +0900 Resolve flake8 commit 243b2f3739b4388d814a886d5cf1b85a05bb526a Author: Subin Yang <ysb8049@naver.com> Date: Sat Mar 30 17:18:13 2019 +0900 Add open manipulator environment * Refactor openmanipulator environment class * Test the training loop with td3 baseline * Add one-shot launch file for gazebo initialization * Refactored env structure * Fix errors * Fix error * Fix errors * fix error * fix error * fix error * fix error * fix error * fix error * fix error * Fix typo * Delete unused script * Change reward * Fix typo, add env name to config * Refactoring config, envs and ros interface (#48) * Refactoring config architecture * Replace network hyper params on agent config * Modify env class and ros interface class * Modify getter and setter on ros interface * Modify wrong code * Fix typo * Add env config * Resolve flake8, typo issue * Resolve conflict during pull remote
This commit is contained in:
Whi Kwon
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -47,6 +47,4 @@
|
||||
|
||||
<!-- ros_control robotis manipulator launch file -->
|
||||
<include file="$(find open_manipulator_gazebo)/launch/open_manipulator_controller.launch"/>
|
||||
|
||||
|
||||
</launch>
|
||||
|
||||
Executable → Regular
+13
-13
@@ -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)
|
||||
+13
-13
@@ -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)
|
||||
+8
-12
@@ -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)
|
||||
+8
-12
@@ -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)
|
||||
Regular → Executable
Regular → Executable
+8
-12
@@ -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)
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
Executable → Regular
+9
-12
@@ -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)
|
||||
@@ -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)
|
||||
+52
@@ -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
|
||||
@@ -0,0 +1,3 @@
|
||||
from .open_manipulator import OpenManipulatorReacherEnv
|
||||
|
||||
__all__ = ["OpenManipulatorReacherEnv"]
|
||||
@@ -0,0 +1,3 @@
|
||||
from .open_manipulator_reacher_env import OpenManipulatorReacherEnv
|
||||
|
||||
__all__ = ["OpenManipulatorReacherEnv"]
|
||||
@@ -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 }}
|
||||
|
||||
# <limit velocity="4.8" effort="1" lower="-0.010" upper="0.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")
|
||||
@@ -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
|
||||
+458
@@ -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)
|
||||
@@ -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()
|
||||
|
||||
@@ -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__":
|
||||
|
||||
@@ -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__":
|
||||
|
||||
@@ -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__":
|
||||
|
||||
Reference in New Issue
Block a user