[DO NOT MERGE] Arm control v2

.gitignore

@@ -69,3 +69,6 @@ log/
 
 .vscode/settings.json
 
+# CLAUDE
+CLAUDE.md
+.claude/

DockerInstallation.md

@@ -10,7 +10,7 @@ Minimal installation setup for Docker. Ask ChatGPT first if you are having troub
 ### 2.1 Build the image (AMD64 Architecture):
 > docker build -t rover2025:rover
 ### 2.2 Build the image (ARM64 Architecture):
-> docker builx build --platform linux/amd64,linux/arm64 -t roverflake2:dev --push .
+> docker buildx build --platform linux/amd64,linux/arm64 -t roverflake2:dev --push .
 
 ### 3. Start a detached container
 > docker compose --compatibility up rover -d

src/arm_control/include/controller_config.h

@@ -45,10 +45,12 @@
 namespace ControllerConfig {
 
     // --- Face Buttons ---
-    constexpr int BTN_B      = 0;   // East
-    constexpr int BTN_A      = 1;   // South
-    constexpr int BTN_Y      = 2;   // North
-    constexpr int BTN_X      = 3;   // West
+    constexpr int BTN_B = 0;   // East (A)
+    constexpr int BTN_A = 1;   // South (B)
+    constexpr int BTN_Y = 2;   // North (X)
+    constexpr int BTN_X = 3;   // West (Y)
+
+    constexpr int BTN_HOME = 5; // Home
 
     constexpr int BTN_UP     = 12;   // Left shoulder
     constexpr int BTN_DOWN   = 13;   // Right shoulder
@@ -82,9 +84,6 @@ namespace ControllerConfig {
     constexpr double CART_BUTTON_SPEED = 0.5;  // unitless, range [0.0, 1.0]
     constexpr double ROT_STICK_SPEED   = 0.6;  // unitless, range [0.0, 1.0] for angular
 
-    // --- Deadzone for analog axes (used later for joystick support) ---
-    constexpr double AXIS_DEADZONE = 0.15;
-
     // --- Frame for Cartesian twist commands ---
     // "base_link" = world-fixed directions (forward is always forward)
     // "ee_base_link" = relative to end-effector orientation
@@ -102,10 +101,33 @@ namespace ControllerConfig {
     constexpr bool INVERT_PITCH = false;
     constexpr bool INVERT_YAW   = true;   // typical: push left = positive yaw
 
+    // --- Mode Toggle (FK / IK) ---
+    constexpr int BTN_MODE_TOGGLE = 4;      // Minus (-) button — edge-triggered FK/IK toggle
+    constexpr double FK_JOINT_SPEED = 45.0; // deg/s per unit of stick input
+
+    // FK axis mapping: which joy axis drives each joint (sticks → joints 0-3)
+    constexpr int FK_AXIS_J0 = AXIS_LEFT_Y;
+    constexpr int FK_AXIS_J1 = AXIS_RIGHT_Y;
+    constexpr int FK_AXIS_J2 = AXIS_LEFT_X;
+    constexpr int FK_AXIS_J3 = AXIS_RIGHT_X;
+    // Joints 4 and 5 use shoulder buttons (repurposed in FK mode)
+    constexpr int FK_BTN_J4_POS = BTN_UP;
+    constexpr int FK_BTN_J4_NEG = BTN_DOWN;
+    constexpr int FK_BTN_J5_POS = BTN_Y;
+    constexpr int FK_BTN_J5_NEG = BTN_A;
+
     // --- Gripper ---
     constexpr int BTN_GRIPPER_TOGGLE = 7;  // ZR button — edge-triggered toggle
+    constexpr int AXIS_GRIPPER_TOGGLE = 5; // ZR analog axis (rests at 1.0, pressed = -1.0)
+    constexpr double AXIS_GRIPPER_PRESSED_THRESHOLD = 0.0; // trigger considered pressed below this
     constexpr double GRIPPER_OPEN_VALUE  = 1.0;
     constexpr double GRIPPER_CLOSE_VALUE = 0.0;
+
+    // --- Gripper Sim Positions ---
+    constexpr double GRIPPER_SIM_LEFT_OPEN_POS   =  0.04;
+    constexpr double GRIPPER_SIM_RIGHT_OPEN_POS  = -0.04;
+    constexpr double GRIPPER_SIM_LEFT_CLOSE_POS  =  0.0;
+    constexpr double GRIPPER_SIM_RIGHT_CLOSE_POS =  0.0;
 }
 
 #elif ACTIVE_CONTROLLER == CONTROLLER_CYBORG_STICK
@@ -151,10 +173,42 @@ namespace ControllerConfig {
     constexpr double AXIS_DEADZONE = 0.15;
     constexpr const char* CART_FRAME_ID = "base_link";
 
+    // --- Mode Toggle (FK / IK) ---
+    constexpr int BTN_MODE_TOGGLE = -1;     // no dedicated mode button on Cyborg
+    constexpr double FK_JOINT_SPEED = 45.0;
+    constexpr int FK_AXIS_J0 = AXIS_STICK_Y;
+    constexpr int FK_AXIS_J1 = AXIS_TWIST;
+    constexpr int FK_AXIS_J2 = AXIS_STICK_X;
+    constexpr int FK_AXIS_J3 = -1;
+    constexpr int FK_BTN_J4_POS = -1;
+    constexpr int FK_BTN_J4_NEG = -1;
+    constexpr int FK_BTN_J5_POS = -1;
+    constexpr int FK_BTN_J5_NEG = -1;
+
     // --- Gripper ---
     constexpr int BTN_GRIPPER_TOGGLE = BTN_TRIGGER;  // trigger = gripper toggle
+    constexpr int AXIS_GRIPPER_TOGGLE = -1;           // no analog trigger axis
+    constexpr double AXIS_GRIPPER_PRESSED_THRESHOLD = 0.0;
     constexpr double GRIPPER_OPEN_VALUE  = 1.0;
     constexpr double GRIPPER_CLOSE_VALUE = 0.0;
+
+    // --- Gripper Sim Positions ---
+    constexpr double GRIPPER_SIM_LEFT_OPEN_POS   =  0.04;
+    constexpr double GRIPPER_SIM_RIGHT_OPEN_POS  = -0.04;
+    constexpr double GRIPPER_SIM_LEFT_CLOSE_POS  =  0.0;
+    constexpr double GRIPPER_SIM_RIGHT_CLOSE_POS =  0.0;
+
+    // --- Orientation ---
+    constexpr double ROT_STICK_SPEED = 0.4;
+    constexpr int AXIS_ROLL  = AXIS_STICK_X;  // stick X → roll
+    constexpr int AXIS_PITCH = AXIS_STICK_Y;  // stick Y → pitch
+    constexpr int AXIS_YAW   = AXIS_TWIST;    // twist   → yaw
+    constexpr bool INVERT_ROLL  = false;
+    constexpr bool INVERT_PITCH = false;
+    constexpr bool INVERT_YAW   = false;
+
+    // --- Misc ---
+    constexpr int BTN_HOME = -1;  // no home button on Cyborg stick
 }
 
 #else

src/arm_control/include/joy_arm_control.h

@@ -4,6 +4,7 @@
 #include "rover_msgs/msg/arm_command.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
 #include "trajectory_msgs/msg/joint_trajectory.hpp"
+#include "std_msgs/msg/float64_multi_array.hpp"
 #include "controller_config.h"
 
 #include <unordered_map>
@@ -28,6 +29,7 @@ bool fk = true; // Decides if joystick outputs forward kinematics or inverse
 rclcpp::Publisher<rover_msgs::msg::ArmCommand>::SharedPtr arm_publisher;
 rclcpp::Publisher<sensor_msgs::msg::JoyFeedback>::SharedPtr joy_vibrator;
 rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_publisher;
+rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr gripper_sim_publisher;
 
 // Subs
 rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_subscriber;
@@ -39,6 +41,8 @@ void joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg);
 void arm_callback(const rover_msgs::msg::ArmCommand::SharedPtr msg);
 /// Bridge: converts MoveIt Servo JointTrajectory → ArmCommand for the physical arm
 void trajectory_callback(const trajectory_msgs::msg::JointTrajectory::SharedPtr msg);
+/// Publish gripper open/close positions to the simulated gripper controller
+void publish_rviz_gripper_command();
 
 // Timers
 rclcpp::TimerBase::SharedPtr timer_;
@@ -51,12 +55,12 @@ int current_gear = 0;
 int prev_paddleR = 0;
 int prev_paddleL = 0;
 
-// Gripper state tracking
+// edge detection
 bool gripper_open_ = false;
 bool prev_gripper_btn_ = false;
+bool prev_home_btn_ = false;
+bool prev_mode_btn_ = false;
 
-// ========== Servo → Physical Arm Bridge ==========
-// Maps URDF joint names (from MoveIt Servo) to firmware axis indices (0-5).
 std::unordered_map<std::string, int> urdf_to_axis_;
 // Axis direction multipliers matching armControlParams.h ArmConstants::axis_dirs.
 // Needed to convert between MoveIt (rad/s) and firmware (deg/s) frames.

src/arm_control/include/moveit_control.h

@@ -25,7 +25,10 @@ purpose: to handle moveit control, as well as servo.
 
 #define PI 3.14159
 
-// Gripper toggle button index
+// Gripper toggle inputs (Nintendo Pro trigger axis)
+// #define GRIPPER_TOGGLE_BTN -1
+#define GRIPPER_TOGGLE_AXIS 5
+#define GRIPPER_AXIS_PRESSED_THRESHOLD 0.0
 #define GRIPPER_TOGGLE_BTN 0  // joystick "button 1" is index 0 (zero-indexed)
 
 // Gripper end-effector command values
@@ -78,12 +81,13 @@ int count_ = 0;
 void publishCommands();	
 void joyCallback(const sensor_msgs::msg::Joy::SharedPtr joy_msg);
 void sendGripperCommand(double value);
-int joyControlMode = CARTESIAN_EE_FRAME;
+// int joyControlMode = CARTESIAN_BASE_FRAME;
 
 // Gripper state tracking
 bool gripper_open_ = false;
 bool prev_gripper_btn_ = false;
 
+
     // void test_send(){
     //     send_command(0.5, 1.0, 1.0, 0.5);
     //     // rclcpp::logger
@@ -168,30 +172,25 @@ bool prev_gripper_btn_ = false;
     }
 
     void servoCallback(const trajectory_msgs::msg::JointTrajectory::SharedPtr msg){
-              rover_msgs::msg::ArmCommand target;
-        //TODO position or vel
-        // target.positions.resize(NUM_JOINTS);
+        if(msg->points.empty()){
+          RCLCPP_WARN(this->get_logger(), "Servo output trajectory has no points. Ignoring.");
+          return;
+        }
+
+        rover_msgs::msg::ArmCommand target;
         target.velocities.resize(NUM_JOINTS_NO_EE);
         target.cmd_type = 'V';
-        // float target_positions[NUM_JOINTS];
-        // float target_velocities[NUM_JOINTS];
-        float inputs[NUM_JOINTS_NO_EE];
-        if(msg->points[0].velocities.size() == NUM_JOINTS_NO_EE){
-     for (int i = 0; i < NUM_JOINTS_NO_EE; i++){
-            // float temp_pos = msg->position[i];
-            // target.positions[i] = moveitToFirmwareOffset(msg->reference.positions[i], i);
-            target.velocities[i] = moveitVelocityToFirmwareOffset(msg->points[0].velocities[i], i);
-        // RCLCPP_INFO(this->get_logger(), "J%i, %lf", i, target.velocities[i]);
-
-        }   
-         arm_publisher->publish(target);
-        // RCLCPP_INFO(this->get_logger(), "Joint Trajrectory Controller good. Servo is commanding arm!");
-
-
+        target.end_effector = gripper_open_ ? GRIPPER_OPEN_VALUE : GRIPPER_CLOSE_VALUE;
+
+        const auto& point = msg->points.back();
+        if(point.velocities.size() == NUM_JOINTS_NO_EE){
+          for (int i = 0; i < NUM_JOINTS_NO_EE; i++){
+            target.velocities[i] = moveitVelocityToFirmwareOffset(point.velocities[i], i);
+          }
+          arm_publisher->publish(target);
         }else{
           RCLCPP_ERROR(this->get_logger(), "Joint Trajrectory Controller going wack. Output does not match size of joints. Ignoring this message.");
         }
-
     }
 
     void arm_callback(const rover_msgs::msg::ArmCommand::SharedPtr msg){

src/arm_control/src/cbs_interface.cpp

@@ -1,72 +1,79 @@
 //* This node will take the cbs topic for the arm panel and send things around
 #include "cbs_interface.h"
+
+#include <algorithm>
+#include <cmath>
+
+namespace {
+
+constexpr double kPanelCenter = 50.0;
+constexpr double kPanelRange = 50.0;
+constexpr double kPanelDeadzone = 0.08;
+constexpr double kIkLinearSpeed = 0.5;   // unitless, matches MoveIt Servo scale.linear
+constexpr double kIkAngularSpeed = 0.6;  // unitless, matches joy_arm_control rotational range
+constexpr double kEeCloseSpeed = -50.0;
+constexpr double kEeOpenSpeed = 50.0;
+
+double normalize_panel_axis(int raw_value) {
+    const double normalized = (static_cast<double>(raw_value) - kPanelCenter) / kPanelRange;
+    const double clamped = std::max(-1.0, std::min(1.0, normalized));
+    return (std::abs(clamped) < kPanelDeadzone) ? 0.0 : clamped;
+}
+
+double end_effector_from_buttons(bool left_btn, bool right_btn) {
+    if (left_btn && right_btn) {
+        return 0.0;
+    }
+    if (left_btn) {
+        return kEeCloseSpeed;
+    }
+    if (right_btn) {
+        return kEeOpenSpeed;
+    }
+    return 0.0;
+}
+
+}  // namespace
+
 void CBSArmInterface::arm_panel_callback(const rover_msgs::msg::ArmPanel::SharedPtr msg){
+    const double left_x = normalize_panel_axis(msg->left.x);
+    const double left_y = normalize_panel_axis(msg->left.y);
+    const double left_z = normalize_panel_axis(msg->left.z);
+    const double right_x = normalize_panel_axis(msg->right.x);
+    const double right_y = normalize_panel_axis(msg->right.y);
+    const double right_z = normalize_panel_axis(msg->right.z);
+    const double ee_command = end_effector_from_buttons(msg->left.button, msg->right.button);
+
    if(ik){
         geometry_msgs::msg::TwistStamped ik_msg;
 
     ik_msg.header.stamp = rclcpp::Clock(RCL_SYSTEM_TIME).now();
-    ik_msg.header.frame_id = "link_tt";
-   //  ik_msg.twist.linear.x = static_cast<float>(msg->
-    ik_msg.twist.linear.x = (static_cast<float>(msg->left.x) - 50)/100 * max_joysticks_output_speed_deg[0]*2 / 180;
-     ik_msg.twist.linear.y = (static_cast<float>(msg->left.y) - 50)/100 * max_joysticks_output_speed_deg[1]*2 *-1 / 180;
-     ik_msg.twist.linear.z = (static_cast<float>(msg->right.x) - 50)/100 * max_joysticks_output_speed_deg[4]*2 / 180;
-     ik_msg.twist.angular.x = (static_cast<float>(msg->right.z) - 50)/100 * max_joysticks_output_speed_deg[5]*2 / 180;
-    ik_msg.twist.angular.y = (static_cast<float>(msg->right.y) - 50)/100 * max_joysticks_output_speed_deg[2]*2*-1 / 180;
-     ik_msg.twist.angular.z = (static_cast<float>(msg->left.z) - 50)/100 * max_joysticks_output_speed_deg[3]*2 / 180;
-    // float value = ik_hmi_speed[index]; ///TODO get speed based on spinbuttons
-    // switch (index)
-    // {
-    //     case 0: // Linear X
-    //         ik_msg.twist.linear.x = value;
-    //         break;
-    //     case 1: // Linear Y
-    //         ik_msg.twist.linear.y = value;
-    //         break;
-    //     case 2: // Linear Z
-    //         ik_msg.twist.linear.z = value;
-    //         break;
-    //     case 3: // Angular X
-    //         ik_msg.twist.angular.x = value;
-    //         break;
-    //     case 4: // Angular Y
-    //         ik_msg.twist.angular.y = value;
-    //         break;
-    //     case 5: // Angular Z
-    //         ik_msg.twist.angular.z = value;
-    //         break;
-    //     default:
-    //         RCLCPP_WARN(this->get_logger(), "Invalid index: %d. Must be 0-5.", index);
-    //         return;
-    // }
-
+    ik_msg.header.frame_id = "base_link";
+    ik_msg.twist.linear.x = left_x * kIkLinearSpeed;
+    ik_msg.twist.linear.y = -left_y * kIkLinearSpeed;
+    ik_msg.twist.linear.z = right_x * kIkLinearSpeed;
+    ik_msg.twist.angular.x = right_z * kIkAngularSpeed;
+    ik_msg.twist.angular.y = -right_y * kIkAngularSpeed;
+    ik_msg.twist.angular.z = left_z * kIkAngularSpeed;
     arm_ik_pub->publish(ik_msg);
-   }else{
-
-
 
+    // Keep gripper/end-effector controllable while in IK mode.
+    rover_msgs::msg::ArmCommand ee_msg;
+    ee_msg.cmd_type = 'V';
+    ee_msg.velocities.assign(NUM_JOINTS_NO_EE, 0.0);
+    ee_msg.end_effector = ee_command;
+    arm_cmd_publisher->publish(ee_msg);
+   }else{
     rover_msgs::msg::ArmCommand cmd_msg;
-    cmd_msg.cmd_type = 'V'; //!SHOULD BE FROM ArmSerialProtocol.h
+    cmd_msg.cmd_type = 'V';
     cmd_msg.velocities.resize(NUM_JOINTS);
-    cmd_msg.velocities[0] = (static_cast<float>(msg->left.x) - 50)/100 * max_joysticks_output_speed_deg[0]*2;
-     cmd_msg.velocities[1] = (static_cast<float>(msg->left.y) - 50)/100 * max_joysticks_output_speed_deg[1]*2 *-1;
-     cmd_msg.velocities[2] = (static_cast<float>(msg->right.y) - 50)/100 * max_joysticks_output_speed_deg[2]*2*-1;
-     cmd_msg.velocities[3] = (static_cast<float>(msg->left.z) - 50)/100 * max_joysticks_output_speed_deg[3]*2;
-     cmd_msg.velocities[4] = (static_cast<float>(msg->right.x) - 50)/100 * max_joysticks_output_speed_deg[4]*2;
-     cmd_msg.velocities[5] = (static_cast<float>(msg->right.z) - 50)/100 * max_joysticks_output_speed_deg[5]*2;
-
-    if(msg->left.button && msg->right.button){
-
-    cmd_msg.end_effector = 0;
-    }else{
-    if(msg->left.button){
-
-    cmd_msg.end_effector = -50;//-0.07;
-    }
-    if(msg->right.button){
-
-    cmd_msg.end_effector = 50;//0.07;
-    }
-    }
+    cmd_msg.velocities[0] = left_x * max_joysticks_output_speed_deg[0];
+    cmd_msg.velocities[1] = -left_y * max_joysticks_output_speed_deg[1];
+    cmd_msg.velocities[2] = -right_y * max_joysticks_output_speed_deg[2];
+    cmd_msg.velocities[3] = left_z * max_joysticks_output_speed_deg[3];
+    cmd_msg.velocities[4] = right_x * max_joysticks_output_speed_deg[4];
+    cmd_msg.velocities[5] = right_z * max_joysticks_output_speed_deg[5];
+    cmd_msg.end_effector = ee_command;
 
     arm_cmd_publisher->publish(cmd_msg);
 }
@@ -95,4 +102,4 @@ int main(int argc, char *argv[]) {
 
     rclcpp::shutdown();
     return 0;
-}
+}