diff --git a/src/humanoid_loco_mujoco/main.py b/src/humanoid_loco_mujoco/main.py index 8d2b980da3..6e87d037c5 100644 --- a/src/humanoid_loco_mujoco/main.py +++ b/src/humanoid_loco_mujoco/main.py @@ -1,6 +1,129 @@ +from typing import List, Any, Union, Tuple, Dict +from types import ModuleType + import numpy as np -from loco_mujoco.task_factories import ImitationFactory, LAFAN1DatasetConf, DefaultDatasetConf, AMASSDatasetConf -from loco_mujoco.core import ObservationType +import jax +import jax.numpy as jnp +from flax import struct +from dataclasses import asdict + +from mujoco import MjSpec, MjModel, MjData +from mujoco.mjx import Model, Data + +import numpy as np +from loco_mujoco.task_factories import ImitationFactory, DefaultDatasetConf +from loco_mujoco.environments import UnitreeG1 + +from loco_mujoco.core.observations import Observation, StatefulObservation +from loco_mujoco.core.control_functions.pd import PDControl, PDControlState + + +# ----- CUSTOM ENVIRONMENT ----- +# custom environment class with fixed waist yaw joint +class CustomUnitreeG1(UnitreeG1): + def __init__(self, spec=None, **kwargs): + self._waist_yaw_joint_name = "waist_yaw_joint" + self._waist_yaw_actuator_name = "waist_yaw" + super().__init__(spec=spec or self.get_default_xml_file_path(), **kwargs) + + @staticmethod + def fix_waist_yaw(spec: MjSpec): + spec.joints = [j for j in spec.joints if j.name != "waist_yaw_joint"] + + def _get_observation_specification(self, spec: MjSpec) -> List[Observation]: + obs = super()._get_observation_specification(spec) + return [o for o in obs if o.xml_name != self._waist_yaw_joint_name] + + def _get_action_specification(self, spec: MjSpec) -> List[str]: + actions = super()._get_action_specification(spec) + return [a for a in actions if a != self._waist_yaw_actuator_name] + +# ----- CUSTOM CONTROL FUNCTION ----- +@struct.dataclass +class CustomControlFunctionState(PDControlState): + moving_average: Union[np.ndarray, jnp.ndarray] + +class CustomControlFunction(PDControl): + def generate_action(self, env, action, model, data, carry, backend): + orig_action, carry = super().generate_action(env, action, model, data, carry, backend) + ma = 0.99 * carry.control_func_state.moving_average + 0.01 * orig_action + state = carry.control_func_state.replace(moving_average=ma) + carry = carry.replace(control_func_state=state) + return ma, carry + + def init_state(self, env, key, model, data, backend): + orig_state = super().init_state(env, key, model, data, backend) + dim = env.info.action_space.shape[0] + ma = backend.zeros_like(dim) + return CustomControlFunctionState(moving_average=ma, **asdict(orig_state)) + + + +# ----- CUSTOM OBSERVATIONS ----- +# custom observation class to observe the center of mass position of the pelvis +class CustomBodyCOMPos(Observation): + dim = 3 + + def __init__(self, name: str, xml_name: str): + self.xml_name = xml_name + super().__init__(name) + + def _init_from_mj(self, env, model, data, current_obs_size): + self.min, self.max = [-np.inf] * self.dim, [np.inf] * self.dim + self.data_type_ind = np.array(self.to_list(data.body(self.xml_name).id)) + self.obs_ind = np.arange(current_obs_size, current_obs_size + self.dim) + self._initialized_from_mj = True + + @classmethod + def data_type(cls): + return "xipos" + + +# custom observation class to observe the moving average of the center of mass position of the pelvis +@struct.dataclass +class CustomBodyCOMPosMovingAverageState: + moving_average: Union[np.ndarray, jnp.ndarray] + +class CustomBodyCOMPosMovingAverage(StatefulObservation): + dim = 3 + + def __init__(self, name: str, xml_name: str): + self.xml_name = xml_name + super().__init__(name) + + def _init_from_mj(self, env, model, data, current_obs_size): + self.min, self.max = [-np.inf] * self.dim, [np.inf] * self.dim + self.obs_ind = np.arange(current_obs_size, current_obs_size + self.dim) + self.data_type_ind = np.array(self.to_list(data.body(self.xml_name).id)) + self._initialized_from_mj = True + + def init_state(self, env, key, model, data, backend): + return CustomBodyCOMPosMovingAverageState(moving_average=backend.zeros(self.dim)) + + def get_obs_and_update_state(self, env, model, data, carry, backend): + obs_states = carry.observation_states + obs_state = getattr(obs_states, self.name) + xipos = backend.squeeze(data.xipos[self.data_type_ind]) + ma = 0.9 * obs_state.moving_average + 0.1 * xipos + obs_states = obs_states.replace(**{self.name: obs_state.replace(moving_average=ma)}) + carry = carry.replace(observation_states=obs_states) + return backend.ravel(ma), carry + + +# ----- REGISTRATION ----- +CustomUnitreeG1.register() +CustomControlFunction.register() +CustomBodyCOMPos.register() +CustomBodyCOMPosMovingAverage.register() + + +# ----- ENVIRONMENT SETUP ----- +observation_spec = [ + CustomBodyCOMPos("pelvis_com", "pelvis"), + CustomBodyCOMPosMovingAverage("pelvis_com_mov_avg", "pelvis"), +] + + randomization_config = { # gravity "randomize_gravity": True, @@ -61,41 +184,19 @@ "add_free_joint_ang_vel_noise": True, "ang_vel_noise_scale": 0.02, } -observation_spec = [ - # prioritized observations - ObservationType.FreeJointPosNoXY(obs_name="free_joint", xml_name="root", group="prioritized", allow_randomization=False), - ObservationType.FreeJointVel(obs_name="free_joint_vel", xml_name="root", group="prioritized", allow_randomization=False), - # 修正左髋关节名称(之前的KeyError问题) - ObservationType.JointPos(obs_name="joint_pos", xml_name="hip_flexion_l", group="prioritized", allow_randomization=False), - # 修正右髋关节名称 - ObservationType.JointVel(obs_name="joint_vel1", xml_name="hip_flexion_r", group="prioritized", allow_randomization=False), - # 修正左膝关节名称 - ObservationType.JointVel(obs_name="joint_vel2", xml_name="knee_angle_l", group="prioritized", allow_randomization=False), - # 将head替换为有效的身体名称(例如torso_link) - ObservationType.BodyPos(obs_name="head_pos", xml_name="torso_link", group="prioritized", allow_randomization=False), - # policy observations --> these will be noisy - ObservationType.ProjectedGravityVector(obs_name="proj_grav", xml_name="root", group="policy", allow_randomization=True), - ObservationType.FreeJointVel(obs_name="free_joint_vel_pi", xml_name="root", group="policy", allow_randomization=True), - # 修正左髋关节名称 - ObservationType.JointPos(obs_name="joint_pos_pi", xml_name="hip_flexion_l", group="policy", allow_randomization=True), - # 修正右髋关节名称 - ObservationType.JointVel(obs_name="joint_vel1_pi", xml_name="hip_flexion_r", group="policy", allow_randomization=True), - # 修正左膝关节名称 - ObservationType.JointVel(obs_name="joint_vel2_pi", xml_name="knee_angle_l", group="policy", allow_randomization=True), - # 将head替换为有效的身体名称(例如torso_link) - ObservationType.BodyPos(obs_name="head_pos_pi", xml_name="torso_link", group="policy", allow_randomization=True), - ObservationType.LastAction(obs_name="last_action_pi", group="policy", allow_randomization=True) -] + + # # example --> you can add as many datasets as you want in the lists! -env = ImitationFactory.make("UnitreeH1", +env = ImitationFactory.make("UnitreeG1", default_dataset_conf=DefaultDatasetConf(["squat", "walk"]), terrain_type="RoughTerrain", terrain_params=dict(random_min_height=-0.1,random_max_height=0.1,random_downsampled_scale=0.5), domain_randomization_type="DefaultRandomizer", domain_randomization_params=randomization_config, observation_spec=observation_spec, - control_type="PDControl", control_params=dict(p_gain=100, d_gain=1), + control_type="CustomControlFunction", + control_params={"p_gain": 100, "d_gain": 1}, n_substeps=20) -env.play_trajectory(n_episodes=3, n_steps_per_episode=500, render=True) +env.play_trajectory(n_episodes=3, n_steps_per_episode=500, render=True) \ No newline at end of file