diff --git a/src/robot_control/include/motion_parameters.hpp b/src/robot_control/include/motion_parameters.hpp
index 92841d7..c47ad4a 100644
--- a/src/robot_control/include/motion_parameters.hpp
+++ b/src/robot_control/include/motion_parameters.hpp
@@ -114,7 +114,7 @@ namespace robot_control
                 //TODO: check the ratio
                 joint_gear_ratio_ = {100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0};  // 传动比 都一样
                 
-                joint_resolution_ = {524288, 131072 , 131072, 131072, 131072, 131072, 131072, 131072, 131072, 131072};  // 分辨率 都一样
+                joint_resolution_ = {131072, 131072 , 131072, 131072, 524288, 131072, 131072, 131072, 131072, 131072};  // 分辨率 都一样
 
 
                 //TODO: 限位需要修改
diff --git a/src/robot_control/src/robot_control_manager.cpp b/src/robot_control/src/robot_control_manager.cpp
index 98cad3e..8b8a5ca 100644
--- a/src/robot_control/src/robot_control_manager.cpp
+++ b/src/robot_control/src/robot_control_manager.cpp
@@ -145,8 +145,8 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
     jointVelocities_.resize(motionParams_.total_joints_);
     jointEfforts_.resize(motionParams_.total_joints_);
 
-    // for (size_t i = 0; i < motionParams_.total_joints_; i++)
-    for (size_t i = 0; i < 3; i++)
+    for (size_t i = 0; i < motionParams_.total_joints_; i++)
+    // for (size_t i = 0; i < 3; i++)
     {
         std::string joint_name = jointStates_.name[i];
         size_t index = 1e5; 
@@ -177,8 +177,7 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
 
     
     // TODO: This block can be deleted
-    // for (size_t i = 0; i < motionParams_.total_joints_; i++)
-    for (size_t i = 0; i < 3; i++)
+    for (size_t i = 0; i < motionParams_.total_joints_; i++)
     {
        if(!jointInited_[i])
        {
@@ -216,11 +215,11 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
     waistController_->UpdateJointStates(waistPositions, waistVelocities, waistEfforts);
 
     // Update the leg controller
-    // size_t legJointSize = motionParams_.leg_joint_indices_.size();
-    // std::vector<double> legPositions(jointPositions_.begin() + motionParams_.leg_joint_indices_[0], jointPositions_.begin() + motionParams_.leg_joint_indices_[0] + legJointSize);
-    // std::vector<double> legVelocities(jointVelocities_.begin() + motionParams_.leg_joint_indices_[0], jointVelocities_.begin() + motionParams_.leg_joint_indices_[0] + legJointSize);
-    // std::vector<double> legEfforts(jointEfforts_.begin() + motionParams_.leg_joint_indices_[0], jointEfforts_.begin() + motionParams_.leg_joint_indices_[0] + legJointSize);
-    // legController_->UpdateJointStates(legPositions, legVelocities, legEfforts);
+    size_t legJointSize = motionParams_.leg_joint_indices_.size();
+    std::vector<double> legPositions(jointPositions_.begin() + motionParams_.leg_joint_indices_[0], jointPositions_.begin() + motionParams_.leg_joint_indices_[0] + legJointSize);
+    std::vector<double> legVelocities(jointVelocities_.begin() + motionParams_.leg_joint_indices_[0], jointVelocities_.begin() + motionParams_.leg_joint_indices_[0] + legJointSize);
+    std::vector<double> legEfforts(jointEfforts_.begin() + motionParams_.leg_joint_indices_[0], jointEfforts_.begin() + motionParams_.leg_joint_indices_[0] + legJointSize);
+    legController_->UpdateJointStates(legPositions, legVelocities, legEfforts);
 }
 
 MotionParameters RobotControlManager::GetMotionParameters()
diff --git a/src/robot_control/src/robot_control_node.cpp b/src/robot_control/src/robot_control_node.cpp
index 0b9c775..a6a825d 100644
--- a/src/robot_control/src/robot_control_node.cpp
+++ b/src/robot_control/src/robot_control_node.cpp
@@ -525,7 +525,7 @@ void RobotControlNode::Publish_joint_trajectory()
     std::stringstream msg_stream; 
     size_t total_joints = robotControlManager_.GetMotionParameters().total_joints_;
 
-    for (size_t i = 0; i < 3; ++i)
+    for (size_t i = 0; i < total_joints; ++i)
     {
         double current_pos = cmd_msg.data[i];