mirror of
https://github.com/wassname/kair_algorithms_draft.git
synced 2026-09-09 11:25:10 +08:00
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
This commit is contained in:
Cheolhui
+456
@@ -0,0 +1,456 @@
|
||||
#! /usr/bin/env python
|
||||
|
||||
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 },
|
||||
'lo':{'j1':-pi*0.9, 'j2':-pi*0.57, 'j3':-pi*0.3, 'j4':-pi*0.57 }}
|
||||
|
||||
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.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()
|
||||
rospy.on_shutdown(self._delete_target_block)
|
||||
|
||||
|
||||
def render(self):
|
||||
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_gripper_sub_position.publish(joints_angles[1])
|
||||
self.pub_joint1_position.publish(joints_angles[2])
|
||||
self.pub_joint2_position.publish(joints_angles[3])
|
||||
self.pub_joint3_position.publish(joints_angles[4])
|
||||
self.pub_joint4_position.publish(joints_angles[5])
|
||||
|
||||
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 = np.array([_joint_pos, _joint_vels, _joint_effos])
|
||||
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 = np.array([_joint_pos, _joint_vels, _joint_effos])
|
||||
|
||||
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")
|
||||
+131
@@ -0,0 +1,131 @@
|
||||
# -*- 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,
|
||||
}
|
||||
|
||||
|
||||
def run(env, args, state_dim, action_dim):
|
||||
"""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]
|
||||
|
||||
# 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
|
||||
agent = Agent(env, args, hyper_params, models, optims, noises)
|
||||
|
||||
# run
|
||||
if args.test:
|
||||
agent.test()
|
||||
else:
|
||||
agent.train()
|
||||
Executable
+151
@@ -0,0 +1,151 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
from math import cos, pi, sin
|
||||
|
||||
import numpy as np
|
||||
|
||||
import rospy
|
||||
from envs.open_manipulator import OpenManipulatorEnv
|
||||
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 = OpenManipulatorEnv()
|
||||
_ = env.reset()
|
||||
# assert obs in specific boundary
|
||||
|
||||
|
||||
def test_forward():
|
||||
env = OpenManipulatorEnv()
|
||||
_ = 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))
|
||||
# define actions
|
||||
# assert obs in specific boundary
|
||||
|
||||
|
||||
def test_block_loc():
|
||||
env = OpenManipulatorEnv()
|
||||
for iter in range(20):
|
||||
b_pose = Pose()
|
||||
b_pose.position.x = np.random.uniform(0.15, .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)
|
||||
rospy.sleep(2.0)
|
||||
env._delete_target_block()
|
||||
# block generation code
|
||||
# assert block in specific boundary (gripper's movable area)
|
||||
|
||||
|
||||
def test_achieve_goal():
|
||||
env = OpenManipulatorEnv()
|
||||
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)
|
||||
r_pose = Pose()
|
||||
r_pose.position = b_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._delete_target_block()
|
||||
|
||||
|
||||
def test_workspace_limit():
|
||||
""" TODO: add static block
|
||||
"""
|
||||
env = OpenManipulatorEnv()
|
||||
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)
|
||||
r_pose = Pose()
|
||||
r_pose.position = b_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._check_for_termination()
|
||||
env._delete_target_block()
|
||||
# define actions
|
||||
# define goal
|
||||
# assert gripper reach goal
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
# test_reset()
|
||||
# test_forward()
|
||||
# test_rotate()
|
||||
# test_block_loc()
|
||||
# test_achieve_goal()
|
||||
test_workspace_limit()
|
||||
Executable
+73
@@ -0,0 +1,73 @@
|
||||
#! /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 envs.open_manipulator.open_manipulator import OpenManipulatorEnv
|
||||
|
||||
# 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 = OpenManipulatorEnv()
|
||||
# env = gym.make("Omreacher-v0")
|
||||
# TODO: uncomment here.
|
||||
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.open_manipulator_reacher_v0." + args.algo
|
||||
example = importlib.import_module(module_path)
|
||||
example.run(env, args, state_dim, action_dim)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -9,21 +9,22 @@
|
||||
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0.025 0.025 0.025"/>
|
||||
<origin xyz="0.0001 0.0001 0.0001"/>
|
||||
<geometry>
|
||||
<box size="0.045 0.045 0.045" />
|
||||
<box size="0.05 0.05 0.05" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0.025 0.025 0.025"/>
|
||||
<origin xyz="0.0001 0.0001 0.0001"/>
|
||||
<geometry>
|
||||
<box size="0.045 0.045 0.045" />
|
||||
<box size="0.0001 0.0001 0.0001" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<gazebo reference="block">
|
||||
<material>Gazebo/Red</material>
|
||||
<gazebo>
|
||||
<static>true</static>
|
||||
<material>Gazebo/Blue</material>
|
||||
<mu1>1000</mu1>
|
||||
<mu2>1000</mu2>
|
||||
</gazebo>
|
||||
</robot>
|
||||
</robot>
|
||||
Reference in New Issue
Block a user