--- a
+++ b/envs/control/loco_reflex_song2019.py
@@ -0,0 +1,549 @@
+# Author(s): Seungmoon Song <seungmoon.song@gmail.com>
+"""
+adapted from:
+- Song and Geyer. "A neural circuitry that emphasizes
+spinal feedback generates diverse behaviours of human locomotion." The
+Journal of physiology, 2015.
+- The control doesn't use muscle states if not needed
+  - still uses muscle force data for postivie force feedback
+- Removed some control pathways
+  - M1: from GLU and HAB
+  - M2: from HAM
+  - M4
+- Added some control pathways
+  - M1: RF
+"""
+
+# - [x y z] -> [anterior lateral superior]
+#         (<-> [posterior medial inferior])
+
+from __future__ import division # '/' always means non-truncating division
+import numpy as np
+
+class LocoCtrl(object):
+    DEBUG = 0
+
+    RIGHT = 0 # r_leg
+    LEFT = 1 # l_leg
+
+    # (todo) use these when handling angles
+    # THETA0 = 0*np.pi/180 # trunk angle when standing straight
+    # S_THETA = 1 # 1: leaning forward > 0; -1: leaning backward > 0
+    # HIP0 = 0*np.pi/180 # hip angle when standing straight
+    # S_HIP = 1 # 1: extension > 0; -1: flexion > 0
+    # KNEE0 = 0*np.pi/180 # knee angle when standing straight
+    # S_KNEE = 1 # 1: extension > 0; -1: flexion > 0
+    # ANKLE0 = 0*np.pi/180 # ankle angle when standing straight
+    # S_ANKLE = 1 # 1: plantar flexion > 0; -1: dorsiflexion > 0
+
+    # muscle names
+    m_keys = ['HAB', 'HAD', 'HFL', 'GLU', 'HAM', 'RF', 'VAS', 'BFSH', 'GAS', 'SOL', 'TA']
+    # body sensor data
+    s_b_keys = ['theta', 'd_pos', 'dtheta']
+        # theta[0]: around local x axis (pointing anterior)
+        # theta[1]: around local y axis (pointing leftward)
+        # theta[2]: around local z axis (pointing upward)
+        # pos[0]: local x
+        # pos[1]: local y
+        # pos[2]: local z
+    # leg sensor data
+    # anglular values follow the Song2015 convention
+    s_l_keys = [
+        'contact_ipsi', 'contact_contra', 'load_ipsi', 'load_contra',
+        'alpha', 'alpha_f', 'dalpha',
+        'phi_hip', 'phi_knee', 'phi_ankle', 'dphi_knee'
+        'F_RF', 'F_VAS', 'F_GAS', 'F_SOL',
+        ]
+    # control states
+    cs_keys = [
+        'ph_st', # leg in stance
+        'ph_st_csw', # leg in stance ^ contra-leg in swing
+        'ph_st_sw0', # leg in stance ^ initial swing
+        'ph_sw', # leg in swing
+        'ph_sw_flex_k', # leg in swing ^ flex knees
+        'ph_sw_hold_k', # leg in swing ^ hold knee
+        'ph_sw_stop_l', # leg in swing ^ stop leg
+        'ph_sw_hold_l' # leg in swing ^ hold leg
+        ]
+    # control parameters
+    cp_keys = [
+        'theta_tgt', 'c0', 'cv', 'alpha_delta',
+        'knee_sw_tgt', 'knee_tgt', 'knee_off_st', 'ankle_tgt',
+        'HFL_3_PG', 'HFL_3_DG', 'HFL_6_PG', 'HFL_6_DG', 'HFL_10_PG',
+        'GLU_3_PG', 'GLU_3_DG', 'GLU_6_PG', 'GLU_6_DG', 'GLU_10_PG',
+        'HAM_3_GLU', 'HAM_9_PG',
+        'RF_1_FG', 'RF_8_DG_knee',
+        'VAS_1_FG', 'VAS_2_PG', 'VAS_10_PG',
+        'BFSH_2_PG', 'BFSH_7_DG_alpha', 'BFSH_7_PG', 'BFSH_8_DG', 'BFSH_8_PG',
+        'BFSH_9_G_HAM', 'BFSH_9_HAM0', 'BFSH_10_PG',
+        'GAS_2_FG',
+        'SOL_1_FG',
+        'TA_5_PG', 'TA_5_G_SOL',
+        'theta_tgt_f', 'c0_f', 'cv_f',
+        'HAB_3_PG', 'HAB_3_DG', 'HAB_6_PG',
+        'HAD_3_PG', 'HAD_3_DG', 'HAD_6_PG'
+        ]
+
+    par_space = (
+        [0.0, -1.0, 0.0, 0.0, \
+        -2.0, -90/15, -1.0, 0.0, \
+        0.0, 0.0, 0.0, 0.0, 0.0, \
+        0.0, 0.0, 0.0, 0.0, 0.0, \
+        0.0, 0.0, \
+        0.0, 0.0, \
+        0.0, 0.0, 0.0, \
+        0.0, 0.0, 0.0, 0.0, 0.0, \
+        0.0, 0.0, 0.0, \
+        0.0, \
+        0.0, \
+        0.0, 0.0, \
+        0.0, 0.0, \
+        0.0, 0.0, 0.0, \
+        0.0, 0.0, 0.0],
+        [6.0, 3.0, 5.0, 3.0, \
+        3.0, 20/15, 15/10, 3.0, \
+        3.0, 3.0, 3.0, 3.0, 3.0, \
+        3.0, 3.0, 3.0, 3.0, 3.0, \
+        3.0, 3.0, \
+        3.0, 3.0, \
+        3.0, 3.0, 3.0, \
+        3.0, 3.0, 3.0, 3.0, 3.0, \
+        3.0, 3.0, 3.0, \
+        3.0, \
+        3.0, \
+        3.0, 3.0, \
+        2.0, 3.0, \
+        3.0, 3.0, 3.0, \
+        3.0, 3.0, 3.0])
+
+    m_map = dict(zip(m_keys, range(len(m_keys))))
+    s_b_map = dict(zip(s_b_keys, range(len(s_b_keys))))
+    s_l_map = dict(zip(s_l_keys, range(len(s_l_keys))))
+    cs_map = dict(zip(cs_keys, range(len(cs_keys))))
+    cp_map = dict(zip(cp_keys, range(len(cp_keys))))
+
+# -----------------------------------------------------------------------------------------------------------------
+    def __init__(self, TIMESTEP, control_mode=1, control_dimension=3, params=np.ones(len(cp_keys))):
+        if self.DEBUG:
+            print("===========================================")
+            print("locomotion controller created in DEBUG mode")
+            print("===========================================")
+
+        self.control_mode = control_mode
+        # 0: spinal control (no brain control)
+        # 1: full control
+        self.control_dimension = control_dimension # 2D or 3D
+
+        if self.control_mode == 0:
+            self.brain_control_on = 0
+        elif self.control_mode == 1:
+            self.brain_control_on = 1
+
+        self.spinal_control_phase = {}
+        self.in_contact = {}
+        self.brain_command = {}
+        self.stim = {}
+
+        self.n_par = len(LocoCtrl.cp_keys)
+        if self.control_dimension == 2:
+            self.n_par = 37
+            self.par_space = (self.par_space[0][0:37], self.par_space[1][0:37])
+        self.cp = {}
+
+        self.reset(params)
+
+# -----------------------------------------------------------------------------------------------------------------
+    def reset(self, params=None):
+        self.in_contact['r_leg'] = 1
+        self.in_contact['l_leg'] = 0
+
+        spinal_control_phase_r = {}
+        spinal_control_phase_r['ph_st'] = 1
+        spinal_control_phase_r['ph_st_csw'] = 0
+        spinal_control_phase_r['ph_st_sw0'] = 0
+        spinal_control_phase_r['ph_st_st'] = 0
+        spinal_control_phase_r['ph_sw'] = 0
+        spinal_control_phase_r['ph_sw_flex_k'] = 0
+        spinal_control_phase_r['ph_sw_hold_k'] = 0
+        spinal_control_phase_r['ph_sw_stop_l'] = 0
+        spinal_control_phase_r['ph_sw_hold_l'] = 0
+        self.spinal_control_phase['r_leg'] = spinal_control_phase_r
+
+        spinal_control_phase_l = {}
+        spinal_control_phase_l['ph_st'] = 0
+        spinal_control_phase_l['ph_st_csw'] = 0
+        spinal_control_phase_l['ph_st_sw0'] = 0
+        spinal_control_phase_l['ph_st_st'] = 0
+        spinal_control_phase_l['ph_sw'] = 1
+        spinal_control_phase_l['ph_sw_flex_k'] = 1
+        spinal_control_phase_l['ph_sw_hold_k'] = 0
+        spinal_control_phase_l['ph_sw_stop_l'] = 0
+        spinal_control_phase_l['ph_sw_hold_l'] = 0
+        self.spinal_control_phase['l_leg'] = spinal_control_phase_l
+
+        self.stim['r_leg'] = dict(zip(self.m_keys, 0.01*np.ones(len(self.m_keys))))
+        self.stim['l_leg'] = dict(zip(self.m_keys, 0.01*np.ones(len(self.m_keys))))
+
+        if params is not None:
+            self.set_control_params(params)
+
+# -----------------------------------------------------------------------------------------------------------------
+    def set_control_params(self, params):
+        if len(params) == self.n_par:
+            self.set_control_params_RL('r_leg', params)
+            self.set_control_params_RL('l_leg', params)
+        elif len(params) == 2*self.n_par:
+            self.set_control_params_RL('r_leg', params[:self.n_par])
+            self.set_control_params_RL('l_leg', params[self.n_par:])
+        else:
+            raise Exception('error in the number of params!!')
+
+# -----------------------------------------------------------------------------------------------------------------
+    def set_control_params_RL(self, s_leg, params):
+        cp = {}
+        cp_map = self.cp_map
+
+        cp['theta_tgt'] = params[cp_map['theta_tgt']] *10*np.pi/180
+        cp['c0'] = params[cp_map['c0']] *20*np.pi/180 +55*np.pi/180
+        cp['cv'] = params[cp_map['cv']] *2*np.pi/180
+        cp['alpha_delta'] = params[cp_map['alpha_delta']] *5*np.pi/180
+        cp['knee_sw_tgt'] = params[cp_map['knee_sw_tgt']] *20*np.pi/180 +120*np.pi/180
+        cp['knee_tgt'] = params[cp_map['knee_tgt']] *15*np.pi/180 +160*np.pi/180
+        cp['knee_off_st'] = params[cp_map['knee_off_st']] *10*np.pi/180 +165*np.pi/180
+        cp['ankle_tgt'] = params[cp_map['ankle_tgt']] *20*np.pi/180 +60*np.pi/180
+
+        cp['HFL_3_PG'] = params[cp_map['HFL_3_PG']] *2.0
+        cp['HFL_3_DG'] = params[cp_map['HFL_3_DG']] *1.0
+        cp['HFL_6_PG'] = params[cp_map['HFL_6_PG']] *1.0
+        cp['HFL_6_DG'] = params[cp_map['HFL_6_DG']] *.1
+        cp['HFL_10_PG'] = params[cp_map['HFL_10_PG']] *1.0
+
+        cp['GLU_3_PG'] = params[cp_map['GLU_3_PG']] *2.0
+        cp['GLU_3_DG'] = params[cp_map['GLU_3_DG']] *0.5
+        cp['GLU_6_PG'] = params[cp_map['GLU_6_PG']] *1.0
+        cp['GLU_6_DG'] = params[cp_map['GLU_6_DG']] *0.1
+        cp['GLU_10_PG'] = params[cp_map['GLU_10_PG']] *.5
+
+        cp['HAM_3_GLU'] = params[cp_map['HAM_3_GLU']] *1.0
+        cp['HAM_9_PG'] = params[cp_map['HAM_9_PG']] *2.0
+
+        cp['RF_1_FG'] = params[cp_map['RF_1_FG']] *0.3
+        cp['RF_8_DG_knee'] = params[cp_map['RF_8_DG_knee']] *0.1
+
+        cp['VAS_1_FG'] = params[cp_map['VAS_1_FG']] *1.0
+        cp['VAS_2_PG'] = params[cp_map['VAS_2_PG']] *2.0
+        cp['VAS_10_PG'] = params[cp_map['VAS_10_PG']] *.3
+
+        cp['BFSH_2_PG'] = params[cp_map['BFSH_2_PG']] *2.0
+        cp['BFSH_7_DG_alpha'] = params[cp_map['BFSH_7_DG_alpha']] *0.2
+        cp['BFSH_7_PG'] = params[cp_map['BFSH_7_PG']] *2.0
+        cp['BFSH_8_DG'] = params[cp_map['BFSH_8_DG']] *1.0
+        cp['BFSH_8_PG'] = params[cp_map['BFSH_8_DG']] *1.0
+        cp['BFSH_9_G_HAM'] = params[cp_map['BFSH_9_G_HAM']] *2.0
+        cp['BFSH_9_HAM0'] = params[cp_map['BFSH_9_HAM0']] *0.3
+        cp['BFSH_10_PG'] = params[cp_map['BFSH_10_PG']] *2.0
+
+        cp['GAS_2_FG'] = params[cp_map['GAS_2_FG']] *1.2
+
+        cp['SOL_1_FG'] = params[cp_map['SOL_1_FG']] *1.2
+
+        cp['TA_5_PG'] = params[cp_map['TA_5_PG']] *2.0
+        cp['TA_5_G_SOL'] = params[cp_map['TA_5_G_SOL']] *0.5
+
+        if self.control_dimension == 3:
+            if len(params) != 46:
+                raise Exception('error in the number of params!!')
+            cp['theta_tgt_f'] = params[cp_map['theta_tgt_f']] *5.0*np.pi/180
+            cp['c0_f'] = params[cp_map['c0_f']] *20*np.pi/180 + 60*np.pi/180
+            cp['cv_f'] = params[cp_map['cv_f']] *10*np.pi/180
+            cp['HAB_3_PG'] = params[cp_map['HAB_3_PG']] *10.0
+            cp['HAB_3_DG'] = params[cp_map['HAB_3_DG']] *1
+            cp['HAB_6_PG'] = params[cp_map['HAB_6_PG']] *2.0
+            cp['HAD_3_PG'] = params[cp_map['HAD_3_PG']] *2.0
+            cp['HAD_3_DG'] = params[cp_map['HAD_3_DG']] *0.3
+            cp['HAD_6_PG'] = params[cp_map['HAD_6_PG']] *2.0
+        elif self.control_dimension == 2:
+            if len(params) != 37:
+                raise Exception('error in the number of params!!')
+
+        self.cp[s_leg] = cp
+
+# -----------------------------------------------------------------------------------------------------------------
+    def update(self, sensor_data):
+        self.sensor_data = sensor_data
+
+        if self.brain_control_on:
+            # update self.brain_command
+            self._brain_control(sensor_data)
+        
+        # updates self.stim
+        self._spinal_control(sensor_data)
+
+        stim = np.array([self.stim['r_leg']['HFL'], self.stim['r_leg']['GLU'],
+            self.stim['r_leg']['HAM'], self.stim['r_leg']['RF'],
+            self.stim['r_leg']['VAS'], self.stim['r_leg']['BFSH'],
+            self.stim['r_leg']['GAS'], self.stim['r_leg']['SOL'],
+            self.stim['r_leg']['TA'],
+            self.stim['l_leg']['HFL'], self.stim['l_leg']['GLU'],
+            self.stim['l_leg']['HAM'], self.stim['l_leg']['RF'],
+            self.stim['l_leg']['VAS'], self.stim['l_leg']['BFSH'],
+            self.stim['l_leg']['GAS'], self.stim['l_leg']['SOL'],
+            self.stim['l_leg']['TA']
+            ])
+        # todo: self._flaten(self.stim)
+        return stim
+
+# -----------------------------------------------------------------------------------------------------------------
+    def _brain_control(self, sensor_data=0):
+        s_b = sensor_data['body']
+        cp = self.cp
+
+        self.brain_command['r_leg'] = {}
+        self.brain_command['l_leg'] = {}
+        for s_leg in ['r_leg', 'l_leg']:
+            if self.control_dimension == 3:
+                self.brain_command[s_leg]['theta_tgt_f'] = cp[s_leg]['theta_tgt_f']
+                sign_frontral = 1 if s_leg is 'r_leg' else -1
+                alpha_tgt_global_frontal = cp[s_leg]['c0_f'] + sign_frontral*cp[s_leg]['cv_f']*s_b['d_pos'][1]
+                theta_f = sign_frontral*s_b['theta'][0]
+                self.brain_command[s_leg]['alpha_tgt_f'] = alpha_tgt_global_frontal - theta_f
+
+            self.brain_command[s_leg]['theta_tgt'] = cp[s_leg]['theta_tgt']
+
+            alpha_tgt_global = cp[s_leg]['c0'] - cp[s_leg]['cv']*s_b['d_pos'][0]
+            self.brain_command[s_leg]['alpha_tgt'] = alpha_tgt_global - s_b['theta'][1]
+            self.brain_command[s_leg]['alpha_delta'] = cp[s_leg]['alpha_delta']
+            self.brain_command[s_leg]['knee_sw_tgt'] = cp[s_leg]['knee_sw_tgt']
+            self.brain_command[s_leg]['knee_tgt'] = cp[s_leg]['knee_tgt']
+            self.brain_command[s_leg]['knee_off_st'] = cp[s_leg]['knee_off_st']
+            self.brain_command[s_leg]['ankle_tgt'] = cp[s_leg]['ankle_tgt']
+            # alpha = hip - 0.5*knee
+            self.brain_command[s_leg]['hip_tgt'] = \
+                self.brain_command[s_leg]['alpha_tgt'] + 0.5*self.brain_command[s_leg]['knee_tgt']
+
+        # select which leg to swing
+        self.brain_command['r_leg']['swing_init'] = 0
+        self.brain_command['l_leg']['swing_init'] = 0
+        if sensor_data['r_leg']['contact_ipsi'] and sensor_data['l_leg']['contact_ipsi']:
+            r_delta_alpha = sensor_data['r_leg']['alpha'] - self.brain_command['r_leg']['alpha_tgt']
+            l_delta_alpha = sensor_data['l_leg']['alpha'] - self.brain_command['l_leg']['alpha_tgt']
+            if r_delta_alpha > l_delta_alpha:
+                self.brain_command['r_leg']['swing_init'] = 1
+            else:
+                self.brain_command['l_leg']['swing_init'] = 1
+    
+# -----------------------------------------------------------------------------------------------------------------
+    def _spinal_control(self, sensor_data):
+        for s_leg in ['r_leg', 'l_leg']:
+            self._update_spinal_control_phase(s_leg, sensor_data)
+            self.stim[s_leg] = self.spinal_control_leg(s_leg, sensor_data)
+
+# -----------------------------------------------------------------------------------------------------------------
+    def _update_spinal_control_phase(self, s_leg, sensor_data):
+        s_l = sensor_data[s_leg]
+
+        alpha_tgt = self.brain_command[s_leg]['alpha_tgt']
+        alpha_delta = self.brain_command[s_leg]['alpha_delta']
+        knee_sw_tgt = self.brain_command[s_leg]['knee_sw_tgt']
+
+        # when foot touches ground
+        if not self.in_contact[s_leg] and s_l['contact_ipsi']:
+            # initiate stance control
+            self.spinal_control_phase[s_leg]['ph_st'] = 1
+            # swing control off
+            self.spinal_control_phase[s_leg]['ph_sw'] = 0
+            self.spinal_control_phase[s_leg]['ph_sw_flex_k'] = 0
+            self.spinal_control_phase[s_leg]['ph_sw_hold_k'] = 0
+            self.spinal_control_phase[s_leg]['ph_sw_stop_l'] = 0
+            self.spinal_control_phase[s_leg]['ph_sw_hold_l'] = 0
+
+        # during stance control
+        if self.spinal_control_phase[s_leg]['ph_st']:
+            # contra-leg in swing (single stance phase)
+            self.spinal_control_phase[s_leg]['ph_st_csw'] = not s_l['contact_contra']
+            # initiate swing
+            self.spinal_control_phase[s_leg]['ph_st_sw0'] = self.brain_command[s_leg]['swing_init']
+            # do not initiate swing
+            self.spinal_control_phase[s_leg]['ph_st_st'] = not self.spinal_control_phase[s_leg]['ph_st_sw0']
+
+        # when foot loses contact
+        if self.in_contact[s_leg] and not s_l['contact_ipsi']:
+            # stance control off
+            self.spinal_control_phase[s_leg]['ph_st'] = 0
+            self.spinal_control_phase[s_leg]['ph_st_csw'] = 0
+            self.spinal_control_phase[s_leg]['ph_st_sw0'] = 0
+            self.spinal_control_phase[s_leg]['ph_st_st'] = 0
+            # initiate swing control
+            self.spinal_control_phase[s_leg]['ph_sw'] = 1
+            # flex knee
+            self.spinal_control_phase[s_leg]['ph_sw_flex_k'] = 1
+
+        # during swing control
+        if self.spinal_control_phase[s_leg]['ph_sw']:
+            if self.spinal_control_phase[s_leg]['ph_sw_flex_k']:
+                if s_l['phi_knee'] < knee_sw_tgt: # knee flexed
+                    self.spinal_control_phase[s_leg]['ph_sw_flex_k'] = 0
+                    # hold knee
+                    self.spinal_control_phase[s_leg]['ph_sw_hold_k'] = 1
+            else:
+                if self.spinal_control_phase[s_leg]['ph_sw_hold_k']:
+                    if s_l['alpha'] < alpha_tgt: # leg swung enough
+                        self.spinal_control_phase[s_leg]['ph_sw_hold_k'] = 0
+                if s_l['alpha'] < alpha_tgt + alpha_delta: # leg swung enough
+                    # stop leg
+                    self.spinal_control_phase[s_leg]['ph_sw_stop_l'] = 1
+                if self.spinal_control_phase[s_leg]['ph_sw_stop_l'] \
+                    and s_l['dalpha'] > 0: # leg started to retract
+                    # hold leg
+                    self.spinal_control_phase[s_leg]['ph_sw_hold_l'] = 1
+
+        self.in_contact[s_leg] = s_l['contact_ipsi']
+
+# -----------------------------------------------------------------------------------------------------------------
+    def spinal_control_leg(self, s_leg, sensor_data):
+        s_l = sensor_data[s_leg]
+        s_b = sensor_data['body']
+        cp = self.cp[s_leg]
+
+        ph_st = self.spinal_control_phase[s_leg]['ph_st']
+        ph_st_csw = self.spinal_control_phase[s_leg]['ph_st_csw']
+        ph_st_sw0 = self.spinal_control_phase[s_leg]['ph_st_sw0']
+        ph_st_st = self.spinal_control_phase[s_leg]['ph_st_st']
+        ph_sw = self.spinal_control_phase[s_leg]['ph_sw']
+        ph_sw_flex_k = self.spinal_control_phase[s_leg]['ph_sw_flex_k']
+        ph_sw_hold_k = self.spinal_control_phase[s_leg]['ph_sw_hold_k']
+        ph_sw_stop_l = self.spinal_control_phase[s_leg]['ph_sw_stop_l']
+        ph_sw_hold_l = self.spinal_control_phase[s_leg]['ph_sw_hold_l']
+
+        theta = s_b['theta'][1]
+        dtheta = s_b['dtheta'][1]
+
+        sign_frontral = 1 if s_leg is 'r_leg' else -1
+        theta_f = sign_frontral*s_b['theta'][0]
+        dtheta_f = sign_frontral*s_b['dtheta'][0]
+
+        theta_tgt = self.brain_command[s_leg]['theta_tgt']
+        alpha_tgt = self.brain_command[s_leg]['alpha_tgt']
+        alpha_delta = self.brain_command[s_leg]['alpha_delta']
+        hip_tgt = self.brain_command[s_leg]['hip_tgt']
+        knee_tgt = self.brain_command[s_leg]['knee_tgt']
+        knee_sw_tgt = self.brain_command[s_leg]['knee_sw_tgt']
+        knee_off_st = self.brain_command[s_leg]['knee_off_st']
+        ankle_tgt = self.brain_command[s_leg]['ankle_tgt']
+
+        stim = {}
+        pre_stim = 0.01
+
+        if self.control_dimension == 3:
+            theta_tgt_f = self.brain_command[s_leg]['theta_tgt_f']
+            alpha_tgt_f = self.brain_command[s_leg]['alpha_tgt_f']
+
+            S_HAB_3 = ph_st*s_l['load_ipsi']*np.maximum(
+                - cp['HAB_3_PG']*(theta_f-theta_tgt_f)
+                - cp['HAB_3_DG']*dtheta_f
+                , 0)
+            S_HAB_6 = (ph_st_sw0*s_l['load_contra'] + ph_sw)*np.maximum(
+                cp['HAB_6_PG']*(s_l['alpha_f'] - alpha_tgt_f)
+                , 0)
+            stim['HAB'] = S_HAB_3 + S_HAB_6
+
+            S_HAD_3 = ph_st*s_l['load_ipsi']*np.maximum(
+                cp['HAD_3_PG']*(theta_f-theta_tgt_f)
+                + cp['HAD_3_DG']*dtheta_f
+                , 0)
+            S_HAD_6 = (ph_st_sw0*s_l['load_contra'] + ph_sw)*np.maximum(
+                - cp['HAD_6_PG']*(s_l['alpha_f'] - alpha_tgt_f)
+                , 0)
+            stim['HAD'] = S_HAD_3 + S_HAD_6
+
+        S_HFL_3 = ph_st*s_l['load_ipsi']*np.maximum(
+            - cp['HFL_3_PG']*(theta-theta_tgt)
+            - cp['HFL_3_DG']*dtheta
+            , 0)
+        S_HFL_6 = (ph_st_sw0*s_l['load_contra'] + ph_sw)*np.maximum(
+            cp['HFL_6_PG']*(s_l['alpha']-alpha_tgt)
+            + cp['HFL_6_DG']*s_l['dalpha']
+            , 0)
+        S_HFL_10 = ph_sw_hold_l*np.maximum(
+            cp['HFL_10_PG']*(s_l['phi_hip'] - hip_tgt)
+            , 0)
+        stim['HFL'] = pre_stim + S_HFL_3 + S_HFL_6 + S_HFL_10
+
+        S_GLU_3 = ph_st*s_l['load_ipsi']*np.maximum(
+            cp['GLU_3_PG']*(theta-theta_tgt)
+            + cp['GLU_3_DG']*dtheta
+            , 0)
+        S_GLU_6 = (ph_st_sw0*s_l['load_contra'] + ph_sw)*np.maximum(
+            - cp['GLU_6_PG']*(s_l['alpha']-alpha_tgt)
+            - cp['GLU_6_DG']*s_l['dalpha']
+            , 0)
+        S_GLU_10 = ph_sw_hold_l*np.maximum(
+            - cp['GLU_10_PG']*(s_l['phi_hip'] - hip_tgt)
+            , 0)
+        stim['GLU'] = pre_stim + S_GLU_3 + S_GLU_6 + S_GLU_10
+
+        S_HAM_3 = cp['HAM_3_GLU']*S_GLU_3
+        S_HAM_9 = ph_sw_stop_l*np.maximum(
+            - cp['HAM_9_PG']*(s_l['alpha'] - (alpha_tgt + alpha_delta))
+            , 0)
+        stim['HAM'] = pre_stim + S_HAM_3 + S_HAM_9
+
+        S_RF_1 = (ph_st_st + ph_st_sw0*(1-s_l['load_contra']))*np.maximum(
+            cp['RF_1_FG']*s_l['F_RF']
+            , 0)
+        S_RF_8 = ph_sw_hold_k*np.maximum(
+            - cp['RF_8_DG_knee']*s_l['dphi_knee']
+            , 0)
+        stim['RF'] = pre_stim + S_RF_1 + S_RF_8
+
+        S_VAS_1 = (ph_st_st + ph_st_sw0*(1-s_l['load_contra']))*np.maximum(
+            cp['VAS_1_FG']*s_l['F_VAS']
+            , 0)
+        S_VAS_2 = -(ph_st_st + ph_st_sw0*(1-s_l['load_contra']))*np.maximum(
+            cp['VAS_2_PG']*(s_l['phi_knee'] - knee_off_st)
+            , 0)
+        S_VAS_10 = ph_sw_hold_l*np.maximum(
+            - cp['VAS_10_PG']*(s_l['phi_knee'] - knee_tgt)
+            , 0)
+        stim['VAS'] = pre_stim + S_VAS_1 + S_VAS_2 + S_VAS_10
+
+        S_BFSH_2 = (ph_st_st + ph_st_sw0*(1-s_l['load_contra']))*np.maximum(
+            cp['BFSH_2_PG']*(s_l['phi_knee'] - knee_off_st)
+            , 0)
+        S_BFSH_7 = (ph_st_sw0*(s_l['load_contra']) + ph_sw_flex_k)*np.maximum(
+            - cp['BFSH_7_DG_alpha']*s_l['dalpha']
+            + cp['BFSH_7_PG']*(s_l['phi_knee'] - knee_sw_tgt)
+            , 0)
+        S_BFSH_8 = ph_sw_hold_k*np.maximum(
+            cp['BFSH_8_DG']*(s_l['dphi_knee'] - knee_off_st)
+            *cp['BFSH_8_PG']*(s_l['alpha'] - alpha_tgt)
+            , 0)
+        S_BFSH_9 = np.maximum(
+            cp['BFSH_9_G_HAM']*(S_HAM_9 - cp['BFSH_9_HAM0'])
+            , 0)
+        S_BFSH_10 = ph_sw_hold_l*np.maximum(
+            cp['BFSH_10_PG']*(s_l['phi_knee'] - knee_tgt)
+            , 0)
+        stim['BFSH'] = pre_stim + S_BFSH_2 + S_BFSH_7 + S_BFSH_8 + S_BFSH_9 + S_BFSH_10
+
+        S_GAS_2 = ph_st*np.maximum(
+            cp['GAS_2_FG']*s_l['F_GAS']
+            , 0)
+        stim['GAS'] = pre_stim + S_GAS_2
+        S_SOL_1 = ph_st*np.maximum(
+            cp['SOL_1_FG']*s_l['F_SOL']
+            , 0)
+        stim['SOL'] = pre_stim + S_SOL_1
+
+        S_TA_5 = np.maximum(
+            cp['TA_5_PG']*(s_l['phi_ankle'] - ankle_tgt)
+            , 0)
+        S_TA_5_st = -ph_st*np.maximum(
+            cp['TA_5_G_SOL']*S_SOL_1
+            , 0)
+        stim['TA'] = pre_stim + S_TA_5 + S_TA_5_st
+
+        for muscle in stim:
+            stim[muscle] = np.clip(stim[muscle], 0.01, 1.0)
+
+        return stim
\ No newline at end of file