def _sample_body_poses(self, num_samples, body_config, max_attemps=32): """Sample body poses. Args: num_samples: Number of samples. body_config: Configuration of the body. max_attemps: Maximum number of attemps to find a feasible placement. Returns: List of poses. """ while True: movable_poses = [] for i in range(num_samples): num_attemps = 0 is_valid = False while not is_valid and num_attemps <= max_attemps: pose = Pose.uniform(x=body_config.POSE.X, y=body_config.POSE.Y, z=body_config.POSE.Z, roll=body_config.POSE.ROLL, pitch=body_config.POSE.PITCH, yaw=body_config.POSE.YAW) # Check if the new pose is distant from other bodies. is_valid = True for other_pose in movable_poses: dist = np.linalg.norm(pose.position[:2] - other_pose.position[:2]) if dist < body_config.MARGIN: is_valid = False num_attemps += 1 break if not is_valid: logger.info( 'Cannot find the placement of body %d. ' 'Start re-sampling.', i) break else: movable_poses.append(pose) if i == num_attemps: break return movable_poses
def _reset_scene(self): """Reset the scene in simulation or the real world. """ super(Grasp4DofEnv, self)._reset_scene() # Reload graspable object. if self.config.SIM.GRASPABLE.RESAMPLE_N_EPISODES: if (self.num_episodes % self.config.SIM.GRASPABLE.RESAMPLE_N_EPISODES == 0): self.graspable_path = None if self.graspable_path is None: if self.config.SIM.GRASPABLE.USE_RANDOM_SAMPLE: self.graspable_path = random.choice(self.all_graspable_paths) else: self.graspable_index = ((self.graspable_index + 1) % len(self.all_graspable_paths)) self.graspable_path = ( self.all_graspable_paths[self.graspable_index]) pose = Pose.uniform(x=self.config.SIM.GRASPABLE.POSE.X, y=self.config.SIM.GRASPABLE.POSE.Y, z=self.config.SIM.GRASPABLE.POSE.Z, roll=self.config.SIM.GRASPABLE.POSE.ROLL, pitch=self.config.SIM.GRASPABLE.POSE.PITCH, yaw=self.config.SIM.GRASPABLE.POSE.YAW) pose = get_transform(source=self.table_pose).transform(pose) scale = np.random.uniform(*self.config.SIM.GRASPABLE.SCALE) logger.info('Loaded the graspable object from %s with scale %.2f...', self.graspable_path, scale) self.graspable = self.simulator.add_body(self.graspable_path, pose, scale=scale, name=GRASPABLE_NAME) logger.debug('Waiting for graspable objects to be stable...') self.simulator.wait_until_stable(self.graspable) # Reset camera. self._reset_camera( self.camera, intrinsics=self.config.KINECT2.DEPTH.INTRINSICS, translation=self.config.KINECT2.DEPTH.TRANSLATION, rotation=self.config.KINECT2.DEPTH.ROTATION, intrinsics_noise=self.config.KINECT2.DEPTH.INTRINSICS_NOISE, translation_noise=self.config.KINECT2.DEPTH.TRANSLATION_NOISE, rotation_noise=self.config.KINECT2.DEPTH.ROTATION_NOISE)
def _sample_body_poses_on_tiles(self, num_samples, body_config, layout, safe_drop_height=0.2, max_attemps=32): """Sample tile poses on the tiles. Args: num_samples: Number of samples. body_config: Configuration of the body. layout: Configuration of the layout. safe_drop_height: Dropping height of the body. max_attemps: Maximum number of attemps to find a feasible placement. Returns: List of poses. """ tile_size = layout.size tile_offset = layout.offset while True: movable_poses = [] for i in range(num_samples): num_attemps = 0 is_valid = False if i == 0 and layout.target is not None: tile_config = layout.target else: tile_config = layout.obstacle while not is_valid and num_attemps <= max_attemps: num_tiles = len(tile_config) tile_id = np.random.choice(num_tiles) tile_center = tile_config[tile_id] x_range = [ tile_offset[0] + (tile_center[0] - 0.5) * tile_size, tile_offset[0] + (tile_center[0] + 0.5) * tile_size ] y_range = [ tile_offset[1] + (tile_center[1] - 0.5) * tile_size, tile_offset[1] + (tile_center[1] + 0.5) * tile_size ] z = self.table_pose.position.z + safe_drop_height pose = Pose.uniform(x=x_range, y=y_range, z=z, roll=[-np.pi, np.pi], pitch=[-np.pi / 2, np.pi / 2], yaw=[-np.pi, np.pi]) is_valid = True for other_pose in movable_poses: dist = np.linalg.norm(pose.position[:2] - other_pose.position[:2]) if dist < body_config.MARGIN: is_valid = False num_attemps += 1 break if not is_valid: logger.info( 'Cannot find the placement of body %d. ' 'Start re-sampling.', i) break else: movable_poses.append(pose) if i == num_attemps: break return movable_poses
def _reset_scene(self): """Reset the scene in simulation or the real world. """ # Configure debug visualizer if self._debug and self.is_simulation: visualizer_info = self.simulator.physics.get_debug_visualizer_info(['yaw', 'pitch', 'dist', 'target']) # Adjust the camera view to your own need. visualizer_info['dist'] = 1.5 visualizer_info['yaw'] = 50 visualizer_info['pitch'] = -20 self.simulator.physics.reset_debug_visualizer(camera_distance=visualizer_info['dist'], camera_yaw=visualizer_info['yaw'], camera_pitch=visualizer_info['pitch'], camera_target_position=visualizer_info['target']) super(FrankaPandaGraspEnv, self)._reset_scene() # Reload graspable object. if self.config.SIM.GRASPABLE.RESAMPLE_N_EPISODES: if (self.num_episodes % self.config.SIM.GRASPABLE.RESAMPLE_N_EPISODES == 0): self.graspable_path = None if self.graspable_path is None: if self.config.SIM.GRASPABLE.USE_RANDOM_SAMPLE: self.graspable_path = random.choice( self.all_graspable_paths) else: self.graspable_index = ((self.graspable_index + 1) % len(self.all_graspable_paths)) self.graspable_path = ( self.all_graspable_paths[self.graspable_index]) pose = Pose.uniform(x=self.config.SIM.GRASPABLE.POSE.X, y=self.config.SIM.GRASPABLE.POSE.Y, z=self.config.SIM.GRASPABLE.POSE.Z, roll=self.config.SIM.GRASPABLE.POSE.ROLL, pitch=self.config.SIM.GRASPABLE.POSE.PITCH, yaw=self.config.SIM.GRASPABLE.POSE.YAW) pose = get_transform(source=self.table_pose).transform(pose) scale = np.random.uniform(*self.config.SIM.GRASPABLE.SCALE) logger.info('Loaded the graspable object from %s with scale %.2f...', self.graspable_path, scale) self.graspable = self.simulator.add_body(self.graspable_path, pose, scale=scale, name=GRASPABLE_NAME, baseMass=0.1) # self.graspable = self.simulator.add_body(self.graspable_path, pose, scale=scale * 0.01, name=GRASPABLE_NAME, collisionFrameOrientation=[-0.5, -0.5, 0.5, 0.5], visualFrameOrientation=[-0.5, -0.5, 0.5, 0.5], baseMass=0.1) logger.debug('Waiting for graspable objects to be stable...') # Change some properties of the object compensating for # artifacts due to simulation self.table.set_dynamics(contact_damping=100., contact_stiffness=100.) self.graspable.set_dynamics( lateral_friction=1., rolling_friction=0.001, spinning_friction=0.001, contact_damping=100., contact_stiffness=100.) self.simulator.wait_until_stable(self.graspable) # Reset camera. self._reset_camera( self.camera, intrinsics=self.config.KINECT2.DEPTH.INTRINSICS, translation=self.config.KINECT2.DEPTH.TRANSLATION, rotation=self.config.KINECT2.DEPTH.ROTATION, intrinsics_noise=self.config.KINECT2.DEPTH.INTRINSICS_NOISE, translation_noise=self.config.KINECT2.DEPTH.TRANSLATION_NOISE, rotation_noise=self.config.KINECT2.DEPTH.ROTATION_NOISE)