diff --git a/src/robot_control/include/motion_parameters.hpp b/src/robot_control/include/motion_parameters.hpp index 4e4c135..fc7d4e9 100644 --- a/src/robot_control/include/motion_parameters.hpp +++ b/src/robot_control/include/motion_parameters.hpp @@ -6,7 +6,7 @@ #include -#define CYCLE_TIME 4 // 插补周期（毫秒） +#define CYCLE_TIME 8 // 插补周期（毫秒） #ifndef DEG2RAD #define DEG2RAD(x) ((x)*0.017453293) @@ -80,8 +80,8 @@ namespace robot_control // 关节速度参数 max_joint_velocity_ = { - 30, - 30, + 35, + 100, 50, 50, 50, @@ -89,8 +89,8 @@ namespace robot_control }; max_joint_acceleration_ = { - 100, - 100, + 80, + 200, 100, 100, 100, @@ -121,7 +121,7 @@ namespace robot_control //TODO: 限位需要修改 joint_limits_ = { - JointLimit(0, 30.0, -30.0, LimitType::POSITION), + JointLimit(0, 40.0, -40.0, LimitType::POSITION), JointLimit(1, 190.0, -190.0, LimitType::POSITION), JointLimit(2, 80.0, 0.0, LimitType::POSITION), // JointLimit(3, 5.0, -5.0, LimitType::POSITION), diff --git a/src/robot_control/include/robot_control_manager.hpp b/src/robot_control/include/robot_control_manager.hpp index e619b13..87594f8 100644 --- a/src/robot_control/include/robot_control_manager.hpp +++ b/src/robot_control/include/robot_control_manager.hpp @@ -18,7 +18,7 @@ #include "interfaces/msg/imu_msg.hpp" using MotorPos = interfaces::msg::MotorPos; -using ImuMsg = interfaces::msg::ImuMsg; +using ImuMsg = sensor_msgs::msg::Imu; namespace robot_control { diff --git a/src/robot_control/include/waist_control.hpp b/src/robot_control/include/waist_control.hpp index 4ec35a3..e246de0 100644 --- a/src/robot_control/include/waist_control.hpp +++ b/src/robot_control/include/waist_control.hpp @@ -22,7 +22,7 @@ namespace robot_control ~WaistControl() override; - void SetHomePositions(const std::vector& home_positions) override; + // void SetHomePositions(const std::vector& home_positions) override; bool SetMoveWaistParametersInternal(double movepitch, double moveyaw); diff --git a/src/robot_control/src/control_base.cpp b/src/robot_control/src/control_base.cpp index b915a74..f6d7b11 100644 --- a/src/robot_control/src/control_base.cpp +++ b/src/robot_control/src/control_base.cpp @@ -158,6 +158,7 @@ void ControlBase::UpdateJointStates( { joint_commands_ = joint_position_; is_joint_position_initialized_ = true; + std::cout << "joint commands initialized" << std::endl; } // 3. 更新关节速度 @@ -194,7 +195,7 @@ bool ControlBase::checkJointLimits(const std::vector& target_joint) { if (target_joint[i] < minLimits_[i] || target_joint[i] > maxLimits_[i]) { - printf("Joint %zu target position %f is out of limits [%f, %f]", i, target_joint[i], minLimits_[i], maxLimits_[i]); + printf("Joint %zu target position %f is out of limits [%f, %f]", i + 1, target_joint[i], minLimits_[i], maxLimits_[i]); return false; } } diff --git a/src/robot_control/src/robot_control_manager.cpp b/src/robot_control/src/robot_control_manager.cpp index be965c2..9785c41 100644 --- a/src/robot_control/src/robot_control_manager.cpp +++ b/src/robot_control/src/robot_control_manager.cpp @@ -196,7 +196,10 @@ std::vector RobotControlManager::GetImuDifference() } //TODO: optimization - gyroInitValues_[2] = gyroInitValues_[2] - 0.1; + // gyroInitValues_[2] = gyroInitValues_[2] - 0.03; + + std::cout << "get gyro init value : " << gyroInitValues_[2] << std::endl; + std::cout << "get gyro value : " << gyroValues_[2] << std::endl; return result; } @@ -204,7 +207,7 @@ std::vector RobotControlManager::GetImuDifference() void RobotControlManager::UpdateImuMsg(const ImuMsg::SharedPtr msg) { imuMsg_ = *msg; - sensor_msgs::msg::Imu tempMsg = imuMsg_.imu; + ImuMsg tempMsg = imuMsg_; if (tempMsg.orientation.w == 0 && tempMsg.orientation.x == 0 && tempMsg.orientation.y == 0 && tempMsg.orientation.z == 0) { @@ -351,7 +354,6 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState:: { if (count > 50) { - waistController_->SetHomePositions(jointPositions_); jointCommands_.data = jointPositions_; isJointInitValueSet_ = true; count = 0; diff --git a/src/robot_control/src/robot_control_node.cpp b/src/robot_control/src/robot_control_node.cpp index 33b75f8..fce9cc9 100644 --- a/src/robot_control/src/robot_control_node.cpp +++ b/src/robot_control/src/robot_control_node.cpp @@ -143,7 +143,7 @@ void RobotControlNode::handle_move_home_accepted(const std::shared_ptr goal_handle) { - RCLCPP_INFO(this->get_logger(), "Executing home goal"); + // RCLCPP_INFO(this->get_logger(), "Executing home goal"); rclcpp::Rate loop_rate(10); // 10Hz更新频率 const auto goal = goal_handle->get_goal(); auto feedback = std::make_shared(); @@ -235,7 +235,7 @@ void RobotControlNode::handle_move_leg_accepted(const std::shared_ptr goal_handle) { - RCLCPP_INFO(this->get_logger(), "Executing leg goal"); + // RCLCPP_INFO(this->get_logger(), "Executing leg goal"); rclcpp::Rate loop_rate(10); // 10Hz更新频率 const auto goal = goal_handle->get_goal(); auto feedback = std::make_shared(); @@ -326,7 +326,7 @@ void RobotControlNode::handle_move_waist_accepted(const std::shared_ptr goal_handle) { - RCLCPP_INFO(this->get_logger(), "Executing waist goal"); + // RCLCPP_INFO(this->get_logger(), "Executing waist goal"); rclcpp::Rate loop_rate(10); // 10Hz更新频率 const auto goal = goal_handle->get_goal(); // 获取目标 auto feedback = std::make_shared(); @@ -422,23 +422,28 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptr 40.0 ? 0.0 : tempValue; + // double wheelAngle = 1.5; + robotControlManager_.SetMoveWheelParameters(goal->move_distance, wheelAngle); double ratio = robotControlManager_.GetWheelRatio(); auto request = std::make_shared(); - request->motor_id = 1; - request->max_speed = static_cast(round(ratio * 40)); - request->add_acc = 10; - request->dec_acc = 10; + if(goal->move_distance > 0.0) + { + request->motor_id = 1; + request->max_speed = static_cast(round((ratio) * 48)); + request->add_acc = 8; + request->dec_acc = 8; + + motorParamClient_->async_send_request(request); + + std::cout << "set motor 1 max speed : " << request->max_speed << std::endl; + + rclcpp::sleep_for(std::chrono::milliseconds(200)); + } - motorParamClient_->async_send_request(request); - - std::cout << "request->max_speed : " << request->max_speed << std::endl; - - rclcpp::sleep_for(std::chrono::milliseconds(1000)); - robotControlManager_.MoveWheel(); std_msgs::msg::Float64MultiArray wheel_commands; @@ -451,8 +456,8 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptrpublish(wheel_commands_msg); @@ -485,14 +490,15 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptrmotor_id = 1; - request->max_speed = 40; - request->add_acc = 10; - request->dec_acc = 10; - - motorParamClient_->async_send_request(request); - - std::cout << "request->max_speed : " << request->max_speed << std::endl; + if(goal->move_distance > 0.0) + { + request->motor_id = 1; + request->max_speed = 48; + request->add_acc = 8; + request->dec_acc = 8; + + motorParamClient_->async_send_request(request); + } // 检查目标是否仍在活跃状态 if (rclcpp::ok()) diff --git a/src/robot_control/src/trapezoidal_trajectory.cpp b/src/robot_control/src/trapezoidal_trajectory.cpp index 2f152fc..4c03a36 100644 --- a/src/robot_control/src/trapezoidal_trajectory.cpp +++ b/src/robot_control/src/trapezoidal_trajectory.cpp @@ -98,6 +98,7 @@ void TrapezoidalTrajectory::calculateTrajectoryParams() std::vector axis_decel_time(dof, 0.0); // 各轴理论减速时间 std::vector axis_cruise_time(dof, 0.0); // 各轴理论匀速时间 std::vector axis_cruise_vel(dof, 0.0); // 各轴理论巡航速度 + total_time_ = 0.0; // 步骤1：计算每个轴的理论最小运动参数（独立约束） for (size_t i = 0; i < dof; ++i) @@ -149,6 +150,11 @@ void TrapezoidalTrajectory::calculateTrajectoryParams() acceleration_time_ = axis_accel_time[i]; deceleration_time_ = axis_decel_time[i]; cruise_time_ = axis_cruise_time[i]; + + // std::cout << "total time : " << total_time_ << std::endl; + // std::cout << "acceleration_time_ : " << acceleration_time_ << std::endl; + // std::cout << "deceleration_time_ : " << deceleration_time_ << std::endl; + // std::cout << "cruise_time_ : " << cruise_time_ << std::endl; } } diff --git a/src/robot_control/src/waist_control.cpp b/src/robot_control/src/waist_control.cpp index 47afba9..e651326 100644 --- a/src/robot_control/src/waist_control.cpp +++ b/src/robot_control/src/waist_control.cpp @@ -88,20 +88,20 @@ bool WaistControl::MoveWaist(std::vector& joint_positions, double dt) return false; } -void WaistControl::SetHomePositions(const std::vector& home_positions) -{ - if (home_positions.size() != total_joints_) - { - throw std::invalid_argument("Home positions size does not match total joints!"); - } +// void WaistControl::SetHomePositions(const std::vector& home_positions) +// { +// if (home_positions.size() != total_joints_) +// { +// throw std::invalid_argument("Home positions size does not match total joints!"); +// } - if (home_positions[1] < 0) - { - joint_home_positions_[1] = joint_home_positions_[1] - 360.0; - } +// // if (home_positions[1] < 0) +// // { +// // joint_home_positions_[1] = joint_home_positions_[1] - 360.0; +// // } - std::cout << "Home positions set to: " << joint_home_positions_[0] << ", " << joint_home_positions_[1] << std::endl; -} +// std::cout << "Home positions set to: " << joint_home_positions_[0] << ", " << joint_home_positions_[1] << std::endl; +// } bool WaistControl::SetMoveWaistParametersInternal(double movepitch, double moveyaw) { diff --git a/src/robot_control/src/wheel_control.cpp b/src/robot_control/src/wheel_control.cpp index 65b10ac..bb5242d 100644 --- a/src/robot_control/src/wheel_control.cpp +++ b/src/robot_control/src/wheel_control.cpp @@ -32,33 +32,63 @@ WheelControl::~WheelControl() delete trapezoidalTrajectory_; } +int tempAdjustCount = 0; + bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, double moveWheelAngle) { double original1 = joint_position_desired_[0]; double original2 = joint_position_desired_[1]; - std::cout << "Original joint_position_desired_: " << original1 << " , " << original2 << std::endl; - // 设置移动轮参数 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; - std::cout << "moveWheelDistance: " << moveWheelDistance << std::endl; - - if(moveWheelDistance > lastMoveDistance && beta != 0) + if(moveWheelDistance > lastMoveDistance && (moveWheelDistance - lastMoveDistance > 1.0) && beta != 0) { - std::cout << " beta: " << beta << std::endl; + tempAdjustCount ++; - for (size_t i = 0; i < joint_directions_.size(); i++) + if (tempAdjustCount > 0) { - joint_home_positions_[i] = joint_home_positions_[i] - beta; + 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; + } } - - double additionDistance = 0.001 / lengthParameters_[0] * 180.0 / M_PI; - - joint_home_positions_[0] = joint_home_positions_[0] + additionDistance; - joint_home_positions_[1] = joint_home_positions_[1] - additionDistance; } for (size_t i = 0; i < joint_directions_.size(); i++) @@ -76,18 +106,25 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub 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; + if (a2 > 0.0 && a1 > 0.0) { - moveRatio_ = a1 / a2; + moveRatio_ = ((a1 / a2) - 1.0) * 0.50; } else { - moveRatio_ = 1.0; + moveRatio_ = 0.0; } + moveRatio_ = moveRatio_ + 1.0; + + // moveRatio_ = 1.0; + 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; + // std::cout << "Move Wheel joint_position_desired_: " << joint_position_desired_[0] << " , " << joint_position_desired_[1] << std::endl; lastMoveDistance = moveWheelDistance;