AI/WalkTheseWays
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix bug in right upper switch behavior; now aligned with diagram in README

Browse Source
This commit is contained in:
gmargo
2022-12-02 12:45:51 -05:00
parent bdb03f65df
commit 0adb741d01
3 changed files with 43 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files
go1_gym_deploy/scripts/deploy_policy.py
+3 -16
View File
@@ -12,7 +12,7 @@ import pathlib
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]
@@ -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()
@@ -37,15 +37,6 @@ 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)
@@ -53,8 +44,6 @@ def load_and_run_policy(label, experiment_name, probe_policy_label=None, max_vel
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:
@@ -83,8 +72,6 @@ def load_policy(logdir):
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)
go1_gym_deploy/utils/cheetah_state_estimator.py
+37 -82
View File
@@ -80,7 +80,8 @@ class StateEstimator:
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
@@ -145,74 +146,50 @@ class StateEstimator:
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")
if self.right_upper_switch_pressed:
self.ctrlmode_right = (self.ctrlmode_right + 1) % 3
self.right_upper_switch_pressed = False
MODE = MODES[self.ctrlmode]
MODE_LEFT = MODES_LEFT[self.ctrlmode_left]
MODE_RIGHT = MODES_RIGHT[self.ctrlmode_right]
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]
# 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
@@ -233,33 +210,11 @@ class StateEstimator:
self.cmd_offset = 0.0
self.cmd_bound = 0.5
self.cmd_duration = 0.5
elif self.mode == 4:
self.cmd_phase = 0.7
else:
self.cmd_phase = 0.5
self.cmd_offset = 0.0
self.cmd_bound = 0.0
self.cmd_duration = 0.5
elif self.mode == 5:
self.cmd_phase = 0.3
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,
go1_gym_deploy/utils/deployment_runner.py
+3 -10
View File
@@ -58,10 +58,6 @@ class DeploymentRunner:
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
@@ -146,10 +142,7 @@ class DeploymentRunner:
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)
@@ -172,7 +165,7 @@ class DeploymentRunner:
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
@@ -190,7 +183,7 @@ class DeploymentRunner:
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]: