11.11 version update

Reviewed-on: #2

.vscode/settings.json

@@ -77,6 +77,7 @@
         "typeindex": "cpp",
         "typeinfo": "cpp",
         "variant": "cpp",
-        "dense": "cpp"
+        "dense": "cpp",
+        "filesystem": "cpp"
     }
 }

src/robot_control/include/control_base.hpp

@@ -5,6 +5,9 @@
 #include "trapezoidal_trajectory.hpp"
 #include "motion_parameters.hpp"
 
+#define POSITION_TOLERANCE 1.0
+#define TIME_OUT_COUNT 10000
+
 namespace robot_control
 {
     class ControlBase
@@ -25,7 +28,9 @@ namespace robot_control
             std::vector<double> joint_zero_positions_;          // 零点位置
             std::vector<int> joint_directions_;                 // 关节运动方向
             std::vector<double> joint_position_;                // 当前关节位置
+            std::vector<double> joint_commands_;                // 当前关节指令
             std::vector<double> joint_velocity_;                // 当前关节速度
+            std::vector<double> joint_velocity_commands_;       // 当前关节速度指令
             std::vector<double> joint_acceleration_;            // 当前关节加速度
             std::vector<double> joint_torque_;                  // 当前关节力矩
             std::vector<double> minLimits_;                     // 关节位置下限
@@ -35,7 +40,10 @@ namespace robot_control
             bool is_moving_;                                    // 是否运动中
             bool is_stopping_;                                  // 是否停止中
             bool is_target_set_;                                // 是否已设置目标点
+            bool is_cmd_send_finished_; 
+            bool is_joint_position_initialized_;              // 是否已初始化关节位置
 
+            int time_out_count_;                                     // 超时时间
         public:
             // 构造函数（子类需调用此构造函数初始化父类）
             ControlBase(
@@ -71,6 +79,10 @@ namespace robot_control
             // 6. 判断是否运动中（通用逻辑，父类实现）
             virtual bool IsMoving();
 
+            virtual bool IsReached(const std::vector<double>& target_joint); // 判断是否到达目标点
+
+            virtual void SetHomePositions(const std::vector<double>& home_positions);
+
             bool checkJointLimits(const std::vector<double>& target_joint);
     };
 

src/robot_control/include/extended_kalman_filter.hpp

@@ -0,0 +1,43 @@
+#pragma once
+
+#include <iostream>
+#include <vector>
+#include <cmath>
+#include <Eigen/Dense>
+#include <fstream>  // 用于保存轨迹数据
+
+namespace robot_control {
+class EKF 
+{
+private:
+    double dt;
+    
+    // 卡尔曼滤波参数
+    Eigen::Matrix4d Q;        // 过程噪声协方差矩阵
+    Eigen::MatrixXd R;        // 测量噪声协方差矩阵
+    
+    // 状态向量 [x, y, θ, ω]
+    Eigen::Vector4d X;
+    // 协方差矩阵
+    Eigen::Matrix4d P;
+    
+    // 角度归一化到[-π, π]
+    double normalizeAngle(double angle) {
+        while (angle > M_PI) angle -= 2.0 * M_PI;
+        while (angle < -M_PI) angle += 2.0 * M_PI;
+        return angle;
+    }
+
+public:
+    EKF(double init_x = 0, double init_y = 0, double init_theta = 0, double init_omega = 0);
+    
+    // 预测步骤
+    void predict(double v, double omega_wheel);
+    
+    // 更新步骤
+    void update(double gyro_omega);
+    
+    // 获取当前状态
+    Eigen::Vector4d getState() { return X; }
+};
+}

src/robot_control/include/motion_parameters.hpp

@@ -6,7 +6,7 @@
 #include <string>
 
 
-#define CYCLE_TIME 4    // 插补周期（毫秒）
+#define CYCLE_TIME 8    // 插补周期（毫秒）
 
 #ifndef DEG2RAD
 #define DEG2RAD(x) ((x)*0.017453293)
@@ -56,11 +56,11 @@ namespace robot_control
                 // 初始化结构参数 unit m
                 //TODO: 修改为实际参数
                 wheel_radius_ = 0.09;
-                wheel_separation_ = 0.26;
+                wheel_separation_ = 0.55;
 
                 wheel_length_ = {
                     wheel_radius_,
-                    wheel_radius_
+                    wheel_separation_
                 };
 
                 // 腿长参数 unit m
@@ -80,36 +80,36 @@ namespace robot_control
 
                 // 关节速度参数
                 max_joint_velocity_ = {
-                    15,
-                    15,
-                    15,
-                    15,
-                    15,
-                    15
-                }; 
-
-                max_joint_acceleration_ = {
-                    50,
-                    50,
+                    35,
+                    100,
                     50,
                     50,
                     50,
                     50
+                }; 
+
+                max_joint_acceleration_ = {
+                    80,
+                    200,
+                    100,
+                    100,
+                    100,
+                    100
                 };
 
                 // 初始化关节索引
                 wheel_joint_indices_ = {0, 1}; 
 
-                waist_joint_indices_ = {0, 1}; 
+                waist_joint_indices_ = {0}; 
 
                 leg_joint_indices_ = {2, 3, 4, 5};
 
-                total_joints_ = 6; 
+                total_joints_ = 1; 
 
                 // 初始化关节方向
                 wheel_joint_directions_ = {1, -1};  
 
-                waist_joint_directions_ = {1, 1};
+                waist_joint_directions_ = {1};
 
                 leg_joint_directions_ = {1, -1, -1, 1};  
 
@@ -121,8 +121,8 @@ namespace robot_control
 
                 //TODO: 限位需要修改
                 joint_limits_ = {
-                    JointLimit(0, 30.0,     -30.0, LimitType::POSITION),
-                    JointLimit(1, 180.0,    -180.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),
                     // JointLimit(4, 50.0,      -50.0, LimitType::POSITION),
@@ -185,8 +185,8 @@ namespace robot_control
                 }
                 
                 waist_zero_positions_ = {
-                    181.0, 
-                    158.5
+                    181.0
+                    // 52.66
                 };
 
                 leg_zero_positions_ = {
@@ -201,8 +201,8 @@ namespace robot_control
                 };
 
                 waist_home_positions_ = {
-                    181.0, 
-                    158.5
+                    181.0
+                    // 52.66
                 };
 
 

src/robot_control/include/robot_control_manager.hpp

@@ -1,9 +1,11 @@
 #pragma once
 
+#include <cmath>
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/float64_multi_array.hpp"
 #include "geometry_msgs/msg/twist.hpp"
 #include "sensor_msgs/msg/joint_state.hpp"
+#include "sensor_msgs/msg/imu.hpp"
 #include "motion_parameters.hpp"
 #include "arm_control.hpp"
 #include "leg_control.hpp"
@@ -13,8 +15,10 @@
 #include "waist_control.hpp"
 
 #include "interfaces/msg/motor_pos.hpp"
+#include "interfaces/msg/imu_msg.hpp"
 
 using MotorPos = interfaces::msg::MotorPos;
+using ImuMsg = sensor_msgs::msg::Imu;
 
 namespace robot_control
 {
@@ -31,24 +35,34 @@ 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){return wheelController_->SetMoveWheelParametersInternal(moveWheelDistance, moveWheelAngle);};
-            bool SetMoveLegParameters(double moveLegDistance){return legController_->SetMoveLegParametersInternal(moveLegDistance);};
-            bool SetMoveWaistParameters(double movePitchAngle, double moveYawAngle){return waistController_->SetMoveWaistParametersInternal(movePitchAngle, moveYawAngle);};
+            bool SetMoveWheelParameters(double moveWheelDistance, double moveWheelAngle);
+            bool SetMoveLegParameters(double moveLegDistance);
+            bool SetMoveWaistParameters(double movePitchAngle, double moveYawAngle);
             
             // 机器人关节指令
             std_msgs::msg::Float64MultiArray GetJointCommands();
             std_msgs::msg::Float64MultiArray GetJointFeedback();
             std_msgs::msg::Float64MultiArray GetWheelCommands();
             std_msgs::msg::Float64MultiArray GetWheelFeedback();
+            double GetWheelRatio();
 
+            // 机器人状态
             void CheckJointLimits(std_msgs::msg::Float64MultiArray& jointCommands);
             void UpdateJointStates(const sensor_msgs::msg::JointState::SharedPtr msg);
             void UpdateWheelStates(const MotorPos::SharedPtr msg);
+            void UpdateImuMsg(const ImuMsg::SharedPtr msg);
 
             MotionParameters GetMotionParameters();
 
+            std::vector<double> GetImuDifference();
+
             bool IsMoving(MovementPart part);
 
             bool RobotInitFinished();
@@ -60,6 +74,9 @@ namespace robot_control
 
             bool isWaistHomed_;
             bool isLegHomed_;
+            bool isWheelHomed_;
+
+            bool is_wheel_jog_;
 
             // 控制器
             LegControl* legController_;
@@ -81,6 +98,14 @@ namespace robot_control
 
             std::vector<bool> jointInited_; // 机器人是否已经初始化
             bool isJointInitValueSet_;
+
+            // 陀螺仪状态
+            ImuMsg imuMsg_;
+            bool isGyroInited_;
+            std::vector<double> gyroValues_; // 陀螺仪数据（弧度/秒）
+            std::vector<double> gyroInitValues_; // 陀螺仪初始位置
+            std::vector<double> gyroVelocities_; // 陀螺仪速度（弧度/秒）
+            std::vector<double> gyroAccelerations_; // 陀螺仪加速度（弧度/秒^2）
             
             // 临时变量
             std::vector<double> tempWaistCmd_;
@@ -94,6 +119,9 @@ namespace robot_control
 
             void AssignTempValues();
             void UpdateJointCommands();
+
+            void QuaternionToRPYRad(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw);
+            void QuaternionToRPYDeg(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw);
     };
 
 } // namespace robot_control_manager

src/robot_control/include/robot_control_node.hpp

@@ -16,7 +16,7 @@
 #include "interfaces/action/move_waist.hpp"
 #include "interfaces/action/move_wheel.hpp"
 #include "interfaces/msg/motor_cmd.hpp"
-
+#include "interfaces/srv/motor_param.hpp"
 
 using MoveHome = interfaces::action::MoveHome;
 using GoalHandleMoveHome = rclcpp_action::ServerGoalHandle<MoveHome>;
@@ -32,6 +32,8 @@ using GoalHandleMoveWheel = rclcpp_action::ServerGoalHandle<MoveWheel>;
 
 using MotorCmd = interfaces::msg::MotorCmd;
 
+using MotorParam = interfaces::srv::MotorParam;
+
 namespace robot_control
 {
     class RobotControlNode : public rclcpp::Node 
@@ -81,6 +83,8 @@ namespace robot_control
             rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr jointStatesSub_;
             rclcpp::Publisher<MotorCmd>::SharedPtr motorCmdPub_;
             rclcpp::Subscription<MotorPos>::SharedPtr wheelStatesSub_;
+            rclcpp::Subscription<ImuMsg>::SharedPtr imuMsgSub_;
+            rclcpp::Client<MotorParam>::SharedPtr motorParamClient_;
 
             RobotControlManager robotControlManager_;
 
@@ -97,11 +101,14 @@ namespace robot_control
             size_t jogIndex_;
             double jogValue_;
 
+            double lastSpeed_;
+
             void Publish_joint_trajectory();
 
             void JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg);
             void JoyCommandCallback(const std_msgs::msg::String::SharedPtr msg);
             void WheelStatesCallback(const MotorPos::SharedPtr cmd_msg);
+            void ImuMsgCallback(const ImuMsg::SharedPtr msg);
     };
 
 } // namespace robot_control

src/robot_control/include/waist_control.hpp

@@ -22,6 +22,8 @@ namespace robot_control
 
             ~WaistControl() override;
 
+            // void SetHomePositions(const std::vector<double>& home_positions) override;
+
             bool SetMoveWaistParametersInternal(double movepitch, double moveyaw);
 
             bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) override;

src/robot_control/include/wheel_control.hpp

@@ -22,8 +22,16 @@ namespace robot_control
 
             ~WheelControl() override;
 
+            double moveRatio_= 1.0;
+
+            double lastMoveDistance = 0.0;
+
+            double GetWheelRatioInternal(){ return moveRatio_ ;};
+
             bool SetMoveWheelParametersInternal(double moveWheelDistance, double moveWheelAngle);
 
+            bool SetMoveWheelParametersInternalJog(double moveWheelDistance, double moveWheelAngle);
+
             bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) override;
 
             bool MoveWheel(std::vector<double>& joint_positions);

src/robot_control/src/control_base.cpp

@@ -35,6 +35,8 @@ ControlBase::ControlBase(
     joint_acceleration_.resize(total_joints_, 0.0);
     joint_torque_.resize(total_joints_, 0.0);
     joint_position_desired_.resize(total_joints_, 0.0);
+    joint_commands_.resize(total_joints_, 0.0);
+    joint_velocity_commands_.resize(total_joints_, 0.0);
     
     // 初始化梯形轨迹规划器
     trapezoidalTrajectory_ = new TrapezoidalTrajectory(maxSpeed, maxAcc);
@@ -42,9 +44,13 @@ ControlBase::ControlBase(
     is_moving_ = false;
     is_stopping_ = false;
     is_target_set_ = false;
+    is_cmd_send_finished_ = false;
+    is_joint_position_initialized_ = false;
 
     stopDurationTime_ = 0.0;
     movingDurationTime_ = 0.0;
+
+    time_out_count_ = 0;
 }
 
 ControlBase::~ControlBase()
@@ -52,6 +58,12 @@ ControlBase::~ControlBase()
     delete trapezoidalTrajectory_;
 }
 
+void ControlBase::SetHomePositions(const std::vector<double>& home_positions)
+{
+    joint_home_positions_ = home_positions;
+    joint_position_desired_ = home_positions;
+}
+
 bool ControlBase::MoveToTargetJoint(std::vector<double>& joint_positions, const std::vector<double>& target_joint, double dt)
 {
     if (joint_position_.size() != target_joint.size())
@@ -61,20 +73,7 @@ bool ControlBase::MoveToTargetJoint(std::vector<double>& joint_positions, const
 
     if (!is_moving_)
     {
-        bool all_joints_reached = true;  
-        const double position_tolerance = 1e-3; 
-
-        for (size_t i = 0; i < target_joint.size(); i++)
-        {
-            double position_error = std::fabs(joint_position_[i] - target_joint[i]);
-            if (position_error > position_tolerance)
-            {
-                all_joints_reached = false;  
-                break;
-            }
-        }
-
-        if (all_joints_reached)
+        if (IsReached(target_joint))
         {
             joint_positions = target_joint;
 
@@ -95,12 +94,14 @@ bool ControlBase::MoveToTargetJoint(std::vector<double>& joint_positions, const
     if (trapezoidalTrajectory_->isFinished(movingDurationTime_))
     {
         joint_positions = target_joint;
+        joint_commands_ = target_joint;
         is_moving_ = false;
         movingDurationTime_ = 0.0;
 
         return true;
     }
 
+    joint_commands_ = desired_pos;
     joint_positions = desired_pos;
     joint_velocity_ = trapezoidalTrajectory_->getVelocity();
     joint_acceleration_ = trapezoidalTrajectory_->getAcceleration();
@@ -153,6 +154,13 @@ void ControlBase::UpdateJointStates(
     // 2. 更新关节位置（同时同步关节角度，假设位置与角度单位一致）
     joint_position_ = joint_positions;
 
+    if( !is_joint_position_initialized_)
+    {
+        joint_commands_ = joint_position_;
+        is_joint_position_initialized_ = true;
+        std::cout << "joint commands initialized" << std::endl;
+    }
+
     // 3. 更新关节速度
     joint_velocity_ = joint_velocities;
 
@@ -165,13 +173,29 @@ bool ControlBase::IsMoving()
     return is_moving_;
 }
 
+bool ControlBase::IsReached(const std::vector<double>& target_joint)
+{
+    bool result = true;
+
+    for (size_t i = 0; i < target_joint.size(); i++)
+    {
+        if (std::abs(joint_position_[i] - target_joint[i]) > POSITION_TOLERANCE)
+        {
+            result = false;
+            break;
+        }
+    }
+
+    return result;
+}
+
 bool ControlBase::checkJointLimits(const std::vector<double>& target_joint)
 {
     for (size_t i = 0; i < target_joint.size(); i++)
     {
         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;
         }
     }

src/robot_control/src/extended_kalman_filter.cpp

@@ -0,0 +1,73 @@
+#include "extended_kalman_filter.hpp"
+
+namespace robot_control {
+EKF::EKF(double init_x, double init_y, double init_theta, double init_omega) 
+{
+    this->dt = 0.001;
+    
+    // 初始化状态向量
+    X << init_x, init_y, init_theta, init_omega;
+    
+    // 初始化协方差矩阵
+    P = Eigen::Matrix4d::Zero();
+    P.diagonal() << 0.1, 0.1, 0.05, 0.01;
+}
+    
+    // 预测步骤
+void EKF::predict(double v, double omega_wheel) {
+    // double x = X(0);
+    // double y = X(1);
+    double theta = X(2);
+    // double omega = X(3);
+    
+    // 状态转移矩阵F
+    Eigen::Matrix4d F = Eigen::Matrix4d::Identity();
+    F(0, 2) = -v * sin(theta) * dt;
+    F(1, 2) = v * cos(theta) * dt;
+    F(2, 3) = dt;
+    
+    // 输入矩阵B
+    Eigen::Matrix<double, 4, 2> B;
+    B << cos(theta) * dt, 0,
+            sin(theta) * dt, 0,
+            0, dt,
+            0, 1;
+            
+    // 控制输入
+    Eigen::Vector2d u;
+    u << v, omega_wheel;
+    
+    // 预测状态
+    X = F * X + B * u;
+    // 预测协方差
+    P = F * P * F.transpose() + Q;
+    
+    // 归一化航向角
+    X(2) = normalizeAngle(X(2));
+}
+    
+// 更新步骤
+void EKF::update(double gyro_omega) {
+    // 观测矩阵H
+    Eigen::Matrix<double, 1, 4> H;
+    H << 0, 0, 0, 1;
+    
+    // 测量残差
+    Eigen::VectorXd z(1);
+    z << gyro_omega;
+    Eigen::VectorXd y = z - H * X;
+    
+    // 计算卡尔曼增益
+    Eigen::MatrixXd S = H * P * H.transpose() + R;
+    Eigen::MatrixXd K = P * H.transpose() * S.inverse();
+    
+    // 更新状态
+    X = X + K * y;
+    // 更新协方差
+    Eigen::Matrix4d I = Eigen::Matrix4d::Identity();
+    P = (I - K * H) * P;
+    
+    // 归一化航向角
+    X(2) = normalizeAngle(X(2));
+}
+}

src/robot_control/src/robot_control_manager.cpp

@@ -12,13 +12,24 @@ RobotControlManager::RobotControlManager()
 
 void RobotControlManager::init()
 {
-    // jointInited_.resize(motionParams_.total_joints_, false);
-    jointInited_.resize(3, false);
+    jointInited_.resize(motionParams_.total_joints_, false);
+    // jointInited_.resize(3, false);
 
     // Initialize the joint commands
     for (size_t i = 0; i < motionParams_.total_joints_; i++)
     {
         jointCommands_.data.push_back(0.0);
+    }
+
+    gyroValues_.resize(4, 0.0);
+    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_.wheel_joint_indices_.size(); i++)
+    {
         wheelCommands_.data.push_back(0.0);
     }
 
@@ -60,8 +71,10 @@ void RobotControlManager::init()
 
     isWaistHomed_ = false;
     isLegHomed_ = false;
+    isWheelHomed_ = false;
 
     isJointInitValueSet_ = false;
+    isGyroInited_ = false;
 }
 
 RobotControlManager::~RobotControlManager()
@@ -71,6 +84,36 @@ 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);
+}
+
+bool RobotControlManager::SetMoveLegParameters(double moveLegDistance)
+{
+    return legController_->SetMoveLegParametersInternal(moveLegDistance);
+}
+
+bool RobotControlManager::SetMoveWaistParameters(double movePitchAngle, double moveYawAngle)
+{
+    return waistController_->SetMoveWaistParametersInternal(movePitchAngle, moveYawAngle);
+}
+
 Float64MultiArray RobotControlManager::GetWheelCommands()
 {
     return wheelCommands_;
@@ -104,6 +147,11 @@ Float64MultiArray RobotControlManager::GetWheelFeedback()
     return tempValues;
 }
 
+double RobotControlManager::GetWheelRatio()
+{
+    return wheelController_->GetWheelRatioInternal();
+}
+
 Float64MultiArray RobotControlManager::GetJointCommands()
 {
     Float64MultiArray tempValues;
@@ -152,6 +200,109 @@ bool isAllTrueManual(const std::vector<bool>& vec) {
     return true;  // 所有元素都是 true
 }
 
+
+
+std::vector<double> RobotControlManager::GetImuDifference()
+{
+    std::vector<double> result;
+    result.resize(gyroValues_.size(), 0.0);
+
+    for (size_t i = 0; i < gyroValues_.size(); i++)
+    {
+        result[i] = gyroValues_[i] - gyroInitValues_[i];
+    }
+
+    //TODO: optimization
+    // 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;
+}
+
+void RobotControlManager::UpdateImuMsg(const ImuMsg::SharedPtr msg)
+{
+    imuMsg_ = *msg;
+    ImuMsg tempMsg = imuMsg_;
+
+    if (tempMsg.orientation.w == 0 && tempMsg.orientation.x == 0 && tempMsg.orientation.y == 0 && tempMsg.orientation.z == 0)
+    {
+        return;
+    }
+
+    if (tempMsg.linear_acceleration.x == 0 && tempMsg.linear_acceleration.y == 0 && tempMsg.linear_acceleration.z == 0)
+    {
+        return;
+    }
+
+    QuaternionToRPYDeg(
+        tempMsg.orientation.w, 
+        tempMsg.orientation.x, 
+        tempMsg.orientation.y, 
+        tempMsg.orientation.z, 
+        gyroValues_[0], 
+        gyroValues_[1], 
+        gyroValues_[2]
+    );
+
+    if (!isGyroInited_)
+    {
+        gyroInitValues_ = gyroValues_;
+        isGyroInited_ = true;
+
+        std::cout << "IMU values : " <<  gyroInitValues_[0] << "  " << gyroInitValues_[1] << "  " << gyroInitValues_[2] << std::endl;
+    }
+    
+
+    gyroVelocities_[0] = tempMsg.angular_velocity.x;
+    gyroVelocities_[1] = tempMsg.angular_velocity.y;
+    gyroVelocities_[2] = tempMsg.angular_velocity.z;
+
+    gyroAccelerations_[0] = tempMsg.linear_acceleration.x;
+    gyroAccelerations_[1] = tempMsg.linear_acceleration.y;
+    gyroAccelerations_[2] = tempMsg.linear_acceleration.z;
+
+    // std::cout << "gyroValues_ : " << gyroValues_[0] << " " << gyroValues_[1] << " " << gyroValues_[2] << std::endl;
+
+}
+
+void RobotControlManager::QuaternionToRPYRad(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw)
+{
+    double norm = sqrt(qw*qw + qx*qx + qy*qy + qz*qz);
+    if (norm < 1e-12) {  // 避免除以零
+        roll = 0;
+        pitch = 0;
+        yaw = 0;
+        return;
+    }
+
+    qw /= norm;
+    qx /= norm;
+    qy /= norm;
+    qz /= norm;
+    
+    // 计算Roll角（X轴）
+    roll = atan2(2 * (qw*qx + qy*qz), 1 - 2 * (qx*qx + qy*qy));
+    
+    // 计算Pitch角（Y轴），限制输入范围避免数值误差（asin输入需在[-1,1]）
+    double sin_pitch = 2 * (qw*qy - qz*qx);
+    sin_pitch = std::max(std::min(sin_pitch, 1.0), -1.0); 
+    pitch = asin(sin_pitch);
+    
+    // 计算Yaw角（Z轴）
+    yaw = atan2(2 * (qw*qz + qx*qy), 1 - 2 * (qy*qy + qz*qz));
+}
+
+void RobotControlManager::QuaternionToRPYDeg(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw)
+{
+    QuaternionToRPYRad(qw, qx, qy, qz, roll, pitch, yaw);
+
+    roll = roll * 180.0 / M_PI;
+    pitch = pitch * 180.0 / M_PI;
+    yaw = yaw * 180.0 / M_PI;
+}
+
 void RobotControlManager::UpdateWheelStates(const MotorPos::SharedPtr msg) 
 {
     motorPos_ = *msg;
@@ -172,6 +323,13 @@ void RobotControlManager::UpdateWheelStates(const MotorPos::SharedPtr msg)
         wheelEfforts_[i] = 0.0;
     }
 
+    if (!isWheelHomed_)
+    {
+        wheelController_ -> SetHomePositions(wheelPositions_);
+        isWheelHomed_ = true;
+    }
+    
+
     wheelController_->UpdateJointStates(wheelPositions_, wheelVelocities_, wheelEfforts_);
 }
 
@@ -247,24 +405,24 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
     waistController_->UpdateJointStates(waistPositions, waistVelocities, waistEfforts);
 
     // Update the leg controller
-    size_t legStartIndex = motionParams_.leg_joint_indices_[0];
-    size_t legJointSize = motionParams_.leg_joint_indices_.size();
+    // size_t legStartIndex = motionParams_.leg_joint_indices_[0];
+    // size_t legJointSize = motionParams_.leg_joint_indices_.size();
 
-    std::vector<double> legPositions(
-        jointPositions_.begin() + legStartIndex, 
-        jointPositions_.begin() + legStartIndex + legJointSize
-    );
+    // std::vector<double> legPositions(
+    //     jointPositions_.begin() + legStartIndex, 
+    //     jointPositions_.begin() + legStartIndex + legJointSize
+    // );
 
-    std::vector<double> legVelocities(
-        jointVelocities_.begin() + legStartIndex, 
-        jointVelocities_.begin() + legStartIndex + legJointSize
-    );
+    // std::vector<double> legVelocities(
+    //     jointVelocities_.begin() + legStartIndex, 
+    //     jointVelocities_.begin() + legStartIndex + legJointSize
+    // );
 
-    std::vector<double> legEfforts(
-        jointEfforts_.begin() + legStartIndex, 
-        jointEfforts_.begin() + legStartIndex + legJointSize
-    );
-    legController_->UpdateJointStates(legPositions, legVelocities, legEfforts);
+    // std::vector<double> legEfforts(
+    //     jointEfforts_.begin() + legStartIndex, 
+    //     jointEfforts_.begin() + legStartIndex + legJointSize
+    // );
+    // legController_->UpdateJointStates(legPositions, legVelocities, legEfforts);
 }
 
 MotionParameters RobotControlManager::GetMotionParameters()
@@ -279,11 +437,11 @@ bool RobotControlManager::IsMoving(MovementPart part)
         case MovementPart::WHEEL:
             return wheelController_->IsMoving();
         case MovementPart::LEG:
-            return legController_->IsMoving();
+            return true; // legController_->IsMoving();
         case MovementPart::WAIST:
             return waistController_->IsMoving();
         case MovementPart::ALL:
-            return wheelController_->IsMoving() && legController_->IsMoving() && waistController_->IsMoving();
+            return wheelController_->IsMoving()  && waistController_->IsMoving() ; //&& legController_->IsMoving();
         default:
             return false;
     }
@@ -299,12 +457,12 @@ bool RobotControlManager::Stop(double dt)
     AssignTempValues();
 
     bool waistStopped = waistController_->Stop(tempWaistCmd_, dt);
-    bool legStopped = legController_->Stop(tempLegCmd_, dt);
+    // bool legStopped = legController_->Stop(tempLegCmd_, dt);
     bool wheelStopped = wheelController_->Stop(tempWheelCmd_, dt);
 
     UpdateJointCommands();
 
-    return legStopped  && waistStopped && wheelStopped;
+    return waistStopped && wheelStopped; //legStopped  && 
 }
 
 bool RobotControlManager::MoveLeg(double dt)
@@ -353,11 +511,13 @@ bool RobotControlManager::GoHome(double dt)
     {
         isWaistHomed_ = waistController_->GoHome(tempWaistCmd_, dt);
     }
+
+    isLegHomed_ = true;
     
-    if (!isLegHomed_)
-    {
-        isLegHomed_ = legController_->GoHome(tempLegCmd_, dt);
-    }
+    // if (!isLegHomed_)
+    // {
+    //     isLegHomed_ = legController_->GoHome(tempLegCmd_, dt);
+    // }
 
     UpdateJointCommands();
 
@@ -392,12 +552,12 @@ void RobotControlManager::AssignTempValues()
         tempWaistCmd_[i] = jointCommands_.data[motionParams_.waist_joint_indices_[i]];
     }
 
-    tempLegCmd_.resize(motionParams_.leg_joint_indices_.size());
+    // tempLegCmd_.resize(motionParams_.leg_joint_indices_.size());
 
-    for (size_t i = 0; i < motionParams_.leg_joint_indices_.size(); i++)
-    {
-        tempLegCmd_[i] = jointCommands_.data[motionParams_.leg_joint_indices_[i]];
-    }
+    // for (size_t i = 0; i < motionParams_.leg_joint_indices_.size(); i++)
+    // {
+    //     tempLegCmd_[i] = jointCommands_.data[motionParams_.leg_joint_indices_[i]];
+    // }
 }
 
 void RobotControlManager::UpdateJointCommands()
@@ -407,8 +567,8 @@ void RobotControlManager::UpdateJointCommands()
         jointCommands_.data[motionParams_.waist_joint_indices_[i]] = tempWaistCmd_[i];
     }
 
-    for (size_t i = 0; i < motionParams_.leg_joint_indices_.size(); i++)
-    {
-        jointCommands_.data[motionParams_.leg_joint_indices_[i]] = tempLegCmd_[i];
-    }
+    // for (size_t i = 0; i < motionParams_.leg_joint_indices_.size(); i++)
+    // {
+    //     jointCommands_.data[motionParams_.leg_joint_indices_[i]] = tempLegCmd_[i];
+    // }
 }

src/robot_control/src/robot_control_node.cpp

@@ -17,8 +17,10 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
     isJogMode_ = false;
     jogDirection_ = 0;
     jogIndex_ = 0;
+    lastSpeed_ = 51;
  
     // 初始化数据文件（设置路径，确保目录存在）
+    #if 0
     data_file_path_ = "/home/demo/ros2_joint_data.txt";  // 数据保存路径
     fs::path file_path(data_file_path_);
     fs::create_directories(file_path.parent_path());  // 自动创建父目录（如果不存在）
@@ -29,6 +31,7 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
         // 写入表头（仅在文件为空时）
         data_file_.seekp(0, std::ios::end);  // 移动到文件末尾
     }
+    #endif
 
     using namespace std::placeholders;
         
@@ -75,10 +78,12 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
 
     motorCmdPub_ = this->create_publisher<MotorCmd>("/motor_cmd", 10);
     ethercatSetPub_ = this->create_publisher<std_msgs::msg::String>("/ethercat/set", 10);
+    imuMsgSub_ = this->create_subscription<ImuMsg>("/imu_msg", 10,std::bind(&RobotControlNode::ImuMsgCallback, this, std::placeholders::_1));
     jointStatesSub_ = this->create_subscription<sensor_msgs::msg::JointState>("/ec_joint_states", 10,std::bind(&RobotControlNode::JointStatesCallback, this, std::placeholders::_1));
     wheelStatesSub_ = this->create_subscription<MotorPos>("/motor_pos", 10,std::bind(&RobotControlNode::WheelStatesCallback, this, std::placeholders::_1));
     joyCommandSub_ = this->create_subscription<std_msgs::msg::String>("/gamepad_msg", 10,std::bind(&RobotControlNode::JoyCommandCallback, this, std::placeholders::_1));
     controlTimer_ = this->create_wall_timer(std::chrono::milliseconds(CYCLE_TIME),std::bind(&RobotControlNode::ControlLoop, this));  // 绑定到新的控制函数);
+    motorParamClient_ = this->create_client<MotorParam>("/motor_param");
 
     lastTime_ = this->now();  // 初始化时间
     std::cout << "RobotFsm Node is created finished!" << std::endl;
@@ -95,8 +100,6 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_home_goal(
     const rclcpp_action::GoalUUID & uuid,
     std::shared_ptr<const MoveHome::Goal> goal)
 {
-    RCLCPP_INFO(this->get_logger(), "Received move home request");
-
     (void)goal;
 
     if (robotControlManager_.IsMoving(MovementPart::ALL))
@@ -125,7 +128,6 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_home_goal(
 rclcpp_action::CancelResponse RobotControlNode::handle_move_home_cancel(
     const std::shared_ptr<GoalHandleMoveHome> goal_handle)
 {
-    RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
     (void)goal_handle;
     return rclcpp_action::CancelResponse::ACCEPT;
 }
@@ -134,7 +136,6 @@ rclcpp_action::CancelResponse RobotControlNode::handle_move_home_cancel(
 void RobotControlNode::handle_move_home_accepted(const std::shared_ptr<GoalHandleMoveHome> goal_handle)
 {
     move_home_executing_ = true;
-    RCLCPP_INFO(this->get_logger(), "Goal accepted");
 
     using namespace std::placeholders;
     std::thread{std::bind(&RobotControlNode::move_home_execute, this, _1), goal_handle}.detach();
@@ -143,7 +144,7 @@ void RobotControlNode::handle_move_home_accepted(const std::shared_ptr<GoalHandl
 // 执行目标计算
 void RobotControlNode::move_home_execute(const std::shared_ptr<GoalHandleMoveHome> 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<MoveHome::Feedback>();
@@ -186,8 +187,6 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_leg_goal(
     const rclcpp_action::GoalUUID & uuid,
     std::shared_ptr<const MoveLeg::Goal> goal)
 {
-    RCLCPP_INFO(this->get_logger(), "Received move leg request");
-
     if (robotControlManager_.IsMoving(MovementPart::LEG))
     {
          RCLCPP_ERROR(this->get_logger(), "Robot is moving");
@@ -221,7 +220,6 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_leg_goal(
 rclcpp_action::CancelResponse RobotControlNode::handle_move_leg_cancel(
     const std::shared_ptr<GoalHandleMoveLeg> goal_handle)
 {
-    RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
     (void)goal_handle;
     return rclcpp_action::CancelResponse::ACCEPT;
 }
@@ -230,7 +228,6 @@ rclcpp_action::CancelResponse RobotControlNode::handle_move_leg_cancel(
 void RobotControlNode::handle_move_leg_accepted(const std::shared_ptr<GoalHandleMoveLeg> goal_handle)
 {
     move_leg_executing_ = true;
-    RCLCPP_INFO(this->get_logger(), "Goal accepted");
 
     using namespace std::placeholders;
     std::thread{std::bind(&RobotControlNode::move_leg_execute, this, _1), goal_handle}.detach();
@@ -239,7 +236,7 @@ void RobotControlNode::handle_move_leg_accepted(const std::shared_ptr<GoalHandle
 // 执行目标计算
 void RobotControlNode::move_leg_execute(const std::shared_ptr<GoalHandleMoveLeg> 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<MoveLeg::Feedback>();
@@ -282,8 +279,6 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_waist_goal(
     const rclcpp_action::GoalUUID & uuid,
     std::shared_ptr<const MoveWaist::Goal> goal)
 {
-    RCLCPP_INFO(this->get_logger(), "Received move waist request");
-
     if (robotControlManager_.IsMoving(MovementPart::WAIST))
     {
          RCLCPP_ERROR(this->get_logger(), "Robot is moving");
@@ -317,7 +312,6 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_waist_goal(
 rclcpp_action::CancelResponse RobotControlNode::handle_move_waist_cancel(
     const std::shared_ptr<GoalHandleMoveWaist> goal_handle)
 {
-    RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
     (void)goal_handle;
     return rclcpp_action::CancelResponse::ACCEPT;
 }
@@ -326,7 +320,6 @@ rclcpp_action::CancelResponse RobotControlNode::handle_move_waist_cancel(
 void RobotControlNode::handle_move_waist_accepted(const std::shared_ptr<GoalHandleMoveWaist> goal_handle)
 {
     move_waist_executing_ = true;
-    RCLCPP_INFO(this->get_logger(), "Goal accepted");
     using namespace std::placeholders;
     std::thread{std::bind(&RobotControlNode::move_waist_execute, this, _1), goal_handle}.detach();
 }
@@ -334,7 +327,7 @@ void RobotControlNode::handle_move_waist_accepted(const std::shared_ptr<GoalHand
 // 执行目标计算
 void RobotControlNode::move_waist_execute(const std::shared_ptr<GoalHandleMoveWaist> 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<MoveWaist::Feedback>();
@@ -377,8 +370,6 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_wheel_goal(
     const rclcpp_action::GoalUUID & uuid,
     std::shared_ptr<const MoveWheel::Goal> goal)
 {
-    RCLCPP_INFO(this->get_logger(), "Received move wheel request");
-
     if (robotControlManager_.IsMoving(MovementPart::WHEEL))
     {
          RCLCPP_ERROR(this->get_logger(), "Robot is moving");
@@ -391,13 +382,20 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_wheel_goal(
         return rclcpp_action::GoalResponse::REJECT;
     }
 
-    if (!robotControlManager_.SetMoveWheelParameters(goal->move_distance, goal->move_angle))
+    if (goal->move_distance > 2.1 || goal->move_distance < -1.0 ||  goal->move_angle < -10 ||  goal->move_angle > 10 )
     {
-        RCLCPP_ERROR(this->get_logger(), "Invalid move wheel request");
+        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");
+    //     return rclcpp_action::GoalResponse::REJECT;
+    // }
+    
+
     (void)uuid;
     return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
 }
@@ -406,7 +404,6 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_wheel_goal(
 rclcpp_action::CancelResponse RobotControlNode::handle_move_wheel_cancel(
     const std::shared_ptr<GoalHandleMoveWheel> goal_handle)
 {
-    RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
     (void)goal_handle;
     return rclcpp_action::CancelResponse::ACCEPT;
 }
@@ -429,6 +426,58 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWh
     auto feedback = std::make_shared<MoveWheel::Feedback>();
     auto result = std::make_shared<MoveWheel::Result>();
 
+    double wheelAngle = 0;
+
+    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;
+
+    robotControlManager_.SetMoveWheelParameters(goal->move_distance, wheelAngle);
+
+    double ratio = robotControlManager_.GetWheelRatio();
+
+    auto request = std::make_shared<MotorParam::Request>();
+
+    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<uint16_t>(round((ratio) * 51));
+
+        // if (goal->move_distance < -0.5)
+        // {
+        //     request->add_acc = 4;
+        // }
+        
+        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(50)); 
+    }
+    
     robotControlManager_.MoveWheel();
 
     std_msgs::msg::Float64MultiArray wheel_commands;
@@ -441,6 +490,9 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWh
     wheel_commands_msg.motor_id = {1,2};
     wheel_commands_msg.motor_angle = {(float)(wheel_commands.data[0]),(float)(wheel_commands.data[1])};
 
+    // std::cout << "wheel_commands.data[0] " << wheel_commands.data[0] << std::endl;
+    // std::cout << "wheel_commands.data[1] " << wheel_commands.data[1] << std::endl;
+
     motorCmdPub_->publish(wheel_commands_msg);
 
     while (move_wheel_executing_ && rclcpp::ok())
@@ -457,7 +509,7 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWh
 
         auto joint_feedback = robotControlManager_.GetWheelFeedback();
 
-        if (joint_feedback.data[0] - wheel_commands.data[0] < 10)
+        if (abs(joint_feedback.data[0] - wheel_commands.data[0]) < 20.0 && abs(joint_feedback.data[1] - wheel_commands.data[1]) < 20.0)
         {
             move_wheel_executing_ = false;
         }
@@ -465,12 +517,23 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWh
         feedback->current_pos = joint_feedback.data[0];
 
         //TODO: get the angle from lidar.
-        feedback->current_angle = 0.0;
+        //feedback->current_angle = robotControlManager_.GetImuDifference()[2];
 
         goal_handle->publish_feedback(feedback);
 
         loop_rate.sleep();
     }
+
+    if ((goal->move_distance > 0.0) && !robotControlManager_.GetJogWheel())   //|| goal->move_distance < -0.5
+    {
+        request->motor_id = 1;
+        request->max_speed = 51;
+        request->add_acc = 8;
+        request->dec_acc = 8;
+        
+        motorParamClient_->async_send_request(request);
+    }
+
     // 检查目标是否仍在活跃状态
     if (rclcpp::ok())
     {
@@ -505,7 +568,10 @@ void RobotControlNode::ControlLoop() {
     {
         if(robotControlManager_.GoHome(dt_sec))
         {
+            robotControlManager_.SetJogWheel(false);
             move_home_executing_ = false;
+            robotControlManager_.WheelReset();
+            robotControlManager_.ImuReset();
         }
     }
 
@@ -545,6 +611,7 @@ void RobotControlNode::Publish_joint_trajectory()
         cmd_msg = robotControlManager_.GetJointCommands();
     }
 
+    #if 0
     data_file_ << 0;
 
     // 2. 保存整个数组数据到txt文件
@@ -557,6 +624,8 @@ void RobotControlNode::Publish_joint_trajectory()
         data_file_.flush();  // 强制刷新
     }
 
+    #endif
+
     std_msgs::msg::String positionMsg;
     std::stringstream msg_stream; 
     size_t total_joints = robotControlManager_.GetMotionParameters().total_joints_;
@@ -683,6 +752,16 @@ void RobotControlNode::JointStatesCallback(const sensor_msgs::msg::JointState::S
     robotControlManager_.UpdateJointStates(msg);
 }
 
+void RobotControlNode::ImuMsgCallback(const ImuMsg::SharedPtr cmd_msg)
+{
+    if (!cmd_msg) {  // 检查消息是否有效
+        std::cout << "get null imu msg!" << std::endl;
+        return;
+    }
+
+    robotControlManager_.UpdateImuMsg(cmd_msg);
+}
+
 void RobotControlNode::WheelStatesCallback(const MotorPos::SharedPtr cmd_msg)
 {
     if (!cmd_msg) {  // 检查消息是否有效

src/robot_control/src/trapezoidal_trajectory.cpp

@@ -98,6 +98,7 @@ void TrapezoidalTrajectory::calculateTrajectoryParams()
     std::vector<double> axis_decel_time(dof, 0.0);  // 各轴理论减速时间
     std::vector<double> axis_cruise_time(dof, 0.0); // 各轴理论匀速时间
     std::vector<double> 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;
         }
     }
     

src/robot_control/src/waist_control.cpp

@@ -62,24 +62,54 @@ bool WaistControl::MoveWaist(std::vector<double>& joint_positions, double dt)
             return true;
         }
     }
-
-    bool result = MoveToTargetJoint(joint_positions, joint_position_desired_, dt);
-
-    if (result)
+    
+    if (!is_cmd_send_finished_)
     {
-        is_target_set_ = false;
+        is_cmd_send_finished_ = MoveToTargetJoint(joint_positions, joint_position_desired_, dt);
+    }
+    else
+    {
+        if (IsReached(joint_position_desired_) || (time_out_count_ > TIME_OUT_COUNT))
+        {
+            std::cout << "Waist reached target position!" << std::endl;
+            is_target_set_ = false;
+            is_cmd_send_finished_ = false;
+            time_out_count_ = 0;
+            return true;
+        }
+        else
+        {
+            time_out_count_ ++;
+            // std::cout << "Waist not reached target position!" << std::endl;
+            return false;
+        }
     }
 
-    return result;
+    return false;
 }
 
+// void WaistControl::SetHomePositions(const std::vector<double>& 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;
+//     // }
+    
+//     std::cout << "Home positions set to: " << joint_home_positions_[0] << ", " << joint_home_positions_[1] << std::endl;
+// }
+
 bool WaistControl::SetMoveWaistParametersInternal(double movepitch, double moveyaw)
 {
     std::vector<double> tempPosition;
     tempPosition.resize(total_joints_, 0.0);
 
-    tempPosition[0] = joint_position_[0] - joint_zero_positions_[0] + movepitch;
-    tempPosition[1] = joint_position_[1] - joint_zero_positions_[1] + moveyaw;
+    tempPosition[0] = joint_commands_[0] - joint_home_positions_[0] + movepitch;
+    // tempPosition[1] = joint_commands_[1] - joint_home_positions_[1] + moveyaw;
 
     if (!checkJointLimits(tempPosition))
     {
@@ -89,10 +119,11 @@ bool WaistControl::SetMoveWaistParametersInternal(double movepitch, double movey
 
     for (size_t i = 0; i < joint_position_desired_.size(); i++)
     {
-        joint_position_desired_[i] = tempPosition[i] + joint_zero_positions_[i];
+        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;
 

src/robot_control/src/wheel_control.cpp

@@ -32,33 +32,199 @@ 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::vector<double> tempHomePositions;
+    std::vector<double> 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 * 0.8 / 360.0 * M_PI * 2 * lengthParameters_[1] / lengthParameters_[0] * 180.0 / M_PI;
+
+    if(moveWheelDistance > lastMoveDistance && (moveWheelDistance - lastMoveDistance > 1.5) && beta != 0)
+    {
+        if (beta > 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_position_[i] + theta;
+            tempDesiredPositions[i] = tempHomePositions[i] + theta;
         }
         else
         {
-            joint_position_desired_[i] = joint_position_[i] - theta;
+            tempDesiredPositions[i] = tempHomePositions[i] - theta; // * 0.98 * 1.02;
         }
     }
 
-    //TODO: 设置移动轮角度
-    (void) moveWheelAngle;
+    double a1 = abs(tempDesiredPositions[0] - original1);
+    double a2 = abs(tempDesiredPositions[1] - original2);
 
-    std::cout << "Move Wheel joint_position_desired_: " << joint_position_desired_[0] <<  " , " << joint_position_desired_[1] << std::endl;
+    if (a2 > 0.0 && a1 > 0.0)
+    {
+        moveRatio_ = ((a1 / a2) - 1.0) * 0.9;
+    }
+    else
+    {
+        moveRatio_ = 0.0;
+    }
+
+    moveRatio_ = 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;
+
+    // std::cout << "Move Wheel joint_position_desired_: " << joint_position_desired_[0] <<  " , " << joint_position_desired_[1] << std::endl;
+
+    lastMoveDistance = moveWheelDistance;
 
     is_target_set_ = true;
 
     return true;
 }
 
+bool WheelControl::SetMoveWheelParametersInternalJog(double moveWheelDistance, double moveWheelAngle)
+{
+    double original1 = joint_position_desired_[0];
+    double original2 = joint_position_desired_[1];
+
+    std::vector<double> tempHomePositions;
+    std::vector<double> 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<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double dt)
 {