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
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__":
|
||||
|
||||
Reference in New Issue
Block a user