From 6c605bad68de3108c3c64de5337e3b159d206a9d Mon Sep 17 00:00:00 2001 From: Christopher Luey Date: Mon, 5 Jan 2026 16:33:45 -0800 Subject: [PATCH 1/8] Odometry Update --- brs_ctrl/robot_interface/interfaces.py | 30 +++++++++ .../robot_interface/mobile_base/odometry.py | 66 +++++++++++++++---- 2 files changed, 82 insertions(+), 14 deletions(-) diff --git a/brs_ctrl/robot_interface/interfaces.py b/brs_ctrl/robot_interface/interfaces.py index c45b44c..65e1161 100644 --- a/brs_ctrl/robot_interface/interfaces.py +++ b/brs_ctrl/robot_interface/interfaces.py @@ -756,6 +756,10 @@ def __init__( [0.01, 0.01, 0.05] ), mobile_base_cmd_limit: Union[np.ndarray, float] = np.array([0.3, 0.3, 0.4]), + # ====== odometry ====== + odometry_topic: str = "/zed2/zed_node/odom", + T_odom2base: Optional[np.ndarray] = None, + wait_for_first_odom_msg: bool = False, # ====== cameras ====== enable_rgb: bool = True, rgb_topics: Optional[Dict[str, str]] = None, @@ -856,6 +860,16 @@ def __init__( TwistStamped, mobile_base_vel_cmd_topic, cmd_qos ) + # odometry + if T_odom2base is None: + T_odom2base = np.eye(4) # Identity if no transform provided + self._odom = Odom( + self, # ROS2 node + odom_topic=odometry_topic, + T_odom2base=T_odom2base, + wait_for_first_msg=wait_for_first_odom_msg, + ) + self._left_gripper, self._right_gripper = left_gripper, right_gripper if self._left_gripper is not None: self._left_gripper.init_hook(self) @@ -1343,3 +1357,19 @@ def last_rgb(self): return return_dict else: return None + + @property + def curr_base_pose(self): + return self._odom.curr_base_pose + + @property + def curr_base_position(self): + return self._odom.curr_base_position + + @property + def curr_base_orientation(self): + return self._odom.curr_base_orientation + + @property + def curr_base_velocity(self): + return self._odom.curr_base_velocity diff --git a/brs_ctrl/robot_interface/mobile_base/odometry.py b/brs_ctrl/robot_interface/mobile_base/odometry.py index dc0fbac..a5b78e2 100644 --- a/brs_ctrl/robot_interface/mobile_base/odometry.py +++ b/brs_ctrl/robot_interface/mobile_base/odometry.py @@ -1,37 +1,73 @@ import numpy as np import quaternion -try: - import rospy -except ImportError as e: - print(f"Failed to import ROS related modules, Odom won't work.") - print(e) - from nav_msgs.msg import Odometry class Odom: + """ + Odometry subscriber for camera-based visual-inertial odometry. + Supports both ROS1 (rospy) and ROS2 (rclpy). + """ + def __init__( self, + node=None, # ROS2 node, or None for ROS1 *, odom_topic: str = "/camera/odom/sample", T_odom2base: np.ndarray, wait_for_first_msg: bool = True, + timeout: float = 10.0, ): - self._sub = rospy.Subscriber( - odom_topic, - Odometry, - self._callback, - ) + """ + Args: + node: ROS2 node to create subscription on. If None, uses ROS1 rospy. + odom_topic: Topic name for nav_msgs/Odometry messages + T_odom2base: 4x4 transformation matrix from odom frame to base frame + wait_for_first_msg: If True, block until first message received + timeout: Timeout in seconds when waiting for first message (ROS2 only) + """ self._T_odom2base = T_odom2base self._curr_base_pose = None self._curr_base_position = None self._curr_base_orientation = None self._curr_base_velocity = None + self._received_first_msg = False + self._use_ros2 = node is not None - if wait_for_first_msg: - print("Waiting for odometry topic...") - rospy.wait_for_message(odom_topic, Odometry) + if self._use_ros2: + # ROS2 + import rclpy + from rclpy.qos import qos_profile_sensor_data + self._node = node + self._sub = node.create_subscription( + Odometry, + odom_topic, + self._callback, + qos_profile_sensor_data, + ) + if wait_for_first_msg: + print(f"Waiting for odometry topic {odom_topic}...") + import time + start_time = time.time() + while not self._received_first_msg: + rclpy.spin_once(node, timeout_sec=0.1) + if time.time() - start_time > timeout: + print(f"Warning: Timed out waiting for odometry on {odom_topic}") + break + if self._received_first_msg: + print("Odometry topic ready!") + else: + # ROS1 + import rospy + self._sub = rospy.Subscriber( + odom_topic, + Odometry, + self._callback, + ) + if wait_for_first_msg: + print("Waiting for odometry topic...") + rospy.wait_for_message(odom_topic, Odometry) def _callback(self, data: Odometry): x, y, z = ( @@ -65,6 +101,8 @@ def _callback(self, data: Odometry): v_yaw = 0 if abs(v_yaw) <= 5e-3 else v_yaw self._curr_base_velocity = np.array([base_xyz_vel[0], base_xyz_vel[1], v_yaw]) + self._received_first_msg = True + @property def curr_base_pose(self): return self._curr_base_pose From 868742308bcb2a861c12832be48ef0ac92903f6f Mon Sep 17 00:00:00 2001 From: Christopher Luey Date: Mon, 5 Jan 2026 17:27:24 -0800 Subject: [PATCH 2/8] Node Name --- brs_ctrl/robot_interface/interfaces.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/brs_ctrl/robot_interface/interfaces.py b/brs_ctrl/robot_interface/interfaces.py index 65e1161..0c08b86 100644 --- a/brs_ctrl/robot_interface/interfaces.py +++ b/brs_ctrl/robot_interface/interfaces.py @@ -757,7 +757,7 @@ def __init__( ), mobile_base_cmd_limit: Union[np.ndarray, float] = np.array([0.3, 0.3, 0.4]), # ====== odometry ====== - odometry_topic: str = "/zed2/zed_node/odom", + odometry_topic: str = "/camera/odom/sample", T_odom2base: Optional[np.ndarray] = None, wait_for_first_odom_msg: bool = False, # ====== cameras ====== From a91978186fbff1d1170ccd10cd98492fbc600980 Mon Sep 17 00:00:00 2001 From: Wensi Ai Date: Mon, 5 Jan 2026 21:33:20 -0800 Subject: [PATCH 3/8] Temp push --- brs_ctrl/joylo/joycon/joycon_interface.py | 15 +++++++++++---- pyproject.toml | 2 +- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/brs_ctrl/joylo/joycon/joycon_interface.py b/brs_ctrl/joylo/joycon/joycon_interface.py index 61b0fd5..ed284c0 100644 --- a/brs_ctrl/joylo/joycon/joycon_interface.py +++ b/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 ====== @@ -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 diff --git a/pyproject.toml b/pyproject.toml index fa88168..c42ba35 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -11,7 +11,7 @@ dependencies = [ "tqdm", "gymnasium", ] -requires-python = ">=3.11" +requires-python = ">=3.10" authors = [ {name = "Yunfan Jiang", email = "yunfanj@cs.stanford.edu"}, ] From 4bb29d10dcd648a9ed080d38b2eeec0382061a14 Mon Sep 17 00:00:00 2001 From: Wensi Ai Date: Mon, 5 Jan 2026 21:33:20 -0800 Subject: [PATCH 4/8] Temp push --- brs_ctrl/joylo/joycon/joycon_interface.py | 26 +++++++++++++++++------ pyproject.toml | 2 +- 2 files changed, 21 insertions(+), 7 deletions(-) diff --git a/brs_ctrl/joylo/joycon/joycon_interface.py b/brs_ctrl/joylo/joycon/joycon_interface.py index 61b0fd5..3493899 100644 --- a/brs_ctrl/joylo/joycon/joycon_interface.py +++ b/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 ====== @@ -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 @@ -423,8 +430,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 ====== @@ -688,10 +695,12 @@ def act(self, curr_torso_q: np.ndarray): diff = self._torso_joints1_2_stand_q - torso_cmd[:2] 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] 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 +711,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 diff --git a/pyproject.toml b/pyproject.toml index fa88168..c42ba35 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -11,7 +11,7 @@ dependencies = [ "tqdm", "gymnasium", ] -requires-python = ">=3.11" +requires-python = ">=3.10" authors = [ {name = "Yunfan Jiang", email = "yunfanj@cs.stanford.edu"}, ] From 42add2e6d7dd06154a992f61e7ba4b0d47f57191 Mon Sep 17 00:00:00 2001 From: Christopher Luey Date: Mon, 5 Jan 2026 22:33:07 -0800 Subject: [PATCH 5/8] Chassis Telemetry --- brs_ctrl/robot_interface/interfaces.py | 55 ++++++++++++++++++++++++++ 1 file changed, 55 insertions(+) diff --git a/brs_ctrl/robot_interface/interfaces.py b/brs_ctrl/robot_interface/interfaces.py index 0c08b86..26373db 100644 --- a/brs_ctrl/robot_interface/interfaces.py +++ b/brs_ctrl/robot_interface/interfaces.py @@ -750,6 +750,8 @@ def __init__( # ====== torso ====== torso_joint_state_topic: str = "/hdas/feedback_torso", torso_joint_target_position_topic: str = "/motion_target/target_joint_state_torso", + # ====== chassis ====== + chassis_joint_state_topic: str = "/hdas/feedback_chassis", # ====== mobile base ====== mobile_base_vel_cmd_topic: str = "/motion_target/target_speed_chassis", mobile_base_cmd_threshold: Union[np.ndarray, float] = np.array( @@ -781,6 +783,7 @@ def __init__( self._left_arm_joint_state_buffer = None self._right_arm_joint_state_buffer = None self._torso_joint_state_buffer = None + self._chassis_joint_state_buffer = None self._state_buffer_size = state_buffer_size self._rgb = None @@ -820,6 +823,9 @@ def __init__( self._torso_joint_state_sub = self.create_subscription( JointState, torso_joint_state_topic, self._torso_state_callback, sens_qos ) + self._chassis_joint_state_sub = self.create_subscription( + JointState, chassis_joint_state_topic, self._chassis_state_callback, sens_qos + ) if enable_rgb: rgb_topics = rgb_topics or { @@ -1291,6 +1297,26 @@ def _torso_state_callback(self, data: JointState): self._torso_joint_state_buffer, np.s_[-self._state_buffer_size :] ) + def _chassis_state_callback(self, data: JointState): + # Chassis feedback: position has 3 values, velocity has 6 values (use first 3) + new_state = { + "joint_position": np.array([data.position[:3]]), + "joint_velocity": np.array([data.velocity[:3]]), + "seq": np.array([data.header.stamp.nanosec]), + "stamp": np.array( + [data.header.stamp.sec + data.header.stamp.nanosec * 1e-9] + ), + } + if self._chassis_joint_state_buffer is None: + self._chassis_joint_state_buffer = new_state + else: + self._chassis_joint_state_buffer = U.any_concat( + [self._chassis_joint_state_buffer, new_state], dim=0 + ) + self._chassis_joint_state_buffer = U.any_slice( + self._chassis_joint_state_buffer, np.s_[-self._state_buffer_size :] + ) + # ---------- Lifecycle ---------- def close(self): # stop the base @@ -1340,6 +1366,35 @@ def last_gripper_state(self): "right_gripper": (U.any_slice(self._right_gripper.state_buffer, -1)), } + @property + def last_chassis_position(self) -> Optional[np.ndarray]: + """Get the last chassis position (3 values) from /hdas/feedback_chassis.""" + if self._chassis_joint_state_buffer is None: + return None + return U.any_slice(self._chassis_joint_state_buffer, -1)["joint_position"] + + @property + def last_chassis_velocity(self) -> Optional[np.ndarray]: + """Get the last chassis velocity (first 3 of 6 values) from /hdas/feedback_chassis.""" + if self._chassis_joint_state_buffer is None: + return None + return U.any_slice(self._chassis_joint_state_buffer, -1)["joint_velocity"] + + @property + def last_joint_velocity(self) -> Optional[Dict[str, np.ndarray]]: + """Get the last joint velocities for all robot parts.""" + if ( + self._left_arm_joint_state_buffer is None + or self._right_arm_joint_state_buffer is None + or self._torso_joint_state_buffer is None + ): + return None + return { + "left_arm": U.any_slice(self._left_arm_joint_state_buffer, -1)["joint_velocity"], + "right_arm": U.any_slice(self._right_arm_joint_state_buffer, -1)["joint_velocity"], + "torso": U.any_slice(self._torso_joint_state_buffer, -1)["joint_velocity"], + } + @property def last_rgb(self): # Because v can be None From 11540c426072bd7b14f0f435f521b415a57d643c Mon Sep 17 00:00:00 2001 From: Yunfan Jiang Date: Thu, 8 Jan 2026 01:48:07 -0800 Subject: [PATCH 6/8] Working --- brs_ctrl/joylo/joycon/joycon_calibration.yaml | 31 +++++++++++++++++++ brs_ctrl/joylo/joycon/joycon_interface.py | 24 +++++++------- 2 files changed, 43 insertions(+), 12 deletions(-) create mode 100644 brs_ctrl/joylo/joycon/joycon_calibration.yaml diff --git a/brs_ctrl/joylo/joycon/joycon_calibration.yaml b/brs_ctrl/joylo/joycon/joycon_calibration.yaml new file mode 100644 index 0000000..9f0b43b --- /dev/null +++ b/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 diff --git a/brs_ctrl/joylo/joycon/joycon_interface.py b/brs_ctrl/joylo/joycon/joycon_interface.py index 3493899..cd20b39 100644 --- a/brs_ctrl/joylo/joycon/joycon_interface.py +++ b/brs_ctrl/joylo/joycon/joycon_interface.py @@ -425,8 +425,8 @@ 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, @@ -686,21 +686,21 @@ def act(self, curr_torso_q: np.ndarray): ) delta_rv *= -1 # because vertical direction is reversed torso_cmd[2] += ( - 2 + 2.1 * 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 + # 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[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], @@ -711,11 +711,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[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 From afff676fce90b1e5f313cf33ad4710a9f312c1d6 Mon Sep 17 00:00:00 2001 From: Christopher Luey Date: Tue, 20 Jan 2026 20:14:57 -0800 Subject: [PATCH 7/8] Working --- brs_ctrl/env.py | 9 +- brs_ctrl/joylo/joycon/joycon_interface.py | 4 +- brs_ctrl/robot_interface/interfaces.py | 135 ++++++++++------------ 3 files changed, 68 insertions(+), 80 deletions(-) diff --git a/brs_ctrl/env.py b/brs_ctrl/env.py index d8d2cca..20a8271 100644 --- a/brs_ctrl/env.py +++ b/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, {} diff --git a/brs_ctrl/joylo/joycon/joycon_interface.py b/brs_ctrl/joylo/joycon/joycon_interface.py index cd20b39..bfd3ef5 100644 --- a/brs_ctrl/joylo/joycon/joycon_interface.py +++ b/brs_ctrl/joylo/joycon/joycon_interface.py @@ -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) @@ -663,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) diff --git a/brs_ctrl/robot_interface/interfaces.py b/brs_ctrl/robot_interface/interfaces.py index 26373db..9d712be 100644 --- a/brs_ctrl/robot_interface/interfaces.py +++ b/brs_ctrl/robot_interface/interfaces.py @@ -408,9 +408,16 @@ def _upper_body_joint_position_control( else 0 ) - # control loop - max_n_steps = max(left_arm_n_steps, right_arm_n_steps, torso_n_steps) - if max_n_steps > 1: + # TORSO: Publish immediately without interpolation for responsive control + # This ensures holding the torso button results in continuous movement + if torso_target_q is not None: + torso_joint_state.header.stamp = rospy.Time.now() + torso_joint_state.position = torso_target_q.tolist() + self._torso_joint_target_position_pub.publish(torso_joint_state) + + # ARMS: Use interpolation loop only for arms (torso already published above) + max_arm_n_steps = max(left_arm_n_steps, right_arm_n_steps) + if max_arm_n_steps > 1: if left_arm_target_q is not None: left_arm_increment = ( (left_arm_target_q - np.array(left_arm_joint_state.position)) @@ -429,17 +436,8 @@ def _upper_body_joint_position_control( ) else: right_arm_increment = np.zeros(6) - if torso_target_q is not None: - torso_increment = ( - (torso_target_q - np.array(torso_joint_state.position)) - / (torso_n_steps - 1) - if torso_n_steps > 1 - else (torso_target_q - np.array(torso_joint_state.position)) - ) - else: - torso_increment = np.zeros(4) - for step in range(max_n_steps - 1): + for step in range(max_arm_n_steps - 1): if left_arm_target_q is not None: left_arm_joint_state.header.stamp = rospy.Time.now() if step <= left_arm_n_steps - 1: @@ -463,18 +461,9 @@ def _upper_body_joint_position_control( self._right_arm_joint_target_position_pub.publish( right_arm_joint_state ) - if torso_target_q is not None: - torso_joint_state.header.stamp = rospy.Time.now() - if step <= torso_n_steps - 1: - torso_joint_state.position = ( - np.array(torso_joint_state.position) + torso_increment - ).tolist() - else: - torso_joint_state.position = torso_target_q.tolist() - self._torso_joint_target_position_pub.publish(torso_joint_state) self._rate.sleep() - # ensure the last position is the target position + # ensure exact final target positions for arms if left_arm_target_q is not None: left_arm_joint_state.header.stamp = rospy.Time.now() left_arm_joint_state.position = left_arm_target_q.tolist() @@ -483,10 +472,6 @@ def _upper_body_joint_position_control( right_arm_joint_state.header.stamp = rospy.Time.now() right_arm_joint_state.position = right_arm_target_q.tolist() self._right_arm_joint_target_position_pub.publish(right_arm_joint_state) - if torso_target_q is not None: - torso_joint_state.header.stamp = rospy.Time.now() - torso_joint_state.position = torso_target_q.tolist() - self._torso_joint_target_position_pub.publish(torso_joint_state) if not ( left_arm_target_q is None and right_arm_target_q is None @@ -774,11 +759,13 @@ def __init__( control_freq: float = 100.0, on_arm_cmd_out_of_range: Literal["raise", "clip"] = "clip", on_torso_cmd_out_of_range: Literal["raise", "clip"] = "clip", + log_clipping_warnings: bool = True, ): super().__init__(publisher_node_name) # Frequency for sleeps (wall clock; prefer timers for periodic callbacks) self._control_freq = float(control_freq) self._sleep_dt = 1.0 / self._control_freq + self._log_clipping_warnings = log_clipping_warnings self._left_arm_joint_state_buffer = None self._right_arm_joint_state_buffer = None @@ -985,19 +972,23 @@ def _upper_body_joint_position_control( left_arm_target_q <= self.left_arm_joint_high, ) for idx in np.where(~left_in_range)[0]: - msg = ( - f"Left arm joint {idx+1} target {left_arm_target_q[idx]} out of range " - f"[{self.left_arm_joint_low[idx]}, {self.left_arm_joint_high[idx]}]." - ) if self._on_arm_cmd_out_of_range == "clip": left_arm_target_q[idx] = np.clip( left_arm_target_q[idx], self.left_arm_joint_low[idx], self.left_arm_joint_high[idx], ) - msg += " Clipped." - self.get_logger().warning(msg) + if self._log_clipping_warnings: + msg = ( + f"Left arm joint {idx+1} target {left_arm_target_q[idx]} out of range " + f"[{self.left_arm_joint_low[idx]}, {self.left_arm_joint_high[idx]}]. Clipped." + ) + self.get_logger().warning(msg) else: + msg = ( + f"Left arm joint {idx+1} target {left_arm_target_q[idx]} out of range " + f"[{self.left_arm_joint_low[idx]}, {self.left_arm_joint_high[idx]}]." + ) raise ValueError(msg) if right_arm_target_q is not None: @@ -1006,19 +997,23 @@ def _upper_body_joint_position_control( right_arm_target_q <= self.right_arm_joint_high, ) for idx in np.where(~right_in_range)[0]: - msg = ( - f"Right arm joint {idx+1} target {right_arm_target_q[idx]} out of range " - f"[{self.right_arm_joint_low[idx]}, {self.right_arm_joint_high[idx]}]." - ) if self._on_arm_cmd_out_of_range == "clip": right_arm_target_q[idx] = np.clip( right_arm_target_q[idx], self.right_arm_joint_low[idx], self.right_arm_joint_high[idx], ) - msg += " Clipped." - self.get_logger().warning(msg) + if self._log_clipping_warnings: + msg = ( + f"Right arm joint {idx+1} target {right_arm_target_q[idx]} out of range " + f"[{self.right_arm_joint_low[idx]}, {self.right_arm_joint_high[idx]}]. Clipped." + ) + self.get_logger().warning(msg) else: + msg = ( + f"Right arm joint {idx+1} target {right_arm_target_q[idx]} out of range " + f"[{self.right_arm_joint_low[idx]}, {self.right_arm_joint_high[idx]}]." + ) raise ValueError(msg) if torso_target_q is not None: @@ -1027,18 +1022,23 @@ def _upper_body_joint_position_control( torso_target_q <= self.torso_joint_high, ) for idx in np.where(~torso_in_range)[0]: - msg = ( - f"Torso joint {idx+1} target {torso_target_q[idx]} out of range " - f"[{self.torso_joint_low[idx]}, {self.torso_joint_high[idx]}]." - ) if self._on_torso_cmd_out_of_range == "clip": torso_target_q[idx] = np.clip( torso_target_q[idx], self.torso_joint_low[idx], self.torso_joint_high[idx], ) - self.get_logger().warning(msg) + if self._log_clipping_warnings: + msg = ( + f"Torso joint {idx+1} target {torso_target_q[idx]} out of range " + f"[{self.torso_joint_low[idx]}, {self.torso_joint_high[idx]}]. Clipped." + ) + self.get_logger().warning(msg) else: + msg = ( + f"Torso joint {idx+1} target {torso_target_q[idx]} out of range " + f"[{self.torso_joint_low[idx]}, {self.torso_joint_high[idx]}]." + ) raise ValueError(msg) # compute step counts @@ -1103,9 +1103,16 @@ def _upper_body_joint_position_control( else 0 ) - # control loop - max_n_steps = max(left_arm_n_steps, right_arm_n_steps, torso_n_steps) - if max_n_steps > 1: + # TORSO: Publish immediately without interpolation for responsive control + # This ensures holding the torso button results in continuous movement + if torso_target_q is not None: + torso_joint_state.header.stamp = self._now_msg() + torso_joint_state.position = torso_target_q.tolist() + self._torso_joint_target_position_pub.publish(torso_joint_state) + + # ARMS: Use interpolation loop only for arms (torso already published above) + max_arm_n_steps = max(left_arm_n_steps, right_arm_n_steps) + if max_arm_n_steps > 1: left_arm_increment = ( ( (left_arm_target_q - np.array(left_arm_joint_state.position)) @@ -1115,7 +1122,7 @@ def _upper_body_joint_position_control( else ( (left_arm_target_q - np.array(left_arm_joint_state.position)) if left_arm_target_q is not None - else np.zeros(6) + else np.zeros(7) ) ) right_arm_increment = ( @@ -1127,23 +1134,11 @@ def _upper_body_joint_position_control( else ( (right_arm_target_q - np.array(right_arm_joint_state.position)) if right_arm_target_q is not None - else np.zeros(6) - ) - ) - torso_increment = ( - ( - (torso_target_q - np.array(torso_joint_state.position)) - / (torso_n_steps - 1) - ) - if (torso_target_q is not None and torso_n_steps > 1) - else ( - (torso_target_q - np.array(torso_joint_state.position)) - if torso_target_q is not None - else np.zeros(4) + else np.zeros(7) ) ) - for step in range(max_n_steps - 1): + for step in range(max_arm_n_steps - 1): if left_arm_target_q is not None: left_arm_joint_state.header.stamp = self._now_msg() if step <= left_arm_n_steps - 1: @@ -1169,19 +1164,9 @@ def _upper_body_joint_position_control( right_arm_joint_state ) - if torso_target_q is not None: - torso_joint_state.header.stamp = self._now_msg() - if step <= torso_n_steps - 1: - torso_joint_state.position = ( - np.array(torso_joint_state.position) + torso_increment - ).tolist() - else: - torso_joint_state.position = torso_target_q.tolist() - self._torso_joint_target_position_pub.publish(torso_joint_state) - self._sleep_tick() - # ensure exact final target positions + # ensure exact final target positions for arms if left_arm_target_q is not None: left_arm_joint_state.header.stamp = self._now_msg() left_arm_joint_state.position = left_arm_target_q.tolist() @@ -1190,10 +1175,6 @@ def _upper_body_joint_position_control( right_arm_joint_state.header.stamp = self._now_msg() right_arm_joint_state.position = right_arm_target_q.tolist() self._right_arm_joint_target_position_pub.publish(right_arm_joint_state) - if torso_target_q is not None: - torso_joint_state.header.stamp = self._now_msg() - torso_joint_state.position = torso_target_q.tolist() - self._torso_joint_target_position_pub.publish(torso_joint_state) if not ( left_arm_target_q is None From cefa41c5ed4245f4abdb5689d523ce1adba084b1 Mon Sep 17 00:00:00 2001 From: Christopher Luey Date: Tue, 20 Jan 2026 20:51:29 -0800 Subject: [PATCH 8/8] Adjust Torso Speed --- brs_ctrl/joylo/joycon/joycon_interface.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/brs_ctrl/joylo/joycon/joycon_interface.py b/brs_ctrl/joylo/joycon/joycon_interface.py index bfd3ef5..a5db58a 100644 --- a/brs_ctrl/joylo/joycon/joycon_interface.py +++ b/brs_ctrl/joylo/joycon/joycon_interface.py @@ -685,8 +685,10 @@ 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.1 + multiplier * delta_rv / (rv_limits[1] - rv_limits[0]) * self._torso_joint_max_delta