From 62c04386cf8272299f8ed2a415097e7e97e1889f Mon Sep 17 00:00:00 2001 From: OjasPunje Date: Tue, 21 Apr 2026 22:26:58 -0400 Subject: [PATCH 1/3] Add joint-by-joint virtual four-bar coupling --- .../arm/arm_bringup/config/athena_arm_controllers.yaml | 1 + .../config/manual_arm_joint_by_joint_controller.yaml | 6 ++++++ .../manual_arm_joint_by_joint_controller.hpp | 3 +++ .../src/manual_arm_joint_by_joint_controller.cpp | 8 +++++++- 4 files changed, 17 insertions(+), 1 deletion(-) diff --git a/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml index 87f976d0..56fc06c8 100644 --- a/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml +++ b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml @@ -121,6 +121,7 @@ manual_arm_joint_by_joint_controller: - 0.785398 - 0.005 # TODO: Find this - 0.03 # Actuator max displacement + virtual_four_bar_coupling_ratio: -1.0 motor_status_broadcaster: ros__parameters: diff --git a/src/subsystems/arm/arm_controllers/config/manual_arm_joint_by_joint_controller.yaml b/src/subsystems/arm/arm_controllers/config/manual_arm_joint_by_joint_controller.yaml index 03f73d72..33b2f49f 100644 --- a/src/subsystems/arm/arm_controllers/config/manual_arm_joint_by_joint_controller.yaml +++ b/src/subsystems/arm/arm_controllers/config/manual_arm_joint_by_joint_controller.yaml @@ -67,3 +67,9 @@ manual_arm_joint_by_joint_controller: not_empty<>: null, } } + virtual_four_bar_coupling_ratio: { + type: double, + default_value: -1.0, + description: "Virtual four-bar ratio applied as elbow_motor_velocity += ratio * shoulder_motor_velocity.", + read_only: true, + } diff --git a/src/subsystems/arm/arm_controllers/include/athena_arm_controllers/manual_arm_joint_by_joint_controller.hpp b/src/subsystems/arm/arm_controllers/include/athena_arm_controllers/manual_arm_joint_by_joint_controller.hpp index bb743e92..a29f5943 100644 --- a/src/subsystems/arm/arm_controllers/include/athena_arm_controllers/manual_arm_joint_by_joint_controller.hpp +++ b/src/subsystems/arm/arm_controllers/include/athena_arm_controllers/manual_arm_joint_by_joint_controller.hpp @@ -94,6 +94,9 @@ class ManualArmJointByJointController : public controller_interface::ControllerI // Represents maximum velocities of each joint std::vector max_velocities_; + // Virtual four-bar ratio applied as elbow_motor_velocity += ratio * shoulder_motor_velocity. + double virtual_four_bar_coupling_ratio_ = -1.0; + // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; realtime_tools::RealtimeBuffer> input_ref_; diff --git a/src/subsystems/arm/arm_controllers/src/manual_arm_joint_by_joint_controller.cpp b/src/subsystems/arm/arm_controllers/src/manual_arm_joint_by_joint_controller.cpp index a81e4c97..5f9824a4 100644 --- a/src/subsystems/arm/arm_controllers/src/manual_arm_joint_by_joint_controller.cpp +++ b/src/subsystems/arm/arm_controllers/src/manual_arm_joint_by_joint_controller.cpp @@ -1,5 +1,6 @@ #include "athena_arm_controllers/manual_arm_joint_by_joint_controller.hpp" +#include #include #include #include @@ -82,6 +83,7 @@ controller_interface::CallbackReturn ManualArmJointByJointController::on_configu num_joints = static_cast(params_.position_joints.size()) + static_cast(params_.velocity_joints.size()); joint_velocities_.resize(num_joints, 0.0); // Output max_velocities_ = params_.joint_max_velocities; + virtual_four_bar_coupling_ratio_ = params_.virtual_four_bar_coupling_ratio; // topics QoS auto subscribers_qos = rclcpp::SystemDefaultsQoS(); @@ -174,7 +176,7 @@ controller_interface::InterfaceConfiguration ManualArmJointByJointController::st state_interfaces_config.names.reserve(num_joints); for (const auto & joint : params_.velocity_joints) { - state_interfaces_config.names.push_back(joint + "/velocity"); + state_interfaces_config.names.push_back(joint + "/position"); } for (const auto & joint : params_.position_joints) { @@ -271,6 +273,10 @@ controller_interface::return_type ManualArmJointByJointController::update( joint_velocities_.resize(7, 0.0); } + // Virtual four-bar compensation. With the current -1.0 ratio, shoulder-only motion commands + // the elbow motor equally in the opposite direction so the net elbow joint angle stays fixed. + joint_velocities_[2] += virtual_four_bar_coupling_ratio_ * joint_velocities_[1]; + // RCLCPP_INFO(get_node()->get_logger(), "Size of Command Interface: %d", command_interfaces_.size()); for (size_t i = 0; i < command_interfaces_.size(); ++i) From 5c8c3049120b1363ae1f24995786d3d1456b9b39 Mon Sep 17 00:00:00 2001 From: OjasPunje Date: Mon, 27 Apr 2026 19:02:02 -0400 Subject: [PATCH 2/3] Add Xbox joystick support to joint-by-joint branch --- .../arm/arm_bringup/config/joystick.yaml | 7 +- src/teleop/teleop/package.xml | 6 + .../teleop/teleop/joystick_publisher.py | 125 ++++++++++++++++-- 3 files changed, 122 insertions(+), 16 deletions(-) diff --git a/src/subsystems/arm/arm_bringup/config/joystick.yaml b/src/subsystems/arm/arm_bringup/config/joystick.yaml index 528cb389..9ea22b42 100644 --- a/src/subsystems/arm/arm_bringup/config/joystick.yaml +++ b/src/subsystems/arm/arm_bringup/config/joystick.yaml @@ -1,7 +1,8 @@ /**: ros__parameters: - # 0: PS4 1: Thrustmaster - joystick_type: 0 + # 0: PS4 1: Thrustmaster 2: Xbox One 3: 8BitDo Ultimate + joystick_type: 2 + joystick_index: -1 # can ids base_yaw_id: 5 @@ -15,4 +16,4 @@ wrist_roll_id: 7 #additional parameters - fast_mode_multiplier: 10 \ No newline at end of file + fast_mode_multiplier: 10 diff --git a/src/teleop/teleop/package.xml b/src/teleop/teleop/package.xml index d54e98de..7c2a46df 100644 --- a/src/teleop/teleop/package.xml +++ b/src/teleop/teleop/package.xml @@ -7,6 +7,12 @@ duttaishan01 TODO: License declaration + geometry_msgs + rclpy + rosidl_runtime_py + sensor_msgs + std_msgs + ament_copyright ament_flake8 ament_pep257 diff --git a/src/teleop/teleop/teleop/joystick_publisher.py b/src/teleop/teleop/teleop/joystick_publisher.py index 9330549d..f9d9c771 100755 --- a/src/teleop/teleop/teleop/joystick_publisher.py +++ b/src/teleop/teleop/teleop/joystick_publisher.py @@ -25,13 +25,20 @@ def __init__(self): namespace='', parameters=[ ('joystick_type', rclpy.Parameter.Type.INTEGER), + ('joystick_index', rclpy.Parameter.Type.INTEGER), ] ) - self.possible_joysticks = ["sony computer entertainment wireless controller", "thrustmaster t.16000m"] + self.possible_joysticks = [ + "sony computer entertainment wireless controller", + "thrustmaster t.16000m", + "xbox one s controller", + "8bitdo ultimate wireless controller", + ] - # 0: PS4 1: Thrustmaster + # 0: PS4 1: Thrustmaster 2: Xbox One 3: 8BitDo Ultimate self.joystick_type = self.get_parameter('joystick_type').value + self.requested_joystick_index = self.get_parameter('joystick_index').value self.get_logger().info(f"Joystick type: {self.joystick_type}") # Detect Jetson platform @@ -57,13 +64,24 @@ def __init__(self): pygame.init() pygame.joystick.init() joysticks = pygame.joystick.get_count() - - for i in range(joysticks): - js = pygame.joystick.Joystick(i) - name = js.get_name().lower() - if(name == self.possible_joysticks[self.joystick_type]): - self.joystick_index = i - break + self.get_logger().info(f"Number of joysticks found: {joysticks}") + + if 0 <= self.requested_joystick_index < joysticks: + self.joystick_index = self.requested_joystick_index + self.get_logger().info(f"Using configured joystick index: {self.joystick_index}") + else: + for i in range(joysticks): + js = pygame.joystick.Joystick(i) + name = js.get_name().lower() + self.get_logger().info(f"Joystick {i}: {name}") + expected_name = self.possible_joysticks[self.joystick_type] + if( + name == expected_name or + (self.joystick_type == 2 and "xbox" in name) or + (self.joystick_type == 3 and ("8bitdo" in name or "ultimate" in name)) + ): + self.joystick_index = i + break # Only begin once a joystick is connected while(joysticks == 0): @@ -75,10 +93,17 @@ def __init__(self): pygame.joystick.init() joysticks = pygame.joystick.get_count() break + + if self.joystick_index is None: + self.get_logger().warn("Configured joystick was not found. Falling back to joystick index 0.") + self.joystick_index = 0 self.controller = pygame.joystick.Joystick(self.joystick_index) self.get_logger().info(f"Using joystick: {self.controller.get_name()}") self.controller.init() + self.get_logger().info( + f"Joystick axes: {self.controller.get_numaxes()}, buttons: {self.controller.get_numbuttons()}, hats: {self.controller.get_numhats()}" + ) if not self.axis_data: self.axis_data = {0: 0, 1: 0, 2: 0, 3: 0, 4: 0, 5: 0} @@ -110,7 +135,7 @@ def controller_inputs(self): elif event.type == pygame.JOYHATMOTION: self.hat_data[event.hat] = event.value - if(self.joystick_type == 0): + if(self.joystick_type == 0): """ PS4 Controller: @@ -192,7 +217,7 @@ def controller_inputs(self): for i in range(13): button_activations[i] = self.button_data[i] - elif(self.joystick_type == 1): + elif(self.joystick_type == 1): """ Thrustmaster: @@ -267,8 +292,82 @@ def controller_inputs(self): # Buttons for i in range(13): button_activations[i] = self.button_data[i] - else: - print('calibrate:', pprint.pprint(self.axis_data)) + elif(self.joystick_type == 2 or self.joystick_type == 3): + + """ + Xbox One / 8BitDo Ultimate Controller: + Joystick Velocities + [0] = l/r left joystick + [1] = u/d left joystick + [2] = l/r right joystick + [3] = u/d right joystick + [4] = left trigger + [5] = right trigger + + Button Activations + [0] = A + [1] = B + [2] = X + [3] = Y + [4] = left bumper + [5] = right bumper + [6] = TV + [7] = Menu + [8] = left joystick button + [9] = right joystick button + """ + + # Left stick: left and right + if(self.axis_data.get(0) < -self.activation): + joystick_vels[0] = ((self.axis_data[0] + self.activation) / (1 - self.activation)) + elif(self.axis_data.get(0) > self.activation): + joystick_vels[0] = ((self.axis_data[0] - self.activation) / (1 - self.activation)) + else: + joystick_vels[0] = 0 + + # Left stick: up and down + axis_1_multiplier = 1 if self.is_jetson else -1 + if(self.axis_data.get(1) < -self.activation): + joystick_vels[1] = axis_1_multiplier * ((self.axis_data[1] + self.activation) / (1 -self.activation)) + elif(self.axis_data.get(1) >self.activation): + joystick_vels[1] = axis_1_multiplier * ((self.axis_data[1] - self.activation) / (1 -self.activation)) + else: + joystick_vels[1] = 0 + + # Right stick: left and right + if(self.axis_data.get(3) < -self.activation): + joystick_vels[2] = ((self.axis_data[3] + self.activation) / (1 -self.activation)) + elif(self.axis_data.get(3) >self.activation): + joystick_vels[2] = ((self.axis_data[3] - self.activation) / (1 -self.activation)) + else: + joystick_vels[2] = 0 + + # Right stick: up and down + axis_index = 5 if self.is_jetson else 4 + if(self.axis_data.get(axis_index) < -self.activation): + joystick_vels[3] = -((self.axis_data[axis_index] + self.activation) / (1 -self.activation)) + elif(self.axis_data.get(axis_index) > self.activation): + joystick_vels[3] = -((self.axis_data[axis_index] - self.activation) / (1 -self.activation)) + else: + joystick_vels[3] = 0 + + # Left Trigger + if(self.axis_data.get(2) > 0): + joystick_vels[4] = self.axis_data[2] + else: + joystick_vels[4] = 0 + + # Right Trigger + if(self.axis_data.get(5) > 0): + joystick_vels[5] = self.axis_data[5] + else: + joystick_vels[5] = 0 + + # Buttons + for i in range(min(10, len(button_activations))): + button_activations[i] = self.button_data.get(i, False) + else: + print('calibrate:', pprint.pprint(self.axis_data)) # Save current numpy array for joystick and buttons self.previous_axes = joystick_vels From a48080cd3e3598ebe88e11d929b6491a94ec3085 Mon Sep 17 00:00:00 2001 From: OjasPunje Date: Mon, 27 Apr 2026 19:45:39 -0400 Subject: [PATCH 3/3] Swap shoulder and elbow RMD node IDs --- src/description/ros2_control/arm/arm.rmd.ros2_control.xacro | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro b/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro index 7efeaf8e..9ec26156 100644 --- a/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro @@ -11,7 +11,7 @@ - 0x149 + 0x144 100 1.0 150 @@ -22,7 +22,7 @@ - 0x144 + 0x149 100 1.0 150