From c1f348775e41c71d0de1bc6699f7a1732ee0bed4 Mon Sep 17 00:00:00 2001 From: Whi Kwon <30768596+whikwon@users.noreply.github.com> Date: Fri, 3 May 2019 21:25:15 +0900 Subject: [PATCH] Add openmanipulator simulation environment agent (#50) --- Makefile | 4 +- README.md | 8 +- docker_train.sh | 5 +- launch/open_manipulator_env.launch | 50 ++ launch/vel_kinematics.launch | 13 - launch/yumi_gazebo_vel.launch | 33 -- scripts/algorithms/common/abstract/agent.py | 2 +- scripts/{examples => config}/__init__.py | 0 .../agent}/__init__.py | 0 .../lunarlander_continuous_v2}/__init__.py | 0 .../agent}/lunarlander_continuous_v2/sac.py | 26 +- .../agent}/lunarlander_continuous_v2/sacfd.py | 26 +- .../agent}/lunarlander_continuous_v2/td3.py | 20 +- .../agent}/lunarlander_continuous_v2/td3fd.py | 20 +- .../open_manipulator_reacher_v0/__init__.py | 0 .../agent/open_manipulator_reacher_v0}/td3.py | 20 +- scripts/config/agent/reacher-v1/__init__.py | 0 .../agent}/reacher-v1/sac.py | 26 +- .../agent}/reacher-v1/sacfd.py | 26 +- scripts/config/agent/reacher-v1/td3.py | 128 +++++ .../agent}/reacher-v1/td3fd.py | 20 +- scripts/config/environment/__init__.py | 0 .../config/environment/open_manipulator.py | 54 ++ scripts/envs/__init__.py | 3 + scripts/envs/open_manipulator/__init__.py | 3 + .../open_manipulator_reacher_env.py | 132 +++++ .../envs/open_manipulator/ros_interface.py | 471 ++++++++++++++++++ scripts/gazebo_test_open_manipulator.py | 150 ++++++ scripts/run_lunarlander_continuous.py | 15 +- scripts/run_open_manipulator_reacher_v0.py | 76 +++ scripts/run_reacher_v1.py | 14 +- urdf/block/model.urdf | 15 +- 32 files changed, 1187 insertions(+), 173 deletions(-) create mode 100755 launch/open_manipulator_env.launch delete mode 100644 launch/vel_kinematics.launch delete mode 100644 launch/yumi_gazebo_vel.launch mode change 100644 => 100755 scripts/algorithms/common/abstract/agent.py rename scripts/{examples => config}/__init__.py (100%) rename scripts/{examples/lunarlander_continuous_v2 => config/agent}/__init__.py (100%) rename scripts/{examples/reacher-v1 => config/agent/lunarlander_continuous_v2}/__init__.py (100%) rename scripts/{examples => config/agent}/lunarlander_continuous_v2/sac.py (83%) rename scripts/{examples => config/agent}/lunarlander_continuous_v2/sacfd.py (84%) rename scripts/{examples => config/agent}/lunarlander_continuous_v2/td3.py (88%) rename scripts/{examples => config/agent}/lunarlander_continuous_v2/td3fd.py (89%) create mode 100755 scripts/config/agent/open_manipulator_reacher_v0/__init__.py rename scripts/{examples/reacher-v1 => config/agent/open_manipulator_reacher_v0}/td3.py (88%) mode change 100644 => 100755 create mode 100644 scripts/config/agent/reacher-v1/__init__.py rename scripts/{examples => config/agent}/reacher-v1/sac.py (83%) rename scripts/{examples => config/agent}/reacher-v1/sacfd.py (84%) create mode 100644 scripts/config/agent/reacher-v1/td3.py rename scripts/{examples => config/agent}/reacher-v1/td3fd.py (89%) create mode 100644 scripts/config/environment/__init__.py create mode 100755 scripts/config/environment/open_manipulator.py create mode 100644 scripts/envs/__init__.py create mode 100644 scripts/envs/open_manipulator/__init__.py create mode 100755 scripts/envs/open_manipulator/open_manipulator_reacher_env.py create mode 100755 scripts/envs/open_manipulator/ros_interface.py create mode 100755 scripts/gazebo_test_open_manipulator.py create mode 100755 scripts/run_open_manipulator_reacher_v0.py 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/docker_train.sh b/docker_train.sh index 913b3d2..1dae1a7 100644 --- a/docker_train.sh +++ b/docker_train.sh @@ -6,9 +6,10 @@ KAIR=$CATKIN_WS/src/kair_algorithms_draft if [ "$1" == "lunarlander" ]; then cd $KAIR/scripts; \ - python run_lunarlander_continuous.py --algo $2 --off-render + python run_lunarlander_continuous.py --algo $2 --off-render elif [ "$1" == "openmanipulator" ]; then - echo "Working" + cd $KAIR/scripts; \ + /opt/ros/$ROS_DISTRO/bin/rosrun kair_algorithms run_open_manipulator_reacher_v0.py --algo $2 --off-render else echo "Unknown parameter" fi diff --git a/launch/open_manipulator_env.launch b/launch/open_manipulator_env.launch new file mode 100755 index 0000000..038b450 --- /dev/null +++ b/launch/open_manipulator_env.launch @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ["$(arg robot_name)/joint_states"] + + + + + + + + + + + + + + + + + + diff --git a/launch/vel_kinematics.launch b/launch/vel_kinematics.launch deleted file mode 100644 index e9b73ef..0000000 --- a/launch/vel_kinematics.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/launch/yumi_gazebo_vel.launch b/launch/yumi_gazebo_vel.launch deleted file mode 100644 index 42a78dd..0000000 --- a/launch/yumi_gazebo_vel.launch +++ /dev/null @@ -1,33 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/scripts/algorithms/common/abstract/agent.py b/scripts/algorithms/common/abstract/agent.py old mode 100644 new mode 100755 index 9c802c6..02d75f2 --- a/scripts/algorithms/common/abstract/agent.py +++ b/scripts/algorithms/common/abstract/agent.py @@ -44,7 +44,7 @@ class AbstractAgent(object): self.args.max_episode_steps = env._max_episode_steps # for logging - self.env_name = str(self.env.env).split("<")[2].replace(">>", "") + self.env_name = str(self.env.env).split("<")[1].replace(">>", "") self.sha = ( subprocess.check_output(["git", "rev-parse", "--short", "HEAD"])[:-1] .decode("ascii") 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/reacher-v1/__init__.py b/scripts/config/agent/lunarlander_continuous_v2/__init__.py similarity index 100% rename from scripts/examples/reacher-v1/__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/config/agent/open_manipulator_reacher_v0/__init__.py b/scripts/config/agent/open_manipulator_reacher_v0/__init__.py new file mode 100755 index 0000000..e69de29 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/config/agent/reacher-v1/td3.py b/scripts/config/agent/reacher-v1/td3.py new file mode 100644 index 0000000..665b1ed --- /dev/null +++ b/scripts/config/agent/reacher-v1/td3.py @@ -0,0 +1,128 @@ +# -*- coding: utf-8 -*- +"""Run module for TD3 on LunarLanderContinuous-v2. + +- Author: whikwon +- Contact: whikwon@gmail.com +""" + +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 + +device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu") + +# hyper parameters +hyper_params = { + "GAMMA": 0.99, + "TAU": 5e-3, + "BUFFER_SIZE": int(1e6), + "BATCH_SIZE": 100, + "LR_ACTOR": 1e-3, + "LR_CRITIC": 1e-3, + "WEIGHT_DECAY": 0.000, + "EXPLORATION_NOISE": 0.1, + "TARGET_POLICY_NOISE": 0.2, + "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 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 = 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( + input_size=state_dim, + output_size=action_dim, + hidden_sizes=hidden_sizes_actor, + output_activation=torch.tanh, + ).to(device) + + actor_target = MLP( + input_size=state_dim, + output_size=action_dim, + hidden_sizes=hidden_sizes_actor, + output_activation=torch.tanh, + ).to(device) + actor_target.load_state_dict(actor.state_dict()) + + # create critic1 + critic1 = MLP( + input_size=state_dim + action_dim, + output_size=1, + hidden_sizes=hidden_sizes_critic, + ).to(device) + + critic1_target = MLP( + input_size=state_dim + action_dim, + output_size=1, + hidden_sizes=hidden_sizes_critic, + ).to(device) + critic1_target.load_state_dict(critic1.state_dict()) + + # create critic2 + critic2 = MLP( + input_size=state_dim + action_dim, + output_size=1, + hidden_sizes=hidden_sizes_critic, + ).to(device) + + critic2_target = MLP( + input_size=state_dim + action_dim, + output_size=1, + hidden_sizes=hidden_sizes_critic, + ).to(device) + critic2_target.load_state_dict(critic2.state_dict()) + + # concat critic parameters to use one optim + critic_parameters = list(critic1.parameters()) + list(critic2.parameters()) + + # create optimizer + actor_optim = optim.Adam( + actor.parameters(), + lr=hyper_params["LR_ACTOR"], + weight_decay=hyper_params["WEIGHT_DECAY"], + ) + + critic_optim = optim.Adam( + critic_parameters, + lr=hyper_params["LR_CRITIC"], + weight_decay=hyper_params["WEIGHT_DECAY"], + ) + + # noise + exploration_noise = GaussianNoise( + action_dim, + min_sigma=hyper_params["EXPLORATION_NOISE"], + max_sigma=hyper_params["EXPLORATION_NOISE"], + ) + + target_policy_noise = GaussianNoise( + action_dim, + min_sigma=hyper_params["TARGET_POLICY_NOISE"], + max_sigma=hyper_params["TARGET_POLICY_NOISE"], + ) + + # make tuples to create an agent + models = (actor, actor_target, critic1, critic1_target, critic2, critic2_target) + optims = (actor_optim, critic_optim) + noises = (exploration_noise, target_policy_noise) + + # create an agent + 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..2da7afb --- /dev/null +++ b/scripts/config/environment/open_manipulator.py @@ -0,0 +1,54 @@ +from math import pi + +from geometry_msgs.msg import Quaternion + + +config = { + "ENV_NAME": "OpenManipulatorReacher", + "MAX_EPISODE_STEPS": 100, + "TERM_COUNT": 5, + "SUCCESS_COUNT": 5, + "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.019, + }, + "LOW": { + "J1": -pi * 0.9, + "J2": -pi * 0.57, + "J3": -pi * 0.3, + "J4": -pi * 0.57, + "GRIP": -0.001, + }, + }, + # 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, + "DISTANCE_THRESHOLD": 0.05, + "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_reacher_env.py b/scripts/envs/open_manipulator/open_manipulator_reacher_env.py new file mode 100755 index 0000000..1b2c730 --- /dev/null +++ b/scripts/envs/open_manipulator/open_manipulator_reacher_env.py @@ -0,0 +1,132 @@ +#! usr/bin/env python + +import numpy as np + +import gym +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 + """ + self.done = False + self.episode_steps += 1 + + act = action.flatten().tolist() + self.ros_interface.set_joints_position(act) + + if self.env_mode == "sim": + self.reward = self.compute_reward() + # TODO: Add termination condition + # if self.ros_interface.check_for_termination(): + # self.done = True + if self.ros_interface.check_for_success(): + self.done = True + + obs = self.ros_interface.get_observation() + + if self.episode_steps == self._max_episode_steps: + self.done = True + self.episode_steps = 0 + + 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..cd3978f --- /dev/null +++ b/scripts/envs/open_manipulator/ros_interface.py @@ -0,0 +1,471 @@ +# ! usr/bin/env python + +from abc import ABCMeta +from math import cos, sin +import time + +import gym +import numpy as np +import rospkg # noqa + +import rospy # noqa +import tf # noqa +import tf.transformations as tr # 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_tf_transformer() + self.init_publish_node() + self.init_subscribe_node() + self.init_robot_pose() + + rospy.on_shutdown(self.delete_target_block) + + def init_tf_transformer(self): + # TODO: write docstring + + self.tf_listenser = tf.TransformListener() + + 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, 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.0)) + self.pub_joint1_position.publish(np.random.uniform(0.0, 0.0)) + self.pub_joint2_position.publish(np.random.uniform(0.0, 0.0)) + self.pub_joint3_position.publish(np.random.uniform(0.0, 0.0)) + self.pub_joint4_position.publish(np.random.uniform(0.0, 0.0)) + + 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)) + try: + ( + self._gripper_position, + self._gripper_orientation, + ) = self.tf_listenser.lookupTransform( + "/world", "/end_effector_link", rospy.Time(0) + ) + except ( + tf.LookupException, + tf.ConnectivityException, + tf.ExtrapolationException, + ): + pass + + def kinematics_pose_callback(self, msg): + """Callback function of gripper kinematic pose subscriber. + To resolve issue w/ subscribing f.k. info from the controller, + here we use the tf.transformation instead. + Args: + msg (KinematicsPose): Callback message contains kinematics pose. + """ + raise NotImplementedError + + 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, joint_angles): + """Move joints using joint position command publishers.""" + self.pub_joint1_position.publish(joint_angles[0]) + self.pub_joint2_position.publish(joint_angles[1]) + self.pub_joint3_position.publish(joint_angles[2]) + self.pub_joint4_position.publish(joint_angles[3]) + self.pub_gripper_position.publish(joint_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"]: + print ("Current episode succeeded") + 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: + print ("Current episode terminated") + 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.init_robot_pose() + time.sleep(0.5) + + 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, z, overhead_orientation = ( + np.random.uniform(*self.cfg["POLAR_RADIAN_BOUNDARY"]), + np.random.uniform(*self.cfg["POLAR_THETA_BOUNDARY"]), + np.random.uniform(*self.cfg["Z_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 = z + + self.block_pose = [ + block_pose_position_x, + block_pose_position_y, + block_pose_position_z, + ] + + # TODO: Add block generation condition when testing gazebo simulation. + +# 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.block_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 new file mode 100755 index 0000000..061caca --- /dev/null +++ b/scripts/gazebo_test_open_manipulator.py @@ -0,0 +1,150 @@ +#!/usr/bin/env python + +from math import cos, pi, sin + +import numpy as np + +import rospy +from config.environment.open_manipulator import config as cfg +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 +) + + +def test_reset(): + env = OpenManipulatorReacherEnv(cfg) + _ = env.reset() + + +def test_forward(): + env = OpenManipulatorReacherEnv(cfg) + _ = env.reset() + _pose = Pose() + _pose.position.x = 0.4 + _pose.position.y = 0.0 + _pose.position.z = 0.1 + _pose.orientation.x = 0.0 + _pose.orientation.y = 0.0 + _pose.orientation.z = 0.0 + _pose.orientation.w = 1.0 + forward_pose = KinematicsPose() + forward_pose.pose = _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("arm", "gripper", forward_pose, 2.0) + except rospy.ServiceException as e: + rospy.loginfo("Path planning service call failed: {0}".format(e)) + + +def test_rotate(): + _qpose = JointPosition() + _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("arm", _qpose, 2.0) + except rospy.ServiceException, e: + rospy.loginfo("Path planning service call failed: {0}".format(e)) + _qpose.position[0] += -1.0 + _qpose.position[3] += -1.0 + try: + _ = task_space_srv("arm", _qpose, 2.0) + except rospy.ServiceException, e: + rospy.loginfo("Path planning service call failed: {0}".format(e)) + + +def test_block_loc(): + env = OpenManipulatorReacherEnv(cfg) + for iter in range(20): + b_pose = Pose() + 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.ros_interface.set_target_block() + rospy.sleep(2.0) + env.ros_interface.delete_target_block() + + +def test_achieve_goal(): + env = OpenManipulatorReacherEnv(cfg) + for iter in range(20): + 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 = block_pose.position + r_pose.position.z = 0.08 + 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("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.ros_interface.delete_target_block() + + +def test_workspace_limit(): + 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) + + 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 = 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("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.ros_interface.check_for_termination() + env.ros_interface.delete_target_block() + + +if __name__ == "__main__": + # test_reset() + # test_forward() + # test_rotate() + # test_block_loc() + # test_achieve_goal() + test_workspace_limit() 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 new file mode 100755 index 0000000..d1563ed --- /dev/null +++ b/scripts/run_open_manipulator_reacher_v0.py @@ -0,0 +1,76 @@ +#! /usr/bin/env python + +# -*- coding: utf-8 -*- +"""Train or test algorithms on OpenManipulator Reacher-v0 on Gazebo. + +- Author: Kh Kim +- Contact: kh.kim@medipixel.io +""" + +import argparse +import importlib + +import algorithms.common.helper_functions as common_utils +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") +parser.add_argument( + "--seed", type=int, default=777, help="random seed for reproducibility" +) +parser.add_argument("--algo", type=str, default="td3", help="choose an algorithm") +parser.add_argument( + "--test", dest="test", action="store_true", help="test mode (no training)" +) +parser.add_argument( + "--load-from", type=str, help="load the saved model and optimizer at the beginning" +) +parser.add_argument( + "--off-render", dest="render", action="store_false", help="turn off rendering" +) +parser.add_argument( + "--render-after", + type=int, + default=0, + help="start rendering after the input number of episode", +) +parser.add_argument("--log", dest="log", action="store_true", help="turn on logging") +parser.add_argument("--save-period", type=int, default=200, help="save model period") +parser.add_argument("--episode-num", type=int, default=20000, help="total episode num") +parser.add_argument( + "--max-episode-steps", type=int, default=-1, help="max episode step" +) +parser.add_argument( + "--demo-path", type=str, default="data/reacher_demo.pkl", help="demonstration path" +) + +parser.set_defaults(test=False) +parser.set_defaults(load_from=None) +parser.set_defaults(render=True) +parser.set_defaults(log=False) +args = parser.parse_args() + + +def main(): + """Main.""" + # env initialization + env = OpenManipulatorReacherEnv(env_cfg) + + # set a random seed + 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 + if args.test: + agent.test() + else: + agent.train() + + +if __name__ == "__main__": + 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__": diff --git a/urdf/block/model.urdf b/urdf/block/model.urdf index ce368af..11938f3 100755 --- a/urdf/block/model.urdf +++ b/urdf/block/model.urdf @@ -9,21 +9,22 @@ - + - + - + - + - - Gazebo/Red + + true + Gazebo/Blue 1000 1000 - + \ No newline at end of file