diff --git a/isaaclab/__init__.py b/isaaclab/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/isaaclab/atlas_21jts_config.py b/isaaclab/atlas_21jts_config.py new file mode 100755 index 0000000..79a3dff --- /dev/null +++ b/isaaclab/atlas_21jts_config.py @@ -0,0 +1,100 @@ +# Copyright (c) 2022-2024, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for a simple Cartpole robot.""" + + +import omni.isaac.lab.sim as sim_utils +from omni.isaac.lab.actuators import ImplicitActuatorCfg +from omni.isaac.lab.assets import ArticulationCfg +from omni.isaac.lab.utils.assets import ISAACLAB_NUCLEUS_DIR + +## +# Configuration +## + + +Atlas_21jts_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"data/assets/usd/Atlas/atlas_21jts.usda", + + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + retain_accelerations=False, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=1.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, solver_position_iteration_count=4, solver_velocity_iteration_count=4 + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 1.05), + joint_pos={".*": 0.0}, + joint_vel={".*": 0.0}, + ), + soft_joint_pos_limit_factor=0.9, + actuators={ + "legs": ImplicitActuatorCfg( + joint_names_expr=["lleg_hpy", "rleg_hpy", "lleg_hpx", "rleg_hpx", "lleg_hpz", "rleg_hpz", "lleg_kn", "rleg_kn", "lleg_aky", "rleg_aky", "lleg_akx", "rleg_akx"], + stiffness={ + "lleg_hpy": 500, + "rleg_hpy": 500, + "lleg_hpx": 500, + "rleg_hpx": 500, + "lleg_hpz": 500, + "rleg_hpz": 500, + "lleg_kn": 500, + "rleg_kn": 500, + "lleg_aky": 500, + "rleg_aky": 500, + "lleg_akx": 500, + "rleg_akx": 500, + }, + damping={ + "lleg_hpy": 50, + "rleg_hpy": 50, + "lleg_hpx": 50, + "rleg_hpx": 50, + "lleg_hpz": 50, + "rleg_hpz": 50, + "lleg_kn": 50, + "rleg_kn": 50, + "lleg_aky": 50, + "rleg_aky": 50, + "lleg_akx": 50, + "rleg_akx": 50, + }, + ), + + "arms": ImplicitActuatorCfg( + joint_names_expr=["larm_shy", "rarm_shy", "larm_shx", "rarm_shx", "larm_shz", "rarm_shz", "larm_el", "rarm_el"], + stiffness={ + "larm_shy": 500, + "rarm_shy": 500, + "larm_shx": 500, + "rarm_shx": 500, + "larm_shz": 500, + "rarm_shz": 500, + "larm_el": 500, + "rarm_el": 500, + }, + damping={ + "larm_shy": 50, + "rarm_shy": 50, + "larm_shx": 50, + "rarm_shx": 50, + "larm_shz": 50, + "rarm_shz": 50, + "larm_el": 50, + "rarm_el": 50, + }, + ) + }, +) \ No newline at end of file diff --git a/isaaclab/joint_monkey.py b/isaaclab/joint_monkey.py new file mode 100755 index 0000000..61b95cb --- /dev/null +++ b/isaaclab/joint_monkey.py @@ -0,0 +1,192 @@ +# Copyright (c) 2022-2024, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This script demonstrates how to use the interactive scene interface to setup a scene with multiple prims. + +.. code-block:: bash + + # Usage + ./isaaclab.sh -p source/standalone/tutorials/02_scene/create_scene.py --num_envs 32 + +""" + +"""Launch Isaac Sim Simulator first.""" + + +import argparse +from omni.isaac.lab.app import AppLauncher +from phc.utils.rotation_conversions import xyzw_to_wxyz + +# add argparse arguments +parser = argparse.ArgumentParser(description="Tutorial on using the interactive scene interface.") +parser.add_argument("--num_envs", type=int, default=1, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() + +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import torch + +import omni.isaac.lab.sim as sim_utils +from omni.isaac.lab.assets import ArticulationCfg, AssetBaseCfg +from omni.isaac.lab.scene import InteractiveScene, InteractiveSceneCfg +from omni.isaac.lab.sim import SimulationContext +from omni.isaac.lab.utils import configclass + +ANIM_SEEK_LOWER = 1 +ANIM_SEEK_UPPER = 2 +ANIM_FINISHED = 3 + +## +# Pre-defined configs +## +# from omni.isaac.lab_assets import CARTPOLE_CFG # isort:skip +from isaaclab.smpl_config import SMPL_CFG, SMPLX_CFG +from isaaclab.atlas_21jts_config import Atlas_21jts_CFG + + +from phc.utils.flags import flags +from phc.utils.motion_lib_smpl import MotionLibSMPL as MotionLibSMPL +from phc.utils.motion_lib_base import FixHeightMode +from poselib.poselib.skeleton.skeleton3d import SkeletonTree, SkeletonMotion, SkeletonState +from smpl_sim.smpllib.smpl_local_robot import SMPL_Robot +from smpl_sim.smpllib.smpl_joint_names import SMPL_BONE_ORDER_NAMES, SMPLX_BONE_ORDER_NAMES, SMPLH_BONE_ORDER_NAMES, SMPL_MUJOCO_NAMES, SMPLH_MUJOCO_NAMES +from phc.learning.network_loader import load_z_encoder, load_z_decoder, load_pnn, load_mcp_mlp +from phc.env.tasks.humanoid_funcs import compute_humanoid_observations_smpl_max, compute_imitation_observations_v6 +from rl_games.algos_torch import torch_ext +from rl_games.algos_torch.players import rescale_actions +import torch +import joblib +from easydict import EasyDict +import numpy as np +import open3d as o3d +import copy +from scipy.spatial.transform import Rotation as sRot + + + +@configclass +class SMPLSceneCfg(InteractiveSceneCfg): + """Configuration for a cart-pole scene.""" + + # ground plane + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # articulation + # robot: ArticulationCfg = Atlas_21jts_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + robot: ArticulationCfg = SMPLX_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + +def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): + + """Runs the simulation loop.""" + # Extract scene entities + # note: we only do this here for readability. + robot = scene["robot"] + # Define simulation stepping + sim_dt = sim.get_physics_dt() + count = 0 + env_ids = torch.zeros(scene.num_envs).to(sim.device).long() + + current_dof = 0 + anim_state = ANIM_SEEK_LOWER + anim_timer = 0.0 + anim_duration = 4.0 # Duration to animate each joint + incremental = 0.1 + + + # Simulation loop + + sim_dof_names = robot.data.joint_names + sim_body_names = robot.data.body_names + ref_root_state = torch.zeros((scene.num_envs, 13)).to(sim.device) + joint_pos = torch.zeros((scene.num_envs, len(sim_dof_names))).to(sim.device) + joint_vel = torch.zeros((scene.num_envs, len(sim_dof_names))).to(sim.device) + ref_root_state[:, 2] = 0.9 + ref_root_state[:, 3] = 1.3 + + while simulation_app.is_running(): + # Apply random action + # -- generate random joint efforts + # efforts = torch.randn_like(robot.data.joint_pos) * 5.0 + # -- apply action to the robot + # robot.set_joint_effort_target(efforts) + + ############################## PHC ######################################## + current_dof_val = joint_pos[:, current_dof].clone() # Preserve current_dof position + joint_pos.zero_() # Efficiently set all positions to zero + joint_pos[:, current_dof] = current_dof_val # Restore current_dof position + if anim_state == ANIM_SEEK_LOWER: + joint_pos[:, current_dof] -= incremental # Adjust the increment as needed + if joint_pos[:, current_dof] <= -np.pi: + joint_pos[:, current_dof] = -np.pi + anim_state = ANIM_SEEK_UPPER + elif anim_state == ANIM_SEEK_UPPER: + joint_pos[:, current_dof] += incremental + if joint_pos[:, current_dof] >= np.pi: + joint_pos[:, current_dof] = np.pi + anim_state = ANIM_FINISHED + elif anim_state == ANIM_FINISHED: + current_dof = (current_dof + 1) % len(sim_dof_names) + anim_state = ANIM_SEEK_LOWER + print("Animating DOF %d ('%s')" % (current_dof, sim_dof_names[current_dof]), anim_timer) + # Update the timer + anim_timer += sim_dt + if anim_timer >= anim_duration: + anim_timer = 0.0 + current_dof = (current_dof + 1) % len(sim_dof_names) + anim_state = ANIM_SEEK_LOWER + + robot.write_root_state_to_sim(ref_root_state, env_ids) + robot.write_joint_state_to_sim(joint_pos, joint_vel, None, env_ids) + + + ############################## PHC ######################################## + + # -- write data to sim + scene.write_data_to_sim() + # Perform step + # sim.step() + sim.render() + # Increment counter + count += 1 + # Update buffers + scene.update(sim_dt) + + +def main(): + """Main function.""" + # Load kit helper + sim_cfg = sim_utils.SimulationCfg(device=args_cli.device) + sim = SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([2.5, 0.0, 4.0], [0.0, 0.0, 2.0]) + # Design scene + scene_cfg = SMPLSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0) + scene = InteractiveScene(scene_cfg) + # Play the simulator + sim.reset() + # Now we are ready! + print("[INFO]: Setup complete...") + # Run the simulator + run_simulator(sim, scene) + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() \ No newline at end of file diff --git a/isaaclab/launch_app.py b/isaaclab/launch_app.py new file mode 100755 index 0000000..b6f505d --- /dev/null +++ b/isaaclab/launch_app.py @@ -0,0 +1,337 @@ +# Copyright (c) 2022-2024, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This script demonstrates how to use the interactive scene interface to setup a scene with multiple prims. + +.. code-block:: bash + + # Usage + ./isaaclab.sh -p source/standalone/tutorials/02_scene/create_scene.py --num_envs 32 + +""" + +"""Launch Isaac Sim Simulator first.""" + + +import argparse +from omni.isaac.lab.app import AppLauncher +from phc.utils.rotation_conversions import xyzw_to_wxyz, wxyz_to_xyzw +import sys + +# add argparse arguments +parser = argparse.ArgumentParser(description="Tutorial on using the interactive scene interface.") +parser.add_argument("--num_envs", type=int, default=2, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli, hydra_args = parser.parse_known_args() + +sys.argv = [sys.argv[0]] + hydra_args + + +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import torch + +import omni.isaac.lab.sim as sim_utils +from omni.isaac.lab.assets import ArticulationCfg, AssetBaseCfg +from omni.isaac.lab.envs import DirectRLEnvCfg +from omni.isaac.lab.sim import SimulationCfg +from omni.isaac.lab.scene import InteractiveScene, InteractiveSceneCfg +from omni.isaac.lab.sim import SimulationContext, PhysxCfg +from omni.isaac.lab.utils import configclass +from omni.isaac.lab.devices.keyboard.se2_keyboard import Se2Keyboard +import carb +from carb.input import KeyboardEventType + + + +## +# Pre-defined configs +## +# from omni.isaac.lab_assets import CARTPOLE_CFG # isort:skip +from isaaclab.smpl_config import SMPL_CFG + + +from phc.utils.flags import flags +from phc.utils.motion_lib_smpl import MotionLibSMPL as MotionLibSMPL +from phc.utils.motion_lib_base import FixHeightMode +from poselib.poselib.skeleton.skeleton3d import SkeletonTree, SkeletonMotion, SkeletonState +from smpl_sim.smpllib.smpl_local_robot import SMPL_Robot +from smpl_sim.smpllib.smpl_joint_names import SMPL_BONE_ORDER_NAMES, SMPLX_BONE_ORDER_NAMES, SMPLH_BONE_ORDER_NAMES, SMPL_MUJOCO_NAMES, SMPLH_MUJOCO_NAMES +from phc.learning.network_loader import load_z_encoder, load_z_decoder, load_pnn, load_mcp_mlp +from phc.env.tasks.humanoid_funcs import compute_humanoid_observations_smpl_max, compute_imitation_observations_v6 +from rl_games.algos_torch import torch_ext +from rl_games.algos_torch.players import rescale_actions +import torch +import joblib +from easydict import EasyDict +import numpy as np +import open3d as o3d +import copy +from scipy.spatial.transform import Rotation as sRot +import time + + + +@configclass +class SMPLSceneCfg(InteractiveSceneCfg): + """Configuration for a cart-pole scene.""" + + # ground plane + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + # articulation + smpl_robot: ArticulationCfg = SMPL_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + +def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): + def reset_robot(robot): + motion_res = motion_lib.get_motion_state(motion_id, motion_time) + ref_root_pos, ref_root_rot, ref_dof_pos, ref_root_vel, ref_root_ang_vel, ref_dof_vel, ref_smpl_params, ref_limb_weights, ref_pose_aa, ref_rb_pos, ref_rb_rot, ref_body_vel, ref_body_ang_vel = \ + motion_res["root_pos"], motion_res["root_rot"], motion_res["dof_pos"], motion_res["root_vel"], motion_res["root_ang_vel"], motion_res["dof_vel"], \ + motion_res["motion_bodies"], motion_res["motion_limb_weights"], motion_res["motion_aa"], motion_res["rg_pos"], motion_res["rb_rot"], motion_res["body_vel"], motion_res["body_ang_vel"] + init_joint_pos = ref_dof_pos[:, gym_to_sim_dof] + init_joint_vel = ref_dof_vel[:, gym_to_sim_dof] + + # ref_joint_pos = torch.from_numpy(sRot.from_rotvec(ref_joint_pos.reshape(-1, 3)).as_euler("xyz")).to(ref_joint_pos).reshape(scene.num_envs, -1) + + init_root_state = torch.cat([ref_root_pos, xyzw_to_wxyz(ref_root_rot), ref_root_vel, ref_root_ang_vel], dim = -1) + robot.write_root_state_to_sim(init_root_state, env_ids) + robot.write_joint_state_to_sim(init_joint_pos, init_joint_vel, None, env_ids) + + def test_keyboard(): + import ipdb; ipdb.set_trace() + print('....') + + keyboard = Se2Keyboard() + keyboard.add_callback(carb.input.KeyboardInput.W, test_keyboard) + + + + device = ( + torch.device("cuda", index=0) + if torch.cuda.is_available() + else torch.device("cpu") + ) + # motion_file = "sample_data/amass_isaac_simple_run_upright_slim.pkl" + motion_file = "data/amass/pkls/singles/hard_z_fut_1_1_2_upright_slim.pkl" + dt = 1/30 + time_steps = 1 + has_upright_start = True + motion_lib_cfg = EasyDict({ + "has_upright_start": has_upright_start, + "motion_file": motion_file, + "fix_height": FixHeightMode.full_fix, + "min_length": -1, + "max_length": 3000, + "im_eval": flags.im_eval, + "multi_thread": False , + "smpl_type": 'smpl', + "randomrize_heading": True, + "device": device, + }) + robot_cfg = { + "mesh": False, + "rel_joint_lm": False, + "upright_start": has_upright_start, + "remove_toe": False, + "real_weight_porpotion_capsules": True, + "real_weight_porpotion_boxes": True, + "model": "smpl", + "big_ankle": True, + "box_body": True, + "body_params": {}, + "joint_params": {}, + "geom_params": {}, + "actuator_params": {}, + } + smpl_robot = SMPL_Robot( + robot_cfg, + data_dir="data/smpl", + ) + + gender_beta = np.zeros((17)) + smpl_robot.load_from_skeleton(betas=torch.from_numpy(gender_beta[None, 1:]), gender=gender_beta[0:1], objs_info=None) + test_good = f"/tmp/smpl/test_good.xml" + smpl_robot.write_xml(test_good) + sk_tree = SkeletonTree.from_mjcf(test_good) + num_motions = 10 + skeleton_trees = [sk_tree] * num_motions + start_idx = 0 + env_ids = torch.zeros(scene.num_envs).to(device).long() + motion_lib = MotionLibSMPL(motion_lib_cfg) + motion_lib.load_motions(skeleton_trees=skeleton_trees, + gender_betas=[torch.from_numpy(gender_beta)] * num_motions, + limb_weights=[np.zeros(10)] * num_motions, + random_sample=False, + start_idx = start_idx, + max_len=-1) + motion_id, time_steps = torch.zeros(scene.num_envs).to(device).long(), torch.zeros(scene.num_envs).to(device).float() + motion_id[:] = 0 + motion_len = motion_lib.get_motion_length(motion_id).item() + motion_time = time_steps % motion_len + + policy_path = "output/HumanoidIm/phc_3/Humanoid.pth" + check_points = [torch_ext.load_checkpoint(policy_path)] + pnn = load_pnn(check_points[0], num_prim = 3, has_lateral = False, activation = "silu", device = device) + running_mean, running_var = check_points[-1]['running_mean_std']['running_mean'], check_points[-1]['running_mean_std']['running_var'] + action_offset = joblib.load("action_offset.pkl") + pd_action_offset = action_offset[0] + pd_action_scale = action_offset[1] + ####### + + + """Runs the simulation loop.""" + # Extract scene entities + # note: we only do this here for readability. + robot = scene["smpl_robot"] + # Define simulation stepping + sim_dt = sim.get_physics_dt() + + ########################################################### + sim_joint_names = robot.data.joint_names + sim_body_names = robot.data.body_names + gym_joint_names = [f"{j}_{axis}" for j in SMPL_MUJOCO_NAMES[1:] for axis in ["x", "y", "z"]] + sim_to_gym_body = [sim_body_names.index(n) for n in SMPL_MUJOCO_NAMES] + sim_to_gym_dof = [sim_joint_names.index(n) for n in gym_joint_names] + + gym_to_sim_dof = [gym_joint_names.index(n) for n in sim_joint_names] + gym_to_sim_body = [SMPL_MUJOCO_NAMES.index(n) for n in sim_body_names] + + + reset_robot(robot) + + # Simulation loop + while simulation_app.is_running(): + start_time = time.time() + # Apply random action + # -- generate random joint efforts + # efforts = torch.randn_like(robot.data.joint_pos) * 5.0 + # -- apply action to the robot + # robot.set_joint_effort_target(efforts) + + ############################## PHC ######################################## + + motion_time = time_steps % motion_len + motion_res = motion_lib.get_motion_state(motion_id, motion_time) + ref_root_pos, ref_root_rot, ref_dof_pos, ref_root_vel, ref_root_ang_vel, ref_dof_vel, ref_smpl_params, ref_limb_weights, ref_pose_aa, ref_rb_pos, ref_rb_rot, ref_body_vel, ref_body_ang_vel = \ + motion_res["root_pos"], motion_res["root_rot"], motion_res["dof_pos"], motion_res["root_vel"], motion_res["root_ang_vel"], motion_res["dof_vel"], \ + motion_res["motion_bodies"], motion_res["motion_limb_weights"], motion_res["motion_aa"], motion_res["rg_pos"], motion_res["rb_rot"], motion_res["body_vel"], motion_res["body_ang_vel"] + + body_pos = robot.data.body_pos_w[:, sim_to_gym_body ] + body_rot = wxyz_to_xyzw(robot.data.body_quat_w[:, sim_to_gym_body]) + body_vel = robot.data.body_lin_vel_w[:, sim_to_gym_body] + body_ang_vel = robot.data.body_ang_vel_w[:, sim_to_gym_body] + + root_pos = body_pos[:, 0, :] + root_rot = body_rot[:, 0, :] + body_pos_subset = body_pos + body_rot_subset = body_rot + body_vel_subset = body_vel + body_ang_vel_subset = body_ang_vel + ref_rb_pos_subset = ref_rb_pos + ref_rb_rot_subset = ref_rb_rot + ref_body_vel_subset = ref_body_vel + ref_body_ang_vel_subset = ref_body_ang_vel + ref_joint_pos = ref_dof_pos[:, gym_to_sim_dof] + ref_joint_vel = ref_dof_vel[:, gym_to_sim_dof] + + # Data replay + + # ref_root_state = torch.cat([ref_root_pos, xyzw_to_wxyz(ref_root_rot), ref_root_vel, ref_root_ang_vel], dim = -1) + # robot.write_root_state_to_sim(ref_root_state, env_ids) + # robot.write_joint_state_to_sim(ref_joint_pos, ref_joint_vel, None, env_ids) + + self_obs = compute_humanoid_observations_smpl_max(body_pos, body_rot, body_vel, body_ang_vel, ref_smpl_params, ref_limb_weights, True, True, has_upright_start, False, False) + task_obs = compute_imitation_observations_v6(root_pos, root_rot, body_pos_subset, body_rot_subset, body_vel_subset, body_ang_vel_subset, ref_rb_pos_subset, ref_rb_rot_subset, ref_body_vel_subset, ref_body_ang_vel_subset, 1, has_upright_start) + + full_obs = torch.cat([self_obs, task_obs], dim = -1) + full_obs = ((full_obs - running_mean.float()) / torch.sqrt(running_var.float() + 1e-05)) + full_obs = torch.clamp(full_obs, min=-5.0, max=5.0) + + with torch.no_grad(): + actions, _ = pnn(full_obs, idx=0) + actions = rescale_actions(-1, 1, torch.clamp(actions, -1, 1)) + actions = actions * pd_action_scale + pd_action_offset + actions = actions[:, gym_to_sim_dof] + + robot.set_joint_position_target(actions, None, env_ids) + + ############################## PHC ######################################## + + if time_steps > motion_len: + print("resetting", time_steps.item()) + reset_robot(robot) + time_steps[:] = 0 + + # -- write data to sim + scene.write_data_to_sim() + # Perform step + sim.step() + # sim.render() + # Increment counter + time_steps += sim_dt + # Update buffers + scene.update(sim_dt) + + # Measure wall time and ensure 30 fps + elapsed_time = time.time() - start_time + sleep_time = max(0, (1.0 / 30.0) - elapsed_time) + # time.sleep(sleep_time) + + +def main(): + """Main function.""" + # Load kit helper + sim_cfg = sim_utils.SimulationCfg( + device=args_cli.device, + dt=1 / 60, + # decimation will be set in the task config + # up axis will always be Z in isaac sim + # use_gpu_pipeline is deduced from the device + gravity=(0.0, 0.0, -9.81), + physx = PhysxCfg( + # num_threads is no longer needed + solver_type=1, + # use_gpu is deduced from the device + max_position_iteration_count=4, + max_velocity_iteration_count=0, + # moved to actor config + # moved to actor config + bounce_threshold_velocity=0.2, + # moved to actor config + # default_buffer_size_multiplier is no longer needed + gpu_max_rigid_contact_count=2**23, + # num_subscenes is no longer needed + # contact_collection is no longer needed + ) + ) + sim = SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([2.5, 0.0, 4.0], [0.0, 0.0, 2.0]) + # Design scene + scene_cfg = SMPLSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0) + scene = InteractiveScene(scene_cfg) + # Play the simulator + sim.reset() + # Now we are ready! + print("[INFO]: Setup complete...") + # Run the simulator + run_simulator(sim, scene) + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() \ No newline at end of file diff --git a/isaaclab/launch_smpl_env.py b/isaaclab/launch_smpl_env.py new file mode 100644 index 0000000..ca1b4b0 --- /dev/null +++ b/isaaclab/launch_smpl_env.py @@ -0,0 +1,421 @@ +# Copyright (c) 2022-2024, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This script demonstrates how to use the interactive scene interface to setup a scene with multiple prims. + +.. code-block:: bash + + # Usage + ./isaaclab.sh -p source/standalone/tutorials/02_scene/create_scene.py --num_envs 32 + +""" + +"""Launch Isaac Sim Simulator first.""" + + +import argparse +from omni.isaac.lab.app import AppLauncher +from smpl_sim.utils.rotation_conversions import xyzw_to_wxyz, wxyz_to_xyzw +import sys + +# add argparse arguments +parser = argparse.ArgumentParser(description="Tutorial on using the interactive scene interface.") +parser.add_argument("--num_envs", type=int, default=2, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli, hydra_args = parser.parse_known_args() + +sys.argv = [sys.argv[0]] + hydra_args + + +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import torch + +import omni.isaac.lab.sim as sim_utils +from omni.isaac.lab.assets import Articulation, ArticulationCfg, AssetBaseCfg +from omni.isaac.lab.envs import DirectRLEnvCfg +from omni.isaac.lab.sim import SimulationCfg +from omni.isaac.lab.scene import InteractiveScene, InteractiveSceneCfg +from omni.isaac.lab.sim import SimulationContext, PhysxCfg +from omni.isaac.lab.utils import configclass +from omni.isaac.lab.envs import DirectRLEnv, DirectRLEnvCfg, ViewerCfg +from omni.isaac.lab.devices.keyboard.se2_keyboard import Se2Keyboard +import omni.isaac.lab.utils.math as lab_math_utils +import carb +import imageio +from carb.input import KeyboardEventType + + + +## +# Pre-defined configs +## +# from omni.isaac.lab_assets import CARTPOLE_CFG # isort:skip +from isaaclab.smpl_config import SMPL_CFG, SMPLX_CFG + +from collections.abc import Sequence +from phc.utils.flags import flags +from phc.utils.motion_lib_smpl import MotionLibSMPL as MotionLibSMPL +from phc.utils.motion_lib_base import FixHeightMode +from poselib.poselib.skeleton.skeleton3d import SkeletonTree, SkeletonMotion, SkeletonState +from smpl_sim.smpllib.smpl_local_robot import SMPL_Robot +from smpl_sim.smpllib.smpl_joint_names import SMPL_BONE_ORDER_NAMES, SMPLX_BONE_ORDER_NAMES, SMPLH_BONE_ORDER_NAMES, SMPL_MUJOCO_NAMES, SMPLH_MUJOCO_NAMES +from phc.learning.network_loader import load_z_encoder, load_z_decoder, load_pnn, load_mcp_mlp +from phc.env.tasks.humanoid_funcs import compute_humanoid_observations_smpl_max, compute_imitation_observations_v6 +from rl_games.algos_torch import torch_ext +from omni.isaac.lab.sensors import TiledCamera, TiledCameraCfg, save_images_to_file +from rl_games.algos_torch.players import rescale_actions +import torch +import joblib +from easydict import EasyDict +import numpy as np +import copy +from scipy.spatial.transform import Rotation as sRot +import time + + + +flags.test=False + +@configclass +class SMPLSceneCfg(InteractiveSceneCfg): + """Configuration for a cart-pole scene.""" + + # ground plane + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + + + +@configclass +class SMPLEnvCfg(DirectRLEnvCfg): + num_actions = 69 + num_observations = 1 + num_states = 1 + + decimation = 2 + + sim = sim_utils.SimulationCfg( + device=args_cli.device, + dt=1 / 60, + # decimation will be set in the task config + # up axis will always be Z in isaac sim + # use_gpu_pipeline is deduced from the device + gravity=(0.0, 0.0, -9.81), + physx = PhysxCfg( + # num_threads is no longer needed + solver_type=1, + # use_gpu is deduced from the device + max_position_iteration_count=4, + max_velocity_iteration_count=0, + # moved to actor config + # moved to actor config + bounce_threshold_velocity=0.2, + # moved to actor config + # default_buffer_size_multiplier is no longer needed + gpu_max_rigid_contact_count=2**23, + # num_subscenes is no longer needed + # contact_collection is no longer needed + ) + ) + + + viewer = ViewerCfg(eye=(20.0, 20.0, 20.0)) + + # smpl_robot: ArticulationCfg = SMPL_CFG.replace(prim_path="/World/envs/env_.*/Robot") + smpl_robot: ArticulationCfg = SMPLX_CFG.replace(prim_path="/World/envs/env_.*/Robot") + + tiled_camera: TiledCameraCfg = TiledCameraCfg( + prim_path="/World/envs/env_.*/Camera", + offset=TiledCameraCfg.OffsetCfg(pos=(-4.0, 0.0, 2.0), rot=(0.9961947, 0, 0.0871557, 0), convention="world"), + data_types=["rgb"], + spawn=sim_utils.PinholeCameraCfg( + focal_length=10.0, focus_distance=50.0, horizontal_aperture=5, clipping_range=(0.1, 20.0) + ), + width=512, + height=512, + ) + + # scene + scene: InteractiveSceneCfg = SMPLSceneCfg(num_envs=args_cli.num_envs, env_spacing=20.0, replicate_physics=True) + + +class SMPLEnv(DirectRLEnv): + cfg: SMPLEnvCfg + + def __init__(self, cfg: SMPLEnvCfg, render_mode: str | None = None, **kwargs): + self.cfg = cfg + self.humanoid_type = "smplx" + super().__init__(cfg, render_mode, **kwargs) + + SMPL_NAMES = SMPLH_MUJOCO_NAMES if self.humanoid_type == "smplx" else SMPL_MUJOCO_NAMES + sim_joint_names = self._robot.data.joint_names + sim_body_names = self._robot.data.body_names + gym_joint_names = [f"{j}_{axis}" for j in SMPL_NAMES[1:] for axis in ["x", "y", "z"]] + + self.sim_to_gym_body = [sim_body_names.index(n) for n in SMPL_NAMES] + self.sim_to_gym_dof = [sim_joint_names.index(n) for n in gym_joint_names] + + self.gym_to_sim_dof = [gym_joint_names.index(n) for n in sim_joint_names] + self.gym_to_sim_body = [SMPL_NAMES.index(n) for n in sim_body_names] + + keyboard_interface = Se2Keyboard() + keyboard_interface.add_callback("R", self.reset) + + + def close(self): + super().close() + + def _configure_gym_env_spaces(self): + self.num_actions = self.cfg.num_actions + self.num_observations = self.cfg.num_observations + self.num_states = self.cfg.num_states + + + def _setup_scene(self): + self._robot = robot = Articulation(self.cfg.smpl_robot) + self._tiled_camera = TiledCamera(self.cfg.tiled_camera) + + # clone, filter, and replicate + self.scene.clone_environments(copy_from_source=False) + # self.scene.filter_collisions(global_prim_paths=[]) + + # add articultion and sensors to scene + self.scene.articulations["robot"] = self._robot + self.scene.sensors["tiled_camera"] = self._tiled_camera + # add lights + light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) + light_cfg.func("/World/Light", light_cfg) + + + # motion_file = "data/amass/pkls/singles/hard_z_fut_1_1_2_upright_slim.pkl" + # motion_file = "data/amass/pkls/singles/0-CMU_88_88_05_poses_upright_slim.pkl" + motion_file = "data/amass_x/mouse.pkl" + self._load_motion(motion_file) + + + def _load_motion(self, motion_file): + + time_steps = 1 + self._has_upright_start = False + motion_lib_cfg = EasyDict({ + "has_upright_start": self._has_upright_start, + "motion_file": motion_file, + "fix_height": FixHeightMode.full_fix, + "min_length": -1, + "max_length": 3000, + "im_eval": flags.im_eval, + "multi_thread": False , + "smpl_type": self.humanoid_type, + "randomrize_heading": True, + "device": self.device, + }) + robot_cfg = { + "mesh": False, + "rel_joint_lm": False, + "upright_start": self._has_upright_start, + "remove_toe": False, + "real_weight_porpotion_capsules": True, + "real_weight_porpotion_boxes": True, + "model": self.humanoid_type, + "big_ankle": True, + "box_body": True, + "body_params": {}, + "joint_params": {}, + "geom_params": {}, + "actuator_params": {}, + } + smpl_robot = SMPL_Robot( + robot_cfg, + data_dir="data/smpl", + ) + + # gender_beta = np.zeros((17)) + gender_beta = np.zeros((16)) + smpl_robot.load_from_skeleton(betas=torch.from_numpy(gender_beta[None, 1:]), gender=gender_beta[0:1], objs_info=None) + test_good = f"/tmp/smpl/test_good.xml" + smpl_robot.write_xml(test_good) + sk_tree = SkeletonTree.from_mjcf(test_good) + num_motions = self.num_envs + skeleton_trees = [sk_tree] * num_motions + start_idx = 0 + + motion_lib = MotionLibSMPL(motion_lib_cfg) + motion_lib.load_motions(skeleton_trees=skeleton_trees, + gender_betas=[torch.from_numpy(gender_beta)] * num_motions, + limb_weights=[np.zeros(10)] * num_motions, + random_sample=False, + start_idx = start_idx, + max_len=-1) + self._motion_lib = motion_lib + self._motion_id, self._motion_time = torch.arange(self.num_envs).to(self.device).long(), torch.zeros(self.num_envs).to(self.device).float() + + + def _pre_physics_step(self, actions: torch.Tensor) -> None: + self.actions = actions + + # robot_root = self._robot.data.root_pos_w + # eyes = robot_root + torch.tensor([0, -4, 0]).to(robot_root) + # targets = robot_root + # self._tiled_camera.set_world_poses_from_view(eyes, targets) + + # head_pos = self._robot.data.body_pos_w[:, self._robot.data.body_names.index("Head")] + # head_rot = self._robot.data.body_quat_w[:, self._robot.data.body_names.index("Head")] + # eye_offset = torch.tensor([[0.0, 0.075, 0.1]] * self.num_envs, device=head_rot.device) + + # eye_pos = head_pos + lab_math_utils.quat_rotate(head_rot, eye_offset) + # self._tiled_camera.set_world_poses(eye_pos, head_rot) + pass + + def _post_physics_step(self) -> None: + + + pass + + def _apply_action(self) -> None: + self._robot.set_joint_position_target(self.actions, joint_ids=None) + + def _get_observations(self) -> dict: + + self._motion_time = (self.episode_length_buf + 1) * self.step_dt + + motion_res = self._motion_lib.get_motion_state(self._motion_id, self._motion_time) + ref_root_pos, ref_root_rot, ref_dof_pos, ref_root_vel, ref_root_ang_vel, ref_dof_vel, ref_smpl_params, ref_limb_weights, ref_pose_aa, ref_rb_pos, ref_rb_rot, ref_body_vel, ref_body_ang_vel = \ + motion_res["root_pos"], motion_res["root_rot"], motion_res["dof_pos"], motion_res["root_vel"], motion_res["root_ang_vel"], motion_res["dof_vel"], \ + motion_res["motion_bodies"], motion_res["motion_limb_weights"], motion_res["motion_aa"], motion_res["rg_pos"], motion_res["rb_rot"], motion_res["body_vel"], motion_res["body_ang_vel"] + + body_pos = self._robot.data.body_pos_w[:, self.sim_to_gym_body ] + body_rot = wxyz_to_xyzw(self._robot.data.body_quat_w[:, self.sim_to_gym_body]) + body_vel = self._robot.data.body_lin_vel_w[:, self.sim_to_gym_body] + body_ang_vel = self._robot.data.body_ang_vel_w[:, self.sim_to_gym_body] + + root_pos = body_pos[:, 0, :] + root_rot = body_rot[:, 0, :] + body_pos_subset = body_pos + body_rot_subset = body_rot + body_vel_subset = body_vel + body_ang_vel_subset = body_ang_vel + ref_rb_pos_subset = ref_rb_pos + ref_rb_rot_subset = ref_rb_rot + ref_body_vel_subset = ref_body_vel + ref_body_ang_vel_subset = ref_body_ang_vel + ref_joint_pos = ref_dof_pos[:, self.gym_to_sim_dof] + ref_joint_vel = ref_dof_vel[:, self.gym_to_sim_dof] + + # Data replay + + # ref_root_state = torch.cat([ref_root_pos, xyzw_to_wxyz(ref_root_rot), ref_root_vel, ref_root_ang_vel], dim = -1) + # robot.write_root_state_to_sim(ref_root_state, env_ids) + # robot.write_joint_state_to_sim(ref_joint_pos, ref_joint_vel, None, env_ids) + + self_obs = compute_humanoid_observations_smpl_max(body_pos, body_rot, body_vel, body_ang_vel, ref_smpl_params, ref_limb_weights, True, True, self._has_upright_start, False, False) + task_obs = compute_imitation_observations_v6(root_pos, root_rot, body_pos_subset, body_rot_subset, body_vel_subset, body_ang_vel_subset, ref_rb_pos_subset, ref_rb_rot_subset, ref_body_vel_subset, ref_body_ang_vel_subset, 1, self._has_upright_start) + + return { + "self_obs": self_obs, + "task_obs": task_obs, + } + + def _get_rewards(self) -> torch.Tensor: + return torch.zeros(self.num_envs) + + def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: + return torch.zeros(self.num_envs), torch.zeros(self.num_envs) + + def _reset_idx(self, env_ids: Sequence[int]): + super()._reset_idx(env_ids) + self._motion_time[env_ids] = 0 + + motion_res = self._motion_lib.get_motion_state(self._motion_id, self._motion_time) + ref_root_pos, ref_root_rot, ref_dof_pos, ref_root_vel, ref_root_ang_vel, ref_dof_vel, ref_smpl_params, ref_limb_weights, ref_pose_aa, ref_rb_pos, ref_rb_rot, ref_body_vel, ref_body_ang_vel = \ + motion_res["root_pos"], motion_res["root_rot"], motion_res["dof_pos"], motion_res["root_vel"], motion_res["root_ang_vel"], motion_res["dof_vel"], \ + motion_res["motion_bodies"], motion_res["motion_limb_weights"], motion_res["motion_aa"], motion_res["rg_pos"], motion_res["rb_rot"], motion_res["body_vel"], motion_res["body_ang_vel"] + init_joint_pos = ref_dof_pos[:, self.gym_to_sim_dof] + init_joint_vel = ref_dof_vel[:, self.gym_to_sim_dof] + + # ref_joint_pos = torch.from_numpy(sRot.from_rotvec(ref_joint_pos.reshape(-1, 3)).as_euler("xyz")).to(ref_joint_pos).reshape(scene.num_envs, -1) + + init_root_state = torch.cat([ref_root_pos, xyzw_to_wxyz(ref_root_rot), ref_root_vel, ref_root_ang_vel], dim = -1) + self._robot.write_root_state_to_sim(init_root_state, env_ids) + self._robot.write_joint_state_to_sim(init_joint_pos, init_joint_vel, None, env_ids) + + +def main(): + """Main function.""" + env_cfg = SMPLEnvCfg() + env = SMPLEnv(env_cfg) + + + device = env.device + if env.humanoid_type == "smplx": + policy_path = "output/HumanoidIm/phc_x_pnn/Humanoid.pth" + else: + policy_path = "output/HumanoidIm/phc_3/Humanoid.pth" + + policy_path = "output/HumanoidIm/phc_x_pnn/Humanoid.pth" + check_points = [torch_ext.load_checkpoint(policy_path)] + pnn = load_pnn(check_points[0], num_prim = 3, has_lateral = False, activation = "silu", device = device) + running_mean, running_var = check_points[-1]['running_mean_std']['running_mean'], check_points[-1]['running_mean_std']['running_var'] + if env.humanoid_type == "smplx": + action_offset = joblib.load("data/action_offset_smplx.pkl") + else: + action_offset = joblib.load("data/action_offset_smpl.pkl") + + pd_action_offset = action_offset[0] + pd_action_scale = action_offset[1] + + writer = imageio.get_writer('tiled_camera_output.mp4', fps=30) + time = 0 + obs_dict, extras = env.reset() + while True: + + #### Test Rendering ##### + # camera_data = env.scene['tiled_camera'].data.output['rgb'] + # camera_data = (camera_data).byte().cpu().numpy() + # batch_size = camera_data.shape[0] + # grid_size = int(np.ceil(np.sqrt(batch_size))) + # frame_height, frame_width = camera_data.shape[1], camera_data.shape[2] + # collage = np.zeros((grid_size * frame_height, grid_size * frame_width, 3), dtype=np.uint8) + # for i in range(batch_size): + # row = i // grid_size + # col = i % grid_size + # collage[row * frame_height:(row + 1) * frame_height, col * frame_width:(col + 1) * frame_width, :] = camera_data[i] + # frame = collage + + # writer.append_data(frame) + + # time += 1 + # if time > 200: + # import ipdb; ipdb.set_trace() + # writer.close() + # break + #### Test Rendering ##### + + self_obs, task_obs = obs_dict["self_obs"], obs_dict["task_obs"] + full_obs = torch.cat([self_obs, task_obs], dim = -1) + full_obs = ((full_obs - running_mean.float()) / torch.sqrt(running_var.float() + 1e-05)) + full_obs = torch.clamp(full_obs, min=-5.0, max=5.0) + + with torch.no_grad(): + actions, _ = pnn(full_obs, idx=0) + actions = rescale_actions(-1, 1, torch.clamp(actions, -1, 1)) + actions = actions * pd_action_scale + pd_action_offset + actions = actions[:, env.gym_to_sim_dof] + + obs_dict, _, _, _, _ = env.step(actions) + + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() \ No newline at end of file diff --git a/isaaclab/run_atlas.py b/isaaclab/run_atlas.py new file mode 100755 index 0000000..0529417 --- /dev/null +++ b/isaaclab/run_atlas.py @@ -0,0 +1,324 @@ +# Copyright (c) 2022-2024, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This script demonstrates how to use the interactive scene interface to setup a scene with multiple prims. + +.. code-block:: bash + + # Usage + ./isaaclab.sh -p source/standalone/tutorials/02_scene/create_scene.py --num_envs 32 + +""" + +"""Launch Isaac Sim Simulator first.""" + + +import argparse +from omni.isaac.lab.app import AppLauncher +from phc.utils.rotation_conversions import xyzw_to_wxyz, wxyz_to_xyzw +import sys + +# add argparse arguments +parser = argparse.ArgumentParser(description="Tutorial on using the interactive scene interface.") +parser.add_argument("--num_envs", type=int, default=None, help="Number of environments to simulate.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +# args_cli = parser.parse_args() +args_cli, hydra_args = parser.parse_known_args() + +sys.argv = [sys.argv[0]] + hydra_args + +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import torch + +import omni.isaac.lab.sim as sim_utils +from omni.isaac.lab.assets import ArticulationCfg, AssetBaseCfg +from omni.isaac.lab.scene import InteractiveScene, InteractiveSceneCfg +from omni.isaac.lab.sim import SimulationContext, PhysxCfg +from omni.isaac.lab.utils import configclass +from omni.isaac.lab.devices.keyboard.se2_keyboard import Se2Keyboard +import carb +## +# Pre-defined configs +## +# from omni.isaac.lab_assets import CARTPOLE_CFG # isort:skip +from isaaclab.smpl_config import SMPL_CFG +from isaaclab.atlas_21jts_config import Atlas_21jts_CFG + + +from phc.utils.flags import flags +from phc.utils.motion_lib_real import MotionLibReal +from phc.utils.motion_lib_base import FixHeightMode +from poselib.poselib.skeleton.skeleton3d import SkeletonTree, SkeletonMotion, SkeletonState +from phc.learning.network_loader import load_z_encoder, load_z_decoder, load_pnn, load_mcp_mlp +from phc.env.tasks.humanoid_funcs import compute_humanoid_observations_smpl_max, compute_imitation_observations_v6 +from rl_games.algos_torch import torch_ext +from rl_games.algos_torch.players import rescale_actions +import torch +import joblib +from easydict import EasyDict +import numpy as np +import open3d as o3d +import copy +from scipy.spatial.transform import Rotation as sRot +import time +from omegaconf import DictConfig, OmegaConf +from multiprocessing import Pool +import hydra +from phc.utils.torch_humanoid_batch import Humanoid_Batch + +@configclass +class SMPLSceneCfg(InteractiveSceneCfg): + """Configuration for a cart-pole scene.""" + + # ground plane + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # articulation + robot: ArticulationCfg = Atlas_21jts_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + +def run_simulator(cfg, sim: sim_utils.SimulationContext, scene: InteractiveScene): + + def test_keyboard(): + import ipdb; ipdb.set_trace() + print('....') + + keyboard = Se2Keyboard() + keyboard.add_callback(carb.input.KeyboardInput.W, test_keyboard) + + def reset_robot(robot): + robot.write_root_state_to_sim(init_root_state, env_ids) + robot.write_joint_state_to_sim(init_joint_pos, init_joint_vel, None, env_ids) + + humanoid_fk = Humanoid_Batch(cfg.robot) + + device = ( + torch.device("cuda", index=0) + if torch.cuda.is_available() + else torch.device("cpu") + ) + motion_file = "data/e_atlas_nohand/v1/amass_phc.pkl" + dt = 1/30 + time_steps = 1 + has_upright_start = True + motion_lib_cfg = EasyDict({ + "has_upright_start": has_upright_start, + "motion_file": motion_file, + "fix_height": FixHeightMode.full_fix, + "min_length": -1, + "max_length": 3000, + "im_eval": flags.im_eval, + "multi_thread": False , + "randomrize_heading": True, + "device": device, + "robot": cfg.robot, + }) + + + gender_beta = np.zeros((17)) + sk_tree = SkeletonTree.from_mjcf(cfg.robot.asset.assetFileName) + num_motions = 10 + skeleton_trees = [sk_tree] * num_motions + start_idx = 0 + motion_lib = MotionLibReal(motion_lib_cfg) + motion_lib.load_motions(skeleton_trees=skeleton_trees, + gender_betas=[torch.from_numpy(gender_beta)] * num_motions, + limb_weights=[np.zeros(10)] * num_motions, + random_sample=False, + start_idx = start_idx, + max_len=-1) + motion_id, time_steps = torch.zeros(1).to(device).long(), torch.zeros(1).to(device).float() + motion_id[:] = 2 + motion_len = motion_lib.get_motion_length(motion_id).item() + motion_time = time_steps % motion_len + + policy_path = "output/HumanoidIm/eatlas_phc_1002/Humanoid.pth" + check_points = [torch_ext.load_checkpoint(policy_path)] + mlp = load_mcp_mlp(check_points[0], activation = "silu", device = device) + running_mean, running_var = check_points[-1]['running_mean_std']['running_mean'], check_points[-1]['running_mean_std']['running_var'] + action_offset = joblib.load("atlas_pd_action_offset_scale.pkl") + pd_action_offset = action_offset[0] + pd_action_scale = action_offset[1] + ####### + + """Runs the simulation loop.""" + # Extract scene entities + # note: we only do this here for readability. + robot = scene["robot"] + # Define simulation stepping + sim_dt = sim.get_physics_dt() + + ########################################################### + sim_joint_names = robot.data.joint_names + sim_body_names = robot.data.body_names + body_names_gym = cfg.robot.body_names + dof_names_gym = ['larm_shy', 'larm_shx', 'larm_shz', 'larm_el', 'lleg_hpy', + 'lleg_hpx', 'lleg_hpz', 'lleg_kn', 'lleg_aky', 'lleg_akx', + 'rarm_shy', 'rarm_shx', 'rarm_shz', 'rarm_el', 'rleg_hpy', + 'rleg_hpx', 'rleg_hpz', 'rleg_kn', 'rleg_aky', 'rleg_akx'] + + sim_to_gym_body = [sim_body_names.index(n) for n in body_names_gym] + sim_to_gym_dof = [sim_joint_names.index(n) for n in dof_names_gym] + + gym_to_sim_body = [body_names_gym.index(n) for n in sim_body_names] + gym_to_sim_dof = [dof_names_gym.index(n) for n in sim_joint_names] + + motion_res = motion_lib.get_motion_state(motion_id, motion_time) + ref_root_pos, ref_root_rot, ref_dof_pos, ref_root_vel, ref_root_ang_vel, ref_dof_vel, ref_smpl_params, ref_limb_weights, ref_pose_aa, ref_rb_pos, ref_rb_rot, ref_body_vel, ref_body_ang_vel = \ + motion_res["root_pos"], motion_res["root_rot"], motion_res["dof_pos"], motion_res["root_vel"], motion_res["root_ang_vel"], motion_res["dof_vel"], \ + motion_res["motion_bodies"], motion_res["motion_limb_weights"], motion_res["motion_aa"], motion_res["rg_pos"], motion_res["rb_rot"], motion_res["body_vel"], motion_res["body_ang_vel"] + init_joint_pos = ref_dof_pos[:, gym_to_sim_dof] + init_joint_vel = ref_dof_vel[:, gym_to_sim_dof] + + # ref_joint_pos = torch.from_numpy(sRot.from_rotvec(ref_joint_pos.reshape(-1, 3)).as_euler("xyz")).to(ref_joint_pos).reshape(scene.num_envs, -1) + env_ids = torch.zeros(scene.num_envs).to(device).long() + + init_root_state = torch.cat([ref_root_pos, xyzw_to_wxyz(ref_root_rot), ref_root_vel, ref_root_ang_vel], dim = -1) + robot.write_root_state_to_sim(init_root_state, env_ids) + robot.write_joint_state_to_sim(init_joint_pos, init_joint_vel, None, env_ids) + time_step = 3 + + # Simulation loop + while simulation_app.is_running(): + start_time = time.time() + # Apply random action + # -- generate random joint efforts + # efforts = torch.randn_like(robot.data.joint_pos) * 5.0 + # -- apply action to the robot + # robot.set_joint_effort_target(efforts) + + ############################## PHC ######################################## + + motion_time = time_steps % motion_len + time_internals = torch.arange(time_step).to(device).repeat(scene.num_envs).view(-1, time_step) * 1/30 + motion_time_steps = motion_time + time_internals + env_ids_steps = motion_id.repeat_interleave(time_step) + # motion_time_steps = motion_time + # env_ids_steps = motion_id + + motion_res = motion_lib.get_motion_state(env_ids_steps, motion_time_steps) + ref_root_pos, ref_root_rot, ref_dof_pos, ref_root_vel, ref_root_ang_vel, ref_dof_vel, ref_smpl_params, ref_limb_weights, ref_pose_aa, ref_rb_pos, ref_rb_rot, ref_body_vel, ref_body_ang_vel = \ + motion_res["root_pos"], motion_res["root_rot"], motion_res["dof_pos"], motion_res["root_vel"], motion_res["root_ang_vel"], motion_res["dof_vel"], \ + motion_res["motion_bodies"], motion_res["motion_limb_weights"], motion_res["motion_aa"], motion_res["rg_pos"], motion_res["rb_rot"], motion_res["body_vel"], motion_res["body_ang_vel"] + + body_pos = robot.data.body_pos_w[:, sim_to_gym_body ] + body_rot = wxyz_to_xyzw(robot.data.body_quat_w[:, sim_to_gym_body]) + body_vel = robot.data.body_lin_vel_w[:, sim_to_gym_body] + body_ang_vel = robot.data.body_ang_vel_w[:, sim_to_gym_body] + + root_pos = body_pos[:, 0, :] + root_rot = body_rot[:, 0, :] + body_pos_subset = body_pos + body_rot_subset = body_rot + body_vel_subset = body_vel + body_ang_vel_subset = body_ang_vel + ref_rb_pos_subset = ref_rb_pos + ref_rb_rot_subset = ref_rb_rot + ref_body_vel_subset = ref_body_vel + ref_body_ang_vel_subset = ref_body_ang_vel + + # Data replay + # ref_joint_pos = ref_dof_pos[..., gym_to_sim_dof] + # ref_joint_vel = ref_dof_vel[..., gym_to_sim_dof] + # ref_root_state = torch.cat([ref_root_pos, xyzw_to_wxyz(ref_root_rot), ref_root_vel, ref_root_ang_vel], dim = -1) + # robot.write_root_state_to_sim(ref_root_state, env_ids) + # robot.write_joint_state_to_sim(ref_joint_pos, ref_joint_vel, None, env_ids) + + self_obs = compute_humanoid_observations_smpl_max(body_pos, body_rot, body_vel, body_ang_vel, ref_smpl_params, ref_limb_weights, True, False, has_upright_start, False, False) + task_obs = compute_imitation_observations_v6(root_pos, root_rot, body_pos_subset, body_rot_subset, body_vel_subset, body_ang_vel_subset, ref_rb_pos_subset, ref_rb_rot_subset, ref_body_vel_subset, ref_body_ang_vel_subset, 3, has_upright_start) + + full_obs = torch.cat([self_obs, task_obs], dim = -1) + full_obs = ((full_obs - running_mean.float()) / torch.sqrt(running_var.float() + 1e-05)) + full_obs = torch.clamp(full_obs, min=-5.0, max=5.0) + + with torch.no_grad(): + actions = mlp(full_obs) + actions = rescale_actions(-1, 1, torch.clamp(actions, -1, 1)) + actions = actions * pd_action_scale + pd_action_offset + actions = actions[:, gym_to_sim_dof] + actions[:] = 0 + + robot.set_joint_position_target(actions, None, env_ids) + + ############################## PHC ######################################## + + if time_steps > motion_len: + print("resetting") + reset_robot(robot) + time_steps[:] = 0 + + # -- write data to sim + scene.write_data_to_sim() + # Perform step + sim.step() + # sim.render() + # Increment counter + time_steps += sim_dt + # Update buffers + scene.update(sim_dt) + + # Measure wall time and ensure 30 fps + elapsed_time = time.time() - start_time + sleep_time = max(0, (1.0 / 30.0) - elapsed_time) + # time.sleep(sleep_time) + +@hydra.main(version_base=None, config_path="../phc/data/cfg", config_name="config") +def main(cfg : DictConfig) -> None: + """Main function.""" + # Load kit helper + sim_cfg = sim_utils.SimulationCfg( + device=args_cli.device, + dt=1 / 60, + # decimation will be set in the task config + # up axis will always be Z in isaac sim + # use_gpu_pipeline is deduced from the device + gravity=(0.0, 0.0, -9.81), + physx = PhysxCfg( + # num_threads is no longer needed + solver_type=1, + # use_gpu is deduced from the device + max_position_iteration_count=4, + max_velocity_iteration_count=0, + # moved to actor config + # moved to actor config + bounce_threshold_velocity=0.2, + # moved to actor config + # default_buffer_size_multiplier is no longer needed + gpu_max_rigid_contact_count=2**23, + # num_subscenes is no longer needed + # contact_collection is no longer needed + ) + ) + sim = SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([2.5, 0.0, 4.0], [0.0, 0.0, 2.0]) + # Design scene + scene_cfg = SMPLSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0) + scene = InteractiveScene(scene_cfg) + # Play the simulator + sim.reset() + # Now we are ready! + print("[INFO]: Setup complete...") + # Run the simulator + run_simulator(cfg, sim, scene) + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() \ No newline at end of file diff --git a/isaaclab/smpl_config.py b/isaaclab/smpl_config.py new file mode 100755 index 0000000..b720306 --- /dev/null +++ b/isaaclab/smpl_config.py @@ -0,0 +1,298 @@ +# Copyright (c) 2022-2024, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for a simple Cartpole robot.""" + + +import omni.isaac.lab.sim as sim_utils +from omni.isaac.lab.actuators import ImplicitActuatorCfg +from omni.isaac.lab.assets import ArticulationCfg +from omni.isaac.lab.utils.assets import ISAACLAB_NUCLEUS_DIR + +## +# Configuration +## + + +SMPL_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"data/assets/usd/smpl_humanoid.usda", + + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + retain_accelerations=False, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=1.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, solver_position_iteration_count=4, solver_velocity_iteration_count=4 + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 1.05), + joint_pos={".*": 0.0}, + joint_vel={".*": 0.0}, + ), + soft_joint_pos_limit_factor=0.9, + actuators={ + "legs": ImplicitActuatorCfg( + joint_names_expr=["L_Hip_.", "R_Hip_.", "L_Knee_.", "R_Knee_.", "L_Ankle_.", "R_Ankle_.", "L_Toe_.", "R_Toe_."], + effort_limit=500, + velocity_limit=100.0, + stiffness={ + "L_Hip_.": 800, + "R_Hip_.": 800, + "L_Knee_.": 800, + "R_Knee_.": 800, + "L_Ankle_.": 800, + "R_Ankle_.": 800, + "L_Toe_.": 500, + "R_Toe_.": 500, + }, + damping={ + "L_Hip_.": 80, + "R_Hip_.": 80, + "L_Knee_.": 80, + "R_Knee_.": 80, + "L_Ankle_.": 80, + "R_Ankle_.": 80, + "L_Toe_.": 50, + "R_Toe_.": 50, + + }, + ), + "torso": ImplicitActuatorCfg( + joint_names_expr=["Torso_.", "Spine_.", "Chest_.", "Neck_.", "Head_.", "L_Thorax_.", "R_Thorax_."], + effort_limit=500, + velocity_limit=100.0, + stiffness={ + "Torso_.": 1000, + "Spine_.": 1000, + "Chest_.": 1000, + "Neck_.": 500, + "Head_.": 500, + "L_Thorax_.": 500, + "R_Thorax_.": 500, + }, + damping={ + "Torso_.": 100, + "Spine_.": 100, + "Chest_.": 100, + "Neck_.": 50, + "Head_.": 50, + "L_Thorax_.": 50, + "R_Thorax_.": 50, + }, + ), + "arms": ImplicitActuatorCfg( + joint_names_expr=["L_Shoulder_.", "R_Shoulder_.", "L_Elbow_.", "R_Elbow_.", "L_Wrist_.", "R_Wrist_.", "L_Hand_.", "R_Hand_."], + effort_limit=300, + velocity_limit=100.0, + stiffness={ + "L_Shoulder_.": 500, + "R_Shoulder_.": 500, + "L_Elbow_.": 300, + "R_Elbow_.": 300, + "L_Wrist_.": 300, + "R_Wrist_.": 300, + "L_Hand_.": 300, + "R_Hand_.": 300, + }, + damping={ + "L_Shoulder_.": 50, + "R_Shoulder_.": 50, + "L_Elbow_.": 30, + "R_Elbow_.": 30, + "L_Wrist_.": 30, + "R_Wrist_.": 30, + }, + ), + }, +) + + +SMPLX_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + # usd_path=f"data/assets/usd/smplx_0.usda", + usd_path=f"data/assets/usd/smplx.usda", + + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + retain_accelerations=False, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=1.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, solver_position_iteration_count=4, solver_velocity_iteration_count=4 + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 1.05), + joint_pos={".*": 0.0}, + joint_vel={".*": 0.0}, + ), + soft_joint_pos_limit_factor=0.9, + actuators={ + "legs": ImplicitActuatorCfg( + joint_names_expr=["L_Hip_.", "R_Hip_.", "L_Knee_.", "R_Knee_.", "L_Ankle_.", "R_Ankle_.", "L_Toe_.", "R_Toe_."], + effort_limit=500, + velocity_limit=100.0, + stiffness={ + "L_Hip_.": 800, + "R_Hip_.": 800, + "L_Knee_.": 800, + "R_Knee_.": 800, + "L_Ankle_.": 800, + "R_Ankle_.": 800, + "L_Toe_.": 500, + "R_Toe_.": 500, + }, + damping={ + "L_Hip_.": 80, + "R_Hip_.": 80, + "L_Knee_.": 80, + "R_Knee_.": 80, + "L_Ankle_.": 80, + "R_Ankle_.": 80, + "L_Toe_.": 50, + "R_Toe_.": 50, + + }, + ), + "torso": ImplicitActuatorCfg( + joint_names_expr=["Torso_.", "Spine_.", "Chest_.", "Neck_.", "Head_.", "L_Thorax_.", "R_Thorax_."], + effort_limit=500, + velocity_limit=100.0, + stiffness={ + "Torso_.": 1000, + "Spine_.": 1000, + "Chest_.": 1000, + "Neck_.": 500, + "Head_.": 500, + "L_Thorax_.": 500, + "R_Thorax_.": 500, + }, + damping={ + "Torso_.": 100, + "Spine_.": 100, + "Chest_.": 100, + "Neck_.": 50, + "Head_.": 50, + "L_Thorax_.": 50, + "R_Thorax_.": 50, + }, + ), + "arms": ImplicitActuatorCfg( + joint_names_expr=["L_Shoulder_.", "R_Shoulder_.", "L_Elbow_.", "R_Elbow_.", "L_Wrist_.", "R_Wrist_."], + effort_limit=300, + velocity_limit=100.0, + stiffness={ + "L_Shoulder_.": 500, + "R_Shoulder_.": 500, + "L_Elbow_.": 300, + "R_Elbow_.": 300, + "L_Wrist_.": 300, + "R_Wrist_.": 300, + }, + damping={ + "L_Shoulder_.": 50, + "R_Shoulder_.": 50, + "L_Elbow_.": 30, + "R_Elbow_.": 30, + "L_Wrist_.": 30, + "R_Wrist_.": 30, + }, + ), + + "hands": ImplicitActuatorCfg( + joint_names_expr=[ + "L_Index1_.", "L_Index2_.", "L_Index3_.", + "L_Middle1_.", "L_Middle2_.", "L_Middle3_.", + "L_Pinky1_.", "L_Pinky2_.", "L_Pinky3_.", + "L_Ring1_.", "L_Ring2_.", "L_Ring3_.", + "L_Thumb1_.", "L_Thumb2_.", "L_Thumb3_.", + "R_Index1_.", "R_Index2_.", "R_Index3_.", + "R_Middle1_.", "R_Middle2_.", "R_Middle3_.", + "R_Pinky1_.", "R_Pinky2_.", "R_Pinky3_.", + "R_Ring1_.", "R_Ring2_.", "R_Ring3_.", + "R_Thumb1_.", "R_Thumb2_.", "R_Thumb3_." + ], + effort_limit=100, + velocity_limit=10.0, + stiffness={ + "L_Index1_.": 100, + "L_Index2_.": 100, + "L_Index3_.": 100, + "L_Middle1_.": 100, + "L_Middle2_.": 100, + "L_Middle3_.": 100, + "L_Pinky1_.": 100, + "L_Pinky2_.": 100, + "L_Pinky3_.": 100, + "L_Ring1_.": 100, + "L_Ring2_.": 100, + "L_Ring3_.": 100, + "L_Thumb1_.": 100, + "L_Thumb2_.": 100, + "L_Thumb3_.": 100, + "R_Index1_.": 100, + "R_Index2_.": 100, + "R_Index3_.": 100, + "R_Middle1_.": 100, + "R_Middle2_.": 100, + "R_Middle3_.": 100, + "R_Pinky1_.": 100, + "R_Pinky2_.": 100, + "R_Pinky3_.": 100, + "R_Ring1_.": 100, + "R_Ring2_.": 100, + "R_Ring3_.": 100, + "R_Thumb1_.": 100, + "R_Thumb2_.": 100, + "R_Thumb3_.": 100, + }, + damping={ + "L_Index1_.": 10, + "L_Index2_.": 10, + "L_Index3_.": 10, + "L_Middle1_.": 10, + "L_Middle2_.": 10, + "L_Middle3_.": 10, + "L_Pinky1_.": 10, + "L_Pinky2_.": 10, + "L_Pinky3_.": 10, + "L_Ring1_.": 10, + "L_Ring2_.": 10, + "L_Ring3_.": 10, + "L_Thumb1_.": 10, + "L_Thumb2_.": 10, + "L_Thumb3_.": 10, + "R_Index1_.": 10, + "R_Index2_.": 10, + "R_Index3_.": 10, + "R_Middle1_.": 10, + "R_Middle2_.": 10, + "R_Middle3_.": 10, + "R_Pinky1_.": 10, + "R_Pinky2_.": 10, + "R_Pinky3_.": 10, + "R_Ring1_.": 10, + "R_Ring2_.": 10, + "R_Ring3_.": 10, + "R_Thumb1_.": 10, + "R_Thumb2_.": 10, + "R_Thumb3_.": 10, + }, + ), + }, +) \ No newline at end of file diff --git a/requirement.txt b/requirement.txt index e1c9254..2f20e53 100644 --- a/requirement.txt +++ b/requirement.txt @@ -12,7 +12,7 @@ opencv-python==4.6.0.66 tqdm pyyaml wandb -mujoco +mujoco>=2.3.7 scikit-image gym git+https://github.com/ZhengyiLuo/smplx.git@master diff --git a/smpl_sim/smpllib/smpl_local_robot.py b/smpl_sim/smpllib/smpl_local_robot.py index 31701cf..5ff6915 100644 --- a/smpl_sim/smpllib/smpl_local_robot.py +++ b/smpl_sim/smpllib/smpl_local_robot.py @@ -1244,6 +1244,8 @@ def __init__(self, cfg, data_dir="data/smpl"): flat_hand_mean=True, num_betas=20, ) + elif self.smpl_model == "mano": + raise NotImplementedError("mano SMPL_Robot not implemented yet") self.load_from_skeleton() self.joint_names = [b.name for b in self.bodies] @@ -1330,11 +1332,11 @@ def load_from_skeleton( self.model_dirs.append(f"/tmp/smpl/{uuid.uuid4()}") self.skeleton = SkeletonMesh(self.model_dirs[-1]) - if self.smpl_model in ["smpl"]: + if self.smpl_model == "smpl": zero_pose = torch.zeros((1, 72)) - elif self.smpl_model in ["smpl", "smplh", "smplx"]: + elif self.smpl_model in ["smplh", "smplx"]: zero_pose = torch.zeros((1, 156)) - elif self.smpl_model in ["mano"]: + elif self.smpl_model == "mano": zero_pose = torch.zeros((1, 48)) if self.upright_start: @@ -1351,7 +1353,11 @@ def load_from_skeleton( joint_range, contype, conaffinity, - ) = (smpl_parser.get_mesh_offsets(zero_pose=zero_pose, v_template = v_template, betas=self.beta, flatfoot=self.flatfoot) ) + ) = ( + smpl_parser.get_mesh_offsets(zero_pose=zero_pose, v_template=v_template, betas=self.beta, flatfoot=self.flatfoot) + if self.smpl_model == "smplx" else + smpl_parser.get_mesh_offsets(zero_pose=zero_pose, betas=self.beta, flatfoot=self.flatfoot) + ) if self.rel_joint_lm: if self.upright_start: diff --git a/smpl_sim/utils/mujoco_utils.py b/smpl_sim/utils/mujoco_utils.py index 51f3969..87bf065 100644 --- a/smpl_sim/utils/mujoco_utils.py +++ b/smpl_sim/utils/mujoco_utils.py @@ -2,6 +2,7 @@ import mujoco from scipy.spatial.transform import Rotation as sRot + def get_body_qposaddr(model): # adapted to mujoco 2.3+ body_qposaddr = dict() @@ -23,6 +24,7 @@ def get_body_qposaddr(model): body_qposaddr[body_name] = (start_qposaddr, end_qposaddr) return body_qposaddr + def get_body_qveladdr(model): # adapted to mujoco 2.3+ body_qveladdr = dict() @@ -42,6 +44,7 @@ def get_body_qveladdr(model): body_qveladdr[body_name] = (start_qveladdr, end_qveladdr) return body_qveladdr + def get_jnt_range(model): jnt_range = dict() for i in range(model.njnt): @@ -53,6 +56,7 @@ def get_jnt_range(model): jnt_range[name] = model.jnt_range[i] return jnt_range + def get_actuator_names(model): actuators = [] for i in range(model.nu): @@ -76,25 +80,23 @@ def add_visual_capsule(scene, point1, point2, radius, rgba): if scene.ngeom >= scene.maxgeom: return scene.ngeom += 1 # increment ngeom - # initialise a new capsule, add it to the scene using mjv_makeConnector + # initialise a new capsule, add it to the scene using mjv_connector mujoco.mjv_initGeom(scene.geoms[scene.ngeom-1], mujoco.mjtGeom.mjGEOM_CAPSULE, np.zeros(3), np.zeros(3), np.zeros(9), rgba.astype(np.float32)) - mujoco.mjv_makeConnector(scene.geoms[scene.ngeom-1], + mujoco.mjv_connector(scene.geoms[scene.ngeom-1], mujoco.mjtGeom.mjGEOM_CAPSULE, radius, - point1[0], point1[1], point1[2], - point2[0], point2[1], point2[2]) + point1, point2) def add_visual_rbox(scene, point1, point2, rgba): """Adds one rectangle to an mjvScene.""" if scene.ngeom >= scene.maxgeom: return scene.ngeom += 1 # increment ngeom - # initialise a new capsule, add it to the scene using mjv_makeConnector + # initialise a new capsule, add it to the scene using mjv_connector mujoco.mjv_initGeom(scene.geoms[scene.ngeom-1], mujoco.mjtGeom.mjGEOM_BOX, np.zeros(3), np.zeros(3), np.zeros(9), rgba.astype(np.float32)) - mujoco.mjv_makeConnector(scene.geoms[scene.ngeom-1], + mujoco.mjv_connector(scene.geoms[scene.ngeom-1], mujoco.mjtGeom.mjGEOM_BOX, 0.01, - point1[0], point1[1], point1[2], - point2[0], point2[1], point2[2]) \ No newline at end of file + point1, point2) diff --git a/test.xml b/test.xml index e69de29..a0a16ff 100644 --- a/test.xml +++ b/test.xml @@ -0,0 +1,500 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +