def reset(self): self.terminated = 0 p.resetSimulation() p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) p.loadURDF(os.path.join(self._urdfRoot, "plane.urdf"), [0, 0, -1]) p.loadURDF(os.path.join(self._urdfRoot, "table/table.urdf"), 0.5000000, 0.00000, -.820000, 0.000000, 0.000000, 0.0, 1.0) xpos = 0.5 + 0.2 * random.random() ypos = 0 + 0.25 * random.random() ang = 3.1415925438 * random.random() orn = p.getQuaternionFromEuler([0, 0, ang]) self.blockUid = p.loadURDF(os.path.join(self._urdfRoot, "block.urdf"), xpos, ypos, -0.1, orn[0], orn[1], orn[2], orn[3]) p.setGravity(0, 0, -10) self._tm700 = tm700.tm700(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 p.stepSimulation() self._observation = self.getExtendedObservation() return np.array(self._observation)
def reset(self): """Environment reset called at the beginning of an episode. """ # Set the camera settings. look = [0.10, -0.20, 0.60] self._cam_pos = look distance = 0.1 pitch = -45 yaw = -75 roll = 120 self._view_matrix = p.computeViewMatrixFromYawPitchRoll(look, distance, yaw, pitch, roll, 2) self.fov = 30. ''' look = [0.90, -0.28, 0.43] distance = 0.15 pitch = -45 yaw = 45 # -45 roll = 180 self._view_matrix = p.computeViewMatrixFromYawPitchRoll(look, distance, yaw, pitch, roll, 2) self.fov = 40. ''' self.focal_length_x = self._width / np.tan(np.radians(self.fov)/2.0) self.focal_length_y = self._height / np.tan(np.radians(self.fov)/2.0) aspect = self._width / self._height self.d_near = 0.01 self.d_far = 1.5 self._proj_matrix = p.computeProjectionMatrixFOV(self.fov, aspect, self.d_near, self.d_far) self._attempted_grasp = False self._env_step = 0 self.terminated = 0 p.resetSimulation() p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) p.loadURDF(os.path.join(self._urdfRoot, "plane.urdf"), [0, 0, -1]) self.table_pose = [0.5000000, 0.00000, -.640000, 0.000000, 0.000000, 0.0, 1.0] self.tableUid = p.loadURDF(os.path.join(self._urdfRoot, "table/table.urdf"), *self.table_pose) #p.setGravity(0, 0, -9.8) p.setGravity(0, 0, 0) self._tm700 = tm700(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 p.stepSimulation() # Choose the objects in the bin. ind = np.random.choice(len(self._urdfList), self._numObjects, replace=False) self._current_urdfList = [self._urdfList[i] for i in ind] self._current_objList = [self._objNameList[i] for i in ind] self._objectUids = self._randomly_place_objects(self._current_urdfList) self._observation = self._get_observation() return np.array(self._observation)
def reset(self): """Environment reset called at the beginning of an episode. """ # Set the camera settings. look = [ 0.1, -0.3, 0.54 ] #[0.4317558029454219, 0.1470448842904527, 0.2876218894185256]#[0.23, 0.2, 0.54] # from where the input is distance = 0.5 pitch = -45 + self._cameraRandom * np.random.uniform(-3, 3) yaw = -45 + self._cameraRandom * np.random.uniform(-3, 3) roll = 180 self._view_matrix = p.computeViewMatrixFromYawPitchRoll( look, distance, yaw, pitch, roll, 2) fov = 20. + self._cameraRandom * np.random.uniform(-2, 2) aspect = self._width / self._height near = 0.01 far = 10 self._proj_matrix = p.computeProjectionMatrixFOV( fov, aspect, near, far) self._attempted_grasp = False self._env_step = 0 self.terminated = 0 p.resetSimulation() p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) p.loadURDF(os.path.join(self._urdfRoot, "plane.urdf"), [0, 0, -1]) self.tableUid = p.loadURDF( os.path.join(self._urdfRoot, "table/table.urdf"), 0.5000000, 0.00000, -.640000, 0.000000, 0.000000, 0.0, 1.0) p.setGravity(0, 0, -10) self._tm700 = tm700(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 p.stepSimulation() # Choose the objects in the bin. urdfList = self._get_random_object(self._numObjects, self._isTest) # urdfList = self._get_block() self._objectUids = self._randomly_place_objects(urdfList) self._observation = self._get_observation() return np.array(self._observation)
def setup(self): """Sets up the robot + tray + objects. """ test = self._test if not self._urdf_list: # Load from procedural random objects. if not self._object_filenames: self._object_filenames = self._get_random_objects( num_objects=self._num_objects, test=test, replace=self._allow_duplicate_objects, ) self._urdf_list = self._object_filenames logging.info('urdf_list %s', self._urdf_list) pybullet.resetSimulation(physicsClientId=self.cid) pybullet.setPhysicsEngineParameter( numSolverIterations=150, physicsClientId=self.cid) pybullet.setTimeStep(self._time_step, physicsClientId=self.cid) pybullet.setGravity(0, 0, -10, physicsClientId=self.cid) plane_path = os.path.join(self._urdf_root, 'plane.urdf') pybullet.loadURDF(plane_path, [0, 0, -1], physicsClientId=self.cid) table_path = os.path.join(self._urdf_root, 'table/table.urdf') pybullet.loadURDF( table_path, [0.5, 0.0, -.82], [0., 0., 0., 1.], physicsClientId=self.cid) self._tm700 = tm700.tm700( urdfRootPath=self._urdf_root) self._block_uids = [] for urdf_name in self._urdf_list: xpos = 0.4 + self._block_random * random.random() ypos = self._block_random * (random.random() - .5) angle = np.pi / 2 + self._block_random * np.pi * random.random() ori = pybullet.getQuaternionFromEuler([0, 0, angle]) uid = pybullet.loadURDF( urdf_name, [xpos, ypos, .15], [ori[0], ori[1], ori[2], ori[3]], physicsClientId=self.cid) self._block_uids.append(uid) for _ in range(500): pybullet.stepSimulation(physicsClientId=self.cid)
def reset(self): """Environment reset called at the beginning of an episode. """ look = [0.4, 0.1, 0.54] distance = 1.5 pitch = -90 yaw = -90 roll = 180 pos_range = [0.45, 0.5, 0.0, 0.1] self._view_matrix = p.computeViewMatrixFromYawPitchRoll( look, distance, yaw, pitch, roll, 2) fov = 20. + self._cameraRandom * np.random.uniform(-2, 2) aspect = self._width / self._height near = 0.01 far = 10 self._proj_matrix = p.computeProjectionMatrixFOV( fov, aspect, near, far) self._attempted_grasp = False self._env_step = 0 self.terminated = 0 p.resetSimulation() p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) p.loadURDF(os.path.join(self._urdfRoot, "plane.urdf"), [0, 0, -1]) self.tableUid = p.loadURDF( os.path.join(self._urdfRoot, "table/table.urdf"), 0.5000000, 0.00000, -.640000, 0.000000, 0.000000, 0.0, 1.0) p.setGravity(0, 0, -10) self._tm700 = tm700(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 p.stepSimulation() # place objs if TASK_SEEN: if RESUME_NUM is not None: self.model_paths = self.model_paths[RESUME_NUM * 3:] self.img_save_cnt = RESUME_NUM self.N_DATA_TO_GENERATE -= RESUME_NUM for _ in tqdm(range(self.N_DATA_TO_GENERATE)): urdfList = [] num_obj = random.randint(1, 3) for i in range(num_obj): if len(self.model_paths) == 0: raise Exception('There is no data left!') urdfList.append(self.model_paths.pop()) self._objectUids, self._objectClasses = self._randomly_place_objects( urdfList, pos_range) self._observation = self._get_observation() self.img_save_cnt += 1 for uid in self._objectUids: p.removeBody(uid) # place unseen objs if TASK_UNSEEN: self.img_save_cnt = 0 for _ in tqdm(range(self.N_UNSEEN_DATA_TO_GENERATE)): urdfList = [] num_obj = random.randint(1, 3) for i in range(num_obj): if len(self.unseen_model_paths) == 0: raise Exception('There is no data left!') urdfList.append(self.unseen_model_paths.pop()) self._objectUids, self._objectClasses = self._randomly_place_objects( urdfList, pos_range) self._observation = self._get_observation(unseen=True) self.img_save_cnt += 1 for uid in self._objectUids: p.removeBody(uid) # terminate sys.exit() return np.array(self._observation)