From 5601e310aa84ccc8c64d6f58dbe5674f5b06a6b3 Mon Sep 17 00:00:00 2001 From: mch5048 Date: Sat, 10 Aug 2019 17:25:10 +0900 Subject: [PATCH] fix init pose --- .../open_manipulator/open_manipulator_reacher_env.py | 1 + scripts/envs/open_manipulator/ros_interface.py | 10 +++++++++- 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/scripts/envs/open_manipulator/open_manipulator_reacher_env.py b/scripts/envs/open_manipulator/open_manipulator_reacher_env.py index f4c9c5d..8de4154 100755 --- a/scripts/envs/open_manipulator/open_manipulator_reacher_env.py +++ b/scripts/envs/open_manipulator/open_manipulator_reacher_env.py @@ -1,5 +1,6 @@ #! usr/bin/env python +import time import gym import numpy as np from gym.utils import seeding diff --git a/scripts/envs/open_manipulator/ros_interface.py b/scripts/envs/open_manipulator/ros_interface.py index be71ae5..f33728e 100755 --- a/scripts/envs/open_manipulator/ros_interface.py +++ b/scripts/envs/open_manipulator/ros_interface.py @@ -114,7 +114,15 @@ class OpenManipulatorRosBaseInterface(object): def init_robot_pose(self): """Initialize robot gripper and joints position.""" - rospy.ServiceProxy("/gazebo/reset_simulation", Empty) + rospy.wait_for_service("/gazebo/reset_simulation") + rospy.ServiceProxy("/gazebo/reset_simulation", Empty)() + for i in range(2000): + 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)) + time.sleep(5) def init_fk_solver(self): self.robot = URDF.from_parameter_server()