Ojas jointbyjoint xbox

src/description/ros2_control/arm/arm.rmd.ros2_control.xacro

@@ -11,7 +11,7 @@
       </hardware>
 
       <joint name="shoulder_pitch">
-        <param name="node_id">0x149</param>
+        <param name="node_id">0x144</param>
         <param name="gear_ratio">100</param>
         <param name="joint_orientation">1.0</param>
         <param name="operating_velocity">150</param> <!-- motor dps -->
@@ -22,7 +22,7 @@
       </joint>
 
       <joint name="elbow_pitch">
-        <param name="node_id">0x144</param>
+        <param name="node_id">0x149</param>
         <param name="gear_ratio">100</param>
         <param name="joint_orientation">1.0</param>
         <param name="operating_velocity">150</param> <!-- motor dps -->

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:

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
+    fast_mode_multiplier: 10

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,
+  }

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<double> 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<ControllerReferenceMsg>::SharedPtr ref_subscriber_ = nullptr;
   realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerReferenceMsg>> input_ref_;

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 <cmath>
 #include <limits>
 #include <memory>
 #include <string>
@@ -82,6 +83,7 @@ controller_interface::CallbackReturn ManualArmJointByJointController::on_configu
   num_joints = static_cast<int>(params_.position_joints.size()) + static_cast<int>(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)

src/teleop/teleop/package.xml

@@ -7,6 +7,12 @@
   <maintainer email="duttaishan01@todo.todo">duttaishan01</maintainer>
   <license>TODO: License declaration</license>
 
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>rosidl_runtime_py</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>

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