Skip to content
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
161 changes: 131 additions & 30 deletions src/humanoid_loco_mujoco/main.py
Original file line number Diff line number Diff line change
@@ -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,
Expand Down Expand Up @@ -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)
Loading