feat: Random movement

src/wt-assign3-2025-5-joint-state-publisher-pkg/src/InverseKinematicNode.cpp

@@ -36,52 +36,17 @@ public:
         //     std::bind(&JointPublisherNode::parametersCallback, this, std::placeholders::_1)
         // );
 
-        // Connect 4 board parameters
-        // Board is at x=0.6, y=0.0, z=0.0 (from BoardLocationPublisher)
-        // Board offset from base_conn_4 to board_conn_4: x=0.281, y=0.031, z=0.006
-        double board_base_x = 0.60;
-        double board_base_y = 0.00;
-        double board_base_z = 0.00;
-        double board_offset_x = 0.281;
-        double board_offset_y = 0.031;
-        double board_offset_z = 0.006;
-
-        // Connect 4 is 7 columns x 6 rows
-        int cols = 7;
-        int rows = 6;
-        double cell_spacing_x = 0.0;  // Horizontal spacing between columns
-        double cell_spacing_y = 0.015;  // 15mm spacing between columns
-        double cell_spacing_z = 0.015;  // 15mm spacing between rows
-
-        // Starting position for top-left cell
-        double board_start_x = board_base_x + board_offset_x;
-        double board_start_y = board_base_y + board_offset_y - (cols - 1) * cell_spacing_y / 2.0;
-        double board_start_z = board_base_z + board_offset_z + 0.10;  // Above the board
-
-        int current_col = 0;
-        int current_row = 0;
-
+        double angle = 0.0;
         while(true) {
-            // Calculate target position for current cell
-            target_x_ = board_start_x + current_row * cell_spacing_x;
-            target_y_ = board_start_y + current_col * cell_spacing_y;
-            target_z_ = board_start_z - current_row * cell_spacing_z;
-
-            // Move to position and update
+            angle += 0.01;
+            // Vary radius from 0.1 to 0.35 to cover full range (close to far)
+            double radius = 0.1 + 0.25 * (0.5 + 0.5 * std::sin(angle * 0.3));
+            target_x_ = radius * std::cos(angle);
+            target_y_ = radius * std::sin(angle);
+            target_z_ = 0.15 + 0.1 * std::sin(angle * 2);
             calculateInverseKinematics(case_number_);
             broadcastJointState();
-
-            // Move to next cell
-            current_col++;
-            if (current_col >= cols) {
-                current_col = 0;
-                current_row++;
-                if (current_row >= rows) {
-                    current_row = 0;  // Start over from top-left
-                }
-            }
-
-            std::this_thread::sleep_for(std::chrono::milliseconds(500));  // Pause at each cell
+            std::this_thread::sleep_for(std::chrono::milliseconds(10));
         }
 
         RCLCPP_INFO(this->get_logger(),
@@ -285,13 +250,15 @@ private:
         double beta = std::atan2(a5_ * std::sin(-theta_3_), a3_ + a5_ * std::cos(-theta_3_));
         theta_2_ = alpha - beta;
 
-        // Wrist joints - position gripper downward for dropping pieces
-        // theta_4: Wrist pitch - point down
-        // theta_5: Wrist roll - keep level
-        // theta_6: Gripper rotation - keep aligned
-        theta_4_ = -M_PI / 2;  // Point gripper down
-        theta_5_ = 0.0;
-        theta_6_ = 0.0;
+        // Wrist joints - add some motion for variety
+        // theta_4: Slowly oscillating wrist pitch
+        // theta_5: Varies with base rotation for coordinated movement
+        // theta_6: Continuous slow rotation
+        static double time_counter = 0.0;
+        time_counter += 0.01;
+        theta_4_ = 0.3 * std::sin(time_counter * 0.5);
+        theta_5_ = 0.4 * std::sin(time_counter * 0.7 + theta_1_);
+        theta_6_ = time_counter * 0.1;
 
         RCLCPP_INFO(this->get_logger(),
             "Calculated angles - J1(base): %f, J2(shoulder): %f, J3(elbow): %f",