diff --git a/cfg/task/.motion.yaml.swp b/cfg/task/.motion.yaml.swp new file mode 100644 index 0000000..a7fa921 Binary files /dev/null and b/cfg/task/.motion.yaml.swp differ diff --git a/cfg/task/GR3/motion.yaml b/cfg/task/GR3/motion.yaml new file mode 100644 index 0000000..9a4c465 --- /dev/null +++ b/cfg/task/GR3/motion.yaml @@ -0,0 +1,127 @@ +# @package task +project: track +name: Motion + +viewer: + resolution: [1280, 720] + lookat: [0., 0., 0.] + eye: [2.0, 2.0, 1.0] + +robot: + name: gr3_fourier_hand_6dof +terrain: plane + +num_envs: 4096 +max_episode_length: 10000 + +sim: + step_dt: 0.02 + isaac_physics_dt: 0.005 + mujoco_physics_dt: 0.002 + +input: + action: + class: JointPosition + action_scaling: + .*hip_pitch.*: 0.548 + .*hip_roll.*: 0.351 + .*hip_yaw.*: 0.548 + .*knee_pitch.*: 0.351 + waist_yaw_joint: 0.541 + waist_roll_joint: 0.439 + waist_pitch_joint: 0.439 + .*ankle.*: 0.439 + .*shoulder.*: 0.173 + .*elbow_pitch.*: 0.173 + # .*wrist_roll.*: 0.173 + # .*wrist_pitch.*: 0.293 + # .*wrist_yaw.*: 0.293 + max_delay: 1 + alpha_range: [0.8, 1.0] + +command: + class: MotionLibGR3 + dataset: [ gr3/sfu_29dof] + occlusion: "amass_copycat_occlusion_v3.pkl" + +observation: + policy: + root_linvel_b: {} + root_angvel_b: {noise_std: 0.2} + gravity_multistep: {steps: 4, noise_std: 0.05} + joint_pos_multistep: {joint_names: "(?!.*(wrist)).*", steps: 4, noise_std: 0.05} + prev_actions: {steps: 2} + ref_qpos: {} + ref_root_pos: {} + ref_root_quat: {} + priv: + root_linvel_b: {} + root_angvel_b: {noise_std: 0.0} + gravity_multistep: {steps: 4, noise_std: 0.0} + joint_pos_multistep: {joint_names: "(?!.*(wrist)).*", steps: 4, noise_std: 0.0} + prev_actions: {steps: 2} + ref_qpos: {} + ref_root_pos: {} + ref_root_quat: {} + ref_kp_pos_gap: {} + ref_kp_quat_gap: {} + body_pos_b: {body_names: [left_thigh_pitch_link, right_thigh_pitch_link, + left_shank_pitch_link, right_shank_pitch_link, + left_foot_roll_link, right_foot_roll_link, + left_upper_arm_pitch_link, right_upper_arm_pitch_link, + left_lower_arm_pitch_link, right_lower_arm_pitch_link, + left_hand_roll_link, right_hand_roll_link], + yaw_only: false} + # body_ori_b: {body_names: [left_thigh_pitch_link, right_thigh_pitch_link, + # left_shank_pitch_link, right_shank_pitch_link, + # left_foot_roll_link, right_foot_roll_link, + # left_upper_arm_pitch_link, right_upper_arm_pitch_link, + # left_lower_arm_pitch_link, right_lower_arm_pitch_link, + # left_hand_roll_link, right_hand_roll_link], + # yaw_only: false} + +reward: + loco: + tracking_anchor_pos: {weight: 1.0} + tracking_anchor_quat: {weight: 1.0} + tracking_qpos: {weight: 1.0, + joint_names: "(?!.*(wrist)).*"} + tracking_kp_pos: {weight: 2.0} + tracking_kp_quat: {weight: 2.0} + tracking_kp_lin_vel: {weight: 2.0} + tracking_kp_ang_vel: {weight: 2.0} + + feet_slip: {weight: 1.0, body_names: .*foot_roll_link} + action_rate_l2: {weight: 0.1} + +randomization: + # push: + # body_names: ["base_link"] + # force_range: [0.0, 0.1] + perturb_body_com: + body_names: ["base_link"] + pos_range: [-0.05, 0.05] + push_by_setting_velocity: + velocity_range: [-1.0, 1.0] + perturb_body_mass: + ".*hand_roll_link": [1.0, 2.0] + "^(?!.*hand_roll_link).*": [0.9, 1.1] + # .*: [0.9, 1.1] + perturb_body_materials: + body_names: ".*foot_roll_link" + static_friction_range: [0.2, 1.6] + dynamic_friction_range: [0.2, 1.2] + restitution_range: [0.0, 0.5] + motor_params: + stiffness_range: + .*: [0.8, 1.1] + damping_range: + .*: [0.8, 1.1] + +termination: + success: {} + root_deviation: {max_distance: 0.4} + root_rot_deviation: {threshold: 0.8} + track_kp_z_error: {threshold: 0.25, + body_names: [ left_hand_roll_link, right_hand_roll_link, + left_foot_roll_link, right_foot_roll_link]} diff --git a/cfg/task/motion.yaml b/cfg/task/motion.yaml index 3ac55b3..bf3daae 100644 --- a/cfg/task/motion.yaml +++ b/cfg/task/motion.yaml @@ -41,7 +41,7 @@ input: command: class: MotionLibG1 - dataset: [sfu_29dof, lafan1] + dataset: [g1/sfu_29dof] occlusion: "amass_copycat_occlusion_v3.pkl" observation: @@ -72,13 +72,13 @@ observation: left_elbow_link, right_elbow_link, left_wrist_yaw_link, right_wrist_yaw_link], yaw_only: false} - body_ori_b: {body_names: [left_hip_pitch_link, right_hip_pitch_link, - left_knee_link, right_knee_link, - left_ankle_roll_link, right_ankle_roll_link, - left_shoulder_roll_link, right_shoulder_roll_link, - left_elbow_link, right_elbow_link, - left_wrist_yaw_link, right_wrist_yaw_link], - yaw_only: false} + # body_ori_b: {body_names: [left_hip_pitch_link, right_hip_pitch_link, + # left_knee_link, right_knee_link, + # left_ankle_roll_link, right_ankle_roll_link, + # left_shoulder_roll_link, right_shoulder_roll_link, + # left_elbow_link, right_elbow_link, + # left_wrist_yaw_link, right_wrist_yaw_link], + # yaw_only: false} reward: loco: diff --git a/filesoutside/humanoid.py b/filesoutside/humanoid.py new file mode 100644 index 0000000..89339a8 --- /dev/null +++ b/filesoutside/humanoid.py @@ -0,0 +1,427 @@ +from pathlib import Path +import active_adaptation.utils.symmetry as symmetry_utils + +from active_adaptation.assets.asset_cfg_bak import ( + AssetCfg, + InitialStateCfg, + ActuatorCfg, + ContactSensorCfg, +) +from active_adaptation.registry import Registry + +registry = Registry.instance() + +FILE_DIR = Path(__file__).parent + +ARMATURE_5020 = 0.003609725 +ARMATURE_7520_14 = 0.010177520 +ARMATURE_7520_22 = 0.025101925 +ARMATURE_4010 = 0.00425 + +NATURAL_FREQ = 10 * 2.0 * 3.1415926535 # 10Hz +DAMPING_RATIO = 2.0 + +STIFFNESS_5020 = ARMATURE_5020 * NATURAL_FREQ**2 +STIFFNESS_7520_14 = ARMATURE_7520_14 * NATURAL_FREQ**2 +STIFFNESS_7520_22 = ARMATURE_7520_22 * NATURAL_FREQ**2 +STIFFNESS_4010 = ARMATURE_4010 * NATURAL_FREQ**2 + +DAMPING_5020 = 2.0 * DAMPING_RATIO * ARMATURE_5020 * NATURAL_FREQ +DAMPING_7520_14 = 2.0 * DAMPING_RATIO * ARMATURE_7520_14 * NATURAL_FREQ +DAMPING_7520_22 = 2.0 * DAMPING_RATIO * ARMATURE_7520_22 * NATURAL_FREQ +DAMPING_4010 = 2.0 * DAMPING_RATIO * ARMATURE_4010 * NATURAL_FREQ + + +G1_WAIST_UNLOCKED_CFG = AssetCfg( # no wrist pitch and yaw + mjcf_path=FILE_DIR / "G1" / "mjcf" / "g1.xml", + usd_path=FILE_DIR / "G1" / "waist_unlocked.usd", + init_state=InitialStateCfg( + pos=(0.0, 0.0, 0.85), + joint_pos={ + ".*_hip_pitch_joint": -0.1, + ".*_knee_joint": 0.3, + ".*_ankle_pitch_joint": -0.2, + # ".*_hip_pitch_joint": -0.28, + # ".*_knee_joint": 0.669, + # ".*_ankle_pitch_joint": -0.363, + ".*_elbow_joint": 0.6, + "left_shoulder_roll_joint": 0.2, + "left_shoulder_pitch_joint": 0.2, + "right_shoulder_roll_joint": -0.2, + "right_shoulder_pitch_joint": 0.2, + "waist_yaw_joint": 0.0, + "waist_roll_joint": 0.0, + "waist_pitch_joint": 0.0, + }, + joint_vel={".*": 0.0}, + ), + self_collisions=False, + actuators={ + "base_legs": ActuatorCfg( + joint_names_expr=".*", + effort_limit={ + ".*_hip_yaw_joint.*": 88.0, + ".*_hip_roll_joint.*": 139.0, + ".*_hip_pitch_joint.*": 88.0, + ".*_knee.*": 139.0, + ".*_ankle.*": 50, + ".*_shoulder.*": 25, + ".*_elbow.*": 25, + ".*_wrist_roll_joint": 25.0, + ".*_wrist_pitch_joint": 5.0, + ".*_wrist_yaw_joint": 5.0, + "waist.*": 50, + }, + velocity_limit={ + ".*_hip_yaw_joint": 32.0, + ".*_hip_roll_joint": 20.0, + ".*_hip_pitch_joint": 32.0, + ".*_knee_joint": 20.0, + ".*_ankle.*": 37.0, + "waist.*": 37.0, + ".*_shoulder_pitch_joint": 37.0, + ".*_shoulder_roll_joint": 37.0, + ".*_shoulder_yaw_joint": 37.0, + ".*_elbow_joint": 37.0, + ".*_wrist_roll_joint": 37.0, + ".*_wrist_pitch_joint": 22.0, + ".*_wrist_yaw_joint": 22.0, + }, + stiffness={ + ".*_hip_yaw_joint": STIFFNESS_7520_14 * 2.0, + ".*_hip_roll_joint": STIFFNESS_7520_22, + ".*_hip_pitch_joint": STIFFNESS_7520_14 * 4.0, + ".*_knee_joint": STIFFNESS_7520_22, + ".*ankle.*": 2.0 * STIFFNESS_5020, + "waist.*": 2.0 * STIFFNESS_5020, + ".*_shoulder_pitch_joint": STIFFNESS_5020, + ".*_shoulder_roll_joint": STIFFNESS_5020, + ".*_shoulder_yaw_joint": STIFFNESS_5020, + ".*_elbow_joint": STIFFNESS_5020, + ".*_wrist_roll_joint": STIFFNESS_5020, + ".*_wrist_pitch_joint": STIFFNESS_4010, + ".*_wrist_yaw_joint": STIFFNESS_4010, + }, + damping={ + ".*_hip_pitch_joint": DAMPING_7520_14, + ".*_hip_roll_joint": DAMPING_7520_22, + ".*_hip_yaw_joint": DAMPING_7520_14, + ".*_knee_joint": DAMPING_7520_22, + ".*ankle.*": 2.0 * DAMPING_5020, + "waist.*": 2.0 * DAMPING_5020, + ".*_shoulder_pitch_joint": DAMPING_5020, + ".*_shoulder_roll_joint": DAMPING_5020, + ".*_shoulder_yaw_joint": DAMPING_5020, + ".*_elbow_joint": DAMPING_5020, + ".*_wrist_roll_joint": DAMPING_5020, + ".*_wrist_pitch_joint": DAMPING_4010, + ".*_wrist_yaw_joint": DAMPING_4010, + }, + armature={ + ".*_hip_pitch_joint": ARMATURE_7520_14, + ".*_hip_roll_joint": ARMATURE_7520_22, + ".*_hip_yaw_joint": ARMATURE_7520_14, + ".*_knee_joint": ARMATURE_7520_22, + ".*ankle.*": 2.0 * ARMATURE_5020, + "waist.*": 2.0 * ARMATURE_5020, + ".*_shoulder_pitch_joint": ARMATURE_5020, + ".*_shoulder_roll_joint": ARMATURE_5020, + ".*_shoulder_yaw_joint": ARMATURE_5020, + ".*_elbow_joint": ARMATURE_5020, + ".*_wrist_roll_joint": ARMATURE_5020, + ".*_wrist_pitch_joint": ARMATURE_4010, + ".*_wrist_yaw_joint": ARMATURE_4010, + }, + friction=0.01, + ), + }, + joint_symmetry_mapping=symmetry_utils.mirrored({ + "left_hip_pitch_joint": (1, "right_hip_pitch_joint"), + "left_hip_roll_joint": (-1, "right_hip_roll_joint"), + "left_hip_yaw_joint": (-1, "right_hip_yaw_joint"), + "left_knee_joint": (1, "right_knee_joint"), + "left_ankle_pitch_joint": (1, "right_ankle_pitch_joint"), + "left_ankle_roll_joint": (-1, "right_ankle_roll_joint"), + "waist_yaw_joint": (-1, "waist_yaw_joint"), + "waist_roll_joint": (-1, "waist_roll_joint"), + "waist_pitch_joint": (1, "waist_pitch_joint"), + "left_shoulder_pitch_joint": (1, "right_shoulder_pitch_joint"), + "left_shoulder_roll_joint": (-1, "right_shoulder_roll_joint"), + "left_shoulder_yaw_joint": (-1, "right_shoulder_yaw_joint"), + "left_elbow_joint": (1, "right_elbow_joint"), + "left_wrist_roll_joint": (-1, "right_wrist_roll_joint"), + "left_wrist_pitch_joint": (1, "right_wrist_pitch_joint"), + "left_wrist_yaw_joint": (-1, "right_wrist_yaw_joint") + }), + spatial_symmetry_mapping=symmetry_utils.mirrored({ + "left_hip_pitch_link": "right_hip_pitch_link", + "left_hip_roll_link": "right_hip_roll_link", + "left_hip_yaw_link": "right_hip_yaw_link", + "left_knee_link": "right_knee_link", + "left_ankle_pitch_link": "right_ankle_pitch_link", + "left_ankle_roll_link": "right_ankle_roll_link", + "pelvis": "pelvis", + "torso_link": "torso_link", + "waist_yaw_link": "waist_yaw_link", + "waist_roll_link": "waist_roll_link", + "left_shoulder_pitch_link": "right_shoulder_pitch_link", + "left_shoulder_roll_link": "right_shoulder_roll_link", + "left_shoulder_yaw_link": "right_shoulder_yaw_link", + "left_elbow_link": "right_elbow_link", + "left_wrist_roll_link": "right_wrist_roll_link", + "left_wrist_yaw_link": "right_wrist_yaw_link", + "left_wrist_pitch_link": "right_wrist_pitch_link" + }), + sensors_isaaclab=[ + ContactSensorCfg( + name="contact_forces", + primary=".*", + secondary=[], + track_air_time=True, + history_length=3 + ), + ], + sensors_mjlab=[ + ContactSensorCfg( + name="contact_forces", + primary=".*", + secondary=[], + track_air_time=True, + history_length=3 + ), + ], + joint_names_isaac=[ + 'left_hip_pitch_joint', 'right_hip_pitch_joint', + 'waist_yaw_joint', + 'left_hip_roll_joint', 'right_hip_roll_joint', + 'waist_roll_joint', + 'left_hip_yaw_joint', 'right_hip_yaw_joint', + 'waist_pitch_joint', + 'left_knee_joint', 'right_knee_joint', + 'left_shoulder_pitch_joint', 'right_shoulder_pitch_joint', + 'left_ankle_pitch_joint', 'right_ankle_pitch_joint', + 'left_shoulder_roll_joint', 'right_shoulder_roll_joint', + 'left_ankle_roll_joint', 'right_ankle_roll_joint', + 'left_shoulder_yaw_joint', 'right_shoulder_yaw_joint', + 'left_elbow_joint', 'right_elbow_joint', + 'left_wrist_roll_joint', 'right_wrist_roll_joint', + 'left_wrist_pitch_joint', 'right_wrist_pitch_joint', + 'left_wrist_yaw_joint', 'right_wrist_yaw_joint' + ], + body_names_isaac=[ + 'pelvis', + 'left_hip_pitch_link', 'right_hip_pitch_link', + 'waist_yaw_link', + 'left_hip_roll_link','right_hip_roll_link', + 'waist_roll_link', + 'left_hip_yaw_link', 'right_hip_yaw_link', + 'torso_link', + 'left_knee_link', 'right_knee_link', + 'left_shoulder_pitch_link', 'right_shoulder_pitch_link', + 'left_ankle_pitch_link', 'right_ankle_pitch_link', + 'left_shoulder_roll_link', 'right_shoulder_roll_link', + 'left_ankle_roll_link', 'right_ankle_roll_link', + 'left_shoulder_yaw_link', 'right_shoulder_yaw_link', + 'left_elbow_link', 'right_elbow_link', + 'left_wrist_roll_link', 'right_wrist_roll_link', + 'left_wrist_pitch_link', 'right_wrist_pitch_link', + 'left_wrist_yaw_link', 'right_wrist_yaw_link' + ], +) +registry.register("asset", "g1_waist_unlocked", G1_WAIST_UNLOCKED_CFG) + +GR3_CFG = AssetCfg( + urdf_path=FILE_DIR / "GR3" / "gr3_fourier_hand_6dof.urdf", + usd_path=FILE_DIR / "GR3" / "gr3_fourier_hand_6dof.usd", # Note: may need to create USD file + use_urdf_in_isaac=True, + init_state=InitialStateCfg( + pos=(0.0, 0.0, 0.70), + joint_pos={ + ".*_hip_pitch_joint": -0.112, + ".*_knee_pitch_joint": 0.369, + ".*_ankle_pitch_joint": -0.263, + ".*_elbow_pitch_joint": -0.3, + "left_shoulder_roll_joint": 0.1, + "left_shoulder_pitch_joint": 0.1, + "right_shoulder_roll_joint": -0.1, + "right_shoulder_pitch_joint": 0.1, + "waist_yaw_joint": 0.0, + "waist_roll_joint": 0.0, + "waist_pitch_joint": 0.0, + }, + joint_vel={".*": 0.0}, + ), + self_collisions=False, + actuators={ + "base_legs": ActuatorCfg( + joint_names_expr=".*", + effort_limit={ + ".*_hip_yaw_joint.*": 88.0, + ".*_hip_roll_joint.*": 139.0, + ".*_hip_pitch_joint.*": 88.0, + ".*_knee_pitch_joint.*": 139.0, + ".*_ankle.*": 50.0, + ".*_shoulder.*": 25.0, + ".*_elbow_pitch_joint.*": 25.0, + ".*_wrist_roll_joint": 25.0, + ".*_wrist_pitch_joint": 5.0, + ".*_wrist_yaw_joint": 5.0, + "waist.*": 50.0, + }, + velocity_limit={ + ".*_hip_yaw_joint": 32.0, + ".*_hip_roll_joint": 20.0, + ".*_hip_pitch_joint": 32.0, + ".*_knee_pitch_joint": 20.0, + ".*_ankle.*": 37.0, + "waist.*": 37.0, + ".*_shoulder_pitch_joint": 37.0, + ".*_shoulder_roll_joint": 37.0, + ".*_shoulder_yaw_joint": 37.0, + ".*_elbow_pitch_joint": 37.0, + ".*_wrist_roll_joint": 37.0, + ".*_wrist_pitch_joint": 22.0, + ".*_wrist_yaw_joint": 22.0, + }, + stiffness={ + ".*_hip_yaw_joint": STIFFNESS_7520_14, + ".*_hip_roll_joint": STIFFNESS_7520_22, + ".*_hip_pitch_joint": STIFFNESS_7520_14, + ".*_knee_pitch_joint": STIFFNESS_7520_22, + ".*ankle.*": 2.0 * STIFFNESS_5020, + "waist.*": 2.0 * STIFFNESS_5020, + ".*_shoulder_pitch_joint": STIFFNESS_5020, + ".*_shoulder_roll_joint": STIFFNESS_5020, + ".*_shoulder_yaw_joint": STIFFNESS_5020, + ".*_elbow_pitch_joint": STIFFNESS_5020, + ".*_wrist_roll_joint": STIFFNESS_5020, + ".*_wrist_pitch_joint": STIFFNESS_4010, + ".*_wrist_yaw_joint": STIFFNESS_4010, + }, + damping={ + ".*_hip_pitch_joint": DAMPING_7520_14, + ".*_hip_roll_joint": DAMPING_7520_22, + ".*_hip_yaw_joint": DAMPING_7520_14, + ".*_knee_pitch_joint": DAMPING_7520_22, + ".*ankle.*": 2.0 * DAMPING_5020, + "waist.*": 2.0 * DAMPING_5020, + ".*_shoulder_pitch_joint": DAMPING_5020, + ".*_shoulder_roll_joint": DAMPING_5020, + ".*_shoulder_yaw_joint": DAMPING_5020, + ".*_elbow_pitch_joint": DAMPING_5020, + ".*_wrist_roll_joint": DAMPING_5020, + ".*_wrist_pitch_joint": DAMPING_4010, + ".*_wrist_yaw_joint": DAMPING_4010, + }, + armature={ + ".*_hip_pitch_joint": ARMATURE_7520_14, + ".*_hip_roll_joint": ARMATURE_7520_22, + ".*_hip_yaw_joint": ARMATURE_7520_14, + ".*_knee_pitch_joint": ARMATURE_7520_22, + ".*ankle.*": 2.0 * ARMATURE_5020, + "waist.*": 2.0 * ARMATURE_5020, + ".*_shoulder_pitch_joint": ARMATURE_5020, + ".*_shoulder_roll_joint": ARMATURE_5020, + ".*_shoulder_yaw_joint": ARMATURE_5020, + ".*_elbow_pitch_joint": ARMATURE_5020, + ".*_wrist_roll_joint": ARMATURE_5020, + ".*_wrist_pitch_joint": ARMATURE_4010, + ".*_wrist_yaw_joint": ARMATURE_4010, + }, + friction=0.01, + ), + }, + joint_symmetry_mapping=symmetry_utils.mirrored({ + "left_hip_pitch_joint": (1, "right_hip_pitch_joint"), + "left_hip_roll_joint": (-1, "right_hip_roll_joint"), + "left_hip_yaw_joint": (-1, "right_hip_yaw_joint"), + "left_knee_pitch_joint": (1, "right_knee_pitch_joint"), + "left_ankle_pitch_joint": (1, "right_ankle_pitch_joint"), + "left_ankle_roll_joint": (-1, "right_ankle_roll_joint"), + "waist_yaw_joint": (-1, "waist_yaw_joint"), + "waist_roll_joint": (-1, "waist_roll_joint"), + "waist_pitch_joint": (1, "waist_pitch_joint"), + "left_shoulder_pitch_joint": (1, "right_shoulder_pitch_joint"), + "left_shoulder_roll_joint": (-1, "right_shoulder_roll_joint"), + "left_shoulder_yaw_joint": (-1, "right_shoulder_yaw_joint"), + "left_elbow_pitch_joint": (1, "right_elbow_pitch_joint"), + "left_wrist_roll_joint": (-1, "right_wrist_roll_joint"), + "left_wrist_pitch_joint": (1, "right_wrist_pitch_joint"), + "left_wrist_yaw_joint": (-1, "right_wrist_yaw_joint") + }), + spatial_symmetry_mapping=symmetry_utils.mirrored({ + "left_thigh_pitch_link": "right_thigh_pitch_link", + "left_thigh_roll_link": "right_thigh_roll_link", + "left_thigh_yaw_link": "right_thigh_yaw_link", + "left_shank_pitch_link": "right_shank_pitch_link", + "left_foot_pitch_link": "right_foot_pitch_link", + "left_foot_roll_link": "right_foot_roll_link", + "base_link": "base_link", + "torso_link": "torso_link", + "waist_yaw_link": "waist_yaw_link", + "waist_roll_link": "waist_roll_link", + "left_upper_arm_pitch_link": "right_upper_arm_pitch_link", + "left_upper_arm_roll_link": "right_upper_arm_roll_link", + "left_upper_arm_yaw_link": "right_upper_arm_yaw_link", + "left_lower_arm_pitch_link": "right_lower_arm_pitch_link", + "left_hand_yaw_link": "right_hand_yaw_link", + "left_hand_pitch_link": "right_hand_pitch_link", + "left_hand_roll_link": "right_hand_roll_link" + }), + sensors_isaaclab=[ + ContactSensorCfg( + name="contact_forces", + primary=".*", + secondary=[], + track_air_time=True, + history_length=3 + ), + ], + sensors_mjlab=[ + ContactSensorCfg( + name="contact_forces", + primary=".*", + secondary=[], + track_air_time=True, + history_length=3 + ), + ], + joint_names_isaac=[ + 'left_hip_pitch_joint', 'right_hip_pitch_joint', + 'waist_yaw_joint', + 'left_hip_roll_joint', 'right_hip_roll_joint', + 'waist_roll_joint', + 'left_hip_yaw_joint', 'right_hip_yaw_joint', + 'waist_pitch_joint', + 'left_knee_pitch_joint', 'right_knee_pitch_joint', + 'left_shoulder_pitch_joint', 'right_shoulder_pitch_joint', + 'left_ankle_pitch_joint', 'right_ankle_pitch_joint', + 'left_shoulder_roll_joint', 'right_shoulder_roll_joint', + 'left_ankle_roll_joint', 'right_ankle_roll_joint', + 'left_shoulder_yaw_joint', 'right_shoulder_yaw_joint', + 'left_elbow_pitch_joint', 'right_elbow_pitch_joint', + 'left_wrist_roll_joint', 'right_wrist_roll_joint', + 'left_wrist_pitch_joint', 'right_wrist_pitch_joint', + 'left_wrist_yaw_joint', 'right_wrist_yaw_joint' + ], + body_names_isaac=[ + 'base_link', + 'left_thigh_pitch_link', 'right_thigh_pitch_link', + 'waist_yaw_link', + 'left_thigh_roll_link', 'right_thigh_roll_link', + 'waist_roll_link', + 'left_thigh_yaw_link', 'right_thigh_yaw_link', + 'torso_link', + 'left_shank_pitch_link', 'right_shank_pitch_link', + 'left_upper_arm_pitch_link', 'right_upper_arm_pitch_link', + 'left_foot_pitch_link', 'right_foot_pitch_link', + 'left_upper_arm_roll_link', 'right_upper_arm_roll_link', + 'left_foot_roll_link', 'right_foot_roll_link', + 'left_upper_arm_yaw_link', 'right_upper_arm_yaw_link', + 'left_lower_arm_pitch_link', 'right_lower_arm_pitch_link', + 'left_hand_yaw_link', 'right_hand_yaw_link', + 'left_hand_pitch_link', 'right_hand_pitch_link', + 'left_hand_roll_link', 'right_hand_roll_link' + ], +) +registry.register("asset", "gr3_fourier_hand_6dof", GR3_CFG) diff --git a/src/track/command.py b/src/track/command.py index f87e61e..2879110 100644 --- a/src/track/command.py +++ b/src/track/command.py @@ -352,4 +352,101 @@ def __init__( joint_range, anchor_body, keypoint_body, - ) \ No newline at end of file + ) + +class MotionLibGR3(MotionLib): + + def __init__( + self, + env, + dataset: List[str], + occlusion: str, + pose_range: Dict[str, Tuple[float, float]] = { + "x": (-0.05, 0.05), + "y": (-0.05, 0.05), + "z": (-0.01, 0.01), + "roll": (-0.1, 0.1), + "pitch": (-0.1, 0.1), + "yaw": (-1.5, 1.5), + }, + velocity_range: Dict[str, Tuple[float, float]] = { + "x": (-0.2, 0.2), + "y": (-0.2, 0.2), + "z": (-0.2, 0.2), + "roll": (-0.52, 0.52), + "pitch": (-0.52, 0.52), + "yaw": (-0.78, 0.78), + }, + joint_range: Tuple[float, float] = (-0.1, 0.1), + anchor_body: str = "base_link", + keypoint_body: List[str] = [ + "base_link", + "left_thigh_pitch_link", "right_thigh_pitch_link", + "left_shank_pitch_link", "right_shank_pitch_link", + "left_foot_roll_link", "right_foot_roll_link", + "left_upper_arm_pitch_link", "right_upper_arm_pitch_link", + "left_lower_arm_pitch_link", "right_lower_arm_pitch_link", + "left_hand_roll_link", "right_hand_roll_link" + ], + ): + # 定义 29 个核心关节（不含手指和头部） + self.core_joint_names = [ + "waist_yaw_joint", "waist_roll_joint", "waist_pitch_joint", + "left_hip_pitch_joint", "left_hip_roll_joint", "left_hip_yaw_joint", "left_knee_pitch_joint", "left_ankle_pitch_joint", "left_ankle_roll_joint", + "right_hip_pitch_joint", "right_hip_roll_joint", "right_hip_yaw_joint", "right_knee_pitch_joint", "right_ankle_pitch_joint", "right_ankle_roll_joint", + "left_shoulder_pitch_joint", "left_shoulder_roll_joint", "left_shoulder_yaw_joint", "left_elbow_pitch_joint", "left_wrist_yaw_joint", "left_wrist_pitch_joint", "left_wrist_roll_joint", + "right_shoulder_pitch_joint", "right_shoulder_roll_joint", "right_shoulder_yaw_joint", "right_elbow_pitch_joint", "right_wrist_yaw_joint", "right_wrist_pitch_joint", "right_wrist_roll_joint" + ] + + super().__init__( + env, + dataset, + occlusion, + pose_range, + velocity_range, + joint_range, + anchor_body, + keypoint_body, + ) + + def load_data(self, data): + robot_joint_names = self.robot.joint_names + robot_body_names = self.robot.body_names + + mapped_data = {} + for k, motion in data.items(): + T = motion["joint_pos"].shape[0] + + # 情况 A: 输入数据是 29 个核心关节 (G1 同步后的核心) + if motion["joint_pos"].shape[1] == 29: + new_joint_pos = np.zeros((T, len(robot_joint_names)), dtype=np.float32) + new_joint_vel = np.zeros((T, len(robot_joint_names)), dtype=np.float32) + for i, name in enumerate(self.core_joint_names): + if name in robot_joint_names: + idx = robot_joint_names.index(name) + new_joint_pos[:, idx] = motion["joint_pos"][:, i] + new_joint_vel[:, idx] = motion["joint_vel"][:, i] + motion["joint_pos"] = new_joint_pos + motion["joint_vel"] = new_joint_vel + + # 填充 Body 信息以匹配机器人结构 + if motion["body_pos_w"].shape[1] != len(robot_body_names): + new_body_pos = np.zeros((T, len(robot_body_names), 3), dtype=np.float32) + new_body_quat = np.zeros((T, len(robot_body_names), 4), dtype=np.float32) + new_body_quat[..., 0] = 1.0 + new_body_lin = np.zeros((T, len(robot_body_names), 3), dtype=np.float32) + new_body_ang = np.zeros((T, len(robot_body_names), 3), dtype=np.float32) + + new_body_pos[:, 0] = motion["body_pos_w"][:, 0] + new_body_quat[:, 0] = motion["body_quat_w"][:, 0] + new_body_lin[:, 0] = motion["body_lin_vel_w"][:, 0] + new_body_ang[:, 0] = motion["body_ang_vel_w"][:, 0] + + motion["body_pos_w"] = new_body_pos + motion["body_quat_w"] = new_body_quat + motion["body_lin_vel_w"] = new_body_lin + motion["body_ang_vel_w"] = new_body_ang + + mapped_data[k] = motion + + super().load_data(mapped_data) \ No newline at end of file