Skip to content

Commit

Permalink
fix bug in right upper switch behavior; now aligned with diagram in R…
Browse files Browse the repository at this point in the history 
…EADME
  • Loading branch information
@gmargo11
gmargo11 committed Dec 2, 2022
1 parent bdb03f6 commit 0adb741
Show file tree
Hide file tree
Showing 3 changed files with 45 additions and 110 deletions.
19 changes: 3 additions & 16 deletions go1_gym_deploy/scripts/deploy_policy.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@

lc = lcm.LCM("udpm://239.255.76.67:7667?ttl=255")

def load_and_run_policy(label, experiment_name, probe_policy_label=None, max_vel=1.0, max_yaw_vel=1.0, max_vel_probe=1.0):
def load_and_run_policy(label, experiment_name, max_vel=1.0, max_yaw_vel=1.0):
# load agent
dirs = glob.glob(f"../../runs/{label}/*")
logdir = sorted(dirs)[0]
Expand All @@ -27,7 +27,7 @@ def load_and_run_policy(label, experiment_name, probe_policy_label=None, max_vel
se = StateEstimator(lc)

control_dt = 0.02
command_profile = RCControllerProfile(dt=control_dt, state_estimator=se, x_scale=max_vel, y_scale=0.6, yaw_scale=max_yaw_vel, probe_vel_multiplier=(max_vel_probe / max_vel))
command_profile = RCControllerProfile(dt=control_dt, state_estimator=se, x_scale=max_vel, y_scale=0.6, yaw_scale=max_yaw_vel)

hardware_agent = LCMAgent(cfg, se, command_profile)
se.spin()
Expand All @@ -37,24 +37,13 @@ def load_and_run_policy(label, experiment_name, probe_policy_label=None, max_vel

policy = load_policy(logdir)

if probe_policy_label is not None:
# load agent
dirs = glob.glob(f"../runs/{probe_policy_label}_*")
probe_policy_logdir = sorted(dirs)[0]
with open(probe_policy_logdir + "/parameters.pkl", 'rb') as file:
probe_cfg = pkl.load(file)
probe_cfg = probe_cfg["Cfg"]
probe_policy = load_policy(probe_policy_logdir)

# load runner
root = f"{pathlib.Path(__file__).parent.resolve()}/../../logs/"
pathlib.Path(root).mkdir(parents=True, exist_ok=True)
deployment_runner = DeploymentRunner(experiment_name=experiment_name, se=None,
log_root=f"{root}/{experiment_name}")
deployment_runner.add_control_agent(hardware_agent, "hardware_closed_loop")
deployment_runner.add_policy(policy)
if probe_policy_label is not None:
deployment_runner.add_probe_policy(probe_policy, probe_cfg)
deployment_runner.add_command_profile(command_profile)

if len(sys.argv) >= 2:
Expand Down Expand Up @@ -83,8 +72,6 @@ def policy(obs, info):
if __name__ == '__main__':
label = "gait-conditioned-agility/pretrain-v0/train"

probe_policy_label = None

experiment_name = "example_experiment"

load_and_run_policy(label, experiment_name=experiment_name, probe_policy_label=probe_policy_label, max_vel=3.0, max_yaw_vel=5.0, max_vel_probe=1.0)
load_and_run_policy(label, experiment_name=experiment_name, max_vel=3.5, max_yaw_vel=5.0)
123 changes: 39 additions & 84 deletions go1_gym_deploy/utils/cheetah_state_estimator.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,7 +80,8 @@ def __init__(self, lc, use_cameras=True):
self.contact_state = np.ones(4)

self.mode = 0
self.ctrlmode = 0
self.ctrlmode_left = 0
self.ctrlmode_right = 0
self.left_stick = [0, 0]
self.right_stick = [0, 0]
self.left_upper_switch = 0
Expand Down Expand Up @@ -145,74 +146,50 @@ def get_rpy(self):
return self.euler

def get_command(self):
MODES = ["mobility", "body_control", "footswing"]
MODES_LEFT = ["body_height", "lat_vel", "stance_width"]
MODES_RIGHT = ["step_frequency", "footswing_height", "body_pitch"]

if self.left_upper_switch_pressed:
self.ctrlmode = (self.ctrlmode + 1) % 3
self.ctrlmode_left = (self.ctrlmode_left + 1) % 3
self.left_upper_switch_pressed = False
print(f"PRESSED{self.ctrlmode}")
# elif self.left_lower_left_switch_pressed:
# self.ctrlmode = 0
# self.left_lower_left_switch_pressed = False
# print("PRESSED0")

MODE = MODES[self.ctrlmode]

if MODE == "mobility":
cmd_x = 1 * self.left_stick[1]
cmd_y = 0 # -1 * self.left_stick[0]
cmd_yaw = -1 * self.right_stick[0]
# cmd_height = 0.3 * self.right_stick[1]
if self.right_upper_switch_pressed:
self.ctrlmode_right = (self.ctrlmode_right + 1) % 3
self.right_upper_switch_pressed = False

MODE_LEFT = MODES_LEFT[self.ctrlmode_left]
MODE_RIGHT = MODES_RIGHT[self.ctrlmode_right]

# always in use
cmd_x = 1 * self.left_stick[1]
cmd_yaw = -1 * self.right_stick[0]

# default values
cmd_y = 0. # -1 * self.left_stick[0]
cmd_height = 0.
cmd_footswing = 0.08
cmd_stance_width = 0.33
cmd_stance_length = 0.40
cmd_ori_pitch = 0.
cmd_ori_roll = 0.
cmd_freq = 3.0

# joystick commands
if MODE_LEFT == "body_height":
cmd_height = 0.3 * self.left_stick[0]
cmd_footswing = 0.08 # max(0, self.left_stick[0]) * 0.27 + 0.03

cmd_stance_width = 0.33
cmd_stance_length = 0.45
cmd_ori_pitch = 0.15
cmd_ori_roll = 0

min_freq = 2.0
max_freq = 4.0
cmd_freq = (1 + self.right_stick[1]) / 2 * (max_freq - min_freq) + min_freq
elif MODE == "body_control":
cmd_x = 1 * self.left_stick[1]
cmd_y = 0 # -1 * self.left_stick[0]
cmd_yaw = -1 * self.right_stick[0]
cmd_height = 0.0
cmd_footswing = 0.10 # max(0, self.left_stick[0]) * 0.27 + 0.03

elif MODE_LEFT == "lat_vel":
cmd_y = 0.6 * self.left_stick[0]
elif MODE_LEFT == "stance_width":
cmd_stance_width = 0.275 + 0.175 * self.left_stick[0]
cmd_stance_length = 0.45 # 0.45 + 0.25 * self.left_stick[1]
cmd_ori_pitch = -0.4 * self.right_stick[1]
cmd_ori_roll = 0.0 # 0.5 * self.right_stick[1]
cmd_freq = 3.2
else:
cmd_x = 1 * self.left_stick[1]
cmd_y = 0 # -1 * self.left_stick[0]
cmd_yaw = -1 * self.right_stick[0]
cmd_height = 0.0
cmd_footswing = max(0, self.left_stick[0]) * 0.32 + 0.03
# cmd_footswing = max(0, self.left_stick[0]) * 0.29 + 0.03

cmd_stance_width = 0.3 # + 0.175 * self.left_stick[0]
cmd_stance_length = 0.40 # + 0.15 * self.left_stick[1]
cmd_ori_pitch = 0.0 # -0.3 * self.left_stick[0]
cmd_ori_roll = 0.0 # 0.5 * self.right_stick[1]

if MODE_RIGHT == "step_frequency":
min_freq = 2.0
max_freq = 4.0
cmd_freq = (1 + self.right_stick[1]) / 2 * (max_freq - min_freq) + min_freq
elif MODE_RIGHT == "footswing_height":
cmd_footswing = max(0, self.right_stick[1]) * 0.32 + 0.03
elif MODE_RIGHT == "body_pitch":
cmd_ori_pitch = -0.4 * self.right_stick[1]

# cmd_freq = (1 + self.left_stick[0]) / 2 * (max_freq-min_freq) + min_freq

# if self.left_upper_switch_pressed:
# if self.cmd_phase == 0.5:
# self.cmd_phase = 0.0
# elif self.cmd_phase == 0.0:
# self.cmd_phase = 0.25
# else:
# self.cmd_phase = 0.5
# self.left_upper_switch_pressed = False
# gait buttons
if self.mode == 0:
self.cmd_phase = 0.5
self.cmd_offset = 0.0
Expand All @@ -233,33 +210,11 @@ def get_command(self):
self.cmd_offset = 0.0
self.cmd_bound = 0.5
self.cmd_duration = 0.5
elif self.mode == 4:
self.cmd_phase = 0.7
self.cmd_offset = 0.0
self.cmd_bound = 0.0
self.cmd_duration = 0.5
elif self.mode == 5:
self.cmd_phase = 0.3
else:
self.cmd_phase = 0.5
self.cmd_offset = 0.0
self.cmd_bound = 0.0
self.cmd_duration = 0.5
elif self.mode == 6:
self.cmd_phase = 0.0
self.cmd_offset = 0.7
self.cmd_bound = 0.0
self.cmd_duration = 0.5
elif self.mode == 7:
self.cmd_phase = 0.0
self.cmd_offset = 0.3
self.cmd_bound = 0.0
self.cmd_duration = 0.5

# cmd_freq = (3.0 if not self.left_upper_switch else 2.0)
# cmd_phase = (0.5 if not self.left_lower_left_switch else 0.0)
# cmd_offset = 0.0
# cmd_duration = 0.5

# print(cmd_x, cmd_y)

return np.array([cmd_x, cmd_y, cmd_yaw, cmd_height, cmd_freq, self.cmd_phase, self.cmd_offset, self.cmd_bound,
self.cmd_duration, cmd_footswing, cmd_ori_pitch, cmd_ori_roll, cmd_stance_width,
Expand Down
13 changes: 3 additions & 10 deletions go1_gym_deploy/utils/deployment_runner.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,10 +58,6 @@ def set_command_agents(self, name):
def add_policy(self, policy):
self.policy = policy

def add_probe_policy(self, probe_policy, probe_cfg):
self.probe_policy = probe_policy
self.probe_cfg = probe_cfg

def add_command_profile(self, command_profile):
self.command_profile = command_profile

Expand Down Expand Up @@ -146,10 +142,7 @@ def run(self, num_log_steps=1000000000, max_steps=100000000, logging=True):
for i in range(max_steps):

policy_info = {}
if self.is_currently_probing:
action = self.probe_policy(control_obs, policy_info)
else:
action = self.policy(control_obs, policy_info)
action = self.policy(control_obs, policy_info)

for agent_name in self.agents.keys():
obs, ret, done, info = self.agents[agent_name].step(action)
Expand All @@ -172,7 +165,7 @@ def run(self, num_log_steps=1000000000, max_steps=100000000, logging=True):
prev_button_states = self.button_states[:]
self.button_states = self.command_profile.get_buttons()

if self.command_profile.state_estimator.right_upper_switch_pressed:
if self.command_profile.state_estimator.left_lower_left_switch_pressed:
if not self.is_currently_probing:
print("START LOGGING")
self.is_currently_probing = True
Expand All @@ -190,7 +183,7 @@ def run(self, num_log_steps=1000000000, max_steps=100000000, logging=True):
self.logger.reset()
time.sleep(1)
control_obs = self.agents[self.control_agent_name].reset()
self.command_profile.state_estimator.right_upper_switch_pressed = False
self.command_profile.state_estimator.left_lower_left_switch_pressed = False

for button in range(4):
if self.command_profile.currently_triggered[button]:
Expand Down

0 comments on commit 0adb741

Please sign in to comment.