mirror of
https://github.com/wassname/kair_algorithms_draft.git
synced 2026-09-09 11:25:10 +08:00
Refactor OpenManipulator env class (#46)
* Merge subin branch Squashed commit of the following: commit 98112b8c05f955b1eb49a6b78023cad0979d5f95 Author: Subin Yang <ysb8049@naver.com> Date: Sat Mar 30 18:19:46 2019 +0900 Remove noqa commit f45571a80afd403c8ec56db8a2fb5cbedf288db7 Author: Subin Yang <ysb8049@naver.com> Date: Sat Mar 30 17:50:39 2019 +0900 Resolve flake8 commit 058d85bc4ed09441d27065e6d304bfb946942a98 Author: Subin Yang <ysb8049@naver.com> Date: Sat Mar 30 17:41:35 2019 +0900 Modify structures of ros interface and reacher env commit ae4c859ffa6b008823050f310bdccbee6a1de30a Author: Subin Yang <ysb8049@naver.com> Date: Sat Mar 30 17:28:15 2019 +0900 Resolve flake8 commit 4c74ec6527b52d75882ddbe1b4f518f9c252a25c Author: Subin Yang <ysb8049@naver.com> Date: Sat Mar 30 17:23:30 2019 +0900 Resolve flake8 commit 243b2f3739b4388d814a886d5cf1b85a05bb526a Author: Subin Yang <ysb8049@naver.com> Date: Sat Mar 30 17:18:13 2019 +0900 Add open manipulator environment * Refactor openmanipulator environment class * Refactored env structure * Fix errors * Fix error * Add open_manipulator launch file * Fix errors * fix error * fix error * fix error * fix error * fix error * fix error * fix error * Fix typo * Delete unused script * Change reward * Fix typo, add env name to config * Change demo file compatible to python2 (#40) * Change demo file to python2 compatible * Add object to classes for compatibility with python2 * Refactoring config, envs and ros interface (#48) * Refactoring config architecture * Replace network hyper params on agent config * Modify env class and ros interface class * Modify getter and setter on ros interface * Modify wrong code * Fix typo * Add env config * Final environment class and test scripts before the test (#43) * new user branch * Resolve formatting issues on test scripts * Resolve formatting issues on test scripts * Merge subin branch Squashed commit of the following: commit 98112b8c05f955b1eb49a6b78023cad0979d5f95 Author: Subin Yang <ysb8049@naver.com> Date: Sat Mar 30 18:19:46 2019 +0900 Remove noqa commit f45571a80afd403c8ec56db8a2fb5cbedf288db7 Author: Subin Yang <ysb8049@naver.com> Date: Sat Mar 30 17:50:39 2019 +0900 Resolve flake8 commit 058d85bc4ed09441d27065e6d304bfb946942a98 Author: Subin Yang <ysb8049@naver.com> Date: Sat Mar 30 17:41:35 2019 +0900 Modify structures of ros interface and reacher env commit ae4c859ffa6b008823050f310bdccbee6a1de30a Author: Subin Yang <ysb8049@naver.com> Date: Sat Mar 30 17:28:15 2019 +0900 Resolve flake8 commit 4c74ec6527b52d75882ddbe1b4f518f9c252a25c Author: Subin Yang <ysb8049@naver.com> Date: Sat Mar 30 17:23:30 2019 +0900 Resolve flake8 commit 243b2f3739b4388d814a886d5cf1b85a05bb526a Author: Subin Yang <ysb8049@naver.com> Date: Sat Mar 30 17:18:13 2019 +0900 Add open manipulator environment * Refactor openmanipulator environment class * Test the training loop with td3 baseline * Add one-shot launch file for gazebo initialization * Refactored env structure * Fix errors * Fix error * Fix errors * fix error * fix error * fix error * fix error * fix error * fix error * fix error * Fix typo * Delete unused script * Change reward * Fix typo, add env name to config * Refactoring config, envs and ros interface (#48) * Refactoring config architecture * Replace network hyper params on agent config * Modify env class and ros interface class * Modify getter and setter on ros interface * Modify wrong code * Fix typo * Add env config * Resolve flake8, typo issue * Resolve conflict during pull remote
This commit is contained in:
Whi Kwon
Executable → Regular
+13
-13
@@ -33,22 +33,28 @@ hyper_params = {
|
||||
"AUTO_ENTROPY_TUNING": True,
|
||||
"WEIGHT_DECAY": 0.0,
|
||||
"INITIAL_RANDOM_ACTION": 5000,
|
||||
"NETWORK": {
|
||||
"ACTOR_HIDDEN_SIZES": [256, 256],
|
||||
"VF_HIDDEN_SIZES": [256, 256],
|
||||
"QF_HIDDEN_SIZES": [256, 256],
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def run(env, args, state_dim, action_dim):
|
||||
def get(env, args):
|
||||
"""Run training or test.
|
||||
|
||||
Args:
|
||||
env (gym.Env): openAI Gym environment with continuous action space
|
||||
args (argparse.Namespace): arguments including training settings
|
||||
state_dim (int): dimension of states
|
||||
action_dim (int): dimension of actions
|
||||
|
||||
"""
|
||||
hidden_sizes_actor = [256, 256]
|
||||
hidden_sizes_vf = [256, 256]
|
||||
hidden_sizes_qf = [256, 256]
|
||||
state_dim = env.observation_space.shape[0]
|
||||
action_dim = env.action_space.shape[0]
|
||||
|
||||
hidden_sizes_actor = hyper_params["NETWORK"]["ACTOR_HIDDEN_SIZES"]
|
||||
hidden_sizes_vf = hyper_params["NETWORK"]["VF_HIDDEN_SIZES"]
|
||||
hidden_sizes_qf = hyper_params["NETWORK"]["QF_HIDDEN_SIZES"]
|
||||
|
||||
# target entropy
|
||||
target_entropy = -np.prod((action_dim,)).item() # heuristic
|
||||
@@ -102,10 +108,4 @@ def run(env, args, state_dim, action_dim):
|
||||
optims = (actor_optim, vf_optim, qf_1_optim, qf_2_optim)
|
||||
|
||||
# create an agent
|
||||
agent = Agent(env, args, hyper_params, models, optims, target_entropy)
|
||||
|
||||
# run
|
||||
if args.test:
|
||||
agent.test()
|
||||
else:
|
||||
agent.train()
|
||||
return Agent(env, args, hyper_params, models, optims, target_entropy)
|
||||
+13
-13
@@ -42,22 +42,28 @@ hyper_params = {
|
||||
"PER_EPS": 1e-6,
|
||||
"PER_EPS_DEMO": 1.0,
|
||||
"INITIAL_RANDOM_ACTION": int(5e3),
|
||||
"NETWORK": {
|
||||
"ACTOR_HIDDEN_SIZES": [256, 256],
|
||||
"VF_HIDDEN_SIZES": [256, 256],
|
||||
"QF_HIDDEN_SIZES": [256, 256],
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def run(env, args, state_dim, action_dim):
|
||||
def get(env, args):
|
||||
"""Run training or test.
|
||||
|
||||
Args:
|
||||
env (gym.Env): openAI Gym environment with continuous action space
|
||||
args (argparse.Namespace): arguments including training settings
|
||||
state_dim (int): dimension of states
|
||||
action_dim (int): dimension of actions
|
||||
|
||||
"""
|
||||
hidden_sizes_actor = [256, 256]
|
||||
hidden_sizes_vf = [256, 256]
|
||||
hidden_sizes_qf = [256, 256]
|
||||
state_dim = env.observation_space.shape[0]
|
||||
action_dim = env.action_space.shape[0]
|
||||
|
||||
hidden_sizes_actor = hyper_params["NETWORK"]["ACTOR_HIDDEN_SIZES"]
|
||||
hidden_sizes_vf = hyper_params["NETWORK"]["VF_HIDDEN_SIZES"]
|
||||
hidden_sizes_qf = hyper_params["NETWORK"]["QF_HIDDEN_SIZES"]
|
||||
|
||||
# target entropy
|
||||
target_entropy = -np.prod((action_dim,)).item() # heuristic
|
||||
@@ -109,10 +115,4 @@ def run(env, args, state_dim, action_dim):
|
||||
optims = (actor_optim, vf_optim, qf_1_optim, qf_2_optim)
|
||||
|
||||
# create an agent
|
||||
agent = Agent(env, args, hyper_params, models, optims, target_entropy)
|
||||
|
||||
# run
|
||||
if args.test:
|
||||
agent.test()
|
||||
else:
|
||||
agent.train()
|
||||
return Agent(env, args, hyper_params, models, optims, target_entropy)
|
||||
+8
-12
@@ -28,21 +28,23 @@ hyper_params = {
|
||||
"TARGET_POLICY_NOISE_CLIP": 0.5,
|
||||
"POLICY_UPDATE_FREQ": 2,
|
||||
"INITIAL_RANDOM_ACTIONS": 1e4,
|
||||
"NETWORK": {"ACTOR_HIDDEN_SIZES": [400, 300], "CRITIC_HIDDEN_SIZES": [400, 300]},
|
||||
}
|
||||
|
||||
|
||||
def run(env, args, state_dim, action_dim):
|
||||
def get(env, args):
|
||||
"""Run training or test.
|
||||
|
||||
Args:
|
||||
env (gym.Env): openAI Gym environment with continuous action space
|
||||
args (argparse.Namespace): arguments including training settings
|
||||
state_dim (int): dimension of states
|
||||
action_dim (int): dimension of actions
|
||||
|
||||
"""
|
||||
hidden_sizes_actor = [400, 300]
|
||||
hidden_sizes_critic = [400, 300]
|
||||
state_dim = env.observation_space.shape[0]
|
||||
action_dim = env.action_space.shape[0]
|
||||
|
||||
hidden_sizes_actor = hyper_params["NETWORK"]["ACTOR_HIDDEN_SIZES"]
|
||||
hidden_sizes_critic = hyper_params["NETWORK"]["CRITIC_HIDDEN_SIZES"]
|
||||
|
||||
# create actor
|
||||
actor = MLP(
|
||||
@@ -123,10 +125,4 @@ def run(env, args, state_dim, action_dim):
|
||||
noises = (exploration_noise, target_policy_noise)
|
||||
|
||||
# create an agent
|
||||
agent = Agent(env, args, hyper_params, models, optims, noises)
|
||||
|
||||
# run
|
||||
if args.test:
|
||||
agent.test()
|
||||
else:
|
||||
agent.train()
|
||||
return Agent(env, args, hyper_params, models, optims, noises)
|
||||
+8
-12
@@ -40,21 +40,23 @@ hyper_params = {
|
||||
"PER_BETA": 1.0,
|
||||
"PER_EPS": 1e-6,
|
||||
"PER_EPS_DEMO": 1.0,
|
||||
"NETWORK": {"ACTOR_HIDDEN_SIZES": [400, 300], "CRITIC_HIDDEN_SIZES": [400, 300]},
|
||||
}
|
||||
|
||||
|
||||
def run(env, args, state_dim, action_dim):
|
||||
def get(env, args):
|
||||
"""Run training or test.
|
||||
|
||||
Args:
|
||||
env (gym.Env): openAI Gym environment with continuous action space
|
||||
args (argparse.Namespace): arguments including training settings
|
||||
state_dim (int): dimension of states
|
||||
action_dim (int): dimension of actions
|
||||
|
||||
"""
|
||||
hidden_sizes_actor = [400, 300]
|
||||
hidden_sizes_critic = [400, 300]
|
||||
state_dim = env.observation_space.shape[0]
|
||||
action_dim = env.action_space.shape[0]
|
||||
|
||||
hidden_sizes_actor = hyper_params["NETWORK"]["ACTOR_HIDDEN_SIZES"]
|
||||
hidden_sizes_critic = hyper_params["NETWORK"]["CRITIC_HIDDEN_SIZES"]
|
||||
|
||||
# create actor
|
||||
actor = MLP(
|
||||
@@ -135,10 +137,4 @@ def run(env, args, state_dim, action_dim):
|
||||
noises = (exploration_noise, target_policy_noise)
|
||||
|
||||
# create an agent
|
||||
agent = Agent(env, args, hyper_params, models, optims, noises)
|
||||
|
||||
# run
|
||||
if args.test:
|
||||
agent.test()
|
||||
else:
|
||||
agent.train()
|
||||
return Agent(env, args, hyper_params, models, optims, noises)
|
||||
Regular → Executable
Regular → Executable
+8
-12
@@ -28,21 +28,23 @@ hyper_params = {
|
||||
"TARGET_POLICY_NOISE_CLIP": 0.5,
|
||||
"POLICY_UPDATE_FREQ": 2,
|
||||
"INITIAL_RANDOM_ACTIONS": 1e4,
|
||||
"NETWORK": {"ACTOR_HIDDEN_SIZES": [400, 300], "CRITIC_HIDDEN_SIZES": [400, 300]},
|
||||
}
|
||||
|
||||
|
||||
def run(env, args, state_dim, action_dim):
|
||||
def get(env, args):
|
||||
"""Run training or test.
|
||||
|
||||
Args:
|
||||
env (gym.Env): openAI Gym environment with continuous action space
|
||||
args (argparse.Namespace): arguments including training settings
|
||||
state_dim (int): dimension of states
|
||||
action_dim (int): dimension of actions
|
||||
|
||||
"""
|
||||
hidden_sizes_actor = [400, 300]
|
||||
hidden_sizes_critic = [400, 300]
|
||||
state_dim = env.observation_space.shape[0]
|
||||
action_dim = env.action_space.shape[0]
|
||||
|
||||
hidden_sizes_actor = hyper_params["NETWORK"]["ACTOR_HIDDEN_SIZES"]
|
||||
hidden_sizes_critic = hyper_params["NETWORK"]["CRITIC_HIDDEN_SIZES"]
|
||||
|
||||
# create actor
|
||||
actor = MLP(
|
||||
@@ -123,10 +125,4 @@ def run(env, args, state_dim, action_dim):
|
||||
noises = (exploration_noise, target_policy_noise)
|
||||
|
||||
# create an agent
|
||||
agent = Agent(env, args, hyper_params, models, optims, noises)
|
||||
|
||||
# run
|
||||
if args.test:
|
||||
agent.test()
|
||||
else:
|
||||
agent.train()
|
||||
return Agent(env, args, hyper_params, models, optims, noises)
|
||||
@@ -34,22 +34,28 @@ hyper_params = {
|
||||
"DELAYED_UPDATE": 2,
|
||||
"WEIGHT_DECAY": 0.0,
|
||||
"INITIAL_RANDOM_ACTION": int(1e4),
|
||||
"NETWORK": {
|
||||
"ACTOR_HIDDEN_SIZES": [256, 256],
|
||||
"VF_HIDDEN_SIZES": [256, 256],
|
||||
"QF_HIDDEN_SIZES": [256, 256],
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def run(env, args, state_dim, action_dim):
|
||||
def get(env, args):
|
||||
"""Run training or test.
|
||||
|
||||
Args:
|
||||
env (gym.Env): openAI Gym environment with continuous action space
|
||||
args (argparse.Namespace): arguments including training settings
|
||||
state_dim (int): dimension of states
|
||||
action_dim (int): dimension of actions
|
||||
|
||||
"""
|
||||
hidden_sizes_actor = [256, 256]
|
||||
hidden_sizes_vf = [256, 256]
|
||||
hidden_sizes_qf = [256, 256]
|
||||
state_dim = env.observation_space.shape[0]
|
||||
action_dim = env.action_space.shape[0]
|
||||
|
||||
hidden_sizes_actor = hyper_params["NETWORK"]["ACTOR_HIDDEN_SIZES"]
|
||||
hidden_sizes_vf = hyper_params["NETWORK"]["VF_HIDDEN_SIZES"]
|
||||
hidden_sizes_qf = hyper_params["NETWORK"]["QF_HIDDEN_SIZES"]
|
||||
|
||||
# target entropy
|
||||
target_entropy = -np.prod((action_dim,)).item() # heuristic
|
||||
@@ -103,10 +109,4 @@ def run(env, args, state_dim, action_dim):
|
||||
optims = (actor_optim, vf_optim, qf_1_optim, qf_2_optim)
|
||||
|
||||
# create an agent
|
||||
agent = Agent(env, args, hyper_params, models, optims, target_entropy)
|
||||
|
||||
# run
|
||||
if args.test:
|
||||
agent.test()
|
||||
else:
|
||||
agent.train()
|
||||
return Agent(env, args, hyper_params, models, optims, target_entropy)
|
||||
@@ -42,22 +42,28 @@ hyper_params = {
|
||||
"PER_EPS": 1e-6,
|
||||
"PER_EPS_DEMO": 1.0,
|
||||
"INITIAL_RANDOM_ACTION": int(1e4),
|
||||
"NETWORK": {
|
||||
"ACTOR_HIDDEN_SIZES": [256, 256],
|
||||
"VF_HIDDEN_SIZES": [256, 256],
|
||||
"QF_HIDDEN_SIZES": [256, 256],
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def run(env, args, state_dim, action_dim):
|
||||
def get(env, args):
|
||||
"""Run training or test.
|
||||
|
||||
Args:
|
||||
env (gym.Env): openAI Gym environment with continuous action space
|
||||
args (argparse.Namespace): arguments including training settings
|
||||
state_dim (int): dimension of states
|
||||
action_dim (int): dimension of actions
|
||||
|
||||
"""
|
||||
hidden_sizes_actor = [256, 256]
|
||||
hidden_sizes_vf = [256, 256]
|
||||
hidden_sizes_qf = [256, 256]
|
||||
state_dim = env.observation_space.shape[0]
|
||||
action_dim = env.action_space.shape[0]
|
||||
|
||||
hidden_sizes_actor = hyper_params["NETWORK"]["ACTOR_HIDDEN_SIZES"]
|
||||
hidden_sizes_vf = hyper_params["NETWORK"]["VF_HIDDEN_SIZES"]
|
||||
hidden_sizes_qf = hyper_params["NETWORK"]["QF_HIDDEN_SIZES"]
|
||||
|
||||
# target entropy
|
||||
target_entropy = -np.prod((action_dim,)).item() # heuristic
|
||||
@@ -109,10 +115,4 @@ def run(env, args, state_dim, action_dim):
|
||||
optims = (actor_optim, vf_optim, qf_1_optim, qf_2_optim)
|
||||
|
||||
# create an agent
|
||||
agent = Agent(env, args, hyper_params, models, optims, target_entropy)
|
||||
|
||||
# run
|
||||
if args.test:
|
||||
agent.test()
|
||||
else:
|
||||
agent.train()
|
||||
return Agent(env, args, hyper_params, models, optims, target_entropy)
|
||||
Executable → Regular
+9
-12
@@ -7,6 +7,7 @@
|
||||
|
||||
import torch
|
||||
import torch.optim as optim
|
||||
|
||||
from algorithms.common.networks.mlp import MLP
|
||||
from algorithms.common.noise import GaussianNoise
|
||||
from algorithms.td3.agent import Agent
|
||||
@@ -27,21 +28,23 @@ hyper_params = {
|
||||
"TARGET_POLICY_NOISE_CLIP": 0.5,
|
||||
"POLICY_UPDATE_FREQ": 2,
|
||||
"INITIAL_RANDOM_ACTIONS": 1e4,
|
||||
"NETWORK": {"ACTOR_HIDDEN_SIZES": [400, 300], "CRITIC_HIDDEN_SIZES": [400, 300]},
|
||||
}
|
||||
|
||||
|
||||
def run(env, args, state_dim, action_dim):
|
||||
def get(env, args):
|
||||
"""Run training or test.
|
||||
|
||||
Args:
|
||||
env (gym.Env): openAI Gym environment with continuous action space
|
||||
args (argparse.Namespace): arguments including training settings
|
||||
state_dim (int): dimension of states
|
||||
action_dim (int): dimension of actions
|
||||
|
||||
"""
|
||||
hidden_sizes_actor = [400, 300]
|
||||
hidden_sizes_critic = [400, 300]
|
||||
state_dim = env.observation_space.shape[0]
|
||||
action_dim = env.action_space.shape[0]
|
||||
|
||||
hidden_sizes_actor = hyper_params["NETWORK"]["ACTOR_HIDDEN_SIZES"]
|
||||
hidden_sizes_critic = hyper_params["NETWORK"]["CRITIC_HIDDEN_SIZES"]
|
||||
|
||||
# create actor
|
||||
actor = MLP(
|
||||
@@ -122,10 +125,4 @@ def run(env, args, state_dim, action_dim):
|
||||
noises = (exploration_noise, target_policy_noise)
|
||||
|
||||
# create an agent
|
||||
agent = Agent(env, args, hyper_params, models, optims, noises)
|
||||
|
||||
# run
|
||||
if args.test:
|
||||
agent.test()
|
||||
else:
|
||||
agent.train()
|
||||
return Agent(env, args, hyper_params, models, optims, noises)
|
||||
@@ -40,21 +40,23 @@ hyper_params = {
|
||||
"PER_BETA": 1.0,
|
||||
"PER_EPS": 1e-6,
|
||||
"PER_EPS_DEMO": 1.0,
|
||||
"NETWORK": {"ACTOR_HIDDEN_SIZES": [400, 300], "CRITIC_HIDDEN_SIZES": [400, 300]},
|
||||
}
|
||||
|
||||
|
||||
def run(env, args, state_dim, action_dim):
|
||||
def get(env, args):
|
||||
"""Run training or test.
|
||||
|
||||
Args:
|
||||
env (gym.Env): openAI Gym environment with continuous action space
|
||||
args (argparse.Namespace): arguments including training settings
|
||||
state_dim (int): dimension of states
|
||||
action_dim (int): dimension of actions
|
||||
|
||||
"""
|
||||
hidden_sizes_actor = [400, 300]
|
||||
hidden_sizes_critic = [400, 300]
|
||||
state_dim = env.observation_space.shape[0]
|
||||
action_dim = env.action_space.shape[0]
|
||||
|
||||
hidden_sizes_actor = hyper_params["NETWORK"]["ACTOR_HIDDEN_SIZES"]
|
||||
hidden_sizes_critic = hyper_params["NETWORK"]["CRITIC_HIDDEN_SIZES"]
|
||||
|
||||
# create actor
|
||||
actor = MLP(
|
||||
@@ -135,10 +137,4 @@ def run(env, args, state_dim, action_dim):
|
||||
noises = (exploration_noise, target_policy_noise)
|
||||
|
||||
# create an agent
|
||||
agent = Agent(env, args, hyper_params, models, optims, noises)
|
||||
|
||||
# run
|
||||
if args.test:
|
||||
agent.test()
|
||||
else:
|
||||
agent.train()
|
||||
return Agent(env, args, hyper_params, models, optims, noises)
|
||||
+52
@@ -0,0 +1,52 @@
|
||||
from math import pi
|
||||
|
||||
from geometry_msgs.msg import Quaternion
|
||||
|
||||
config = {
|
||||
"TERM_COUNT": 10,
|
||||
"SUCCESS_COUNT": 10,
|
||||
"OVERHEAD_ORIENTATION": Quaternion(
|
||||
x=-0.00142460053167, y=0.999994209902, z=-0.00177030764765, w=0.00253311793936
|
||||
),
|
||||
# box boundary
|
||||
"POLAR_RADIAN_BOUNDARY": (0.134, 0.32),
|
||||
"POLAR_THETA_BOUNDARY": (-pi * 0.7 / 4, pi * 0.7 / 4),
|
||||
"Z_BOUNDARY": (0.05, 0.28),
|
||||
"JOINT_LIMITS": {
|
||||
"HIGH": {
|
||||
"J1": pi * 0.9,
|
||||
"J2": pi * 0.5,
|
||||
"J3": pi * 0.44,
|
||||
"J4": pi * 0.65,
|
||||
"GRIP": -0.001,
|
||||
},
|
||||
"LOW": {
|
||||
"J1": -pi * 0.9,
|
||||
"J2": -pi * 0.57,
|
||||
"J3": -pi * 0.3,
|
||||
"J4": -pi * 0.57,
|
||||
"GRIP": 0.019,
|
||||
},
|
||||
},
|
||||
# Global variables
|
||||
"ACTION_DIM": 5, # Cartesian
|
||||
"OBSERVATION_DIM": (25,),
|
||||
# terminal condition
|
||||
"INNER_RADIAN": 0.134,
|
||||
"OUTER_RADIAN": 0.3,
|
||||
"LOWER_RADIAN": 0.384,
|
||||
"INNER_Z": 0.321,
|
||||
"OUTER_Z": 0.250,
|
||||
"LOWER_Z": 0.116,
|
||||
"ENV_MODE": "sim",
|
||||
"TRAIN_MODE": True,
|
||||
"MAX_EPISODE_STEPS": 100,
|
||||
"DISTANCE_THRESHOLD": 0.1,
|
||||
"REWARD_RESCALE_RATIO": 1.0,
|
||||
"REWARD_FUNC": "l2",
|
||||
"CONTROL_MODE": "position",
|
||||
}
|
||||
|
||||
|
||||
def get():
|
||||
return config
|
||||
@@ -0,0 +1,3 @@
|
||||
from .open_manipulator import OpenManipulatorReacherEnv
|
||||
|
||||
__all__ = ["OpenManipulatorReacherEnv"]
|
||||
@@ -0,0 +1,3 @@
|
||||
from .open_manipulator_reacher_env import OpenManipulatorReacherEnv
|
||||
|
||||
__all__ = ["OpenManipulatorReacherEnv"]
|
||||
@@ -1,523 +0,0 @@
|
||||
#! /usr/bin/env python
|
||||
|
||||
import gym
|
||||
import copy
|
||||
import math
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
from random import *
|
||||
from string import Template
|
||||
from math import pi, cos, sin, radians
|
||||
import cv2
|
||||
import numpy as np
|
||||
import rospkg
|
||||
|
||||
import rospy
|
||||
from control_msgs.msg import JointTrajectoryControllerState
|
||||
from cv_bridge import CvBridge, CvBridgeError
|
||||
from gazebo_msgs.msg import ContactsState
|
||||
from gazebo_msgs.srv import DeleteModel, GetModelState, SetModelState, SpawnModel
|
||||
# reads open_manipulator's state
|
||||
from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion
|
||||
from open_manipulator_msgs.msg import *
|
||||
from sensor_msgs.msg import Image, JointState
|
||||
from std_msgs.msg import *
|
||||
from tf import TransformListener
|
||||
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||||
|
||||
base_dir = os.path.dirname(os.path.realpath(__file__))
|
||||
|
||||
overhead_orientation = Quaternion(
|
||||
x=-0.00142460053167, y=0.999994209902, z=-0.00177030764765, w=0.00253311793936
|
||||
)
|
||||
|
||||
# safe joint limits
|
||||
joint_limits = {'hi':{'j1':pi*0.9, 'j2':pi*0.5, 'j3':pi*0.44, 'j4': pi*0.65, 'grip':-.001 },
|
||||
'lo':{'j1':-pi*0.9, 'j2':-pi*0.57, 'j3':-pi*0.3, 'j4':-pi*0.57, 'grip': .019 }}
|
||||
|
||||
# <limit velocity="4.8" effort="1" lower="-0.010" upper="0.019" />
|
||||
|
||||
|
||||
cartesian_limits = {}
|
||||
# safe cartesian limits
|
||||
# robot @ home-pose : (0.134, 0.0, 0.241)
|
||||
|
||||
# episode termination condition
|
||||
X_MIN = 0.1
|
||||
X_MAX = 0.5
|
||||
Y_MIN = -0.3
|
||||
Y_MAX = 0.3
|
||||
Z_MIN = 0.0
|
||||
Z_MAX = 0.6
|
||||
TERM_COUNT = 10
|
||||
SUC_COUNT = 10
|
||||
|
||||
|
||||
# Global variables
|
||||
# -------------------------
|
||||
ACTION_DIM = 3 # Cartesian
|
||||
OBS_DIM = (100, 100, 3) # POMDP
|
||||
STATE_DIM = 24 # MDP
|
||||
|
||||
|
||||
class OpenManipulatorEnv:
|
||||
def __init__(
|
||||
self,
|
||||
max_steps=700,
|
||||
isdagger=False,
|
||||
isPOMDP=False,
|
||||
isreal=False,
|
||||
train_indicator=0,
|
||||
):
|
||||
"""An implementation of OpenAI-Gym style robot reacher environment
|
||||
TODO: add method that receives target object's pose as state
|
||||
"""
|
||||
rospy.init_node("OpenManipulatorEnv")
|
||||
self.train_indicator = train_indicator # 0: Train 1:Test
|
||||
self.isdagger = isdagger
|
||||
self.isPOMDP = isPOMDP
|
||||
self.isreal = isreal
|
||||
self.currentDist = 1
|
||||
self.previousDist = 1
|
||||
self.reached = False
|
||||
self.tf = TransformListener()
|
||||
self.bridge = CvBridge()
|
||||
self.joint_speeds = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
self.joint_positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
self.joint_velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
self.joint_efforts = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
self.right_endpoint_position = [0, 0, 0]
|
||||
self.max_steps = max_steps
|
||||
self.done = False
|
||||
self.reward = 0
|
||||
self.reward_rescale = 1.0
|
||||
self.isDemo = False
|
||||
self.reward_type = "sparse"
|
||||
self.termination_count = 0
|
||||
self.success_count = 0
|
||||
self._control_mode = 'position' # TODO: add 'velocity', 'effort' control methods
|
||||
|
||||
self.pub_gripper_position = rospy.Publisher(
|
||||
"/open_manipulator/gripper_position/command", Float64, queue_size=1
|
||||
)
|
||||
self.pub_gripper_sub_position = rospy.Publisher(
|
||||
"/open_manipulator/gripper_sub_position/command", Float64, queue_size=1
|
||||
)
|
||||
self.pub_joint1_position = rospy.Publisher(
|
||||
"/open_manipulator/joint1_position/command", Float64, queue_size=1
|
||||
)
|
||||
self.pub_joint2_position = rospy.Publisher(
|
||||
"/open_manipulator/joint2_position/command", Float64, queue_size=1
|
||||
)
|
||||
self.pub_joint3_position = rospy.Publisher(
|
||||
"/open_manipulator/joint3_position/command", Float64, queue_size=1
|
||||
)
|
||||
self.pub_joint4_position = rospy.Publisher(
|
||||
"/open_manipulator/joint4_position/command", Float64, queue_size=1
|
||||
)
|
||||
|
||||
# TODO: manage this attribute when it's real test environment
|
||||
self.joints_position_cmd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
self.kinematics_cmd = [0.0, 0.0, 0.0]
|
||||
self.sub_joint_state = rospy.Subscriber(
|
||||
"/open_manipulator/joint_states", JointState, self.joint_state_callback
|
||||
)
|
||||
# joint position/velocity/effort
|
||||
self.sub_kinematics_pose = rospy.Subscriber(
|
||||
"/open_manipulator/gripper/kinematics_pose",
|
||||
KinematicsPose,
|
||||
self.kinematics_pose_callback,
|
||||
)
|
||||
self.sub_robot_state = rospy.Subscriber(
|
||||
"/open_manipulator/states", OpenManipulatorState, self.robot_state_callback
|
||||
)
|
||||
# cs position / orientation
|
||||
# variables for subscribe the joint states
|
||||
self.joint_names = [
|
||||
"gripper",
|
||||
"gripper_sub",
|
||||
"joint1",
|
||||
"joint2",
|
||||
"joint3",
|
||||
"joint4",
|
||||
] # name: [gripper, gripper_sub, joint1, joint2, joint3, joint4]
|
||||
self.joint_positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
self.joint_velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
self.joint_efforts = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
|
||||
# ee pose of robot -> used to compute reward.
|
||||
self.gripper_position = [0.0, 0.0, 0.0] # [x, y, z] cartesian position
|
||||
self.gripper_orientiation = [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
] # [x, y, z, w] quaternion orientation
|
||||
self.distance_threshold = 0.1
|
||||
|
||||
# used for per-step elapsed time measurement
|
||||
self.tic = 0.0
|
||||
self.toc = 0.0
|
||||
self.elapsed = 0.0
|
||||
# self.initial_state = self.get_joints_states().copy()
|
||||
self._action_scale = 1.0
|
||||
|
||||
# open manipulator statets
|
||||
self.moving_state = ""
|
||||
self.actuator_state = ""
|
||||
self.init_robot_pose()
|
||||
# TODO: replace following attributes with those inherited from gym.env
|
||||
self.reward_range = None
|
||||
self.metadata = None
|
||||
self._max_episode_steps = 50
|
||||
|
||||
rospy.on_shutdown(self._delete_target_block)
|
||||
|
||||
def get_observation(self):
|
||||
"""
|
||||
Get robot observation.
|
||||
|
||||
:return: robot observation
|
||||
"""
|
||||
# cartesian space : TODO: consider if more info (e.g. lin/ang velocitiy in CS) is necessary.
|
||||
gripper_pos = np.array(self.gripper_position)
|
||||
gripper_ori = np.array(self.gripper_orientiation)
|
||||
|
||||
# joint space
|
||||
robot_joint_angles = np.array(self.joint_positions)
|
||||
robot_joint_velocities = np.array(self.joint_velocities)
|
||||
robot_joint_efforts = np.array(self.joint_efforts)
|
||||
|
||||
obs = np.concatenate(
|
||||
(gripper_pos, gripper_ori, robot_joint_angles,
|
||||
robot_joint_velocities, robot_joint_efforts))
|
||||
return obs
|
||||
|
||||
@property
|
||||
def observation_space(self):
|
||||
""" return the open manipulator's state space for this specific environment.
|
||||
"""
|
||||
return gym.spaces.Box(
|
||||
-np.inf,
|
||||
np.inf,
|
||||
shape=self.get_observation().shape,
|
||||
dtype=np.float32)
|
||||
|
||||
@property
|
||||
def action_space(self):
|
||||
""" return the open manipulator's action space for this specific environment.
|
||||
TODO: expand to various action space types.
|
||||
"""
|
||||
if self._control_mode == 'position':
|
||||
lower_bounds = np.array(
|
||||
[joint_limits['lo']['j1'], joint_limits['lo']['j2'], joint_limits['lo']['j3'], joint_limits['lo']['j4'], joint_limits['lo']['grip']])
|
||||
upper_bounds = np.array(
|
||||
[joint_limits['hi']['j1'], joint_limits['hi']['j2'], joint_limits['hi']['j3'], joint_limits['hi']['j4'], joint_limits['hi']['grip']])
|
||||
elif self._control_mode == 'velocity':
|
||||
raise NotImplementedError('Control mode %s is not implemented yet.' % self._control_mode)
|
||||
|
||||
elif self._control_mode == 'effort':
|
||||
raise NotImplementedError('Control mode %s is not implemented yet.' % self._control_mode)
|
||||
else:
|
||||
raise ValueError('Control mode %s is not known!' % self._control_mode)
|
||||
return gym.spaces.Box(
|
||||
lower_bounds,
|
||||
upper_bounds,
|
||||
dtype=np.float32)
|
||||
|
||||
def seed(seed):
|
||||
""" apply random seed to the environment.
|
||||
TODO: implement this method.
|
||||
"""
|
||||
return True
|
||||
|
||||
def render(self, mode):
|
||||
pass
|
||||
|
||||
def robot_state_callback(self, msg):
|
||||
self.moving_state = msg.open_manipulator_moving_state # "MOVING" / "STOPPED"
|
||||
self.actuator_state = (
|
||||
msg.open_manipulator_actuator_state
|
||||
) # "ACTUATOR_ENABLE" / "ACTUATOR_DISABLE"
|
||||
|
||||
def joint_state_callback(self, msg):
|
||||
"""Callback function of joint states subscriber.
|
||||
Argument: msg
|
||||
"""
|
||||
self.joints_states = msg
|
||||
self.joint_names = self.joints_states.name
|
||||
self.joint_positions = self.joints_states.position
|
||||
self.joint_velocities = self.joints_states.velocity
|
||||
self.joint_efforts = self.joints_states.effort
|
||||
# penalize jerky motion in reward for shaped reward setting.
|
||||
self.squared_sum_vel = np.linalg.norm(np.array(self.joint_velocities))
|
||||
|
||||
def kinematics_pose_callback(self, msg):
|
||||
"""Callback function of gripper kinematic pose subscriber.
|
||||
Argument: msg
|
||||
"""
|
||||
self.kinematics_pose = msg
|
||||
_gripper_position = self.kinematics_pose.pose.position
|
||||
self.gripper_position = [
|
||||
_gripper_position.x,
|
||||
_gripper_position.y,
|
||||
_gripper_position.z,
|
||||
]
|
||||
_gripper_orientiation = self.kinematics_pose.pose.orientation
|
||||
self.gripper_orientiation = [
|
||||
_gripper_orientiation.x,
|
||||
_gripper_orientiation.y,
|
||||
_gripper_orientiation.z,
|
||||
_gripper_orientiation.w,
|
||||
]
|
||||
|
||||
# get and set function
|
||||
# ----------------------------
|
||||
|
||||
def get_joints_states(self):
|
||||
"""Returns current joints states of robot including position, velocity, effort
|
||||
Returns: Float64[] self.joints_position, self.joints_velocity, self.joint_effort
|
||||
"""
|
||||
return self.joint_positions, self.joint_velocities, self.joint_efforts
|
||||
|
||||
def get_gripper_pose(self):
|
||||
"""Returns gripper end effector position
|
||||
Returns: Pose().position, Pose().orientation
|
||||
"""
|
||||
return self.gripper_position, self.gripper_orientiation
|
||||
|
||||
def get_gripper_position(self):
|
||||
"""Returns gripper end effector position
|
||||
Returns: Pose().position
|
||||
"""
|
||||
return self.gripper_position
|
||||
|
||||
def set_joints_position(self, joints_angles):
|
||||
"""Move joints using joint position command publishers.
|
||||
Argument: joints_position_cmd
|
||||
self.joints_position_cmd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
"""
|
||||
# rospy.loginfo(Set joint position)
|
||||
self.pub_gripper_position.publish(joints_angles[0])
|
||||
self.pub_joint1_position.publish(joints_angles[1])
|
||||
self.pub_joint2_position.publish(joints_angles[2])
|
||||
self.pub_joint3_position.publish(joints_angles[3])
|
||||
self.pub_joint4_position.publish(joints_angles[4])
|
||||
|
||||
def step(self, action=np.array([1, 1, 1, 1, 1, 1]), step=0):
|
||||
"""Function executed each time step.
|
||||
Here we get the action execute it in a time step and retrieve the
|
||||
observations generated by that action.
|
||||
:param action:
|
||||
:return: obs, reward, done
|
||||
"""
|
||||
self.prev_tic = self.tic
|
||||
self.tic = time.time()
|
||||
self.elapsed = time.time() - self.prev_tic
|
||||
self.done = False
|
||||
if step == self.max_steps:
|
||||
self.done = True
|
||||
act = action.flatten().tolist()
|
||||
self.set_joints_position(act)
|
||||
curDist = self._get_dist()
|
||||
if not self.isreal:
|
||||
self.reward = self._compute_reward()
|
||||
if self._check_for_termination():
|
||||
print("======================================================")
|
||||
print("Terminates current Episode : OUT OF BOUNDARY")
|
||||
print("======================================================")
|
||||
elif self._check_for_success():
|
||||
print("======================================================")
|
||||
print("Succeeded current Episode")
|
||||
print("======================================================")
|
||||
_joint_pos, _joint_vels, _joint_effos = self.get_joints_states()
|
||||
# obj_pos = self._get_target_obj_obs() # TODO: implement this function call.
|
||||
|
||||
if np.mod(step, 10) == 0:
|
||||
if not self.isreal:
|
||||
print("DISTANCE : ", curDist)
|
||||
print("PER STEP ELAPSED : ", self.elapsed)
|
||||
print("SPARSE REWARD : ", self.reward_rescale * self.reward)
|
||||
print("Current EE pos: ", self.gripper_position)
|
||||
print("Actions: ", act)
|
||||
|
||||
|
||||
obs = self.get_observation()
|
||||
info = ''
|
||||
|
||||
return obs, self.reward_rescale * self.reward, self.done, info
|
||||
|
||||
def reset(self):
|
||||
# Attempt to reset the simulator. Since we randomize initial conditions, it
|
||||
# is possible to get into a state with numerical issues (e.g. due to penetration or
|
||||
# Gimbel lock) or we may not achieve an initial condition (e.g. an object is within the hand).
|
||||
# In this case, we just keep randomizing until we eventually achieve a valid initial
|
||||
# configuration.
|
||||
did_reset_sim = False
|
||||
self._reset_gazebo_world()
|
||||
_joint_pos, _joint_vels, _joint_effos = self.get_joints_states()
|
||||
obs = self.get_observation()
|
||||
|
||||
return obs
|
||||
|
||||
def _check_robot_moving(self):
|
||||
"""Check if robot has reached its initial pose.
|
||||
"""
|
||||
while not rospy.is_shutdown():
|
||||
if self.moving_state == "STOPPED":
|
||||
break
|
||||
return True
|
||||
|
||||
def _reset_gazebo_world(self):
|
||||
"""
|
||||
Method that randomly initialize the state of robot agent and surrounding envs (including target obj.)
|
||||
"""
|
||||
self._delete_target_block()
|
||||
|
||||
self.pub_gripper_position.publish(np.random.uniform(0.0, 0.1))
|
||||
self.pub_joint1_position.publish(np.random.uniform(-0.1, 0.1))
|
||||
self.pub_joint2_position.publish(np.random.uniform(-0.1, 0.1))
|
||||
self.pub_joint3_position.publish(np.random.uniform(-0.1, 0.1))
|
||||
self.pub_joint4_position.publish(np.random.uniform(-0.1, 0.1))
|
||||
self._load_target_block()
|
||||
|
||||
def init_robot_pose(self):
|
||||
self.pub_gripper_position.publish(np.random.uniform(0.0, 0.1))
|
||||
self.pub_joint1_position.publish(np.random.uniform(-0.1, 0.1))
|
||||
self.pub_joint2_position.publish(np.random.uniform(-0.1, 0.1))
|
||||
self.pub_joint3_position.publish(np.random.uniform(-0.1, 0.1))
|
||||
self.pub_joint4_position.publish(np.random.uniform(-0.1, 0.1))
|
||||
self._load_target_block()
|
||||
|
||||
def _delete_target_block(self):
|
||||
# This will be called on ROS Exit, deleting Gazebo models
|
||||
# Do not wait for the Gazebo Delete Model service, since
|
||||
# Gazebo should already be running. If the service is not
|
||||
# available since Gazebo has been killed, it is fine to error out
|
||||
try:
|
||||
delete_model = rospy.ServiceProxy("/gazebo/delete_model", DeleteModel)
|
||||
resp_delete = delete_model("block")
|
||||
except rospy.ServiceException as e:
|
||||
rospy.loginfo("Delete Model service call failed: {0}".format(e))
|
||||
|
||||
def _load_target_block(self,
|
||||
block_pose=Pose(position=Point(x=0.6725, y=0.1265, z=0.7825)),
|
||||
block_reference_frame="world",
|
||||
):
|
||||
# Get Models' Path
|
||||
model_path = rospkg.RosPack().get_path("kair_algorithms") + "/urdf/"
|
||||
|
||||
# Load Block URDF
|
||||
block_xml = ""
|
||||
|
||||
with open(model_path + "block/model.urdf", "r") as block_file:
|
||||
block_xml = block_file.read().replace("\n", "")
|
||||
|
||||
# Spawn Block URDF
|
||||
rospy.wait_for_service("/gazebo/spawn_urdf_model")
|
||||
try:
|
||||
spawn_urdf = rospy.ServiceProxy("/gazebo/spawn_urdf_model", SpawnModel)
|
||||
resp_urdf = spawn_urdf(
|
||||
"block", block_xml, "/", block_pose, block_reference_frame
|
||||
)
|
||||
except rospy.ServiceException as e:
|
||||
rospy.logerr("Spawn URDF service call failed: {0}".format(e))
|
||||
|
||||
def _geom_interpolation(self, in_rad, out_rad, in_z, out_z, query):
|
||||
"""interpolates along the outer shell of work space, based on z-position.
|
||||
must feed the corresponding radius from inner radius.
|
||||
"""
|
||||
slope = (out_z - in_z)/(out_rad - in_rad)
|
||||
intercept = in_z
|
||||
return slope*(query - in_rad) + intercept
|
||||
|
||||
|
||||
def _check_for_termination(self):
|
||||
"""
|
||||
Check if the agent has reached undesirable state. If so, terminate the episode early.
|
||||
based on the polar coordinate
|
||||
"""
|
||||
_ee_pose = self.get_gripper_position()
|
||||
# define gemetry
|
||||
inner_rad = 0.134
|
||||
outer_rad = 0.3
|
||||
lower_rad = 0.384
|
||||
inner_z = 0.321
|
||||
outer_z = 0.250
|
||||
lower_z = 0.116
|
||||
rob_rad = np.linalg.norm([_ee_pose[0], _ee_pose[1]])
|
||||
rob_z = _ee_pose[2]
|
||||
if self.joint_positions[0] <= abs(joint_limits['hi']['j1']/2):
|
||||
if rob_rad < inner_rad:
|
||||
self.termination_count += 1
|
||||
rospy.logwarn('OUT OF BOUNDARY : exceeds inner radius limit')
|
||||
elif inner_rad <= rob_rad < outer_rad:
|
||||
upper_z = self._geom_interpolation(inner_rad, outer_rad, inner_z, outer_z, rob_rad)
|
||||
if rob_z > upper_z:
|
||||
self.termination_count += 1
|
||||
rospy.logwarn('OUT OF BOUNDARY : exceeds upper z limit')
|
||||
elif outer_rad <= rob_rad < lower_rad:
|
||||
bevel_z = self._geom_interpolation(outer_rad, lower_rad, outer_z, lower_z, rob_rad)
|
||||
if rob_z > bevel_z:
|
||||
self.termination_count += 1
|
||||
rospy.logwarn('OUT OF BOUNDARY : exceeds bevel z limit')
|
||||
else:
|
||||
self.termination_count += 1
|
||||
rospy.logwarn('OUT OF BOUNDARY : exceeds outer radius limit')
|
||||
else: # joint_1 limit exceeds
|
||||
self.termination_count += 1
|
||||
rospy.logwarn('OUT OF BOUNDARY : joint_1_limit exceeds')
|
||||
|
||||
if self.termination_count == TERM_COUNT:
|
||||
self.done = True
|
||||
self.termination_count = 0
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
|
||||
def _check_for_success(self):
|
||||
"""
|
||||
Check if the agent has succeeded the episode.
|
||||
"""
|
||||
_dist = self._get_dist()
|
||||
if _dist < self.distance_threshold:
|
||||
self.success_count += 1
|
||||
if self.success_count == SUC_COUNT:
|
||||
self.done = True
|
||||
self.success_count = 0
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
|
||||
def _compute_reward(self):
|
||||
"""Computes shaped/sparse reward for each episode.
|
||||
"""
|
||||
cur_dist = self._get_dist()
|
||||
if self.reward_type == "sparse":
|
||||
return (cur_dist <= self.distance_threshold).astype(
|
||||
np.float32
|
||||
) # 1 for success else 0
|
||||
else:
|
||||
return -cur_dist - self.squared_sum_vel # -L2 distance -l2_norm(joint_vels)
|
||||
|
||||
def _get_dist(self):
|
||||
rospy.wait_for_service("/gazebo/get_model_state")
|
||||
try:
|
||||
object_state_srv = rospy.ServiceProxy(
|
||||
"/gazebo/get_model_state", GetModelState
|
||||
)
|
||||
object_state = object_state_srv("block", "world")
|
||||
self._obj_pose = np.array(
|
||||
[
|
||||
object_state.pose.position.x,
|
||||
object_state.pose.position.y,
|
||||
object_state.pose.position.z,
|
||||
]
|
||||
)
|
||||
except rospy.ServiceException as e:
|
||||
rospy.logerr("Spawn URDF service call failed: {0}".format(e))
|
||||
_ee_pose = np.array(self.get_gripper_position()) # FK state of robot
|
||||
return np.linalg.norm(_ee_pose - self._obj_pose)
|
||||
|
||||
def close(self):
|
||||
rospy.signal_shutdown("done")
|
||||
@@ -0,0 +1,134 @@
|
||||
#! usr/bin/env python
|
||||
|
||||
import gym
|
||||
import numpy as np
|
||||
from gym.utils import seeding
|
||||
|
||||
from ros_interface import (
|
||||
OpenManipulatorRosGazeboInterface,
|
||||
OpenManipulatorRosRealInterface,
|
||||
)
|
||||
|
||||
|
||||
class OpenManipulatorReacherEnv(gym.Env):
|
||||
# TODO: write docstring
|
||||
"""Open Manipulator Reacher environment on gym.
|
||||
|
||||
Attributes:
|
||||
cfg (dict): environment config
|
||||
env_mode (str): select mode (sim, real)
|
||||
_max_episode_steps (int): max steps per episodes
|
||||
reward_rescale_ratio (float):
|
||||
reward_func (str): function name for calculating reward
|
||||
|
||||
"""
|
||||
# TODO: cfg or config
|
||||
def __init__(self, cfg):
|
||||
"""Initialization.
|
||||
|
||||
Args:
|
||||
cfg (dict): environment config
|
||||
|
||||
"""
|
||||
self.cfg = cfg
|
||||
self.env_name = self.cfg["ENV_NAME"]
|
||||
self.env_mode = self.cfg["ENV_MODE"]
|
||||
self._max_episode_steps = self.cfg["MAX_EPISODE_STEPS"]
|
||||
self.reward_rescale_ratio = self.cfg["REWARD_RESCALE_RATIO"]
|
||||
self.reward_func = self.cfg["REWARD_FUNC"]
|
||||
|
||||
assert self.env_mode in ["sim", "real"]
|
||||
if self.env_mode == "sim":
|
||||
self.ros_interface = OpenManipulatorRosGazeboInterface(self.cfg)
|
||||
else:
|
||||
self.ros_interface = OpenManipulatorRosRealInterface()
|
||||
|
||||
self.episode_steps = 0
|
||||
self.done = False
|
||||
self.reward = 0
|
||||
|
||||
self.action_space = self.ros_interface.get_action_space()
|
||||
self.observation_space = self.ros_interface.get_observation_space()
|
||||
self.seed()
|
||||
|
||||
def seed(self, seed=None):
|
||||
"""Set random seed."""
|
||||
self.np_random, seed = seeding.np_random(seed)
|
||||
return [seed]
|
||||
|
||||
def step(self, action):
|
||||
"""Function executed each time step.
|
||||
|
||||
Here we get the action execute it in a time step and retrieve the
|
||||
observations generated by that action.
|
||||
|
||||
Args:
|
||||
action: action
|
||||
Returns:
|
||||
Tuple of obs, reward_rescale * reward, done
|
||||
"""
|
||||
if action is None:
|
||||
action = np.array([1, 1, 1, 1, 1, 1])
|
||||
|
||||
self.done = False
|
||||
|
||||
if self.episode_steps == self._max_episode_steps:
|
||||
self.done = True
|
||||
self.episode_steps = 0
|
||||
|
||||
act = action.flatten().tolist()
|
||||
self.ros_interface.set_joints_position(act)
|
||||
|
||||
if self.env_mode == "sim":
|
||||
self.reward = self.compute_reward()
|
||||
if self.ros_interface.check_for_termination():
|
||||
print ("Terminates current Episode : OUT OF BOUNDARY")
|
||||
elif self.ros_interface.check_for_success():
|
||||
print ("Succeeded current Episode")
|
||||
obs = self.ros_interface.get_observation()
|
||||
|
||||
self.episode_steps += 1
|
||||
|
||||
return obs, self.reward_rescale_ratio * self.reward, self.done, None
|
||||
|
||||
def reset(self):
|
||||
"""Attempt to reset the simulator.
|
||||
|
||||
Since we randomize initial conditions, it is possible to get into
|
||||
a state with numerical issues (e.g. due to penetration or
|
||||
Gimbel lock) or we may not achieve an initial condition (e.g. an
|
||||
object is within the hand).
|
||||
|
||||
In this case, we just keep randomizing until we eventually achieve
|
||||
a valid initial
|
||||
configuration.
|
||||
|
||||
Returns:
|
||||
obs (array) : Array of joint position, joint velocity, joint effort
|
||||
"""
|
||||
self.ros_interface.reset_gazebo_world()
|
||||
obs = self.ros_interface.get_observation()
|
||||
|
||||
return obs
|
||||
|
||||
def compute_reward(self):
|
||||
"""Computes shaped/sparse reward for each episode.
|
||||
|
||||
Returns:
|
||||
reward (Float64) : L2 distance of current distance and squared sum velocity.
|
||||
"""
|
||||
cur_dist = self.ros_interface.get_dist()
|
||||
if self.reward_func == "sparse":
|
||||
# 1 for success else 0
|
||||
reward = cur_dist <= self.ros_interface.distance_threshold
|
||||
reward = reward.astype(np.float32)
|
||||
elif self.reward_func == "l2":
|
||||
# - L2 distance
|
||||
reward = -cur_dist
|
||||
else:
|
||||
raise ValueError
|
||||
|
||||
return reward
|
||||
|
||||
def render(self):
|
||||
pass
|
||||
+458
@@ -0,0 +1,458 @@
|
||||
# ! usr/bin/env python
|
||||
|
||||
from abc import ABCMeta
|
||||
from math import cos, sin
|
||||
|
||||
import gym
|
||||
import numpy as np
|
||||
import rospkg # noqa
|
||||
|
||||
import rospy # noqa
|
||||
from gazebo_msgs.srv import DeleteModel, GetModelState, SpawnModel
|
||||
from geometry_msgs.msg import Pose
|
||||
from open_manipulator_msgs.msg import KinematicsPose, OpenManipulatorState
|
||||
from sensor_msgs.msg import JointState
|
||||
from std_msgs.msg import Float64
|
||||
|
||||
|
||||
class OpenManipulatorRosBaseInterface(object):
|
||||
"""Open Manipulator Interface based on ROS."""
|
||||
|
||||
__metaclass__ = ABCMeta
|
||||
|
||||
def __init__(self, cfg):
|
||||
"""Initialization."""
|
||||
self.cfg = cfg
|
||||
self.train_mode = self.cfg["TRAIN_MODE"]
|
||||
|
||||
self.joint_speeds = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
self.joint_positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
self.joint_velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
self.joint_efforts = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
self.right_endpoint_position = [0, 0, 0]
|
||||
|
||||
self.termination_count = 0
|
||||
self.success_count = 0
|
||||
|
||||
self.init_publish_node()
|
||||
self.init_subscribe_node()
|
||||
self.init_robot_pose()
|
||||
|
||||
rospy.on_shutdown(self.delete_target_block)
|
||||
|
||||
def init_publish_node(self):
|
||||
# TODO: write docstring
|
||||
|
||||
self.pub_gripper_position = rospy.Publisher(
|
||||
"/open_manipulator/gripper_position/command", Float64, queue_size=1
|
||||
)
|
||||
self.pub_gripper_sub_position = rospy.Publisher(
|
||||
"/open_manipulator/gripper_sub_position/command", Float64, queue_size=1
|
||||
)
|
||||
self.pub_joint1_position = rospy.Publisher(
|
||||
"/open_manipulator/joint1_position/command", Float64, queue_size=1
|
||||
)
|
||||
self.pub_joint2_position = rospy.Publisher(
|
||||
"/open_manipulator/joint2_position/command", Float64, queue_size=1
|
||||
)
|
||||
self.pub_joint3_position = rospy.Publisher(
|
||||
"/open_manipulator/joint3_position/command", Float64, queue_size=1
|
||||
)
|
||||
self.pub_joint4_position = rospy.Publisher(
|
||||
"/open_manipulator/joint4_position/command", Float64, queue_size=1
|
||||
)
|
||||
|
||||
self.joints_position_cmd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
self.kinematics_cmd = [0.0, 0.0, 0.0]
|
||||
|
||||
def init_subscribe_node(self):
|
||||
# TODO: write docstring
|
||||
|
||||
self.sub_joint_state = rospy.Subscriber(
|
||||
"/open_manipulator/joint_states", JointState, self.joint_state_callback
|
||||
)
|
||||
self.sub_kinematics_pose = rospy.Subscriber(
|
||||
"/open_manipulator/gripper/kinematics_pose",
|
||||
KinematicsPose,
|
||||
self.kinematics_pose_callback,
|
||||
)
|
||||
self.sub_robot_state = rospy.Subscriber(
|
||||
"/open_manipulator/states", OpenManipulatorState, self.robot_state_callback
|
||||
)
|
||||
|
||||
self.joint_names = [
|
||||
"gripper",
|
||||
"gripper_sub",
|
||||
"joint1",
|
||||
"joint2",
|
||||
"joint3",
|
||||
"joint4",
|
||||
]
|
||||
self.joint_positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
self.joint_velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
self.joint_efforts = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
|
||||
self._gripper_position = [0.0, 0.0, 0.0]
|
||||
self._gripper_orientation = [0.0, 0.0, 0.0]
|
||||
self.distance_threshold = self.cfg["DISTANCE_THRESHOLD"]
|
||||
|
||||
self.moving_state = ""
|
||||
self.actuator_state = ""
|
||||
|
||||
def init_robot_pose(self):
|
||||
"""Initialize robot gripper and joints position."""
|
||||
self.pub_gripper_position.publish(np.random.uniform(0.0, 0.1))
|
||||
self.pub_joint1_position.publish(np.random.uniform(-0.1, 0.1))
|
||||
self.pub_joint2_position.publish(np.random.uniform(-0.1, 0.1))
|
||||
self.pub_joint3_position.publish(np.random.uniform(-0.1, 0.1))
|
||||
self.pub_joint4_position.publish(np.random.uniform(-0.1, 0.1))
|
||||
|
||||
def joint_state_callback(self, msg):
|
||||
"""Callback function of joint states subscriber.
|
||||
|
||||
Args:
|
||||
msg (JointState): Callback message contains joint state.
|
||||
"""
|
||||
joints_states = msg
|
||||
self.joint_names = joints_states.name
|
||||
self.joint_positions = joints_states.position
|
||||
self.joint_velocities = joints_states.velocity
|
||||
self.joint_efforts = joints_states.effort
|
||||
# penalize jerky motion in reward for shaped reward setting.
|
||||
self.squared_sum_vel = np.linalg.norm(np.array(self.joint_velocities))
|
||||
|
||||
def kinematics_pose_callback(self, msg):
|
||||
"""Callback function of gripper kinematic pose subscriber.
|
||||
|
||||
Args:
|
||||
msg (KinematicsPose): Callback message contains kinematics pose.
|
||||
"""
|
||||
self.kinematics_pose = msg
|
||||
_gripper_position = self.kinematics_pose.pose.position
|
||||
self._gripper_position = [
|
||||
_gripper_position.x,
|
||||
_gripper_position.y,
|
||||
_gripper_position.z,
|
||||
]
|
||||
_gripper_orientation = self.kinematics_pose.pose.orientation
|
||||
self._gripper_orientation = [
|
||||
_gripper_orientation.x,
|
||||
_gripper_orientation.y,
|
||||
_gripper_orientation.z,
|
||||
_gripper_orientation.w,
|
||||
]
|
||||
|
||||
def robot_state_callback(self, msg):
|
||||
"""Callback function of robot state subscriber.
|
||||
|
||||
Args:
|
||||
msg (states): Callback message contains openmanipulator's states.
|
||||
"""
|
||||
# "MOVING" / "STOPPED"
|
||||
self.moving_state = msg.open_manipulator_moving_state
|
||||
# "ACTUATOR_ENABLE" / "ACTUATOR_DISABLE"
|
||||
self.actuator_state = msg.open_manipulator_actuator_state
|
||||
|
||||
def check_robot_moving(self):
|
||||
"""Check if robot has reached its initial pose.
|
||||
|
||||
Returns:
|
||||
True if not stopped.
|
||||
"""
|
||||
while not rospy.is_shutdown():
|
||||
if self.moving_state == "STOPPED":
|
||||
break
|
||||
return True
|
||||
|
||||
@property
|
||||
def joints_states(self):
|
||||
"""Returns current joints states of robot including position, velocity, effort.
|
||||
|
||||
Returns:
|
||||
Tuple of JointState
|
||||
"""
|
||||
return self.joint_positions, self.joint_velocities, self.joint_efforts
|
||||
|
||||
@property
|
||||
def gripper_position(self):
|
||||
"""Returns gripper end effector position.
|
||||
|
||||
Returns:
|
||||
Position
|
||||
"""
|
||||
return self._gripper_position
|
||||
|
||||
@property
|
||||
def gripper_orientation(self):
|
||||
"""Returns gripper orientation.
|
||||
|
||||
Returns:
|
||||
Orientation
|
||||
"""
|
||||
return self._gripper_orientation
|
||||
|
||||
def get_observation(self):
|
||||
"""Get robot observation."""
|
||||
gripper_pos = np.array(self._gripper_position)
|
||||
gripper_ori = np.array(self._gripper_orientation)
|
||||
|
||||
# joint space
|
||||
robot_joint_angles = np.array(self.joint_positions)
|
||||
robot_joint_velocities = np.array(self.joint_velocities)
|
||||
robot_joint_efforts = np.array(self.joint_efforts)
|
||||
|
||||
obs = np.concatenate(
|
||||
(
|
||||
gripper_pos,
|
||||
gripper_ori,
|
||||
robot_joint_angles,
|
||||
robot_joint_velocities,
|
||||
robot_joint_efforts,
|
||||
)
|
||||
)
|
||||
return obs
|
||||
|
||||
def get_action_space(self):
|
||||
"""Return the open manipulator's action space for this specific environment."""
|
||||
control_mode = self.cfg["CONTROL_MODE"]
|
||||
|
||||
if control_mode == "position":
|
||||
joint_limits = self.cfg["JOINT_LIMITS"]
|
||||
|
||||
lower_bounds = np.array(
|
||||
[
|
||||
joint_limits["LOW"]["J1"],
|
||||
joint_limits["LOW"]["J2"],
|
||||
joint_limits["LOW"]["J3"],
|
||||
joint_limits["LOW"]["J4"],
|
||||
joint_limits["LOW"]["GRIP"],
|
||||
]
|
||||
)
|
||||
upper_bounds = np.array(
|
||||
[
|
||||
joint_limits["HIGH"]["J1"],
|
||||
joint_limits["HIGH"]["J2"],
|
||||
joint_limits["HIGH"]["J3"],
|
||||
joint_limits["HIGH"]["J4"],
|
||||
joint_limits["HIGH"]["GRIP"],
|
||||
]
|
||||
)
|
||||
elif control_mode == "velocity":
|
||||
raise NotImplementedError(
|
||||
"Control mode %s is not implemented yet." % control_mode
|
||||
)
|
||||
|
||||
elif control_mode == "effort":
|
||||
raise NotImplementedError(
|
||||
"Control mode %s is not implemented yet." % control_mode
|
||||
)
|
||||
else:
|
||||
raise ValueError("Control mode %s is not known!" % control_mode)
|
||||
print (lower_bounds, upper_bounds, self.cfg["ACTION_DIM"])
|
||||
return gym.spaces.Box(low=lower_bounds, high=upper_bounds, dtype=np.float32)
|
||||
|
||||
def get_observation_space(self):
|
||||
"""Return the open manipulator's state space for this specific environment."""
|
||||
return gym.spaces.Box(
|
||||
low=-np.inf,
|
||||
high=np.inf,
|
||||
shape=self.cfg["OBSERVATION_DIM"],
|
||||
dtype=np.float32,
|
||||
)
|
||||
|
||||
def set_joints_position(self, joints_angles):
|
||||
"""Move joints using joint position command publishers."""
|
||||
self.pub_gripper_position.publish(joints_angles[0])
|
||||
self.pub_joint1_position.publish(joints_angles[1])
|
||||
self.pub_joint2_position.publish(joints_angles[2])
|
||||
self.pub_joint3_position.publish(joints_angles[3])
|
||||
self.pub_joint4_position.publish(joints_angles[4])
|
||||
|
||||
def _geom_interpolation(self, in_rad, out_rad, in_z, out_z, query):
|
||||
"""interpolates along the outer shell of work space, based on z-position.
|
||||
|
||||
must feed the corresponding radius from inner radius.
|
||||
"""
|
||||
slope = (out_z - in_z) / (out_rad - in_rad)
|
||||
intercept = in_z
|
||||
return slope * (query - in_rad) + intercept
|
||||
|
||||
def check_for_success(self):
|
||||
"""Check if the agent has succeeded the episode.
|
||||
|
||||
Returns:
|
||||
True when count reaches suc_count, else False.
|
||||
"""
|
||||
dist = self.get_dist()
|
||||
if dist < self.distance_threshold:
|
||||
self.success_count += 1
|
||||
if self.success_count == self.cfg["SUCCESS_COUNT"]:
|
||||
self.done = True
|
||||
self.success_count = 0
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
else:
|
||||
return False
|
||||
|
||||
def check_for_termination(self):
|
||||
"""Check if the agent has reached undesirable state.
|
||||
|
||||
If so, terminate the episode early.
|
||||
|
||||
Returns:
|
||||
True when count reaches term_count, else False.
|
||||
"""
|
||||
_ee_pose = self._gripper_position
|
||||
|
||||
inner_rad, outer_rad, lower_rad, inner_z, outer_z, lower_z, term_count = (
|
||||
self.cfg["INNER_RADIAN"],
|
||||
self.cfg["OUTER_RADIAN"],
|
||||
self.cfg["LOWER_RADIAN"],
|
||||
self.cfg["INNER_Z"],
|
||||
self.cfg["OUTER_Z"],
|
||||
self.cfg["LOWER_Z"],
|
||||
self.cfg["TERM_COUNT"],
|
||||
)
|
||||
|
||||
rob_rad = np.linalg.norm([_ee_pose[0], _ee_pose[1]])
|
||||
rob_z = _ee_pose[2]
|
||||
if self.joint_positions[0] <= abs(self.cfg["JOINT_LIMITS"]["HIGH"]["J1"] / 2):
|
||||
if rob_rad < self.cfg["INNER_RADIAN"]:
|
||||
self.termination_count += 1
|
||||
rospy.logwarn("OUT OF BOUNDARY : exceeds inner radius limit")
|
||||
elif self.cfg["INNER_RADIAN"] <= rob_rad < self.cfg["OUTER_RADIAN"]:
|
||||
upper_z = self._geom_interpolation(
|
||||
inner_rad, outer_rad, inner_z, outer_z, rob_rad
|
||||
)
|
||||
if rob_z > upper_z:
|
||||
self.termination_count += 1
|
||||
rospy.logwarn("OUT OF BOUNDARY : exceeds upper z limit")
|
||||
elif outer_rad <= rob_rad < lower_rad:
|
||||
bevel_z = self._geom_interpolation(
|
||||
outer_rad, lower_rad, outer_z, lower_z, rob_rad
|
||||
)
|
||||
if rob_z > bevel_z:
|
||||
self.termination_count += 1
|
||||
rospy.logwarn("OUT OF BOUNDARY : exceeds bevel z limit")
|
||||
else:
|
||||
self.termination_count += 1
|
||||
rospy.logwarn("OUT OF BOUNDARY : exceeds outer radius limit")
|
||||
else:
|
||||
# joint_1 limit exceeds
|
||||
self.termination_count += 1
|
||||
rospy.logwarn("OUT OF BOUNDARY : joint_1_limit exceeds")
|
||||
|
||||
if self.termination_count == term_count:
|
||||
self.done = True
|
||||
self.termination_count = 0
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
|
||||
def close(self):
|
||||
"""Close by rospy shutdown."""
|
||||
rospy.signal_shutdown("done")
|
||||
|
||||
|
||||
class OpenManipulatorRosGazeboInterface(OpenManipulatorRosBaseInterface):
|
||||
# TODO: write docstring
|
||||
"""Open Manipulator Interface based on ROS for Gazebo."""
|
||||
|
||||
def __init__(self, cfg):
|
||||
rospy.init_node("OpenManipulatorRosGazeboInterface")
|
||||
super(OpenManipulatorRosGazeboInterface, self).__init__(cfg)
|
||||
|
||||
def reset_gazebo_world(self, block_pose=None):
|
||||
"""Initialize randomly the state of robot agent and surrounding envs (including target obj.)."""
|
||||
if block_pose is not None:
|
||||
assert self.train_mode is True
|
||||
|
||||
self.delete_target_block()
|
||||
|
||||
self.pub_gripper_position.publish(np.random.uniform(0.0, 0.1))
|
||||
self.pub_joint1_position.publish(np.random.uniform(-0.1, 0.1))
|
||||
self.pub_joint2_position.publish(np.random.uniform(-0.1, 0.1))
|
||||
self.pub_joint3_position.publish(np.random.uniform(-0.1, 0.1))
|
||||
self.pub_joint4_position.publish(np.random.uniform(-0.1, 0.1))
|
||||
|
||||
self.set_target_block(block_pose)
|
||||
|
||||
def set_target_block(self, block_pose=None):
|
||||
"""Set target block Gazebo model"""
|
||||
# random generated blocks for train
|
||||
if block_pose is None:
|
||||
polar_rad, polar_theta, overhead_orientation = (
|
||||
np.random.uniform(*self.cfg["POLAR_RADIAN_BOUNDARY"]),
|
||||
np.random.uniform(*self.cfg["POLAR_THETA_BOUNDARY"]),
|
||||
self.cfg["OVERHEAD_ORIENTATION"],
|
||||
)
|
||||
|
||||
block_pose = Pose()
|
||||
block_pose.position.x = polar_rad * cos(polar_theta)
|
||||
block_pose.position.y = polar_rad * sin(polar_theta)
|
||||
block_pose.position.z = np.random.uniform(0.05, 0.28)
|
||||
block_pose.orientation = overhead_orientation
|
||||
|
||||
block_reference_frame = "world"
|
||||
model_path = rospkg.RosPack().get_path("kair_algorithms") + "/urdf/"
|
||||
|
||||
with open(model_path + "block/model.urdf", "r") as block_file:
|
||||
block_xml = block_file.read().replace("\n", "")
|
||||
|
||||
rospy.wait_for_service("/gazebo/spawn_urdf_model")
|
||||
|
||||
try:
|
||||
spawn_urdf = rospy.ServiceProxy("/gazebo/spawn_urdf_model", SpawnModel)
|
||||
spawn_urdf("block", block_xml, "/", block_pose, block_reference_frame)
|
||||
except rospy.ServiceException as e:
|
||||
rospy.logerr("Spawn URDF service call failed: {0}".format(e))
|
||||
|
||||
def delete_target_block(self):
|
||||
"""This will be called on ROS Exit, deleting Gazebo models.
|
||||
|
||||
Do not wait for the Gazebo Delete Model service, since
|
||||
Gazebo should already be running. If the service is not
|
||||
available since Gazebo has been killed, it is fine to error out
|
||||
"""
|
||||
try:
|
||||
delete_model = rospy.ServiceProxy("/gazebo/delete_model", DeleteModel)
|
||||
delete_model("block")
|
||||
except rospy.ServiceException as e:
|
||||
rospy.loginfo("Delete Model service call failed: {0}".format(e))
|
||||
|
||||
def get_dist(self):
|
||||
"""Get distance between end effector pose and object pose.
|
||||
|
||||
Returns:
|
||||
L2 norm of end effector pose and object pose.
|
||||
"""
|
||||
rospy.wait_for_service("/gazebo/get_model_state")
|
||||
|
||||
try:
|
||||
object_state_srv = rospy.ServiceProxy(
|
||||
"/gazebo/get_model_state", GetModelState
|
||||
)
|
||||
object_state = object_state_srv("block", "world")
|
||||
object_pose = [
|
||||
object_state.pose.position.x,
|
||||
object_state.pose.position.y,
|
||||
object_state.pose.position.z,
|
||||
]
|
||||
self._obj_pose = np.array(object_pose)
|
||||
except rospy.ServiceException as e:
|
||||
rospy.logerr("Spawn URDF service call failed: {0}".format(e))
|
||||
|
||||
# FK state of robot
|
||||
end_effector_pose = np.array(self._gripper_position)
|
||||
|
||||
return np.linalg.norm(end_effector_pose - self._obj_pose)
|
||||
|
||||
|
||||
class OpenManipulatorRosRealInterface(OpenManipulatorRosBaseInterface):
|
||||
# TODO: write docstring
|
||||
"""Open Manipulator Interface based on ROS for real environment."""
|
||||
|
||||
def __init__(self, cfg):
|
||||
rospy.init_node("OpenManipulatorRosRealInterface")
|
||||
super(OpenManipulatorRosRealInterface, self).__init__(cfg)
|
||||
@@ -5,26 +5,26 @@ from math import cos, pi, sin
|
||||
import numpy as np
|
||||
|
||||
import rospy
|
||||
from envs.open_manipulator import OpenManipulatorEnv
|
||||
from config.environment import open_manipulator
|
||||
from envs.open_manipulator import OpenManipulatorReacherEnv
|
||||
from geometry_msgs.msg import Pose, Quaternion
|
||||
from open_manipulator_msgs.msg import JointPosition, KinematicsPose
|
||||
from open_manipulator_msgs.srv import SetJointPosition, SetKinematicsPose
|
||||
|
||||
overhead_orientation = Quaternion(
|
||||
x=-0.00142460053167,
|
||||
y=0.999994209902,
|
||||
z=-0.00177030764765,
|
||||
w=0.00253311793936)
|
||||
x=-0.00142460053167, y=0.999994209902, z=-0.00177030764765, w=0.00253311793936
|
||||
)
|
||||
|
||||
cfg = open_manipulator
|
||||
|
||||
|
||||
def test_reset():
|
||||
env = OpenManipulatorEnv()
|
||||
env = OpenManipulatorReacherEnv(cfg)
|
||||
_ = env.reset()
|
||||
# assert obs in specific boundary
|
||||
|
||||
|
||||
def test_forward():
|
||||
env = OpenManipulatorEnv()
|
||||
env = OpenManipulatorReacherEnv(cfg)
|
||||
_ = env.reset()
|
||||
_pose = Pose()
|
||||
_pose.position.x = 0.4
|
||||
@@ -40,7 +40,9 @@ def test_forward():
|
||||
forward_pose.max_velocity_scaling_factor = 0.0
|
||||
forward_pose.tolerance = 0.0
|
||||
try:
|
||||
task_space_srv = rospy.ServiceProxy('/open_manipulator/goal_task_space_path', SetKinematicsPose)
|
||||
task_space_srv = rospy.ServiceProxy(
|
||||
"/open_manipulator/goal_task_space_path", SetKinematicsPose
|
||||
)
|
||||
_ = task_space_srv("arm", "gripper", forward_pose, 2.0)
|
||||
except rospy.ServiceException as e:
|
||||
rospy.loginfo("Path planning service call failed: {0}".format(e))
|
||||
@@ -48,12 +50,14 @@ def test_forward():
|
||||
|
||||
def test_rotate():
|
||||
_qpose = JointPosition()
|
||||
_qpose.joint_name = ['joint1', 'joint2', 'joint3', 'joint4']
|
||||
_qpose.joint_name = ["joint1", "joint2", "joint3", "joint4"]
|
||||
_qpose.position = [0.5, 0.0, 0.0, 0.5]
|
||||
_qpose.max_accelerations_scaling_factor = 0.0
|
||||
_qpose.max_velocity_scaling_factor = 0.0
|
||||
try:
|
||||
task_space_srv = rospy.ServiceProxy('/open_manipulator/goal_joint_space_path_from_present', SetJointPosition)
|
||||
task_space_srv = rospy.ServiceProxy(
|
||||
"/open_manipulator/goal_joint_space_path_from_present", SetJointPosition
|
||||
)
|
||||
_ = task_space_srv("arm", _qpose, 2.0)
|
||||
except rospy.ServiceException, e:
|
||||
rospy.loginfo("Path planning service call failed: {0}".format(e))
|
||||
@@ -63,36 +67,33 @@ def test_rotate():
|
||||
_ = task_space_srv("arm", _qpose, 2.0)
|
||||
except rospy.ServiceException, e:
|
||||
rospy.loginfo("Path planning service call failed: {0}".format(e))
|
||||
# define actions
|
||||
# assert obs in specific boundary
|
||||
|
||||
|
||||
def test_block_loc():
|
||||
env = OpenManipulatorEnv()
|
||||
env = OpenManipulatorReacherEnv(cfg)
|
||||
for iter in range(20):
|
||||
b_pose = Pose()
|
||||
b_pose.position.x = np.random.uniform(0.15, .20)
|
||||
b_pose.position.x = np.random.uniform(0.15, 0.20)
|
||||
b_pose.position.y = np.random.uniform(-0.2, 0.2)
|
||||
b_pose.position.z = 0.00
|
||||
b_pose.orientation = overhead_orientation
|
||||
env._load_target_block(block_pose=b_pose)
|
||||
env.ros_interface.set_target_block()
|
||||
rospy.sleep(2.0)
|
||||
env._delete_target_block()
|
||||
# block generation code
|
||||
# assert block in specific boundary (gripper's movable area)
|
||||
env.ros_interface.delete_target_block()
|
||||
|
||||
|
||||
def test_achieve_goal():
|
||||
env = OpenManipulatorEnv()
|
||||
env = OpenManipulatorReacherEnv(cfg)
|
||||
for iter in range(20):
|
||||
b_pose = Pose()
|
||||
b_pose.position.x = np.random.uniform(0.25, .6)
|
||||
b_pose.position.y = np.random.uniform(-0.4, 0.4)
|
||||
b_pose.position.z = 0.00
|
||||
b_pose.orientation = overhead_orientation
|
||||
env._load_target_block(block_pose=b_pose)
|
||||
block_pose = Pose()
|
||||
block_pose.position.x = np.random.uniform(0.25, 0.6)
|
||||
block_pose.position.y = np.random.uniform(-0.4, 0.4)
|
||||
block_pose.position.z = 0.00
|
||||
block_pose.orientation = overhead_orientation
|
||||
env.ros_interface.set_target_block(block_pose)
|
||||
|
||||
r_pose = Pose()
|
||||
r_pose.position = b_pose.position
|
||||
r_pose.position = block_pose.position
|
||||
r_pose.position.z = 0.08
|
||||
forward_pose = KinematicsPose()
|
||||
forward_pose.pose = r_pose
|
||||
@@ -100,49 +101,49 @@ def test_achieve_goal():
|
||||
forward_pose.max_velocity_scaling_factor = 0.0
|
||||
forward_pose.tolerance = 0.0
|
||||
try:
|
||||
task_space_srv = rospy.ServiceProxy('/open_manipulator/goal_task_space_path', SetKinematicsPose)
|
||||
task_space_srv = rospy.ServiceProxy(
|
||||
"/open_manipulator/goal_task_space_path", SetKinematicsPose
|
||||
)
|
||||
_ = task_space_srv("arm", "gripper", forward_pose, 2.0)
|
||||
except rospy.ServiceException, e:
|
||||
rospy.loginfo("Path planning service call failed: {0}".format(e))
|
||||
rospy.sleep(5.0)
|
||||
env._delete_target_block()
|
||||
env.ros_interface.delete_target_block()
|
||||
|
||||
|
||||
def test_workspace_limit():
|
||||
""" TODO: add static block
|
||||
"""
|
||||
env = OpenManipulatorEnv()
|
||||
env = OpenManipulatorReacherEnv(cfg)
|
||||
for iter in range(100):
|
||||
_polar_rad = np.random.uniform(0.134, 0.32)
|
||||
_polar_theta = np.random.uniform(-pi * 0.7 / 4, pi * 0.7 / 4)
|
||||
|
||||
b_pose = Pose()
|
||||
b_pose.position.x = _polar_rad * cos(_polar_theta)
|
||||
b_pose.position.y = _polar_rad * sin(_polar_theta)
|
||||
b_pose.position.z = np.random.uniform(0.05, 0.28)
|
||||
b_pose.orientation = overhead_orientation
|
||||
env._load_target_block(block_pose=b_pose)
|
||||
block_pose = Pose()
|
||||
block_pose.position.x = _polar_rad * cos(_polar_theta)
|
||||
block_pose.position.y = _polar_rad * sin(_polar_theta)
|
||||
block_pose.position.z = np.random.uniform(0.05, 0.28)
|
||||
block_pose.orientation = overhead_orientation
|
||||
env.ros_interface.set_target_block(block_pose)
|
||||
|
||||
r_pose = Pose()
|
||||
r_pose.position = b_pose.position
|
||||
r_pose.position = block_pose.position
|
||||
forward_pose = KinematicsPose()
|
||||
forward_pose.pose = r_pose
|
||||
forward_pose.max_accelerations_scaling_factor = 0.0
|
||||
forward_pose.max_velocity_scaling_factor = 0.0
|
||||
forward_pose.tolerance = 0.0
|
||||
try:
|
||||
task_space_srv = rospy.ServiceProxy('/open_manipulator/goal_task_space_path', SetKinematicsPose)
|
||||
task_space_srv = rospy.ServiceProxy(
|
||||
"/open_manipulator/goal_task_space_path", SetKinematicsPose
|
||||
)
|
||||
_ = task_space_srv("arm", "gripper", forward_pose, 3.0)
|
||||
except rospy.ServiceException, e:
|
||||
rospy.loginfo("Path planning service call failed: {0}".format(e))
|
||||
rospy.sleep(3.0)
|
||||
env._check_for_termination()
|
||||
env._delete_target_block()
|
||||
# define actions
|
||||
# define goal
|
||||
# assert gripper reach goal
|
||||
env.ros_interface.check_for_termination()
|
||||
env.ros_interface.delete_target_block()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
# test_reset()
|
||||
# test_forward()
|
||||
# test_rotate()
|
||||
|
||||
@@ -56,16 +56,19 @@ def main():
|
||||
"""Main."""
|
||||
# env initialization
|
||||
env = gym.make("LunarLanderContinuous-v2")
|
||||
state_dim = env.observation_space.shape[0]
|
||||
action_dim = env.action_space.shape[0]
|
||||
|
||||
# set a random seed
|
||||
common_utils.set_random_seed(args.seed, env)
|
||||
|
||||
# run
|
||||
module_path = "examples.lunarlander_continuous_v2." + args.algo
|
||||
example = importlib.import_module(module_path)
|
||||
example.run(env, args, state_dim, action_dim)
|
||||
module_path = "config.agent.lunarlander_continuous_v2." + args.algo
|
||||
agent = importlib.import_module(module_path)
|
||||
agent = agent.get(env, args)
|
||||
|
||||
# run
|
||||
if args.test:
|
||||
agent.test()
|
||||
else:
|
||||
agent.train()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
@@ -11,7 +11,8 @@ import argparse
|
||||
import importlib
|
||||
|
||||
import algorithms.common.helper_functions as common_utils
|
||||
from envs.open_manipulator.open_manipulator import OpenManipulatorEnv
|
||||
from config.environment.open_manipulator import config as env_cfg
|
||||
from envs.open_manipulator.open_manipulator_reacher_env import OpenManipulatorReacherEnv
|
||||
|
||||
# configurations
|
||||
parser = argparse.ArgumentParser(description="Pytorch RL algorithms")
|
||||
@@ -54,18 +55,21 @@ args = parser.parse_args()
|
||||
def main():
|
||||
"""Main."""
|
||||
# env initialization
|
||||
env = OpenManipulatorEnv()
|
||||
# env = gym.make("Omreacher-v0")
|
||||
# TODO: uncomment here.
|
||||
state_dim = env.observation_space.shape[0]
|
||||
action_dim = env.action_space.shape[0]
|
||||
env = OpenManipulatorReacherEnv(env_cfg)
|
||||
|
||||
# set a random seed
|
||||
# common_utils.set_random_seed(args.seed, env)
|
||||
common_utils.set_random_seed(args.seed, env)
|
||||
|
||||
# agent initialization
|
||||
module_path = "config.agent.open_manipulator_reacher_v0." + args.algo
|
||||
agent = importlib.import_module(module_path)
|
||||
agent = agent.get(env, args)
|
||||
|
||||
# run
|
||||
module_path = "examples.open_manipulator_reacher_v0." + args.algo
|
||||
example = importlib.import_module(module_path)
|
||||
example.run(env, args, state_dim, action_dim)
|
||||
if args.test:
|
||||
agent.test()
|
||||
else:
|
||||
agent.train()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
@@ -54,16 +54,20 @@ def main():
|
||||
"""Main."""
|
||||
# env initialization
|
||||
env = gym.make("Reacher-v1")
|
||||
state_dim = env.observation_space.shape[0]
|
||||
action_dim = env.action_space.shape[0]
|
||||
|
||||
# set a random seed
|
||||
common_utils.set_random_seed(args.seed, env)
|
||||
|
||||
# agent initialization
|
||||
module_path = "config.agent.reacher-v1." + args.algo
|
||||
agent = importlib.import_module(module_path)
|
||||
agent = agent.get(env, args)
|
||||
|
||||
# run
|
||||
module_path = "examples.reacher-v1." + args.algo
|
||||
example = importlib.import_module(module_path)
|
||||
example.run(env, args, state_dim, action_dim)
|
||||
if args.test:
|
||||
agent.test()
|
||||
else:
|
||||
agent.train()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
Reference in New Issue
Block a user