diff --git a/go1_gym_deploy/scripts/deploy_policy.py b/go1_gym_deploy/scripts/deploy_policy.py index ba137af..3c203d5 100644 --- a/go1_gym_deploy/scripts/deploy_policy.py +++ b/go1_gym_deploy/scripts/deploy_policy.py @@ -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) diff --git a/go1_gym_deploy/utils/cheetah_state_estimator.py b/go1_gym_deploy/utils/cheetah_state_estimator.py index 169262e..44c83d5 100755 --- a/go1_gym_deploy/utils/cheetah_state_estimator.py +++ b/go1_gym_deploy/utils/cheetah_state_estimator.py @@ -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, diff --git a/go1_gym_deploy/utils/deployment_runner.py b/go1_gym_deploy/utils/deployment_runner.py index f23cba0..e985f92 100755 --- a/go1_gym_deploy/utils/deployment_runner.py +++ b/go1_gym_deploy/utils/deployment_runner.py @@ -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]: