Arm tuning pt2

rosdep-keys.txt

@@ -85,4 +85,6 @@ libncurses-dev
 usbutils
 tinyxml2
 clang-tidy
-python3-vcstool
+python3-vcstool
+joy_linux
+position_controllers

src/Arm/arm_control/launch/end_effector.launch.py

@@ -17,8 +17,9 @@ def generate_launch_description():
                 plugin="ros_phoenix::TalonSRX",
                 name="end_effector",
                 parameters=[
-                    {"id": 13},
+                    {"id": 21},
                     {"max_voltage": 24.0},
+                    {"max_current": 6.0},
                     {"brake_mode": True},
                 ],
             ),

src/Bringup/launch/control.launch.py

@@ -111,11 +111,15 @@ def generate_launch_description():
         condition=IfCondition(use_arm),
     )
 
-
     delayed_spawners = RegisterEventHandler(
         OnProcessStart(
             target_action=controller_manager,
-            on_start=[jsb_spawner, chassis_controller_spawner, arm_controller_move_it_spawner, arm_controller_servo_spawner],
+            on_start=[
+                jsb_spawner,
+                chassis_controller_spawner,
+                arm_controller_move_it_spawner,
+                arm_controller_servo_spawner,
+            ],
         )
     )
 

src/Cameras/video_streaming/config/cameras.yaml

@@ -11,7 +11,7 @@ input_node:
       path: "/dev/v4l/by-id/drive_camera"
       type: 0
     EndEffector:
-      path: "/dev/v4l/by-id/usb-Sonix_Technology_Co.__Ltd._USB_2.0_Camera_SN5100-video-index0"
+      path: "/dev/v4l/by-id/usb-MACROSILICON_USB_Video_20200909-video-index0"
       type: 0
       encoded: true
     Bottom:

src/HW-Devices/hardware/src/TalonSRXWrapper.cpp

@@ -123,6 +123,15 @@ void TalonSRXWrapper::write() {
     }
     return;
   }
+  if (std::abs(command_ - position_) > M_PI &&
+      control_type_ == motors::ControlMode::Position) {
+    RCLCPP_WARN_THROTTLE(
+        debug_node_->get_logger(), *debug_node_->get_clock(), 1000,
+        "%s: Large position error (%.2f) for id %d, which may indicate a "
+        "problem with sensor configuration or an unexpected jump in position",
+        __FUNCTION__, command_ - position_, id_);
+    return;
+  }
   double output = 0.0;
   if (crossover_mode_ && std::abs(command_) > M_PI) {
     RCLCPP_WARN_THROTTLE(
@@ -140,7 +149,7 @@ void TalonSRXWrapper::write() {
       if (output < 0)
         output += sensor_ticks_;
     } else {
-      output = (command_)*sensor_ticks_ / (2.0 * M_PI) + sensor_offset_ticks_;
+      output = command_ * sensor_ticks_ / (2.0 * M_PI) + sensor_offset_ticks_;
     }
   } else if (control_type_ == motors::ControlMode::Velocity) {
     // Talons use d / 100ms as vel
@@ -229,6 +238,7 @@ void TalonSRXWrapper::configure() {
     command_ = position_;
   }
   talon_controller_->Set(motors::ControlMode::Disabled, 0.0);
+  talon_controller_->ClearStickyFaults();
   RCLCPP_INFO(debug_node_->get_logger(),
               "%s: Successfully configured Motor Controller %d", __FUNCTION__,
               id_);

src/Teleop-Control/joystick_control/launch/controller.launch.py

@@ -8,70 +8,59 @@
 from ament_index_python.packages import get_package_share_directory
 
 
-def get_joy_id(dev_path):
-    """Checks for a symlink and returns the integer ID, or a default."""
-    if os.path.exists(dev_path):
-        # Resolves /dev/joy_a -> /dev/input/jsX
-        real_path = os.path.realpath(dev_path)
-        # Pulls the digits out of the path string
-        try:
-            print(f"Found symlink {dev_path} -> {real_path}")
-            return int("".join(filter(str.isdigit, real_path)))
-        except ValueError:
-            print(f"ERROR: Device {real_path} not a event joystick path")
-            return -1
-    else:
-        print(f"ERROR: Symlink {dev_path} does not exist")
-        return -1
-
-
 def generate_launch_description():
     pkg_joystick_control = get_package_share_directory("joystick_control")
     parameters_file = os.path.join(pkg_joystick_control, "pxn.yaml")
     # Detect IDs dynamically
-    id_arm = get_joy_id("/dev/input/by-id/usb-LiteStar_PXN-2113_Pro-joystick")
-    id_drive = get_joy_id("/dev/input/by-id/usb-Thrustmaster_T.1600M-joystick")
+    arm_dev = "/dev/input/by-id/usb-LiteStar_PXN-2113_Pro-joystick"
+    drive_dev = "/dev/input/by-id/usb-Thrustmaster_T.16000M-joystick"
+
+    ld = LaunchDescription()
 
-    return LaunchDescription(
-        [
-            # Node(
-            #     package="joy",
-            #     executable="joy_node",
-            #     name="joy_node_a",
-            #     parameters=[
-            #         {
-            #             "device_id": id_drive,
-            #             "deadzone": 0.05,
-            #         }
-            #     ],
-            #     remappings=[("/joy", "/drive/joy")],
-            # ),
-            Node(
-                package="joy",
-                executable="joy_node",
-                name="joy_node_arm",
-                condition=IfCondition(PythonExpression([str(id_arm), " >= 0"])),
-                parameters=[
-                    {
-                        "device_id": id_arm,
-                        "deadzone": 0.05,
-                    }
-                ],
-                remappings=[("/joy", "/arm/joy")],
-            ),
-            Node(
-                package="joystick_control",
-                executable="arm",
-                name="arm_teleop_node",
-                parameters=[parameters_file],
-                remappings=[("/joy", "/arm/joy")],
-            ),
-            # Node(
-            #     package="joystick_control",
-            #     executable="drive",
-            #     name="drive_teleop_node",
-            #     parameters=[parameters_file],
-            #     remappings=[("/joy", "/drive/joy")],
-            # ),
-        ]
+    ld.add_action(
+        Node(
+            package="joy_linux",
+            executable="joy_linux_node",
+            name="joy_node_a",
+            parameters=[
+                {
+                    "dev": drive_dev,
+                    "deadzone": 0.05,
+                }
+            ],
+            remappings=[("/joy", "/drive/joy")],
+        )
+    )
+    ld.add_action(
+        Node(
+            package="joy_linux",
+            executable="joy_linux_node",
+            name="joy_node_arm",
+            parameters=[
+                {
+                    "dev": arm_dev,
+                    "deadzone": 0.05,
+                }
+            ],
+            remappings=[("/joy", "/arm/joy")],
+        ),
+    )
+    ld.add_action(
+        Node(
+            package="joystick_control",
+            executable="arm",
+            name="arm_teleop_node",
+            parameters=[parameters_file],
+            remappings=[("/joy", "/arm/joy")],
+        ),
+    )
+    ld.add_action(
+        Node(
+            package="joystick_control",
+            executable="drive",
+            name="drive_teleop_node",
+            parameters=[parameters_file],
+            remappings=[("/joy", "/drive/joy")],
+        ),
     )
+    return ld

src/Teleop-Control/joystick_control/src/arm.cpp

@@ -120,6 +120,9 @@ void arm::arm_control(std::shared_ptr<sensor_msgs::msg::Joy> joystickMsg) {
     moveit_servo_state(true);
     RCLCPP_INFO(this->get_logger(), "Switched to manual control");
   }
+  if (current_state_ != NONE) {
+    endeffector_control(joystickMsg);
+  }
 
   switch (current_state_) {
   case MANUAL:
@@ -144,20 +147,21 @@ void arm::manual_arm_control(
 
   joint_msg.header.stamp = joystickMsg->header.stamp;
 
-  joint_msg.velocities = {axes[kJoint1Axis],
-                          axes[kJoint2Axis],
+  joint_msg.velocities = {-axes[kJoint1Axis],
+                          -axes[kJoint2Axis],
                           axes[kJoint3Axis],
                           axes[kJoint4Axis],
-                          static_cast<double>(buttons[kWristYaw_positive] -
-                                              buttons[kWristYaw_negative]),
-                          axes[kJoint6Axis]};
+                          -static_cast<double>(buttons[kWristYaw_positive] -
+                                               buttons[kWristYaw_negative]),
+                          -axes[kJoint6Axis]};
 
   joint_pub_->publish(joint_msg);
 }
 
 void arm::ik_arm_control(std::shared_ptr<sensor_msgs::msg::Joy> joystickMsg) {
   auto twist_msg = geometry_msgs::msg::TwistStamped();
   twist_msg.header.stamp = joystickMsg->header.stamp;
+  twist_msg.header.frame_id = "Link_6";
 
   auto &axes = joystickMsg->axes;
   auto &buttons = joystickMsg->buttons;
@@ -187,6 +191,8 @@ void arm::declareParameters() {
   this->declare_parameter("ik_button", 10);
   this->declare_parameter("manual_button", 11);
   this->declare_parameter("disable_button", 9);
+  this->declare_parameter("claw_close_button", 0);
+  this->declare_parameter("claw_open_button", 1);
 }
 void arm::loadParameters() {
   this->get_parameter("throttle.axis", kThrottleAxis);
@@ -200,6 +206,8 @@ void arm::loadParameters() {
   this->get_parameter("ik_button", kIkButton);
   this->get_parameter("manual_button", kManualButton);
   this->get_parameter("disable_button", kDisableButton);
+  this->get_parameter("claw_close_button", kClawOpen);
+  this->get_parameter("claw_open_button", kClawClose);
 }
 
 int main(int argc, char **argv) {

src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro

@@ -10,14 +10,14 @@
       <joint name="Joint_1">
         <param name="type">TalonSRX</param>
         <param name="can_id">14</param>
-        <param name="kP">5.0</param>
-        <param name="kI">0.0</param>
+        <param name="kP">11.0</param>
+        <param name="kI">0.0005</param>
         <param name="kD">1.0</param>
         <param name="kF">0.0</param>
         <param name="sensor_type">absolute</param> <!-- or "quadrature" -->
         <param name="sensor_ticks">4096</param>
-        <param name="sensor_offset">1.57</param>
-        <param name="crossover">true</param> <!-- or "continuous" -->
+        <param name="sensor_offset">0.0</param>
+        <param name="crossover">false</param> <!-- or "continuous" -->
         <param name="invert">false</param>
         <param name="invert_sensor">false</param>
         <command_interface name="position"/>
@@ -27,14 +27,14 @@
       <joint name="Joint_3">
         <param name="type">TalonSRX</param>
         <param name="can_id">11</param>
-        <param name="kP">4.0</param>
-        <param name="kI">0.1</param>
-        <param name="kD">0.5</param>
+        <param name="kP">6.0</param>
+        <param name="kI">0.001</param>
+        <param name="kD">0.01</param>
         <param name="kF">0.0</param>
         <param name="sensor_type">absolute</param> <!-- or "quadrature" -->
         <param name="sensor_ticks">4096</param>
-        <param name="sensor_offset">-1.29154</param>
-        <param name="crossover">true</param> <!-- or "continuous" -->
+        <param name="sensor_offset">0.0</param>
+        <param name="crossover">false</param> <!-- or "continuous" -->
         <param name="invert">false</param>
         <param name="invert_sensor">false</param>
         <command_interface name="position"/>
@@ -44,14 +44,14 @@
       <joint name="Joint_4">
         <param name="type">TalonSRX</param>
         <param name="can_id">12</param>
-        <param name="kP">1.0</param>
-        <param name="kI">0.0</param>
+        <param name="kP">4.0</param>
+        <param name="kI">0.001</param>
         <param name="kD">0.0</param>
         <param name="kF">0.0</param>
         <param name="sensor_type">absolute</param> <!-- or "quadrature" -->
         <param name="sensor_ticks">4096</param>
-        <param name="sensor_offset">1.6057</param>
-        <param name="crossover">true</param> <!-- or "continuous" -->
+        <param name="sensor_offset">0.0</param>
+        <param name="crossover">false</param> <!-- or "continuous" -->
         <param name="invert">true</param>
         <param name="invert_sensor">true</param>
         <command_interface name="position"/>
@@ -61,14 +61,14 @@
       <joint name="Joint_5">
         <param name="type">TalonSRX</param>
         <param name="can_id">19</param>
-        <param name="kP">2.0</param>
-        <param name="kI">0.1</param>
-        <param name="kD">0.2</param>
+        <param name="kP">8.5</param>
+        <param name="kI">0.001</param>
+        <param name="kD">0.001</param>
         <param name="kF">0.0</param>
         <param name="sensor_type">absolute</param> <!-- or "quadrature" -->
         <param name="sensor_ticks">4096</param>
-        <param name="sensor_offset">0.191986</param>
-        <param name="crossover">true</param> <!-- or "continuous" -->
+        <param name="sensor_offset">0.0</param>
+        <param name="crossover">false</param> <!-- or "continuous" -->
         <param name="invert">false</param>
         <param name="invert_sensor">false</param>
         <command_interface name="position"/>
@@ -78,8 +78,8 @@
       <joint name="Joint_6">
         <param name="type">TalonSRX</param>
         <param name="can_id">15</param>
-        <param name="kP">0.15</param>
-        <param name="kI">0.0</param>
+        <param name="kP">0.01</param>
+        <param name="kI">0.000001</param>
         <param name="kD">0.0</param>
         <param name="kF">0.0</param>
         <param name="sensor_type">quadrature</param> <!-- or "quadrature" -->

src/URDF/rover_urdf/urdf/arm_urdf.xacro

@@ -118,7 +118,7 @@
         link="Link_2" />
         <axis
         xyz="0 0 -1" />
-        <limit lower="-3.3" upper="3.3" effort="100.0" velocity="0.1"/>
+        <limit lower="-6.3" upper="6.3" effort="100.0" velocity="0.5"/>
     </joint>
     <link
         name="Link_3">
@@ -361,7 +361,7 @@
         link="Link_5" />
         <axis
         xyz="0 0 -1" />
-        <limit lower="-3.14" upper="3.14" effort="100.0" velocity="0.2"/>
+        <limit lower="-3.14" upper="3.14" effort="100.0" velocity="0.8"/>
     </joint>
     <link
         name="Link_6">
@@ -430,7 +430,7 @@
         link="Link_6" />
         <axis
         xyz="0 -1 0" />
-        <limit lower="-1.0" upper="1.0" effort="100.0" velocity="0.1"/>
+        <limit lower="-1.0" upper="1.0" effort="100.0" velocity="0.4"/>
     </joint>
     <link
         name="EndEffector">
@@ -499,7 +499,7 @@
         link="EndEffector" />
         <axis
         xyz="0 0 1" />
-        <limit effort="100.0" velocity="0.3"/>
+        <limit effort="100.0" velocity="0.6"/>
     </joint>
 
   </xacro:macro>