diff --git a/.vscode/settings.json b/.vscode/settings.json index 92cf2cb..dc3c735 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -8,7 +8,7 @@ "ros.distro": "humble", // We use "black" as a formatter: "python.formatting.provider": "black", - "editor.formatOnSave": true, + "editor.formatOnSave": false, "python.formatting.blackArgs": [ "--line-length", "120" diff --git a/src/controllers/config/rl_velocity_go2_cfg.yaml b/src/controllers/config/rl_velocity_go2_cfg.yaml index ddcd377..04b8f1f 100644 --- a/src/controllers/config/rl_velocity_go2_cfg.yaml +++ b/src/controllers/config/rl_velocity_go2_cfg.yaml @@ -1,16 +1,19 @@ # POLICY CONFIG -policy_path: "locomotion_go2/velocity_policy_unidirectional.pt" +policy_path: "locomotion_go2/oliver/flatAmpVision/2025-08-12_14-23-30_flat_DR5_improved_minimal_motion_files_SEED_3/policy.pt" -control_dt: 0.001 -decimation: 4 +control_dt: 0.002 +decimation: 1 action_dim: 12 -action_scale: 0.35 -stiffness: 25.0 -damping: 0.5 +action_scale: 0.25 +stiffness: 27.0 +damping: 1.5 # PROCESSING CONFIG device: "cuda:0" -use_threading: False +use_threading: True +use_buffer: True +obs_buffer_length: 5 + # MISC CONFIG diff --git a/src/controllers/config/rl_velocity_go2_torque_cfg.yaml b/src/controllers/config/rl_velocity_go2_torque_cfg.yaml new file mode 100644 index 0000000..79f692d --- /dev/null +++ b/src/controllers/config/rl_velocity_go2_torque_cfg.yaml @@ -0,0 +1,33 @@ +# POLICY CONFIG +policy_path: "torqueComplexReward/TORQUE_complex_reward_10_SEED_42/policy.pt" + +control_dt: 0.0001 +decimation: 1 +action_dim: 12 +action_scale: 10.0 +stiffness: 0.0 +damping: 0.0 + +# PROCESSING CONFIG +device: "cuda:0" +use_threading: False +use_buffer: True +obs_buffer_length: 5 + + + +# MISC CONFIG +ISAAC_LAB_DEFAULT_JOINT_POS: [ + 0.1000, # 0 + -0.1000, # 1 + 0.1000, # 2 + -0.1000, # 3 + 0.8000, # 4 + 0.8000, # 5 + 1.0000, # 6 + 1.0000, # 7 + -1.5000, # 8 + -1.5000, # 9 + -1.5000, # 10 + -1.5000, # 11 + ] # from asset.data.default_joint_pos; corresponds to ISAACLAB_JOINT_ORDER; is used as action offset \ No newline at end of file diff --git a/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-03_14-14-03_flat_test_DR4_SEED_1/policy.pt b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-03_14-14-03_flat_test_DR4_SEED_1/policy.pt new file mode 100644 index 0000000..e59230b Binary files /dev/null and b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-03_14-14-03_flat_test_DR4_SEED_1/policy.pt differ diff --git a/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-05_16-25-55_flat_test_DR5_better_slow_motion_file_SEED_1/policy.pt b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-05_16-25-55_flat_test_DR5_better_slow_motion_file_SEED_1/policy.pt new file mode 100644 index 0000000..06d7e9a Binary files /dev/null and b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-05_16-25-55_flat_test_DR5_better_slow_motion_file_SEED_1/policy.pt differ diff --git a/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-06_08-47-51_flat_test_DR5_improved_minimal_motion_files_SEED_1/policy.pt b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-06_08-47-51_flat_test_DR5_improved_minimal_motion_files_SEED_1/policy.pt new file mode 100644 index 0000000..8e1c286 Binary files /dev/null and b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-06_08-47-51_flat_test_DR5_improved_minimal_motion_files_SEED_1/policy.pt differ diff --git a/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-07_16-41-10_flat_test_DR5_improved_minimal_motion_files_SEED_1/policy.pt b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-07_16-41-10_flat_test_DR5_improved_minimal_motion_files_SEED_1/policy.pt new file mode 100644 index 0000000..b5f610f Binary files /dev/null and b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-07_16-41-10_flat_test_DR5_improved_minimal_motion_files_SEED_1/policy.pt differ diff --git a/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-08_09-24-48_flat_test_DR5_improved_minimal_motion_files_SEED_1/policy.pt b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-08_09-24-48_flat_test_DR5_improved_minimal_motion_files_SEED_1/policy.pt new file mode 100644 index 0000000..effde15 Binary files /dev/null and b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-08_09-24-48_flat_test_DR5_improved_minimal_motion_files_SEED_1/policy.pt differ diff --git a/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-09_21-28-55_flat_test_DR5_improved_minimal_motion_files_SEED_1/policy.pt b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-09_21-28-55_flat_test_DR5_improved_minimal_motion_files_SEED_1/policy.pt new file mode 100644 index 0000000..85c41d7 Binary files /dev/null and b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-09_21-28-55_flat_test_DR5_improved_minimal_motion_files_SEED_1/policy.pt differ diff --git a/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-12_14-23-30_flat_DR5_improved_minimal_motion_files_MinimalReward1_SEED_1/policy.pt b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-12_14-23-30_flat_DR5_improved_minimal_motion_files_MinimalReward1_SEED_1/policy.pt new file mode 100644 index 0000000..951d7f6 Binary files /dev/null and b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-12_14-23-30_flat_DR5_improved_minimal_motion_files_MinimalReward1_SEED_1/policy.pt differ diff --git a/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-12_14-23-30_flat_DR5_improved_minimal_motion_files_MinimalReward1_SEED_3/policy.pt b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-12_14-23-30_flat_DR5_improved_minimal_motion_files_MinimalReward1_SEED_3/policy.pt new file mode 100644 index 0000000..d1c6d30 Binary files /dev/null and b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-12_14-23-30_flat_DR5_improved_minimal_motion_files_MinimalReward1_SEED_3/policy.pt differ diff --git a/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-12_14-23-30_flat_DR5_improved_minimal_motion_files_MinimalReward2_SEED_1/policy.pt b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-12_14-23-30_flat_DR5_improved_minimal_motion_files_MinimalReward2_SEED_1/policy.pt new file mode 100644 index 0000000..1d6a577 Binary files /dev/null and b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-12_14-23-30_flat_DR5_improved_minimal_motion_files_MinimalReward2_SEED_1/policy.pt differ diff --git a/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-12_14-23-30_flat_DR5_improved_minimal_motion_files_MinimalReward2_SEED_2/policy.pt b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-12_14-23-30_flat_DR5_improved_minimal_motion_files_MinimalReward2_SEED_2/policy.pt new file mode 100644 index 0000000..be7f2f8 Binary files /dev/null and b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-12_14-23-30_flat_DR5_improved_minimal_motion_files_MinimalReward2_SEED_2/policy.pt differ diff --git a/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-12_14-23-30_flat_DR5_improved_minimal_motion_files_NoViconObs_SEED_2/policy.pt b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-12_14-23-30_flat_DR5_improved_minimal_motion_files_NoViconObs_SEED_2/policy.pt new file mode 100644 index 0000000..24f3ea0 Binary files /dev/null and b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-12_14-23-30_flat_DR5_improved_minimal_motion_files_NoViconObs_SEED_2/policy.pt differ diff --git a/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-12_14-23-30_flat_DR5_improved_minimal_motion_files_SEED_1/policy.pt b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-12_14-23-30_flat_DR5_improved_minimal_motion_files_SEED_1/policy.pt new file mode 100644 index 0000000..17d3636 Binary files /dev/null and b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-12_14-23-30_flat_DR5_improved_minimal_motion_files_SEED_1/policy.pt differ diff --git a/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-12_14-23-30_flat_DR5_improved_minimal_motion_files_SEED_2/policy.pt b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-12_14-23-30_flat_DR5_improved_minimal_motion_files_SEED_2/policy.pt new file mode 100644 index 0000000..24f3ea0 Binary files /dev/null and b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-12_14-23-30_flat_DR5_improved_minimal_motion_files_SEED_2/policy.pt differ diff --git a/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-12_14-23-30_flat_DR5_improved_minimal_motion_files_SEED_3/policy.pt b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-12_14-23-30_flat_DR5_improved_minimal_motion_files_SEED_3/policy.pt new file mode 100644 index 0000000..2e858e2 Binary files /dev/null and b/src/controllers/policies/locomotion_go2/oliver/flatAmpVision/2025-08-12_14-23-30_flat_DR5_improved_minimal_motion_files_SEED_3/policy.pt differ diff --git a/src/controllers/policies/locomotion_go2/oliver/flatComplexRew/2025-08-12_14-23-30_complexReward_DR5_SEED_1/policy.pt b/src/controllers/policies/locomotion_go2/oliver/flatComplexRew/2025-08-12_14-23-30_complexReward_DR5_SEED_1/policy.pt new file mode 100644 index 0000000..3b0ec19 Binary files /dev/null and b/src/controllers/policies/locomotion_go2/oliver/flatComplexRew/2025-08-12_14-23-30_complexReward_DR5_SEED_1/policy.pt differ diff --git a/src/controllers/policies/locomotion_go2/oliver/flatComplexRew/2025-08-12_14-23-30_complexReward_DR5_SEED_3/policy.pt b/src/controllers/policies/locomotion_go2/oliver/flatComplexRew/2025-08-12_14-23-30_complexReward_DR5_SEED_3/policy.pt new file mode 100644 index 0000000..c0da69e Binary files /dev/null and b/src/controllers/policies/locomotion_go2/oliver/flatComplexRew/2025-08-12_14-23-30_complexReward_DR5_SEED_3/policy.pt differ diff --git a/src/controllers/policies/torqueComplexReward/TORQUE_complex_reward_10_SEED_42/policy.pt b/src/controllers/policies/torqueComplexReward/TORQUE_complex_reward_10_SEED_42/policy.pt new file mode 100644 index 0000000..a6f82fd Binary files /dev/null and b/src/controllers/policies/torqueComplexReward/TORQUE_complex_reward_10_SEED_42/policy.pt differ diff --git a/src/controllers/policies/torqueComplexReward/TORQUE_complex_reward_15_SEED_42/policy.pt b/src/controllers/policies/torqueComplexReward/TORQUE_complex_reward_15_SEED_42/policy.pt new file mode 100644 index 0000000..5c7a163 Binary files /dev/null and b/src/controllers/policies/torqueComplexReward/TORQUE_complex_reward_15_SEED_42/policy.pt differ diff --git a/src/controllers/rl_velocity_locomotion_controller.py b/src/controllers/rl_velocity_locomotion_controller.py index ac26373..15718f1 100644 --- a/src/controllers/rl_velocity_locomotion_controller.py +++ b/src/controllers/rl_velocity_locomotion_controller.py @@ -1,8 +1,8 @@ import time -from typing import Any, Dict, TYPE_CHECKING +from typing import TYPE_CHECKING, Any, Dict -import torch import numpy as np +import torch from commands.command_manager import CommandTerm from controllers.rl_controller_base import RLControllerBase @@ -109,7 +109,6 @@ def register_observations(self): self.obs_manager.register("lin_vel_b", ObsTerm(lin_vel_b, obs_dim=3, device=self.device)) self.obs_manager.register("ang_vel_b", ObsTerm(ang_vel_b, obs_dim=3, device=self.device)) - self.obs_manager.register("projected_gravity", ObsTerm(projected_gravity_b, obs_dim=3, device=self.device)) self.obs_manager.register( "velocity_commands", ObsTerm( @@ -119,6 +118,7 @@ def register_observations(self): device=self.device, ), ) + self.obs_manager.register("projected_gravity", ObsTerm(projected_gravity_b, obs_dim=3, device=self.device)) self.obs_manager.register( "joint_pos", ObsTerm( @@ -190,7 +190,10 @@ def compute_lowlevelcmd(self, state): try: if not self.use_threading: obs_tensor = self.obs_manager.compute_full_tensor(state, batch_idx=0) - joint_pos_targets = self.compute_joint_pos_targets_from_policy(obs_tensor) + obs = self.obs_manager.get_from_buffer().squeeze() + + + joint_pos_targets = self.compute_joint_pos_targets_from_policy(obs) else: joint_pos_targets = self.compute_joint_pos_targets() @@ -227,6 +230,67 @@ def compute_lowlevelcmd(self, state): } return self.cmd + + +class RLQuadrupedLocomotionVelocityControllerTorque(RLQuadrupedLocomotionVelocityController): + """ + Velocity-conditioned quadruped RL Locomotion Controller + Uses contact-implicit reinforcement learning policy + """ + + def compute_lowlevelcmd(self, state): + """ + Compute motor commands using the learned policy. + + :param state: Current robot state + :return: Motor commands dictionary + """ + if self.robot.mj_model is not None: + self.robot.mj_model.update(state) + + start_time = time.perf_counter() + + try: + if not self.use_threading: + obs_tensor = self.obs_manager.compute_full_tensor(state, batch_idx=0) + obs = self.obs_manager.get_from_buffer().squeeze() + + + torques = self.compute_joint_pos_targets_from_policy(obs) + else: + raise ValueError("Should be run w/o threading for faster inference.") + + torques = np.clip(torques, -23.5, 23.5) + + # Prepare motor commands + self.cmd = { + f"motor_{i}": { + "q": 0, + "kp": 0, + "dq": 0.0, + "kd": 0, + "tau": torques[i], + } + for i in range(12) + } + + # Track command preparation time + self.cmd_preparation_time = time.perf_counter() - start_time + + except Exception as e: + self.logger.error(f"Error computing torques: {e}") + self.cmd = { + f"motor_{i}": { + "q": self.default_joint_pos[i], + "kp": self.Kp, + "dq": 0.0, + "kd": self.Kd, + "tau": 0.0, + } + for i in range(self.robot.num_joints) + } + + return self.cmd class RLHumanoidLocomotionVelocityController(RLControllerBase): @@ -644,11 +708,11 @@ def register_observations(self): from state_manager.observations import ( ang_vel_b, - projected_gravity_b, joint_pos_rel, joint_vel, last_action, phase_with_timing, + projected_gravity_b, velocity_commands, ) diff --git a/src/master_manager/master_manager/low_level_cmd_publisher.py b/src/master_manager/master_manager/low_level_cmd_publisher.py index ae0f284..9e7130f 100644 --- a/src/master_manager/master_manager/low_level_cmd_publisher.py +++ b/src/master_manager/master_manager/low_level_cmd_publisher.py @@ -125,6 +125,7 @@ def low_level_cmd_callback(self): # Publish the command self.dds_cmd.crc = self.crc.Crc(self.dds_cmd) + self.logger.debug(f"Sending dds_cmd: {motor_commands}") self.dds_pub.Write(self.dds_cmd) if combined_state.get("robot/base_pos_w", None) is not None: diff --git a/src/robot_interfaces/config/real_go2_cfg.yaml b/src/robot_interfaces/config/real_go2_cfg.yaml new file mode 100644 index 0000000..e69de29 diff --git a/src/robots/go2/go2.py b/src/robots/go2/go2.py index 24508dc..36aef00 100644 --- a/src/robots/go2/go2.py +++ b/src/robots/go2/go2.py @@ -4,16 +4,25 @@ from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_ as Go2LowCmd_ from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_ as Go2LowState_ -from controllers.rl_contact_locomotion_controller import RLQuadrupedLocomotionContactController -from controllers.rl_velocity_locomotion_controller import RLQuadrupedLocomotionVelocityController +from controllers.rl_contact_locomotion_controller import ( + RLQuadrupedLocomotionContactController, +) +from controllers.rl_velocity_locomotion_controller import ( + RLQuadrupedLocomotionVelocityController, + RLQuadrupedLocomotionVelocityControllerTorque, +) from controllers.stand_controller import ( + Go2StanceController, Go2StandDownController, Go2StandUpController, Go2StayDownController, - Go2StanceController, ) from robots.robot_base import RobotBase -from state_manager.msg_handlers import go2_low_state_handler, vicon_handler, sport_mode_state_handler +from state_manager.msg_handlers import ( + go2_low_state_handler, + sport_mode_state_handler, + vicon_handler, +) from state_manager.state_manager import DDSStateSubscriber, ROS2StateSubscriber from utils.joint_mapping import JointMappingInterface @@ -287,6 +296,7 @@ def available_controllers(self) -> "Dict[str, Dict[str, Type[ControllerBase]]]": }, "LOCOMOTION": { "RL-VELOCITY": RLQuadrupedLocomotionVelocityController, + "RL-VELOCITYTORQUE": RLQuadrupedLocomotionVelocityControllerTorque, }, } else: diff --git a/src/tasks/task_configs.py b/src/tasks/task_configs.py index deb7abe..69797fe 100644 --- a/src/tasks/task_configs.py +++ b/src/tasks/task_configs.py @@ -3,10 +3,18 @@ "controller": "controllers/config/rl_velocity_go2_cfg.yaml", "robot_interface": "robot_interfaces/config/sim_go2_cfg.yaml", }, + "rl-velocitytorque-sim-go2": { + "controller": "controllers/config/rl_velocity_go2_torque_cfg.yaml", + "robot_interface": "robot_interfaces/config/sim_go2_cfg.yaml", + }, "rl-velocity-real-go2": { "controller": "controllers/config/rl_velocity_go2_cfg.yaml", "robot_interface": "robot_interfaces/config/real_go2_cfg.yaml", }, + "rl-velocitytorque-real-go2": { + "controller": "controllers/config/rl_velocity_go2_torque_cfg.yaml", + "robot_interface": "robot_interfaces/config/real_go2_cfg.yaml", + }, "rl-contact-sim-go2": { "controller": "controllers/config/rl_contact_go2_cfg.yaml", "robot_interface": "robot_interfaces/config/sim_go2_cfg.yaml", diff --git a/unitree_mujoco_container b/unitree_mujoco_container index 7a1f163..45b4e71 160000 --- a/unitree_mujoco_container +++ b/unitree_mujoco_container @@ -1 +1 @@ -Subproject commit 7a1f16362ff653b45feb41273c97b212d5604c5b +Subproject commit 45b4e719548c909196ee9d10337502912bbc90d8