From df2e5808f5a41069b61a6d1a20b833e34fc39e8b Mon Sep 17 00:00:00 2001 From: Wessel Tip Date: Thu, 27 Nov 2025 12:47:30 +0100 Subject: [PATCH] feat: Random movement --- .../src/InverseKinematicNode.cpp | 67 +++++-------------- 1 file changed, 17 insertions(+), 50 deletions(-) diff --git a/src/wt-assign3-2025-5-joint-state-publisher-pkg/src/InverseKinematicNode.cpp b/src/wt-assign3-2025-5-joint-state-publisher-pkg/src/InverseKinematicNode.cpp index ffaec97..f5ade31 100644 --- a/src/wt-assign3-2025-5-joint-state-publisher-pkg/src/InverseKinematicNode.cpp +++ b/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",