Add Go2torque controller

.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"

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

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

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,
         )
 

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:

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:

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",