mirror of
https://github.com/wassname/kair_algorithms_draft.git
synced 2026-09-09 11:25:10 +08:00
Add openmanipulator simulation environment agent (#50)
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
|
||||
|
||||
+3
-2
@@ -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
|
||||
|
||||
Executable
+50
@@ -0,0 +1,50 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- gazebo related args -->
|
||||
<arg name="paused" default="false"/>
|
||||
<arg name="use_sim_time" default="true"/>
|
||||
<arg name="gui" default="true"/>
|
||||
<arg name="headless" default="false"/>
|
||||
<arg name="debug" default="false"/>
|
||||
|
||||
<!-- rviz & tf related args -->
|
||||
<arg name="robot_name" default="open_manipulator"/>
|
||||
<arg name="open_rviz" default="false" />
|
||||
<arg name="use_gui" default="false" />
|
||||
|
||||
<!-- gazebo related -->
|
||||
<rosparam file="$(find open_manipulator_gazebo)/config/gazebo_controller.yaml" command="load" />
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||
<arg name="world_name" value="$(find open_manipulator_gazebo)/worlds/empty.world"/>
|
||||
<arg name="debug" value="$(arg debug)" />
|
||||
<arg name="gui" value="$(arg gui)" />
|
||||
<arg name="paused" value="$(arg paused)"/>
|
||||
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
|
||||
<arg name="headless" value="$(arg headless)"/>
|
||||
</include>
|
||||
|
||||
<!-- rviz related -->
|
||||
<!-- Send joint values -->
|
||||
<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher">
|
||||
<param name="/use_gui" value="$(arg use_gui)"/>
|
||||
<rosparam param="source_list" subst_value="true">["$(arg robot_name)/joint_states"]</rosparam>
|
||||
</node>
|
||||
<!-- Combine joint values to TF-->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"/>
|
||||
|
||||
<!-- Show in Rviz -->
|
||||
<group if="$(arg open_rviz)">
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find open_manipulator_description)/rviz/open_manipulator.rviz"/>
|
||||
</group>
|
||||
|
||||
<!-- Load the URDF into the ROS Parameter Server -->
|
||||
<param name="robot_description"
|
||||
command="$(find xacro)/xacro --inorder '$(find open_manipulator_description)/urdf/open_manipulator.urdf.xacro'"/>
|
||||
|
||||
<!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
|
||||
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
|
||||
args="-urdf -model open_manipulator -z 0.0 -param robot_description"/>
|
||||
|
||||
<!-- ros_control robotis manipulator launch file -->
|
||||
<include file="$(find open_manipulator_gazebo)/launch/open_manipulator_controller.launch"/>
|
||||
</launch>
|
||||
@@ -1,13 +0,0 @@
|
||||
<launch>
|
||||
|
||||
<!--Sawyer URDF-->
|
||||
<arg name="sawyer_urdf" value="robot_description"/>
|
||||
<param name="$(arg sawyer_urdf)" textfile="$(find ddpg)/urdf/sawyer.urdf"/>
|
||||
|
||||
<!--Target pose publishing node-->
|
||||
<!--node name="basic_ui" pkg="telehaptics" type="basic_ui.py" output="screen"/-->
|
||||
|
||||
<!--Bare velocity controller node -->
|
||||
<node name="velocity_control" pkg="ddpg" type="sawyer_velocity_control.py" output="screen"/>
|
||||
|
||||
</launch>
|
||||
@@ -1,33 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
|
||||
<arg name="controllers" default="joint_state_controller joint_trajectory_vel_controller"/>
|
||||
<arg name="hardware_interface" default="hardware_interface/VelocityJointInterface"/>
|
||||
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||
<arg name="paused" value="false"/>
|
||||
<arg name="use_sim_time" value="true"/>
|
||||
<arg name="gui" value="true"/>
|
||||
<arg name="headless" value="false"/>
|
||||
<arg name="debug" value="false"/>
|
||||
</include>
|
||||
|
||||
<!-- the urdf/sdf parameter -->
|
||||
<param name="robot_description"
|
||||
command="$(find xacro)/xacro.py $(find yumi_description)/urdf/yumi.urdf.xacro prefix:=$(arg hardware_interface)"/>
|
||||
|
||||
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model yumi" respawn="false" output="screen" />
|
||||
|
||||
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
|
||||
<remap from="/joint_states" to="/yumi/joint_states" />
|
||||
</node>
|
||||
|
||||
<!-- Load joint controller configurations from YAML file to parameter server -->
|
||||
<rosparam file="$(find yumi_control)/config/controllers.yaml" command="load" ns="/yumi"/>
|
||||
|
||||
<!-- load the controllers -->
|
||||
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="$(arg controllers)" ns="/yumi">
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
Regular → Executable
+1
-1
@@ -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")
|
||||
|
||||
+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
+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)
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
+54
@@ -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
|
||||
@@ -0,0 +1,3 @@
|
||||
from .open_manipulator import OpenManipulatorReacherEnv
|
||||
|
||||
__all__ = ["OpenManipulatorReacherEnv"]
|
||||
@@ -0,0 +1,3 @@
|
||||
from .open_manipulator_reacher_env import OpenManipulatorReacherEnv
|
||||
|
||||
__all__ = ["OpenManipulatorReacherEnv"]
|
||||
@@ -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
|
||||
+471
@@ -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)
|
||||
Executable
+150
@@ -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()
|
||||
@@ -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__":
|
||||
|
||||
Executable
+76
@@ -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()
|
||||
@@ -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__":
|
||||
|
||||
@@ -9,21 +9,22 @@
|
||||
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0.025 0.025 0.025"/>
|
||||
<origin xyz="0.0001 0.0001 0.0001"/>
|
||||
<geometry>
|
||||
<box size="0.045 0.045 0.045" />
|
||||
<box size="0.05 0.05 0.05" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0.025 0.025 0.025"/>
|
||||
<origin xyz="0.0001 0.0001 0.0001"/>
|
||||
<geometry>
|
||||
<box size="0.045 0.045 0.045" />
|
||||
<box size="0.0001 0.0001 0.0001" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<gazebo reference="block">
|
||||
<material>Gazebo/Red</material>
|
||||
<gazebo>
|
||||
<static>true</static>
|
||||
<material>Gazebo/Blue</material>
|
||||
<mu1>1000</mu1>
|
||||
<mu2>1000</mu2>
|
||||
</gazebo>
|
||||
</robot>
|
||||
</robot>
|
||||
Reference in New Issue
Block a user