def reset(self): self._step_counter = 0 self.terminated = False self.n_contacts = 0 if not self._first_reset_flag: # print('first reset,loading urdf...') p.resetSimulation() self._first_reset_flag = True p.setPhysicsEngineParameter(numSolverIterations=150) p.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf"), [0, 0, 0]) p.setGravity(0, 0, GRAVITY) self._inmoov = inmoov.Inmoov(urdf_path=self.urdf_path, positional_control=True) self._inmoov_id = self._inmoov.inmoov_id self._tomato_id = p.loadURDF( os.path.join(self.urdf_path, "tomato_plant.urdf"), [0.4, 0.4, 0.5]) print('fast reset,resetting robot joints') self._inmoov.reset_joints() # p.resetSimulation() # p.setPhysicsEngineParameter(numSolverIterations=150) # p.setGravity(0., 0., GRAVITY) return self.get_observation()
def reset(self): self.terminated = False self.n_contacts = 0 self.n_steps_outside = 0 p.resetSimulation() p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timestep) self._inmoov = inmoov.Inmoov(urdf_path=self._urdf_sjtu) p.loadURDF(os.path.join(self._urdf_root, "plane.urdf")) self.table_uid = p.loadURDF(os.path.join(self._urdf_root, "table/table.urdf"), 0.0000000, 1.00000, 0.50000, 0.000000, 0.000000, 0.0, 1.0) # Initialize button position x_pos = 0 y_pos = 1 if self._random_target: x_pos += 0.15 * self.np_random.uniform(-1, 1) y_pos += 0.3 * self.np_random.uniform(-1, 1) self.button_uid = p.loadURDF("/urdf/simple_button.urdf", [x_pos, y_pos, Z_TABLE]) self.button_pos = np.array([x_pos, y_pos, Z_TABLE]) p.setGravity(0, 0, -10) self._env_step_counter = 0 # Close the gripper and wait for the arm to be in rest position for _ in range(5): self._inmoov.apply_action_pos([0, 0, 0]) p.stepSimulation() # Randomize init arm pos: take 5 random actions for _ in range(N_RANDOM_ACTIONS_AT_INIT): action = np.zeros(3) rand_direction = self.np_random.normal((3,)) # L2 normalize, so that the random direction is not too high or too low rand_direction /= np.linalg.norm(rand_direction, 2) action[:3] += DELTA_V_CONTINUOUS * rand_direction self._inmoov.apply_action_pos(list(action)) p.stepSimulation() self._observation = self.getExtendedObservation() # TODO: why self.button_pos = np.array(p.getLinkState(self.button_uid, BUTTON_LINK_IDX)[0]) self.button_pos[2] += BUTTON_DISTANCE_HEIGHT # Set the target position on the top of the button if self.saver is not None: self.saver.reset(self._observation, self.getTargetPos(), self.getGroundTruth()) if self.srl_model != "raw_pixels": return self.getSRLState(self._observation) return np.array(self._observation)
def __init__(self, urdf_root=pybullet_data.getDataPath(), renders=False, is_discrete=True, multi_view=False, name="inmoov_onearm_tomato_gym", max_distance=0.8, action_repeat=1, shape_reward=False, action_joints=False, record_data=False, random_target=False, force_down=True, state_dim=-1, learn_states=False, verbose=False, save_path='srl_zoo/data/', env_rank=0, srl_pipe=None, srl_model="raw_pixels", **_): super(InmoovOneArmTomatoGymEnv, self).__init__(srl_model=srl_model, relative_pos=RELATIVE_POS, env_rank=env_rank, srl_pipe=srl_pipe) self._timestep = 1. / 240. # URDF PATH self._urdf_root = urdf_root self._urdf_sjtu = "../../urdf_robot/" self._action_repeat = action_repeat self._observation = [] self._env_step_counter = 0 self._renders = renders self._width = RENDER_WIDTH self._height = RENDER_HEIGHT ### TODO: check self._cam_dist = 1.1 self._cam_yaw = 145 self._cam_pitch = -36 self._cam_roll = 0 self._max_distance = max_distance ### TODO: delete self._shape_reward = shape_reward self._random_target = random_target self._force_down = force_down ### TODO: check self.camera_target_pos = (0.0, 0.0, 1.0) self._is_discrete = is_discrete self.terminated = False self.renderer = p.ER_TINY_RENDERER self.debug = True self.n_contacts = 0 self.state_dim = state_dim self.action_joints = action_joints self.relative_pos = RELATIVE_POS self.cuda = th.cuda.is_available() self.saver = None self.multi_view = multi_view self.verbose = verbose self.max_steps = MAX_STEPS self.n_steps_outside = 0 self.tomato_uid = None self.tomato_pos = None self._inmoov = inmoov.Inmoov(urdf_path=self._urdf_sjtu) ### TODO:delete self.table_uid = None self.button_pos = None self.button_uid = None self._kuka = None self.action = None self.srl_model = srl_model if record_data: self.saver = EpisodeSaver(name, max_distance, state_dim, globals_=getGlobals(), relative_pos=RELATIVE_POS, learn_states=learn_states, path=save_path) if self._renders: client_id = p.connect(p.SHARED_MEMORY) if client_id < 0: p.connect(p.GUI) ### TODO:check p.resetDebugVisualizerCamera(1.3, 180, -41, [0.52, -0.2, -0.33]) self.debug = True # Debug sliders for moving the camera self.x_slider = p.addUserDebugParameter("x_slider", -10, 10, self.camera_target_pos[0]) self.y_slider = p.addUserDebugParameter("y_slider", -10, 10, self.camera_target_pos[1]) self.z_slider = p.addUserDebugParameter("z_slider", -10, 10, self.camera_target_pos[2]) self.dist_slider = p.addUserDebugParameter("cam_dist", 0, 10, self._cam_dist) self.yaw_slider = p.addUserDebugParameter("cam_yaw", -180, 180, self._cam_yaw) self.pitch_slider = p.addUserDebugParameter( "cam_pitch", -180, 180, self._cam_pitch) else: p.connect(p.DIRECT) global CONNECTED_TO_SIMULATOR CONNECTED_TO_SIMULATOR = True if self._is_discrete: self.action_space = spaces.Discrete(N_DISCRETE_ACTIONS) else: if self.action_joints: # 7 angles for the arm rotation, from -1 to 1 #TODO:action_dim should be changed action_dim = 7 self._action_bound = 1 else: # 3 direction for the arm movement, from -1 to 1 action_dim = 3 self._action_bound = 1 action_high = np.array([self._action_bound] * action_dim) self.action_space = spaces.Box(-action_high, action_high, dtype=np.float32) if self.srl_model == "ground_truth": self.state_dim = self.getGroundTruthDim() elif self.srl_model == "joints": self.state_dim = self.getJointsDim() elif self.srl_model == "joints_position": self.state_dim = self.getGroundTruthDim() + self.getJointsDim() if self.srl_model == "raw_pixels": self.observation_space = spaces.Box(low=0, high=255, shape=(self._height, self._width, 3), dtype=np.uint8) else: self.observation_space = spaces.Box(low=-np.inf, high=np.inf, shape=(self.state_dim, ), dtype=np.float32)
def reset(self): self.terminated = False self.n_contacts = 0 self.n_steps_outside = 0 p.resetSimulation() p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timestep) p.loadURDF(os.path.join(self._urdf_root, "plane.urdf"), [0, 0, -1]) #self.table_uid = p.loadURDF(os.path.join(self._urdf_root, "table/table.urdf"), 0.5000000, 0.00000, -.820000, # 0.000000, 0.000000, 0.0, 1.0) tomato2Id = p.loadURDF(os.path.join(self._urdf_sjtu, "tomato_plant.urdf"), [0.4, 0.4, 0.5], baseOrientation=[0, 0, 0, 1]) # Initialize tomato position ### TODO:change x_pos = 0.5 y_pos = 0 if self._random_target: x_pos += 0.15 * self.np_random.uniform(-1, 1) y_pos += 0.3 * self.np_random.uniform(-1, 1) self.button_uid = p.loadURDF("/urdf/simple_button.urdf", [x_pos, y_pos, Z_TABLE]) self.button_pos = np.array([x_pos, y_pos, Z_TABLE]) p.setGravity(0, 0, -9.8) self._inmoov = inmoov.Inmoov(urdf_path=self._urdf_sjtu) self._env_step_counter = 0 # Close the gripper and wait for the arm to be in rest position for _ in range(500): if self.action_joints: ### TODO:change self._inmoov.applyAction( list(np.array(self._kuka.joint_positions)[:7]) + [0, 0]) else: self._inmoov.applyAction([0, 0, 0, 0, 0]) p.stepSimulation() # Randomize init arm pos: take 5 random actions for _ in range(N_RANDOM_ACTIONS_AT_INIT): if self._is_discrete: action = [0, 0, 0, 0, 0] sign = 1 if self.np_random.rand() > 0.5 else -1 action_idx = self.np_random.randint(3) # dx, dy or dz action[action_idx] += sign * DELTA_V else: if self.action_joints: joints = np.array(self._kuka.joint_positions)[:7] joints += DELTA_THETA * self.np_random.normal(joints.shape) action = list(joints) + [0, 0] else: action = np.zeros(5) rand_direction = self.np_random.normal((3, )) # L2 normalize, so that the random direction is not too high or too low rand_direction /= np.linalg.norm(rand_direction, 2) action[:3] += DELTA_V_CONTINUOUS * rand_direction self._kuka.applyAction(list(action)) p.stepSimulation() self._observation = self.getExtendedObservation() self.button_pos = np.array( p.getLinkState(self.button_uid, BUTTON_LINK_IDX)[0]) self.button_pos[ 2] += BUTTON_DISTANCE_HEIGHT # Set the target position on the top of the button if self.saver is not None: self.saver.reset(self._observation, self.getTargetPos(), self.getGroundTruth()) if self.srl_model != "raw_pixels": return self.getSRLState(self._observation) return np.array(self._observation)