def reset(self):
"""Reset."""
if not self._done and self._num_steps > 0:
for obs in self.observations:
obs.on_episode_end()
for reward_fn in self.reward_fns:
reward_fn.on_episode_end()
self._num_steps = 0
self._episode_reward = 0.0
self._done = False
if self.config.MAX_STEPS is not None:
if self.config.MAX_STEPS == 0:
self._done = True
if self.is_simulation:
self.simulator.reset()
self.simulator.start()
self._reset()
for obs in self.observations:
obs.on_episode_start()
for reward_fn in self.reward_fns:
reward_fn.on_episode_start()
logger.info('episode: %d', self.num_episodes)
self._obs_data = None
self._prev_obs_data = None
self._obs_step = None
return self.get_observation()
def convert_wrl_to_obj(wrl_paths, output_dir, body_name=''):
"""Convert a WRL files into .obj files using meshconv.
Args:
wrl_paths: List of paths to the input WRL files.
output_dir: Output directory.
body_name: Name of the body.
Returns:
obj_filenames: List of filenames of the output OBJ files.
"""
num_pieces = len(wrl_paths)
obj_filenames = []
for i, wrl_path in enumerate(wrl_paths):
basename = '%s_vhacd_%d_of_%d' % (body_name, i, num_pieces)
filename = '%s.obj' % (basename)
obj_filenames.append(filename)
obj_path = os.path.join(output_dir, basename)
command = MESHCONV_COMMAND.format(input_path=wrl_path,
output_path=obj_path)
logger.info(command)
os.system(command)
return obj_filenames
def __call__(self, data):
"""Call function.
Args:
data: The input data.
"""
# Create a file for saving the episode data.
if self._num_entries_this_file == 0:
if self._use_random_name:
timestamp = time_utils.get_timestamp_as_string()
else:
timestamp = '%06d' % (self._num_files)
filename = 'data_%s.pickle' % (timestamp)
self._output_path = os.path.join(self._output_dir, filename)
self._num_files += 1
if self._file:
self._file.close()
self._file = open(self._output_path, 'wb')
# Append the episode to the file.
logger.info('Saving data to file %s (%d / %d)...', self._output_path,
self._num_entries_this_file, self._num_entries_per_file)
num_entries = self.write(data)
# Update the cursor.
self._num_entries_this_file += num_entries
self._num_entries_this_file %= self._num_entries_per_file
def main():
args = parse_args()
if args.input_pattern[-4:] != '.obj':
input_pattern = '%s.obj' % (args.input_pattern)
else:
input_pattern = args.input_pattern
input_paths = glob.glob(input_pattern)
logger.info('Found %d .obj files at %s.', len(input_paths), input_pattern)
for i, input_path in enumerate(input_paths):
logger.info('Processing object %d / %d.', i, len(input_paths))
output_dir = input_path.replace('textured.obj', '')
if args.decompose:
process_object(input_path,
output_dir=output_dir,
rgba=args.rgba,
scale=args.scale,
mass=args.mass,
density=args.density)
else:
create_urdf(input_path,
output_dir=output_dir,
rgba=args.rgba,
scale=args.scale,
mass=args.mass)
def _sample(self, position, body_mask, num_episodes, num_steps):
num_bodies = int(np.sum(body_mask))
body_id = int(num_episodes) % int(num_bodies)
position = position[:num_bodies]
target_position = position[body_id:body_id+1]
base_angle = (num_episodes * SEED) % (2 * np.pi)
if num_steps == 0:
self.last_end = None
for i in range(self.max_attemps):
start = np.random.uniform(-1., 1., [2])
delta_angle = np.random.uniform(-0.25 * np.pi, 0.25 * np.pi)
angle = base_angle + delta_angle
# angle = base_angle
motion = [1.0 * np.cos(angle), 1.0 * np.sin(angle)]
motion = np.array(motion, dtype=np.float32)
motion += np.random.uniform(-0.3, 0.3, [2])
motion = np.clip(motion, -1.0, 1.0)
waypoints = self.get_waypoints(start, motion)
# Check if is_safe.
is_safe = self.is_waypoint_clear(
waypoints[0], None, position, self.start_margin)
if not is_safe:
continue
# Check if is_effective.
is_effective = not self.is_waypoint_clear(
waypoints[0],
waypoints[1],
target_position,
self.motion_margin)
if not is_effective:
continue
if is_effective and is_safe:
x = ((waypoints[1][0] - self.cspace_offset[0]) /
(self.cspace_range[0]))
y = ((waypoints[1][1] - self.cspace_offset[1]) /
(self.cspace_range[1]))
self.last_end = np.asarray([x, y])
break
if i == self.max_attemps - 1:
logger.info('HeuristicSampler did not find a good sample.')
start = np.array(start, dtype=np.float32)
motion = np.array(motion, dtype=np.float32)
action = np.concatenate([start, motion], axis=-1)
return action
def main():
args = parse_args()
logger.info('Creating the FrankaPanda command line client...')
franka_panda_cli = FrankaPandaCLI(args.mode, args.env_config, args.debug,
args.assets_dir)
logger.info('Running the Franka_Panda command line client...')
franka_panda_cli.start()
def main():
args = parse_args()
logger.info('Creating the Sawyer command line client...')
sawyer_cli = SawyerCLI(args.mode, args.env_config, args.debug,
args.assets_dir)
logger.info('Running the Sawyer command line client...')
sawyer_cli.start()
def _reset(self):
"""Reset."""
observations = super(PushEnv, self)._reset()
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)
if self.use_recording:
hostname = socket.gethostname().split('.')[0]
self.recording_camera = self._create_camera(
height=self.config.RECORDING.CAMERA.HEIGHT,
width=self.config.RECORDING.CAMERA.WIDTH,
intrinsics=self.config.RECORDING.CAMERA.INTRINSICS,
translation=self.config.RECORDING.CAMERA.TRANSLATION,
rotation=self.config.RECORDING.CAMERA.ROTATION)
recording_tmp_dir = os.path.join('/tmp', 'recording')
if not os.path.exists(recording_tmp_dir):
os.makedirs(recording_tmp_dir)
if self.task_name is None:
recording_output_dir = os.path.join(
self.config.RECORDING.OUTPUT_DIR, 'data_collection')
else:
recording_output_dir = os.path.join(
self.config.RECORDING.OUTPUT_DIR,
'%s_layout%02d' % (self.task_name, self.layout_id))
if not os.path.exists(recording_output_dir):
logger.info('Saving recorded videos to %s ...',
recording_output_dir)
os.makedirs(recording_output_dir)
if self.video_writer is not None:
self.video_writer.release()
shutil.copyfile(self.recording_tmp_path,
self.recording_output_path)
name = '%s_%s.avi' % (hostname,
time_utils.get_timestamp_as_string())
resolution = (self.config.RECORDING.CAMERA.WIDTH,
self.config.RECORDING.CAMERA.HEIGHT)
self.recording_tmp_path = os.path.join(recording_tmp_dir, name)
self.recording_output_path = os.path.join(recording_output_dir,
name)
self.video_writer = cv2.VideoWriter(
self.recording_tmp_path, cv2.VideoWriter_fourcc(*'XVID'),
self.config.RECORDING.FPS, resolution)
return observations
def __init__(self, time_step=1e-3, use_visualizer=True, worker_id=0):
"""
Initialization function.
Args:
time_step: The time step of the simulation. Run real-time
simulation if it is set to None.
use_visualizer: If use the visualizer.
worker_id: The key of the simulation client.
"""
logger.info('pybullet API Version: %s.' % (pybullet.getAPIVersion()))
if use_visualizer:
self._uid = pybullet.connect(pybullet.GUI)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_SHADOWS, 1)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
assert worker_id == 0
logger.info('Connected client %d to GUI.', self._uid)
else:
logger.info('Use worker_id %d for the simulation.', worker_id)
self._uid = pybullet.connect(pybullet.DIRECT, key=worker_id)
logger.info('Connected client %d to DIRECT.', self._uid)
self._time_step = time_step
self._start_time = None
self._num_steps = None
self._gravity = None
def collect(tf_env,
tf_policy,
output_dir,
checkpoint=None,
num_iterations=500000,
episodes_per_file=500,
summary_interval=1000):
"""A simple train and eval for SAC."""
if not os.path.isdir(output_dir):
logger.info('Making output directory %s...', output_dir)
os.makedirs(output_dir)
global_step = tf.compat.v1.train.get_or_create_global_step()
with tf.compat.v2.summary.record_if(
lambda: tf.math.equal(global_step % summary_interval, 0)):
# Make the replay buffer.
replay_buffer = tfrecord_replay_buffer.TFRecordReplayBuffer(
data_spec=tf_policy.trajectory_spec,
experiment_id='exp',
file_prefix=os.path.join(output_dir, 'data'),
episodes_per_file=episodes_per_file)
replay_observer = [replay_buffer.add_batch]
collect_policy = tf_policy
collect_op = dynamic_step_driver.DynamicStepDriver(
tf_env, collect_policy, observers=replay_observer,
num_steps=1).run()
with tf.compat.v1.Session() as sess:
# Initialize training.
try:
common.initialize_uninitialized_variables(sess)
except Exception:
pass
# Restore checkpoint.
if checkpoint is not None:
if os.path.isdir(checkpoint):
train_dir = os.path.join(checkpoint, 'train')
checkpoint_path = tf.train.latest_checkpoint(train_dir)
else:
checkpoint_path = checkpoint
restorer = tf.train.Saver(name='restorer')
restorer.restore(sess, checkpoint_path)
collect_call = sess.make_callable(collect_op)
for _ in range(num_iterations):
collect_call()
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 run_convex_decomposition(input_path,
output_path='output.wrl',
log_path='log.txt'):
"""Run the convex decomposition with V-HACD.
Args:
input_path: Path to the input OBJ file.
output_path: Path to the output WRL file.
log_path: Path to the log file.
"""
# TODO(kuanfang): Temporally remove the constraints of number of groups for
# simplicity, will add it back if we find it necessary in the future.
command = VHACD_COMMAND.format(input_path=input_path,
output_path=output_path,
log_path=log_path)
logger.info(command)
os.system(command)
def parse_config_files_and_bindings(args):
if args.env_config is None:
env_config = None
else:
env_config = YamlConfig(args.env_config).as_easydict()
if args.policy_config is None:
policy_config = None
else:
policy_config = YamlConfig(args.policy_config).as_easydict()
if args.config_bindings is not None:
parsed_bindings = ast.literal_eval(args.config_bindings)
logger.info('Config Bindings: %r', parsed_bindings)
env_config.update(parsed_bindings)
policy_config.update(parsed_bindings)
return env_config, policy_config
def _reset_scene(self):
"""Reset the scene in simulation or the real world."""
super(PushEnv, self)._reset_scene()
# Simulation.
if self.is_simulation:
self.num_movable_bodies = np.random.randint(
low=self.min_movable_bodies, high=self.max_movable_bodies + 1)
self._load_movable_bodies()
if self.layouts is not None:
layout = self.layouts[self.layout_id]
self._load_tiles(layout.region,
layout.size,
layout.offset,
z_offset=0.001 - 0.025,
rgba=layout.region_rgba)
if layout.goal is not None:
self._load_tiles(layout.goal,
layout.size,
layout.offset,
z_offset=0.0015 - 0.025,
rgba=layout.goal_rgba)
else:
self.num_movable_bodies = self.max_movable_bodies
logger.info('Assume there are %d movable objects on the table.',
self.num_movable_bodies)
self.movable_body_mask = np.array(
[1] * self.num_movable_bodies + [0] *
(self.max_movable_bodies - self.num_movable_bodies))
# Attributes
self.attributes = {
'num_episodes': self.num_episodes,
'num_steps': self.num_steps,
'layout_id': self.layout_id,
'movable_body_mask': self.movable_body_mask,
'is_safe': True,
'is_effective': True,
}
def __init__(self, output_dir, num_entries_per_file, use_random_name=True):
"""Initialize.
Args:
output_dir: Output directory.
num_entries_per_file: Number of entries in each file.
use_random_name: Use randomly generated name if True.
"""
self._output_dir = output_dir
self._num_entries_per_file = num_entries_per_file
self._use_random_name = use_random_name
self._file = None
self._output_path = None
self._num_files = 0
self._num_entries_this_file = 0
if not os.path.isdir(output_dir):
logger.info('Making output directory %s...', output_dir)
os.makedirs(output_dir)
def parse_config_files_and_bindings(args):
"""Parse the config files and the bindings.
Args:
args: The arguments.
Returns:
env_config: Environment configuration.
policy_config: Policy configuration.
"""
if args.env_config is None:
env_config = dict()
else:
env_config = YamlConfig(args.env_config).as_easydict()
if args.policy_config is None:
policy_config = dict()
else:
policy_config = YamlConfig(args.policy_config).as_easydict()
if args.config_bindings is not None:
parsed_bindings = ast.literal_eval(args.config_bindings)
logger.info('Config Bindings: %r', parsed_bindings)
env_config.update(parsed_bindings)
policy_config.update(parsed_bindings)
logger.info('Environment Config:')
pprint.pprint(env_config)
logger.info('Policy Config:')
pprint.pprint(policy_config)
return env_config, policy_config
def step(self, action):
"""Take a step.
See parent class.
"""
observation, reward, done, info = super(PushEnv, self).step(action)
if done and reward >= self.config.SUCCESS_THRESH:
self.num_successes += 1
self.num_successes_by_step[self._num_steps] += 1
if done:
logger.info(
'num_successes: %d, success_rate: %.3f',
self.num_successes,
self.num_successes / float(self._num_episodes + 1e-14),
)
text = ('num_successes_by_step: ' +
', '.join(['%d'] * int(self.config.MAX_STEPS + 1)))
logger.info(text, *self.num_successes_by_step)
logger.info(
'num_total_steps %d, '
'unsafe: %.3f, ineffective: %.3f, useful: %.3f',
self.num_total_steps,
self.num_unsafe / float(self.num_total_steps + 1e-14),
self.num_ineffective / float(self.num_total_steps + 1e-14),
self.num_useful / float(self.num_total_steps + 1e-14))
return observation, reward, done, info
def step(self, action):
"""Take a step.
See parent class.
"""
if self._done:
raise ValueError('The environment is done. Forget to reset?')
self._execute_action(action)
self._num_steps += 1
observation = self.get_observation()
reward, termination = self.get_reward()
self._episode_reward += reward
self._done = (self._done or termination)
if self.config.MAX_STEPS is not None:
if self.num_steps >= self.config.MAX_STEPS:
self._done = True
logger.info('step: %d, reward: %.3f', self.num_steps, reward)
if self._done:
self._num_episodes += 1
self._total_reward += self.episode_reward
logger.info(
'episode_reward: %.3f, avg_episode_reward: %.3f',
self.episode_reward,
float(self.total_reward) / (self._num_episodes + 1e-14),
)
if self.debug:
self.render()
return observation, reward, self._done, None
def main():
args = parse_args()
if os.path.isdir(args.log_dir):
logger.warn('Output directory %s already exists.',
os.path.abspath(args.log_dir))
processes = []
for key in range(args.num_copies):
logger.info('Starting copy %d / %d...', key, args.num_copies)
hostname = socket.gethostname()
hostname = hostname.split('.')[0]
log_dir = os.path.abspath(args.log_dir)
log_dir = os.path.join(log_dir, 'logs')
if not os.path.isdir(log_dir):
logger.info('Making output directory %s...', log_dir)
os.makedirs(log_dir)
log_path = os.path.join(log_dir, '%s_%02d.log' % (hostname, key))
command = '%s >> %s' % (args.command, log_path)
logger.info('Run the command: %s', command)
process = subprocess.Popen(command, shell=True)
processes.append(process)
def cleanup():
for ind, process in enumerate(processes):
process.kill()
logger.info('Killed process %d.', ind)
atexit.register(cleanup)
logger.info('All copies have been started. Running episode generation...')
while True:
pass
def process_object(input_path,
output_dir,
rgba,
scale=1.0,
mass=0.1,
density=None):
"""Process a single object.
Args:
input_path: The path to the input .obj file.
output_dir: The directory of the processed model, including the .urdf
file, and the decomposed .obj files.
rgba: The color of the visual model.
scale: The scale of the model.
mass: The mass of the whole object.
density: The density of the whole object.
"""
body_name = os.path.splitext(os.path.basename(input_path))[0]
# Copy the mesh(visual model) to the output directory (with the URDF file).
# logger.info('Copying the mesh from %s to %s...'
# % (input_path, output_dir))
# command = 'cp %s %s' % (input_path, output_dir)
# logger.info(command)
# os.system(command)
if output_dir is None:
output_dir = os.path.dirname(input_path)
output_dir = os.path.join(output_dir, body_name)
if not os.path.isdir(output_dir):
os.makedirs(output_dir)
tmp_dir = os.path.join(output_dir, 'tmp')
if not os.path.isdir(tmp_dir):
os.mkdir(tmp_dir)
tmp_output_path = os.path.join(tmp_dir, 'output.wrl')
# Run V-HACD to generate the collision meshes (as .wrl files).
logger.info('Running the V-HACD convex decomposition...')
log_path = os.path.join('./meshes', 'log.txt')
run_convex_decomposition(input_path=input_path,
output_path=tmp_output_path,
log_path=log_path)
# Split the V-HACD results (as the .wrl file) into pieces.
logger.info('Splitting the V-HACD results at: %s...' % (tmp_output_path))
wrl_paths = split_wrl_file(tmp_output_path, tmp_dir=tmp_dir)
# Convert each .wrl file (each piece of V-HACD results) into .obj file.
logger.info('Converting the split V-HACD results to .obj format.')
vhacd_filenames = convert_wrl_to_obj(wrl_paths, output_dir, body_name)
# Read the mesh and compute the mesh properties.
logger.info('Reading the obj file at: %s...' % (input_path))
vertices, triangles = mesh_utils.read_from_obj(input_path)
centroid = mesh_utils.compute_centroid(vertices, triangles)
volume = mesh_utils.compute_volume(vertices, triangles)
surface_area = mesh_utils.compute_surface_area(vertices, triangles)
if mass is None:
raise ValueError('The volume is problematic. Do not use the density.')
mass = volume * density
# mass = surface_area * args.density
else:
mass = mass
logger.info('Object Information: \n'
'\tbody_name: %s\n'
'\tcentroid: %s\n'
'\tvolume: %f\n'
'\tsurface_area: %f\n'
'\tmass: %f\n' %
(body_name, centroid, volume, surface_area, mass))
# Create the URDF file.
urdf_path = os.path.join(output_dir, body_name + '.urdf')
logger.info('Writing the URDF files at: %s...' % (urdf_path))
# Create the visual meshes.
with open('./tools/templates/visual_template.xml', 'r') as f:
visual_template = f.read()
visual_text = ''
for vhacd_filename in vhacd_filenames:
visual_text += visual_template.format(filename=vhacd_filename,
scale=scale)
# Create the collision meshes.
with open('./tools/templates/collision_template.xml', 'r') as f:
collision_template = f.read()
collision_text = ''
for vhacd_filename in vhacd_filenames:
collision_text += collision_template.format(filename=vhacd_filename,
scale=scale)
if rgba is None:
rgba = random_rgba()
# Write to the URDF file.
with open('./tools/templates/urdf_template.xml', 'r') as f:
urdf_template = f.read()
with open(urdf_path, 'w') as f:
urdf_text = urdf_template.format(body_name=body_name,
mass=mass,
ixx=1,
iyy=1,
izz=1,
ixy=0,
ixz=0,
iyz=0,
cx=centroid[0],
cy=centroid[1],
cz=centroid[2],
visual=visual_text,
collision=collision_text,
rgba=rgba)
f.write(urdf_text)
# Clean up the temporal directory.
logger.info('Removing the tmp folder...')
command = 'rm -rf %s' % tmp_dir
logger.info(command)
os.system(command)
def main():
args = parse_args()
# Configuration.
env_config, policy_config = parse_config_files_and_bindings(args)
# Set the random seed.
if args.seed is not None:
random.seed(args.seed)
np.random.seed(args.seed)
# Simulator.
if args.use_simulator:
simulator = Simulator(worker_id=args.worker_id,
use_visualizer=bool(args.debug),
assets_dir=args.assets_dir)
else:
simulator = None
# Environment.
env_class = getattr(envs, args.env)
env = env_class(simulator=simulator, config=env_config, debug=args.debug)
# Policy.
policy_class = getattr(policies, args.policy)
policy = policy_class(env=env, config=policy_config)
# Output directory.
if args.output_dir is not None:
hostname = socket.gethostname()
hostname = hostname.split('.')[0]
output_dir = os.path.abspath(args.output_dir)
output_dir = os.path.join(output_dir, hostname, '%02d' % (args.key))
if not os.path.isdir(output_dir):
logger.info('Making output directory %s...', output_dir)
os.makedirs(output_dir)
# Generate and write episodes.
logger.info('Start running...')
env.reset()
num_episodes_this_file = 0
for episode_index, episode in generate_episodes(
env,
policy,
num_steps=args.num_steps,
num_episodes=args.num_episodes,
timeout=args.timeout,
debug=args.debug):
if args.output_dir:
# Create a file for saving the episode data.
if num_episodes_this_file == 0:
timestamp = time_utils.get_timestamp_as_string()
filename = 'episodes_%s.hdf5' % (timestamp)
output_path = os.path.join(output_dir, filename)
logger.info('Created a new file %s...', output_path)
# Append the episode to the file.
logger.info('Saving episode %d to file %s (%d / %d)...',
episode_index, output_path, num_episodes_this_file,
args.num_episodes_per_file)
with h5py.File(output_path, 'a') as fout:
name = str(uuid.uuid4())
group = fout.create_group(name)
hdf5_utils.write_data_to_hdf5(group, episode)
num_episodes_this_file += 1
num_episodes_this_file %= args.num_episodes_per_file
if args.pause:
input('Press [Enter] to start a new episode.')
def cleanup():
for ind, process in enumerate(processes):
process.kill()
logger.info('Killed process %d.', ind)
def _execute_action(self, action): # NOQA
"""Execute the robot action.
Args:
action: A dictionary of mode and argument of the action.
"""
self.attributes = {
'num_episodes': self.num_episodes,
'num_steps': self.num_steps,
'layout_id': self.layout_id,
'movable_body_mask': self.movable_body_mask,
'is_safe': True,
'is_effective': True,
}
waypoints = self._compute_all_waypoints(action)
self.phase = 'initial'
self.num_waypoints = 0
self.interrupt = False
self.start_status = self._get_movable_status()
while (self.phase != 'done'):
if self.is_simulation:
self.simulator.step()
if self.use_recording:
if (self.simulator.num_steps %
self.config.RECORDING.NUM_STEPS == 0):
self._record_screenshot()
if not (self.simulator.num_steps % self.config.SIM.STEPS_CHECK
== 0):
continue
# Phase transition.
if self._is_phase_ready():
self.phase = self._get_next_phase()
if self.config.DEBUG and self.debug:
logger.info('phase: %s, num_waypoints: %d', self.phase,
self.num_waypoints)
if self.is_simulation:
self.max_phase_steps = self.simulator.num_steps
if self.phase == 'motion':
self.max_phase_steps += (
self.config.SIM.MAX_MOTION_STEPS)
elif self.phase == 'offstage':
self.max_phase_steps += (
self.config.SIM.MAX_OFFSTAGE_STEPS)
else:
self.max_phase_steps += (
self.config.SIM.MAX_PHASE_STEPS)
if self.phase == 'pre':
pose = waypoints[self.num_waypoints][0].copy()
pose.z = self.config.ARM.GRIPPER_SAFE_HEIGHT
self.robot.move_to_gripper_pose(pose)
elif self.phase == 'start':
pose = waypoints[self.num_waypoints][0]
self.robot.move_to_gripper_pose(pose)
elif self.phase == 'motion':
pose = waypoints[self.num_waypoints][1]
self.robot.move_to_gripper_pose(pose)
elif self.phase == 'post':
self.num_waypoints += 1
pose = self.robot.end_effector.pose
pose.z = self.config.ARM.GRIPPER_SAFE_HEIGHT
self.robot.move_to_gripper_pose(pose)
elif self.phase == 'offstage':
self.robot.move_to_joint_positions(
self.config.ARM.OFFSTAGE_POSITIONS)
self.interrupt = False
if self._check_singularity():
self.interrupt = True
if not self._check_safety():
self.interrupt = True
self.attributes['is_safe'] = False
if self.interrupt:
if self.phase == 'done':
self._done = True
break
if self.is_simulation:
self.simulator.wait_until_stable(self.movable_bodies)
# Update attributes.
self.end_status = self._get_movable_status()
self.attributes['is_effective'] = self._check_effectiveness()
self.num_total_steps += 1
self.num_unsafe += int(not self.attributes['is_safe'])
self.num_ineffective += int(not self.attributes['is_effective'])
self.num_useful += int(self.attributes['is_safe']
and self.attributes['is_effective'])
def _load_movable_bodies(self):
"""Load movable bodies."""
assert self.simulator is not None
is_valid = False
while not is_valid:
logger.info('Loading movable objects...')
is_valid = True
self.movable_bodies = []
# Sample placements of the movable objects.
is_target = False
if self.layouts is None:
movable_poses = self._sample_body_poses(
self.num_movable_bodies, self.movable_config)
else:
layout = self.layouts[self.layout_id]
movable_poses = self._sample_body_poses_on_tiles(
self.num_movable_bodies, self.movable_config, layout)
is_target = (layout.target is not None)
for i in range(self.num_movable_bodies):
if i == 0 and is_target:
path = random.choice(self.target_movable_paths)
else:
path = random.choice(self.movable_paths)
pose = movable_poses[i]
scale = np.random.uniform(*self.movable_config.SCALE)
name = 'movable_%d' % i
# Add the body.
body = self.simulator.add_body(path, pose, scale, name=name)
if self.config.USE_RANDOM_RGBA:
r = np.random.uniform(0., 1.)
g = np.random.uniform(0., 1.)
b = np.random.uniform(0., 1.)
body.set_color(rgba=[r, g, b, 1.0], specular=[0, 0, 0])
# Wait for the new body to be dropped onto the table.
self.simulator.wait_until_stable(
body,
linear_velocity_threshold=0.1,
angular_velocity_threshold=0.1,
max_steps=500)
# Change physical properties.
mass = robot_env.get_config_value(self.movable_config.MASS)
lateral_friction = robot_env.get_config_value(
self.movable_config.FRICTION)
body.set_dynamics(mass=mass,
lateral_friction=lateral_friction,
rolling_friction=None,
spinning_friction=None)
self.movable_bodies.append(body)
for body in self.movable_bodies:
if body.position.z < self.table.position.z:
is_valid = False
break
if not is_valid:
logger.info('Invalid arrangement, reset the scene...')
for i, body in enumerate(self.movable_bodies):
self.simulator.remove_body(body.name)
logger.info('Waiting for movable objects to be stable...')
self.simulator.wait_until_stable(self.movable_bodies)
def create_urdf(input_path, output_dir, rgba, scale=1.0, mass=0.1):
"""Process a single object.
Args:
input_path: The path to the input .obj file.
output_dir: The directory of the processed model, including the .urdf
file, and the decomposed .obj files.
rgba: The color of the visual model.
scale: The scale of the model.
mass: The mass of the whole object.
"""
body_name = os.path.splitext(os.path.basename(input_path))[0]
body_path = '%s.obj' % (body_name)
# Read the mesh and compute the mesh properties.
logger.info('Reading the obj file at: %s...' % (input_path))
vertices, triangles = mesh_utils.read_from_obj(input_path)
centroid = mesh_utils.compute_centroid(vertices, triangles)
volume = mesh_utils.compute_volume(vertices, triangles)
surface_area = mesh_utils.compute_surface_area(vertices, triangles)
logger.info('Object Information: \n'
'\tbody_name: %s\n'
'\tcentroid: %s\n'
'\tvolume: %f\n'
'\tsurface_area: %f\n'
'\tmass: %f\n' %
(body_name, centroid, volume, surface_area, mass))
# Create the URDF file.
if output_dir is None:
output_dir = os.path.dirname(input_path)
urdf_path = os.path.join(output_dir, body_name + '.urdf')
logger.info('Writing the URDF files at: %s...' % (urdf_path))
# Create the visual meshes.
with open('./tools/templates/visual_template.xml', 'r') as f:
visual_template = f.read()
visual_text = visual_template.format(filename=body_path,
scale=scale,
cx=-centroid[0],
cy=-centroid[1],
cz=-centroid[2])
# Create the collision meshes.
with open('./tools/templates/collision_template.xml', 'r') as f:
collision_template = f.read()
collision_text = collision_template.format(filename=body_path,
scale=scale,
cx=-centroid[0],
cy=-centroid[1],
cz=-centroid[2])
if rgba is None:
rgba = random_rgba()
# Write to the URDF file.
with open('./tools/templates/urdf_template.xml', 'r') as f:
urdf_template = f.read()
with open(urdf_path, 'w') as f:
urdf_text = urdf_template.format(
body_name=body_name,
mass=mass,
ixx=1,
iyy=1,
izz=1,
ixy=0,
ixz=0,
iyz=0,
cx=0, #centroid[0],
cy=0, #centroid[1],
cz=0, #centroid[2],
visual=visual_text,
collision=collision_text,
rgba=rgba)
f.write(urdf_text)
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 get_observation(self):
"""Returns the observation data of the current step."""
images = self.camera.frames()
image = images['rgb']
depth = images['depth']
point_cloud = self.camera.deproject_depth_image(depth)
# Crop.
if self.crop_max is not None and self.crop_min is not None:
crop_mask = np.logical_and(
np.all(point_cloud >= self.crop_min, axis=-1),
np.all(point_cloud <= self.crop_max, axis=-1))
point_cloud = point_cloud[crop_mask]
# Segment.
if self.env.simulator:
segmask = images['segmask']
segmask = segmask.flatten()
segmask = pc_utils.convert_segment_ids(segmask, self.body_ids)
point_cloud = pc_utils.group_by_labels(point_cloud, segmask,
self.num_bodies,
self.num_points)
else:
point_cloud = pc_utils.remove_table(point_cloud)
segmask = pc_utils.cluster(point_cloud,
num_clusters=self.num_bodies,
method='dbscan')
point_cloud = point_cloud[segmask != -1]
segmask = pc_utils.cluster(point_cloud,
num_clusters=self.num_bodies)
point_cloud = pc_utils.group_by_labels(point_cloud, segmask,
self.num_bodies,
self.num_points)
# Confirm target.
if self.confirm_target:
# Click the target position.
self.target_position = None
self.depth = depth
self.ax.cla()
self.ax.imshow(image)
logger.info('Please click the target object...')
while self.target_position is None:
plt.pause(1e-3)
logger.info('Target Position: %r', self.target_position)
# Exchange the target object with the first object.
centers = np.mean(point_cloud, axis=1)
dists = np.linalg.norm(centers -
self.target_position[np.newaxis, :],
axis=-1)
target_id = dists.argmin()
if target_id != 0:
tmp = copy.deepcopy(point_cloud)
point_cloud[0, :] = tmp[target_id, :]
point_cloud[target_id, :] = tmp[0, :]
# Show the segmented point cloud.
pc_utils.show2d(point_cloud, self.camera, self.ax, image=image)
return point_cloud
def __del__(self):
pybullet.disconnect(physicsClientId=self.uid)
logger.info('Disconnected client %d to pybullet server.', self._uid)
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)
