update @huiyu

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/leg_control.hpp

@@ -6,7 +6,6 @@
 
 namespace robot_control
 {
-
     /**
      * @brief LegControl 类用于控制腿部运动
      * 
@@ -31,6 +30,8 @@ namespace robot_control
 
             bool SetMoveLegParametersInternal(double moveDistance);
 
+            double calculate_angle_from_links(double side1, double side2, double side_opposite, bool is_degree = false);
+
             bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) override;
 
             bool MoveUp(std::vector<double>& joint_positions, double dt);

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,17 +56,20 @@ 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
                 leg_length_ = {
-                    0.70,
-                    0.66
+                    0.66,
+                    0.385,
+                    0.362,
+                    0.55,
+                    0.4
                 };
 
                 waist_length_ = {
@@ -80,21 +83,25 @@ namespace robot_control
 
                 // 关节速度参数
                 max_joint_velocity_ = {
-                    15,
-                    15,
-                    15,
-                    15,
-                    15,
-                    15
-                }; 
-
-                max_joint_acceleration_ = {
+                    30,
+                    30,
                     50,
                     50,
                     50,
                     50,
                     50,
                     50
+                }; 
+
+                max_joint_acceleration_ = {
+                    80,
+                    80,
+                    100,
+                    100,
+                    100,
+                    100,
+                    100,
+                    100
                 };
 
                 // 初始化关节索引
@@ -102,35 +109,33 @@ namespace robot_control
 
                 waist_joint_indices_ = {0, 1}; 
 
-                leg_joint_indices_ = {2, 3, 4, 5};
+                leg_joint_indices_ = {2, 3, 4, 5, 6, 7};
 
-                total_joints_ = 6; 
+                total_joints_ = 8; 
 
                 // 初始化关节方向
                 wheel_joint_directions_ = {1, -1};  
 
                 waist_joint_directions_ = {1, 1};
 
-                leg_joint_directions_ = {1, -1, -1, 1};  
+                leg_joint_directions_ = {-1, 1, 1, -1, -1, 1};  
 
                 //TODO: check the ratio
-                joint_gear_ratio_ = {100.0, 100.0, 100.0, 100.0, 100.0, 100.0};  // 传动比 都一样
+                joint_gear_ratio_ = {100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0};  // 传动比 都一样
                 
-                joint_resolution_ = {524288, 131072, 131072 , 131072, 131072, 131072};  // 分辨率 都一样
+                joint_resolution_ = {524288, 524288, 524288 , 524288, 524288, 524288, 524288, 524288};  // 分辨率 都一样
 
 
                 //TODO: 限位需要修改
                 joint_limits_ = {
-                    JointLimit(0, 30.0,     -30.0, LimitType::POSITION),
-                    JointLimit(1, 180.0,    -180.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),
-                    JointLimit(3, 0.0,     -60.0, LimitType::POSITION),
-                    JointLimit(4, 0,     -80.0, LimitType::POSITION),
-                    // JointLimit(7, 5.0,     -5.0, LimitType::POSITION),
-                    // JointLimit(8, 50.0,     -50.0, LimitType::POSITION),
-                    JointLimit(5, 60.0,     0.0, LimitType::POSITION),
+                    JointLimit(0, 40.0,     -40.0, LimitType::POSITION),
+                    JointLimit(1, 190.0,    -190.0, LimitType::POSITION),
+                    JointLimit(2, 0.0,      -90.0, LimitType::POSITION),
+                    JointLimit(3, 90.0,     0.0, LimitType::POSITION),
+                    JointLimit(4, 60,       0.0, LimitType::POSITION),
+                    JointLimit(5, 0.0,      -90.0, LimitType::POSITION),
+                    JointLimit(6, 0.0,      -60.0, LimitType::POSITION),
+                    JointLimit(7, 90.0,     0.0, LimitType::POSITION),
                 };
 
                 // 初始化限制参数
@@ -185,24 +190,22 @@ namespace robot_control
                 }
                 
                 waist_zero_positions_ = {
-                    181.0, 
-                    158.5
+                    350.0,
+                    261.82
                 };
 
                 leg_zero_positions_ = {
-                    -18.36, 
-                    // 129.71, 
-                    // 45.02,
-                    25.54, 
-                    -21.54, 
-                    // 167.96, 
-                    // 16.26, 
-                    -127.24
+                    217.52,        //316786.46
+                    120.84,        //175986.005
+                    108.7,         //158305.849
+                    221.95,        //323238.116
+                    234.14,        //340991.09
+                    125.39         //182612.423
                 };
 
                 waist_home_positions_ = {
-                    181.0, 
-                    158.5
+                    350.0,
+                    261.82
                 };
 
 
@@ -210,16 +213,23 @@ namespace robot_control
                 // 前腿零点为站直时候的状态, 然后 髋部 pitch + 49度, 大腿长 0.38m, 38度为 0.25m , 小腿长0.36  膝盖 pitch 再收回 35.5 度, 剩余 13.5度 长度为 0.35m
                 // 初始化初始位置
                 leg_home_positions_ = {
-                    30.64, 
-                    // 129.71, 
-                    // 9.52,
-                    0.54, 
-                    -70.54, 
-                    // 167.96, 
-                    // 51.76, 
-                    -102.24
+                    217.52 - 65.0, 
+                    120.84 + 41.0, 
+                    108.7 + 40.63,   //217479
+                    221.95 - 41.0, 
+                    234.14 - 29.504,  //298023 
+                    125.39 + 65.0
                 };
 
+                // leg_home_positions_ = {
+                //     217.52 - 90.0,      //185.714.46
+                //     120.84 + 90.0,      //307058
+                //     108.7 + 0.0,        //158305
+                //     221.95 - 90.0,      //192166
+                //     234.14 - 0.0,       //340991
+                //     125.39 + 90.0       //313684
+                // };
+
                 // 初始化零点位置
                 wheel_home_positions_ = {
                     0.0,

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,9 @@
 #include "interfaces/action/move_waist.hpp"
 #include "interfaces/action/move_wheel.hpp"
 #include "interfaces/msg/motor_cmd.hpp"
+#include "interfaces/srv/motor_param.hpp"
 
+#define RECORD_FLAG 0
 
 using MoveHome = interfaces::action::MoveHome;
 using GoalHandleMoveHome = rclcpp_action::ServerGoalHandle<MoveHome>;
@@ -32,6 +34,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 +85,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 +103,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/leg_control.cpp

@@ -75,48 +75,97 @@ bool LegControl::MoveUp(std::vector<double>& joint_positions, double dt)
     return result;
 }
 
+double LegControl::calculate_angle_from_links(double side1, double side2, double side_opposite, bool is_degree) 
+{
+    const double EPS = 1e-6; // 浮点精度容错值
+    if (side1 < EPS || side2 < EPS || side_opposite < EPS) {
+        throw std::invalid_argument("连杆长度必须大于0！");
+    }
+
+    // 2. 检查是否能构成三角形（任意两边之和大于第三边，容错EPS）
+    if ((side1 + side2 - side_opposite) < EPS ||
+        (side1 + side_opposite - side2) < EPS ||
+        (side2 + side_opposite - side1) < EPS) {
+        throw std::invalid_argument("三边长度无法构成三角形（任意两边之和需大于第三边）！");
+    }
+
+    // 3. 余弦定理计算夹角（弧度）
+    double numerator = side1 * side1 + side2 * side2 - side_opposite * side_opposite;
+    double denominator = 2 * side1 * side2;
+    double cos_theta = numerator / denominator;
+
+    // 4. 处理浮点精度误差：确保cos_theta在[-1, 1]范围内（避免acos定义域错误）
+    cos_theta = std::max(-1.0, std::min(1.0, cos_theta));
+
+    // 5. 计算弧度值
+    double angle_rad = acos(cos_theta);
+
+    // 6. 按需转换为角度
+    if (is_degree) {
+        return angle_rad * 180.0 / M_PI;
+    }
+    return angle_rad;
+}
+
 bool LegControl::SetMoveLegParametersInternal(double moveDistance)
 {
     std::vector<double> tempPosition;
     tempPosition.resize(total_joints_, 0.0);
 
-    double currentFrountLegLength = lengthParameters_[0] * abs(std::cos(DEG2RAD(joint_position_[0] - joint_zero_positions_[0])));
+    double backJoint1 = DEG2RAD(joint_position_[0] - joint_zero_positions_[0]);
+    double frountJoint1 = DEG2RAD(joint_position_[1] - joint_zero_positions_[1]);
+    double frountJoint2 = DEG2RAD(joint_position_[2] - joint_zero_positions_[2]);
 
-    double currentBackLegLength = lengthParameters_[1] * abs(std::cos(DEG2RAD(joint_position_[1] - joint_zero_positions_[1])));
+    double currentBackLegHeight = lengthParameters_[0] * abs(std::sin(backJoint1));
+    double currentFrountLegHeight = lengthParameters_[1] * abs(std::sin(frountJoint1)) + lengthParameters_[2] * abs(std::sin(frountJoint1 + frountJoint2));
+    double currentFrountLegDistance = lengthParameters_[1] * abs(std::cos(frountJoint1)) + lengthParameters_[2] * abs(std::cos(frountJoint1 + frountJoint2));
 
     if (moveDistance > 0)
     {
-        if ((moveDistance + currentFrountLegLength) > (lengthParameters_[0] - 0.02) || (moveDistance + currentBackLegLength) > (lengthParameters_[1] - 0.03))
+        if ((moveDistance + currentFrountLegHeight) > lengthParameters_[3])
         {
             std::cout << "Move distance is too large! " << std::endl;
-            std::cout << " Current Front Leg Length: " << currentFrountLegLength << std::endl;
-            std::cout << " Current Back Leg Length: " << currentBackLegLength << std::endl;
+            std::cout << " Current Front Leg Length: " << currentFrountLegHeight << std::endl;
+            std::cout << " Current Back Leg Length: " << currentBackLegHeight << std::endl;
             std::cout << " Move Distance: " << moveDistance << std::endl;
             return false;
         }
     }
     else
     {
-        if (moveDistance < (-currentFrountLegLength + 0.03) || moveDistance < (-currentBackLegLength + 0.03))
+        if (moveDistance  + currentFrountLegHeight < lengthParameters_[4])
         {
             std::cout << "Move distance is too large!" << std::endl;
-            std::cout << " Current Front Leg Length: " << currentFrountLegLength << std::endl;
-            std::cout << " Current Back Leg Length: " << currentBackLegLength << std::endl;
+            std::cout << " Current Front Leg Length: " << currentFrountLegHeight << std::endl;
+            std::cout << " Current Back Leg Length: " << currentBackLegHeight << std::endl;
             std::cout << " Move Distance: " << moveDistance << std::endl;
             return false;
         }
     }
 
-    double finalFrountLegLength = currentFrountLegLength + moveDistance;
-    double finalBackLegLength = currentBackLegLength + moveDistance;
+    double finalFrountLegHeight = currentFrountLegHeight + moveDistance;
+    double finalBackLegHeight = currentBackLegHeight + moveDistance;
     
-    double frontFinalAngle = RAD2DEG(std::acos(finalFrountLegLength / lengthParameters_[0]));
-    double backFinalAngle = RAD2DEG(std::acos(finalBackLegLength / lengthParameters_[1]));
+    double backFinalAngle = RAD2DEG(std::acos(finalBackLegHeight / lengthParameters_[0]));
 
-    tempPosition[0] = joint_directions_[0] * frontFinalAngle;
-    tempPosition[1] = joint_directions_[1] * backFinalAngle;
-    tempPosition[2] = joint_directions_[2] * frontFinalAngle;
-    tempPosition[3] = joint_directions_[3] * backFinalAngle;
+    double tempFrountLegLength = std::sqrt(finalFrountLegHeight * finalFrountLegHeight + currentFrountLegDistance * currentFrountLegDistance);
+    double frountAngle1_part1 = calculate_angle_from_links(finalFrountLegHeight, tempFrountLegLength, currentFrountLegDistance, true);
+    double frountAngle1_part2 = calculate_angle_from_links(tempFrountLegLength, lengthParameters_[1], lengthParameters_[2], true);
+
+    std::cout << "frountAngle1_part1 : " << frountAngle1_part1 << std::endl;
+    std::cout << "frountAngle1_part2 : " << frountAngle1_part2 << std::endl;
+
+    double frountFinalAngle1 = frountAngle1_part1 + frountAngle1_part2;
+
+    double frountFinalAngle2 = calculate_angle_from_links(lengthParameters_[1], lengthParameters_[2], tempFrountLegLength, true);
+    
+
+    tempPosition[0] = joint_directions_[0] * backFinalAngle;
+    tempPosition[1] = joint_directions_[1] * (90.0 - frountFinalAngle1);    //零点为水平方向
+    tempPosition[2] = joint_directions_[2] * (180.0 - frountFinalAngle2);
+    tempPosition[3] = joint_directions_[3] * (90.0 - frountFinalAngle1);
+    tempPosition[4] = joint_directions_[4] * (180.0 - frountFinalAngle2);
+    tempPosition[5] = joint_directions_[5] * backFinalAngle;
 
     if (!checkJointLimits(tempPosition))
     {
@@ -129,10 +178,15 @@ bool LegControl::SetMoveLegParametersInternal(double moveDistance)
         joint_position_desired_[i] = tempPosition[i] + joint_zero_positions_[i];
     }
 
-    std::cout << "Target Joint Position: " 
-    << joint_position_desired_[0] 
-    << ", " << joint_position_desired_[1] << ", " 
-    <<  joint_position_desired_[2] << ", " << joint_position_desired_[3] << std::endl;
+    std::cout << "Target relative joint Position: " 
+    <<  tempPosition[0] << ", " << tempPosition[1] << ", " 
+    <<  tempPosition[2] << ", " << tempPosition[3] << ", " 
+    <<  tempPosition[4] << ", " << tempPosition[5] << std::endl;
+
+    std::cout << "Target abs Joint Position: " 
+    <<  joint_position_desired_[0] << ", " << joint_position_desired_[1] << ", " 
+    <<  joint_position_desired_[2] << ", " << joint_position_desired_[3] << ", " 
+    <<  joint_position_desired_[4] << ", " << joint_position_desired_[5] << std::endl;
 
     is_target_set_ = true;
 

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_);
 }
 
@@ -283,7 +441,7 @@ bool RobotControlManager::IsMoving(MovementPart part)
         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;
     }
@@ -304,7 +462,7 @@ bool RobotControlManager::Stop(double dt)
 
     UpdateJointCommands();
 
-    return legStopped  && waistStopped && wheelStopped;
+    return waistStopped && legStopped && wheelStopped;
 }
 
 bool RobotControlManager::MoveLeg(double dt)
@@ -338,7 +496,6 @@ bool RobotControlManager::MoveWheel()
     for (size_t i = 0; i < motionParams_.wheel_joint_indices_.size(); i++)
     {
         wheelCommands_.data[motionParams_.wheel_joint_indices_[i]] = tempWheelCmd_[i];
-        // std::cout << "Wheel commands: " << i << " " << wheelCommands_.data[motionParams_.wheel_joint_indices_[i]] <<  std::endl;
     }
 
     return result;
@@ -353,7 +510,7 @@ bool RobotControlManager::GoHome(double dt)
     {
         isWaistHomed_ = waistController_->GoHome(tempWaistCmd_, dt);
     }
-    
+
     if (!isLegHomed_)
     {
         isLegHomed_ = legController_->GoHome(tempLegCmd_, dt);

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 RECORD_FLAG
     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>("/openzen/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,6 @@ 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::Rate loop_rate(10); // 10Hz更新频率
     const auto goal = goal_handle->get_goal();
     auto feedback = std::make_shared<MoveLeg::Feedback>();
@@ -253,7 +249,6 @@ void RobotControlNode::move_leg_execute(const std::shared_ptr<GoalHandleMoveLeg>
             goal_handle->canceled(result);
             RCLCPP_INFO(this->get_logger(), "move leg canceled");
             move_leg_executing_ = false;
-            //TODO: ADD STOP LOGIC
             return;
         }
 
@@ -282,8 +277,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 +310,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 +318,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 +325,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 +368,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,12 +380,11 @@ 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;
     }
-    
 
     (void)uuid;
     return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
@@ -406,7 +394,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 +416,46 @@ 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;
+    }
+
+    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
+    {
+        request->motor_id = 1;
+        request->max_speed = static_cast<uint16_t>(round((ratio) * 51));
+        
+        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 +468,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 +487,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 +495,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())
+    {
+        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 +546,10 @@ void RobotControlNode::ControlLoop() {
     {
         if(robotControlManager_.GoHome(dt_sec))
         {
+            robotControlManager_.SetJogWheel(false);
             move_home_executing_ = false;
+            robotControlManager_.WheelReset();
+            robotControlManager_.ImuReset();
         }
     }
 
@@ -545,6 +589,7 @@ void RobotControlNode::Publish_joint_trajectory()
         cmd_msg = robotControlManager_.GetJointCommands();
     }
 
+    #if RECORD_FLAG
     data_file_ << 0;
 
     // 2. 保存整个数组数据到txt文件
@@ -557,6 +602,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 +730,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/rs485_driver.cpp

@@ -1,419 +0,0 @@
-#include "rs485_driver.hpp"
-#include <fcntl.h>
-#include <unistd.h>
-#include <cstring>
-#include <iostream>
-#include <chrono>
-#include <thread>
-
-namespace robot_control
-{
-
-    const unsigned char CRC8Table[]={  
-        0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, 65,  
-        157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96, 130, 220,  
-        35, 125, 159, 193, 66, 28, 254, 160, 225, 191, 93, 3, 128, 222, 60, 98,  
-        190, 224, 2, 92, 223, 129, 99, 61, 124, 34, 192, 158, 29, 67, 161, 255,  
-        70, 24, 250, 164, 39, 121, 155, 197, 132, 218, 56, 102, 229, 187, 89, 7,  
-        219, 133, 103, 57, 186, 228, 6, 88, 25, 71, 165, 251, 120, 38, 196, 154,  
-        101, 59, 217, 135, 4, 90, 184, 230, 167, 249, 27, 69, 198, 152, 122, 36,  
-        248, 166, 68, 26, 153, 199, 37, 123, 58, 100, 134, 216, 91, 5, 231, 185,  
-        140, 210, 48, 110, 237, 179, 81, 15, 78, 16, 242, 172, 47, 113, 147, 205,  
-        17, 79, 173, 243, 112, 46, 204, 146, 211, 141, 111, 49, 178, 236, 14, 80,  
-        175, 241, 19, 77, 206, 144, 114, 44, 109, 51, 209, 143, 12, 82, 176, 238,  
-        50, 108, 142, 208, 83, 13, 239, 177, 240, 174, 76, 18, 145, 207, 45, 115,  
-        202, 148, 118, 40, 171, 245, 23, 73, 8, 86, 180, 234, 105, 55, 213, 139,  
-        87, 9, 235, 181, 54, 104, 138, 212, 149, 203, 41, 119, 244, 170, 72, 22,  
-        233, 183, 85, 11, 136, 214, 52, 106, 43, 117, 151, 201, 74, 20, 246, 168,  
-        116, 42, 200, 150, 21, 75, 169, 247, 182, 232, 10, 84, 215, 137, 107, 53  
-    }; 
-    /**
-        *	@name		CRC8_Table
-    * @brief  This function is used to Get CRC8 check value.
-        * @param  p :Array pointer. counter :Number of array elements.
-    * @retval crc8
-    */
-    unsigned char CRC8_Table(unsigned char *p, int counter)  
-    {  
-        unsigned char crc8 = 0;  
-    
-        for( ; counter > 0; counter--)
-        {  
-        crc8 = CRC8Table[crc8^*p];  
-        p++;  
-        }  
-        return(crc8);  
-    }
-
-    RS485Driver::RS485Driver(size_t maxMotors) : max_motors_(maxMotors), serial_port_(-1), is_open_(false)
-    {
-        control_index_ = 0;
-    }
-    
-    RS485Driver::~RS485Driver()
-    {
-        closePort();
-    }
-    
-    speed_t RS485Driver::convertBaudRate(int baud_rate)
-    {
-        switch (baud_rate)
-        {
-            case 9600: return B9600;
-            case 19200: return B19200;
-            case 38400: return B38400;
-            case 57600: return B57600;
-            case 115200: return B115200;
-            case 230400: return B230400;
-            case 460800: return B460800;
-            case 500000: return B500000;
-            case 576000: return B576000;
-            case 921600: return B921600;
-            default: return B0;
-        }
-    }
-    
-    bool RS485Driver::openPort(const std::string &port_name, int baud_rate)
-    {
-        // 关闭已打开的端口
-        if (is_open_)
-        {
-            closePort();
-        }
-        
-        // 打开串口
-        serial_port_ = open(port_name.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
-        if (serial_port_ == -1)
-        {
-            std::cerr << "无法打开串口: " << port_name << std::endl;
-            return false;
-        }
-        
-        // 保存原始配置
-        if (tcgetattr(serial_port_, &original_termios_) == -1)
-        {
-            std::cerr << "无法获取串口属性" << std::endl;
-            close(serial_port_);
-            serial_port_ = -1;
-            return false;
-        }
-        
-        // 配置串口
-        struct termios tty;
-        memset(&tty, 0, sizeof(tty));
-        
-        // 设置波特率
-        speed_t speed = convertBaudRate(baud_rate);
-        if (speed == B0)
-        {
-            std::cerr << "不支持的波特率: " << baud_rate << std::endl;
-            closePort();
-            return false;
-        }
-        
-        cfsetospeed(&tty, speed);
-        cfsetispeed(&tty, speed);
-        
-        // 8位数据位，无奇偶校验，1位停止位
-        tty.c_cflag &= ~PARENB;  // 无奇偶校验
-        tty.c_cflag &= ~CSTOPB;  // 1位停止位
-        tty.c_cflag &= ~CSIZE;   // 清除数据位设置
-        tty.c_cflag |= CS8;      // 8位数据位
-        
-        // 禁用硬件流控制
-        tty.c_cflag &= ~CRTSCTS;
-        
-        // 启用接收器，设置本地模式
-        tty.c_cflag |= (CLOCAL | CREAD);
-        
-        // 禁用软件流控制
-        tty.c_iflag &= ~(IXON | IXOFF | IXANY);
-        
-        // 原始输入模式
-        tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
-        
-        // 原始输出模式
-        tty.c_oflag &= ~OPOST;
-        
-        // 设置读取超时
-        tty.c_cc[VTIME] = 10;  // 1秒超时
-        tty.c_cc[VMIN] = 0;
-        
-        // 应用配置
-        if (tcsetattr(serial_port_, TCSANOW, &tty) != 0)
-        {
-            std::cerr << "无法设置串口属性" << std::endl;
-            closePort();
-            return false;
-        }
-        
-        is_open_ = true;
-        std::cout << "串口 " << port_name << " 已打开，波特率: " << baud_rate << std::endl;
-        return true;
-    }
-    
-    void RS485Driver::closePort()
-    {
-        if (is_open_)
-        {
-            // 恢复原始配置
-            tcsetattr(serial_port_, TCSANOW, &original_termios_);
-            close(serial_port_);
-            is_open_ = false;
-            serial_port_ = -1;
-            std::cout << "串口已关闭" << std::endl;
-        }
-    }
-    
-    bool RS485Driver::sendData(const std::vector<uint8_t> &data)
-    {
-        if (!is_open_)
-        {
-            std::cerr << "串口未打开，无法发送数据" << std::endl;
-            return false;
-        }
-        
-        ssize_t bytes_written = write(serial_port_, data.data(), data.size());
-        if (bytes_written != static_cast<ssize_t>(data.size()))
-        {
-            std::cerr << "发送数据失败，预期发送 " << data.size() 
-                      << " 字节，实际发送 " << bytes_written << " 字节" << std::endl;
-            return false;
-        }
-        
-        return true;
-    }
-    
-    bool RS485Driver::receiveData(std::vector<uint8_t> &data, size_t max_length, int timeout_ms)
-    {
-        if (!is_open_)
-        {
-            std::cerr << "串口未打开，无法接收数据" << std::endl;
-            return false;
-        }
-        
-        data.clear();
-        uint8_t buffer[256];
-        ssize_t bytes_read;
-        auto start_time = std::chrono::steady_clock::now();
-        
-        while (data.size() < max_length)
-        {
-            // 检查超时
-            auto current_time = std::chrono::steady_clock::now();
-            auto elapsed_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
-                current_time - start_time).count();
-            
-            if (elapsed_ms >= timeout_ms)
-            {
-                break;  // 超时
-            }
-            
-            // 尝试读取数据
-            bytes_read = read(serial_port_, buffer, std::min(sizeof(buffer), max_length - data.size()));
-            
-            if (bytes_read > 0)
-            {
-                data.insert(data.end(), buffer, buffer + bytes_read);
-                start_time = std::chrono::steady_clock::now();  // 重置超时计时器
-            }
-            else if (bytes_read < 0)
-            {
-                std::cerr << "接收数据错误" << std::endl;
-                return false;
-            }
-            else
-            {
-                // 没有数据，短暂休眠
-                std::this_thread::sleep_for(std::chrono::milliseconds(10));
-            }
-        }
-        
-        return !data.empty();
-    }
-
-    uint8_t RS485Driver::calculateChecksum(const std::vector<uint8_t> &data)
-    {
-        uint8_t checksum = 0;
-        for (uint8_t byte : data)
-        {
-            checksum ^= byte;
-        }
-        return checksum;
-    }
-    
-    bool RS485Driver::controlMotor(uint8_t motor_id, int32_t speed, bool stop)
-    {
-        if (!isOpen())
-        {
-            std::cerr << "RS485未初始化，无法控制电机" << std::endl;
-            return false;
-        }
-        
-        if (motor_id < 1 || motor_id > max_motors_)
-        {
-            std::cerr << "无效的电机ID: " << static_cast<int>(motor_id) 
-                      << ", 必须在1到" << static_cast<int>(max_motors_) << "之间" << std::endl;
-            return false;
-        }
-        
-        // 限制速度范围
-        if (speed > 1000) speed = 1000;
-        if (speed < -1000) speed = -1000;
-        
-        // 构建命令帧
-        std::vector<uint8_t> command;
-        command.push_back(START_BYTE);
-        
-        if (stop)
-        {
-            command.push_back(STOP_CMD);
-        }
-        else
-        {
-            command.push_back(CONTROL_CMD);
-        }
-        
-        command.push_back(motor_id);
-        
-        // 将速度转换为4字节并添加到命令
-        uint8_t speed_bytes[4];
-        std::memcpy(speed_bytes, &speed, sizeof(speed));
-        for (int i = 0; i < 4; ++i)
-        {
-            command.push_back(speed_bytes[i]);
-        }
-        
-        // 计算并添加校验和
-        uint8_t checksum = calculateChecksum(command);
-        command.push_back(checksum);
-        
-        // 添加结束字节
-        command.push_back(STOP_BYTE);
-        
-        // 发送命令
-        bool success = sendData(command);
-        
-        if (success)
-        {
-            if (stop)
-            {
-                std::cout << "已发送停止命令到电机 " << static_cast<int>(motor_id) << std::endl;
-            }
-            else
-            {
-                std::cout << "已发送命令到电机 " << static_cast<int>(motor_id) 
-                          << ", 速度: " << speed << std::endl;
-            }
-        }
-        else
-        {
-            std::cerr << "发送命令到电机 " << static_cast<int>(motor_id) << " 失败" << std::endl;
-        }
-        
-        return success;
-    }
-    
-    bool RS485Driver::controlAllMotors(const std::vector<int32_t> &speeds)
-    {
-        if (speeds.size() != max_motors_)
-        {
-            std::cerr << "控制所有电机需要提供 " << static_cast<int>(max_motors_) 
-                      << " 个速度值，实际提供了 " << speeds.size() << " 个" << std::endl;
-            return false;
-        }
-        
-        if (!isOpen())
-        {
-            std::cerr << "RS485未初始化，无法控制电机" << std::endl;
-            return false;
-        }
-        
-        // 构建命令帧
-        
-        bool success = false;
-
-        static uint8_t i = 1;
-
-        // 添加所有电机的速度
-
-        std::vector<uint8_t> command;
-
-        if (i < 5)
-        {
-            control_index_ = 1;
-            // 限制速度范围
-            int32_t clamped_speed = speeds[control_index_ - 1];
-            if (clamped_speed > 1000) clamped_speed = 1000;
-            if (clamped_speed < -1000) clamped_speed = -1000;
-
-            uint8_t high_byte = (clamped_speed >> 8) & 0xFF;  // 高8位
-            uint8_t low_byte  = clamped_speed & 0xFF;         // 低8位
-
-            command.push_back(2);
-            command.push_back(CONTROL_CMD);
-            command.push_back(high_byte);
-            command.push_back(low_byte);
-            command.push_back(ZERO_CMD);
-            command.push_back(ZERO_CMD);
-            command.push_back(DEFAULT_TIME);
-            command.push_back(ZERO_CMD);
-            command.push_back(ZERO_CMD);
-
-            unsigned char crc8 = CRC8_Table(command.data(), 9);
-
-            command.push_back(crc8);
-            
-            // 发送命令
-            success = sendData(command);
-            // std::cout << "已发送命令到电机 " << static_cast<int>(control_index_) << ", 速度: " << clamped_speed << std::endl;
-        }
-        else if ( i > 5)
-        {
-            control_index_ = 2;
-            // 限制速度范围
-            int32_t clamped_speed = speeds[control_index_ - 1];
-            if (clamped_speed > 1000) clamped_speed = 1000;
-            if (clamped_speed < -1000) clamped_speed = -1000;
-
-            uint8_t high_byte = (clamped_speed >> 8) & 0xFF;  // 高8位
-            uint8_t low_byte  = clamped_speed & 0xFF;         // 低8位
-
-            command.push_back(3);
-            command.push_back(CONTROL_CMD);
-            command.push_back(high_byte);
-            command.push_back(low_byte);
-            command.push_back(ZERO_CMD);
-            command.push_back(ZERO_CMD);
-            command.push_back(DEFAULT_TIME);
-            command.push_back(ZERO_CMD);
-            command.push_back(ZERO_CMD);
-
-            unsigned char crc8 = CRC8_Table(command.data(), 9);
-
-            command.push_back(crc8);
-            
-            // 发送命令
-            success = sendData(command);
-            // std::cout << "已发送命令到电机 " << static_cast<int>(control_index_) << ", 速度: " << clamped_speed << std::endl;
-        }
-
-        ++i;
-
-        // std::cout << "i = " << static_cast<int>(i) << std::endl;
-
-        if (i > 8)
-        {
-            i = 1;
-            return success;
-        }
-        
-        return false;
-    }
-    
-    bool RS485Driver::stopAllMotors()
-    {
-        std::vector<int32_t> speeds(max_motors_, 0);
-        return controlAllMotors(speeds);
-    }
-}

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] << std::endl;
 
     is_target_set_ = true;
 

src/robot_control/src/wheel_control.cpp

@@ -32,33 +32,193 @@ 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;
+        }
+    }
+    
     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];
+        }
+    }
+
+    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)
 {