This commit is contained in:
Yecheng Shao 2024-07-17 17:20:50 +08:00
parent 90956fc4df
commit 30dcc48623
6 changed files with 134 additions and 3 deletions

View File

@ -2,9 +2,11 @@ from legged_gym import LEGGED_GYM_ROOT_DIR, LEGGED_GYM_ENVS_DIR
from legged_gym.envs.go2.go2_config import GO2RoughCfg, GO2RoughCfgPPO from legged_gym.envs.go2.go2_config import GO2RoughCfg, GO2RoughCfgPPO
from legged_gym.envs.h1.h1_config import H1RoughCfg, H1RoughCfgPPO from legged_gym.envs.h1.h1_config import H1RoughCfg, H1RoughCfgPPO
from legged_gym.envs.h1_2.h1_2_config import H1_2RoughCfg, H1_2RoughCfgPPO
from .base.legged_robot import LeggedRobot from .base.legged_robot import LeggedRobot
from legged_gym.utils.task_registry import task_registry from legged_gym.utils.task_registry import task_registry
task_registry.register("go2", LeggedRobot, GO2RoughCfg(), GO2RoughCfgPPO()) task_registry.register("go2", LeggedRobot, GO2RoughCfg(), GO2RoughCfgPPO())
task_registry.register("h1", LeggedRobot, H1RoughCfg(), H1RoughCfgPPO()) task_registry.register("h1", LeggedRobot, H1RoughCfg(), H1RoughCfgPPO())
task_registry.register("h1_2", LeggedRobot, H1_2RoughCfg(), H1_2RoughCfgPPO())

View File

@ -0,0 +1,121 @@
from legged_gym.envs.base.legged_robot_config import LeggedRobotCfg, LeggedRobotCfgPPO
class H1_2RoughCfg(LeggedRobotCfg):
class init_state(LeggedRobotCfg.init_state):
pos = [0.0, 0.0, 1.0] # x,y,z [m]
default_joint_angles = { # = target angles [rad] when action = 0.0
'left_hip_yaw_joint': 0,
'left_hip_roll_joint': 0,
'left_hip_pitch_joint': -0.6,
'left_knee_joint': 1.2,
'left_ankle_pitch_joint': -0.6,
'left_ankle_roll_joint': 0.0,
'right_hip_yaw_joint': 0,
'right_hip_roll_joint': 0,
'right_hip_pitch_joint': -0.6,
'right_knee_joint': 1.2,
'right_ankle_pitch_joint': -0.6,
'right_ankle_roll_joint': 0.0,
'torso_joint': 0,
'left_shoulder_pitch_joint': 0.4,
'left_shoulder_roll_joint': 0,
'left_shoulder_yaw_joint': 0,
'left_elbow_pitch_joint': 0.3,
'right_shoulder_pitch_joint': 0.4,
'right_shoulder_roll_joint': 0,
'right_shoulder_yaw_joint': 0,
'right_elbow_pitch_joint': 0.3,
}
class env(LeggedRobotCfg.env):
num_actions = 21
num_observations = 12 + num_actions * 3
num_envs = 8192
class control(LeggedRobotCfg.control):
# PD Drive parameters:
control_type = 'P'
# PD Drive parameters:
stiffness = {
'hip_yaw_joint': 200.,
'hip_roll_joint': 200.,
'hip_pitch_joint': 200.,
'knee_joint': 300.,
'ankle_pitch_joint': 60.,
'ankle_roll_joint': 40.,
'torso_joint': 600.,
'shoulder_pitch_joint': 80.,
'shoulder_roll_joint': 80.,
'shoulder_yaw_joint': 40.,
'elbow_pitch_joint': 60.,
} # [N*m/rad]
damping = {
'hip_yaw_joint': 5.0,
'hip_roll_joint': 5.0,
'hip_pitch_joint': 5.0,
'knee_joint': 7.5,
'ankle_pitch_joint': 1.0,
'ankle_roll_joint': 0.3,
'torso_joint': 15.0,
'shoulder_pitch_joint': 2.0,
'shoulder_roll_joint': 2.0,
'shoulder_yaw_joint': 1.0,
'elbow_pitch_joint': 1.0,
} # [N*m/rad] # [N*m*s/rad]
# action scale: target angle = actionScale * action + defaultAngle
action_scale = 0.25
# decimation: Number of control action updates @ sim DT per policy DT
decimation = 10
class asset(LeggedRobotCfg.asset):
file = '{LEGGED_GYM_ROOT_DIR}/resources/robots/h1_2/h1_2_simplified.urdf'
name = "h1_2"
foot_name = "ankle_roll"
penalize_contacts_on = []
terminate_after_contacts_on = []
self_collisions = 1 # 1 to disable, 0 to enable...bitwise filter
flip_visual_attachments = False
armature = 6e-4 # stablize semi-euler integration for end effectors
class sim(LeggedRobotCfg.sim):
dt = 0.002 # stablize semi-euler integration for end effectors
class rewards(LeggedRobotCfg.rewards):
soft_dof_pos_limit = 0.9
base_height_target = 0.98
class scales(LeggedRobotCfg.rewards.scales):
tracking_lin_vel = 1.0
tracking_ang_vel = 0.5
lin_vel_z = -0.2
ang_vel_xy = -0.1
orientation = -0.1
base_height = -10.0
dof_acc = -3.5e-9
feet_air_time = 1.0
collision = 0.0
action_rate = -0.001
torques = 0.0
dof_pos_limits = -1.0
only_positive_rewards = False # if true negative total rewards are clipped at zero (avoids early termination problems)
class normalization(LeggedRobotCfg.normalization):
clip_actions = 10.0
class H1_2RoughCfgPPO(LeggedRobotCfgPPO):
class policy(LeggedRobotCfgPPO.policy):
init_noise_std = 0.3
activation = 'tanh'
class runner(LeggedRobotCfgPPO.runner):
run_name = ''
experiment_name = 'h1_2'

View File

@ -1,5 +1,4 @@
import sys import sys
sys.path.append("/home/unitree/unitree_rl_gym")
from legged_gym import LEGGED_GYM_ROOT_DIR from legged_gym import LEGGED_GYM_ROOT_DIR
import os import os

View File

@ -2,7 +2,6 @@ import numpy as np
import os import os
from datetime import datetime from datetime import datetime
import sys import sys
sys.path.append("/home/unitree/unitree_rl_gym/")
import isaacgym import isaacgym
from legged_gym.envs import * from legged_gym.envs import *

View File

@ -4,7 +4,6 @@ from typing import Tuple
import torch import torch
import numpy as np import numpy as np
import sys import sys
sys.path.append("/home/unitree/unitree_rl_gym/rsl_rl")
from rsl_rl.env import VecEnv from rsl_rl.env import VecEnv
from rsl_rl.runners import OnPolicyRunner from rsl_rl.runners import OnPolicyRunner

11
setup.py Normal file
View File

@ -0,0 +1,11 @@
from setuptools import find_packages
from distutils.core import setup
setup(name='unitree_rl_gym',
version='1.0.0',
author='Unitree Robotics',
license="BSD-3-Clause",
packages=find_packages(),
author_email='support@unitree.com',
description='Template RL environments for Unitree Robots',
install_requires=['isaacgym', 'rsl-rl', 'matplotlib', 'numpy==1.20', 'tensorboard'])