From 1e9516a4e6ff52c7e83e34b5484c4b90e7602180 Mon Sep 17 00:00:00 2001 From: ConnorN Date: Sat, 21 Feb 2026 14:10:12 +0000 Subject: [PATCH 1/6] fix: talon HW interface for position node --- .../hardware/include/TalonSRXWrapper.hpp | 7 +- .../hardware/src/TalonSRXWrapper.cpp | 149 ++++++++++-------- 2 files changed, 84 insertions(+), 72 deletions(-) diff --git a/src/HW-Devices/hardware/include/TalonSRXWrapper.hpp b/src/HW-Devices/hardware/include/TalonSRXWrapper.hpp index e4d972f8..83b15533 100644 --- a/src/HW-Devices/hardware/include/TalonSRXWrapper.hpp +++ b/src/HW-Devices/hardware/include/TalonSRXWrapper.hpp @@ -41,8 +41,6 @@ class TalonSRXWrapper : public BaseWrapper { private: enum class SensorType { PWM, RELATIVE, ANALOG }; - // Wrap to symmetric range [-range/2, range/2) - static int wrap_symmetric(int x, int range); // Parameters int id_; @@ -50,14 +48,17 @@ class TalonSRXWrapper : public BaseWrapper { double kI_; double kD_; double kF_; + double sensor_offset_ticks_; ctre::phoenix::motorcontrol::ControlMode control_type_; SensorType sensor_type_; int sensor_ticks_; - double sensor_offset_; bool crossover_mode_; bool inverted_; bool invert_sensor_; + bool initialized_; + rclcpp::Time start_time_; rclcpp::Publisher::SharedPtr debug_pub_; + static constexpr double kWaitDurationSec = 5.0; // Talon-specific handle std::shared_ptr talon_controller_; diff --git a/src/HW-Devices/hardware/src/TalonSRXWrapper.cpp b/src/HW-Devices/hardware/src/TalonSRXWrapper.cpp index 4f5a0ea9..f7f77a26 100644 --- a/src/HW-Devices/hardware/src/TalonSRXWrapper.cpp +++ b/src/HW-Devices/hardware/src/TalonSRXWrapper.cpp @@ -8,11 +8,12 @@ TalonSRXWrapper::TalonSRXWrapper(const hardware_interface::ComponentInfo &joint, kF_(0.0), control_type_(ctre::phoenix::motorcontrol::ControlMode::Disabled), sensor_type_(SensorType::RELATIVE), sensor_ticks_(4096), - sensor_offset_(0.0), crossover_mode_(false), inverted_(false), - invert_sensor_(false), debug_pub_(nullptr), talon_controller_(nullptr) { + sensor_offset_ticks_(0), crossover_mode_(false), inverted_(false), + invert_sensor_(false), debug_pub_(nullptr), talon_controller_(nullptr), + initialized_(false) { std::string sensor_type_str; std::string can_interface = "can0"; - + double sensor_offset = 0.0; for (const auto ¶m : joint.parameters) { if (param.first == "can_id") { id_ = std::stoi(param.second); @@ -21,7 +22,7 @@ TalonSRXWrapper::TalonSRXWrapper(const hardware_interface::ComponentInfo &joint, } else if (param.first == "sensor_ticks") { sensor_ticks_ = std::stoi(param.second); } else if (param.first == "sensor_offset") { - sensor_offset_ = std::stod(param.second); + sensor_offset = std::stod(param.second); } else if (param.first == "crossover") { crossover_mode_ = (param.second == "true"); } else if (param.first == "kP") { @@ -86,6 +87,8 @@ TalonSRXWrapper::TalonSRXWrapper(const hardware_interface::ComponentInfo &joint, } else if (sensor_type_str == "analog") { sensor_type_ = SensorType::ANALOG; } + sensor_offset_ticks_ = + static_cast(sensor_offset * sensor_ticks_ / (2.0 * M_PI)); } void TalonSRXWrapper::pub_status() const { @@ -106,41 +109,62 @@ void TalonSRXWrapper::pub_status() const { } void TalonSRXWrapper::write() { + if (!initialized_) { + if ((debug_node_->now() - start_time_).seconds() > kWaitDurationSec) { + initialized_ = true; + read(); + if (control_type_ == motors::ControlMode::Position) { + command_ = position_; + } else { + command_ = 0.0; + } + RCLCPP_INFO(debug_node_->get_logger(), "TalonSRX initialized for id %d", + id_); + } + return; + } double output = 0.0; + if (crossover_mode_ && std::abs(command_) > M_PI) { + RCLCPP_WARN_THROTTLE( + debug_node_->get_logger(), *debug_node_->get_clock(), 1000, + "%s: Command %.2f is outside of [-pi, pi] in crossover mode, which may " + "cause unexpected behavior for id %d", + __FUNCTION__, command_, id_); + } if (control_type_ == motors::ControlMode::Position) { - output = command_ * sensor_ticks_ / (2.0 * M_PI); + if (crossover_mode_) { + double normalized = (command_ + M_PI) / (2.0 * M_PI); + double ticks = normalized * sensor_ticks_; + double ticks_with_offset = ticks + sensor_offset_ticks_; + output = fmod(ticks_with_offset, sensor_ticks_); + if (output < 0) + output += sensor_ticks_; + } else { + output = (command_)*sensor_ticks_ / (2.0 * M_PI) + sensor_offset_ticks_; + } } else if (control_type_ == motors::ControlMode::Velocity) { - output = command_ * sensor_ticks_ / (2.0 * M_PI) / - 10.0; // Convert rad/s to ticks per 100ms + // Talons use d / 100ms as vel + output = (command_ * sensor_ticks_ / (2.0 * M_PI)) / 10.0; } talon_controller_->Set(control_type_, output); } -int TalonSRXWrapper::wrap_symmetric(const int x, const int range) { - assert(range > 0); - - const int half = range / 2; - - int y = (x + half) % range; - if (y < 0) - y += range; - - return y - half; -} - void TalonSRXWrapper::read() { - int raw_ticks = - static_cast(talon_controller_->GetSelectedSensorPosition()); + int raw_position = + talon_controller_->GetSelectedSensorPosition() - sensor_offset_ticks_; if (crossover_mode_) { - // adjust between -sensor_ticks_/2 and sensor_ticks_/2 - raw_ticks = wrap_symmetric(raw_ticks, sensor_ticks_); + raw_position %= sensor_ticks_; + if (raw_position < 0) { + raw_position += sensor_ticks_; + } + double normalized = static_cast(raw_position) / sensor_ticks_; + position_ = normalized * 2.0 * M_PI - M_PI; + } else { + position_ = raw_position * (2.0 * M_PI) / sensor_ticks_; } - position_ = raw_ticks * 2.0 * M_PI / sensor_ticks_; double raw_velocity = talon_controller_->GetSelectedSensorVelocity(); - // Convert raw velocity to rad/s - // The TalonSRX reports velocity in ticks per 100ms, so we need to convert - // it to radians per second. - velocity_ = (raw_velocity / sensor_ticks_) * 2.0 * M_PI * 10; + // Talons use d / 100ms as vel + velocity_ = raw_velocity * 10 * (2.0 * M_PI) / sensor_ticks_; } void TalonSRXWrapper::configure() { @@ -149,7 +173,8 @@ void TalonSRXWrapper::configure() { if (talon_controller_->GetFirmwareVersion() == -1) { RCLCPP_ERROR_THROTTLE( debug_node_->get_logger(), *debug_node_->get_clock(), 500, - "%s: Motor controller not responding, retrying...", __FUNCTION__); + "%s: Motor controller for id %d not responding, retrying...", + __FUNCTION__, id_); continue; } SlotConfiguration slot; @@ -164,62 +189,48 @@ void TalonSRXWrapper::configure() { config.pulseWidthPeriod_EdgesPerRot = sensor_ticks_; config.continuousCurrentLimit = 10.0; config.peakCurrentLimit = 10.0; - config.peakCurrentDuration = 100; // ms + config.peakCurrentDuration = 100; talon_controller_->EnableCurrentLimit(true); - ErrorCode error = - talon_controller_->ConfigAllSettings(config, /*timeout*/ 50); + ErrorCode error = talon_controller_->ConfigAllSettings(config, 50); + if (error != ErrorCode::OK) { RCLCPP_ERROR_THROTTLE( debug_node_->get_logger(), *debug_node_->get_clock(), 500, - "%s: Failed to configure motor controller with code: %d", - __FUNCTION__, error); + "%s: Failed to configure motor controller (%d) with code: %d", + __FUNCTION__, id_, error); continue; } break; } - - int current_ticks = 0; - int offset_ticks = - static_cast(sensor_offset_ * sensor_ticks_ / (2.0 * M_PI)); if (sensor_type_ == SensorType::PWM) { - auto &sc = talon_controller_->GetSensorCollection(); - current_ticks = sc.GetPulseWidthPosition() & 0xFFF; + talon_controller_->ConfigSelectedFeedbackSensor( + TalonSRXFeedbackDevice::CTRE_MagEncoder_Absolute); } else if (sensor_type_ == SensorType::ANALOG) { - auto &sc = talon_controller_->GetSensorCollection(); - current_ticks = sc.GetAnalogIn() & 0xFFF; - } - if (invert_sensor_) { - current_ticks = sensor_ticks_ - current_ticks; - } - if (crossover_mode_) { - current_ticks = wrap_symmetric(current_ticks, sensor_ticks_); - } - int real_ticks = current_ticks - offset_ticks; - - // Talon SRX expects a double for SetSelectedSensorPosition - double ticks_double = static_cast(real_ticks); - // If exactly one of invert_sensor_ or inverted_ is true, negate the ticks. - // This ensures that if both are true (double inversion), the effect cancels - // out. - bool final_invert = invert_sensor_ != inverted_; - if (final_invert) { - ticks_double = -ticks_double; + talon_controller_->ConfigSelectedFeedbackSensor( + TalonSRXFeedbackDevice::Analog); + } else { + talon_controller_->ConfigSelectedFeedbackSensor(FeedbackDevice::QuadEncoder, + 0, 0); } talon_controller_->SetNeutralMode(NeutralMode::Brake); + talon_controller_->ConfigFeedbackNotContinuous(crossover_mode_); talon_controller_->ConfigSelectedFeedbackCoefficient(1.0); - - // The Talons virtualize a QuadEncoder from the PWM signal if no quad encoder - // is present. The absolute part is set at the beginning from the - // SetSelectedSensorPosition - talon_controller_->ConfigSelectedFeedbackSensor(FeedbackDevice::QuadEncoder, - 0, 0); talon_controller_->EnableVoltageCompensation(true); - talon_controller_->SetInverted(inverted_); talon_controller_->SetSensorPhase(invert_sensor_); + talon_controller_->SetInverted(inverted_); + talon_controller_->ConfigPulseWidthPeriod_FilterWindowSz(1); talon_controller_->SelectProfileSlot(0, 0); - talon_controller_->SetSelectedSensorPosition(ticks_double, 0, 50); + talon_controller_->SetStatusFramePeriod( + StatusFrameEnhanced::Status_2_Feedback0, 20); + talon_controller_->SetIntegralAccumulator(0); + read(); + if (control_type_ == motors::ControlMode::Position) { + command_ = position_; + } + talon_controller_->Set(motors::ControlMode::Disabled, 0.0); RCLCPP_INFO(debug_node_->get_logger(), - "%s: Successfully configured Motor Controller %d, with offset %d", - __FUNCTION__, id_, real_ticks); + "%s: Successfully configured Motor Controller %d", __FUNCTION__, + id_); + start_time_ = debug_node_->now(); } \ No newline at end of file From 12f162272592e272b6e33155d8dd448678929f70 Mon Sep 17 00:00:00 2001 From: ConnorN Date: Sat, 21 Feb 2026 14:14:27 +0000 Subject: [PATCH 2/6] feat: joystick control --- .devcontainer/base-station/devcontainer.json | 1 + .../joystick_control/config/pxn.yaml | 19 +- .../joystick_control/include/arm.hpp | 37 ++-- .../launch/controller.launch.py | 62 +++--- .../joystick_control/src/arm.cpp | 209 ++++++++++++++---- 5 files changed, 233 insertions(+), 95 deletions(-) diff --git a/.devcontainer/base-station/devcontainer.json b/.devcontainer/base-station/devcontainer.json index 40f788ca..33e82665 100644 --- a/.devcontainer/base-station/devcontainer.json +++ b/.devcontainer/base-station/devcontainer.json @@ -8,6 +8,7 @@ "--privileged", "--volume=/dev/v4l/by-id:/dev/v4l/by-id", "--volume=/dev/serial/by-id:/dev/serial/by-id", + "--volume=/dev/input/by-id:/dev/input/by-id", "--volume=/sys:/sys", "--volume=/etc/timezone:/etc/timezone", "--volume=/tmp/argus_socket:/tmp/argus_socket", diff --git a/src/Teleop-Control/joystick_control/config/pxn.yaml b/src/Teleop-Control/joystick_control/config/pxn.yaml index 9eda3e15..fdfc2703 100644 --- a/src/Teleop-Control/joystick_control/config/pxn.yaml +++ b/src/Teleop-Control/joystick_control/config/pxn.yaml @@ -1,28 +1,23 @@ drive_teleop_node: ros__parameters: - #TODO change these for an actual PXN forward_axis: 1 strafe_axis: 0 yaw_axis: 2 max_linear: 2.0 max_angular: 1.0 + arm_teleop_node: ros__parameters: throttle: axis: 2 - base_axis: 0 - wrist_roll: 4 + joint_1_axis: 0 + joint_2_axis: 1 + joint_3_axis: 5 + joint_4_axis: 3 wrist_yaw_positive: 3 wrist_yaw_negative: 5 - act1_axis: 1 - act2_axis: 5 - elbow_yaw: 3 - max_base_speed: 1.0 - max_wrist_roll_speed: 1.0 - max_wrist_speed: 1.0 - max_act1_speed: 1.0 - max_act2_speed: 1.0 - max_elbow_yaw_speed: 1.0 + joint_6_axis: 4 + science_teleop_node: ros__parameters: drill_button: 0 diff --git a/src/Teleop-Control/joystick_control/include/arm.hpp b/src/Teleop-Control/joystick_control/include/arm.hpp index ae5a9cf6..88087e50 100644 --- a/src/Teleop-Control/joystick_control/include/arm.hpp +++ b/src/Teleop-Control/joystick_control/include/arm.hpp @@ -2,39 +2,48 @@ #define ARM_HPP #include "control_msgs/msg/joint_jog.hpp" -#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" #include "interfaces/srv/move_servo.hpp" #include "rclcpp/rclcpp.hpp" +#include "ros_phoenix/msg/motor_control.hpp" #include "sensor_msgs/msg/joy.hpp" #include "std_msgs/msg/float32.hpp" class arm : public rclcpp::Node { public: arm(); - void arm_control(std::shared_ptr joystickMsg); private: + void arm_control(std::shared_ptr joystickMsg); + void manual_arm_control(std::shared_ptr joystickMsg); + void ik_arm_control(std::shared_ptr joystickMsg); + void endeffector_control(std::shared_ptr joystickMsg); + bool moveit_servo_state(bool enable); rclcpp::Subscription::SharedPtr joy_sub_; rclcpp::Publisher::SharedPtr joint_pub_; - control_msgs::msg::JointJog joint_msg_; + rclcpp::Publisher::SharedPtr ik_pub_; + rclcpp::Publisher::SharedPtr eef_pub_; + enum ArmState { NONE = 0, MANUAL, IK }; + + ArmState current_state_; void declareParameters(); void loadParameters(); + bool initialized_ = false; - double kMaxBaseSpeed; - double kMaxWristRollSpeed; - double kMaxWristSpeed; - double kMaxAct1Speed; - double kMaxAct2Speed; - double kMaxElbowYawSpeed; int kThrottleAxis; - int kBaseAxis; - int kWristRoll; + int kJoint1Axis; + int kJoint2Axis; + int kJoint3Axis; + int kJoint4Axis; + int kJoint6Axis; int kWristYaw_positive; int kWristYaw_negative; - int kAct1Axis; - int kAct2Axis; - int kElbowYaw; + int kDisableButton; + int kIkButton; + int kManualButton; + int kClawOpen; + int kClawClose; }; #endif \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/launch/controller.launch.py b/src/Teleop-Control/joystick_control/launch/controller.launch.py index 8fdc4989..0b473d96 100644 --- a/src/Teleop-Control/joystick_control/launch/controller.launch.py +++ b/src/Teleop-Control/joystick_control/launch/controller.launch.py @@ -2,11 +2,13 @@ import launch_ros.actions from launch_ros.actions import Node from launch import LaunchDescription +from launch.conditions import IfCondition +from launch.substitutions import PythonExpression import os from ament_index_python.packages import get_package_share_directory -def get_joy_id(dev_path, default_id): +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 @@ -16,44 +18,42 @@ def get_joy_id(dev_path, default_id): print(f"Found symlink {dev_path} -> {real_path}") return int("".join(filter(str.isdigit, real_path))) except ValueError: - print( - f"Could not parse joystick ID from {real_path}, using default {default_id}" - ) - return default_id + print(f"ERROR: Device {real_path} not a event joystick path") + return -1 else: - print(f"Symlink {dev_path} does not exist, using default ID {default_id}") - return default_id + 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 - # We use 0 and 1 as defaults if the symlinks aren't found - id_a = get_joy_id("/dev/input/by-id/usb-LiteStar_PXN-2113_Pro-joystick", 1) - id_b = get_joy_id("/dev/input/by-id/usb-Thrustmaster_T.1600M-joystick", 0) + 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") 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_a", + name="joy_node_arm", + condition=IfCondition(PythonExpression([str(id_arm), " >= 0"])), parameters=[ { - "device_id": id_a, - "deadzone": 0.05, - } - ], - remappings=[("/joy", "/drive/joy")], - ), - Node( - package="joy", - executable="joy_node", - name="joy_node_b", - parameters=[ - { - "device_id": id_b, + "device_id": id_arm, "deadzone": 0.05, } ], @@ -66,12 +66,12 @@ def generate_launch_description(): parameters=[parameters_file], remappings=[("/joy", "/arm/joy")], ), - Node( - package="joystick_control", - executable="drive", - name="drive_teleop_node", - parameters=[parameters_file], - remappings=[("/joy", "/drive/joy")], - ), + # Node( + # package="joystick_control", + # executable="drive", + # name="drive_teleop_node", + # parameters=[parameters_file], + # remappings=[("/joy", "/drive/joy")], + # ), ] ) diff --git a/src/Teleop-Control/joystick_control/src/arm.cpp b/src/Teleop-Control/joystick_control/src/arm.cpp index 3a0c6137..8771f21e 100644 --- a/src/Teleop-Control/joystick_control/src/arm.cpp +++ b/src/Teleop-Control/joystick_control/src/arm.cpp @@ -1,72 +1,205 @@ #include "arm.hpp" +#include "controller_manager_msgs/srv/switch_controller.hpp" +#include "std_srvs/srv/trigger.hpp" arm::arm() : Node("arm_node") { declareParameters(); loadParameters(); + current_state_ = NONE; joy_sub_ = this->create_subscription( "/joy", 10, std::bind(&arm::arm_control, this, std::placeholders::_1)); joint_pub_ = this->create_publisher( "/servo_node/delta_joint_cmds", 10); - joint_msg_ = control_msgs::msg::JointJog(); - joint_msg_.joint_names = {"Joint_1", "Joint_2", "Joint_3", - "Joint_4", "Joint_5", "Joint_6"}; + ik_pub_ = this->create_publisher( + "/servo_node/delta_twist_cmds", 10); + eef_pub_ = this->create_publisher( + "/end_effector/set", 10); RCLCPP_INFO(this->get_logger(), "Arm controller started"); } +void arm::endeffector_control( + std::shared_ptr joystickMsg) { + auto &buttons = joystickMsg->buttons; + auto eef_control_msg = ros_phoenix::msg::MotorControl(); + eef_control_msg.mode = ros_phoenix::msg::MotorControl::PERCENT_OUTPUT; + // set the value - close is negative + double value = 0.0; + if (buttons[kClawClose]) { + value = -0.6; + } else if (buttons[kClawOpen]) { + value = 0.5; + } + eef_control_msg.value = value; + eef_pub_->publish(eef_control_msg); +} + +bool arm::moveit_servo_state(bool enable) { + constexpr int attempts = 3; + + auto switch_client = + this->create_client( + "/controller_manager/switch_controller"); + auto switch_request = std::make_shared< + controller_manager_msgs::srv::SwitchController::Request>(); + if (enable) { + switch_request->start_controllers.push_back("arm_controller_servo"); + switch_request->stop_controllers.push_back("arm_controller_move_group"); + } else { + switch_request->start_controllers.push_back("arm_controller_move_group"); + switch_request->stop_controllers.push_back("arm_controller_servo"); + } + switch_request->strictness = + controller_manager_msgs::srv::SwitchController::Request::STRICT; + switch_request->start_asap = true; + auto future_result = switch_client->async_send_request( + switch_request, + [this](rclcpp::Client:: + SharedFuture future) { + try { + auto result = future.get(); + if (result->ok) { + RCLCPP_INFO(this->get_logger(), + "Controller switched successfully!"); + } else { + RCLCPP_ERROR(this->get_logger(), "Controller switch failed!"); + } + } catch (const std::exception &e) { + RCLCPP_ERROR(this->get_logger(), "Service call failed: %s", e.what()); + } + }); + + std::string moveit_service_name = + enable ? "/servo_node/start_servo" : "/servo_node/stop_servo"; + if (enable) { + RCLCPP_INFO(this->get_logger(), "Enabling MoveIt Servo"); + } else { + RCLCPP_INFO(this->get_logger(), "Disabling MoveIt Servo"); + } + auto moveit_client = + this->create_client(moveit_service_name.c_str()); + int i = 0; + while (!moveit_client->wait_for_service(std::chrono::milliseconds(500))) { + RCLCPP_WARN(this->get_logger(), "Service not available, Waiting..."); + if (++i >= attempts) { + RCLCPP_ERROR(this->get_logger(), + "Service not available after %d attempts. Giving up", i); + return false; + } + } + auto request = std::make_shared(); + moveit_client->async_send_request(request); + return true; +} void arm::arm_control(std::shared_ptr joystickMsg) { - auto joint_msg_ = control_msgs::msg::JointJog(); + auto &buttons = joystickMsg->buttons; + if (!initialized_) { + if (std::abs(joystickMsg->axes[kJoint1Axis]) < 0.01 && + std::abs(joystickMsg->axes[kJoint2Axis]) < 0.01 && + std::abs(joystickMsg->axes[kJoint3Axis]) < 0.01 && + std::abs(joystickMsg->axes[kJoint4Axis]) < 0.01 && + std::abs(joystickMsg->axes[kJoint6Axis]) < 0.01) { + initialized_ = true; + } + return; + } + + if (buttons[kDisableButton] && current_state_ != NONE) { + current_state_ = NONE; + moveit_servo_state(false); + RCLCPP_INFO(this->get_logger(), "Arm disabled"); + return; + } else if (buttons[kIkButton] && current_state_ != IK) { + current_state_ = IK; + moveit_servo_state(true); + RCLCPP_INFO(this->get_logger(), "Switched to IK control"); + } else if (buttons[kManualButton] && current_state_ != MANUAL) { + current_state_ = MANUAL; + moveit_servo_state(true); + RCLCPP_INFO(this->get_logger(), "Switched to manual control"); + } + + switch (current_state_) { + case MANUAL: + manual_arm_control(joystickMsg); + break; + case IK: + ik_arm_control(joystickMsg); + break; + default: + break; + } +} + +void arm::manual_arm_control( + std::shared_ptr joystickMsg) { + auto joint_msg = control_msgs::msg::JointJog(); + joint_msg.joint_names = {"Joint_1", "Joint_2", "Joint_3", + "Joint_4", "Joint_5", "Joint_6"}; + + auto &axes = joystickMsg->axes; + auto &buttons = joystickMsg->buttons; + + joint_msg.header.stamp = joystickMsg->header.stamp; + + joint_msg.velocities = {axes[kJoint1Axis], + axes[kJoint2Axis], + axes[kJoint3Axis], + axes[kJoint4Axis], + static_cast(buttons[kWristYaw_positive] - + buttons[kWristYaw_negative]), + axes[kJoint6Axis]}; + + joint_pub_->publish(joint_msg); +} - auto axes = joystickMsg->axes; - auto buttons = joystickMsg->buttons; +void arm::ik_arm_control(std::shared_ptr joystickMsg) { + auto twist_msg = geometry_msgs::msg::TwistStamped(); + twist_msg.header.stamp = joystickMsg->header.stamp; - joint_msg_.header = joystickMsg->header; + auto &axes = joystickMsg->axes; + auto &buttons = joystickMsg->buttons; - joint_msg_.velocities = {axes[kBaseAxis] * kMaxBaseSpeed, - axes[kAct1Axis] * kMaxAct1Speed, - axes[kAct2Axis] * kMaxAct2Speed, - axes[kWristRoll] * kMaxWristRollSpeed, - axes[kElbowYaw] * kMaxElbowYawSpeed, - buttons[kWristYaw_positive] - - buttons[kWristYaw_negative] * kMaxWristSpeed * - axes[kThrottleAxis]}; + twist_msg.twist.linear.x = axes[kJoint2Axis]; + twist_msg.twist.linear.y = axes[kJoint1Axis]; + twist_msg.twist.linear.z = axes[kJoint3Axis]; + twist_msg.twist.angular.x = axes[kJoint4Axis]; + twist_msg.twist.angular.y = axes[kJoint6Axis]; + twist_msg.twist.angular.z = + (static_cast(buttons[kWristYaw_positive] - + buttons[kWristYaw_negative])) * + axes[kThrottleAxis]; - joint_pub_->publish(joint_msg_); + ik_pub_->publish(twist_msg); } void arm::declareParameters() { this->declare_parameter("throttle.axis", 2); - this->declare_parameter("base_axis", 0); - this->declare_parameter("wrist_roll", 4); + this->declare_parameter("joint_1_axis", 0); + this->declare_parameter("joint_2_axis", 1); + this->declare_parameter("joint_3_axis", 5); + this->declare_parameter("joint_4_axis", 3); + this->declare_parameter("joint_6_axis", 4); this->declare_parameter("wrist_yaw_positive", 3); this->declare_parameter("wrist_yaw_negative", 5); - this->declare_parameter("act1_axis", 1); - this->declare_parameter("act2_axis", 5); - this->declare_parameter("elbow_yaw", 6); - this->declare_parameter("max_base_speed", 1.0); - this->declare_parameter("max_wrist_roll_speed", 1.0); - this->declare_parameter("max_wrist_speed", 1.0); - this->declare_parameter("max_act1_speed", 1.0); - this->declare_parameter("max_act2_speed", 1.0); - this->declare_parameter("max_elbow_yaw_speed", 1.0); + this->declare_parameter("ik_button", 10); + this->declare_parameter("manual_button", 11); + this->declare_parameter("disable_button", 9); } void arm::loadParameters() { this->get_parameter("throttle.axis", kThrottleAxis); - this->get_parameter("base_axis", kBaseAxis); - this->get_parameter("wrist_roll", kWristRoll); + this->get_parameter("joint_1_axis", kJoint1Axis); + this->get_parameter("joint_2_axis", kJoint2Axis); + this->get_parameter("joint_3_axis", kJoint3Axis); + this->get_parameter("joint_4_axis", kJoint4Axis); + this->get_parameter("joint_6_axis", kJoint6Axis); this->get_parameter("wrist_yaw_positive", kWristYaw_positive); this->get_parameter("wrist_yaw_negative", kWristYaw_negative); - this->get_parameter("act1_axis", kAct1Axis); - this->get_parameter("act2_axis", kAct2Axis); - this->get_parameter("elbow_yaw", kElbowYaw); - this->get_parameter("max_base_speed", kMaxBaseSpeed); - this->get_parameter("max_wrist_roll_speed", kMaxWristRollSpeed); - this->get_parameter("max_wrist_speed", kMaxWristSpeed); - this->get_parameter("max_act1_speed", kMaxAct1Speed); - this->get_parameter("max_act2_speed", kMaxAct2Speed); - this->get_parameter("max_elbow_yaw_speed", kMaxElbowYawSpeed); + this->get_parameter("ik_button", kIkButton); + this->get_parameter("manual_button", kManualButton); + this->get_parameter("disable_button", kDisableButton); } int main(int argc, char **argv) { From c90e0e79dd1405d0df0f43d5a6cb3fa39dab9ba2 Mon Sep 17 00:00:00 2001 From: ConnorN Date: Sat, 21 Feb 2026 14:16:04 +0000 Subject: [PATCH 3/6] feat: custom position vs trajectory mode --- .../src/odrive_hardware_interface.cpp | 36 +++++++++++++++---- src/URDF/rover_urdf/urdf/arm_urdf.xacro | 20 +++++++---- .../urdf/chassis_urdf.ros2_control.xacro | 4 +++ src/URDF/rover_urdf/urdf/chassis_urdf.xacro | 2 +- 4 files changed, 47 insertions(+), 15 deletions(-) diff --git a/src/HW-Devices/ros_odrive/odrive_ros2_control/src/odrive_hardware_interface.cpp b/src/HW-Devices/ros_odrive/odrive_ros2_control/src/odrive_hardware_interface.cpp index 0c43b3c6..5576c168 100644 --- a/src/HW-Devices/ros_odrive/odrive_ros2_control/src/odrive_hardware_interface.cpp +++ b/src/HW-Devices/ros_odrive/odrive_ros2_control/src/odrive_hardware_interface.cpp @@ -49,8 +49,10 @@ class ODriveHardwareInterface final }; struct Axis { - Axis(SocketCanIntf *can_intf, uint32_t node_id, double multiplier) - : can_intf_(can_intf), node_id_(node_id), multiplier_(multiplier) {} + Axis(SocketCanIntf *can_intf, uint32_t node_id, double multiplier, + ODriveInputMode input_mode) + : can_intf_(can_intf), node_id_(node_id), multiplier_(multiplier), + input_mode_(input_mode) {} void on_can_msg(const rclcpp::Time ×tamp, const can_frame &frame); @@ -93,6 +95,8 @@ struct Axis { bool vel_input_enabled_ = false; bool torque_input_enabled_ = false; + ODriveInputMode input_mode_; + template bool send(const T &msg) const { struct can_frame frame; frame.can_id = node_id_ << 5 | msg.cmd_id; @@ -133,13 +137,31 @@ ODriveHardwareInterface::on_init(const hardware_interface::HardwareInfo &info) { double multiplier = 1.0; if (joint.parameters.find("multiplier") != joint.parameters.end()) { multiplier = std::stod(joint.parameters.at("multiplier")); - RCLCPP_ERROR(rclcpp::get_logger("ODriveHardwareInterface"), - "Setting Joint %s multiplier to %f'", joint.name.c_str(), - multiplier); + RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), + "Setting Joint %s multiplier to %f'", joint.name.c_str(), + multiplier); + } + ODriveInputMode input_mode = INPUT_MODE_PASSTHROUGH; + if (joint.parameters.find("input_mode") != joint.parameters.end()) { + const std::string input_mode_str = joint.parameters.at("input_mode"); + if (input_mode_str == "Traj") { + input_mode = INPUT_MODE_TRAP_TRAJ; + RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), + "Setting Joint %s input type to Trajectory", + joint.name.c_str()); + } else if (input_mode_str == "Passthrough") { + input_mode = INPUT_MODE_PASSTHROUGH; + RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), + "Setting Joint %s input type to Passthrough", + joint.name.c_str()); + } else { + RCLCPP_WARN(rclcpp::get_logger("ODriveHardwareInterface"), + "Unknown input type %s on Joint %s", input_mode_str.c_str(), + joint.name.c_str()); + } } - axes_.emplace_back(&can_intf_, std::stoi(joint.parameters.at("node_id")), - multiplier); + multiplier, input_mode); } return CallbackReturn::SUCCESS; diff --git a/src/URDF/rover_urdf/urdf/arm_urdf.xacro b/src/URDF/rover_urdf/urdf/arm_urdf.xacro index 4b3a6be3..9714cb7a 100644 --- a/src/URDF/rover_urdf/urdf/arm_urdf.xacro +++ b/src/URDF/rover_urdf/urdf/arm_urdf.xacro @@ -108,7 +108,7 @@ + type="revolute"> @@ -117,7 +117,8 @@ + xyz="0 0 -1" /> + @@ -212,7 +213,7 @@ + type="revolute"> @@ -222,6 +223,7 @@ link="Link_3" /> + @@ -274,7 +276,7 @@ + type="revolute"> @@ -284,6 +286,7 @@ link="Link_4" /> + @@ -348,7 +351,7 @@ + type="revolute"> @@ -358,6 +361,7 @@ link="Link_5" /> + @@ -416,7 +420,7 @@ + type="revolute"> @@ -425,7 +429,8 @@ + xyz="0 -1 0" /> + @@ -494,6 +499,7 @@ link="EndEffector" /> + diff --git a/src/URDF/rover_urdf/urdf/chassis_urdf.ros2_control.xacro b/src/URDF/rover_urdf/urdf/chassis_urdf.ros2_control.xacro index 68f30247..3dad3371 100644 --- a/src/URDF/rover_urdf/urdf/chassis_urdf.ros2_control.xacro +++ b/src/URDF/rover_urdf/urdf/chassis_urdf.ros2_control.xacro @@ -40,23 +40,27 @@ 2 -0.0166666667 + Traj 3 0.0166666667 + Traj 1 + Traj 0.0166666667 4 + Traj -0.0166666667 diff --git a/src/URDF/rover_urdf/urdf/chassis_urdf.xacro b/src/URDF/rover_urdf/urdf/chassis_urdf.xacro index a5e8aa16..1d2436be 100644 --- a/src/URDF/rover_urdf/urdf/chassis_urdf.xacro +++ b/src/URDF/rover_urdf/urdf/chassis_urdf.xacro @@ -801,7 +801,7 @@ name="Left_back_wheel_arm_joint" type="continuous"> From 869beaf5e1f90314bdf9158807ed8766a1afeef1 Mon Sep 17 00:00:00 2001 From: ConnorN Date: Sat, 21 Feb 2026 14:19:48 +0000 Subject: [PATCH 4/6] feat: arm urdf and tuning --- src/Arm/arm_control/config/arm_config.yaml | 8 +- src/Arm/arm_control/config/moveit.rviz | 420 +++++++++++++++++- src/Arm/arm_control/launch/servo.launch.py | 2 + .../urdf/arm_urdf.ros2_control.xacro | 96 ++-- 4 files changed, 465 insertions(+), 61 deletions(-) diff --git a/src/Arm/arm_control/config/arm_config.yaml b/src/Arm/arm_control/config/arm_config.yaml index 1cafc4ac..b006417a 100644 --- a/src/Arm/arm_control/config/arm_config.yaml +++ b/src/Arm/arm_control/config/arm_config.yaml @@ -8,9 +8,7 @@ use_gazebo: false ## Properties of outgoing commands -publish_period: 0.08 # 1/Nominal publish rate [seconds] supposed to be 0.08 -publish_delay: 0.005 #supposed to be 0.005 -max_expected_latency: 0.1 # delay between sending a command and the robot executing it [seconds] +publish_period: 0.08 # 1/Nominal publish rate [seconds] low_latency_mode: false command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s @@ -30,8 +28,6 @@ publish_joint_positions: true publish_joint_velocities: false publish_joint_accelerations: false -#low_pass_filter_coeff: 2.0 - ## Plugins for smoothing outgoing commands smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" @@ -73,7 +69,7 @@ status_topic: ~/status # Publish status to this topic command_out_topic: /arm_controller/joint_trajectory # Publish outgoing commands here ## Collision checking for the entire robot body -check_collisions: true # Check collisions? +check_collisions: false # Check collisions? collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m] scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m] diff --git a/src/Arm/arm_control/config/moveit.rviz b/src/Arm/arm_control/config/moveit.rviz index 99bb740f..9f36967e 100644 --- a/src/Arm/arm_control/config/moveit.rviz +++ b/src/Arm/arm_control/config/moveit.rviz @@ -1,51 +1,453 @@ Panels: - Class: rviz_common/Displays + Help Height: 70 Name: Displays Property Tree Widget: Expanded: - /MotionPlanning1 + Splitter Ratio: 0.5 + Tree Height: 163 - Class: rviz_common/Help Name: Help - Class: rviz_common/Views + Expanded: + - /Current View1 Name: Views + Splitter Ratio: 0.5 Visualization Manager: + Class: "" Displays: - - Class: rviz_default_plugins/Grid + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: Value: true - - Class: moveit_rviz_plugin/MotionPlanning + - Acceleration_Scaling_Factor: 1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 Name: MotionPlanning Planned Path: - Loop Animation: true + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Antenna_Back: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Antenna_Front: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + EndEffector: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + GPS_Left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + GPS_Right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + Link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Rocker_Left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Rocker_Right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Sus_Arm_Back_Left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Sus_Arm_Back_Right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Sus_Arm_Front_Left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Sus_Arm_Front_Right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Wheel_Arm_Back_Left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Wheel_Arm_Back_Right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Wheel_Arm_Front_Left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Wheel_Arm_Front_Right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Wheel_Back_Left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Wheel_Back_Right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Wheel_Front_Left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Wheel_Front_Right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Zed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Robot Alpha: 0.20000000298023224 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false State Display Time: 0.05 s + Trail Step Size: 1 Trajectory Topic: display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 Planning Scene Topic: monitored_planning_scene Robot Description: robot_description Scene Geometry: Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Antenna_Back: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Antenna_Front: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + EndEffector: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + GPS_Left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + GPS_Right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + Link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Rocker_Left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Rocker_Right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Sus_Arm_Back_Left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Sus_Arm_Back_Right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Sus_Arm_Front_Left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Sus_Arm_Front_Right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Wheel_Arm_Back_Left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Wheel_Arm_Back_Right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Wheel_Arm_Front_Left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Wheel_Arm_Front_Right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Wheel_Back_Left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Wheel_Back_Right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Wheel_Front_Left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Wheel_Front_Right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Zed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true Value: true + Velocity_Scaling_Factor: 1 + Enabled: true Global Options: + Background Color: 48; 48; 48 Fixed Frame: base_link + Frame Rate: 30 + Name: root Tools: - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true - Class: rviz_default_plugins/MoveCamera - Class: rviz_default_plugins/Select + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: Class: rviz_default_plugins/Orbit - Distance: 2.0 + Distance: 2.6132895946502686 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false Focal Point: - X: -0.1 + X: -0.10000000149011612 Y: 0.25 - Z: 0.30 + Z: 0.30000001192092896 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false Name: Current View - Pitch: 0.5 + Near Clip Distance: 0.009999999776482582 + Pitch: -0.13999998569488525 Target Frame: base_link - Yaw: -0.623 + Value: Orbit (rviz_default_plugins) + Yaw: 5.565185070037842 + Saved: ~ Window Geometry: + Displays: + collapsed: false Height: 975 - QMainWindow State: 000000ff00000000fd0000000100000000000002b400000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d00000123000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000166000001910000018800fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fd000000b5000000a400ffffff000001f60000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000002b400000379fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00fffffffb000000100044006900730070006c006100790073010000003b00000124000000c700fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000165000001930000016900fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fe000000b6000000a000ffffff000001f60000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false Width: 1200 + X: 120 + Y: 27 diff --git a/src/Arm/arm_control/launch/servo.launch.py b/src/Arm/arm_control/launch/servo.launch.py index 0ffed9f5..479f5b36 100644 --- a/src/Arm/arm_control/launch/servo.launch.py +++ b/src/Arm/arm_control/launch/servo.launch.py @@ -54,6 +54,7 @@ def arm_launch(moveit_config, launch_package_path=None) -> LaunchDescription: moveit_config.joint_limits, moveit_config.trajectory_execution, moveit_config.planning_scene_monitor, + {"publish_robot_description_semantic": True}, ] move_group = Node( @@ -79,6 +80,7 @@ def arm_launch(moveit_config, launch_package_path=None) -> LaunchDescription: { "moveit_servo": servo_yaml, "publish_frequency": 15.0, + "butterworth_filter_coeff": 10.0, }, moveit_config.robot_description_semantic, moveit_config.robot_description_kinematics, diff --git a/src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro b/src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro index 01d60b56..8ac90407 100644 --- a/src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro +++ b/src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro @@ -4,70 +4,70 @@ - mock_components/GenericSystem - + + ros2_control_rover_arm/RoverArmHardwareInterface TalonSRX - 10 - 3.0 + 14 + 5.0 0.0 1.0 - 0.5 + 0.0 absolute 4096 - 0.0 + 1.57 true false - true + false - + TalonSRX 11 - 10.0 - 0.0 - 10.0 - 100 + 4.0 + 0.1 + 0.5 + 0.0 absolute 4096 - -2.7 + -1.29154 true false - true + false - + TalonSRX - 14 + 12 1.0 0.0 0.0 - 60.0 + 0.0 absolute 4096 - 3.14 + 1.6057 true true - false + true - + TalonSRX - 15 + 19 2.0 - 0.0 - 0.0 - 5.0 + 0.1 + 0.2 + 0.0 absolute 4096 - -1.0 + 0.191986 true false false @@ -75,40 +75,44 @@ - + TalonSRX - 12 - 20.0 + 15 + 0.15 0.0 0.0 - 7.8 - absolute - 4096 - -0.23 - true + 0.0 + quadrature + 2039808 + 0.0 + true true true - - TalonSRX - 13 - 0.012 - 0.0 - 0.0 - 0.005 - quadrature - 2163000 - 0.0 - true - true - true + + + + + + odrive_ros2_control_plugin/ODriveHardwareInterface + can0 + + + 10 + -1.0 + + + + mock_components/GenericSystem + + From 0bcd541ec4b318f91ca6fb37e24d3cf48bcd304b Mon Sep 17 00:00:00 2001 From: ConnorN Date: Sat, 21 Feb 2026 14:21:11 +0000 Subject: [PATCH 5/6] feat: use position pass forward controller for arm --- rosdep-keys.txt | 1 + src/Arm/arm_control/config/arm_config.yaml | 4 +-- .../config/moveit_controllers.yaml | 4 +-- src/Arm/arm_control/package.xml | 3 +- src/Bringup/launch/control.launch.py | 30 +++++++++++++++++-- .../joystick_control/CMakeLists.txt | 2 ++ .../rover_urdf/config/ros2_controllers.yaml | 22 ++++++++++++-- 7 files changed, 56 insertions(+), 10 deletions(-) diff --git a/rosdep-keys.txt b/rosdep-keys.txt index c79763d3..4a9c4743 100644 --- a/rosdep-keys.txt +++ b/rosdep-keys.txt @@ -78,6 +78,7 @@ joint_trajectory_controller diff_drive_controller joint_state_publisher_gui libjsoncpp +controller_manager_msgs flex bison libncurses-dev diff --git a/src/Arm/arm_control/config/arm_config.yaml b/src/Arm/arm_control/config/arm_config.yaml index b006417a..7f6fe134 100644 --- a/src/Arm/arm_control/config/arm_config.yaml +++ b/src/Arm/arm_control/config/arm_config.yaml @@ -21,7 +21,7 @@ scale: # What type of topic does your robot driver expect? # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory -command_out_type: trajectory_msgs/JointTrajectory +command_out_type: std_msgs/Float64MultiArray # What to publish? Can save some bandwidth as most robots only require positions or velocities publish_joint_positions: true @@ -66,7 +66,7 @@ cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian t joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands joint_topic: /joint_states # Get joint states from this tpoic status_topic: ~/status # Publish status to this topic -command_out_topic: /arm_controller/joint_trajectory # Publish outgoing commands here +command_out_topic: /arm_controller_servo/commands # Publish outgoing commands here ## Collision checking for the entire robot body check_collisions: false # Check collisions? diff --git a/src/Arm/arm_control/config/moveit_controllers.yaml b/src/Arm/arm_control/config/moveit_controllers.yaml index d95643aa..c7d374c3 100644 --- a/src/Arm/arm_control/config/moveit_controllers.yaml +++ b/src/Arm/arm_control/config/moveit_controllers.yaml @@ -4,9 +4,9 @@ moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControll moveit_simple_controller_manager: controller_names: - - arm_controller + - arm_controller_move_group - arm_controller: + arm_controller_move_group: type: FollowJointTrajectory joints: - Joint_1 diff --git a/src/Arm/arm_control/package.xml b/src/Arm/arm_control/package.xml index ba50bb4a..ba04c105 100644 --- a/src/Arm/arm_control/package.xml +++ b/src/Arm/arm_control/package.xml @@ -22,15 +22,16 @@ moveit_kinematics moveit_planners moveit_simple_controller_manager + position_controllers joint_state_publisher joint_state_publisher_gui tf2_ros xacro - controller_manager + joint_trajectory_controller moveit_configs_utils moveit_ros_move_group moveit_ros_visualization diff --git a/src/Bringup/launch/control.launch.py b/src/Bringup/launch/control.launch.py index 0ef3d2a5..598e0439 100644 --- a/src/Bringup/launch/control.launch.py +++ b/src/Bringup/launch/control.launch.py @@ -88,21 +88,34 @@ def generate_launch_description(): condition=IfCondition(use_drive), ) - arm_controller_spawner = Node( + arm_controller_move_it_spawner = Node( package="controller_manager", executable="spawner", arguments=[ - "arm_controller", + "arm_controller_move_group", ], parameters=[control_yaml], output="screen", condition=IfCondition(use_arm), ) + arm_controller_servo_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "arm_controller_servo", + "--inactive", + ], + parameters=[control_yaml], + output="screen", + condition=IfCondition(use_arm), + ) + + delayed_spawners = RegisterEventHandler( OnProcessStart( target_action=controller_manager, - on_start=[jsb_spawner, chassis_controller_spawner, arm_controller_spawner], + on_start=[jsb_spawner, chassis_controller_spawner, arm_controller_move_it_spawner, arm_controller_servo_spawner], ) ) @@ -116,6 +129,16 @@ def generate_launch_description(): ), condition=IfCondition(use_arm), ) + eef_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("arm_control"), + "launch", + "end_effector.launch.py", + ) + ), + condition=IfCondition(use_arm), + ) ld = LaunchDescription() ld.add_action(declare_use_arm_cmd) @@ -124,4 +147,5 @@ def generate_launch_description(): ld.add_action(controller_manager) ld.add_action(delayed_spawners) ld.add_action(arm_launch) + ld.add_action(eef_launch) return ld diff --git a/src/Teleop-Control/joystick_control/CMakeLists.txt b/src/Teleop-Control/joystick_control/CMakeLists.txt index 00bd7154..9f6b74ba 100644 --- a/src/Teleop-Control/joystick_control/CMakeLists.txt +++ b/src/Teleop-Control/joystick_control/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(ros_phoenix REQUIRED) find_package(interfaces REQUIRED) find_package(control_msgs REQUIRED) find_package(std_srvs REQUIRED) +find_package(controller_manager_msgs REQUIRED) find_package(std_msgs REQUIRED) set(CMAKE_CXX_STANDARD 14) @@ -51,6 +52,7 @@ ament_target_dependencies(arm interfaces control_msgs std_srvs + controller_manager_msgs ) ament_target_dependencies(science diff --git a/src/URDF/rover_urdf/config/ros2_controllers.yaml b/src/URDF/rover_urdf/config/ros2_controllers.yaml index 29b3c0ee..387d6c46 100644 --- a/src/URDF/rover_urdf/config/ros2_controllers.yaml +++ b/src/URDF/rover_urdf/config/ros2_controllers.yaml @@ -3,7 +3,7 @@ ros__parameters: update_rate: 100 # Hz - arm_controller: + arm_controller_move_group: type: joint_trajectory_controller/JointTrajectoryController joint_state_broadcaster: @@ -11,8 +11,26 @@ chassis_controller: type: swerve_controller/SwerveController + + arm_controller_servo: + type: position_controllers/JointGroupPositionController -arm_controller: +arm_controller_move_group: + ros__parameters: + joints: + - Joint_1 + - Joint_2 + - Joint_3 + - Joint_4 + - Joint_5 + - Joint_6 + command_interfaces: + - position + state_interfaces: + - position + - velocity + +arm_controller_servo: ros__parameters: joints: - Joint_1 From 03e2a953dc8bee53dffd618aacddcc1a7f8c7d01 Mon Sep 17 00:00:00 2001 From: ConnorN Date: Sat, 21 Feb 2026 14:21:24 +0000 Subject: [PATCH 6/6] feat: end effector control --- .../arm_control/launch/end_effector.launch.py | 32 +++++++++++++++++++ 1 file changed, 32 insertions(+) create mode 100644 src/Arm/arm_control/launch/end_effector.launch.py diff --git a/src/Arm/arm_control/launch/end_effector.launch.py b/src/Arm/arm_control/launch/end_effector.launch.py new file mode 100644 index 00000000..ce2e01ad --- /dev/null +++ b/src/Arm/arm_control/launch/end_effector.launch.py @@ -0,0 +1,32 @@ +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + """Generate launch description with multiple components.""" + container = ComposableNodeContainer( + name="PhoenixContainerEEF", + namespace="", + package="ros_phoenix", + executable="phoenix_container", + parameters=[{"interface": "can0"}], + composable_node_descriptions=[ + ComposableNode( + package="ros_phoenix", + plugin="ros_phoenix::TalonSRX", + name="end_effector", + parameters=[ + {"id": 13}, + {"max_voltage": 24.0}, + {"brake_mode": True}, + ], + ), + ], + ) + + return launch.LaunchDescription( + [ + container, + ] + )