From 912e81384d1c9d2db050ffcc7ed126f810afaafa Mon Sep 17 00:00:00 2001 From: Benjamin Bolte Date: Mon, 1 Apr 2024 02:07:57 -0700 Subject: [PATCH] tweaks --- sim/ref.py | 411 ++++++++++++++++++++++++++++-------- sim/scripts/drop_urdf.py | 42 ++-- sim/scripts/print_joints.py | 4 - sim/stompy/joints.py | 54 ++--- 4 files changed, 375 insertions(+), 136 deletions(-) diff --git a/sim/ref.py b/sim/ref.py index 5e54b6a0..140fdf69 100644 --- a/sim/ref.py +++ b/sim/ref.py @@ -1,4 +1,10 @@ -# API references. +"""API references.""" + +dir_gymapi_indexdomain = [ + "DOMAIN_ACTOR", + "DOMAIN_ENV", + "DOMAIN_SIM", +] dir_gymapi = [ "AXIS_ALL", @@ -256,14 +262,6 @@ "Version", "VhacdParams", "Viewer", - "__builtins__", - "__cached__", - "__doc__", - "__file__", - "__loader__", - "__name__", - "__package__", - "__spec__", "_format_path", "_import_active_version", "absolute_import", @@ -287,32 +285,6 @@ ] dir_simparams = [ - "__class__", - "__delattr__", - "__dir__", - "__doc__", - "__eq__", - "__format__", - "__ge__", - "__getattribute__", - "__getstate__", - "__gt__", - "__hash__", - "__init__", - "__init_subclass__", - "__le__", - "__lt__", - "__module__", - "__ne__", - "__new__", - "__reduce__", - "__reduce_ex__", - "__repr__", - "__setattr__", - "__setstate__", - "__sizeof__", - "__str__", - "__subclasshook__", "dt", "enable_actor_creation_warning", "flex", @@ -331,63 +303,9 @@ "CC_ALL_SUBSTEPS", "CC_LAST_SUBSTEP", "CC_NEVER", - "__class__", - "__delattr__", - "__dir__", - "__doc__", - "__entries", - "__eq__", - "__format__", - "__ge__", - "__getattribute__", - "__getstate__", - "__gt__", - "__hash__", - "__init__", - "__init_subclass__", - "__int__", - "__le__", - "__lt__", - "__members__", - "__module__", - "__ne__", - "__new__", - "__reduce__", - "__reduce_ex__", - "__repr__", - "__setattr__", - "__setstate__", - "__sizeof__", - "__str__", - "__subclasshook__", - "name", ] dir_simparams_physx = [ - "__class__", - "__delattr__", - "__dir__", - "__doc__", - "__eq__", - "__format__", - "__ge__", - "__getattribute__", - "__gt__", - "__hash__", - "__init__", - "__init_subclass__", - "__le__", - "__lt__", - "__module__", - "__ne__", - "__new__", - "__reduce__", - "__reduce_ex__", - "__repr__", - "__setattr__", - "__sizeof__", - "__str__", - "__subclasshook__", "always_use_articulations", "bounce_threshold_velocity", "contact_collection", @@ -405,3 +323,318 @@ "solver_type", "use_gpu", ] + +dir_gym = [ + "acquire_actor_root_state_tensor", + "acquire_dof_force_tensor", + "acquire_dof_state_tensor", + "acquire_force_sensor_tensor", + "acquire_jacobian_tensor", + "acquire_mass_matrix_tensor", + "acquire_net_contact_force_tensor", + "acquire_particle_state_tensor", + "acquire_pneumatic_pressure_tensor", + "acquire_pneumatic_target_tensor", + "acquire_rigid_body_state_tensor", + "add_ground", + "add_heightfield", + "add_lines", + "add_triangle_mesh", + "apply_actor_dof_efforts", + "apply_body_force_at_pos", + "apply_body_forces", + "apply_dof_effort", + "apply_rigid_body_force_at_pos_tensors", + "apply_rigid_body_force_tensors", + "attach_camera_to_body", + "attach_sim", + "begin_aggregate", + "clear_lines", + "create_actor", + "create_aggregate", + "create_asset_force_sensor", + "create_box", + "create_camera_sensor", + "create_capsule", + "create_cloth_grid", + "create_env", + "create_performance_timers", + "create_rigid_body_attractor", + "create_sim", + "create_sphere", + "create_tet_grid", + "create_texture_from_buffer", + "create_texture_from_file", + "create_usd_exporter", + "create_viewer", + "debug_print_asset", + "destroy_camera_sensor", + "destroy_env", + "destroy_performance_timers", + "destroy_sim", + "destroy_usd_exporter", + "destroy_viewer", + "draw_env_rigid_contacts", + "draw_env_soft_contacts", + "draw_viewer", + "enable_actor_dof_force_sensors", + "end_access_image_tensors", + "end_aggregate", + "export_usd_asset", + "export_usd_sim", + "fetch_results", + "find_actor_actuator_index", + "find_actor_dof_handle", + "find_actor_dof_index", + "find_actor_fixed_tendon_joint_index", + "find_actor_handle", + "find_actor_index", + "find_actor_joint_handle", + "find_actor_joint_index", + "find_actor_rigid_body_handle", + "find_actor_rigid_body_index", + "find_actor_tendon_index", + "find_asset_actuator_index", + "find_asset_dof_index", + "find_asset_joint_index", + "find_asset_rigid_body_index", + "find_asset_tendon_index", + "free_texture", + "get_actor_actuator_count", + "get_actor_actuator_joint_name", + "get_actor_actuator_name", + "get_actor_actuator_properties", + "get_actor_asset", + "get_actor_count", + "get_actor_dof_count", + "get_actor_dof_dict", + "get_actor_dof_forces", + "get_actor_dof_frames", + "get_actor_dof_handle", + "get_actor_dof_index", + "get_actor_dof_names", + "get_actor_dof_position_targets", + "get_actor_dof_properties", + "get_actor_dof_states", + "get_actor_dof_velocity_targets", + "get_actor_fixed_tendon_joint_coefficients", + "get_actor_fixed_tendon_joint_name", + "get_actor_force_sensor", + "get_actor_force_sensor_count", + "get_actor_handle", + "get_actor_index", + "get_actor_joint_count", + "get_actor_joint_dict", + "get_actor_joint_handle", + "get_actor_joint_index", + "get_actor_joint_names", + "get_actor_joint_transforms", + "get_actor_name", + "get_actor_rigid_body_count", + "get_actor_rigid_body_dict", + "get_actor_rigid_body_handle", + "get_actor_rigid_body_index", + "get_actor_rigid_body_names", + "get_actor_rigid_body_properties", + "get_actor_rigid_body_shape_indices", + "get_actor_rigid_body_states", + "get_actor_rigid_shape_count", + "get_actor_rigid_shape_properties", + "get_actor_root_rigid_body_handle", + "get_actor_scale", + "get_actor_soft_body_count", + "get_actor_soft_materials", + "get_actor_tendon_count", + "get_actor_tendon_name", + "get_actor_tendon_offset", + "get_actor_tendon_properties", + "get_actor_tetrahedra_range", + "get_actor_triangle_range", + "get_asset_actuator_count", + "get_asset_actuator_joint_name", + "get_asset_actuator_name", + "get_asset_actuator_properties", + "get_asset_dof_count", + "get_asset_dof_dict", + "get_asset_dof_name", + "get_asset_dof_names", + "get_asset_dof_properties", + "get_asset_dof_type", + "get_asset_fixed_tendon_joint_coefficients", + "get_asset_fixed_tendon_joint_name", + "get_asset_joint_count", + "get_asset_joint_dict", + "get_asset_joint_name", + "get_asset_joint_names", + "get_asset_joint_type", + "get_asset_rigid_body_count", + "get_asset_rigid_body_dict", + "get_asset_rigid_body_name", + "get_asset_rigid_body_names", + "get_asset_rigid_body_shape_indices", + "get_asset_rigid_shape_count", + "get_asset_rigid_shape_properties", + "get_asset_soft_body_count", + "get_asset_soft_materials", + "get_asset_tendon_count", + "get_asset_tendon_name", + "get_asset_tendon_properties", + "get_attractor_properties", + "get_camera_image", + "get_camera_image_gpu_tensor", + "get_camera_proj_matrix", + "get_camera_transform", + "get_camera_view_matrix", + "get_dof_frame", + "get_dof_position", + "get_dof_target_position", + "get_dof_target_velocity", + "get_dof_type_string", + "get_dof_velocity", + "get_elapsed_time", + "get_env", + "get_env_count", + "get_env_dof_count", + "get_env_joint_count", + "get_env_origin", + "get_env_rigid_body_count", + "get_env_rigid_body_states", + "get_env_rigid_contact_forces", + "get_env_rigid_contacts", + "get_frame_count", + "get_joint_handle", + "get_joint_name", + "get_joint_position", + "get_joint_target_position", + "get_joint_target_velocity", + "get_joint_transform", + "get_joint_type_string", + "get_joint_velocity", + "get_performance_timers", + "get_pneumatic_pressure", + "get_pneumatic_target", + "get_rigid_angular_velocity", + "get_rigid_body_color", + "get_rigid_body_segmentation_id", + "get_rigid_body_texture", + "get_rigid_contact_forces", + "get_rigid_contacts", + "get_rigid_handle", + "get_rigid_linear_velocity", + "get_rigid_name", + "get_rigid_transform", + "get_sensor", + "get_sim_actor_count", + "get_sim_dof_count", + "get_sim_force_sensor_count", + "get_sim_joint_count", + "get_sim_params", + "get_sim_rigid_body_count", + "get_sim_rigid_body_states", + "get_sim_tetrahedra", + "get_sim_tetrahedra_count", + "get_sim_time", + "get_sim_triangle_count", + "get_sim_triangles", + "get_soft_contacts", + "get_usd_export_root", + "get_vec_actor_dof_states", + "get_vec_env_rigid_contact_forces", + "get_vec_rigid_angular_velocity", + "get_vec_rigid_linear_velocity", + "get_vec_rigid_transform", + "get_version", + "get_viewer_camera_handle", + "get_viewer_camera_transform", + "get_viewer_mouse_position", + "get_viewer_size", + "load_asset", + "load_mjcf", + "load_opensim", + "load_sim", + "load_urdf", + "load_usd", + "omni_connect", + "omni_disconnect", + "poll_viewer_events", + "prepare_sim", + "query_viewer_action_events", + "query_viewer_has_closed", + "refresh_actor_root_state_tensor", + "refresh_dof_force_tensor", + "refresh_dof_state_tensor", + "refresh_force_sensor_tensor", + "refresh_jacobian_tensors", + "refresh_mass_matrix_tensors", + "refresh_net_contact_force_tensor", + "refresh_particle_state_tensor", + "refresh_pneumatic_pressure_tensor", + "refresh_pneumatic_target_tensor", + "refresh_rigid_body_state_tensor", + "render_all_camera_sensors", + "reset_actor_materials", + "reset_actor_particles_to_rest", + "set_actor_dof_position_targets", + "set_actor_dof_properties", + "set_actor_dof_states", + "set_actor_dof_velocity_targets", + "set_actor_fixed_tendon_joint_coefficients", + "set_actor_rigid_body_properties", + "set_actor_rigid_body_states", + "set_actor_rigid_shape_properties", + "set_actor_root_state_tensor", + "set_actor_root_state_tensor_indexed", + "set_actor_scale", + "set_actor_soft_materials", + "set_actor_tendon_offset", + "set_actor_tendon_properties", + "set_asset_fixed_tendon_joint_coefficients", + "set_asset_rigid_shape_properties", + "set_asset_tendon_properties", + "set_attractor_properties", + "set_attractor_target", + "set_camera_location", + "set_camera_transform", + "set_dof_actuation_force_tensor", + "set_dof_actuation_force_tensor_indexed", + "set_dof_position_target_tensor", + "set_dof_position_target_tensor_indexed", + "set_dof_state_tensor", + "set_dof_state_tensor_indexed", + "set_dof_target_position", + "set_dof_target_velocity", + "set_dof_velocity_target_tensor", + "set_dof_velocity_target_tensor_indexed", + "set_env_rigid_body_states", + "set_joint_target_position", + "set_joint_target_velocity", + "set_light_parameters", + "set_particle_state_tensor", + "set_particle_state_tensor_indexed", + "set_pneumatic_pressure", + "set_pneumatic_pressure_tensor", + "set_pneumatic_pressure_tensor_indexed", + "set_pneumatic_target", + "set_pneumatic_target_tensor", + "set_pneumatic_target_tensor_indexed", + "set_rigid_angular_velocity", + "set_rigid_body_color", + "set_rigid_body_segmentation_id", + "set_rigid_body_state_tensor", + "set_rigid_body_texture", + "set_rigid_linear_velocity", + "set_rigid_transform", + "set_sim_device", + "set_sim_params", + "set_sim_rigid_body_states", + "set_usd_export_root", + "simulate", + "start_access_image_tensors", + "step_graphics", + "subscribe_viewer_keyboard_event", + "subscribe_viewer_mouse_event", + "sync_frame_time", + "viewer_camera_look_at", + "write_camera_image_to_file", + "write_viewer_image_to_file", +] diff --git a/sim/scripts/drop_urdf.py b/sim/scripts/drop_urdf.py index 20eb8c04..97f755ea 100644 --- a/sim/scripts/drop_urdf.py +++ b/sim/scripts/drop_urdf.py @@ -7,6 +7,7 @@ """ import logging +import time from dataclasses import dataclass from typing import Any @@ -19,17 +20,21 @@ logger = logging.getLogger(__name__) Gym = Any +Env = Any Sim = Any +Robot = Any Viewer = Any Args = Any -# DRIVE_MODE = gymapi.DOF_MODE_EFFORT -DRIVE_MODE = gymapi.DOF_MODE_POS +DRIVE_MODE = gymapi.DOF_MODE_EFFORT +# DRIVE_MODE = gymapi.DOF_MODE_POS # Stiffness and damping are Kp and Kd parameters for the PD controller # that drives the joints to the desired position. -STIFFNESS = 80.0 -DAMPING = 5.0 +# STIFFNESS = 80.0 +# DAMPING = 5.0 +STIFFNESS = 0.0 +DAMPING = 0.0 # Armature is a parameter that can be used to model the inertia of the joint. # We set it to zero because the URDF already models the inertia of the joints. @@ -39,7 +44,9 @@ @dataclass class GymParams: gym: Gym + env: Env sim: Sim + robot: Robot viewer: Viewer args: Args @@ -57,8 +64,8 @@ def load_gym() -> GymParams: sim_params.dt = 1.0 / 60.0 sim_params.physx.solver_type = 1 - sim_params.physx.num_position_iterations = 4 - sim_params.physx.num_velocity_iterations = 1 + sim_params.physx.num_position_iterations = 5 + sim_params.physx.num_velocity_iterations = 5 sim_params.physx.contact_offset = 0.01 sim_params.physx.rest_offset = 0.0 sim_params.physx.bounce_threshold_velocity = 0.1 @@ -112,11 +119,6 @@ def load_gym() -> GymParams: initial_pose.p = gymapi.Vec3(0.0, 5.0, 0.0) robot = gym.create_actor(env, robot_asset, initial_pose, "robot") - # Log all the DOF names. - dof_props = gym.get_actor_dof_properties(env, robot) - for i, prop in enumerate(dof_props): - logger.debug("DOF %d: %s", i, prop) - # Configure DOF properties. props = gym.get_actor_dof_properties(env, robot) props["driveMode"] = DRIVE_MODE @@ -132,7 +134,9 @@ def load_gym() -> GymParams: return GymParams( gym=gym, + env=env, sim=sim, + robot=robot, viewer=viewer, args=args, ) @@ -140,7 +144,9 @@ def load_gym() -> GymParams: def run_gym(gym: GymParams) -> None: flag = False - joints = Stompy.legs.all_joints() + joints = Stompy.all_joints() + last_time = time.time() + dof_ids = gym.gym.get_actor_dof_dict(gym.env, gym.robot) while not gym.gym.query_viewer_has_closed(gym.viewer): gym.gym.simulate(gym.sim) @@ -150,10 +156,14 @@ def run_gym(gym: GymParams) -> None: gym.gym.sync_frame_time(gym.sim) # Every second, set the target effort for each joint to the reverse. - effort = 1.0 if flag else -1.0 - flag = not flag - for joint in joints: - gym.gym.set_dof_force_target(gym.sim, joint, effort) + curr_time = time.time() + if curr_time - last_time > 1.0: + last_time = curr_time + effort = 10.0 if flag else -10.0 + flag = not flag + for joint in joints: + joint_id = dof_ids[joint] + gym.gym.apply_dof_effort(gym.env, joint_id, effort) gym.gym.destroy_viewer(gym.viewer) gym.gym.destroy_sim(gym.sim) diff --git a/sim/scripts/print_joints.py b/sim/scripts/print_joints.py index 1fc55fe0..58f743f7 100644 --- a/sim/scripts/print_joints.py +++ b/sim/scripts/print_joints.py @@ -55,10 +55,6 @@ def replace_leaves(tree: Dict, prefix: str) -> None: replace_leaves(joint_tree, "") - import json - - print(json.dumps(joint_tree, indent=2)) - # Prints the tree. def print_tree(tree: Dict, depth: int = 0) -> None: for key, value in tree.items(): diff --git a/sim/stompy/joints.py b/sim/stompy/joints.py index c5b6edc5..c866297b 100644 --- a/sim/stompy/joints.py +++ b/sim/stompy/joints.py @@ -42,64 +42,64 @@ def __str__(self) -> str: class Head(Node): - left_right: str = "joint_head_1_x4_1_dof" - up_down: str = "joint_head_1_x4_2_dof" + left_right: str = "joint_head_1_x4_1_dof_x4" + up_down: str = "joint_head_1_x4_2_dof_x4" class Torso(Node): - pitch: str = "joint_torso_1_x8_1_dof" + pitch: str = "joint_torso_1_x8_1_dof_x8" class LeftHand(Node): - hand_roll: str = "joint_left_arm_2_hand_1_x4_1_dof" - hand_grip: str = "joint_left_arm_2_hand_1_x4_2_dof" + hand_roll: str = "joint_left_arm_2_hand_1_x4_1_dof_x4" + hand_grip: str = "joint_left_arm_2_hand_1_x4_2_dof_x4" slider_a: str = "joint_left_arm_2_hand_1_slider_1" slider_b: str = "joint_left_arm_2_hand_1_slider_2" class LeftArm(Node): - shoulder_yaw: str = "joint_left_arm_2_x8_1_dof" - shoulder_pitch: str = "joint_left_arm_2_x8_2_dof" - shoulder_roll: str = "joint_left_arm_2_x6_1_dof" - elbow_yaw: str = "joint_left_arm_2_x6_2_dof" + shoulder_yaw: str = "joint_left_arm_2_x8_1_dof_x8" + shoulder_pitch: str = "joint_left_arm_2_x8_2_dof_x8" + shoulder_roll: str = "joint_left_arm_2_x6_1_dof_x6" + elbow_yaw: str = "joint_left_arm_2_x6_2_dof_x6" hand: Node = LeftHand() class RightHand(Node): - hand_roll: str = "joint_right_arm_1_hand_1_x4_1_dof" - hand_grip: str = "joint_right_arm_1_hand_1_x4_2_dof" + hand_roll: str = "joint_right_arm_1_hand_1_x4_1_dof_x4" + hand_grip: str = "joint_right_arm_1_hand_1_x4_2_dof_x4" slider_a: str = "joint_right_arm_1_hand_1_slider_1" slider_b: str = "joint_right_arm_1_hand_1_slider_2" class RightArm(Node): - shoulder_yaw: str = "joint_right_arm_1_x8_1_dof" - shoulder_pitch: str = "joint_right_arm_1_x8_2_dof" - shoulder_roll: str = "joint_right_arm_1_x6_1_dof" - elbow_yaw: str = "joint_right_arm_1_x6_2_dof" + shoulder_yaw: str = "joint_right_arm_1_x8_1_dof_x8" + shoulder_pitch: str = "joint_right_arm_1_x8_2_dof_x8" + shoulder_roll: str = "joint_right_arm_1_x6_1_dof_x6" + elbow_yaw: str = "joint_right_arm_1_x6_2_dof_x6" hand: Node = RightHand() class LeftLeg(Node): - hip_roll: str = "joint_legs_1_x8_2_dof" - hip_yaw: str = "joint_legs_1_left_leg_1_x8_1_dof" - hip_pitch: str = "joint_legs_1_left_leg_1_x10_1_dof" - knee_motor: str = "joint_legs_1_left_leg_1_x10_2_dof" + hip_roll: str = "joint_legs_1_x8_2_dof_x8" + hip_yaw: str = "joint_legs_1_left_leg_1_x8_1_dof_x8" + hip_pitch: str = "joint_legs_1_left_leg_1_x10_1_dof_x10" + knee_motor: str = "joint_legs_1_left_leg_1_x10_2_dof_x10" knee: str = "joint_legs_1_left_leg_1_knee_revolute" - ankle_motor: str = "joint_legs_1_left_leg_1_x6_1_dof" + ankle_motor: str = "joint_legs_1_left_leg_1_x6_1_dof_x6" ankle: str = "joint_legs_1_left_leg_1_ankle_revolute" - foot_roll: str = "joint_legs_1_left_leg_1_x4_1_dof" + foot_roll: str = "joint_legs_1_left_leg_1_x4_1_dof_x4" class RightLeg(Node): - hip_roll: str = "joint_legs_1_x8_1_dof" - hip_yaw: str = "joint_legs_1_right_leg_1_x8_1_dof" - hip_pitch: str = "joint_legs_1_right_leg_1_x10_2_dof" - knee_motor: str = "joint_legs_1_right_leg_1_x10_1_dof" + hip_roll: str = "joint_legs_1_x8_1_dof_x8" + hip_yaw: str = "joint_legs_1_right_leg_1_x8_1_dof_x8" + hip_pitch: str = "joint_legs_1_right_leg_1_x10_2_dof_x10" + knee_motor: str = "joint_legs_1_right_leg_1_x10_1_dof_x10" knee: str = "joint_legs_1_right_leg_1_knee_revolute" - ankle_motor: str = "joint_legs_1_right_leg_1_x6_1_dof" + ankle_motor: str = "joint_legs_1_right_leg_1_x6_1_dof_x6" ankle: str = "joint_legs_1_right_leg_1_ankle_revolute" - foot_roll: str = "joint_legs_1_right_leg_1_x4_1_dof" + foot_roll: str = "joint_legs_1_right_leg_1_x4_1_dof_x4" class Legs(Node):