Odometry Update for ROS2

brs_ctrl/env.py

@@ -133,10 +133,15 @@ def __init__(self, control_freq: float = 100.0, img_resolution: int = 256):
 
     def _spin_thread(self):
         # run executor until close() flips the flag
+        # Use shorter timeout for more responsive callback processing
         while self._spin and rclpy.ok():
-            self._executor.spin_once(timeout_sec=0.05)
+            self._executor.spin_once(timeout_sec=0.005)
 
     def step(self, action: Dict[str, np.ndarray]):
+        # Process any pending callbacks to ensure fresh state before control
+        # This ensures torso feedback is up-to-date for responsive control
+        self._executor.spin_once(timeout_sec=0)
+
         self.robot_interface.control(
             arm_cmd={
                 "left": action["action.left_arm_joints"],
@@ -152,6 +157,8 @@ def step(self, action: Dict[str, np.ndarray]):
         return self._get_observation(), 0.0, False, False, {}
 
     def reset(self, seed=None, options=None):
+        # Process pending callbacks to ensure fresh state
+        self._executor.spin_once(timeout_sec=0)
         obs = self._get_observation()
         return obs, {}
 

brs_ctrl/joylo/joycon/joycon_calibration.yaml

@@ -0,0 +1,31 @@
+joystick:
+  left:
+    horizontal:
+      center_range:
+      - 1968
+      - 1971
+      limits:
+      - 645
+      - 3337
+    vertical:
+      center_range:
+      - 2295
+      - 2300
+      limits:
+      - 1131
+      - 3402
+  right:
+    horizontal:
+      center_range:
+      - 2047
+      - 2051
+      limits:
+      - 696
+      - 3346
+    vertical:
+      center_range:
+      - 1808
+      - 1838
+      limits:
+      - 665
+      - 2918

brs_ctrl/joylo/joycon/joycon_interface.py

@@ -28,8 +28,8 @@ def __init__(
         mobile_base_yaw_rotate_max: float = 0.4,
         # ====== torso ======
         torso_joint_max_delta: float = 0.1,
-        torso_joints1_2_stand_q: np.ndarray = np.array([0, 0]),
-        torso_joints1_2_squat_q: np.ndarray = np.array([1.74, -2.70]),
+        torso_joints1_2_stand_q: np.ndarray = np.array([0, 0, 0]),
+        torso_joints1_2_squat_q: np.ndarray = np.array([1.74, -2.70, -0.96]),
         # ====== gripper ======
         gripper_toggle_mode: bool = False,
         # ====== common ======
@@ -296,7 +296,7 @@ def act(self, curr_torso_q: np.ndarray):
         rh = self._rh_ema
         rv = self._rv_ema
 
-        torso_cmd = curr_torso_q
+        torso_cmd = curr_torso_q.copy()  # Must copy to avoid modifying input array
         if rh > rh_neutral_range[1] or rh < rh_neutral_range[0]:
             rh = np.clip(rh, rh_limits[0], rh_limits[1])
             delta_rh = rh - 0.5 * sum(rh_neutral_range)
@@ -325,13 +325,15 @@ def act(self, curr_torso_q: np.ndarray):
                 * self._torso_joint_max_delta
             )
         if self.jc_left.get_button_up():
-            diff = self._torso_joints1_2_stand_q - torso_cmd[:2]
+            diff = self._torso_joints1_2_stand_q - torso_cmd[:3]
             torso_cmd[0] += np.sign(diff[0]) * self._torso_joint_max_delta * 1.74 / 2.7
             torso_cmd[1] += np.sign(diff[1]) * self._torso_joint_max_delta
+            torso_cmd[2] += np.sign(diff[2]) * self._torso_joint_max_delta * 0.96 / 2.7
         elif self.jc_left.get_button_down():
-            diff = self._torso_joints1_2_squat_q - torso_cmd[:2]
+            diff = self._torso_joints1_2_squat_q - torso_cmd[:3]
             torso_cmd[0] += np.sign(diff[0]) * self._torso_joint_max_delta * 1.74 / 2.7
             torso_cmd[1] += np.sign(diff[1]) * self._torso_joint_max_delta
+            torso_cmd[2] += np.sign(diff[2]) * self._torso_joint_max_delta * 0.96 / 2.7
         torso_cmd[0] = np.clip(
             torso_cmd[0],
             self._torso_joints1_2_stand_q[0],
@@ -342,6 +344,11 @@ def act(self, curr_torso_q: np.ndarray):
             self._torso_joints1_2_squat_q[1],
             self._torso_joints1_2_stand_q[1],
         )
+        torso_cmd[2] = np.clip(
+            torso_cmd[2],
+            self._torso_joints1_2_squat_q[2],
+            self._torso_joints1_2_stand_q[2],
+        )
         torso_cmd = np.clip(torso_cmd, self.torso_joint_low, self.torso_joint_high)
 
         # process gripper
@@ -418,13 +425,13 @@ def __init__(
         *,
         calibration_file: Optional[str] = None,
         # ====== mobile base ======
-        mobile_base_x_move_max: float = 0.3,
-        mobile_base_y_move_max: float = 0.3,
+        mobile_base_x_move_max: float = 0.4,
+        mobile_base_y_move_max: float = 0.4,
         mobile_base_yaw_rotate_max: float = 0.4,
         # ====== torso ======
         torso_joint_max_delta: float = 0.1,
-        torso_joints1_2_stand_q: np.ndarray = np.array([0, 0]),
-        torso_joints1_2_squat_q: np.ndarray = np.array([1.74, -2.70]),
+        torso_joints1_2_stand_q: np.ndarray = np.array([0, 0, 0]),
+        torso_joints1_2_squat_q: np.ndarray = np.array([1.74, -2.70, -0.96]),
         # ====== gripper ======
         gripper_toggle_mode: bool = False,
         # ====== common ======
@@ -656,7 +663,7 @@ def act(self, curr_torso_q: np.ndarray):
         rh = self._rh_ema
         rv = self._rv_ema
 
-        torso_cmd = curr_torso_q
+        torso_cmd = curr_torso_q.copy()  # Must copy to avoid modifying input array
         if rh > rh_neutral_range[1] or rh < rh_neutral_range[0]:
             rh = np.clip(rh, rh_limits[0], rh_limits[1])
             delta_rh = rh - 0.5 * sum(rh_neutral_range)
@@ -678,20 +685,24 @@ def act(self, curr_torso_q: np.ndarray):
                 abs(delta_rv), a_max=(rv_limits[1] - rv_limits[0]) / 2, a_min=0
             )
             delta_rv *= -1  # because vertical direction is reversed
+            # Asymmetric speed: faster forward (joystick down), slower backward (up)
+            multiplier = 1.5 if delta_rv > 0 else 2.75
             torso_cmd[2] += (
-                2
+                multiplier
                 * delta_rv
                 / (rv_limits[1] - rv_limits[0])
                 * self._torso_joint_max_delta
             )
         if self.jc_left.get_button_up():
-            diff = self._torso_joints1_2_stand_q - torso_cmd[:2]
+            diff = self._torso_joints1_2_stand_q - torso_cmd[:3]
             torso_cmd[0] += np.sign(diff[0]) * self._torso_joint_max_delta * 1.74 / 2.7
             torso_cmd[1] += np.sign(diff[1]) * self._torso_joint_max_delta
+            # torso_cmd[2] += np.sign(diff[2]) * self._torso_joint_max_delta * 0.96 / 2.7
         elif self.jc_left.get_button_down():
-            diff = self._torso_joints1_2_squat_q - torso_cmd[:2]
+            diff = self._torso_joints1_2_squat_q - torso_cmd[:3]
             torso_cmd[0] += np.sign(diff[0]) * self._torso_joint_max_delta * 1.74 / 2.7
             torso_cmd[1] += np.sign(diff[1]) * self._torso_joint_max_delta
+            # torso_cmd[2] += np.sign(diff[2]) * self._torso_joint_max_delta * 0.96 / 2.7
         torso_cmd[0] = np.clip(
             torso_cmd[0],
             self._torso_joints1_2_stand_q[0],
@@ -702,6 +713,11 @@ def act(self, curr_torso_q: np.ndarray):
             self._torso_joints1_2_squat_q[1],
             self._torso_joints1_2_stand_q[1],
         )
+        # torso_cmd[2] = np.clip(
+        #     torso_cmd[2],
+        #     self._torso_joints1_2_squat_q[2],
+        #     self._torso_joints1_2_stand_q[2],
+        # )
         torso_cmd = np.clip(torso_cmd, self.torso_joint_low, self.torso_joint_high)
 
         # process gripper