diff --git a/loco_mujoco/datasets/data_generation/conversion/convert_MyoSuiteHumanoid_run.py b/loco_mujoco/datasets/data_generation/conversion/convert_MyoSuiteHumanoid_run.py deleted file mode 100644 index db96c08..0000000 --- a/loco_mujoco/datasets/data_generation/conversion/convert_MyoSuiteHumanoid_run.py +++ /dev/null @@ -1,104 +0,0 @@ -import os -import numpy as np -from loco_mujoco.environments.humanoids import MyoSuiteHumanoid -from loco_mujoco.utils.dataset import adapt_mocap - -from scipy.spatial.transform import Rotation as R - - -def reorder_shoulder_orientation(keys, old, new): - # adapt arm data rotation --> data shoulder rotation order is 'zxy' new one is 'yxy' - arr = np.vstack([dataset[k] for k in keys]).T - arr = R.from_euler(old, arr).as_euler(new).T - for i, k in enumerate(keys): - dataset[k] = arr[i] - - -# get the xml_handle from the environment -xml_handle = MyoSuiteHumanoid().xml_handle - -all_joints = [] -for prefix in ["q_", "dq_"]: - for j in xml_handle.find_all("joint"): - all_joints.append(j.name) - -# define the joints for which we have data -joint_conf = dict(pelvis_tx=(1.0, 0.0), - pelvis_tz=(1.0, 0.0), - pelvis_ty=(1.0, -1.0), - pelvis_tilt=(1.0, -0.22), - pelvis_list=(1.0, 0.0), - pelvis_rotation=(1.0, 0.0), - hip_flexion_r=(1.0, 0.2), - hip_adduction_r=(1.0, 0.0), - hip_rotation_r=(1.0, 0.0), - knee_angle_r=(-1.0, 0.0), - ankle_angle_r=(1.0, 0.1), - hip_flexion_l=(1.0, 0.2), - hip_adduction_l=(1.0, 0.0), - hip_rotation_l=(1.0, 0.0), - knee_angle_l=(-1.0, 0.0), - ankle_angle_l=(1.0, 0.1), - lumbar_extension=(1.0, 0.25), - lumbar_bending=(1.0, 0.0), - lumbar_rotation=(1.0, 0.0), - arm_flex_r=(1.0, 0.0), - arm_add_r=(-1.0, 0.0), - arm_rot_r=(1.0, 0.0), - elbow_flex_r=(1.0, 0.0), - pro_sup_r=(1.0, -np.pi/2), - arm_flex_l=(1.0, 0.0), - arm_add_l=(-1.0, 0.0), - arm_rot_l=(1.0, 0.0), - elbow_flex_l=(1.0, 0.0), - pro_sup_l=(1.0, -np.pi/2) - ) - -unavailable_keys = dict() -for j in all_joints: - if j not in joint_conf.keys(): - jh = xml_handle.find("joint", j) - unavailable_keys[j] = jh.ref if jh.ref is not None else 0.0 - - -rename_map = dict(lumbar_extension="L5_S1_Flex_Ext", - lumbar_bending="L5_S1_Lat_Bending", - lumbar_rotation="L5_S1_axial_rotation", - arm_flex_r='elv_angle_r', - arm_add_r='shoulder_elv_r', - arm_rot_r='shoulder1_r2_r', - arm_flex_l='elv_angle_l', - arm_add_l='shoulder_elv_l', - arm_rot_l='shoulder1_r2_l', - pro_sup_r='pro_sup' - ) - -path_mat = "../00_raw_mocap_data/raw_running_mocap_data.mat" -dir_target_path = "../generated_data" -if not os.path.exists(dir_target_path): - os.makedirs(dir_target_path) -target_path = os.path.join(dir_target_path, "myosuite_humanoid_running.npz") - -dataset = adapt_mocap(path_mat, joint_conf=joint_conf, unavailable_keys=unavailable_keys, rename_map=rename_map, - discard_first=28500, discard_last=1000) - -old_order = 'zxy' -new_order = 'yxy' -keys_right = ['q_elv_angle_r', 'q_shoulder_elv_r', 'q_shoulder1_r2_r'] -keys_left = ['q_elv_angle_l', 'q_shoulder_elv_l', 'q_shoulder1_r2_l'] -reorder_shoulder_orientation(keys_right, old_order, new_order) -reorder_shoulder_orientation(keys_left, old_order, new_order) - -# recalculating the velocity for the arm data -dt = 1.0 / 500.0 -for k in keys_right+keys_left: - data = dataset[k] - vel_data = np.zeros_like(data) - vel_data[:-1] = np.diff(data) / dt - dataset["d"+k] = vel_data - -# remove the last data point -for k, i in dataset.items(): - dataset[k] = i[:-1] - -np.savez(file=target_path, **dataset) diff --git a/loco_mujoco/datasets/data_generation/replay/replay_MyosuiteHumanoid_run.py b/loco_mujoco/datasets/data_generation/replay/replay_MyosuiteHumanoid_run.py deleted file mode 100644 index 7053dea..0000000 --- a/loco_mujoco/datasets/data_generation/replay/replay_MyosuiteHumanoid_run.py +++ /dev/null @@ -1,29 +0,0 @@ -import numpy as np -from loco_mujoco.environments import MyoSuiteHumanoid - - -def experiment(): - np.random.seed(1) - - # define env and data frequencies - env_freq = 1000 # hz, added here as a reminder - traj_data_freq = 500 # hz, frequency of the mocap dataset - desired_contr_freq = 100 # hz, this will also be the dataset frequency after downsampling - n_substeps = env_freq//desired_contr_freq - - # prepare trajectory params - traj_params = dict(traj_path="../generated_data/myosuite_humanoid_running.npz", - traj_dt=(1/traj_data_freq), - control_dt=(1/desired_contr_freq), - clip_trajectory_to_joint_ranges=False) - - # MDP - gamma = 0.99 - horizon = 1000 - mdp = MyoSuiteHumanoid(gamma=gamma, horizon=horizon, n_substeps=n_substeps, traj_params=traj_params) - - mdp.play_trajectory(record=False) - - -if __name__ == '__main__': - experiment() diff --git a/loco_mujoco/datasets/data_generation/replay/replay_MyosuiteHumanoid_walk.py b/loco_mujoco/datasets/data_generation/replay/replay_MyosuiteHumanoid_walk.py deleted file mode 100644 index 317ba42..0000000 --- a/loco_mujoco/datasets/data_generation/replay/replay_MyosuiteHumanoid_walk.py +++ /dev/null @@ -1,29 +0,0 @@ -import numpy as np -from loco_mujoco.environments import MyoSuiteHumanoid - - -def experiment(): - #np.random.seed(1) - - # define env and data frequencies - env_freq = 1000 # hz, added here as a reminder - traj_data_freq = 500 # hz, frequency of the mocap dataset - desired_contr_freq = 100 # hz, this will also be the dataset frequency after downsampling - n_substeps = env_freq//desired_contr_freq - - # prepare trajectory params - traj_params = dict(traj_path="../generated_data/myosuite_humanoid_walking.npz", - traj_dt=(1/traj_data_freq), - control_dt=(1/desired_contr_freq), - clip_trajectory_to_joint_ranges=True) - - # MDP - gamma = 0.99 - horizon = 1000 - mdp = MyoSuiteHumanoid(gamma=gamma, horizon=horizon, n_substeps=n_substeps, traj_params=traj_params) - - mdp.play_trajectory(record=False) - - -if __name__ == '__main__': - experiment() diff --git a/loco_mujoco/utils/__init__.py b/loco_mujoco/utils/__init__.py index d27b8f2..2a0ee84 100644 --- a/loco_mujoco/utils/__init__.py +++ b/loco_mujoco/utils/__init__.py @@ -3,4 +3,5 @@ from .checks import * from .video import video2gif from .domain_randomization import * +from .myomodel_init import fetch_myoskeleton, clear_myoskeleton from .dataset import download_all_datasets, download_real_datasets, download_perfect_datasets