From 865268de4e83d6a784fd482271532ab22e7f4d94 Mon Sep 17 00:00:00 2001 From: whikwon Date: Fri, 9 Aug 2019 23:08:27 -0700 Subject: [PATCH] temp saving --- scripts/config/environment/open_manipulator.py | 2 +- .../open_manipulator_reacher_env.py | 1 + scripts/envs/open_manipulator/ros_interface.py | 17 +++++++---------- 3 files changed, 9 insertions(+), 11 deletions(-) diff --git a/scripts/config/environment/open_manipulator.py b/scripts/config/environment/open_manipulator.py index 7d22f43..31f9823 100755 --- a/scripts/config/environment/open_manipulator.py +++ b/scripts/config/environment/open_manipulator.py @@ -32,7 +32,7 @@ config = { }, # Global variables "ACTION_DIM": 5, # Cartesian - "OBSERVATION_DIM": (25,), + "OBSERVATION_DIM": (21,), # terminal condition "INNER_RADIAN": 0.134, "OUTER_RADIAN": 0.3, diff --git a/scripts/envs/open_manipulator/open_manipulator_reacher_env.py b/scripts/envs/open_manipulator/open_manipulator_reacher_env.py index 17485b3..f4c9c5d 100755 --- a/scripts/envs/open_manipulator/open_manipulator_reacher_env.py +++ b/scripts/envs/open_manipulator/open_manipulator_reacher_env.py @@ -106,6 +106,7 @@ class OpenManipulatorReacherEnv(gym.Env): """ self.ros_interface.reset_gazebo_world() obs = self.ros_interface.get_observation() + print("RESET:", obs) return obs diff --git a/scripts/envs/open_manipulator/ros_interface.py b/scripts/envs/open_manipulator/ros_interface.py index ddea996..be71ae5 100755 --- a/scripts/envs/open_manipulator/ros_interface.py +++ b/scripts/envs/open_manipulator/ros_interface.py @@ -17,6 +17,7 @@ from open_manipulator_msgs.msg import KinematicsPose, OpenManipulatorState from pykdl_utils.kdl_kinematics import KDLKinematics from sensor_msgs.msg import JointState from std_msgs.msg import Float64 +from std_srvs.srv import Empty from urdf_parser_py.urdf import URDF # noqa @@ -113,11 +114,7 @@ class OpenManipulatorRosBaseInterface(object): def init_robot_pose(self): """Initialize robot gripper and joints position.""" - self.pub_gripper_position.publish(np.random.uniform(0.0, 0.0)) - self.pub_joint1_position.publish(np.random.uniform(0.0, 0.0)) - self.pub_joint2_position.publish(np.random.uniform(0.0, 0.0)) - self.pub_joint3_position.publish(np.random.uniform(0.0, 0.0)) - self.pub_joint4_position.publish(np.random.uniform(0.0, 0.0)) + rospy.ServiceProxy("/gazebo/reset_simulation", Empty) def init_fk_solver(self): self.robot = URDF.from_parameter_server() @@ -211,8 +208,8 @@ class OpenManipulatorRosBaseInterface(object): def get_observation(self): """Get robot observation.""" - gripper_pos = np.array(self._gripper_position) - gripper_ori = np.array(self._gripper_orientation) +# gripper_pos = np.array(self._gripper_position) +# gripper_ori = np.array(self._gripper_orientation) # joint space robot_joint_angles = np.array(self.joint_positions) @@ -221,11 +218,12 @@ class OpenManipulatorRosBaseInterface(object): obs = np.concatenate( ( - gripper_pos, - gripper_ori, +# gripper_pos, +# gripper_ori, robot_joint_angles, robot_joint_velocities, robot_joint_efforts, + self.block_pose, ) ) return obs @@ -388,7 +386,6 @@ class OpenManipulatorRosGazeboInterface(OpenManipulatorRosBaseInterface): # self.delete_target_block() self.init_robot_pose() self.success_count = 0 - time.sleep(0.5) self.set_target_block(block_pose)