Joint by joint comp direction adjust

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

@@ -24,7 +24,7 @@
       <joint name="elbow_pitch">
         <param name="node_id">0x149</param>
         <param name="gear_ratio">100</param>
-        <param name="joint_orientation">1.0</param>
+        <param name="joint_orientation">-1.0</param>
         <param name="operating_velocity">150</param> <!-- motor dps -->
         <state_interface name="position"/>
         <state_interface name="velocity"/>

src/description/ros2_control/arm/arm.smc_2dof.ros2_control.xacro

@@ -12,7 +12,8 @@
 
       <joint name="base_yaw">
         <param name="node_id">0x148</param>
-        <param name="gear_ratio">-100</param>
+        <param name="gear_ratio">100</param>
+        <param name="joint_orientation">1.0</param>
         <param name="operating_velocity">300</param> <!-- motor dps -->
         <command_interface name="position"/>
         <command_interface name="velocity"/>
@@ -25,6 +26,7 @@
       <joint name="wrist_pitch">
         <param name="node_id">0x145</param>
         <param name="gear_ratio">40</param>
+        <param name="joint_orientation">1.0</param>
         <param name="operating_velocity">1000</param> <!-- motor dps -->
         <command_interface name="position"/>
         <command_interface name="velocity"/>
@@ -37,6 +39,7 @@
       <joint name="wrist_roll">
         <param name="node_id">0x147</param>
         <param name="gear_ratio">40</param>
+        <param name="joint_orientation">1.0</param>
         <param name="operating_velocity">1000</param> <!-- motor dps -->
         <command_interface name="position"/>
         <command_interface name="velocity"/>

src/description/ros2_control/arm/arm.smc_3dof.ros2_control.xacro

@@ -13,6 +13,7 @@
       <joint name="base_yaw">
         <param name="node_id">0x145</param>
         <param name="gear_ratio">40</param>
+        <param name="joint_orientation">1.0</param>
         <param name="operating_velocity">300</param> <!-- motor dps -->
         <command_interface name="position"/>
         <command_interface name="velocity"/>

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<int> joint_node_ids;
   std::vector<int> joint_gear_ratios;
+  std::vector<int> joint_orientation;
   std::vector<bool> joint_initialization_;
 
   // Modes for control mode

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;

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:

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

src/subsystems/arm/arm_controllers/src/manual_2dof_wrist_joint_by_joint_controller.cpp

@@ -202,10 +202,10 @@ 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<float>((*current_ref)->buttons[1]) * max_velocities_[0];
+    joint_velocities_[0] = (*current_ref)->axes[1] * static_cast<float>((*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<float>((*current_ref)->buttons[1]) * max_velocities_[1];
+    joint_velocities_[1] = (*current_ref)->axes[0] * static_cast<float>((*current_ref)->buttons[1]) * max_velocities_[1];
 
     if(DEBUG_MODE == 1){
       logger_function();

src/subsystems/arm/arm_controllers/src/manual_arm_joint_by_joint_controller.cpp

@@ -190,7 +190,7 @@ 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];
@@ -200,7 +200,7 @@ controller_interface::return_type ManualArmJointByJointController::update(
     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];
 

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."