def test_get_fk():
"""Test 'panda_fk.get_fk' functionality."""
hand_origin = gymapi.Transform()
hand_origin.p = gymapi.Vec3(1., 2., 3.)
# Joint angles are zero
joints = np.zeros(16)
fk_left_actual = utils.panda_fk.get_fk(joints, hand_origin, "left")
fk_right_actual = utils.panda_fk.get_fk(joints, hand_origin, "right")
fk_mid_actual = utils.panda_fk.get_fk(joints, hand_origin, "mid")
fk_expected = np.eye(4)
assert fk_expected == pytest.approx(fk_left_actual)
assert fk_expected == pytest.approx(fk_right_actual)
assert fk_expected == pytest.approx(fk_mid_actual)
# Joint angles are non-zero
joints = np.array([
0.1, -0.2, 1.4, -0.4, 0.4, -0.6, 1.2, -1.1, 0.35, 0.76, 0.22, -0.33,
0.72, 0.78, 0.03, 0.02
])
fk_actual = utils.panda_fk.get_fk(joints, hand_origin, "right")
fk_expected = np.array([[0.62718193, 0.56042884, 0.54089032, -1.5753273],
[-0.39134237, 0.82717439, -0.40327866, 0.72207522],
[-0.67341961, 0.04125579, 0.73810838, 3.74801295],
[0., 0., 0., 1.]])
assert fk_expected == pytest.approx(fk_actual)
def load_urdf(self,
filename,
position,
orientation,
use_fixed_base=0,
scale=1.0,
*args,
**kwargs):
"""Load a URDF file in the simulator.
Args:
filename (str): a relative or absolute path to the URDF file on the file system of the physics server.
position (np.array[float[3]]): create the base of the object at the specified position in world space
coordinates [x,y,z].
orientation (np.array[float[4]]): create the base of the object at the specified orientation as world
space quaternion [x,y,z,w].
use_fixed_base (bool): force the base of the loaded object to be static
scale (float): scale factor to the URDF model.
Returns:
int (non-negative): unique id associated to the load model.
"""
dirname = os.path.dirname(filename)
filename = os.path.basename(filename)
name = ''.join(filename.split('.')[:-1])
asset = self.gym.load_asset(dirname, filename)
position = gymapi.Vec3(*position)
orientation = gymapi.Quat(*orientation)
pose = gymapi.Transform(position, orientation)
self.gym.create_actor(self.env, asset, pose, name)
self._body_cnt += 1
self.bodies[self._body_cnt] = name
return self._body_cnt
def set_gravity(self, gravity=(0, 0, -9.81)):
"""Set the gravity in the simulator with the given acceleration.
By default, there is no gravitational force enabled in the simulator.
Args:
gravity (list, tuple of 3 floats): acceleration in the x, y, z directions.
"""
gravity = gymapi.Vec3(*gravity)
self.params.gravity = gravity
self.gym.set_sim_params(self.sim, self.params)
def __init__(self,
render=True,
num_instances=1,
middleware=None,
**kwargs):
"""
Initialize Isaac gym simulator.
Args:
render (bool): if True, it will open the GUI, otherwise, it will just run the server.
num_instances (int): number of simulator instances.
middleware (MiddleWare, None): middleware instance.
**kwargs (dict): optional arguments (this is not used here).
"""
super(Isaac, self).__init__(render=render,
num_instances=num_instances,
middleware=middleware,
**kwargs)
# define variables
self.gym = gymapi.acquire_gym()
self.sim = self.gym.create_sim()
self.params = gymapi.SimParams()
self.gym.get_sim_params(self.sim, self.params)
spacing = 10
lower, upper = gymapi.Vec3(-spacing, -spacing,
0.), gymapi.Vec3(spacing, spacing, spacing)
self.env = self.gym.create_env(self.sim, lower, upper)
self.viewer = None
self.dt = 1. / 60
self.num_substeps = 2
if render: # gui
self.viewer = self.gym.create_viewer(None, 1920, 1080)
# keep track of loaded bodies
self.bodies = OrderedDict() # {body_id: Body}
self._body_cnt = 0
def create_sim(gym, use_viewer, args):
"""Set sim parameters and create a Sim object."""
# Set simulation parameters
sim_type = gymapi.SIM_FLEX # Can also use SIM_PHYSX
sim_params = gymapi.SimParams()
sim_params.dt = 1.0 / 1500
sim_params.substeps = 1
sim_params.gravity = gymapi.Vec3(0.0, -9.81, 0.0)
if args.mode in ["shake", "twist"]:
sim_params.gravity = gymapi.Vec3(0.0, 0.0, 0.0)
# Set stress visualization parameters
sim_params.stress_visualization = True
sim_params.stress_visualization_min = 1.0e2
sim_params.stress_visualization_max = 1e5
# Set FleX-specific parameters
sim_params.flex.solver_type = 5
sim_params.flex.num_outer_iterations = 10
sim_params.flex.num_inner_iterations = 200
sim_params.flex.relaxation = 0.75
sim_params.flex.warm_start = 0.8
sim_params.flex.deterministic_mode = True
# Set contact parameters
sim_params.flex.shape_collision_distance = 5e-4
sim_params.flex.contact_regularization = 1.0e-6
sim_params.flex.shape_collision_margin = 1.0e-4
sim_params.flex.dynamic_friction = 0.7
# Create Sim object
gpu_physics = 0
gpu_render = 0
if not use_viewer:
gpu_render = -1
return gym.create_sim(gpu_physics, gpu_render, sim_type,
sim_params), sim_params
def test_jacobian():
"""Test 'panda_fk.jacobian' functionality."""
hand_origin = gymapi.Transform()
hand_origin.p = gymapi.Vec3(1., 2., 3.)
# Joint angles are zero
joints = np.zeros(16)
jacobian_actual = utils.panda_fk.jacobian(joints, hand_origin)
jacobian_expected = np.array([[1., 0., 0., 2., -3.112, 0.],
[0., 1., 0., -1., 0., 3.112],
[0., 0., 1., 0., 1., -2.],
[0., 0., 0., 0., 0., 1.],
[0., 0., 0., 0., 1., 0.],
[0., 0., 0., 1., 0., 0.]])
assert jacobian_expected == pytest.approx(jacobian_actual)
def get_ball_positions(self):
# TODO: get ball positions
vec = gymapi.Vec3(0, 0, 3)
return np.array([vec, vec])
def __init__(self, env_type, num_envs):
args = gymutil.parse_arguments(description="Basketbot Simulaton",
custom_parameters=[{
"name":
"--render",
"action":
"store_true",
"help":
"Render the simulation"
}, {
"name":
"--show_axis",
"action":
"store_true",
"help":
"Visualize DOF axis"
}])
self.render = args.render
self.gym = gymapi.acquire_gym()
# create a simulator
sim_params = gymapi.SimParams()
sim_params.substeps = 2
sim_params.dt = env_type.dt
sim_params.up_axis = gymapi.UP_AXIS_Z
sim_params.gravity = gymapi.Vec3(0.0, 0.0, -9.81)
if args.physics_engine == gymapi.SIM_FLEX:
raise NotImplementedError(
"Flex simulation engine is not supported! Choose PhysX.")
elif args.physics_engine == gymapi.SIM_PHYSX:
sim_params.physx.use_gpu = True
# sim_params.physx.solver_type = 1
self.sim = self.gym.create_sim(args.compute_device_id,
args.graphics_device_id,
args.physics_engine, sim_params)
# Create the ground plane:
plane_params = gymapi.PlaneParams()
plane_params.normal = gymapi.Vec3(0, 0, 1)
self.gym.add_ground(self.sim, plane_params)
# Load the robot:
asset_root = "robot_description"
asset_file = "urdf/robot/wam_4dof.urdf"
asset_options = gymapi.AssetOptions()
asset_options.fix_base_link = True
robot_asset = self.gym.load_asset(self.sim, asset_root, asset_file,
asset_options)
# Make ball asset:
asset_options = gymapi.AssetOptions()
ball_asset = self.gym.create_sphere(self.sim, 0.0375, asset_options)
# Env gird:
envs_per_row = 3
envs_per_row = int(np.sqrt(num_envs))
# env_spacing = 1.2
env_spacing = 0.0
env_lower = gymapi.Vec3(-env_spacing, 0.0, -env_spacing)
env_upper = gymapi.Vec3(env_spacing, env_spacing, env_spacing)
# Create and populate envs:
self.envs = []
for i in range(num_envs):
# env
env = self.gym.create_env(self.sim, env_lower, env_upper,
envs_per_row)
# robot
default_pose = gymapi.Transform() # Default pose
robot = self.gym.create_actor(env, robot_asset, default_pose,
"Robot", i)
# ball 1
ball1_pose = gymapi.Transform()
ball1_pose.p = gymapi.Vec3(0.87, 0.086, 1.07)
ball1 = self.gym.create_actor(env, ball_asset, ball1_pose, "Ball1",
i)
# ball 2
ball2_pose = gymapi.Transform()
ball2_pose.p = gymapi.Vec3(0.87, -0.086, 2.0)
ball2 = self.gym.create_actor(env, ball_asset, ball2_pose, "Ball2",
i)
self.envs.append(
IsaacJugglingWam4(self.gym, env, [robot], [ball1, ball2]))
# Create viewer (if rendering)
self.viewer = None
if self.render:
self.viewer = self.gym.create_viewer(self.sim,
gymapi.CameraProperties())
cam_pos = gymapi.Vec3(1.7, 1.7, 1.5)
cam_target = gymapi.Vec3(0.8, 0, 1.3)
self.gym.viewer_camera_look_at(self.viewer, None, cam_pos,
cam_target)
if self.viewer is None:
self.render = False
print("*** Failed to create viewer -> rendering deactivated")
args = gymutil.parse_arguments(
description="Kuka Bin Test",
custom_parameters=[
{"name": "--num_envs", "type": int, "default": 1, "help": "Number of environments to create"},
{"name": "--num_objects", "type": int, "default": 10, "help": "Number of objects in the bin"},
{"name": "--object_type", "type": int, "default": 0, "help": "Type of bjects to place in the bin: 0 - box, 1 - meat can, 2 - banana, 3 - mug, 4 - brick, 5 - random"}])
num_envs = args.num_envs
# configure sim
sim_type = args.physics_engine
sim_params = gymapi.SimParams()
sim_params.up_axis = gymapi.UP_AXIS_Z
sim_params.gravity = gymapi.Vec3(0.0, 0.0, -9.8)
if sim_type is gymapi.SIM_FLEX:
sim_params.substeps = 4
sim_params.flex.solver_type = 5
sim_params.flex.num_outer_iterations = 6
sim_params.flex.num_inner_iterations = 50
sim_params.flex.relaxation = 0.7
sim_params.flex.warm_start = 0.1
sim_params.flex.shape_collision_distance = 5e-4
sim_params.flex.contact_regularization = 1.0e-6
sim_params.flex.shape_collision_margin = 1.0e-4
sim_params.flex.deterministic_mode = True
sim = gym.create_sim(args.compute_device_id, args.graphics_device_id, sim_type, sim_params)
def main():
"""Run grasp evaluation."""
# Create command line flag options
parser = argparse.ArgumentParser(description='Options')
parser.add_argument('--object', required=True, help="Name of object")
parser.add_argument('--grasp_ind',
default=0,
type=int,
help="Index of grasp candidate to test")
parser.add_argument('--ori_start',
default=0,
type=int,
help="Start index of vectors to test. [0, 15]")
parser.add_argument('--ori_end',
default=5,
type=int,
help="End index of vectors to test. [0, 15]")
parser.add_argument('--density',
default='1000',
type=str,
help="Density of object [kg/m^3]")
parser.add_argument('--youngs',
default='5e5',
type=str,
help="Elastic modulus of object [Pa]")
parser.add_argument('--poissons',
default='0.3',
type=str,
help="Poisson's ratio of object")
parser.add_argument(
'--mode',
default='pickup',
type=str,
help="Name of test to run, one of {pickup, reorient, shake, twist}")
parser.add_argument('--viewer',
dest='viewer',
action='store_true',
help="Display graphical viewer")
parser.add_argument('--no-viewer',
dest='viewer',
action='store_false',
help="Do not display graphical viewer.")
parser.add_argument('--fill',
dest='fill',
action='store_true',
help="Run only tests without pre-existing results")
parser.add_argument(
'--no-fill',
dest='fill',
action='store_false',
help="Run all tests regardless of pre-existing results")
parser.add_argument('--write',
dest='write_results',
action='store_true',
help="Record results")
parser.add_argument('--no-write',
dest='write_results',
action='store_false',
help="Do not record results")
parser.add_argument(
'--tag',
default='',
type=str,
help="Additional tring to add onto name of results files.")
parser.set_defaults(viewer=False)
parser.set_defaults(viewer=True)
parser.set_defaults(write_results=False)
args = parser.parse_args()
use_viewer = args.viewer
write_results = args.write_results
object_name = args.object
object_path = os.path.join(ASSETS_DIR, object_name)
folder_name = object_name + RESULTS_STORAGE_TAG
object_file_name = object_name + "_" + args.density + "_" + args.youngs + "_" + \
args.poissons + "_" + args.mode + "_tag" + args.tag + "_results.h5"
h5_file_path = os.path.join(RESULTS_DIR, folder_name, args.youngs,
object_file_name)
# Optionally skip data collection if good data already exists (args.fill flag)
if os.path.exists(h5_file_path) and args.fill:
existing_h5 = h5py.File(h5_file_path, 'r')
existing_timed_out = existing_h5['timed_out'][args.grasp_ind,
args.ori_start]
existing_succeeded = True
if args.mode == "pickup":
existing_pos_under_gravity_dset = existing_h5[
'positions_under_gravity']
if np.all(existing_pos_under_gravity_dset[args.grasp_ind] == 0):
existing_succeeded = False
if args.mode == "reorient":
reorientation_meshes_dset = existing_h5['reorientation_meshes']
if np.all(reorientation_meshes_dset[args.grasp_ind, args.ori_start,
0] == 0):
existing_succeeded = False
if args.mode == "shake":
shake_fail_accs_dset = existing_h5['shake_fail_accs']
if shake_fail_accs_dset[args.grasp_ind, args.ori_start] == 0.0:
existing_succeeded = False
if args.mode == "twist":
twist_fail_accs_dset = existing_h5['twist_fail_accs']
if twist_fail_accs_dset[args.grasp_ind, args.ori_start] == 0.0:
existing_succeeded = False
existing_h5.close()
if existing_timed_out == 0.0 and existing_succeeded:
print("Data already exists, returning")
return
else:
print("Existing data is imperfect, rerunning")
# Get the grasp candidates
grasp_file_name = object_name + "_grasps.h5"
f = h5py.File(os.path.realpath(os.path.join(object_path, grasp_file_name)),
'r')
grasp_candidate_poses = f['poses'][args.grasp_ind:args.grasp_ind + 1]
num_grasp_poses = f['poses'].shape[0]
f.close()
# Create Gym object
gym = gymapi.acquire_gym()
sim, sim_params = create_sim(gym, use_viewer, args)
# Define scene and environments
envs_per_row = 6
env_dim = 0.3
if args.mode in ["shake", "twist"]:
env_dim = 1.0
# Define environment as half-cube (half in vertical direction)
env_lower = gymapi.Vec3(-env_dim, 0, -env_dim)
env_upper = gymapi.Vec3(env_dim, env_dim, env_dim)
# Define asset properties
asset_root = './'
asset_options = gymapi.AssetOptions()
asset_options.flip_visual_attachments = False
asset_options.armature = 0.0 # Additional moment of inertia due to motors
# 1e-4 # Collision distance for rigid bodies. Minkowski sum of collision
# mesh and sphere. Default value is large, so set explicitly
asset_options.thickness = 0.0
asset_options.linear_damping = 1.0 # Linear damping for rigid bodies
asset_options.angular_damping = 0.0 # Angular damping for rigid bodies
asset_options.disable_gravity = True
# Activates PD position, velocity, or torque controller, instead of doing
# DOF control in post-processing
asset_options.default_dof_drive_mode = gymapi.DOF_MODE_VEL
# Load Franka and object assets
asset_file_franka = 'franka_description/robots/franka_panda_fem_simple_v4_with_arm.urdf'
asset_file_platform = os.path.join(ASSETS_DIR, 'platform.urdf')
asset_file_object = os.path.join(object_path, "soft_body.urdf")
# Set object parameters based on command line args (TODO: Use new methods)
set_parameter_result = False
fail_counter = 0
while set_parameter_result is False and fail_counter < 10:
try:
set_parameter_result = set_object_parameters(
asset_file_object,
density=args.density,
youngs=args.youngs,
poissons=args.poissons)
except BaseException:
fail_counter += 1
pass
# Set asset options
asset_options.fix_base_link = True
asset_handle_franka = gym.load_asset(sim, asset_root, asset_file_franka,
asset_options)
asset_options.fix_base_link = False
asset_options.min_particle_mass = 1e-20 # 1e-4 by default
asset_handle_object = gym.load_asset(sim, asset_root, asset_file_object,
asset_options)
asset_options.fix_base_link = True
asset_handle_platform = gym.load_asset(sim, asset_root,
asset_file_platform, asset_options)
# Define camera properties and create Viewer object
camera_props = gymapi.CameraProperties()
camera_props.width = 1920
camera_props.height = 1080
# Set up camera angles if using graphical display (viewer)
if use_viewer:
viewer = gym.create_viewer(sim, camera_props)
camera_target = gymapi.Vec3(0.0, 1.0, 0.0)
camera_pos = gymapi.Vec3(-0, 1.02, 0.5)
if args.object == "heart":
camera_pos = gymapi.Vec3(-0, 1.02, 0.3)
gym.viewer_camera_look_at(viewer, None, camera_pos, camera_target)
else:
viewer = None
# Define transforms to convert between Trimesh and Isaac Gym conventions
from_trimesh_transform = gymapi.Transform()
from_trimesh_transform.r = gymapi.Quat(0, 0.7071068, 0,
0.7071068) # Rot_y(90), R_ba
neg_rot_x_transform = gymapi.Transform()
neg_rot_x = gymapi.Quat(0.7071068, 0, 0, -0.7071068)
neg_rot_x_transform.r = neg_rot_x
# Get 16 equally spaced vectors in a unit sphere
all_directions, _, _, _ = uniform_sphere.get_uniform_directions_regular(16)
num_directions = len(all_directions)
all_directions = all_directions[args.ori_start:args.ori_end + 1]
# Create environments, Franka actor, and object actor
env_handles = []
franka_handles = []
object_handles = []
platform_handles = []
hand_origins = []
env_spread = grasp_candidate_poses
if args.mode.lower() in ["reorient", "shake", "twist"]:
env_spread = all_directions
for i, test_grasp_pose in enumerate(env_spread):
if args.mode.lower() in ["reorient", "shake", "twist"]:
test_grasp_pose = grasp_candidate_poses[0]
# Create environment
env_handle = gym.create_env(sim, env_lower, env_upper, envs_per_row)
env_handles.append(env_handle)
# Define shared pose/collision parameters
pose = gymapi.Transform()
grasp_transform = gymapi.Transform()
grasp_transform.r = gymapi.Quat(test_grasp_pose[4], test_grasp_pose[5],
test_grasp_pose[6], test_grasp_pose[3])
identity_quat = gymapi.Quat(0., 0., 0., 1.)
_, desired_rpy = metrics_features_utils.get_desired_rpy(
identity_quat, grasp_transform.r)
collision_group = i
collision_filter = 0
# Create Franka actors
pose.p = gymapi.Vec3(test_grasp_pose[0], test_grasp_pose[1],
test_grasp_pose[2])
pose.p = neg_rot_x_transform.transform_vector(pose.p)
pose.p.y += PLATFORM_HEIGHT
franka_handle = gym.create_actor(env_handle, asset_handle_franka, pose,
f"franka_{i}", collision_group, 1)
franka_handles.append(franka_handle)
direction = np.array(
[all_directions[i][1], all_directions[i][2],
all_directions[i][0]]) # Read direction as y-up convention
curr_joint_positions = gym.get_actor_dof_states(
env_handle, franka_handle, gymapi.STATE_ALL)
curr_joint_positions['pos'] = [
0., 0., 0., 0., 0., 0., 0., desired_rpy[0], desired_rpy[1],
desired_rpy[2], 0.0, 0.0, 0.0, 0, 0.04, 0.04
]
curr_joint_positions['pos'] = np.zeros(16)
twist_axis = np.array([0., 0., 1.])
pose_transform = R.from_euler('ZYX', desired_rpy)
twist_transform = R.align_vectors(np.expand_dims(direction, axis=0),
np.expand_dims(twist_axis,
axis=0))[0]
twist_eulers = twist_transform.as_euler('xyz')
pose_correction = twist_transform.inv() * pose_transform
pose_correction_euler = pose_correction.as_euler('xyz')
# Correct for translation offset to match grasp
q0 = np.array([0., 0., -0.112])
q0_ = twist_transform.apply(q0)
disp_offset = q0 - q0_
curr_joint_positions['pos'] = [
disp_offset[0], disp_offset[1], disp_offset[2], twist_eulers[2],
twist_eulers[1], twist_eulers[0], 0., pose_correction_euler[2],
pose_correction_euler[1], pose_correction_euler[0], 0.0, 0.0, 0.0,
0, 0.04, 0.04
]
hand_origin = pose
hand_origins.append(hand_origin)
finger_pose = gymapi.Transform()
finger_pose.p = pose.p
gym.set_actor_dof_states(env_handle, franka_handle,
curr_joint_positions, gymapi.STATE_ALL)
# Create soft object
tet_file_name = os.path.join(object_path, args.object + ".tet")
height_of_object = get_height_of_objects(tet_file_name)
pose = gymapi.Transform()
pose.r = neg_rot_x
pose.p = from_trimesh_transform.transform_vector(
gymapi.Vec3(0.0, 0.0, 0.0))
object_height_buffer = 0.001
pose.p.y += PLATFORM_HEIGHT + object_height_buffer
object_handle = gym.create_actor(env_handle, asset_handle_object, pose,
f"object_{i}", collision_group,
collision_filter)
object_handles.append(object_handle)
# Create platform
height_of_platform = 0.005
pose.p.y -= (height_of_platform + object_height_buffer +
+0.5 * height_of_object)
platform_handle = gym.create_actor(env_handle, asset_handle_platform,
pose, f"platform_{i}",
collision_group, 1)
platform_handles.append(platform_handle)
# Run simulation and view results
state = 'open'
history = 10
f_errs = np.ones(history, dtype=np.float32)
panda_fsms = []
directions = all_directions
for i in range(len(env_handles)):
if args.mode.lower() in ["reorient", "shake", "twist"]:
test_grasp_pose = grasp_candidate_poses[0]
directions = all_directions[i:i + 1]
else:
test_grasp_pose = env_spread[i]
pure_grasp_transform = gymapi.Transform()
pure_grasp_transform.r = gymapi.Quat(test_grasp_pose[4],
test_grasp_pose[5],
test_grasp_pose[6],
test_grasp_pose[3])
grasp_transform = gymapi.Transform()
grasp_transform.r = neg_rot_x * gymapi.Quat(
test_grasp_pose[4], test_grasp_pose[5], test_grasp_pose[6],
test_grasp_pose[3])
panda_fsm = pandafsm.PandaFsm(
gym_handle=gym,
sim_handle=sim,
env_handles=env_handles,
franka_handle=franka_handles[i],
platform_handle=platform_handles[i],
state=state,
object_cof=sim_params.flex.dynamic_friction,
f_errs=f_errs,
grasp_transform=grasp_transform,
obj_name=object_name,
env_id=i,
hand_origin=hand_origins[i],
viewer=viewer,
envs_per_row=envs_per_row,
env_dim=env_dim,
youngs=args.youngs,
density=args.density,
directions=np.asarray(directions),
mode=args.mode.lower())
panda_fsms.append(panda_fsm)
# Make updating plot
all_done = False
loop_start = timeit.default_timer()
while not all_done:
# If it is taking too long, declare fail
if (timeit.default_timer() - loop_start > 700
and panda_fsms[i].state != 'reorient') or (
timeit.default_timer() - loop_start > 400
and panda_fsms[i].state == "squeeze_for_metric"):
print("Timed out")
for i in range(len(env_handles)):
if panda_fsms[i].state != "done":
panda_fsms[i].state = "done"
panda_fsms[i].timed_out = True
if use_viewer:
pass
for i in range(len(env_handles)):
panda_fsms[i].update_previous_particle_state_tensor()
all_done = all(panda_fsms[i].state == 'done'
for i in range(len(env_handles)))
gym.refresh_particle_state_tensor(sim)
for i in range(len(env_handles)):
if panda_fsms[i].state != "done":
panda_fsms[i].run_state_machine()
# Run simulation
gym.simulate(sim)
gym.fetch_results(sim, True)
gym.clear_lines(viewer)
gym.step_graphics(sim)
if use_viewer:
gym.draw_viewer(viewer, sim, True)
# Clean up
if use_viewer:
gym.destroy_viewer(viewer)
gym.destroy_sim(sim)
print("Finished the simulation")
if write_results:
metrics_features_utils.write_metrics_to_h5(args, num_grasp_poses,
num_directions,
h5_file_path, panda_fsms)
return