본문 바로가기

Isaac Gym

시뮬레이션 오답 노트. #6-2 [Quadruped Robot Failed - DRL]

728x90

#Intro

저번에 이어서 오늘은 왜 내가 대학원에 가야하는가에 대한 정답을 얻은 계기를 써보려 한다. 

열심히 해봤는데 큰 벽이 느껴진다기 보단 세상에는 정말 재밌고 엄청난 기술이 많구나 라는걸 느끼게 된다. 

그림 1. Custom Model Reinforcement-Learning

 

2.  Reinforcement Learning

Custom Model을 Simulation에 넣어보니 inertial문제도 해결 했겠다.

부푼 기대를 가지고 코드에 내 모델을 직접 넣는데, 그 이전에 코드가 어떻게 흘러가는지 이해할 필요가 있었다. 

그림 2. Code의 Structure

일단은 코드를 작성할 때 내가 원하는 모델을 강화학습 시키기 위해서는 위와같은 구조를 이해하고 

Train할때 어떤 파라미터들을 건드려야 하는지 분석 할 필요가 있었다.

Custom 모델을 넣기 위해 Adding a New Env 부분을 참조하여 코드를 일부 수정하고나서

학습을 위해 고려할 파라미터들을 쭉 펼쳐봤는데 한두개가 아니였다.

 

아래는 해당 코드에서 고려한 파라미터들이다.

# Copyright (c) 2021 ETH Zurich, Nikita Rudin

from .base_config import BaseConfig

class LeggedRobotCfg(BaseConfig):
    class env:
        num_envs = 4096
        num_observations = 235
        num_privileged_obs = None # if not None a priviledge_obs_buf will be returned by step() (critic obs for assymetric training). None is returned otherwise 
        num_actions = 12
        env_spacing = 3.  # not used with heightfields/trimeshes 
        send_timeouts = True # send time out information to the algorithm
        episode_length_s = 20 # episode length in seconds

    class terrain:
        mesh_type = 'trimesh' # "heightfield" # none, plane, heightfield or trimesh
        horizontal_scale = 0.1 # [m]
        vertical_scale = 0.005 # [m]
        border_size = 25 # [m]
        curriculum = True
        static_friction = 1.0
        dynamic_friction = 1.0
        restitution = 0.
        # rough terrain only:
        measure_heights = True
        measured_points_x = [-0.8, -0.7, -0.6, -0.5, -0.4, -0.3, -0.2, -0.1, 0., 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8] # 1mx1.6m rectangle (without center line)
        measured_points_y = [-0.5, -0.4, -0.3, -0.2, -0.1, 0., 0.1, 0.2, 0.3, 0.4, 0.5]
        selected = False # select a unique terrain type and pass all arguments
        terrain_kwargs = None # Dict of arguments for selected terrain
        max_init_terrain_level = 5 # starting curriculum state
        terrain_length = 8.
        terrain_width = 8.
        num_rows= 10 # number of terrain rows (levels)
        num_cols = 20 # number of terrain cols (types)
        # terrain types: [smooth slope, rough slope, stairs up, stairs down, discrete]
        terrain_proportions = [0.1, 0.1, 0.35, 0.25, 0.2]
        # trimesh only:
        slope_treshold = 0.75 # slopes above this threshold will be corrected to vertical surfaces

    class commands:
        curriculum = False
        max_curriculum = 1.
        num_commands = 4 # default: lin_vel_x, lin_vel_y, ang_vel_yaw, heading (in heading mode ang_vel_yaw is recomputed from heading error)
        resampling_time = 10. # time before command are changed[s]
        heading_command = True # if true: compute ang vel command from heading error
        class ranges:
            lin_vel_x = [-1.0, 1.0] # min max [m/s]
            lin_vel_y = [-1.0, 1.0]   # min max [m/s]
            ang_vel_yaw = [-1, 1]    # min max [rad/s]
            heading = [-3.14, 3.14]

    class init_state:
        pos = [0.0, 0.0, 1.] # x,y,z [m]
        rot = [0.0, 0.0, 0.0, 1.0] # x,y,z,w [quat]
        lin_vel = [0.0, 0.0, 0.0]  # x,y,z [m/s]
        ang_vel = [0.0, 0.0, 0.0]  # x,y,z [rad/s]
        default_joint_angles = { # target angles when action = 0.0
            "joint_a": 0., 
            "joint_b": 0.}

    class control:
        control_type = 'P' # P: position, V: velocity, T: torques
        # PD Drive parameters:
        stiffness = {'joint_a': 10.0, 'joint_b': 15.}  # [N*m/rad]
        damping = {'joint_a': 1.0, 'joint_b': 1.5}     # [N*m*s/rad]
        # action scale: target angle = actionScale * action + defaultAngle
        action_scale = 0.5
        # decimation: Number of control action updates @ sim DT per policy DT
        decimation = 4

    class asset:
        file = ""
        foot_name = "None" # name of the feet bodies, used to index body state and contact force tensors
        penalize_contacts_on = []
        terminate_after_contacts_on = []
        disable_gravity = False
        collapse_fixed_joints = True # merge bodies connected by fixed joints. Specific fixed joints can be kept by adding " <... dont_collapse="true">
        fix_base_link = False # fixe the base of the robot
        default_dof_drive_mode = 3 # see GymDofDriveModeFlags (0 is none, 1 is pos tgt, 2 is vel tgt, 3 effort)
        self_collisions = 0 # 1 to disable, 0 to enable...bitwise filter
        replace_cylinder_with_capsule = True # replace collision cylinders with capsules, leads to faster/more stable simulation
        flip_visual_attachments = True # Some .obj meshes must be flipped from y-up to z-up
        
        density = 0.001
        angular_damping = 0.
        linear_damping = 0.
        max_angular_velocity = 1000.
        max_linear_velocity = 1000.
        armature = 0.
        thickness = 0.01

    class domain_rand:
        randomize_friction = True
        friction_range = [0.5, 1.25]
        randomize_base_mass = False
        added_mass_range = [-1., 1.]
        push_robots = True
        push_interval_s = 15
        max_push_vel_xy = 1.

    class rewards:
        class scales:
            termination = -0.0
            tracking_lin_vel = 1.0
            tracking_ang_vel = 0.5
            lin_vel_z = -2.0
            ang_vel_xy = -0.05
            orientation = -0.
            torques = -0.00001
            dof_vel = -0.
            dof_acc = -2.5e-7
            base_height = -0. 
            feet_air_time =  1.0
            collision = -1.
            feet_stumble = -0.0 
            action_rate = -0.01
            stand_still = -0.

        only_positive_rewards = True # if true negative total rewards are clipped at zero (avoids early termination problems)
        tracking_sigma = 0.25 # tracking reward = exp(-error^2/sigma)
        soft_dof_pos_limit = 1. # percentage of urdf limits, values above this limit are penalized
        soft_dof_vel_limit = 1.
        soft_torque_limit = 1.
        base_height_target = 1.
        max_contact_force = 100. # forces above this value are penalized

    class normalization:
        class obs_scales:
            lin_vel = 2.0
            ang_vel = 0.25
            dof_pos = 1.0
            dof_vel = 0.05
            height_measurements = 5.0
        clip_observations = 100.
        clip_actions = 100.

    class noise:
        add_noise = True
        noise_level = 1.0 # scales other values
        class noise_scales:
            dof_pos = 0.01
            dof_vel = 1.5
            lin_vel = 0.1
            ang_vel = 0.2
            gravity = 0.05
            height_measurements = 0.1

    # viewer camera:
    class viewer:
        ref_env = 0
        pos = [10, 0, 6]  # [m]
        lookat = [11., 5, 3.]  # [m]

    class sim:
        dt =  0.005
        substeps = 1
        gravity = [0., 0. ,-9.81]  # [m/s^2]
        up_axis = 1  # 0 is y, 1 is z

        class physx:
            num_threads = 10
            solver_type = 1  # 0: pgs, 1: tgs
            num_position_iterations = 4
            num_velocity_iterations = 0
            contact_offset = 0.01  # [m]
            rest_offset = 0.0   # [m]
            bounce_threshold_velocity = 0.5 #0.5 [m/s]
            max_depenetration_velocity = 1.0
            max_gpu_contact_pairs = 2**23 #2**24 -> needed for 8000 envs and more
            default_buffer_size_multiplier = 5
            contact_collection = 2 # 0: never, 1: last sub-step, 2: all sub-steps (default=2)

class LeggedRobotCfgPPO(BaseConfig):
    seed = 1
    runner_class_name = 'OnPolicyRunner'
    class policy:
        init_noise_std = 1.0
        actor_hidden_dims = [512, 256, 128]
        critic_hidden_dims = [512, 256, 128]
        activation = 'elu' # can be elu, relu, selu, crelu, lrelu, tanh, sigmoid
        # only for 'ActorCriticRecurrent':
        # rnn_type = 'lstm'
        # rnn_hidden_size = 512
        # rnn_num_layers = 1
        
    class algorithm:
        # training params
        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 # mini batch size = num_envs*nsteps / nminibatches
        learning_rate = 1.e-3 #5.e-4
        schedule = 'adaptive' # could be adaptive, fixed
        gamma = 0.99
        lam = 0.95
        desired_kl = 0.01
        max_grad_norm = 1.

    class runner:
        policy_class_name = 'ActorCritic'
        algorithm_class_name = 'PPO'
        num_steps_per_env = 24 # per iteration
        max_iterations = 1500 # number of policy updates

        # logging
        save_interval = 50 # check for potential saves every this many iterations
        experiment_name = 'test'
        run_name = ''
        # load and resume
        resume = False
        load_run = -1 # -1 = last run
        checkpoint = -1 # -1 = last saved model
        resume_path = None # updated from load_run and chkpt

대략 200줄이나 되는 코드로 하이퍼 파리미터들이 정리 된다.

이모든것을 하나하나 곱씹고 이해하면서 어떻게 observation 돼서 state로 나타나는지 이해하는 것 보다

주어진 자료를 바탕으로 역으로 해석해 나가는 것이 빠르다고 생각하여 탑다운 방식으로 문제를 해결해보고자 했다.

 

솔직히 처음 봤을땐 숨이 턱 막혔지만 논문을 참조하며 하나 둘 씩 제거해 봤다.

#Comparison

이미 존재하는 모델들의 Config파일을 비교 해보는 방법

간단하면서도 이해하기 가장 쉬운 방법이였는데 한가지 예시 파일로 a1 모델을 참조 했다.

# Copyright (c) 2021 ETH Zurich, Nikita Rudin

from legged_gym.envs.base.legged_robot_config import LeggedRobotCfg, LeggedRobotCfgPPO

class A1RoughCfg( LeggedRobotCfg ):
    class init_state( LeggedRobotCfg.init_state ):
        pos = [0.0, 0.0, 0.42] # x,y,z [m]
        default_joint_angles = { # = target angles [rad] when action = 0.0
            'FL_hip_joint': 0.1,   # [rad]
            'RL_hip_joint': 0.1,   # [rad]
            'FR_hip_joint': -0.1 ,  # [rad]
            'RR_hip_joint': -0.1,   # [rad]

            'FL_thigh_joint': 0.8,     # [rad]
            'RL_thigh_joint': 1.,   # [rad]
            'FR_thigh_joint': 0.8,     # [rad]
            'RR_thigh_joint': 1.,   # [rad]

            'FL_calf_joint': -1.5,   # [rad]
            'RL_calf_joint': -1.5,    # [rad]
            'FR_calf_joint': -1.5,  # [rad]
            'RR_calf_joint': -1.5,    # [rad]
        }

    class control( LeggedRobotCfg.control ):
        # PD Drive parameters:
        control_type = 'P'
        stiffness = {'joint': 20.}  # [N*m/rad]
        damping = {'joint': 0.5}     # [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 = 4

    class asset( LeggedRobotCfg.asset ):
        file = '{LEGGED_GYM_ROOT_DIR}/resources/robots/a1/urdf/a1.urdf'
        foot_name = "foot"
        penalize_contacts_on = ["thigh", "calf"]
        terminate_after_contacts_on = ["base"]
        self_collisions = 1 # 1 to disable, 0 to enable...bitwise filter
  
    class rewards( LeggedRobotCfg.rewards ):
        soft_dof_pos_limit = 0.9
        base_height_target = 0.25
        class scales( LeggedRobotCfg.rewards.scales ):
            torques = -0.0002
            dof_pos_limits = -10.0

class A1RoughCfgPPO( LeggedRobotCfgPPO ):
    class algorithm( LeggedRobotCfgPPO.algorithm ):
        entropy_coef = 0.01
    class runner( LeggedRobotCfgPPO.runner ):
        run_name = ''
        experiment_name = 'rough_a1'
1. Init_State

학습의 방향이되는 가장중요한 target은 Leg의 Position이다. 이는 깃허브 문서에도 기재 돼 있다.

<joint name=''>
	<limit effort='[Nm]' velocity='[rad/s]' lower='[rad]' upper='[rad]'/>
</joint>

default_joint_angles는 처음 URDF만들때 설정한 lower와 upper 기재된 [rad]값을 따른다. 

 

여기서 Design적인 문제가 발생하는데, 내가 원하는 position이 stable한 target으로 삼기에 적합한지에 대한 해석이 필요하다.

그저 남이 주는 모델을 기반으로 CAD파일을 변환해서 Target Position을 0에 맞추고 모델을 학습시키면 이게 설계적으로 

맞는 target인지 고려해볼 필요가 있었다. 

 

이번학기에 다관절에 관한 kinematics를 배우기 때문에

아직 이론적인 배경을 섬세하게 접목시키는 것은 어렵다고 판단하여 

최대한 Inertia가 적고 무게 중심을 아래쪽에 배치하는 Position을 Target으로 잡고자 하였다.

 

이부분에서의 문제를 최소화 하고자 단순하게 Gravity만 작용하는 Simulation을 만들고 다양한 각도를 넣어보면서

10cm정도 되는 공중에서 떨어뜨려도 제대로 4발로 설 수 있는 각도를 찾아서 기입하였다.

 

2. Stiffness & Damping

두번째 파라미터로는 Target Position에서 벗어났을때 얼마나 빨리 React해서 목표를 찾아가는지 설정하는

Stiffness 와 Damping 상수 이다.

 

이부분을 건드릴 때는 Rotational mechanical system에 대한 공부가 필요했는데 간단한 영상들을 모아서 이해해본 바로는

Stiffness는 Control시에 회복하는 속도를 나타내고 Damping은 신호가 너무 튀지 않고 목표 주변에서 안정적으로 잡아주게 끔 하는 요소라고 해석했다.

3, Rewards 

Simulation에서는 Leg의 Position을 타겟으로 잡고 난 뒤, base의 높이로 보상을 준다. 

이 외에도 평가 요소가 꽤 많은데, 그중 대표적으로 나를 괴롭혔던 것이 Ang velocity와 Action exploration이다.

솔직히 여기서 많은 부분이 막혔는데, 뭐가 문제인지 감이 오질 않았다. 모델에 가하는 외부 힘의 Noise계수를 줄여서 

가벼운 모델에 대한 학습 방법을 적용 시켜 보기도 하고, Ant 로봇 처럼 최대한 펼쳐진 구조로 안정적이게 target과 init state를 

잡아보기도 하고, base의 높이를 낮춰보기도 하고 다양한 시도를 해봤지만 a1 로봇처럼 완벽한 학습을 이루지 못하고

자꾸 엉덩이로 주저앉는 모습을 보였다.

 

#Limitation

어떤 파라미터를 건드려야 좋은 결과를 내는지 알아보고 싶었으나 한번 학습하는 데 많게는 20분 적게는 3분 정도 소요 된다는 점이

무작정  그래프만 보고 문제를 해결하는 것 보다 밑바닥 부터 차근차근 배워서 다시 쌓아보는게 더 빠르겠다고 생각한다.

설계의 시작점 부터 많이 잘못 됐다고 판단하고 프로젝트를 마무리 할 생각이다.

Cartpole이나 Acrobot으로 넘어가 문제를 다시 접근하는게 필요하다고 느꼈다.

그림 3. a1 로봇

 

그림 4. Custom Model 결과

 

겨울방학동안 혼자 열심히 공부해 보면서 정말 내가 어디까지 할 수 있을까 부딪혀보는 좋은 계기가 됐다.

마음같아서는 계속 매달리면서 프로젝트를 완성시켜보고 싶지만, 곧 개강시즌인데다가 대학원을 준비할 때 더 넓은 그림을 

놓칠 것 같아서 중단하고 조금더 범용성 넓은 프로젝트로 도전해보려고 한다. 

 

Quadruped 로봇 task를 선정하게 된 이유 중 하나가 

고령화가 오는 사회에 삶의 질을 보태는 기술중 하나지 않은가 하는 비전을 가지고 시작하게 됐다. 

이 외에도 강화학습과 vision or slam같은 기술이 붙게되면, 가사도움 기술로 응용도 돼서

굉장히 흥미로운 주제였는데, 다른 일들이 겹쳐 잠시 미루게 된것이 아쉬울 따름이다.

 

앞으로의 블로그 일정은 Acrobot 아니면 Cartpole을 만들고 simluation과 real 을 잇는것에 집중해볼 예정이다.

그림 5. Gym Data으로 Servo Motor Control