From 94c83d037e9e1939df4954eaff2effda1c5d32be Mon Sep 17 00:00:00 2001 From: Cheolhui Date: Sun, 31 Mar 2019 20:23:35 +0900 Subject: [PATCH] Test the training loop with td3 baseline --- scripts/algorithms/common/abstract/agent.py | 2 +- .../envs/open_manipulator/open_manipulator.py | 87 ++++++++++++++++--- scripts/run_open_manipulator_reacher_v0.py | 3 +- 3 files changed, 79 insertions(+), 13 deletions(-) mode change 100644 => 100755 scripts/algorithms/common/abstract/agent.py diff --git a/scripts/algorithms/common/abstract/agent.py b/scripts/algorithms/common/abstract/agent.py old mode 100644 new mode 100755 index 9c802c6..02d75f2 --- a/scripts/algorithms/common/abstract/agent.py +++ b/scripts/algorithms/common/abstract/agent.py @@ -44,7 +44,7 @@ class AbstractAgent(object): self.args.max_episode_steps = env._max_episode_steps # for logging - self.env_name = str(self.env.env).split("<")[2].replace(">>", "") + self.env_name = str(self.env.env).split("<")[1].replace(">>", "") self.sha = ( subprocess.check_output(["git", "rev-parse", "--short", "HEAD"])[:-1] .decode("ascii") diff --git a/scripts/envs/open_manipulator/open_manipulator.py b/scripts/envs/open_manipulator/open_manipulator.py index a47a34e..1841f3a 100755 --- a/scripts/envs/open_manipulator/open_manipulator.py +++ b/scripts/envs/open_manipulator/open_manipulator.py @@ -1,5 +1,6 @@ #! /usr/bin/env python +import gym import copy import math import os @@ -32,8 +33,11 @@ overhead_orientation = Quaternion( ) # 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 }} +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 }} + +# + cartesian_limits = {} # safe cartesian limits @@ -92,6 +96,7 @@ class OpenManipulatorEnv: 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 @@ -161,10 +166,72 @@ class OpenManipulatorEnv: 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) - def render(self): + @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): @@ -232,11 +299,10 @@ class OpenManipulatorEnv: """ # 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]) + 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. @@ -275,7 +341,8 @@ class OpenManipulatorEnv: print("Current EE pos: ", self.gripper_position) print("Actions: ", act) - obs = np.array([_joint_pos, _joint_vels, _joint_effos]) + + obs = self.get_observation() info = '' return obs, self.reward_rescale * self.reward, self.done, info @@ -289,7 +356,7 @@ class OpenManipulatorEnv: 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]) + obs = self.get_observation() return obs diff --git a/scripts/run_open_manipulator_reacher_v0.py b/scripts/run_open_manipulator_reacher_v0.py index 0c62e3b..fe60dc3 100755 --- a/scripts/run_open_manipulator_reacher_v0.py +++ b/scripts/run_open_manipulator_reacher_v0.py @@ -59,9 +59,8 @@ def main(): # 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) + # common_utils.set_random_seed(args.seed, env) # run module_path = "examples.open_manipulator_reacher_v0." + args.algo