From 09d6277aa7a7698ed2900c4e330893c1c093e7f9 Mon Sep 17 00:00:00 2001 From: sdf548 Date: Sat, 27 Dec 2025 11:03:40 +0800 Subject: [PATCH] =?UTF-8?q?=E6=8F=90=E4=BA=A4filter=5Famass=5Fsimple?= =?UTF-8?q?=E8=AE=AD=E7=BB=83?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- filter_amass_simple | 279 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 279 insertions(+) create mode 100644 filter_amass_simple diff --git a/filter_amass_simple b/filter_amass_simple new file mode 100644 index 0000000000..bf63c961b9 --- /dev/null +++ b/filter_amass_simple @@ -0,0 +1,279 @@ +import math +import numpy as np +from isaacgym import gymapi, gymutil + + +def clamp(x, min_value, max_value): + return max(min(x, max_value), min_value) + + +# simple asset descriptor for selecting from a list + + +class AssetDesc: + + def __init__(self, file_name, flip_visual_attachments=False): + self.file_name = file_name + self.flip_visual_attachments = flip_visual_attachments +# load asset +asset_root = "/" +asset_root = "./" + +asset_descriptors = [ + AssetDesc("test_good.xml", False), +] + +# parse arguments +args = gymutil.parse_arguments( + description="Joint monkey: Animate degree-of-freedom ranges", + custom_parameters=[{ + "name": + "--asset_id", + "type": + int, + "default": + 0, + "help": + "Asset id (0 - %d)" % (len(asset_descriptors) - 1) + }, { + "name": "--speed_scale", + "type": float, + "default": 1.0, + "help": "Animation speed scale" + }, { + "name": "--show_axis", + "action": "store_true", + "help": "Visualize DOF axis" + }]) + +if args.asset_id < 0 or args.asset_id >= len(asset_descriptors): + print("*** Invalid asset_id specified. Valid range is 0 to %d" % + (len(asset_descriptors) - 1)) + quit() + +# initialize gym +gym = gymapi.acquire_gym() + +# configure sim +sim_params = gymapi.SimParams() +sim_params.dt = dt = 1.0 / 60.0 +if args.physics_engine == gymapi.SIM_FLEX: + pass +elif args.physics_engine == gymapi.SIM_PHYSX: + sim_params.physx.solver_type = 1 + sim_params.physx.num_position_iterations = 6 + sim_params.physx.num_velocity_iterations = 0 + sim_params.physx.num_threads = args.num_threads + sim_params.physx.use_gpu = args.use_gpu + +sim_params.use_gpu_pipeline = False +if args.use_gpu_pipeline: + print("WARNING: Forcing CPU pipeline.") + +sim = gym.create_sim(args.compute_device_id, args.graphics_device_id, + args.physics_engine, sim_params) +if sim is None: + print("*** Failed to create sim") + quit() + +# add ground plane +plane_params = gymapi.PlaneParams() +gym.add_ground(sim, plane_params) + +# create viewer +viewer = gym.create_viewer(sim, gymapi.CameraProperties()) +if viewer is None: + print("*** Failed to create viewer") + quit() + + +asset_file = asset_descriptors[args.asset_id].file_name + +asset_options = gymapi.AssetOptions() +asset_options.fix_base_link = True +asset_options.flip_visual_attachments = asset_descriptors[ + args.asset_id].flip_visual_attachments +asset_options.use_mesh_materials = True +asset_options.replace_cylinder_with_capsule = True + +print("Loading asset '%s' from '%s'" % (asset_file, asset_root)) +asset = gym.load_asset(sim, asset_root, asset_file, asset_options) + +# get array of DOF names +dof_names = gym.get_asset_dof_names(asset) + +# get array of DOF properties +dof_props = gym.get_asset_dof_properties(asset) + +# create an array of DOF states that will be used to update the actors +num_dofs = gym.get_asset_dof_count(asset) +dof_states = np.zeros(num_dofs, dtype=gymapi.DofState.dtype) + +# get list of DOF types +dof_types = [gym.get_asset_dof_type(asset, i) for i in range(num_dofs)] + +# get the position slice of the DOF state array +dof_positions = dof_states['pos'] + +# get the limit-related slices of the DOF properties array +stiffnesses = dof_props['stiffness'] +dampings = dof_props['damping'] +armatures = dof_props['armature'] +has_limits = dof_props['hasLimits'] +lower_limits = dof_props['lower'] +upper_limits = dof_props['upper'] + +# initialize default positions, limits, and speeds (make sure they are in reasonable ranges) +defaults = np.zeros(num_dofs) +speeds = np.zeros(num_dofs) +for i in range(num_dofs): + if has_limits[i]: + if dof_types[i] == gymapi.DOF_ROTATION: + lower_limits[i] = clamp(lower_limits[i], -math.pi, math.pi) + upper_limits[i] = clamp(upper_limits[i], -math.pi, math.pi) + # make sure our default position is in range + if lower_limits[i] > 0.0: + defaults[i] = lower_limits[i] + elif upper_limits[i] < 0.0: + defaults[i] = upper_limits[i] + else: + # set reasonable animation limits for unlimited joints + if dof_types[i] == gymapi.DOF_ROTATION: + # unlimited revolute joint + lower_limits[i] = -math.pi + upper_limits[i] = math.pi + elif dof_types[i] == gymapi.DOF_TRANSLATION: + # unlimited prismatic joint + lower_limits[i] = -1.0 + upper_limits[i] = 1.0 + # set DOF position to default + dof_positions[i] = defaults[i] + # set speed depending on DOF type and range of motion + if dof_types[i] == gymapi.DOF_ROTATION: + speeds[i] = args.speed_scale * clamp( + 2 * + (upper_limits[i] - lower_limits[i]), 0.25 * math.pi, 3.0 * math.pi) + else: + speeds[i] = args.speed_scale * clamp( + 2 * (upper_limits[i] - lower_limits[i]), 0.1, 7.0) + +# Print DOF properties +for i in range(num_dofs): + print("DOF %d" % i) + print(" Name: '%s'" % dof_names[i]) + print(" Type: %s" % gym.get_dof_type_string(dof_types[i])) + print(" Stiffness: %r" % stiffnesses[i]) + print(" Damping: %r" % dampings[i]) + print(" Armature: %r" % armatures[i]) + print(" Limited? %r" % has_limits[i]) + if has_limits[i]: + print(" Lower %f" % lower_limits[i]) + print(" Upper %f" % upper_limits[i]) + +# set up the env grid +num_envs = 36 +num_per_row = 6 +spacing = 2.5 +env_lower = gymapi.Vec3(-spacing, 0.0, -spacing) +env_upper = gymapi.Vec3(spacing, spacing, spacing) + +# position the camera +cam_pos = gymapi.Vec3(17.2, 2.0, 16) +cam_target = gymapi.Vec3(5, -2.5, 13) +gym.viewer_camera_look_at(viewer, None, cam_pos, cam_target) + +# cache useful handles +envs = [] +actor_handles = [] + +print("Creating %d environments" % num_envs) +for i in range(num_envs): + # create env + env = gym.create_env(sim, env_lower, env_upper, num_per_row) + envs.append(env) + + # add actor + pose = gymapi.Transform() + pose.p = gymapi.Vec3(0.0, 1.32, 0.0) + pose.r = gymapi.Quat(-0.707107, 0.0, 0.0, 0.707107) + + actor_handle = gym.create_actor(env, asset, pose, "actor", i, 1) + actor_handles.append(actor_handle) + + # set default DOF positions + gym.set_actor_dof_states(env, actor_handle, dof_states, gymapi.STATE_ALL) + +# joint animation states +ANIM_SEEK_LOWER = 1 +ANIM_SEEK_UPPER = 2 +ANIM_SEEK_DEFAULT = 3 +ANIM_FINISHED = 4 + +# initialize animation state +anim_state = ANIM_SEEK_LOWER +current_dof = 0 +print("Animating DOF %d ('%s')" % (current_dof, dof_names[current_dof])) + +while not gym.query_viewer_has_closed(viewer): + + # step the physics + gym.simulate(sim) + gym.fetch_results(sim, True) + + speed = speeds[current_dof] + + # animate the dofs + if anim_state == ANIM_SEEK_LOWER: + dof_positions[current_dof] -= speed * dt + if dof_positions[current_dof] <= lower_limits[current_dof]: + dof_positions[current_dof] = lower_limits[current_dof] + anim_state = ANIM_SEEK_UPPER + elif anim_state == ANIM_SEEK_UPPER: + dof_positions[current_dof] += speed * dt + if dof_positions[current_dof] >= upper_limits[current_dof]: + dof_positions[current_dof] = upper_limits[current_dof] + anim_state = ANIM_SEEK_DEFAULT + if anim_state == ANIM_SEEK_DEFAULT: + dof_positions[current_dof] -= speed * dt + if dof_positions[current_dof] <= defaults[current_dof]: + dof_positions[current_dof] = defaults[current_dof] + anim_state = ANIM_FINISHED + elif anim_state == ANIM_FINISHED: + dof_positions[current_dof] = defaults[current_dof] + current_dof = (current_dof + 1) % num_dofs + anim_state = ANIM_SEEK_LOWER + print("Animating DOF %d ('%s')" % + (current_dof, dof_names[current_dof])) + + if args.show_axis: + gym.clear_lines(viewer) + + # clone actor state in all of the environments + for i in range(num_envs): + gym.set_actor_dof_states(envs[i], actor_handles[i], dof_states, + gymapi.STATE_POS) + + if args.show_axis: + # get the DOF frame (origin and axis) + dof_handle = gym.get_actor_dof_handle(envs[i], actor_handles[i], + current_dof) + frame = gym.get_dof_frame(envs[i], dof_handle) + + # draw a line from DOF origin along the DOF axis + p1 = frame.origin + p2 = frame.origin + frame.axis * 0.7 + color = gymapi.Vec3(1.0, 0.0, 0.0) + gymutil.draw_line(p1, p2, color, gym, viewer, envs[i]) + + # update the viewer + gym.step_graphics(sim) + gym.draw_viewer(viewer, sim, True) + + # Wait for dt to elapse in real time. + # This synchronizes the physics simulation with the rendering rate. + gym.sync_frame_time(sim) + +print("Done") + +gym.destroy_viewer(viewer) +gym.destroy_sim(sim)