Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[New Robot] OpenLoong Support #14

Closed
wants to merge 11 commits into from
Closed
Show file tree
Hide file tree
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
9 changes: 9 additions & 0 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,15 @@ python scripts/rsl_rl/train.py --task RobotLab-Isaac-Velocity-Flat-Unitree-H1-v0
python scripts/rsl_rl/play.py --task RobotLab-Isaac-Velocity-Flat-Unitree-H1-v0
```

OpenLoong OpenLoong

```bash
# Train
python scripts/rsl_rl/train.py --task RobotLab-Isaac-Velocity-Flat-OpenLoong-OpenLoong-v0 --headless
# Play
python scripts/rsl_rl/play.py --task RobotLab-Isaac-Velocity-Flat-OpenLoong-OpenLoong-v0
```

The above configs are flat, you can change Flat to Rough

**Note**
Expand Down
Binary file not shown.
81 changes: 81 additions & 0 deletions exts/robot_lab/robot_lab/assets/openloong.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,81 @@
"""Configuration for OPENLOONG robots.

The following configurations are available:

* :obj:`OPENLOONG_OPENLOONG_CFG`: OPENLOONG OPENLOONG humanoid robot

Reference: https://www.openloong.org.cn/cn
"""

from robot_lab.assets import ISAACLAB_ASSETS_DATA_DIR

import omni.isaac.lab.sim as sim_utils
from omni.isaac.lab.actuators import ImplicitActuatorCfg
from omni.isaac.lab.assets.articulation import ArticulationCfg

##
# Configuration
##


OPENLOONG_OPENLOONG_CFG = ArticulationCfg(
spawn=sim_utils.UsdFileCfg(
usd_path=f"{ISAACLAB_ASSETS_DATA_DIR}/Robots/OpenLoong/OpenLoong.usd",
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.09),
joint_pos={
#left_leg
'l_hip_roll' : 0.,
'l_hip_yaw' : 0. ,
'l_hip_pitch' : 0.,
'l_knee_pitch' : 0.0,
'l_ankle_pitch': 0.,
'l_ankle_roll': 0.,
#right_leg
'r_hip_roll' : 0.,
'r_hip_yaw' : 0.,
'r_hip_pitch' : 0.,
'r_knee_pitch' : 0.0,
'r_ankle_pitch': 0.,
'r_ankle_roll': 0.,
},
joint_vel={".*": 0.0},
),
soft_joint_pos_limit_factor=0.9,
actuators={
"actuators": ImplicitActuatorCfg(
joint_names_expr=[".*"],
stiffness={
'.*_hip_roll': 251.625,
'.*_hip_pitch': 200,
'.*_hip_yaw': 362.5214,
'.*_knee_pitch': 200,
'.*_ankle_pitch': 10.9805,
'.*_ankle_roll': 10.9805,
},
damping={
'.*_hip_roll': 10,
'.*_hip_pitch': 10,
'.*_hip_yaw': 10,
'.*_knee_pitch':10,
'.*_ankle_pitch': 10,
'.*_ankle_roll': 10,
},
),
},
)
"""Configuration for the OPENLOONG OPENLOONG Humanoid robot."""
Original file line number Diff line number Diff line change
@@ -0,0 +1,27 @@
import gymnasium as gym

from . import agents, flat_env_cfg, rough_env_cfg

##
# Register Gym environments.
##

gym.register(
id="RobotLab-Isaac-Velocity-Rough-OpenLoong-OpenLoong-v0",
entry_point="omni.isaac.lab.envs:ManagerBasedRLEnv",
disable_env_checker=True,
kwargs={
"env_cfg_entry_point": rough_env_cfg.OpenLoongOpenLoongRoughEnvCfg,
"rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:OpenLoongOpenLoongRoughPPORunnerCfg",
},
)

gym.register(
id="RobotLab-Isaac-Velocity-Flat-OpenLoong-OpenLoong-v0",
entry_point="omni.isaac.lab.envs:ManagerBasedRLEnv",
disable_env_checker=True,
kwargs={
"env_cfg_entry_point": flat_env_cfg.OpenLoongOpenLoongFlatEnvCfg,
"rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:OpenLoongOpenLoongFlatPPORunnerCfg",
},
)
Original file line number Diff line number Diff line change
@@ -0,0 +1,46 @@
from omni.isaac.lab.utils import configclass
from omni.isaac.lab_tasks.utils.wrappers.rsl_rl import (
RslRlOnPolicyRunnerCfg,
RslRlPpoActorCriticCfg,
RslRlPpoAlgorithmCfg,
)


@configclass
class OpenLoongOpenLoongRoughPPORunnerCfg(RslRlOnPolicyRunnerCfg):
num_steps_per_env = 24
max_iterations = 3000
save_interval = 50
experiment_name = "openloong_rough"
empirical_normalization = False
policy = RslRlPpoActorCriticCfg(
init_noise_std=1.0,
actor_hidden_dims=[512, 256, 128],
critic_hidden_dims=[512, 256, 128],
activation="elu",
)
algorithm = RslRlPpoAlgorithmCfg(
value_loss_coef=1.0,
use_clipped_value_loss=True,
clip_param=0.2,
entropy_coef=0.01,
num_learning_epochs=5,
num_mini_batches=4,
learning_rate=1.0e-3,
schedule="adaptive",
gamma=0.99,
lam=0.95,
desired_kl=0.01,
max_grad_norm=1.0,
)


@configclass
class OpenLoongOpenLoongFlatPPORunnerCfg(OpenLoongOpenLoongRoughPPORunnerCfg):
def __post_init__(self):
super().__post_init__()

self.max_iterations = 1000
self.experiment_name = "openloong_flat"
self.policy.actor_hidden_dims = [128, 128, 128]
self.policy.critic_hidden_dims = [128, 128, 128]
Original file line number Diff line number Diff line change
@@ -0,0 +1,29 @@
from omni.isaac.lab.utils import configclass

from .rough_env_cfg import OpenLoongOpenLoongRoughEnvCfg


@configclass
class OpenLoongOpenLoongFlatEnvCfg(OpenLoongOpenLoongRoughEnvCfg):
def __post_init__(self):
# Temporarily not run disable_zerow_eight_rewards() in parent class to override rewards
self._run_disable_zero_weight_rewards = False
# post init of parent
super().__post_init__()

# override rewards
self.rewards.feet_air_time.weight = 1.0
self.rewards.feet_air_time.params["threshold"] = 0.5
# change terrain to flat
self.scene.terrain.terrain_type = "plane"
self.scene.terrain.terrain_generator = None
# no height scan
self.scene.height_scanner = None
self.observations.policy.height_scan = None
# no terrain curriculum
self.curriculum.terrain_levels = None

# Now executing disable_zerow_eight_rewards()
self._run_disable_zero_weight_rewards = True
if self._run_disable_zero_weight_rewards:
self.disable_zero_weight_rewards()
Original file line number Diff line number Diff line change
@@ -0,0 +1,128 @@
import robot_lab.tasks.locomotion.velocity.mdp as mdp
from robot_lab.tasks.locomotion.velocity.velocity_env_cfg import LocomotionVelocityRoughEnvCfg

from omni.isaac.lab.utils import configclass

##
# Pre-defined configs
##
# use local assets
from robot_lab.assets.openloong import OPENLOONG_OPENLOONG_CFG # isort: skip


@configclass
class OpenLoongOpenLoongRoughEnvCfg(LocomotionVelocityRoughEnvCfg):
_run_disable_zero_weight_rewards = True

def __post_init__(self):
# post init of parent
super().__post_init__()

# ------------------------------Sence------------------------------
# switch robot to fftai-GR1T1
self.scene.robot = OPENLOONG_OPENLOONG_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/base_link"

# ------------------------------Observations------------------------------
# self.observations.policy.base_lin_vel = None
# self.observations.policy.height_scan = None
self.observations.AMP = None
# ------------------------------Actions------------------------------
# reduce action scale
# self.actions.joint_pos.scale = 1.0

# ------------------------------Events------------------------------
self.events.reset_base_amp = None
self.events.reset_robot_joints_amp = None
self.events.base_external_force_torque.params["asset_cfg"].body_names = ["base_link"]
self.events.reset_robot_joints.params["position_range"] = (1.0, 1.0)
self.events.reset_base.params = {
"pose_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5), "yaw": (-3.14, 3.14)},
"velocity_range": {
"x": (0.0, 0.0),
"y": (0.0, 0.0),
"z": (0.0, 0.0),
"roll": (0.0, 0.0),
"pitch": (0.0, 0.0),
"yaw": (0.0, 0.0),
},
}
self.events.push_robot = None
self.events.add_base_mass = None
self.events.randomize_actuator_gains = None
self.events.randomize_joint_parameters = None

# ------------------------------Rewards------------------------------
# General
# UNUESD self.rewards.is_alive.weight = 0
self.rewards.is_terminated.weight = -200

# Root penalties
self.rewards.lin_vel_z_l2.weight = 0
self.rewards.ang_vel_xy_l2.weight = -0.05
self.rewards.flat_orientation_l2.weight = -1.0
self.rewards.base_height_l2.weight = 0
self.rewards.body_lin_acc_l2.weight = 0

# Joint penaltie
self.rewards.joint_torques_l2.weight = 0
# UNUESD self.rewards.joint_vel_l1.weight = 0.0
self.rewards.joint_vel_l2.weight = 0
self.rewards.joint_acc_l2.weight = -1.25e-7
self.rewards.create_joint_deviation_l1_rewterm(
"joint_deviation_other_l1", -1.0, [".*_hip_yaw", ".*_hip_roll"]
)
self.rewards.create_joint_deviation_l1_rewterm("joint_deviation_knee_l1", -0.1, ['.*_knee_pitch'])
self.rewards.joint_pos_limits.weight = -1.0
self.rewards.joint_pos_limits.params["asset_cfg"].joint_names = [".*_ankle_pitch",".*_ankle_roll"]
self.rewards.joint_vel_limits.weight = 0

# Action penalties
self.rewards.applied_torque_limits.weight = 0
self.rewards.action_rate_l2.weight = -0.005
# UNUESD self.rewards.action_l2.weight = 0.0

# Contact sensor
self.rewards.undesired_contacts.weight = 0
self.rewards.contact_forces.weight = 0

# Velocity-tracking rewards
self.rewards.track_lin_vel_xy_exp.weight = 1.0
self.rewards.track_lin_vel_xy_exp.func = mdp.track_lin_vel_xy_yaw_frame_exp
self.rewards.track_lin_vel_xy_exp.params["std"] = 0.5
self.rewards.track_ang_vel_z_exp.weight = 1.0
self.rewards.track_ang_vel_z_exp.func = mdp.track_ang_vel_z_world_exp
self.rewards.track_ang_vel_z_exp.params["std"] = 0.5

# Others
self.rewards.feet_air_time.weight = 0.25
self.rewards.feet_air_time.func = mdp.feet_air_time_positive_biped
self.rewards.feet_air_time.params["sensor_cfg"].body_names = ["Link_r_ankle_roll","Link_l_ankle_roll"]
self.rewards.feet_air_time.params["threshold"] = 0.4
self.rewards.foot_contact.weight = 0
self.rewards.base_height_rough_l2.weight = 0
self.rewards.feet_slide.weight = -0.25
self.rewards.feet_slide.params["sensor_cfg"].body_names = ["Link_r_ankle_roll","Link_l_ankle_roll"]
self.rewards.feet_slide.params["asset_cfg"].body_names = ["Link_r_ankle_roll","Link_l_ankle_roll"]
self.rewards.joint_power.weight = 0
self.rewards.stand_still_when_zero_command.weight = 0

# If the weight of rewards is 0, set rewards to None
if self._run_disable_zero_weight_rewards:
self.disable_zero_weight_rewards()

# ------------------------------Terminations------------------------------
self.terminations.illegal_contact.params["sensor_cfg"].body_names = [
"base_link",
".*_waist_.*",
".*_head_.*",
".*_arm_r_.*",
".*_arm_l_.*",
".*_hip_r_.*",
".*_hip_l_.*"
]

# ------------------------------Commands------------------------------
self.commands.base_velocity.ranges.lin_vel_x = (0.0, 1.0)
self.commands.base_velocity.ranges.lin_vel_y = (0.0, 0.0)
self.commands.base_velocity.ranges.ang_vel_z = (-1.0, 1.0)