wassname/kair_algorithms_draft
1
0
Fork 0
mirror of https://github.com/wassname/kair_algorithms_draft.git synced 2026-09-09 11:25:10 +08:00
Code Issues Packages Projects Releases Wiki Activity

Refactor OpenManipulator env class (#46)

Browse Source 
* 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
2019-04-09 20:45:21 +09:00
committed by whikwon
parent 7ec5c23c4e
commit 37e9697b1b
28 changed files with 828 additions and 710 deletions
Download Patch File Download Diff File Expand all files Collapse all files
scripts/envs/__init__.py
+3
View File
@@ -0,0 +1,3 @@
from .open_manipulator import OpenManipulatorReacherEnv
__all__ = ["OpenManipulatorReacherEnv"]
scripts/envs/open_manipulator/__init__.py
+3
View File
@@ -0,0 +1,3 @@
from .open_manipulator_reacher_env import OpenManipulatorReacherEnv
__all__ = ["OpenManipulatorReacherEnv"]
scripts/envs/open_manipulator/open_manipulator.py
-523
View File
@@ -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")
scripts/envs/open_manipulator/open_manipulator_reacher_env.py
Executable
+134
View File
@@ -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
scripts/envs/open_manipulator/ros_interface.py
Executable
+458
View File
@@ -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)