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