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)
