fix: resolve jerky movement during robotic arm initialization

config/cabin_robot.xml

@@ -20,9 +20,9 @@
             </Camera>
 
             <DexHand>
-<!--                <RH56DFTP id="hand1" default_force="500" default_speed="500" ip_address="192.168.1.223" port="6000">-->
+                <RH56DFTP id="hand1" default_force="500" default_speed="500" ip_address="192.168.1.224" port="6000">
-<!--                    <Freedom order="01" default_force="500" default_speed="500" />-->
+                    <Freedom order="01" default_force="500" default_speed="500" />
-<!--                </RH56DFTP>-->
+                </RH56DFTP>
 <!--                <RH56DFTP id="hand2" default_force="500" default_speed="500" ip_address="192.168.1.224" port="6000">-->
 <!--                    <Freedom order="01" default_force="500" default_speed="500" />-->
 <!--                </RH56DFTP>-->
@@ -49,17 +49,17 @@
                             <Motor id="21" jointName="L_WRIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
                             <Motor id="22" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
                         </LeftArmCan>
-                        <RightArmCan id = " " devId = " " channelId ="1"  enable="false" toolFrame="R_FINGER_TIP">
+                        <RightArmCan id = " " devId = " " channelId ="1"  enable="true" toolFrame="R_FINGER_TIP">
-<!--                            <Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
+                            <Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
-<!--                            <Motor id="17" jointName="R_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
+                            <Motor id="17" jointName="R_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
-<!--                            <Motor id="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
+                            <Motor id="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
-<!--                            <Motor id="19" jointName="R_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
+                            <Motor id="19" jointName="R_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
-<!--                            <Motor id="20" jointName="R_WRIST_P" limitQLb="-3.14" limitQUb="3.14" limitQd="3.0"/>-->
+                            <Motor id="20" jointName="R_WRIST_P" limitQLb="-3.14" limitQUb="3.14" limitQd="3.0"/>
                             <Motor id="28" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/>
-<!--                            <Motor id="1" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>-->
+                            <Motor id="29" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>
                         </RightArmCan>
                         <HeadCan id = " " devId = " " channelId ="2" enable="true">
-<!--                            <Motor id="32" jointName="HEAD_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
+                            <Motor id="32" jointName="HEAD_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
                             <Motor id="30" jointName="HEAD_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
                             <Motor id="31" jointName="HEAD_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
                         </HeadCan>

src/devices/motor/ti5_motor/ti5_motor.h

@@ -31,6 +31,9 @@ namespace  cmvr {
                 }
                 if (protocol_->comm_proto == MotorProtocolInterface::CommProto::CANOPEN ) {
                     auto canopen_protocol = std::dynamic_pointer_cast<Ti5MotorCanopenProtocol>(protocol_);
+                    // torqueOff(node_id_);
+                    canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_RESET_COMMUNICATION);
+                    // canopen_protocol->torqueOff(node_id_);
                     canopen_protocol->configPdo(node_id_);
                     canopen_protocol->configProfile(node_id_,4000,8000,8000);
                     canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_ENTER_PRE_OPERATIONAL);
@@ -38,6 +41,7 @@ namespace  cmvr {
                     canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x06);
                     canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x0F);
                     canopen_protocol->setLimitQd(node_id_,6.0);
+                    // canopen_protocol->torqueOff(node_id_);
                 }
             }
         };

src/devices/robot/humanoid_robot/include/humanoid_robot.h

@@ -194,6 +194,11 @@ namespace cmvr::device{
 
         std::shared_ptr<JointSpacePlanner> joint_space_planner_{nullptr};
 
+        // 每个电机组的锁和执行状态
+        std::mutex left_arm_mutex_, right_arm_mutex_, head_mutex_, waist_mutex_;
+        std::atomic<bool> is_left_arm_busy_{false}, is_right_arm_busy_{false};
+        std::atomic<bool> is_head_busy_{false}, is_waist_busy_{false};
+
 
     };
 

src/devices/robot/humanoid_robot/src/humanoid_robot.cpp

@@ -220,11 +220,67 @@ template<int DOF>
 void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double acc) {
     if (cmd.empty()) return;
 
-    for (auto c: cmd) {
+    LOG(INFO) << "vel : " << vel << "acc : " <<  acc << endl;
-        LOG(INFO)  << c.joint_name << c.rad  << endl;
+    for (const auto &js: cmd) {
-        LOG(INFO) << vel << " "<< acc << endl;
+        LOG(INFO) << js.joint_name << " , "<< js.rad  << endl;
     }
 
+    // 获取命令中的第一个电机，判断其所属电机组
+    auto &first_jp = cmd.at(0);
+    std::string group = "";
+
+    if (first_jp.joint_name.find("L_") != std::string::npos) {
+        group = "L_";
+    } else if (first_jp.joint_name.find("R_") != std::string::npos) {
+        group = "R_";
+    } else if (first_jp.joint_name.find("HEAD_") != std::string::npos) {
+        group = "HEAD_";
+    } else if (first_jp.joint_name.find("WAIST_") != std::string::npos) {
+        group = "WAIST_";
+    } else {
+        LOG(ERROR) << "Unknown joint group for " << first_jp.joint_name;
+        return;
+    }
+
+    // 检查所有电机是否属于同一组
+    for (auto &jp : cmd) {
+        if (jp.joint_name.find(group) == std::string::npos) {
+            LOG(ERROR) << "Mixed motor groups detected, skipping command.";
+            return;  // 发现不属于同一组，直接返回
+        }
+    }
+
+    // 获取电机组的互斥锁和忙碌状态
+    std::mutex* group_mutex = nullptr;
+    std::atomic<bool>* is_busy_flag = nullptr;
+
+    // 根据电机组选择对应的mutex和忙碌标志
+    if (group == "L_") {
+        group_mutex = &left_arm_mutex_;
+        is_busy_flag = &is_left_arm_busy_;
+    } else if (group == "R_") {
+        group_mutex = &right_arm_mutex_;
+        is_busy_flag = &is_right_arm_busy_;
+    } else if (group == "HEAD_") {
+        group_mutex = &head_mutex_;
+        is_busy_flag = &is_head_busy_;
+    } else if (group == "WAIST_") {
+        group_mutex = &waist_mutex_;
+        is_busy_flag = &is_waist_busy_;
+    }
+
+    // 检查电机组是否已忙碌
+    bool is_busy = is_busy_flag->load();
+    if (is_busy) {
+        LOG(ERROR) << "  Another thread is already controlling this motor group :" << group << ", skipping command.";
+        return;
+    }
+
+    // 设置为忙碌
+    is_busy_flag->store(true);
+
+    // 使用 mutex 锁住当前电机组，保证只有一个线程在控制
+    std::lock_guard<std::mutex> lock(*group_mutex);
 
     // 1. 读当前关节角
     std::unordered_map<std::string, double> cur_joints_angle;
@@ -265,7 +321,6 @@ void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double
         return;
     }
 
-
     // 3. 采样
     const double dt = 0.001;
     auto samples = joint_space_planner_->sampleTrajectory(traj, dt);
@@ -274,9 +329,8 @@ void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double
         return;
     }
 
-
     // 4. 切模式、设速度
-    std::vector<std::shared_ptr<AbstractMotor> > motors;
+    std::vector<std::shared_ptr<AbstractMotor>> motors;
     motors.reserve(cmd.size());
     for (auto &jp: cmd) {
         auto motor = motor_manager_->getMotor(jp.joint_name);
@@ -291,13 +345,11 @@ void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double
         motors.push_back(motor);
     }
 
-
     // 5. 下发命令
     const auto t0 = std::chrono::steady_clock::now();
     size_t k = 1;
     const size_t K = samples.size();
 
-
     while (k < K) {
         const auto &s = samples[k];
         for (size_t i = 0; i < motors.size(); ++i) {
@@ -321,9 +373,15 @@ void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double
         }
         LOG(INFO) << jp.joint_name << " pos err = " << std::abs(it->second - jp.rad);
     }
+
+    // 结束后恢复状态
+    is_busy_flag->store(false);
 }
 
 
+
+
+
 template<int DOF>
 void HumanoidRobot<DOF>::calibrateZeroQ(const std::string &joint_name) {
     auto motor = motor_manager_->getMotor(joint_name);