From a3cc2780cf8e7fc72fe245e2c8d1cbc57cf5bd37 Mon Sep 17 00:00:00 2001 From: Adrian Cires Date: Thu, 16 Apr 2026 16:30:25 -0400 Subject: [PATCH 1/3] Give jetson internet from framework --- .../scripts/give_jetson_internet_framework.sh | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) create mode 100755 src/tools/scripts/give_jetson_internet_framework.sh diff --git a/src/tools/scripts/give_jetson_internet_framework.sh b/src/tools/scripts/give_jetson_internet_framework.sh new file mode 100755 index 00000000..8ab18f15 --- /dev/null +++ b/src/tools/scripts/give_jetson_internet_framework.sh @@ -0,0 +1,22 @@ +#!/bin/bash +set -e + +INET_IF="wlp5s0" +JETSON_IF="enx9cbf0d007947" + +sudo sysctl -w net.ipv4.ip_forward=1 + +if ! grep -q '^net.ipv4.ip_forward=1$' /etc/sysctl.conf; then + echo "net.ipv4.ip_forward=1" | sudo tee -a /etc/sysctl.conf +fi + +sudo iptables -t nat -C POSTROUTING -o "$INET_IF" -j MASQUERADE 2>/dev/null || \ +sudo iptables -t nat -A POSTROUTING -o "$INET_IF" -j MASQUERADE + +sudo iptables -C FORWARD -i "$JETSON_IF" -o "$INET_IF" -j ACCEPT 2>/dev/null || \ +sudo iptables -A FORWARD -i "$JETSON_IF" -o "$INET_IF" -j ACCEPT + +sudo iptables -C FORWARD -i "$INET_IF" -o "$JETSON_IF" -m conntrack --ctstate RELATED,ESTABLISHED -j ACCEPT 2>/dev/null || \ +sudo iptables -A FORWARD -i "$INET_IF" -o "$JETSON_IF" -m conntrack --ctstate RELATED,ESTABLISHED -j ACCEPT + +echo "Done. IP forwarding and NAT are configured." From 80fcede0915a38028e88ceb8a821b6d81c37c3ba Mon Sep 17 00:00:00 2001 From: Ishan Dutta Date: Mon, 4 May 2026 15:42:34 -0400 Subject: [PATCH 2/3] direction adjust --- .../src/manual_2dof_wrist_joint_by_joint_controller.cpp | 2 +- .../src/manual_arm_joint_by_joint_controller.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/subsystems/arm/arm_controllers/src/manual_2dof_wrist_joint_by_joint_controller.cpp b/src/subsystems/arm/arm_controllers/src/manual_2dof_wrist_joint_by_joint_controller.cpp index bdd218cc..6eae671d 100644 --- a/src/subsystems/arm/arm_controllers/src/manual_2dof_wrist_joint_by_joint_controller.cpp +++ b/src/subsystems/arm/arm_controllers/src/manual_2dof_wrist_joint_by_joint_controller.cpp @@ -202,7 +202,7 @@ controller_interface::return_type Manual2DOFWristJointByJointController::update( if (!std::isnan((*current_ref)->axes[0])) { // Wrist Pitch: U/D on Left Joystick AND O button - joint_velocities_[0] = -(*current_ref)->axes[1] * static_cast((*current_ref)->buttons[1]) * max_velocities_[0]; + joint_velocities_[0] = (*current_ref)->axes[1] * static_cast((*current_ref)->buttons[1]) * max_velocities_[0]; // Wrist Roll: L/R on Left Joystick AND O button joint_velocities_[1] = -(*current_ref)->axes[0] * static_cast((*current_ref)->buttons[1]) * max_velocities_[1]; 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 935ccbd9..8616ee13 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 @@ -193,7 +193,7 @@ controller_interface::return_type ManualArmJointByJointController::update( joint_velocities_[1] = ((*current_ref)->buttons[1] == 1) ? 0.0 : -(*current_ref)->axes[1] * max_velocities_[1]; // Elbow Pitch: U/D Right Stick - joint_velocities_[2] = (*current_ref)->axes[3] * max_velocities_[2]; + joint_velocities_[2] = -(*current_ref)->axes[3] * max_velocities_[2]; } else{ // RCLCPP_INFO(get_node()->get_logger(), "Returning NaN"); From f22dced4f4ed597566d9ee99ec00f68db72c7058 Mon Sep 17 00:00:00 2001 From: Ishan Dutta Date: Mon, 4 May 2026 16:05:36 -0400 Subject: [PATCH 3/3] added joint orientation for SMCs, flipped virtual four bar value --- src/description/ros2_control/arm/arm.rmd.ros2_control.xacro | 2 +- .../ros2_control/arm/arm.smc_2dof.ros2_control.xacro | 5 ++++- .../ros2_control/arm/arm.smc_3dof.ros2_control.xacro | 1 + .../include/smc_ros2_control/smc_hardware_interface.hpp | 1 + .../smc_ros2_control/src/smc_hardware_interface.cpp | 6 ++++-- .../arm/arm_bringup/config/athena_arm_controllers.yaml | 2 +- .../config/manual_arm_joint_by_joint_controller.yaml | 2 +- .../src/manual_2dof_wrist_joint_by_joint_controller.cpp | 2 +- .../src/manual_arm_joint_by_joint_controller.cpp | 6 +++--- 9 files changed, 17 insertions(+), 10 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 9ec26156..37d243cf 100644 --- a/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro @@ -24,7 +24,7 @@ 0x149 100 - 1.0 + -1.0 150 diff --git a/src/description/ros2_control/arm/arm.smc_2dof.ros2_control.xacro b/src/description/ros2_control/arm/arm.smc_2dof.ros2_control.xacro index 2df1e9af..1988b0f4 100644 --- a/src/description/ros2_control/arm/arm.smc_2dof.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.smc_2dof.ros2_control.xacro @@ -12,7 +12,8 @@ 0x148 - -100 + 100 + 1.0 300 @@ -25,6 +26,7 @@ 0x145 40 + 1.0 1000 @@ -37,6 +39,7 @@ 0x147 40 + 1.0 1000 diff --git a/src/description/ros2_control/arm/arm.smc_3dof.ros2_control.xacro b/src/description/ros2_control/arm/arm.smc_3dof.ros2_control.xacro index 2b3ab85f..e50c884a 100644 --- a/src/description/ros2_control/arm/arm.smc_3dof.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.smc_3dof.ros2_control.xacro @@ -13,6 +13,7 @@ 0x145 40 + 1.0 300 diff --git a/src/hardware_interfaces/smc_ros2_control/include/smc_ros2_control/smc_hardware_interface.hpp b/src/hardware_interfaces/smc_ros2_control/include/smc_ros2_control/smc_hardware_interface.hpp index 7bf69abf..275d3ed4 100644 --- a/src/hardware_interfaces/smc_ros2_control/include/smc_ros2_control/smc_hardware_interface.hpp +++ b/src/hardware_interfaces/smc_ros2_control/include/smc_ros2_control/smc_hardware_interface.hpp @@ -118,6 +118,7 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface // Inher // Joint specific parameters std::vector joint_node_ids; std::vector joint_gear_ratios; + std::vector joint_orientation; std::vector joint_initialization_; // Modes for control mode diff --git a/src/hardware_interfaces/smc_ros2_control/src/smc_hardware_interface.cpp b/src/hardware_interfaces/smc_ros2_control/src/smc_hardware_interface.cpp index 34f215e2..7664791c 100644 --- a/src/hardware_interfaces/smc_ros2_control/src/smc_hardware_interface.cpp +++ b/src/hardware_interfaces/smc_ros2_control/src/smc_hardware_interface.cpp @@ -83,6 +83,7 @@ void SMCHardwareInterface::logger_function(){ oss << "\nJOINT: " << info_.joints[i].name << "\n" << "Parameters: CAN ID: 0x" << std::hex << std::uppercase << joint_node_ids[i] << " | Gear Ratio: " << joint_gear_ratios[i] << "\n" + << " | Orientation: " << joint_orientation[i] << "\n" << "-- Commands --\n" << "Control Mode: " << control_mode << "\n" << "Motor Position: " << motor_position[i] @@ -115,6 +116,7 @@ hardware_interface::CallbackReturn SMCHardwareInterface::on_init( int gear_ratio = std::abs(std::stoi(joint.parameters.at("gear_ratio"))); joint_node_ids.push_back(std::clamp(std::stoi(joint.parameters.at("node_id"), nullptr, 0), 0x141, 0x160)); joint_gear_ratios.push_back(gear_ratio); + joint_orientation.push_back(std::stoi(joint.parameters.at("joint_orientation")) == -1 ? -1 : 1); operating_velocity = std::clamp(std::stoi(joint.parameters.at("operating_velocity")), 0, 65*gear_ratio); } @@ -374,7 +376,7 @@ hardware_interface::return_type smc_ros2_control::SMCHardwareInterface::write( if(control_level_[i] == integration_level_t::POSITION && std::isfinite(joint_command_position_[i])) { // CALCULATE DESIRED JOINT ANGLE - joint_angle = calculate_motor_position_from_desired_joint_position(joint_command_position_[i], joint_gear_ratios[i]); + joint_angle = joint_orientation[i]*calculate_motor_position_from_desired_joint_position(joint_command_position_[i], joint_gear_ratios[i]); // ENCODING CAN MESSAGE data[0] = ABSOLUTE_POS_CONTROL_CMD; @@ -389,7 +391,7 @@ hardware_interface::return_type smc_ros2_control::SMCHardwareInterface::write( else if(control_level_[i] == integration_level_t::VELOCITY && std::isfinite(joint_command_velocity_[i])) { // CALCULATE DESIRED JOINT VELOCITY - joint_velocity = calculate_motor_velocity_from_desired_joint_velocity(joint_command_velocity_[i], joint_gear_ratios[i]); + joint_velocity = joint_orientation[i]*calculate_motor_velocity_from_desired_joint_velocity(joint_command_velocity_[i], joint_gear_ratios[i]); // ENCODING CAN MESSAGE data[0] = SPEED_CONTROL_CMD; 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 13a2c68b..8af0fdcc 100644 --- a/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml +++ b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml @@ -159,7 +159,7 @@ manual_arm_joint_by_joint_controller: - 0.08 # base_yaw - 0.05148721 # 2.95 dps - 0.05148721 - virtual_four_bar_coupling_ratio: -1.0 + virtual_four_bar_coupling_ratio: 1.0 manual_2dof_wrist_joint_by_joint_controller: 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 5d8eeec5..d135d2af 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 @@ -61,7 +61,7 @@ manual_arm_joint_by_joint_controller: virtual_four_bar_coupling_ratio: { type: double, - default_value: -1.0, + 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/src/manual_2dof_wrist_joint_by_joint_controller.cpp b/src/subsystems/arm/arm_controllers/src/manual_2dof_wrist_joint_by_joint_controller.cpp index 6eae671d..002f86d6 100644 --- a/src/subsystems/arm/arm_controllers/src/manual_2dof_wrist_joint_by_joint_controller.cpp +++ b/src/subsystems/arm/arm_controllers/src/manual_2dof_wrist_joint_by_joint_controller.cpp @@ -205,7 +205,7 @@ controller_interface::return_type Manual2DOFWristJointByJointController::update( joint_velocities_[0] = (*current_ref)->axes[1] * static_cast((*current_ref)->buttons[1]) * max_velocities_[0]; // Wrist Roll: L/R on Left Joystick AND O button - joint_velocities_[1] = -(*current_ref)->axes[0] * static_cast((*current_ref)->buttons[1]) * max_velocities_[1]; + joint_velocities_[1] = (*current_ref)->axes[0] * static_cast((*current_ref)->buttons[1]) * max_velocities_[1]; if(DEBUG_MODE == 1){ logger_function(); 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 8616ee13..f38f88ab 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 @@ -190,17 +190,17 @@ controller_interface::return_type ManualArmJointByJointController::update( joint_velocities_[0] = ((*current_ref)->buttons[1] == 1) ? 0.0 : (*current_ref)->axes[0] * max_velocities_[0]; // Shoulder Pitch: U/D Left Stick - joint_velocities_[1] = ((*current_ref)->buttons[1] == 1) ? 0.0 : -(*current_ref)->axes[1] * max_velocities_[1]; + joint_velocities_[1] = ((*current_ref)->buttons[1] == 1) ? 0.0 : (*current_ref)->axes[1] * max_velocities_[1]; // Elbow Pitch: U/D Right Stick - joint_velocities_[2] = -(*current_ref)->axes[3] * max_velocities_[2]; + joint_velocities_[2] = (*current_ref)->axes[3] * max_velocities_[2]; } else{ // RCLCPP_INFO(get_node()->get_logger(), "Returning NaN"); joint_velocities_.resize(num_joints, 0.0); } - // Virtual four-bar compensation. With the current -1.0 ratio, shoulder-only motion commands + // 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];