diff --git a/.vscode/settings.json b/.vscode/settings.json index f58e5a2..b6fd8ff 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -77,6 +77,7 @@ "typeindex": "cpp", "typeinfo": "cpp", "variant": "cpp", - "dense": "cpp" + "dense": "cpp", + "filesystem": "cpp" } } \ No newline at end of file diff --git a/src/robot_control/include/motion_parameters.hpp b/src/robot_control/include/motion_parameters.hpp index fc7d4e9..5e47e13 100644 --- a/src/robot_control/include/motion_parameters.hpp +++ b/src/robot_control/include/motion_parameters.hpp @@ -100,16 +100,16 @@ namespace robot_control // 初始化关节索引 wheel_joint_indices_ = {0, 1}; - waist_joint_indices_ = {0, 1}; + waist_joint_indices_ = {0}; leg_joint_indices_ = {2, 3, 4, 5}; - total_joints_ = 2; + total_joints_ = 1; // 初始化关节方向 wheel_joint_directions_ = {1, -1}; - waist_joint_directions_ = {1, 1}; + waist_joint_directions_ = {1}; leg_joint_directions_ = {1, -1, -1, 1}; @@ -185,8 +185,8 @@ namespace robot_control } waist_zero_positions_ = { - 181.0, - 52.66 + 181.0 + // 52.66 }; leg_zero_positions_ = { @@ -201,8 +201,8 @@ namespace robot_control }; waist_home_positions_ = { - 181.0, - 52.66 + 181.0 + // 52.66 }; diff --git a/src/robot_control/include/robot_control_manager.hpp b/src/robot_control/include/robot_control_manager.hpp index 87594f8..218247b 100644 --- a/src/robot_control/include/robot_control_manager.hpp +++ b/src/robot_control/include/robot_control_manager.hpp @@ -35,6 +35,11 @@ namespace robot_control bool Stop(double dt); bool GoHome(double dt); void JogAxis(size_t axis, int dir); + void SetJogWheel(bool value); + bool GetJogWheel(); + + void WheelReset(){isWheelHomed_ = false;}; + void ImuReset(){isGyroInited_ = false;} // 检查参数是否合理 bool SetMoveWheelParameters(double moveWheelDistance, double moveWheelAngle); @@ -71,6 +76,8 @@ namespace robot_control bool isLegHomed_; bool isWheelHomed_; + bool is_wheel_jog_; + // 控制器 LegControl* legController_; WheelControl* wheelController_; diff --git a/src/robot_control/include/robot_control_node.hpp b/src/robot_control/include/robot_control_node.hpp index 645b116..178b433 100644 --- a/src/robot_control/include/robot_control_node.hpp +++ b/src/robot_control/include/robot_control_node.hpp @@ -101,6 +101,8 @@ namespace robot_control size_t jogIndex_; double jogValue_; + double lastSpeed_; + void Publish_joint_trajectory(); void JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg); diff --git a/src/robot_control/include/wheel_control.hpp b/src/robot_control/include/wheel_control.hpp index 7721436..b5025e5 100644 --- a/src/robot_control/include/wheel_control.hpp +++ b/src/robot_control/include/wheel_control.hpp @@ -30,6 +30,8 @@ namespace robot_control bool SetMoveWheelParametersInternal(double moveWheelDistance, double moveWheelAngle); + bool SetMoveWheelParametersInternalJog(double moveWheelDistance, double moveWheelAngle); + bool MoveToTargetPoint(std::vector& joint_positions, const std::vector& target_pos, double duration) override; bool MoveWheel(std::vector& joint_positions); diff --git a/src/robot_control/src/robot_control_manager.cpp b/src/robot_control/src/robot_control_manager.cpp index 9785c41..3d9f359 100644 --- a/src/robot_control/src/robot_control_manager.cpp +++ b/src/robot_control/src/robot_control_manager.cpp @@ -25,8 +25,10 @@ void RobotControlManager::init() gyroVelocities_.resize(3, 0.0); gyroAccelerations_.resize(3, 0.0); + is_wheel_jog_ = false; + // Initialize the wheel commands - for (size_t i = 0; i < motionParams_.waist_joint_indices_.size(); i++) + for (size_t i = 0; i < motionParams_.wheel_joint_indices_.size(); i++) { wheelCommands_.data.push_back(0.0); } @@ -82,8 +84,23 @@ RobotControlManager::~RobotControlManager() delete wheelController_; } +void RobotControlManager::SetJogWheel(bool value) +{ + is_wheel_jog_ = value; +} + +bool RobotControlManager::GetJogWheel() +{ + return is_wheel_jog_; +} + bool RobotControlManager::SetMoveWheelParameters(double moveWheelDistance, double moveWheelAngle) { + if (is_wheel_jog_) + { + return wheelController_->SetMoveWheelParametersInternalJog(moveWheelDistance, moveWheelAngle); + } + return wheelController_->SetMoveWheelParametersInternal(moveWheelDistance, moveWheelAngle); } diff --git a/src/robot_control/src/robot_control_node.cpp b/src/robot_control/src/robot_control_node.cpp index fce9cc9..66daafa 100644 --- a/src/robot_control/src/robot_control_node.cpp +++ b/src/robot_control/src/robot_control_node.cpp @@ -17,6 +17,7 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node") isJogMode_ = false; jogDirection_ = 0; jogIndex_ = 0; + lastSpeed_ = 51; // 初始化数据文件（设置路径，确保目录存在） #if 0 @@ -381,6 +382,13 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_wheel_goal( return rclcpp_action::GoalResponse::REJECT; } + if (goal->move_distance > 2.1 || goal->move_distance < -1.0 || goal->move_angle < -10 || goal->move_angle > 10 ) + { + RCLCPP_ERROR(this->get_logger(), "exceed limit"); + return rclcpp_action::GoalResponse::REJECT; + } + + // if (!robotControlManager_.SetMoveWheelParameters(goal->move_distance, goal->move_angle)) // { // RCLCPP_ERROR(this->get_logger(), "Invalid move wheel request"); @@ -418,9 +426,25 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptr(); auto result = std::make_shared(); - double tempValue = robotControlManager_.GetImuDifference()[2]; + double wheelAngle = 0; - double wheelAngle = abs(tempValue) > 40.0 ? 0.0 : tempValue; + if (abs(goal->move_angle) > 0) + { + robotControlManager_.SetJogWheel(true); + wheelAngle = goal->move_angle; + } + + if (abs(goal->move_angle) == 0 && abs(goal->move_distance) == 0) + { + robotControlManager_.SetJogWheel(false); + } + + if (!robotControlManager_.GetJogWheel()) + { + double tempValue = robotControlManager_.GetImuDifference()[2]; + wheelAngle = abs(tempValue) > 40.0 ? 0.0 : tempValue; + } + // double wheelAngle = 1.5; @@ -430,10 +454,20 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptr(); - if(goal->move_distance > 0.0) + if((goal->move_distance > 0.1 ) && !robotControlManager_.GetJogWheel()) // || goal->move_distance < -0.5 { + // if (lastSpeed_ < 51 && goal->move_distance < -0.5) + // { + // ratio = 1.02; + // } request->motor_id = 1; - request->max_speed = static_cast(round((ratio) * 48)); + request->max_speed = static_cast(round((ratio) * 51)); + + // if (goal->move_distance < -0.5) + // { + // request->add_acc = 4; + // } + request->add_acc = 8; request->dec_acc = 8; @@ -441,7 +475,7 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptrmax_speed << std::endl; - rclcpp::sleep_for(std::chrono::milliseconds(200)); + rclcpp::sleep_for(std::chrono::milliseconds(50)); } robotControlManager_.MoveWheel(); @@ -475,7 +509,7 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptrmove_distance > 0.0) + if ((goal->move_distance > 0.0) && !robotControlManager_.GetJogWheel()) //|| goal->move_distance < -0.5 { request->motor_id = 1; - request->max_speed = 48; + request->max_speed = 51; request->add_acc = 8; request->dec_acc = 8; @@ -534,7 +568,10 @@ void RobotControlNode::ControlLoop() { { if(robotControlManager_.GoHome(dt_sec)) { + robotControlManager_.SetJogWheel(false); move_home_executing_ = false; + robotControlManager_.WheelReset(); + robotControlManager_.ImuReset(); } } diff --git a/src/robot_control/src/waist_control.cpp b/src/robot_control/src/waist_control.cpp index e651326..2657f9f 100644 --- a/src/robot_control/src/waist_control.cpp +++ b/src/robot_control/src/waist_control.cpp @@ -109,7 +109,7 @@ bool WaistControl::SetMoveWaistParametersInternal(double movepitch, double movey tempPosition.resize(total_joints_, 0.0); tempPosition[0] = joint_commands_[0] - joint_home_positions_[0] + movepitch; - tempPosition[1] = joint_commands_[1] - joint_home_positions_[1] + moveyaw; + // tempPosition[1] = joint_commands_[1] - joint_home_positions_[1] + moveyaw; if (!checkJointLimits(tempPosition)) { @@ -122,7 +122,8 @@ bool WaistControl::SetMoveWaistParametersInternal(double movepitch, double movey joint_position_desired_[i] = tempPosition[i] + joint_home_positions_[i]; } - std::cout << "Waist Target Joint Position: " << joint_position_desired_[0] << ", " << joint_position_desired_[1] << std::endl; + // std::cout << "Waist Target Joint Position: " << joint_position_desired_[0] << ", " << joint_position_desired_[1] << std::endl; + std::cout << "Waist Target Joint Position: " << joint_position_desired_[0] << std::endl; is_target_set_ = true; diff --git a/src/robot_control/src/wheel_control.cpp b/src/robot_control/src/wheel_control.cpp index bb5242d..b72ad04 100644 --- a/src/robot_control/src/wheel_control.cpp +++ b/src/robot_control/src/wheel_control.cpp @@ -39,79 +39,80 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub double original1 = joint_position_desired_[0]; double original2 = joint_position_desired_[1]; + std::vector tempHomePositions; + std::vector tempDesiredPositions; + + tempHomePositions.resize(total_joints_, 0.0); + tempDesiredPositions.resize(total_joints_, 0.0); + + for (size_t i = 0; i < total_joints_; i++) + { + tempHomePositions[i] = joint_home_positions_[i]; + tempDesiredPositions[i] = joint_position_desired_[i]; + } + + // 设置移动轮参数 double theta = moveWheelDistance / lengthParameters_[0] * 180.0 / M_PI; - double beta = moveWheelAngle / 360.0 * M_PI * 2 * lengthParameters_[1] / lengthParameters_[0] * 180.0 / M_PI; + double beta = moveWheelAngle * 0.8 / 360.0 * M_PI * 2 * lengthParameters_[1] / lengthParameters_[0] * 180.0 / M_PI; - if(moveWheelDistance > lastMoveDistance && (moveWheelDistance - lastMoveDistance > 1.0) && beta != 0) + if(moveWheelDistance > lastMoveDistance && (moveWheelDistance - lastMoveDistance > 1.5) && beta != 0) { - tempAdjustCount ++; - - if (tempAdjustCount > 0) + if (beta > 0) { - if (tempAdjustCount == 1) - { - for (size_t i = 0; i < joint_directions_.size(); i++) - { - joint_home_positions_[i] = joint_home_positions_[i] - beta; - } - - if (beta > 0) - { - joint_home_positions_[1] = joint_home_positions_[1] + beta; - } - else - { - joint_home_positions_[0] = joint_home_positions_[0] + beta; - } - - } - else if (tempAdjustCount == 2) - { - for (size_t i = 0; i < joint_directions_.size(); i++) - { - joint_home_positions_[i] = joint_home_positions_[i] - beta; - } - - // joint_home_positions_[0] = joint_home_positions_[0] - beta; - // joint_home_positions_[1] = joint_home_positions_[1] + beta; - - if (beta > 0) - { - joint_home_positions_[1] = joint_home_positions_[1] + beta; - } - else - { - joint_home_positions_[0] = joint_home_positions_[0] + beta; - } - - tempAdjustCount = 0; - } + tempHomePositions[0] = joint_home_positions_[0] - beta - 8; } + else + { + tempHomePositions[0] = joint_home_positions_[0] - beta + 8; + } + } + + if(moveWheelDistance < lastMoveDistance && (moveWheelDistance - lastMoveDistance < -1.5) && beta != 0) + { + tempAdjustCount ++; + if (beta > 0) + { + tempHomePositions[0] = joint_home_positions_[0] - beta * 2.0; + + } + else + { + tempHomePositions[0] = joint_home_positions_[0] - beta; + } + + if (tempAdjustCount == 1) + { + tempHomePositions[0] - 40; + tempAdjustCount = 0; + } + + // if (tempAdjustCount == 2) + // { + // tempAdjustCount = 0; + // } + } for (size_t i = 0; i < joint_directions_.size(); i++) { if (joint_directions_[i] == 1) { - joint_position_desired_[i] = joint_home_positions_[i] + theta; + tempDesiredPositions[i] = tempHomePositions[i] + theta; } else { - joint_position_desired_[i] = joint_home_positions_[i] - theta; // * 0.98 * 1.02; + tempDesiredPositions[i] = tempHomePositions[i] - theta; // * 0.98 * 1.02; } } - double a1 = abs(joint_position_desired_[0] - original1); - double a2 = abs(joint_position_desired_[1] - original2); - - std::cout << "motor 1 distance : " << a1 << std::endl; - std::cout << "motor 2 distance : " << a2 << std::endl; + double a1 = abs(tempDesiredPositions[0] - original1); + double a2 = abs(tempDesiredPositions[1] - original2); if (a2 > 0.0 && a1 > 0.0) { - moveRatio_ = ((a1 / a2) - 1.0) * 0.50; + moveRatio_ = ((a1 / a2) - 1.0) * 0.9; } else { @@ -120,7 +121,32 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub moveRatio_ = moveRatio_ + 1.0; - // moveRatio_ = 1.0; + if (moveRatio_ > 0.99 && moveRatio_ < 1.009) + { + for (size_t i = 0; i < joint_directions_.size(); i++) + { + if (joint_directions_[i] == 1) + { + joint_position_desired_[i] = joint_home_positions_[i] + theta; + } + else + { + joint_position_desired_[i] = joint_home_positions_[i] - theta; // * 0.98 * 1.02; + } + } + } + else + { + std::cout << "change distance " << std::endl; + for (size_t i = 0; i < joint_directions_.size(); i++) + { + joint_position_desired_[i] = tempDesiredPositions[i]; + joint_home_positions_[i] = tempHomePositions[i]; + } + } + + std::cout << "motor 1 distance : " << joint_position_desired_[0] - original1 << std::endl; + std::cout << "motor 2 distance : " << joint_position_desired_[1] - original2 << std::endl; std::cout << "Set move ratio : " << moveRatio_ << std::endl; @@ -133,6 +159,72 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub return true; } +bool WheelControl::SetMoveWheelParametersInternalJog(double moveWheelDistance, double moveWheelAngle) +{ + double original1 = joint_position_desired_[0]; + double original2 = joint_position_desired_[1]; + + std::vector tempHomePositions; + std::vector tempDesiredPositions; + + tempHomePositions.resize(total_joints_, 0.0); + tempDesiredPositions.resize(total_joints_, 0.0); + + for (size_t i = 0; i < total_joints_; i++) + { + tempHomePositions[i] = joint_home_positions_[i]; + tempDesiredPositions[i] = joint_position_desired_[i]; + } + + + // 设置移动轮参数 + double theta = moveWheelDistance / lengthParameters_[0] * 180.0 / M_PI; + + double beta = moveWheelAngle / 360.0 * M_PI * 2 * lengthParameters_[1] / lengthParameters_[0] * 180.0 / M_PI; + + for (size_t i = 0; i < joint_directions_.size(); i++) + { + tempHomePositions[0] = joint_home_positions_[0] + beta; + tempHomePositions[1] = joint_home_positions_[1] + beta; + } + + + for (size_t i = 0; i < joint_directions_.size(); i++) + { + if (joint_directions_[i] == 1) + { + tempHomePositions[i] += theta; + tempDesiredPositions[i] = tempHomePositions[i] ; + } + else + { + tempHomePositions[i] -= theta; + tempDesiredPositions[i] = tempHomePositions[i]; // * 0.98 * 1.02; + } + } + + moveRatio_ = 1; + + for (size_t i = 0; i < joint_directions_.size(); i++) + { + joint_position_desired_[i] = tempDesiredPositions[i]; + joint_home_positions_[i] = tempHomePositions[i]; + } + + + std::cout << "motor 1 distance : " << joint_position_desired_[0] - original1 << std::endl; + std::cout << "motor 2 distance : " << joint_position_desired_[1] - original2 << std::endl; + + std::cout << "Set move ratio : " << moveRatio_ << std::endl; + + // std::cout << "Move Wheel joint_position_desired_: " << joint_position_desired_[0] << " , " << joint_position_desired_[1] << std::endl; + + is_target_set_ = true; + + return true; +} + + bool WheelControl::MoveToTargetPoint(std::vector& joint_positions, const std::vector& target_pos, double dt) {